视觉SLAM笔记（56） 位姿图优化
该文本详细探讨了视觉SLAM中的位姿图优化问题，分为三部分：
使用g2o对位姿图进行优化，并展示了其在仿真数据下的表现；
通过李代数实现顶点和边的表示，并比较了不同雅可比计算方法的效果；
强调了后端优化的重要性及其在实际中的应用价值。
摘要需涵盖核心内容：基于g2o和李代数的位姿图优化方法及其效果分析。
视觉SLAM笔记（56） 位姿图优化
- 1. g2o 原生位姿图
- 2. 李代数上的位姿图优化
- 3. 关于后端优化
1. g2o 原生位姿图
请看以下步骤演示如何使用 g2o 实现位姿图优化：首先，请运行 g2o_viewer 软件并打开已经预先生成的仿真位姿图文件 sphere.g2o
$ g2o_viewer sphere.g2o
AI助手如图所示：
该位姿图基于 g2o 提供的 create_sphere 程序进行仿真生成。
其真实运动轨迹呈球形，并由分层结构构成，每一层均呈现圆形特征。
每一分层均为规则圆形结构，在不同尺寸的基础上相互叠加形成了完整且对称的整体球体。
这些圆环组合在一起不仅数量众多（共 2500 个位姿节点），而且呈现出环绕上升的特点。
然后，在时间区间t − 1到t之间创建了一系列连接节点的边项，在此过程中特别标记出了一类具有里程计特性的边项称为odometry边项（里程计）。与此同时，在整个系统运行过程中还形成了层与层之间的连接关系这一类特殊的边项被命名为loop closure（回环）。随后的操作是在每一类边上引入观测噪声元素，并依据odometry边项所具有的噪声特性对相关节点重新设定初始位置参数值。通过上述操作流程最终构建出了一个包含累计误差特征的空间定位图数据集。该数据集在局部空间呈现类似于球体表面的一小块区域特征但它整体呈现出显著偏离球体形态的整体结构特征
基于这些带噪声的边和节点初始值的基础上，
通过优化整个位姿图
获得近似真值的数据
然而
实际当中的机器人不可能呈现这样理想化的运动轨迹
以及完整的里程计与回环观测数据
假设立足于一个完美的正球模型
仿真有助于直观判断优化结果是否正确
只需观察其各角度是否呈圆形即可
建议运行g2o_viewer中的优化功能。
VERTEX_SE3:QUAT 0 -0.125664 -1.53894e-17 99.9999 0.706662 4.32706e-17 0.707551 -4.3325e-17
...
EDGE_SE3:QUAT 2449 2499 -0.186998 -0.0161808 -6.29417 -0.00401022 0.0316215 -0.00624077 0.999472 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 40000 0 0 40000 0 40000
AI助手可以看出该节点类型被定义为 VERTEX_SE3 并用于表示相机的位姿。该框架默认通过四元数和平移向量来描述位置与姿态。这些字段对应于 ID、平移分量（tx、ty、tz）和旋转四元数（qx、qy、qz、qw）。其中前三个参数是平移分量（tx、ty、tz），后四个参数则构成单位四元数以描述旋转部分。
此外，在本实现中，默认情况下我们采用了对角信息矩阵。 由于信息矩阵是对称的特性，在存储时仅需关注其下半部分元素。 注意到的是，在本实现中，默认情况下我们采用了对角信息矩阵。
为了实现该位姿图的优化目标, 能够采用 g2o 默认设置下的顶点和边. 因此, 在利用g2o进行优化时, 并不需要额外做过多的工作. 这是因为仿真数据同样是g2o生成的, 所以在利用g2o进行优化时, 只需根据具体情况调整一些关键参数设置即可.
程序 pose_graph_g2o_SE3.cpp 采用L-M优化算法对该姿态图进行优化处理，并将最终结果保存为result.g2o文件
#include <iostream>
#include <fstream>
#include <string>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
using namespace std;
int main(int argc, char** argv)
{
if (argc != 2)
{
cout << "Usage: pose_graph_g2o_SE3 sphere.g2o" << endl;
return 1;
}
ifstream fin(argv[1]);
if (!fin)
{
cout << "file " << argv[1] << " does not exist." << endl;
return 1;
}
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> Block; // 6x6 BlockSolver
Block::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<Block::PoseMatrixType>(); // 线性方程求解器
Block* solver_ptr = new Block(std::unique_ptr<Block::LinearSolverType>(linearSolver)); // 矩阵块求解器
// 梯度下降方法，从GN, LM, DogLeg 中选
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr<Block>(solver_ptr));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
while (!fin.eof())
{
string name;
fin >> name;
if (name == "VERTEX_SE3:QUAT")
{
// SE3 顶点
g2o::VertexSE3* v = new g2o::VertexSE3();
int index = 0;
fin >> index;
v->setId(index);
v->read(fin);
optimizer.addVertex(v);
vertexCnt++;
if (index == 0)
v->setFixed(true);
}
else if (name == "EDGE_SE3:QUAT")
{
// SE3-SE3 边
g2o::EdgeSE3* e = new g2o::EdgeSE3();
int idx1, idx2; // 关联的两个顶点
fin >> idx1 >> idx2;
e->setId(edgeCnt++);
e->setVertex(0, optimizer.vertices()[idx1]);
e->setVertex(1, optimizer.vertices()[idx2]);
e->read(fin);
optimizer.addEdge(e);
}
if (!fin.good()) break;
}
cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;
cout << "prepare optimizing ..." << endl;
optimizer.setVerbose(true);
optimizer.initializeOptimization();
cout << "calling optimizing ..." << endl;
optimizer.optimize(30);
cout << "saving optimization results ..." << endl;
optimizer.save("result.g2o");
return 0;
}
AI助手采用了6 \times 6块求解器，并采用了Levenberg-Marquardt（LM）下降方法，在迭代次数设定为30次的情况下执行运算
$ ./pose_graph_g2o_SE3 ../sphere.g2o
AI助手
然后，用 g2o_viewer 打开 result.g2o 查看结果
$ g2o_viewer result.g2o
AI助手
该系统实现了将非标准形状优化为外观上完整的球体。
整个流程基本上等同于点击g2o_viewer中的Optimize按钮
2. 李代数上的位姿图优化
在接下来的内容中，请基于前面完成的李代数推导部分，在此代码文件pose_graph_g2o_lie_algebra.cpp中构建自定义的顶点和边类，并尝试将Sophus库应用于g2o框架以实现李代数域上的优化工作
尝试将Sophus库导入g2o框架，在代码文件pose_graph_g2o_lie_algebra.cpp$中定义并实现相应的顶点和边类
#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Core>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <sophus/se3.h>
#include <sophus/so3.h>
using namespace std;
using Sophus::SE3;
using Sophus::SO3;
typedef Eigen::Matrix<double, 6, 6> Matrix6d;
// 给定误差求J_R^{-1}的近似
Matrix6d JRInv(SE3 e)
{
Matrix6d J;
J.block(0, 0, 3, 3) = SO3::hat(e.so3().log());
J.block(0, 3, 3, 3) = SO3::hat(e.translation());
J.block(3, 0, 3, 3) = Eigen::Matrix3d::Zero(3, 3);
J.block(3, 3, 3, 3) = SO3::hat(e.so3().log());
J = J * 0.5 + Matrix6d::Identity();
return J;
}
// 李代数顶点
typedef Eigen::Matrix<double, 6, 1> Vector6d;
class VertexSE3LieAlgebra : public g2o::BaseVertex<6, SE3>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
bool read(istream& is)
{
double data[7];
for (int i = 0; i < 7; i++)
is >> data[i];
setEstimate(SE3(
Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
Eigen::Vector3d(data[0], data[1], data[2])
));
}
bool write(ostream& os) const
{
os << id() << " ";
Eigen::Quaterniond q = _estimate.unit_quaternion();
os << _estimate.translation().transpose() << " ";
os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << endl;
return true;
}
virtual void setToOriginImpl()
{
_estimate = Sophus::SE3();
}
// 左乘更新
virtual void oplusImpl(const double* update)
{
Sophus::SE3 up(
Sophus::SO3(update[3], update[4], update[5]),
Eigen::Vector3d(update[0], update[1], update[2])
);
_estimate = up * _estimate;
}
};
// 两个李代数节点之边
class EdgeSE3LieAlgebra : public g2o::BaseBinaryEdge<6, SE3, VertexSE3LieAlgebra, VertexSE3LieAlgebra>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
bool read(istream& is)
{
double data[7];
for (int i = 0; i < 7; i++)
is >> data[i];
Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
q.normalize();
setMeasurement(
Sophus::SE3(q, Eigen::Vector3d(data[0], data[1], data[2]))
);
for (int i = 0; i < information().rows() && is.good(); i++)
for (int j = i; j < information().cols() && is.good(); j++)
{
is >> information() (i, j);
if (i != j)
information() (j, i) = information() (i, j);
}
return true;
}
bool write(ostream& os) const
{
VertexSE3LieAlgebra* v1 = static_cast<VertexSE3LieAlgebra*> (_vertices[0]);
VertexSE3LieAlgebra* v2 = static_cast<VertexSE3LieAlgebra*> (_vertices[1]);
os << v1->id() << " " << v2->id() << " ";
SE3 m = _measurement;
Eigen::Quaterniond q = m.unit_quaternion();
os << m.translation().transpose() << " ";
os << q.coeffs()[0] << " " << q.coeffs()[1] << " " << q.coeffs()[2] << " " << q.coeffs()[3] << " ";
// information matrix
for (int i = 0; i < information().rows(); i++)
for (int j = i; j < information().cols(); j++)
{
os << information() (i, j) << " ";
}
os << endl;
return true;
}
// 误差计算与书中推导一致
virtual void computeError()
{
Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
_error = (_measurement.inverse()*v1.inverse()*v2).log();
}
// 雅可比计算
virtual void linearizeOplus()
{
Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
Matrix6d J = JRInv(SE3::exp(_error));
// 尝试把J近似为I？
_jacobianOplusXi = -J * v2.inverse().Adj();
_jacobianOplusXj = J * v2.inverse().Adj();
}
};
AI助手为了开发对g2o文件进行存储与读取操作，并实现了read和write函数；
同时将该顶点模拟成g2o内置SE3顶点的形式，
从而使g2o_viewer能够识别并显示该顶点；
然而在外部看来却看不出任何不同，
值得特别指出的是，在这一部分中涉及到了雅可比矩阵的计算过程；
这些方案各有优劣：
- 避免显式定义雅可比函数, 通过数值方法自动求解雅可比矩阵
- 采用精确或近似的方法计算雅可比矩阵
这里采用JRInv()函数来生成近似值Jr⁻¹。我们可以考虑将其近似为单位矩阵I，并且可以选择关闭oplusImpl函数以观察到的影响。随后使用g2o来进行优化问题：
$ ./pose_graph_g2o_lie ../sphere.g2o
AI助手
发现，迭代 23 次后，总体误差保持不变，事实上可以让优化算法停止了
而上一个实验中用满了 30 次迭代后误差仍在下降
在执行优化后审视 result_lie.g2o 的结果时, 视觉上几乎无异
如果在这个 g2o_viewer 操作界面中点击 Optimize 按钮，则会启动基于其自带的 SE3 顶点进行优化操作。在下方提供的文本输入框中可以查看详细的优化参数设置情况。
经计算得出，在SE3边上定义的整体误差数值为44,360（即略低于），这略微低于之前的30次迭代所得值（即44,811）。研究表明，在应用李代数进行优化的过程中，在较之之前的较少迭代次数中取得了更为显著的效果。
3. 关于后端优化
球作为实例具有高度代表性
该实例在结构上与典型的里程计边缘（Odometry）及回环边缘（Loop Closure）相匹配
这正是SLAM技术中常见的位姿图构成元素
同时，在计算资源方面表现出一定的复杂性特征：总共包含约2,500个位姿节点以及约1万条连接它们的边
从性能优化角度来看，该实例的表现尚欠理想
另一方面而言，在无需预设机器人运动方式的前提下难以深入探讨其稀疏特性。普遍认为位姿图是最简单结构的图类，在这一前提下我们可以观察到其独特的分布特征：当机器人以直线路径向前移动时会呈现出带状布局的位姿分布；而当其执行"左、右手配合缓慢动作"时则会形成多个小型回环需经过优化处理（Loopy motion），最终呈现出类似球体那样的密集型位姿分布模式
不管怎样，在缺乏进一步信息的情况下，不再能够有效地应用位姿图的求解结构。
自从PTAM提出以来, 认识到的是, 后端的优化并非必须实时回应前端的图像数据。人们更习惯性地将前后端分离, 运行于两个独立线程之中。历史上被称作跟踪与建图。尽管如此称呼, 建图部分主要涉及的是对后端的优化工作。形象地说, 前端必须实时处理视频流如每秒30Hz。而这一过程可以较为缓慢地进行, 只要等到完成时就将结果传递给前端即可。因此通常不会对后端的优化设置过高的速度要求
参考：
相关推荐：
[视觉SLAM笔记（55） 位姿图]
该文主要探讨了位姿图的相关技术及其在视觉SLAM中的应用前景。
[视觉SLAM笔记（54） Ceres 操作后端优化]
本节详细阐述了Ceres框架在后端优化过程中的具体实现方法。
[视觉SLAM笔记（53） g2o 操作后端优化]
文章重点分析了g2o算法在解决复杂场景下的路径规划问题中的有效性。
[视觉SLAM笔记（52） BA 与图优化]
通过深入讨论BA算法与图优化技术之间的关系, 提升了整体系统的运行效率。
[视觉SLAM笔记（51） 非线性系统和 EKF]
本文深入探讨了非线性系统及其在EKF滤波器中的应用原理。
谢谢！