视觉SLAM实践入门——（14）高斯牛顿法求解PnP

最新推荐文章于 2024-01-04 11:12:20 发布

原创

最新推荐文章于 2024-01-04 11:12:20 发布 · 759 阅读

2 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文详细阐述了如何使用高斯牛顿法解决PnP问题，包括提取ORB特征、匹配点对，计算雅可比矩阵，构建并求解Hx=b的优化过程，直到达到局部极小值。关键步骤涉及矩阵运算和迭代求解SE3变换。

使用高斯牛顿法求解PnP的步骤：

1、定义向量类型（使用Eigen库可以方便地进行矩阵运算，但是涉及内存对齐的问题。将Eigen的数据类型作为vector模板参数，需要指定对应的内存分配器，关于内存对齐的细节可以参考https://zhuanlan.zhihu.com/p/93824687）

2、提取ORB特征并匹配点对

3、计算雅可比矩阵 J

4、利用 J ，增量方程的 H、b

5、求解 Hx = b

6、更新优化变量

7、如果目标函数下降，继续迭代，直至达到局部极小值或者迭代次数到达上限

#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 <Eigen/Core>

#include <chrono>

#include <sophus/se3.hpp>

//提取ORB特征并匹配
void findAndMatchOrbFeature(const Mat &img1, const Mat &img2, 
	vector<KeyPoint> &kp1, vector<KeyPoint> &kp2, 
	vector<DMatch> &matches)
{	
	Ptr<FeatureDetector> detector = ORB::create();
	Ptr<Descr