1 g2o简介

g2o(General Graphic Optimization)是基于图优化的库。

图优化是把优化问题表现成图的一种方式。一个图由若干个顶点(Vertex)，以及连接这些顶点的边(Edge)组成。用顶点表示优化变量，用边表示误差项。

更具体的介绍网上有很多, 这里就不多说了.

在这里推荐几篇g2o的讲解文章, 讲的很好.

从零开始一起学习SLAM | 理解图优化，一步步带你看懂g2o代码
https://blog.csdn.net/electech6/article/details/86534426
从零开始一起学习SLAM | 掌握g2o顶点编程套路
https://blog.csdn.net/electech6/article/details/88018481
从零开始一起学习SLAM | 掌握g2o边的代码套路
https://zhuanlan.zhihu.com/p/58521241
SLAM本质剖析-G2O
https://www.guyuehome.com/34679

2 基于g2o的后端优化的代码讲解

关于karto中位姿图的节点及边的讲解, 在上一篇文章中已经介绍完了, 感兴趣的同学可以看看.

从零开始搭二维激光SLAM — Karto后端的位姿图相关的数据结构分析
https://blog.csdn.net/tiancailx/article/details/121317039

2.1 头文件

首先, 声明一个G2oSolver类, 继承karto中的优化接口ScanSolver.

class G2oSolver : public karto::ScanSolver
{public:G2oSolver();virtual ~G2oSolver();public:virtual void Clear();virtual void Compute();virtual const karto::ScanSolver::IdPoseVector &GetCorrections() const;virtual void AddNode(karto::Vertex<karto::LocalizedRangeScan> *pVertex);virtual void AddConstraint(karto::Edge<karto::LocalizedRangeScan> *pEdge);void publishGraphVisualization(visualization_msgs::MarkerArray &marray);private:karto::ScanSolver::IdPoseVector mCorrections;g2o::SparseOptimizer mOptimizer;
};

2.2 优化器的初始化

g2o的初始化基本就这4个步骤, 上边的第一篇文章中已经讲的很清楚了, 我就不多说了.
将优化器的初始化放在构造函数中了, 使用LinearSolverCSparse线性求解器, 使用了LM下降算法.

typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;G2oSolver::G2oSolver()
{// 第1步：创建一个线性求解器LinearSolverSlamLinearSolver *linearSolver = new SlamLinearSolver();linearSolver->setBlockOrdering(false);// 第2步：创建BlockSolver。并用上面定义的线性求解器初始化SlamBlockSolver *blockSolver = new SlamBlockSolver(linearSolver);// 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化g2o::OptimizationAlgorithmLevenberg *solver =new g2o::OptimizationAlgorithmLevenberg(blockSolver);// 第4步：创建稀疏优化器（SparseOptimizer）mOptimizer.setAlgorithm(solver);
}

2.3 向位姿图中添加节点

karto中的顶点的数据结构是 karto::Vertexkarto::LocalizedRangeScan , 在这里要转成g2o定义好的数据结构 VertexSE2.

通过源码可知,
VertexSE2 : public BaseVertex<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 edgeg2o::EdgeSE2 *odometry = new g2o::EdgeSE2;// Set source and targetint 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 measurementkarto::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 optimizerROS_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 placeg2o::OptimizableGraph::Vertex *first = mOptimizer.vertex(0);if (!first){ROS_ERROR("[g2o] No Node with ID 0 found!");return;}first->setFixed(true);// Do the graph optimizationmOptimizer.initializeOptimization();int iter = mOptimizer.optimize(40);// Write the result so it can be used by the mapperg2o::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

3.3 结果分析

启动之后, 会显示出使用的优化器的具体类型.

[ 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来进行后端优化的计算.

从零开始搭二维激光SLAM --- 基于g2o的后端优化的代码实现相关推荐

从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建
上篇文章讲解了如何在ROS中发布栅格地图,以及如何向栅格地图赋值．这篇文章来讲讲如何将激光雷达的数据构建成栅格地图．雷达的数据点所在位置表示为占用,从雷达开始到这点之间的区域表示为空闲． 1 GM ...
从零开始搭二维激光SLAM --- 基于ICP的帧间匹配
上一篇文章讲解了如何将激光雷达的sensor_msgs/LaserScan格式转换成pcl::PointCloud< pcl::PointXYZ>格式, 本篇文章将要讲解如何使用这个格式调 ...
从零开始搭二维激光SLAM --- 激光雷达数据效果对比
我们知道,不同品牌的激光雷达产生的数据是不一样的,那这些不同点是如何影响建图效果的呢? 这篇文章就是来分析这个问题,将从不同光强下的点云效果,不同夹角下的点云效果,以及 1 激光雷达的技术指标激光雷 ...
从零开始搭二维激光SLAM --- Hector论文公式推导与相关代码解析
这篇文章将带领大家推导一下hector slam论文中的公式．之后再对这部分公式对应的代码进行讲解下． markdown打公式太费劲了,所以我用手写了．(懒) 然后csdn又限制了图片文件大小,我是照 ...
二维激光SLAM( 使用Laser Scan Matcher )
目录一.Laser Scan Matcher安装配置二.二维激光定位一.Laser Scan Matcher安装配置 ROS自带的laser_scan_matcher库,使用的是CSM(Cano ...
基于ceres的后端优化的代码实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达来源丨从零开始搭SLAM 作者丨李太白lx 由于g2o天然是进行位姿图优化的, 所以十分契合karto ...
使用二维激光雷达和cartographer_ros实现实时SLAM
在前面已经完成了cartographer_ros的安装和demo的运行了.接下来,就要放到机器人上,实时进行SLAM了. 前一篇内容的链接如下: Cartographer_ros的下载.配置及编译与问 ...
二维码识别基于stm32 ov7725
二维码识别基于stm32 ov7725 二维码识别技术在我们的生活中应用的很方便了,但基本都是基于手机的,目前在公交车上也存在了扫描二维码的机器,上次坏了之后,发现一只小企鹅,原来是基于linux的 ...
视觉SLAM笔记（53） g2o 操作后端优化
视觉SLAM笔记(53) g2o 操作后端优化 1. BA 数据集 2. g2o 求解 BA 3. 求解 1. BA 数据集目录下的 common 文件夹是实验的数据集部分它的布局如图所示其中, ...
二维码生成【前端，后端】
二维码生成,可以在后端生成图片,存储在服务器,然后把虚拟路径返回到前端,也可以把数据传输到前端,通过前端框架生成在线二维码,在前端生成二维码不占用内存,但是网络原因会影响显示. 一.后端: Strin ...

从零开始搭二维激光SLAM --- 基于g2o的后端优化的代码实现