物体6D位姿估计

一、6D定义

6D位姿，即3D位置和3D姿态（朝向），可以认为是相对概念，代表两个坐标系之间相对的位移和旋转；即拍摄时，相机坐标系相对于原始物体所在世界坐标系的平移和旋转变换[R,T]；其中R的方向为旋转轴的方向，模长为旋转角大小，为逆时针。
我们所估计的6D位姿是物体6D位姿(等价于相机外参)，即从物体所在的坐标系(以物体的重心为原点)到相机坐标系(以相机光心为原点)的旋转和平移，故即使在同一个场景下，不同物体的6D位姿也不同，但他们所在的相机坐标系是一致的。用公式表示为：Tc=R*Tm+t
其中为Tc物体在相机系的坐标，Tm为物体在世界系的坐标，R、t分别为旋转矩阵和平移向量。若将坐标扩充为4维T=[T 1]T，该公式也可以表示为：*Tc=[R t]Tm
1.在世界坐标系到相机坐标系的变换过程中，可以假设有一个中间坐标系吗，该坐标系的原点与世界坐标系一致，基于相机系一致。即中间坐标系的坐标标与世界系只需要旋转：Tz=R*Tm,与相机系只需要平移：Tc=Tz+t

2.旋转矩阵R的特点：旋转矩阵(Rotate Matrix)的性质分析

二、problem

给定RGB图像，估计图像中物体的6D位姿信息

6D即6个方向的自由度，6D位姿包括了3维位置和3维空间朝向

三、6D位姿求解分类

RGB图像输入、点云、RGB+点云

四、方案

（1）RGB图像估计位姿

原理:提取输入图片的特征，将这些特征与原有的3D模型匹配，最后通过PNP算法建立2D-3D坐标的对应关系，从而估计目标物体的6D位姿。

1.基于特征匹配的位姿估计方法：速度快，鲁棒性好。但对于弱纹理物体或者物体存在严重遮挡时，特征匹配方法失效。

2.基于模板匹配：处理低纹理的物体，通过不同距离的策略匹配输入的图片和3D模型，但是当场景混叠或者物体表面发生形变，此时模板匹配效果变差

3.基于深度卷积神经网络的6D位姿估计直接从RGB图片中估计物体6D位姿。

PoseNet:直接从RGB图回归6D相机位姿;聚类模型的3D特征，估计物体6D位姿，单一视图的几何约束，估计3D物体参数并恢复6D位姿，基于SSD框架SSD-6D，通过InceptionV4分支将输入映射为6个不同尺度的特征图，再分别与大小为（4+C+V+R）的卷积核进行卷积，以确定类别、2D边界框、视觉点和平面旋转角度，通过映射关系获取6D位姿)

                                                              SSD-6D网络结构

4.基于关键点检测：将任务解耦为两个阶段，先检测2D图片中物体的关键点坐标，在通过PnP算法估计6D位姿（利用图像分割算法确定输入RGB图片中包含物体的区域，在从分割区域中检测关键点坐标）（包括：通过输出关键点的像素级热力图，解决物体间的遮挡问题，通过yolo网络同时检测物体并估计其6D位姿；PVNet(2019CVPR),先检测物体的可见部分，其上的每个像素预测一个指向物体关键点的方向向量，再用PANSAC对关键点进行投票，最后与物体的3D模型匹配，估计其6D位姿；分割驱动的6D位姿估计算法，物体的每个可见部分都会产生局部预测位姿，利用多个局部预测代替全局预测，算法的鲁棒性好；针对无纹理物体提出Pix2Pose，将物体3D模型坐标点转化为图片坐标中的RGB值，利用生成模型补全物体被遮挡的部分，再通过预测彩色图建立2D-3D对应关系，但当处理低纹理物体或低分辨率图片时，基于关键点的检测的方法性能下降，且不能实现端对端的优化）

(2)基于点云信息的6D位姿估计

1.PointNet直接处理点云数据，耦合分类，部分分割和语义分割,利用最大池化解决点云的无序性，利用T-Net保证旋转不变形，利用进一步用一组稀疏的关键点代表整个输入框架，对小扰动或者缺省值的鲁棒性较好。

2.PointNet++在不同尺度逐层提取局部特征，利用采用采样层，组合层，特征提取层得到深层特征，利用多尺度和多分辨率组合的方式有效解决点云数据不均匀问题。

3.Frustum-PointNet结合2D检测算法来定位目标，再用其对应的点云数据视锥，进行边界框回归，实现3D目标检测和位姿估计

4.深度学习网络VoxelNet将三维云划分为一定数量的Voxel，随机采样和归一化后，对每个非空的Voxel进行特征提取，再经过3D卷积层进一步抽象特征，最后使用RPN（Region Proposal Network）网络对物体进行3D检测与位置回归

(缺点：点云数据量巨大，冗余度高和非结构化性强，存在特征提取难、计算量大，场景遮挡和空间形变敏感问题)

(3)基于RGB-D图片的6D位姿估计

方法：

1.将深度信息和RGB信息一起输入神经网络来完成预测（densefusion）

2.利用深度信息来对RGB网络的输出进行refinement。不断迭代优化结果（posecnn）

DenseFusion

1.网络使用RGB图像求出颜色嵌入图信息

2.使用深度图像求出点云信息

3.将color embedding和gemetry embedding两种信息融合后，送入神经网络进行位姿预测

poseCNN

分为三项任务语义分割，3D位置，3D旋转。使用VGG16作为骨干网络，引入用于对称目标姿态估计训练损失函数ShapeMatch-Loss,通过在图像中定位其中心并预测其与相机的距离来估计对象的3D平移；通过回归的四元素表示来估计对象的3D旋转。在网络完成预测后，会调用ICP算法使用深度图来精炼网络。

MaskedFusion

MaskedFusion在DenseFusion网络基础上进行修改，主要针对数据融合部分进行修改，在改进后MaskedFusion会消耗更多的训练时间，但是准确度更高，并且不会消耗更多的预测时间。

该网络会先将RGB-D信息分为cropped RGB图像、Cropped Depth图像和Cropped Mask图像一起输入网络，相比较DenseFusion网络更多考虑了Mask语义信息（其包含了物体的形状信息）

整个结构分为三个子任务：

任务一：image segmentation

通过语义分割对场景中每个对象检测和分类->二进制mask

任务二：6D pose estimation

对每个对象从不同类型的数据中提取特征->逐像素融合->6D pose estimation->6D pose

任务三：pose refinement

优化部分完全采用了densefusion给出的优化网路