solvepnp三维位姿估算
2017-08-22 20:17
1681 查看
一、前言
关于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
![](https://oscdn.geek-share.com/Uploads/Images/Content/202011/09/ed044e93e743da2c7bd7066933b4d45d.jpg)
最后得到的三个旋转角度都是相对于世界坐标系的旋转角度。
关于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
![](https://oscdn.geek-share.com/Uploads/Images/Content/202011/09/ed044e93e743da2c7bd7066933b4d45d.jpg)
最后得到的三个旋转角度都是相对于世界坐标系的旋转角度。
相关文章推荐
- 三维姿态估计:关于solvePnP与cvPOSIT
- 三维姿态:关于solvePnP与cvPOSIT
- 相机位姿估计1_1:OpenCV:solvePnP二次封装与性能测试
- PnP 单目相机位姿估计(二):solvePnP利用二维码求解相机世界坐标
- 三维重建:多点透视cvSolvePNP的替代函数(Code)
- VVS(Virtual Visual Servoing)在单目位姿估算中的应用
- 相机位姿估计2:[应用]实时位姿估计与三维重建相机姿态
- 【计算机视觉】opencv靶标相机姿态解算2 根据四个特征点估计相机姿态 及 实时位姿估计与三维重建相机姿态
- 【计算机视觉】opencv靶标相机姿态解算3 根据两幅图像的位姿估计结果求某点的世界坐标 (三维重建)
- 三维重建面试1-位姿追踪:单应矩阵、本质矩阵和基本矩阵
- opencv 中的solvePnP() 程序
- opencv中3D点根据相机参数投影成2D点+solvePnP函数计算相机姿态+2D坐标到3D+相机参数calibration(标定与配准,求得深度彩色相机的内参与外参,再进行配准)
- PnP 单目相机位姿估计(三):二维码角点检测
- solvePnp
- 三维重建面试12:室内三维物体的位姿识别论文列表
- Opencv 里的solvePnP函数使用问题
- OPENCV之从calibrateCamera到solvePnP(一)
- OPENCV之从calibrateCamera到solvePnP(二)
- 【计算机视觉】matlab编程实现根据PNP求解位姿(有问题版)