RGB-D SLAM——g2o篇（三）
一.g2o的使用
在该部分中，我们调用g2o程序优化一个球，就是在论文中经常看到的那个例子，论文名字为《g2o：a general framework for(hyper) graph optimization》，优化前后结果如下：
在使用g2o_viewer的时候，如果你是ubuntu14.04使用了现在的版本，那么在运行g2o_viewer的时候会出现错误，原因是：现在的g2o使用Qt5的，而ubuntu14.04使用的是Qt4的，所以会发生错误，具体解决方法如下（主要还是给g2o版本的问题）：
1.卸载现有的g2o：
sudo rm -rf /usr/local/include/g2o
sudo rm -rf /usr/local/lib/libg2o*
2.切换到有效版本并重装g2o：
git clone https://github.com/RainerKuemmerle/g2o/
git log |grep 8ba8a*
git checkout 8ba8a03f7863e1011e3270bb73c8ed9383ccc2a2
sudo apt-get install libqt4-dev
sudo apt-get install qt4-qmake
sudo apt-get install libqglviewer-dev
mkdir build
cd build
cmake ../
make -j8在terminator里输入g2o_viewer即可
二.g2o的结构
g2o里面有各种各样的求解器，而它的顶点、边的类型多种多样。通过自定义顶点和边，事实上，只要一个优化问题能够表达成图，就可以用g2o去求解它。常见的，比如bundle adjustment，ICP，数据拟合等。g2o是一个C++项目，其中矩阵数据结构多来自Eigen。
g2o最基本的类结构是怎么样的呢？我们如何来表达一个Graph，选择求解器呢？
先看上部分，SparseOptimizer是我们需要维护的东西，是一个Optimizable Graph，也是一个Hyper Graph。一个SparseOptimizer含有很多个顶点（继承与Base Vertex）和多条边（继承自BaseUnaryEdge，BaseBinaryEdge或BaseMultiEdge）。这些Base Vertex和Base Edge都是抽象的基类，而实际用的顶点和边，都是它们的派生类。
我们用SparseOptimizer.addVertex 和 SparseOptimizer.addEdge 向图中添加顶点和边，然后调用SpaseOptimizer.optimize来优化。
在优化前，需要指定我们用的求解器和迭代算法。从图下半部分来看，一个SparseOptimization拥有一个Optimization Algorithm,继承自Gusss-Newton，Levernberg-Marquardt，Powell’s dogleg 三者之一，同时拥有一个Solver，含有俩个部分。一个是SparseBlockMatrix，用于计算稀疏的雅克比和海塞；一个用于计算 HΔx=−b H Δ x = − b ，需要一个线性方程的求解器。而这个求解器，可以从PCG，CSparse，Choldmod三者选一。
则一共三个步骤：
1.选择一个线性方程求解器，从 PCG, CSparse, Choldmod中选
2.选择一个 BlockSolver
3.选择一个迭代策略，从GN, LM, Doglog中选
实例如下：
俩张图：
像素的坐标假设为: zji=[u,v]ji z i j = [ u , v ] i j
对于相机1和相机2，根据投影我们知道：
λ1[zj11]=CXj λ 1 [ z 1 j 1 ] = C X j
λ2[zj21]=C(RXj+t) λ 2 [ z 2 j 1 ] = C ( R X j + t )
所以，在此问题中，我们可以构建一个优化问题，并表示成图去求解，这里的优化问题是什么呢？可以这样子写：
在这个图中，有俩种结点：。
1.相机位姿图结点：表达俩个相机所在的位置，是一个SE(3)里的元素。
2.特征点的空间结点：是一个XYZ坐标
相应的，边主要是空间点到像素坐标的投影关系，也就是： λ[zj11]=C(RXj+t) λ [ z 1 j 1 ] = C ( R X j + t )
代码如下：
#include return 0;
}
cout<<"找到了"<"组对应特征点。"<// 构造g2o中的图
// 先构造求解器
g2o::SparseOptimizer optimizer;
// 使用Cholmod中的线性方程求解器
g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod ();
// 6*3 的参数，6指的是Pose，3指的是空间点
g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
// L-M 下降
g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver ); //下降的方法采用列文伯格-马夸克方法
optimizer.setAlgorithm( algorithm );
optimizer.setVerbose( false );
// 添加节点
// 两个位姿节点
for ( int i=0; i<2; i++ )
{
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
v->setId(i);
if ( i == 0)
v->setFixed( true ); // 第一个点固定为零
// 预设值为单位Pose，因为我们不知道任何信息
v->setEstimate( g2o::SE3Quat() );
optimizer.addVertex( v );
}
// 很多个特征点的节点
// 以第一帧为准
for ( size_t i=0; inew g2o::VertexSBAPointXYZ();
v->setId( 2 + i );
// 由于深度不知道，只能把深度设置为1了
double z = 1;
double x = ( pts1[i].x - cx ) * z / fx;
double y = ( pts1[i].y - cy ) * z / fy;
v->setMarginalized(true);
v->setEstimate( Eigen::Vector3d(x,y,z) );
optimizer.addVertex( v ); //将路标点设为顶点，这里有很多个路标点
}
// 准备相机参数
g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
camera->setId(0);
optimizer.addParameter( camera );
// 准备边
// 第一帧
vectornew g2o::EdgeProjectXYZ2UV(); //设置边的类型，这里是反投影
edge->setVertex( 0, dynamic_cast (optimizer.vertex(i+2)) );//优化路标点，这里指的是投影到空间点对这些空间点进行优化
edge->setVertex( 1, dynamic_cast (optimizer.vertex(0)) ); //vertexSE3Expmap类型，这里是反投影的边，优化的应该是C
edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );//观测方程
edge->setInformation( Eigen::Matrix2d::Identity() ); //信息矩阵，2d的
edge->setParameterId(0, 0);
// 核函数
edge->setRobustKernel( new g2o::RobustKernelHuber() );
optimizer.addEdge( edge );
edges.push_back(edge);
}
// 第二帧
for ( size_t i=0; inew g2o::EdgeProjectXYZ2UV();
edge->setVertex( 0, dynamic_cast (optimizer.vertex(i+2)) );//匹配好的空间点
edge->setVertex( 1, dynamic_cast (optimizer.vertex(1)) ); //这里同样指的是位姿，决策变量是R，t，优化的是R,t
edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );//观测方程
edge->setInformation( Eigen::Matrix2d::Identity() );
edge->setParameterId(0,0);
// 核函数
edge->setRobustKernel( new g2o::RobustKernelHuber() );
optimizer.addEdge( edge );
edges.push_back(edge);
}
cout<<"开始优化"<true);
optimizer.initializeOptimization();
optimizer.optimize(10);
cout<<"优化完毕"<//我们比较关心两帧之间的变换矩阵
g2o::VertexSE3Expmap* v = dynamic_cast( optimizer.vertex(1) );
Eigen::Isometry3d pose = v->estimate();
cout<<"Pose="<// 以及所有特征点的位置
for ( size_t i=0; idynamic_cast (optimizer.vertex(i+2));
cout<<"vertex id "<2<<", pos = ";
Eigen::Vector3d pos = v->estimate();
cout<0)<<","<1)<<","<2)<// 估计inlier的个数
int inliers = 0;
for ( auto e:edges )
{
e->computeError();
// chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
if ( e->chi2() > 1 )
{
cout<<"error = "<chi2()<else
{
inliers++;
}
}
cout<<"inliers in total points: "<"/"<"ba.g2o");
return 0;
}
int findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector"和"<"个特征点"< matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");
double knn_match_ratio=0.8;
vector< vectorif (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
matches.push_back( matches_knn[i][0] );
}
if (matches.size() <= 20) //匹配点太少
return false;
for ( auto m:matches )
{
points1.push_back( kp1[m.queryIdx].pt );
points2.push_back( kp2[m.trainIdx].pt );
}
return true;
}
至此，我们已经通过图优化完成了其pose估计，三维点调整，并将reprojected后误差较大的点给去掉了，结果如下图所示：
在上面的图中，我们迭代了10次，由于Pose即(R,t)是决策变量，所以最后只有一个结果。而对pose则进行了调整。同时对误差进行了计算，对误差大于那个点的进行舍去，如下图所示：
最终显示有794个点是符合要求的。
参考：
http://www.cnblogs.com/gaoxiang12/p/5304272.html