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

使用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<DescriptorExtractor> desc = ORB::create();
	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
	
	//检测角点
	detector->detect(img1, kp1);
	detector->detect(img2, kp2);
	
	//计算描述子
	Mat desc1, desc2;	
	desc->compute(img1, kp1, desc1);
	desc->compute(img2, kp2, desc2);
	
	//匹配点对
	vector<DMatch> allMatches;
	matcher->match(desc1, desc2, allMatches);
	
	//检测误匹配
	double minDist = 10000;
	for(auto m : allMatches)
		if(m.distance < minDist)	minDist = m.distance;
	
	double useDist = max(2*minDist, 30.0);
	for(auto m : allMatches)	
		if(m.distance <= useDist)
			matches.push_back(m);	
}

//将像素坐标转化为归一化坐标
Point2d pixelToNor(const Point2d &p, const Mat & k)
{
	//像素坐标 = k * 归一化坐标
	//xp = fx * xn + cx
	return Point2d((p.x - k.at<double>(0, 2)) / k.at<double>(0, 0), (p.y - k.at<double>(1, 2)) / k.at<double>(1, 1));
}

//顶点，模板参数：优化变量维度和数据类型
class Vertex : public g2o::BaseVertex<6, Sophus::SE3d> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	//重置
	virtual void setToOriginImpl() override {	//override保留字表示当前函数重写了基类的虚函数
		_estimate = Sophus::SE3d();
	}

	//更新
	virtual void oplusImpl(const double *update) override {
		Eigen::Matrix<double, 6, 1> delta_r;
		delta_r << update[0], update[1], update[2], update[3], update[4], update[5];
		_estimate = Sophus::SE3d::exp(delta_r) * _estimate;
	}

	//存盘和读盘：留空
	virtual bool read(istream &in) {}

	virtual bool write(ostream &out) const {}
};

//边， 模板参数：观测值维度，类型，连接顶点类型
class Edge : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, Vertex> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

	Edge(const Eigen::Vector3d &p) : BaseUnaryEdge(), _p(p) {}

	//计算误差
	virtual void computeError() override 
	{	
		const Vertex *v = static_cast<const Vertex *> (_vertices[0]);	//static_cast将_vertices[0]的类型强制转换为const CurveFittingVertex *
		const Sophus::SE3d T = v->estimate();
		_error = _measurement - T*_p;
	}

	//计算雅可比矩阵
	virtual void linearizeOplus() override 
	{
		const Vertex *v = static_cast<const Vertex *> (_vertices[0]);
		const Sophus::SE3d T = v->estimate();
		
		// e = p - Tp'
		// de/d李代数
		
		Eigen::Vector3d p = T*_p;
		//<>指定子块大小，()指定子块起点
		_jacobianOplusXi.block<3, 3>(0, 0) = -Eigen::Matrix<double, 3, 3>::Identity();
		_jacobianOplusXi.block<3, 3>(0, 3) = Sophus::SO3d::hat(p);	//hat返回向量对应的反对称矩阵
	}

	virtual bool read(istream &in) {}

	virtual bool write(ostream &out) const {}

private:
	Eigen::Vector3d _p;
};

//用g20进行优化
void poseEstimate3d3d(const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &p1s3d, 
	const vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> &p2s3d, 
	Mat &R, Mat &t)
{
	//构建图优化，先设定g2o
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> BlockSolverType;  //优化变量维度为6，误差值维度为3
	typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

	//梯度下降方法，可以从GN, LM, DogLeg 中选
	auto solver = new g2o::OptimizationAlgorithmGaussNewton(
	g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
	g2o::SparseOptimizer optimizer;     // 图模型
	optimizer.setAlgorithm(solver);   // 设置求解器
	optimizer.setVerbose(true);       // 打开调试输出

	//往图中增加顶点
	Vertex *v = new Vertex();
	v->setEstimate(Sophus::SE3d());
	v->setId(0);
	optimizer.addVertex(v);

	//往图中增加边
	for (int i = 0; i < (int)p1s3d.size(); i++) 
	{
		Edge *edge = new Edge(p1s3d[i]);
		edge->setId(i);
		edge->setVertex(0, v);                // 设置连接的顶点
		edge->setMeasurement(p2s3d[i]);      // 观测数值
		edge->setInformation(Eigen::Matrix<double, 3, 3>::Identity()); //信息矩阵
		optimizer.addEdge(edge);
	}

	//执行优化
	optimizer.initializeOptimization();
	optimizer.optimize(10);

	//以Mat格式重构T
	Sophus::SE3d T = v->estimate();
	Eigen::Matrix3d R_ = T.rotationMatrix();
	Eigen::Vector3d t_ = T.translation();	
	R = (Mat_<double>(3, 3) << 
		R_(0, 0), R_(0, 1), R_(0, 2),
		R_(1, 0), R_(1, 1), R_(1, 2),
		R_(2, 0), R_(2, 1), R_(2, 2));
	t = (Mat_<double>(3, 1) << 
		t_(0, 0), t_(1, 0), t_(2, 0));
}

int main(int argc, char **argv)
{
	if(argc != 5)
	{
		cout << "error!" << endl;
		return 0;
	}

	Mat img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	Mat img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

	//提取ORB特征、匹配点对
	vector<KeyPoint> kp1, kp2;
	vector<DMatch> matches;
	findAndMatchOrbFeature(img1, img2, kp1, kp2, matches);
	
		
	//读取深度图，获得3d点对，每个像素占据16字节（单通道）
	Mat img3 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
	Mat img4 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);	
	Mat k = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
	vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> p1s3d, p2s3d;
	for(auto m : matches)
	{
		Point2d p12d = kp1[m.queryIdx].pt;
		unsigned short pixel1 = img3.ptr<unsigned short>((int)p12d.y)[(int)p12d.x];
		Point2d p22d = kp2[m.trainIdx].pt;
		unsigned short pixel2 = img4.ptr<unsigned short>((int)p22d.y)[(int)p22d.x];
		
		if(pixel1 == 0 || pixel2 == 0)	continue;
		
		double depth = pixel1 / 5000.0;		
		Point2d pNor2d = pixelToNor(p12d, k);				
		Eigen::Vector3d p3d(pNor2d.x*depth, pNor2d.y*depth, depth);
		p1s3d.push_back(p3d);
		
		depth = pixel2 / 5000.0;
		pNor2d = pixelToNor(p22d, k);		
		p3d = Eigen::Vector3d(pNor2d.x*depth, pNor2d.y*depth, depth);
		p2s3d.push_back(p3d);		
	}
	
	//g2o求解ICP估计问题	
	Mat R, t;
	poseEstimate3d3d(p1s3d, p2s3d, R, t);
	cout << "R: " << endl << R << endl << "t: " << t.t() << endl;
	
	//验证点对
	for(int i = 0; i < (int)p1s3d.size(); i++)
	{
		Mat p1 = (Mat_<double>(3, 1) << p2s3d[i](0, 0), p2s3d[i](1, 0), p2s3d[i](2, 0));
		Mat p2 = (Mat_<double>(3, 1) << p1s3d[i](0, 0), p1s3d[i](1, 0), p1s3d[i](2, 0));
		
		cout << i << "\t:" << p1.t() << " and " << (R*p2+t).t() << endl;
	}
	
	
	return 0;
}