1 g2o配置文件g2o(generalgraphicoptimization )是一个基于图像优化的库。
图的优化是将优化问题表示为图的方法之一。 图表由多个顶点和连接这些顶点的边组成。 用顶点表示优化变量，用边表示误差项。
网上有很多更具体的介绍，这里就不多说了。
这里推荐几篇g2o的说明文章。 是个好故事。
从零开始一起学习贫民窟|了解地图优化，一步一步带你了解g2o码
3359 blog.csdn.net/electech6/article/details/86534426
掌握从零开始一起学习贫民窟的g2o顶点编程之路
3359 blog.csdn.net/electech6/article/details/88018481
掌握从零开始一起学习贫民窟的g2o边的代码道
3359 Zhan.zhi Hu.com/p/58521241
SLAM的本质分析-G2O
https://www.guyuehome.com/34679
基于g2o的后端优化代码讲解关于karto中位姿势图的节点和边的讲解在上一篇文章中进行了介绍，感兴趣的学生可以看看。
关于零到二维激光SLAM — Karto后端姿态图的数据结构分析
3359 blog.csdn.net/tiancai LX/article/details/121317039
2.1头文件首先声明继承karto优化接口ScanSolver的G2oSolver类。
clas sg2 osolver :公共密钥：扫描解决方案{公共： G2 osolver (； 虚拟~ g2osolver (； 公共： virtual void clear (； virtual void Compute (； virtualconstkarto : scan solver :3360 idposevectorgetcorrections () const； virtualvoidaddnode (karto :3360 vertex karto :3360 localizedrangescan * p vertex )； virtualvoidaddconstraint (karto :3360 edge karto 33603360 localizedrangescan * p edge )； voidpublishgraphvisualization (visualization _ msgs 33603360 markerarraymarry )； private : karto : scan solver : idposevectormcorrections； g2o :3360 sparseoptimizermoptimizer； (； 2.2优化器初始化g2o的初始化基本上关于这四个步骤，上面第一篇文章已经很清楚了，就不多说了。
将优化程序初始化放入构造函数，使用线性求解器，使用LM下降算法。
typedefg2o : block solverg 2o : blocksolvertraits-1、-1 SlamBlockSolver； typedefg2o :3360 linearsolvercsparseslamblocksolver 3360: posematrixtypeslamlinearsolver； G2oSolver:G2oSolver () /步骤1 :创建线性求解器linearsolverslamlinearsolver * linear solver=newslamlinearsolver )； 线性解决方案集块顺序(false； //创建步骤块解决程序。 用上面定义的线性求解器初始化slamblocksolver * block solver=newslamblocksolver (线性求解器)； //步骤3 :创建总求解器。 然后，从GN、LM、DogLeg中选择一个，用上述块求解器从g2o :3360 optimizationalgorithmlevenberg * solver=ne wg2o 3360: on 步骤4 :创建稀疏优化器(moptimizer.setalgorithm ) solver )； } 2.3要添加到位图中的节点karto的顶点的数据结构为karto :3360 vertex karto 3360: localizedrangescan，在此转换为g2o定义的数据结构VertexSE2。
从源代码中可以看到，
vertex se 2: publicbasevertex 3，SE2 //2D pose Vertex，(x，y，theta
)VertexSE2代表了 2D 情况下的 pose 的顶点, 分别是 (x,y,theta).
这里的代码十分清晰, 就是获取到节点匹配后的位姿, 转成 g2o::SE2 格式传入到 poseVertex->setEstimate() 函数中.
并且设置一下这个节点的ID就可以了.
设置完节点再将这个节点放入优化器中.
void G2oSolver::AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex){ karto::Pose2 odom = pVertex->GetObject()->GetCorrectedPose(); g2o::VertexSE2 *poseVertex = new g2o::VertexSE2; poseVertex->setEstimate(g2o::SE2(odom.GetX(), odom.GetY(), odom.GetHeading())); poseVertex->setId(pVertex->GetObject()->GetUniqueId()); mOptimizer.addVertex(poseVertex); ROS_DEBUG("[g2o] Adding node %d.", pVertex->GetObject()->GetUniqueId());} 2.4 向位姿图中添加边(约束)首先, 先声明一个 EdgeSE2 格式的边, 然后向这个边中添加数据.
首先向边中添加2个节点对应的id号.
然后将这两个位姿间的位姿变换作为观测添加到 g2o::SE2 measurement() 函数中.
正常情况下 通过2个节点的pose与观测是一样的, 不会对优化产生作用. 但是在检测出回环后, 节点的位姿将会改变, 这时再通过2个节点的pose计算出的位姿变换与这个观测将不一样, 就会对优化产生作用.
再将kartoz中的协方差矩阵, 求逆, 添加到边的信息矩阵中.
最后将设置好的边添加到位姿图中.
void G2oSolver::AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge){ // Create a new edge g2o::EdgeSE2 *odometry = new g2o::EdgeSE2; // Set source and target int sourceID = pEdge->GetSource()->GetObject()->GetUniqueId(); int targetID = pEdge->GetTarget()->GetObject()->GetUniqueId(); odometry->vertices()[0] = mOptimizer.vertex(sourceID); odometry->vertices()[1] = mOptimizer.vertex(targetID); // Set the measurement (odometry distance between vertices) karto::LinkInfo *pLinkInfo = (karto::LinkInfo *)(pEdge->GetLabel()); karto::Pose2 diff = pLinkInfo->GetPoseDifference(); g2o::SE2 measurement(diff.GetX(), diff.GetY(), diff.GetHeading()); odometry->setMeasurement(measurement); // Set the covariance of the measurement karto::Matrix3 precisionMatrix = pLinkInfo->GetCovariance().Inverse(); Eigen::Matrix<double, 3, 3> info; info(0, 0) = precisionMatrix(0, 0); info(0, 1) = info(1, 0) = precisionMatrix(0, 1); info(0, 2) = info(2, 0) = precisionMatrix(0, 2); info(1, 1) = precisionMatrix(1, 1); info(1, 2) = info(2, 1) = precisionMatrix(1, 2); info(2, 2) = precisionMatrix(2, 2); odometry->setInformation(info); // Add the constraint to the optimizer ROS_DEBUG("[g2o] Adding Edge from node %d to node %d.", sourceID, targetID); mOptimizer.addEdge(odometry);} 2.5 进行求解进行求解, 并将优化后的位姿进行保存.
void G2oSolver::Compute(){ mCorrections.clear(); // Fix the first node in the graph to hold the map in place g2o::OptimizableGraph::Vertex *first = mOptimizer.vertex(0); if (!first) { ROS_ERROR("[g2o] No Node with ID 0 found!"); return; } first->setFixed(true); // Do the graph optimization mOptimizer.initializeOptimization(); int iter = mOptimizer.optimize(40); // Write the result so it can be used by the mapper g2o::SparseOptimizer::VertexContainer nodes = mOptimizer.activeVertices(); for (g2o::SparseOptimizer::VertexContainer::const_iterator n = nodes.begin(); n != nodes.end(); n++) { double estimate[3]; if ((*n)->getEstimateData(estimate)) { karto::Pose2 pose(estimate[0], estimate[1], estimate[2]); mCorrections.push_back(std::make_pair((*n)->id(), pose)); } else { ROS_ERROR("[g2o] Could not get estimated pose from Optimizer!"); } }} 3 运行 3.1 依赖这篇文章的代码是需要依赖 kinetic 版本的 g2o的, 需要通过如下指令进行安装.
sudo apt-get install ros-kinetic-libg2o
没有测试过 ubuntu1804的g2o, 不知道能不能编译通过.
3.2 运行本篇文章对应的数据包, 请在我的公众号中回复 lesson6 获得，并将launch中的bag_filename更改成您实际的目录名。
我将之前使用过的数据包的链接都放在腾讯文档里了, 腾讯文档的地址如下:
https://docs.qq.com/sheet/DVElRQVNlY0tHU01I?tab=BB08J2
通过如下命令运行本篇文章对应的程序
roslaunch lesson6 karto_slam_outdoor.launch solver_type:=g2o_solver
启动之后, 会显示出使用的优化器的具体类型.
[ INFO] [1636946951.395714883]: ----> Karto SLAM started.[ INFO] [1636946951.445798371]: Use back end.[ INFO] [1636946951.445875510]: solver type is G2OSolver.在运行前期, 由于没有找到回环, 所以一直没有进行优化.
进行优化的时候, 会在终端中打印 [g2o] 字样的log.
[ INFO] [1636947150.294108945, 1606808847.990224492]: [g2o] Optimization finished after 31 iterations.[ INFO] [1636947150.332113885, 1606808848.030497104]: [g2o] Clearing Optimizer..之前 Karto的后端优化与回环检测功能对比测试与分析 这篇文章分析过, 如果不加回环和优化功能, 会在最后产生十分明显的叠图.
优化前 优化后 最终的地图可以看到, 优化还是起了作用的.
4 总结这篇文章简要说明了如何使用g2o进行karto的优化的计算, 只是定性的分析了一下g2o优化确实产生了效果, 如果有同学想要自己定量的分析可以自己更改下代码做一下分析.
下篇文章将使用ceres来进行后端优化的计算.