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
关于karto中位姿图的节点及边的讲解, 在上一篇文章中已经介绍完了, 感兴趣的同学可以看看.
从零开始搭二维激光SLAM — Karto后端的位姿图相关的数据结构分析
https://blog.csdn.net/tiancailx/article/details/121317039
首先, 声明一个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;
};
g2o的初始化基本就这4个步骤, 上边的第一篇文章中已经讲的很清楚了, 我就不多说了.
将优化器的初始化放在构造函数中了, 使用LinearSolverCSparse线性求解器, 使用了LM下降算法.
typedef g2o::BlockSolver<g2o::BlockSolverTraits<-1, -1>> SlamBlockSolver;
typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearSolver;
G2oSolver::G2oSolver()
{
// 第1步:创建一个线性求解器LinearSolver
SlamLinearSolver *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);
}
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());
}
首先, 先声明一个 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);
}
进行求解, 并将优化后的位姿进行保存.
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!");
}
}
}
这篇文章的代码是需要依赖 kinetic 版本的 g2o的, 需要通过如下指令进行安装.
sudo apt-get install ros-kinetic-libg2o
没有测试过 ubuntu1804的g2o, 不知道能不能编译通过.
本篇文章对应的数据包, 请在我的公众号中回复 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的后端优化与回环检测功能对比测试与分析 这篇文章分析过, 如果不加回环和优化功能, 会在最后产生十分明显的叠图.
这篇文章简要说明了如何使用g2o进行karto的优化的计算, 只是定性的分析了一下g2o优化确实产生了效果, 如果有同学想要自己定量的分析可以自己更改下代码做一下分析.
下篇文章将使用ceres来进行后端优化的计算.