知识库

记录点点滴滴

solvepnp三维位姿估算

一、前言

关于PNP问题就是指通过世界中的N个特征点与图像成像中的N个像点,计算出其投影关系,从而获得相机或物体位姿的问题。

opencv提供的solvepnp函数就是用来解决pnp问题。利用该函数可以实现测算相机/物体的空间姿态,也可以用来空间定位。

 

二、solvepnp函数解析

solvepnp函数原型

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

objectPoints:特征点的世界坐标,坐标值需为float型,不能为double型,可以为mat类型,也可以直接输入vector<point3f>

imagePoints:特征点在图像中的像素坐标,可以输入mat类型,也可以直接输入vector<point3f>,注意输入点的顺序要与前面的特征点的世界坐标一一对应

cameraMatrix:相机内参矩阵

distCoeffs:相机的畸变参数【Mat_<double>(5, 1)】

rvec:输出的旋转向量

tvec:输出的平移矩阵

函数分析

solvepnp通过2D点和3D点求解相机的位姿(R,t),在opencv3中常用的方法是epnp, DLS,迭代法。如果3D点分布在一个平面上,那么通过点对(3D点2D点)算出单应矩阵。然后进行SVD分解。如果平面,可直接使用SVD分解。在opencv2中,没有DLS方法。

 

三、旋转向量转换旋转矩阵

应用罗德里格斯变换

double rm[9];

cv::Mat rotM(3, 3, CV_64FC1, rm);

Rodrigues(rvec, rotM);

 

四、由旋转矩阵求得姿态角

代码:

float theta_z = atan2(rotMat[1][0], rotMat[0][0])*57.2958;

float theta_y = atan2(-rotMat[2][0], sqrt(rotMat[2][0] * rotMat[2][0] + rotMat[2][2] * rotMat[2][2]))*57.2958;

float theta_x = atan2(rotMat[2][1], rotMat[2][2])*57.2958;

 

理论:

以下图片内容转载自VShawn

《solvepnp三维位姿估算》

 

最后得到的三个旋转角度都是相对于世界坐标系的旋转角度。

 

点赞

发表评论

邮箱地址不会被公开。 必填项已用*标注