视觉SLAM实践入门——（17）使用非线性优化方法求解ICP

最新推荐文章于 2024-04-25 22:00:17 发布

原创

最新推荐文章于 2024-04-25 22:00:17 发布 · 1.2k 阅读

6 ·

CC 4.0 BY-SA版权

文章标签：

#slam

本文详细介绍了如何使用g2o库进行ICP（Iterative Closest Point）优化，包括定义顶点和边类型、ORB特征提取与匹配、构造图优化模型以及实际求解过程。重点展示了如何通过优化同时考虑位姿和空间点，提升点云配准的精度和灵活性。

使用g2o求解ICP的步骤：

1、定义顶点和边的类型

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

3、定义并设置求解器

4、定义并设置顶点和边

5、调用求解器的 initializeOptimization 函数进行优化

使用g2o进行优化，不仅可以对位姿进行估计，还可以同时对空间点进行优化（将空间点也加入优化变量中）

如果同时考虑点和相机，整个问题变得更自由，可能会得到其他的解。例如：可以让相机少转一些角度，而把点多移动一些

在BA中希望有尽可能多的约束，因为多次观测会带来更多的信息，能够更准确地估计每个变量

通过几个g2o的例子可以看到，g2o在求解非线性优化问题时具有很明显的通用性

使用g2o求解问题的关键在于构建图优化：

1、定义顶点，要明确优化变量是什么

2、定义边，要明确误差是什么、雅可比的计算公式

3、定义求解器，完成一些配置

#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 <Eigen/SVD>
#include <Eigen/Dense>

#include <chrono>


#include <sophus/se3.hpp>

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>


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