SLAM十四讲——Pnp求解之DLT

学习目标

        学习PnP求解相机位姿，学习PnP的求解之直接线性变换DLT

---

目录

• PnP
• 直接线性变换DLT

---

 一、PnP

1.PnP的概念

PnP（Perspective-n-Point）是求解3D到2D点对运动的方法，它描述了已知n个3D空间点以及他们的投影位置时，估计相机的位姿。

注意：3D点是世界坐标系下的坐标，而非相机坐标系下的坐标，可以选择以物体质心为空间坐标系的原点，其他点的坐标则是相对该质心的位置。而2D点是该3D点在图像中投影位置，即像素坐标。

2.openCV的pnp的方法

C++:
bool cv::solvePnP	(	InputArray 	objectPoints,
                        InputArray 	imagePoints,
                        InputArray 	cameraMatrix,
                        InputArray 	distCoeffs,
                        OutputArray 	rvec,
                        OutputArray 	tvec,
                        bool 	useExtrinsicGuess = false,
                        int 	flags = SOLVEPNP_ITERATIVE 
                    )	
	
Python:
cv.solvePnP(  objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, flags]]]]	) ->	retval, rvec, tvec

参数详解： 

• objectPoints：3D点的坐标构成的数组；
• imagePoints：2D点的坐标构成的数组，注意：该2D点与3D点是一一匹配的；
• cameraMatrix：相机内参矩阵，格式如下：

                                            

• distCoeffs：畸变系数，长度为4、5、8、12、14的输入向量；
• rvec：输出的旋转向量，它是3X1的向量，如果需要旋转矩阵，则使用罗德里格斯公式（Rodrigues公式）将旋转向量转化为旋转矩阵；
• tvec：输出的平移向量；
• useExtrinsicGuess：该参数和flags=SOLVEPNP_ITERATIVE配合使用。
• flags：表示求解PnP问题的方法，取值很多，具体可以查看opencv的该方法的api，本文只提供几种说明。

SOLVEPNP_DLT： 使用直接线性变换求解Pnp;

SOLVEPNP_P3P：使用P3P求解PnP；

SOLVEPNP_EPNP：使用EPNP求解PnP；

SOLVEPNP_ITERATIVE：使用迭代，通过最小化重投影误差来求解。

二、DLT

1.DLT需要6对2D-3D匹配点才能求解相机的位姿。

2.求解原理如下：

    设空间点P，它的齐次坐标为Pw=(X, Y, Z, 1)T,在图像I1中的投影点坐标为p1=(u1,v1,1)T,求解相机的R和t。

    根据像素坐标系和世界坐标系的转化公式，如下所示：K是已知的相机内参矩阵，中间的3 Ⅹ4矩阵Z包含了旋转和平移的信息，但不是变换矩阵T（T的前3X3矩阵表示旋转R，后3X1表示平移向量t）。

 使用最后一行把s消去，得到下边的两个约束，如下所示：

 

简化表示，定义行向量：

 则上述两个约束则表示为

 其中，t是待求变量，根据上式可以看出每个特征点都有两个关于t的线性约束。而t一共有12个待求变量，所以最少可以通过6对匹配点实现矩阵Z的求解，这种方法称为直接线性变换。