引言

本文接着分享空间目标位姿跟踪和滤波算法中用到的一些常用内容，希望为后来者减少一些基础性内容的工作时间。以往分享总结见文章：位姿跟踪 | 相关内容目录和链接总结（不断更新中~~~）

本文介绍如何利用特征点求解目标体坐标系相对于相机坐标系的位姿。

技术流程

1. PNP介绍

PnP（Perspective-n-Point）是根据图像中特征点的二维像素坐标及其对应的三维空间坐标，来估计相机在参考坐标系中位姿的一类算法。

直观来讲，当相机观察到空间中的某一物体时，我们已经知道了该物体在某一参考坐标系下的位置和姿态，那么如何通过图片中物体的成像判断出相机此时在参考坐标系下的位姿？

这正是PnP要解决的问题，即利用已知三维结构与图像的对应关系求解相机与参考坐标系的相对关系（相机的外参）。

总的来说，PNP问题求解的是相机相对于参考坐标系的位姿。需要已知特征点像素坐标，以及对应的参考坐标系下的坐标值。

一般来说，利用特征点的像素坐标，以及已知的相机内参，可以获取特征点在相机坐标系下的坐标。

在已知特征点在相机坐标系下的坐标和参考坐标系下的坐标位置后，利用ICP方法可以求解相机坐标系和参考坐标系之间的相对位姿！！

2. ICP介绍

迭代最近点算法ICP （ Iterative Closest Point）：已知三维点在两个坐标系中的坐标，求这两个坐标系的变换关系，称为 ICP 问题。

ICP 算法的目的是要找到特征点匹配之间的旋转参数$R$和平移参数$T$，使得两点数据之间满足某种度量准则下的最优匹配。

设参考坐标系下特征点坐标集合为$S_1$，相机坐标系下特征点坐标集合为$S_2$，$P_i$和$Q_i$ （又可记作$p_1^i$和$p_2^i$）分别表示特征点集$S_1$和$S_2$中某个特征点坐标。

构造目标函数：

利用SVD分解求解ICP问题，具体步骤为：

1. 首先计算特征点集的均值以及去中心点化的点坐标：

2. 定义矩阵$M$：

3. 对$M$进行奇异值分解：

4. 计算旋转矩阵$R$的最优解为：

5. 计算平移矩阵$T$：
   

a. 利用ICP求解目标相对相机的位姿

得到特征点坐标在相机坐标系和目标坐标系下的坐标值后，可以利用奇异值分解（SVD）求解目标坐标系相对于参考坐标系的旋转矩阵$R$和平移矩阵$T$，即为求解相对位姿问题。

b. 利用ICP求解相机帧间运动

ICP算法还可以根据前后两帧图像中匹配好的特征点在相机坐标系下的三维坐标，求解相机帧间运动。

直观来讲，当相机在某处观察某一物体时，我们知道了相机此时与物体之间的相对位姿关系；当相机运动到另一处，我们亦知道此时相机与物体的相对位姿关系。

ICP问题可以求解这两次相机与物体的相对位姿关系，从而确定相机发生了怎样的运动。

参考网址：

1. ICP 问题之 SVD
2. 视觉SLAM中的对极约束、三角测量、PnP、ICP问题