（普）学习路线------物体坐标系转换、姿态估计

参考

如何理解物体的6D位姿估计任务？

​​​​​刚体6D位姿估计方法综述

PointNet-基于PointNet直接从点云中估计抓取位姿

由于世界上的所有物体，我们都可以建立它的三维模型，即以物体中心点为坐标原点，建立一个三维坐标系，那么我们就能得到物体所有部位点在自身坐标系下的3D坐标值。由于物体是刚体，所以物体在自身坐标系下的3D坐标值永远是不会变的（物体坐标系和物体之间不发生相对运动），进而它在物体坐标系下的三维位置和三维朝向也不会变。

但物体在相机系下的三维位置和三维朝向时时刻刻都在发生变化，因为相机的移动就表示相机坐标系的移动变化，（相机坐标系和物体之间发生相对移动），那么物体在相机系下的3D坐标值就会变化。比如我们开车行驶在道路上，前方车辆在我们的视角下，三维位置和朝向时时刻刻发生变化，因为我方视角是移动变化的，但其在它自身坐标系下的三维位置和朝向是永远不变的，这一点很重要。

这里面需要区别物体姿态和相机姿态。物体姿态是估计当前帧坐标系，相对物体自身坐标系，物体在这两个坐标系之间的变换关系。而相机姿态是，当前帧坐标系相对前一帧坐标系，物体在这两个坐标系之间的变换。当前一帧坐标系为物体坐标系时，此时的相机的姿态估计就是物体姿态估计。
    
由于在6D姿态估计任务中，我们知道物体在自身坐标系下三维模型，就知道了它在自身坐标系下的三维位置和朝向。如果我们能知道物体从它自身坐标系变换到相机系下，所发生的R和t为多大，我们就能准确知道物体当前时刻在相机系下的三维位置和朝向是怎样的了。所以说，准确地估计出物体在相机系下的R和t，就能让机器自己知道它眼前的物体所处的位置和朝向。

物体所有部位点，在自身坐标系下，有一组坐标值；在中间坐标系下，有一组值；在相机系下，也有一组值。
中间坐标系和相机坐标系，基相同，但原点不同；
中间坐标系和物体自身坐标系，原点相同，但基不同。
因此，两组坐标值在中间坐标系和物体自身坐标系之间发生的变换，就是旋转R。

两组坐标值在中间坐标系和相机坐标系之间发生的变换，就是平移t。由于物体中心点在自身坐标系下的坐标为（0,0,0），那么其在相机系下的3D坐标值就是平移t。

pnp的算法原理：找到若干匹配的点对，一是知道这些关键点在物体3维模型下的3D坐标值，
二是知道这些关键点在相机系下的2D像素点，那么便可以求得这些关键点在相机系下的3D坐标值，知道了点在不同坐标系下的坐标值，从而就能知道旋转矩阵R和平移t，将物体从物体坐标系变换到相机坐标系。后面可以再通过RANSAC优化算法，使得预测结果更加准确