1 PnP解算

PnP问题：Perspective-n-Point问题。
参考下图，

给定n个3D空间参考点，以及各点在相机图像上对应的成像点，求参考点所在坐标系与相机的空间关系。
即：
已知条件1：给定匹配点对：世界坐标系（图中OwXwYwZw）下的n个3D点坐标及其对应在图像坐标系（图中ouv）下的2D点坐标。
已知条件2：相机的内参。
求：世界坐标系OwXwYwZw与相机坐标系OcXcYcZc之间的位姿变换关系。
PnP问题的用途：相机位姿获取，物体位姿测量，AR/VR，机器人操作，SLAM中位姿初值求解……
常用解法：DLT，P3P，EPnP，UPnP。

2 OpenCV的solvePnP

2.1 函数原型定义

OpenCV提供了PnP问题的解算函数，且包含有多种解法。
有以下两个函数。

1) solvePnP

bool solvePnP( InputArray objectPoints, InputArray imagePoints,InputArray cameraMatrix, InputArray distCoeffs,OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess = false, int flags = SOLVEPNP_ITERATIVE );

作用：根据3D-2D点对应关系，获得物体的位姿。
此函数返回旋转和平移向量，可用来将物体坐标系中的3D点变换到相机坐标系下。

2) solvePnPRansac

bool solvePnPRansac( InputArray objectPoints, InputArray imagePoints,InputArray cameraMatrix, InputArray distCoeffs,OutputArray rvec, OutputArray tvec,bool useExtrinsicGuess = false, int iterationsCount = 100,float reprojectionError = 8.0, double confidence = 0.99,OutputArray inliers = noArray(), int flags = SOLVEPNP_ITERATIVE );

与solvePnP功能相同，但这个函数使用RANSAC算法剔除异常样本。
RANSAC：Random Sample Consensus（随机抽样一致）。它是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。它于1981年由Fischler和Bolles最先提出。
因此RANSAC使得PnP函数能够抵抗异常值。

2.2 函数参数

参数：
objectPoints：世界坐标系（上图中OwXwYwZw）下的3D点坐标数组
imagePoints：图像（上图中ouv）中对应3D点的成像点坐标数组
cameraMatrix：相机内参矩阵，3×3
distCoeffs：相机畸变系数数组，可以为NULL，此时视为无畸变。
rvec和tvec：计算结果输出，rvec为旋转向量，tvec为平移向量，两者合并表达的是物体整体（即世界坐标系）在相机坐标系中的位姿

以下参数为可选：
useExtrinsicGuess，这个参数仅用于flags=SOLVEPNP_ITERATIVE，此值如果为true (1)，需要rvec和tvec有输入值，以便函数把输入值作为旋转和平移的估计初始值.
flags：PnP解算方法，详见下节。

solvePnPRansac需要的可选参数
iterationsCount：迭代次数；
reprojectionError：RANSAC使用的内点阈值，即考虑作为内点的观察点与计算点投影之间的最大允许距离
confidence：算法得到有用结果的概率；
inliers：包含 objectPoints 和 imagePoints 中的内点索引的输出向量 .

2.3 PnP解算方法（flags取值）

SOLVEPNP_ITERATIVE：缺省方法，基于 Levenberg-Marquardt 优化的迭代方法，使重投影误差最小化
SOLVEPNP_P3P：方法基于论文 X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”
SOLVEPNP_AP3P：方法基于论文 T. Ke, S. Roumeliotis "An Efficient Algebraic Solution to the Perspective-Three-Point Problem
SOLVEPNP_EPNP：方法基于论文 F. Moreno-Noguer, V. Lepetit and P. Fua “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”
SOLVEPNP_DLS：方法基于论文 J. Hesch and S. Roumeliotis. “A Direct Least-Squares (DLS) Method for PnP”
SOLVEPNP_UPNP：方法基于论文 A. Penate-Sanchez, J. Andrade-Cetto, F. Moreno-Noguer. “Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”
SOLVEPNP_IPPE：方法基于论文 T. Collins and A. Bartoli. “Infinitesimal Plane-Based Pose Estimation”
SOLVEPNP_IPPE_SQUARE：方法基于论文 Toby Collins and Adrien Bartoli. “Infinitesimal Plane-Based Pose Estimation”
SOLVEPNP_SQPNP：方法基于论文 “A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem” by G. Terzakis and M.Lourakis

2.4 solvePnp的点对

一般来说，解算PnP，最少需要4个物体点与其成像点构成的点对，几个特例如下：

当SOLVEPNP_ITERATIVE并且useExtrinsicGuess=true时（rvec和tvec有初值），所需的点数最小可以为3；
P3P methods (SOLVEPNP_P3P, SOLVEPNP_AP3P): 需且仅需4个输入点来获得唯一解；
SOLVEPNP_IPPE：输入点数必须 >= 4，并且所有物体点必须共面；
SOLVEPNP_IPPE_SQUARE：对点的顺序有规定，具体这里略；
除了这几个外，其它方法（flags）要求点的数量必须 >= 4 ，对物体点位置顺序等没有特殊规定。

3 solvePnP使用

使用solvePnP前，需要已具备如下参数：

vector<Point3f>objPts; //3D点数组，世界坐标系物体点坐标，至少4个点
vector<Point2f>imgPts;    //2D点数组，与以上物体点一一对应的图像点坐标
Mat cameraMatrix;   //相机内参矩阵，3x3矩阵
Mat distCoeff;  //相机畸变系数矩阵，我一般是用1x5矩阵，如果相机没有畸变，可以把所有元素置为0

然后调用

Mat rvec, tvec;  //声明用于接收运算结果的两个矢量
solvePnP(objPts, imgPts, cameraMatrix, distCoeff, rvec, tvec);

得到解算结果后，rvec为旋转矢量形式，后续计算不方便，所以一般会用Rodrigues公式转为旋转矩阵，以下直接将rvec和tvec一起转为位姿矩阵

Mat wldInCam = Mat::zeros(4, 4, CV_64FC1);
Rodrigues(rvec, wldInCam(Rect(0, 0, 3, 3)));
tvec.copyTo(wldInCam(Rect(0, 3, 1, 3)));

以上得到的wldInCam即为世界坐标系在相机坐标系中的位姿，如果需要求相机在世界坐标系中的位姿，可取逆即可：

Mat camInWld = wldInCam.inv();

参考

[PnP] PnP问题之DLT解法

PnP解算及SolvePnp用法相关推荐

ceres实现的pnp解算后的位姿优化代码详解
论文阅读模块将分享点云处理,SLAM,三维视觉,高精地图相关的文章.公众号致力于理解三维视觉领域相关内容的干货分享,欢迎各位加入我,我们一起每天一篇文章阅读,开启分享之旅,有兴趣的可联系微信diany ...
RoboMaster视觉教程（6）目标位置解算（PnP求解目标与摄像头间的相对位置）
RoboMaster视觉教程(6)目标位置解算(PnP求解目标与摄像头间的相对位置) 概览算法原理 solvePnP的使用流程实验:测量二维码相对于摄像头的位置 RoboMaster视觉程序中的位 ...
相机矫正_实战 | 我用位姿解算实现单目相机测距
在项目过程中,总遇到需要单目视觉给出目标测距信息的情况,其实单目相机本不适合测距,即使能给出,精度也有限,只能在有限制的条件下或者对精度要求很不高的情况下进行应用.该文结合SLAM方法,通过3D-2D ...
单目相机三维姿态解算
单目相机三维姿态解算 Abstract:This passage mainly describes how to solve pose(Yaw,Pitch,Roll)with signal camer ...
实战 | 巧用位姿解算实现单目相机测距
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达在项目过程中,总遇到需要单目视觉给出目标测距信息的情况,其实单目相 ...
Pixhawk代码分析-姿态解算篇A
姿态解算篇A 基本知识 1.如何实现控制一个无人机系统的算法主要有两类:姿态检测算法.姿态控制算法.姿态控制.被控对象.姿态检测三个部分构成一个闭环控制系统.被控对象的模型是由其物理系统决定,设计无 ...
树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示
截至2021年,树莓派出的最新款应该是Raspberry Pi 400,设计得跟键盘一样,很难想象到这是个树莓派,尤其是它的标语写的很好"你的下一个电脑,何必是电脑",不言而喻.反 ...
LPMS-IMU姿态解算
参考文章:AHRS姿态解算说明(加速度+陀螺仪+磁力计原理及原始数据分析) AHRS俗称航姿参考系统,AHRS由加速度计,磁场计,陀螺仪构成,AHRS的真正参考来自于地球的重力场和地球的磁场--他的静 ...
RT-Thread实战笔记|MPU6050使用详解及DMP姿态解算
小伙伴们大家好,好久不更新RT-Thread实战笔记啦,今天来搞一搞MPU6050,话不多说,淦! 本章源码获取欢迎文末留言区或者公众号后台回复"MPU6050"即可获取本教程源 ...

PnP解算及SolvePnp用法