学习高翔一起做rgbd-slam中关于g20部分的学习一
程序员文章站
2024-02-26 17:11:22
...
目录
第四步：SparseOptimizer：稀疏优化器 最终要的就是这玩意
g2o:图优化中用的一个库;
在SLAM里，图优化一般分解为两个任务：
1、构建图：机器人位姿作为顶点，位姿间关系作为边。
2、优化图。调整机器人的位姿（顶点）来尽量满足边的约束，使得误差最小。
下面看一张整体图:
只需要按照步骤依次构建就好，下面解释一下每个名称的含义：
第一步：LinearSolver：线性求解器
LinearSolverCholmod ：使用sparse cholesky分解法。继承自LinearSolverCCS
LinearSolverCSparse：使用CSparse法。继承自LinearSolverCCS
LinearSolverPCG ：使用preconditioned conjugate gradient 法，继承自LinearSolver
LinearSolverDense ：使用dense cholesky分解法。继承自LinearSolver
LinearSolverEigen： 依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver//先定义一个BlockSolver;表示pose是6维，观测点是3维。用于3D SLAM中的BA
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
//依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
typedef g2o::LinearSolverEigen< SlamBlockSolver::PoseMatrixType SlamLinearSolver;//在把LinearSolverEigen定义好
// 第1步：创建一个线性求解器LinearSolver
SlamLinearSolver* linearSolver = new SlamLinearSolver();
第二步：BlockSolver：块求解器
BlockSolver 内部包含 LinearSolver，用上面我们定义的线性求解器LinearSolver来初始化
BlockSolver_6_3 ：表示pose 是6维，观测点是3维。用于3D SLAM中的BA
BlockSolver_7_3：在BlockSolver_6_3 的基础上多了一个scale
BlockSolver_3_2：表示pose 是3维，观测点是2维// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
SlamBlockSolver* blockSolver = new SlamBlockSolver( unique_ptr<SlamLinearSolver>(linearSolver));第三步：solver：总求解器
并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<SlamBlockSolver>(blockSolver) );第四步：SparseOptimizer：稀疏优化器 最终要的就是这玩意
// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 图模型 创建稀疏优化器
globalOptimizer.setAlgorithm( solver );// 设置求解器 用前面定义好的求解器作为求解方法
globalOptimizer.setVerbose( false );// 关闭调试输出 其中setVerbose是设置优化过程输出信息用的第五步 添加顶点和边
顶点：
一：先来看看上图中和vertex有关的第①个类： HyperGraph::Vertex。它在这个路径g2o/core/hyper_graph.h。 HyperGraph::Vertex 是个abstract vertex，必须通过派生来使用。
二：图中第②个类，我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的， 而OptimizableGraph的定义在g2o/core/optimizable_graph.h。我们找到vertex定义，发现果然，OptimizableGraph 继承自 HyperGraph。
三：OptimizableGraph::Vertex 非常底层，具体使用时一般都会进行扩展，因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第③个类：BaseVertex 路径：g2o/core/base_vertex.h
BaseVertex<D,T>
D：并非是顶点（更确切的说是状态变量）的维度，而是其在流形空间（manifold）的最小表示，这里一定要区别开.
T：就是顶点（状态变量）的类型.
自定义顶点一般需要考虑重写如下函数：
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以。
setToOriginImpl：顶点重置函数，设定被优化变量的原始值。
oplusImpl：顶点更新函数。非常重要的一个函数，主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后，就是通过这个函数对估计值进行调整的，因此这个函数的内容一定要重视。
本代码不需要自己定义，用g2o本身的顶点类型就行
g2o本身内部定义了一些常用的顶点类型
VertexSE2 : public BaseVertex<3, SE2> //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex<6, Isometry3> //6d vector (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion)
VertexPointXY : public BaseVertex<2, Vector2>
VertexPointXYZ : public BaseVertex<3, Vector3>
VertexSBAPointXYZ : public BaseVertex<3, Vector3>
// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
VertexSE3Expmap : public BaseVertex<6, SE3Quat>
// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
// qw is assumed to be positive, otherwise there is an ambiguity in qx,qy,qz as a rotation
VertexCam : public BaseVertex<6, SBACam>
// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
VertexSim3Expmap : public BaseVertex<7, Sim3>// 第5步：定义图的顶点,并添加到SparseOptimizer中
g2o::VertexSE3 *v = new g2o::VertexSE3();//g2o本身内部定义了一些常用的顶点类型
v->setId( currIndex ); //setId(int) 定义节点编号
/*setEstimate(type) 函数来设定初始值估计为单位矩阵 Identity是VertexSE3中设置打默认值
VertexSE3();
virtual void setToOriginImpl() {
_estimate = Isometry3::Identity();
}
*/
v->setEstimate( Eigen::Isometry3d::Identity() );
v->setFixed( true ); //第一个顶点固定，不用优化
globalOptimizer.addVertex( v );//添加顶点到globalOptimizer边：
BaseUnaryEdge，BaseBinaryEdge，BaseMultiEdge 分别表示一元边，两元边，多元边。
一元边你可以理解为一条边只连接一个顶点。
两元边理解为一条边连接两个顶点，也就是我们常见的边啦。
多元边理解为一条边可以连接多个（3个以上）顶点
边和顶点的成员函数还是差别比较大的，边主要有以下几个重要的成员函数
virtual bool read(std::istream& is);
virtual bool write(std::ostream& os) const;
virtual void computeError();
virtual void linearizeOplus();read，write：分别是读盘、存盘函数，一般情况下不需要进行读/写操作的话，仅仅声明一下就可以
computeError函数：非常重要，是使用当前顶点的值计算的测量值与真实的测量值之间的误差
linearizeOplus函数：非常重要，是在当前顶点的值下，该误差对优化变量的偏导数，也就是我们说的Jacobian
如果不自定义边不用在意这些，用g2o提供的边就行
// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation( information );// 信息矩阵：协方差矩阵之逆
// 边的估计即是pnp求解之结果 观测数值
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);这个代码往图中加入：边连接两个顶点的ID,信息矩阵(协方差矩阵之逆 )，观测数值(T的值)
整体slamEnd代码如下：
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;
//g2o的头文件
#include "slamBase.h"
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
FRAME readFrame(int index,ParameterReader& pd);
double normofTransform(cv::Mat rvec,cv::Mat tvec);
int main(int argc,char** argv)
{
ParameterReader pd;
int startIndex = atoi(pd.getData("start_index").c_str());
int endIndex = atoi(pd.getData("end_index").c_str());
cout<<"Initializing..."<<endl;
int currIndex = startIndex;// 当前索引为currIndex
FRAME lastFrame = readFrame(currIndex,pd);//获取这个index的帧的数据
string detector = pd.getData("detector");
string descriptor = pd.getData("descriptor");
CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
computeKeyPointsAndDesp(lastFrame,detector,descriptor);
PointCloud::Ptr cloud = image2PointCloud(lastFrame.rgb,lastFrame.depth,camera);
pcl::visualization::CloudViewer viewer("viewer");
// 是否显示点云
bool visualize = pd.getData("visualize_pointcloud") == string("yes");
int min_inliers = atoi(pd.getData("min_inliers").c_str());
double max_norm = atof(pd.getData("max_norm").c_str());
typedef g2o::BlockSolver_6_3 SlamBlockSolver;//先定义一个BlockSolver;表示pose是6维，观测点是3维。用于3D SLAM中的BA
//依赖项只有eigen，使用eigen中sparse Cholesky 求解，因此编译好后可以方便的在其他地方使用，性能和CSparse差不多。继承自LinearSolver
typedef g2o::LinearSolverEigen< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;//在把LinearSolverEigen定义好
// 第1步：创建一个线性求解器LinearSolver
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering( false );
// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
SlamBlockSolver* blockSolver = new SlamBlockSolver( unique_ptr<SlamLinearSolver>(linearSolver));
// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr<SlamBlockSolver>(blockSolver) );
// 第4步：创建终极大boss 稀疏优化器（SparseOptimizer）
g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 图模型 创建稀疏优化器
globalOptimizer.setAlgorithm( solver );// 设置求解器 用前面定义好的求解器作为求解方法
globalOptimizer.setVerbose( false );// 关闭调试输出 其中setVerbose是设置优化过程输出信息用的
// 第5步：定义图的顶点和边。并添加到SparseOptimizer中
g2o::VertexSE3 *v = new g2o::VertexSE3();//g2o本身内部定义了一些常用的顶点类型
v->setId( currIndex ); //setId(int) 定义节点编号
v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵 setEstimate(type) 函数来设定初始值
v->setFixed( true ); //第一个顶点固定，不用优化
globalOptimizer.addVertex( v );//添加顶点到globalOptimizer
int lastIndex = currIndex;
//循环
for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
{
cout<<"Reading files "<<currIndex<<endl;
FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
computeKeyPointsAndDesp( currFrame, detector, descriptor );
// 比较currFrame 和 lastFrame
RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
if ( result.inliers < min_inliers ) //inliers不够，放弃该帧
continue;
// 计算运动范围是否太大
double norm = normofTransform(result.rvec, result.tvec);
cout<<"norm = "<<norm<<endl;
if ( norm >= max_norm )
continue;
Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
cout<<"T="<<T.matrix()<<endl;
// 向g2o中增加这个顶点与上一帧联系的边
// 顶点部分
// 顶点只需设定id即可
//g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );//估计为单位矩阵 setEstimate(type) 函数来设定初始值
globalOptimizer.addVertex(v);
// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation( information );// 信息矩阵：协方差矩阵之逆
// 边的估计即是pnp求解之结果 观测数值
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);
lastFrame = currFrame;
lastIndex = currIndex;
// cloud = joinPointCloud( cloud, currFrame, T, camera );
}
//pcl::io::savePCDFile( "../data/result.pcd", *cloud );
cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
// 第6步：设置优化参数，开始执行优化
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定优化步数
globalOptimizer.save( "/home/stf/MySlam_two/r.g2o" );
cout<<"Optimization done."<<endl;
globalOptimizer.clear();
return 0;
return 0;
}
FRAME readFrame(int index,ParameterReader& pd)
{
FRAME f;
string rgbDir = pd.getData("rgb_dir");
string depthDir = pd.getData("depth_dir");
string rgbExt = pd.getData("rgb_extension");
string depthExt = pd.getData("depth_extension");
stringstream ss;
ss<<rgbDir<<index<<rgbExt;
string filename;
ss>>filename;
cout<<filename;
f.rgb = cv::imread(filename);
ss.clear();
filename.clear();
ss<<depthDir<<index<<depthExt;
ss>>filename;
f.depth = cv::imread(filename,-1);
f.frameID = index;
return f;
}
double normofTransform(cv::Mat rvec,cv::Mat tvec)
{
return fabs(min(cv::norm(rvec),2*M_PI-cv::norm(rvec))) + fabs(cv::norm(tvec));
}
上一篇： Dubbo控制台安装