【自动驾驶】常见位姿估计算法的比较： 三角测量、PNP、ICP、

PnP问题 (3D-2D)

DLT(直接线性变换算法)

相机标定工程用到的是DLT(直接线性变换算法) ，它是一类PnP问题 (3D-2D) 。请参考【位姿估计 | 视觉SLAM | 笔记】常见位姿估计算法的比较

PnP（Perspective-n-Point）是根据图像中

• 1.特征点的二维像素坐标
• 2.特征点的三维空间坐标

来估计相机在参考坐标系中位姿。

PnP是一类问题，针对不同的情况有不同的解法，常见的算法有：P3P、DLT、EPnP、UPnP等。

ICP算法

imu两时刻的姿态变化，反应了车辆的姿态变化：
两个时间点之间imu的运动，我猜测应该是使用ICP算法求解出来的。
我猜测一下：
时间t1,imu姿态四元数为(a1,b1,c1,w1)，观测到A障碍物的某角点P在imu坐标系下为P1(x1,y1,z1)
时间t2,imu姿态四元数为(a2,b2,c2,w2)，观测到A障碍物的某角点P在imu坐标系下为P2(x2,y2,z2)

以车子拐弯为例，在这两个时刻，imu既平移了，也旋转了。障碍物A在imu坐标系下的两个坐标变化就比较大。通过联立方程，就能求出imu的姿态变化。

细节请参考【位姿估计 | 视觉SLAM | 笔记】常见位姿估计算法的比较
这里是以相机为例，把相机换成imu就可以了。对于障碍物A的点P在imu坐标系下的坐标，可通过激光雷达来获得。

PnP虽然写的是地图到图像的匹配，地图中的是3d点，图像是2d点，因此是从3d和2d的一个匹配，你要算相机的位姿，就叫PnP。不一定是地图，也可以是激光点云，因为激光点云也是3d点。

ICP是同一个深度相机的前后两帧深度图像的匹配，你要计算这前后两帧的时间段内相机的运动，就叫ICP。

1.对极几何:你有两个单目图像，通过特征点匹配后，你要算相机是怎么运动的，就是对极几何。

注意：对极几何是不是三角测量。
算法使用条件：一个相机，不同时刻。
已知：两张单目图像。
求解：相机在这段时间内的运动 R,t.
方法：将两张图进行特征点(像素坐标系下)匹配。然后重点就是假设出相机相对运动参数R,t，假设出点P在O1相机坐标系中的坐标。如下图：

2.三角测量：(不是对极几何)

算法使用条件：2个相机，同一时刻。
已知：
(1)两个相机同时拍摄的世界坐标系下的同一点P(未知)，他们在各自像素坐标系下的坐标(u1,v1),(u2,v2)
(2)两个相机之间的相对位姿。

求解：世界坐标系下的点P。
只需要一个点，因为只求这个点P。但是怎么求呢？相对位姿怎么转化，需要把公式推导一下再分析。

3.DLT

算法使用条件：1个相机，激光雷达，同一时刻。
已知：
(1)世界坐标系的n个点的坐标
(2)这n个世界点在像素坐标系进行匹配，得到n个匹配点对，在像素坐标系下的坐标。
求解：相机的姿态：R(3x3),t(3x1)，共3x3+3x1=12个未知量
提示：每个点对会提供2个约束，根据线性方程组性质可知，12个未知量需要12/2=6个点对。如果大于6个点就使用SVD最小二乘求解。

4.ICP

RGB-D深度相机，可使用ICP算法。
算法使用条件：1个深度相机，不同时刻。
已知：空间中同一点在不同时刻(一般是前后帧)的3D相机坐标系下的三维坐标值，通过RGB-D深度相机图像匹配找到这两个点。

"在ICP问题中，图像信息仅仅用来做特征点的匹配，而并不参与视图几何的运算。也就是说，ICP问题的求解用不到相机的内参与特征点的像素坐标。 "
注意，这里的图像信息是有深度的，做特征点匹配也是在这深度图像中进行匹配的。

求解：相机帧间运动，即，相机在这段时间内的运动 R,t。
提示：每一个点P可以得到3个约束，根据线性方程组性质可知，12个未知量需要12/3=4,那么理论上至少需要4个点就可以求得相机的运动R、T。如果大于4个点就使用最小二乘求解。