1.一种将旋转和平移相解耦的6D位姿估计方法,其特征在于包括如下步骤:S1.将RGBD图片进行图像分割得到包含目标的点云和RGB图片;
S2.分别提取包含目标的点云和RGB图片的特征,并根据特征估计目标的旋转;
S3.使用得到的目标的旋转对包含目标的点云进行旋转,得到与相机点云同方向的目标点云,然后估计点云的平移;点云平移为与相机点云同方向的目标点云,和相机点云之间的平移;相机点云具体为相机得到的深度图片经过转换后得到的点云;
S4.通过迭代对目标的旋转和点云的平移进行优化,得到最终优化的6D位姿估计结果。
2.根据权利要求1所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于所述的步骤S1,包括采用PSPNet网络对相机得到的图像进行分割;将分割后的深度图片转换初始点云,并根据以检测中心为圆心,目标最大长度为直径做球,该球以外的点视为噪声点,剔除掉噪声点云,分割后输出为包含目标的点云和RGB图片。
3.根据权利要求2所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于所述的步骤S2包括,采用初始旋转的PointNet网络对包含目标的点云进行特征提取,得到点云特征;采用CNN网络对RGB图片的颜色信息进行特征提取,得到颜色特征;然后将点云特征和颜色特征点对点的进行融合得到特征F;将特征F输入到初始旋转的多层感知机中,对初始旋转的多层感知机进行训练,初始旋转的多层感知机输出四元数Q,四元数Q用于表示目标的旋转方向。
4.根据权利要求3所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于对初始旋转的多层感知机训练包括,采用初始旋转的多层感知机为每一个像素计算出一个长度为
4的向量表示目标旋转,初始旋转的多层感知机为一种基于全连接网络的结构,并采用旋转损失函数进行训练:vj=xj‑Rxj
其中,Lr表示特征估计目标的旋转的损失;xj表示相机点云的第j个点;R表示真实旋转;
表示第j个点的估计旋转;N表示相机点云的点的数量;ω表示旋转损失超参数;vj表示第j个点叉乘真实旋转之后形成的向量; 表示第j个点叉乘估计旋转之后形成的向量;
<·,·>表示两个向量之间的余弦值;||·||表示向量的长度; 表示估计向量的平均值。
5.根据权利要求4所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于所述的步骤S3,包括如下步骤:A1.将四元数Q对包含目标的点云进行旋转,得到与相机点云同方向的目标点云,使得与相机点云同方向的目标点云,和相机点云的方向与数量一致;
A2.将与相机点云同方向的目标点云输入到初始平移的目标PointNet网络中提取出目标特征,将相机点云输入到初始平移的源PointNet网络中提取相机点云特征,并将目标特征和相机点云特征点对点进行拼接,得到拼接特征;同时将特征F复制N份拼接到得到的拼接特征中,并输入到初始平移的多层感知机中,输出一个3维的向量表示目标相对于相机的平移T,N为相机点云的数量。
6.根据权利要求5所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于所述的步骤A1,包括将目标的包含目标的点云转换成3D点云,然后叉乘,得到的旋转矩阵,旋转矩阵由四元数Q转换求得,得到与相机点云同方向的目标点云;对包含目标的点云进行采样,采样步骤为,将包含目标的点云根据其在相机坐标下的深度进行排序,选出深度最小的N个点,其中N为相机点云的数量;
所述的步骤A2,具体为采用去除了STN(空间转换网络)结构的初始平移的目标PointNet网络或初始平移的源PointNet网络,平移损失函数为:其中,Ld表示平移估计部分的损失;N为相机点云的数量;xj表示相机点云的第j个点;R表示真实旋转; 表示第j个点的估计旋转;t表示真实平移;表示估计平移。
7.根据权利要求6所述的将旋转和平移相解耦的6D位姿估计方法,其特征在于步骤S4,每一次迭代包括如下步骤:B1.将上一次迭代得到的旋转与平移应用到目标包含目标的点云的点云中并采样,得到一个粗对齐的点云,该点云与相机点云数量一致;
B2.将与相机点云同方向的目标点云输入到第i次迭代的旋转估计中的源PointNet网络中提取特征,将相机点云输入到第i次迭代的旋转估计中的目标PointNet网络中提取特征,然后点对点的进行拼接;
B3.将特征F复制N份拼接到B2中的特征中,N为相机点云的数量,然后将该特征输入到第i次迭代的旋转估计中的MLP回归网络中,输出一个四元数表示目标相对于相机的旋转,具体为:k
第k次迭代得到的旋转Q表示为,
i‑1
其中,Q 表示第i‑1次迭代得到的旋转; 表示第i次迭代的旋转估计中的源PointNet网络、第i次迭代的旋转估计中的目标PointNet网络和第i次迭代的旋转估计中的MLP回归i‑1网络;ASS(·)表示步骤A1所述的采样步骤;ρ表示目标点云;T 表示第i‑1次迭代得到的平移; 表示相机点云;I表示迭代总数;
i
采用步骤S3的方法求得第i次迭代得到的平移T:
其中, 表示第i次迭代的平移估计中的源PointNet网络、第i次迭代的平移估计中的目标PointNet网络和第i次迭代的平移估计中的MLP回归网络。
B4.计算总损失函数:
其中,L表示总损失;Lr表示特征估计目标的旋转的损失;Ld表示平移估计部分的损失;β表示总损失超参数; 表示第k次迭代的旋转损失; 表示第k次迭代的平移损失;
设置评价指标,评价指标为目标点云在分别与估计的位姿和真实位姿转换之后点之间的平均距离,其中,R表示真实旋转; 表示第j个点的估计旋转;xj表示相机点云的第j个点;N为相机点云的数量;并采用得到的评价指标ADD对迭代结果进行评价,选取最优的6D位姿估计结果。