视觉SLAM实践入门——（13）使用OpenCV的EPnP求解PnP

最新推荐文章于 2025-11-16 12:53:29 发布

原创

最新推荐文章于 2025-11-16 12:53:29 发布 · 2.1k 阅读

13 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文介绍了如何通过ORB特征提取、深度图处理，解决PnP问题，包括匹配2D-3D点对、排除深度为0的坏点，以及使用solvePnP求解旋转向量和平移向量，最后转换为旋转矩阵。

求解PnP问题需要3d-2d点对

1、提取ORB特征并匹配

2、从深度图获取特征点的深度值，深度为0的点视为坏点。如果将深度乘以一个非零系数（代码中取1/5000），相当于对平移向量除以这个系数，对旋转矩阵几乎没有影响，验证了平移向量具有尺度的问题

3、根据深度值计算3d点坐标，同步获取2d点坐标（最好在一个循环内一起获取，分开获取，需要注意获取2d点坐标时也要根据3d点深度进行筛选）

4、调用solvePnP求解（默认方法是迭代法），求解得到的是旋转向量和平移向量

5、调用Rodrigues将旋转向量转化为旋转矩阵

#include <iostream>
using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
using namespace cv;


#include <chrono>


void findAndMatchOrbFeature(const Mat &img1, const Mat &img2, 
	vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, 
	vector<DMatch> &matches)
{	
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<DescriptorExtractor> desc = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	
	//检测角点
	de