最近项目上需要程序可以快速估算出目标的位姿，并且要有较高的泛化性。简单说就是不需要提前建立完整的CAD模型，只需要拍几张多角度图片就可以，通过纹理匹配，估算出目标位姿。查阅了几天资料，理了一下初步思路：
首先6D位姿是指一个刚体在三维空间中的位置和姿态。具体来说它包含6个自由度（6 Degrees of Freedom, 6 DoF）。位置：x，y，z的，这三个量决定了物体在空间中的坐标位置。姿态：roll，pitch，yaw，这三个量分别是绕x、y、z的旋转量，描述物体的朝向。通常在数学上用齐次变换矩阵来表示6D位姿。
位置xyz的获取通过检测分割模型进行定位，姿态rpy通过局部特征点匹配+PnP解算得出。
准备工作：
一台深度相机
一个检测分割模型
第一步：
假设目标是一个方体有前后左右上下六个面，从斜上方拍摄确保能得到3个面的点云。根据分割结果，提取目标RGB图做为模板图，和目标点云。
第二步：
提取模板图特征点，和对应的3D坐标。
第三步：
计算目标点云的OBB有向包围盒，得出包围盒在OBB坐标系下的最小点、最大点、世界坐标系下包围盒的中心的坐标，以及OBB坐标系到世界坐标系的旋转矩阵R。其中世界坐标系下包围盒的中心的坐标等价于OBB坐标系到世界坐标系的平移矩阵t。
第四步：
通过对旋转平移矩阵的逆变换，将模板图特征点的3D坐标从世界坐标系变换到OBB坐标系。
第五步：
拍摄一张目标图片，根据分割结果提取目标作为目标图。目标点云的平均xyz作为位置。计算目标图特征点和模板图特征点匹配，得出目标图2D特征点-模板图3D特征点的2D-3D点对。
第六步：
用PnP算法结算2D-3D点对，得出相机坐标系到OBB坐标系的旋转平移矩阵，将坐标轴和包围盒投影到图片上。