CN111739096A - 深度传感器移动时自定位方法及*** - Google Patents

深度传感器移动时自定位方法及*** Download PDF

Info

Publication number
CN111739096A
CN111739096A CN202010588348.2A CN202010588348A CN111739096A CN 111739096 A CN111739096 A CN 111739096A CN 202010588348 A CN202010588348 A CN 202010588348A CN 111739096 A CN111739096 A CN 111739096A
Authority
CN
China
Prior art keywords
point cloud
depth sensor
matrix
self
reference point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010588348.2A
Other languages
English (en)
Other versions
CN111739096B (zh
Inventor
蒲灿
杨传宇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Amigaga Technology Co ltd
Original Assignee
Shenzhen Amigaga Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Amigaga Technology Co ltd filed Critical Shenzhen Amigaga Technology Co ltd
Priority to CN202010588348.2A priority Critical patent/CN111739096B/zh
Priority claimed from CN202010588348.2A external-priority patent/CN111739096B/zh
Publication of CN111739096A publication Critical patent/CN111739096A/zh
Application granted granted Critical
Publication of CN111739096B publication Critical patent/CN111739096B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提供深度传感器移动时自定位方法及***，方法包括以下步骤：在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。该***提升了匹配精度，增强了当点云中含有较强噪声，较多异常值和点云大部分被遮挡时的鲁棒性，提高了准确度。

Description

深度传感器移动时自定位方法及***
技术领域
本发明属于计算机三维视觉技术领域，具体涉及深度传感器移动时自定位方法及***。
背景技术
目前的深度传感器主要有双目视觉深度相机、主动视觉双目深度测距仪(带结构光)、TOF深度相机(飞行时间深度相机)、结构光深度相机、激光测距仪、雷达测距仪或超声波测距仪等等。
现有的深度传感器可利用其六维位姿进行自定位。六维位姿包括相对参考坐标系，深度传感器在当前坐标系绕x,y,z轴的位移量与旋转角度。而目前可使用三维点云匹配的方法获取六维位姿，三维点云匹配的方法主要有以下三大类：
1、基于最近点迭代法(ICP)的衍生类算法，此大类主要通过使用EM算法最小化基准点云和目标点云对应点之间的最短欧氏距离，采用迭代的方式求取两个点云之间的相对六维姿势。由于假设来自于不同点云的点相隔距离小于阈值则为对应点，对应点这一假设在现实中几乎不存在，从而限制了该类方法的精确度。
2、基于特征点匹配的六维位姿获取方法，此大类方法主要通过从点云中提取局部的特征向量，然后通过随机采样，RANSAC或者霍夫变换等方法估计相对位姿。由于点云的点密度和噪声程度会极大的影响局部的特征向量提取，因而该类方法对点云的噪声，异常值，和传感器的采样密度具有较高的敏感性，从而影响精度。
3、基于概率模型的点云匹配方法，其主要思路为利用某一数学概率模型(如高斯混合模型，学生t混合模型Student’s-t mixture model，狄利克雷法冯米塞斯费舍尔混合模型Dirichlet process von Mises-Fisher mixture等)来描述点云分布，通过概率匹配求取六维姿势，但是这个方法计算复杂度比较高，速度比较慢。
发明内容
针对现有技术中的缺陷，本发明提供一种深度传感器的移动自定位方法及***，提高了定位的精准度。
第一方面，一种深度传感器移动时自定位方法，包括以下步骤：
在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；
定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；
定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
优选地，所述将基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿具体包括：
计算所述基准点云和目标点云中各点的不确定度分布；所述不确定度分布采用协方差矩阵表示；
执行初始化步骤；
点云自适应下采样；
在EM算法的E阶段更新基准点云和目标点云之间的平均最短距离，以及基准点云中各点的协方差矩阵；计算系数coef；
在EM算法的M阶段求取当前最优旋转矩阵和最优平移矩阵；
根据预设的收敛条件判断是否收敛，如果没收敛，返回所述点云自适应下采样步骤循环执行；如果收敛，输出最终得到的最优旋转矩阵和最优平移矩阵，将所述最优旋转矩阵和最优平移矩阵转换为当前时刻下深度传感器的六维位姿。
优选地，所述初始化步骤具体包括：
初始化能量函数、旋转矩阵和平移矩阵，对基准点云和目标点云进行归一化，设置初始参数；
所述能量函数采用带权重的高斯混合模型进行刻画；其中，权重由以下两部分确定：基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置的第一权重，以及基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置的由颜色差异造成的第二权重。
优选地，所述能量函数为：
Loss＝∫prob(ξ)log[GMMM(ξ)·GMMF(ξ)]dξ；
其中，prob(ξ)为位置ξ出现一点的概率，GMMM(ξ)为基准点云的概率分布，GMMF(ξ)为目标点云的概率分布。
优选地，利用预设的优化算法将所述能量函数约分化简得到：
Figure BDA0002555490820000031
其中，
Figure BDA0002555490820000032
基准点云M中有S1个点pi，i为整数且1≤i≤S1；目标点云F中有S2个点pj，j为整数且1≤j≤S2；∑pi表示基准点云M中点pi的协方差矩阵，∑pj表示目标点云F中点pj的协方差矩阵，θpi(pj，pi)和θpj(pi，pj)分别为所述第二权重。
优选地，所述在EM算法的M阶段求取当前最优旋转矩阵和最优平移矩阵具体包括：
利用预设的优化算法求解所述能量函数，得到当前最优旋转矩阵和最优平移矩阵。
优选地，最终最优旋转矩阵为
Figure BDA0002555490820000041
最终最优平移矩阵为
Figure BDA0002555490820000042
其中，
Figure BDA0002555490820000043
分别为迭代结束后得到的当前阶段旋转矩阵和平移矩阵，R0为初始的旋转矩阵，t0为初始的平移矩阵，poF、poM分别为目标点云F与基准点云M的几何中心，Lrmax为最大点云半径尺寸。
优选地，所述根据预设的收敛条件判断是否收敛具体包括：
当存在以下任一中收敛条件时，判断为收敛：
(1)迭代次数超过预设的最大迭代次数；
(2)基准点云和目标点云的平均最短距离小于预设的最小阈值距离；
(3)前后两次迭代所得旋转角度之差小于预设的最小角度变化值，且前后两次迭代所得两次平移距离之差小于预设的最小平移距离。
第二方面，一种深度传感器移动时自定位***，包括：
采样单元：用于在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
匹配单元：用于将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
定位单元：用于利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
由上述技术方案可知，本发明提供的一种深度传感器的移动自定位方法及***，通过将传感器不确定度分布融入点云概率分布建模，将颜色信息融入门函数，提升了匹配精度，增强了当点云中含有较强噪声，较多异常值和点云大部分被遮挡时的鲁棒性，通过自适应采样，提高了准确度，增加了工作量程范围，加快了方法收敛的时间。通过对点云归一化处理，缩短了方法收敛时间。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍。在所有附图中，类似的元件或部分一般由类似的附图标记标识。附图中，各元件或部分并不一定按照实际的比例绘制。
图1为本发明实施例一提供的移动自定位方法的流程图。
图2为本发明实施例一提供的移动自定位方法的示意图。
图3为本发明实施例一提供的六维位姿获取方法的流程图。
图4为本发明实施例二提供的移动自定位***的模块框图。
具体实施方式
下面将结合附图对本发明技术方案的实施例进行详细的描述。以下实施例仅用于更加清楚地说明本发明的技术方案，因此只作为示例，而不能以此来限制本发明的保护范围。需要注意的是，除非另有说明，本申请使用的技术术语或者科学术语应当为本发明所属领域技术人员所理解的通常意义。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
实施例一：
一种深度传感器移动时自定位方法，参见图1、2，包括以下步骤：
S1：在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；
S2：定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；
S3：定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
具体地，针对同一场景，在不同时刻使用同一深度传感器，需要匹配的目标点云和基准点云的场景有部分重叠即可。
具体地，目标点云为固定不动的点云，基准点云可以进行平移旋转变换。假设基准点云M中共有S1个点pi(i为整数且1≤i≤S1)，目标点云F中共有S2个点pj(j为整数且1≤j≤S2)。
S4：将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
S5：利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
具体地，图2为本实施例提供的深度传感器的移动自定位示意图，图2中0为地平面，t1与t2时刻(其中t1时刻早于t2时刻)对应同一深度传感器，圆柱状物1为预设场景。其中场景内容任意，不限于图2中的圆柱状物1。在t1时刻，将此时刻深度传感器的相机坐标系作为参考坐标系，以此时刻深度传感器获取的三维点云为目标点云，使用本发明内容，用来自于t2时刻深度传感器的三维点云与之匹配获取相对六维姿势，即t2时刻相机坐标系相对于t1时刻的沿x，y，z轴的位移量与旋转角度，由此可得传感器六维位姿变化，从而完成深度传感器在移动过程中的自定位。
具体地，运行该移动自定位方法的计算处理装置应具有并行计算能力，这样能加快方法收敛，如nvidia jeston Xavier等，搭载ubuntu16.04***，运行环境为gcc 4.9.3，g++4.9.3，cuda toolkit 8.0以上，Matlab 2016b以上。
该移动自定位方法，提升了匹配精度，增强了当点云中含有较强噪声，较多异常值和点云大部分被遮挡时的鲁棒性。
具体地，参见图3，S4步骤具体包括：
S11：计算所述基准点云和目标点云中各点的不确定度分布；所述不确定度分布采用协方差矩阵表示；
具体地，点云中每个点的不确定度分布用协方差矩阵表示。其中∑pi表示基准点云M中点pi的协方差矩阵，∑pj表示目标点云F中点pj的协方差矩阵。因此可以用以下高斯函数表示基准点云M中的一点pi在三维空间的某一位置ξ出现的概率：
Figure BDA0002555490820000071
同理可得，目标点云F中的一点pj在三维空间的某一位置ξ出现的概率为：
Figure BDA0002555490820000072
S12：执行初始化步骤，具体包括：
初始化能量函数、旋转矩阵和平移矩阵，对基准点云和目标点云进行归一化，设置初始参数；
A、初始化能量函数。
所述能量函数采用带权重的高斯混合模型进行刻画；其中，权重由以下两部分确定：
1、基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置的权重。例如
Figure BDA0002555490820000081
表示当基准点云M中一点pi出现在三维空间ξ位置时，来自于目标点云F中的一点pF同样出现在同一位置ξ时的权重。同理可得，
Figure BDA0002555490820000082
表示当目标点云F中一点pj出现在三维空间ξ位置时，来自于基准点云M中的一点pM同样出现在同一位置ξ时的权重。
2、基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置时的颜色差异造成的权重。由于不同点云中的两点出现在同一空间位置时，它们被默认为匹配点，颜色应该相似，所以该方法用如下值域为{0，1}的门函数来刻画颜色差异造成的权重。
Figure BDA0002555490820000083
上述门函数表示出现在同一位置上ξ的两点pi与pF的颜色三元素红绿蓝之差应该在正负阈值范围内时，取值为1。其中，rth为红元素的阈值(正数，通常设为3)，gth为绿元素的阈值(正数，通常设为3)，bth为蓝原色的阈值(正数，通常设为3)，点pi的RGB值为(rpi，gpi，bpi)，点pF的RGB值为(rpF，gpF，bpF)。
同理可得：
Figure BDA0002555490820000084
上述门函数表示出现在同一位置上ξ的两点pj与pM的颜色三元素红绿蓝之差应该在正负阈值范围内时，取值为1。其中，点pj的RGB值为(rpj，gpj，bpj)，点pM的RGB值为(rpM，gpM，bpM)。
因此得到基准点云M的概率分布为：
Figure BDA0002555490820000091
目标点云F的概率分布为：
Figure BDA0002555490820000092
基准点云M和目标点云F的概率分布的乘积越大，说明基准点云M和目标点云F重合区域最多，最终能量函数为：
Loss＝∫prob(ξ)log[GMMM(ξ)·GMMF(ξ)]dξ；
其中，prob(ξ)为位置ξ出现一点的概率。通过约分化简和使用EM算法，最终优化的能量函数为：
Figure BDA0002555490820000093
其中，
Figure BDA0002555490820000094
B、初始化旋转矩阵和平移矩阵。
具体地，旋转矩阵R0和平移矩阵t0可以由用户自行提供，或者是初始化的旋转矩阵R0默认值为3×3单位矩阵，平移矩阵t0默认值为3×1的零矩阵。
C、对基准点云和目标点云进行归一化。
具体地，以x轴负方向无穷远处为参考点，选取x轴方向上的点云中最远点和最近点计算x轴的轴向半径rx。同理计算y轴方向的轴向半径ry与z轴方向的轴向半径rz，点云的半径尺寸则为
Figure BDA0002555490820000095
定义基准点云和目标点云中最大的半径尺寸为Lrmax，求取基准点云M的几何中心poM与目标点云F的几何中心poF，则基准点云和目标点云的归一化公式为：
Figure BDA0002555490820000096
D、其他初始参数设置。
具体地，用户可以自行设置最大迭代次数、最小阈值距离、最小角度变化值和最小平移距离。最大迭代次数越大，程序运行时间越长，得到的结果越精准，一般设置为50就能取得较好结果。最小阈值距离可设为0.001，最小角度变化值可设为0.01度，最小平移距离0.0001。
S13：点云自适应下采样；
具体地，随着迭代次数增大，采样率逐渐增高。采样率低，程序收敛速度快，但是精度不够高。采样率高，程序收敛速度慢，但是精度较高。通常可将采样率设置为从30％逐渐变为100％。若有其他信息，如语义信息，也可对不同物体采用不同采样率。
S14：在E阶段更新基准点云和目标点云之间的平均最短距离，以及基准点云中各点的协方差矩阵；计算系数coef；
S15：在M阶段求取当前最优旋转矩阵和最优平移矩阵，具体包括：
利用预设的优化算法求解所述能量函数，得到当前最优旋转矩阵
Figure BDA0002555490820000103
和最优平移矩阵
Figure BDA0002555490820000104
S16：根据预设的收敛条件判断是否收敛，如果没收敛，返回步骤S13；如果收敛，执行步骤S17；
具体地，当存在以下任一中收敛条件时，判断为收敛：
(1)迭代次数超过预设的最大迭代次数；
(2)基准点云和目标点云的平均最短距离小于预设的最小阈值距离；
(3)前后两次迭代所得的旋转角度之差小于预设的最小角度变化值，且前后两次迭代所得的平移距离之差小于预设的最小平移距离。
S17：输出最终得到的最优旋转矩阵和最优平移矩阵，将所述最优旋转矩阵和最优平移矩阵转换为当前时刻下深度传感器的六维位姿。
具体地，最终得到的最优旋转矩阵为
Figure BDA0002555490820000101
最优平移矩阵为
Figure BDA0002555490820000102
最优平移矩阵转换时，令平移向量等于平移矩阵，从而得到沿x，y，z轴方向的位移[dx，dy，dz]′。使用matlab中的函数rotm2eul将最优旋转矩阵转换为绕x，y，z轴旋转的欧拉角[alpha，beta，gama]′，则最终的六维位姿为[dx，dy，dz，alpha，beta，gama]′，则完成对传感器进行定位。
该方法通过将传感器不确定度分布融入点云概率分布建模，将颜色信息融入门函数，提升了匹配精度，增强了点云中含有较强噪声，较多异常值和点云大部分被遮挡等现象时的鲁棒性，通过自适应采样，提高了准确度，增加了工作量程范围，加快了方法收敛的时间。通过对点云归一化处理，缩短了方法收敛时间。
本发明实施例所提供的方法，为简要描述，实施例部分未提及之处，可参考前述方法实施例中相应内容。
实施例二：
一种深度传感器的移动自定位***，参见图4，包括：
采样单元：用于在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
匹配单元：用于将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
定位单元：用于利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
该***通过将传感器不确定度分布融入点云概率分布建模，将颜色信息融入门函数，提升了匹配精度，增强了当点云中含有较强噪声，较多异常值和点云大部分被遮挡时的鲁棒性，通过自适应采样，提高了准确度，增加了工作量程范围，加快了方法收敛的时间。通过对点云归一化处理，缩短了方法收敛时间。
本发明实施例所提供的***，为简要描述，实施例部分未提及之处，可参考前述方法实施例中相应内容。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围，其均应涵盖在本发明的权利要求和说明书的范围当中。

Claims (10)

1.一种深度传感器移动时自定位方法，其特征在于，包括以下步骤：
在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；
定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；
定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
2.根据权利要求1所述深度传感器移动时自定位方法，其特征在于，所述将基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿具体包括：
计算所述基准点云和目标点云中各点的不确定度分布；所述不确定度分布采用协方差矩阵表示；
执行初始化步骤；
点云自适应下采样；
在EM算法的E阶段更新基准点云和目标点云之间的平均最短距离，以及基准点云中各点的协方差矩阵；计算系数coef；
在EM算法的M阶段求取当前最优旋转矩阵和最优平移矩阵；
根据预设的收敛条件判断是否收敛，如果没收敛，返回所述点云自适应下采样步骤循环执行；如果收敛，输出最终得到的最优旋转矩阵和最优平移矩阵，将所述最优旋转矩阵和最优平移矩阵转换为当前时刻下深度传感器的六维位姿。
3.根据权利要求2所述深度传感器移动时自定位方法，其特征在于，所述初始化步骤具体包括：
初始化能量函数、旋转矩阵和平移矩阵，对基准点云和目标点云进行归一化，设置初始参数；
所述能量函数采用带权重的高斯混合模型进行刻画；其中，权重由以下两部分确定：基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置的第一权重，以及基准点云中的一点和目标点云中的一点同时出现在三维空间中同一位置的由颜色差异造成的第二权重。
4.根据权利要求3所述深度传感器移动时自定位方法，其特征在于，
所述能量函数为：
Loss＝∫prob(ξ)log[GMMM(ξ)·GMMF(ξ)]dξ；
其中，prob(ξ)为位置ξ出现一点的概率，GMMM(ξ)为基准点云的概率分布，GMMF(ξ)为目标点云的概率分布。
5.根据权利要求4所述深度传感器移动时自定位方法，其特征在于，
利用预设的优化算法将所述能量函数约分化简得到：
Figure FDA0002555490810000021
其中，
Figure FDA0002555490810000022
基准点云M中有S1个点pi，i为整数且1≤i≤S1；目标点云F中有S2个点pj，j为整数且1≤j≤S2；
Figure FDA0002555490810000023
表示基准点云M中点pi的协方差矩阵，
Figure FDA0002555490810000024
表示目标点云F中点pj的协方差矩阵，
Figure FDA0002555490810000025
和
Figure FDA0002555490810000026
分别为所述第二权重。
6.根据权利要求5所述深度传感器移动时自定位方法，其特征在于，所述在EM算法的M阶段求取当前最优旋转矩阵和最优平移矩阵具体包括：
利用预设的优化算法求解所述能量函数，得到当前最优旋转矩阵和最优平移矩阵。
7.根据权利要求6所述深度传感器移动时自定位方法，其特征在于，
最终最优旋转矩阵为
Figure FDA0002555490810000031
最终最优平移矩阵为
Figure FDA0002555490810000032
其中，
Figure FDA0002555490810000033
分别为迭代结束后得到的当前阶段最优旋转矩阵和平移矩阵，R0为初始的旋转矩阵，t0为初始的平移矩阵，poF、poM分别为目标点云F与基准点云M的几何中心，Lrmax为最大点云半径尺寸。
8.根据权利要求2所述深度传感器移动时自定位方法，其特征在于，所述根据预设的收敛条件判断是否收敛具体包括：
当存在以下任一中收敛条件时，判断为收敛：
(1)迭代次数超过预设的最大迭代次数；
(2)基准点云和目标点云的平均最短距离小于预设的最小阈值距离；
(3)前后两次迭代所得旋转角度之差小于预设的最小角度变化值，且前后两次迭代所得两次平移距离之差小于预设的最小平移距离。
9.一种深度传感器移动时自定位***，其特征在于，包括：
采样单元：用于在预设场景中，定义初始时刻下深度传感器的相机坐标系为参考坐标系；定义初始时刻下深度传感器利用所述参考坐标系获得的三维点云为目标点云；定义当前时刻下深度传感器利用当前其相机坐标系获得的三维点云为基准点云；
匹配单元：用于将所述基准点云与目标点云进行匹配，得到当前时刻下深度传感器的六维位姿；
定位单元：用于利用当前时刻下深度传感器的六维位姿进行深度传感器的定位。
10.根据权利要求9所述深度传感器移动时自定位***，其特征在于，所述匹配单元具体用于：
计算所述基准点云和目标点云中各点的不确定度分布；所述不确定度分布采用协方差矩阵表示；
执行初始化步骤；
点云自适应下采样；
在EM算法的E阶段更新基准点云和目标点云之间的平均最短距离，以及基准点云中各点的协方差矩阵；计算系数coef；
在EM算法的M阶段求取当前最优旋转矩阵和最优平移矩阵；
根据预设的收敛条件判断是否收敛，如果没收敛，返回所述点云自适应下采样步骤循环执行；如果收敛，输出最终得到的最优旋转矩阵和最优平移矩阵，将所述最优旋转矩阵和最优平移矩阵转换为当前时刻下深度传感器的六维位姿。
CN202010588348.2A 2020-06-24 深度传感器移动时自定位方法及*** Active CN111739096B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010588348.2A CN111739096B (zh) 2020-06-24 深度传感器移动时自定位方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010588348.2A CN111739096B (zh) 2020-06-24 深度传感器移动时自定位方法及***

Publications (2)

Publication Number Publication Date
CN111739096A true CN111739096A (zh) 2020-10-02
CN111739096B CN111739096B (zh) 2024-08-02

Family

ID=

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4123332A1 (en) 2021-07-23 2023-01-25 Stichting IMEC Nederland Method for estimating a location of at least one mobile network node and respective network

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109202912A (zh) * 2018-11-15 2019-01-15 太原理工大学 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法
CN110264502A (zh) * 2019-05-17 2019-09-20 华为技术有限公司 点云配准方法和装置
DE102019108733A1 (de) * 2018-04-11 2019-10-17 Nvidia Corporation Schnelle mehrskalige Punktwolkenregistrierung mit einer hierarchischen Gauß-Mischung

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102019108733A1 (de) * 2018-04-11 2019-10-17 Nvidia Corporation Schnelle mehrskalige Punktwolkenregistrierung mit einer hierarchischen Gauß-Mischung
CN109202912A (zh) * 2018-11-15 2019-01-15 太原理工大学 一种基于单目深度传感器和机械臂配准目标轮廓点云的方法
CN110264502A (zh) * 2019-05-17 2019-09-20 华为技术有限公司 点云配准方法和装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
C. PU等: "DUGMA: Dynamic Uncertainty-Based Gaussian Mixture Alignment", 《INTERNATIONAL CONFERENCE ON 3D VISION (3DV)》, 8 September 2018 (2018-09-08), pages 766 - 774 *
李亮: "基于点云匹配的智能车定位方法研究", 《中国优秀博士学位论文全文数据库：工程科技Ⅱ辑》, no. 1, 15 January 2020 (2020-01-15), pages 1 - 152 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4123332A1 (en) 2021-07-23 2023-01-25 Stichting IMEC Nederland Method for estimating a location of at least one mobile network node and respective network

Similar Documents

Publication Publication Date Title
CN110322500B (zh) 即时定位与地图构建的优化方法及装置、介质和电子设备
CN109345588B (zh) 一种基于Tag的六自由度姿态估计方法
CN111784770B (zh) 基于shot和icp算法的无序抓取中的三维姿态估计方法
CN102175222B (zh) 基于立体视觉的吊车避障***
Zhou et al. T-LOAM: Truncated least squares LiDAR-only odometry and mapping in real time
CN108022262A (zh) 一种基于点的邻域重心向量特征的点云配准方法
CN114549549B (zh) 一种动态环境下基于实例分割的动态目标建模跟踪方法
CN111612841A (zh) 目标定位方法及装置、移动机器人及可读存储介质
CN114331879A (zh) 一种均衡化二阶梯度直方图描述子的可见光与红外图像配准方法
CN109255815B (zh) 一种基于零阶球谐函数的物体检测与识别方法
CN113902782A (zh) 一种挖掘机周边障碍物三维点云的快速配准方法及***
CN112184765A (zh) 一种基于视觉的水下航行器自主跟踪方法
CN116091727A (zh) 一种基于多尺度特征描述的复杂曲面点云配准方法、电子设备及存储介质
CN116643291A (zh) 一种视觉与激光雷达联合剔除动态目标的slam方法
CN116468786A (zh) 一种面向动态环境的基于点线联合的语义slam方法
WO2022110767A1 (zh) 定位与地图构建方法、装置、机器人及计算机存储介质
CN111739096B (zh) 深度传感器移动时自定位方法及***
CN111739096A (zh) 深度传感器移动时自定位方法及***
CN115952248A (zh) 终端设备的位姿处理方法、装置、设备、介质及产品
CN113239936B (zh) 一种基于深度学习与特征点提取的无人机视觉导航方法
CN116359910A (zh) 低照度条件下快速移动目标多视角协同跟踪方法及装置
CN115861352A (zh) 单目视觉、imu和激光雷达的数据融合和边缘提取方法
CN115239776A (zh) 点云的配准方法、装置、设备和介质
Chen et al. A Framework for 3D Object Detection and Pose Estimation in Unstructured Environment Using Single Shot Detector and Refined LineMOD Template Matching
CN113592958A (zh) 一种基于单目视觉的auv对接坞站光学引导方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant