基于PnP的目标位姿求解

一、PnP求解简介

PnP常用于对合作目标进行位姿估计，其中合作目标上存在相对位置关系已知的标志点，PnP是针对3D-2D映射关系的一种方法，OpenCV中已有PnP求解的实现函数solvePnP，函数声明如下：

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

其中：

flags：求解方法默认采用的是迭代的方法SOLVEPNP_ITERATIVE，另外还有：EPnP，CV_EPNP，UPnP等
rvec：rvec的第i列表示世界坐标系中的第i个坐标轴方向的单位向量在摄像机坐标系里的坐标；
tvec：tvec表示世界坐标系的原点在摄像机坐标系的坐标；

二、P4P求解实例

#include <opencv2/opencv.hpp>
#include <math.h>
#include <iostream>
#include <fstream>
using namespace std;
using namespace cv;int main()
{vector<cv::Point2f> Points2D;//初始化相机参数Opencvdouble camD[9] = {600.7, 0,    665.8,0,    798.1,  667.6,0,      0,      1 };cv::Mat camera_matrix = cv::Mat(3, 3, CV_64FC1, camD);//畸变参数double distCoeffD[5] = { -0.189314, 0.444657, 0, 0, -1.57547 };cv::Mat distortion_coefficients = cv::Mat(5, 1, CV_64FC1, distCoeffD);//特征点的像素坐标Points2D.push_back(cv::Point2f(585, 488));  //P1Points2D.push_back(cv::Point2f(581, 590));  //P2Points2D.push_back(cv::Point2f(586, 697));  //P3Points2D.push_back(cv::Point2f(685, 788));  //P4//特征点世界坐标vector<cv::Point3f> Points3D;Points3D.push_back(cv::Point3f(0, 0, 0));       //P1 三维坐标的单位是毫米Points3D.push_back(cv::Point3f(0, 100, 0));     //P2Points3D.push_back(cv::Point3f(100, 0, 0));     //P3Points3D.push_back(cv::Point3f(100, 100, 0));   //P4//初始化输出矩阵cv::Mat rvec = cv::Mat::zeros(3, 1, CV_64FC1);cv::Mat tvec = cv::Mat::zeros(3, 1, CV_64FC1);//三种方法求解//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_ITERATIVE);    //实测迭代法似乎只能用4个共面特征点求解，5个点或非共面4点解不出正确的解cv::solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_P3P);            //Gao的方法可以使用任意四个特征点，特征点数量不能少于4也不能多于4//solvePnP(Points3D, Points2D, camera_matrix, distortion_coefficients, rvec, tvec, false, CV_EPNP);         //该方法可以用于N点位姿估计;与前两种有偏差//旋转向量变旋转矩阵//提取旋转矩阵double rm[9];cv::Mat rotM(3, 3, CV_64FC1, rm);Rodrigues(rvec, rotM);cout << "use CV_P3P:the rotation matrix is :\n" << rotM << endl;cout << "use CV_P3P:the tran matrix is :\n" << tvec  << endl;return 0;
}

三、OpenCV 官方文档介绍

- solvePnP函数

四、可参考代码

github: SolvePNP

基于PnP的目标位姿求解相关推荐

基于点特征的各位姿求解算法对比（pose-estimation-compared）
基于点特征的位姿求解算法: (1)非迭代:P3P . P4P . RPnP (2)迭代:正交迭代(OI)算法 . L-M 算法 . SoftPOSIT 算法 . Tsai算法(初始解不迭代,精确解迭代 ...
包裹点云位姿估计_【泡泡点云时空】基于点云监督学习的6D目标位姿回归
相比色彩信息, 深度信息更易于进行物体的位姿估计: 提出了第一个直接使用无序点集作为输入的位姿估计深度学习框架: 提出的方法在 YCB-video 数据集上超越了最新方法: 旋转表示.模型设计.损失函 ...
【单目标优化求解】基于matlab增强型黑猩猩优化器算法求解单目标优化问题【含Matlab源码 2013期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[单目标优化求解]基于matlab增强型黑猩猩优化器算法求解单目标优化问题[含Matlab源码 2013期] 点击上面蓝色字体,直接付费下 ...
【单目标优化求解】基于matlab黑猩猩算法求解单目标问题【含Matlab源码 1413期】
一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[单目标优化求解]基于matlab黑猩猩算法求解单目标问题[含Matlab源码 1413期] 点击上面蓝色字体,直接付费下载,即可. 获取代 ...
【智能优化算法】基于矮猫鼬优化算法求解单目标优化问题附matlab代码
1 简介基于矮猫鼬优化算法求解单目标优化问题 2 部分代码 %___________________________________________________________________ ...
【多目标优化求解】基于matlab粘菌算法MOSMA求解多目标优化问题【含Matlab源码 2279期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[多目标优化求解]基于matlab粘菌算法MOSMA求解多目标优化问题[含Matlab源码 2279期] 点击上面蓝色字体,直接付费下载, ...
基于图像的三维重建——P3P求解相机位姿(5)
文章目录 P3P简介直接线性变换法 Kneip算法算法原理及流程基于RANSAC的Kneip算法 P3P简介相机位姿求解是SFM中必要的一步,而P3P问题的目的就是通过三维点(3对)对应关系确 ...
【多目标优化求解】基于matlab灰狼优化算法求解多目标优化问题【含Matlab源码 007期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[多目标优化求解]基于matlab灰狼优化算法求解多目标优化问题 [含Matlab源码 007期] 获取代码方式2: 通过订阅紫极神光博客 ...
【多目标优化求解】基于matlab飞蛾扑火算法 (NSMFO)求解多目标优化问题【含Matlab源码 2312期】
⛄一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[多目标优化求解]基于matlab飞蛾扑火算法 (NSMFO)求解多目标优化问题 [含Matlab源码 2312期] 点击上面蓝色字体,直 ...

基于PnP的目标位姿求解

一、PnP求解简介

二、P4P求解实例

三、OpenCV 官方文档介绍

- solvePnP函数

四、可参考代码

基于PnP的目标位姿求解相关推荐

最新文章

热门文章