返回

[代码实践] GTSAM 学习记录(一):2D 位姿图优化

Ceres 学习记录(一)G2O 学习记录(一) 中我分别记录了怎么使用 Ceres 和 G2O 来求解非线性最小二乘问题,在这篇文章中会结合同样的问题作为例子,学习一下 GTSAM 的基本使用方法,并和前两种优化库作对比。这篇文章只会简单介绍一下如何在编程中使用 GTSAM 求解一个最小二乘问题,关于因子图的相关知识和 GTSAM 的其他各种用法可以参考:Youtube: GTSAM Tutorial - 董靖 或者 bilibili: 泡泡机器人公开课】第五十六课:gtsam_tutorial-董靖

待求解问题

这里用的例子和 Ceres 学习记录(一) 的一样,是我在学习深蓝学院的激光slam课程第六次作业中提供的例子,这里简单介绍一下。主要是给定一系列节点和边然后进行图优化。这里的节点是机器人一系列位置(x, y, yaw),边则是前后两帧相对位姿观测值以及信息矩阵。优化过程主要是降低相对位姿观测值和由实际前后位姿计算出的预测值之间产生的误差。问题中节点的边的结构如下:

/**
 * Custom Edge and Vertex 
 */
struct myEdge
{
  int xi,xj;    // 对应节点的索引
  Eigen::Vector3d measurement;  // 两个节点之间的相对位姿的观测值
                                // 分别为:dx, dy, dtheta
  Eigen::Matrix3d infoMatrix;   // 本次观测的信息矩阵
};

typedef Eigen::Vector3d myVertex;   // 机器人位姿,x, y, theta

而在 GSTAM 上我们不是用位姿图来看待这个问题而是使用因子图来构建问题。这里不会很详细地介绍因子图的概念,在这个问题上,目前我们可以简单的将其理解为:

  • 位姿图中的位姿节点:相当于因子图中的变量节点,每个变量以一个高斯分布的形式表示,主要参数为均值和方差
  • 位姿图中的边(观测):相当于因子图中的因子节点,每个因子同样是一个高斯分布的形式表示,主要参数为均值和方差

在这种不严谨地假设下,我们可以来看一下 GTSAM 的基本使用过程:

GTSAM 基本使用方法

这里由于 GTSAM 已经自定义好了大量节点类型可供使用,在这里直接使用 gtsam::Pose2 作为变量节点类型。

GTSAM 的整体使用过程如下:

// 相关头文件
// 变量类型
#include <gtsam/geometry/Pose2.h>
// 变量节点索引类型
#include <gtsam/inference/Key.h>
// 因子类型
#include <gtsam/slam/BetweenFactor.h>
// 变量节点类型
#include <gtsam/nonlinear/Values.h>
// 非线性因子图
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// 优化器头文件,这里选用高斯牛顿
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>


/// 获取我们需要优化的节点和边
std::vector<myVertex> Vertexs;
std::vector<myEdge> Edges;

// 初始化因子图
gtsam::NonlinearFactorGraph graph;

// 设定第一个节点初值
gtsam::noiseModel::Diagonal::shared_ptr priorNoise =\
         gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.2, 0.2, 0.1));
graph.addPrior(0, gtsam::Pose2(0, 0, 0), priorNoise);

// 将因子节点添加进因子图中
for (const auto& edge: edges) {
    gtsam::Matrix sqrtInfo = edge.infoMatrix.array().sqrt();
    gtsam::Pose2 measurement(edge.measurement(0),
                            edge.measurement(1),
                            edge.measurement(2));
    graph.emplace_shared<gtsam::BetweenFactor<gtsam::Pose2>>(edge.xi, edge.xj,
                    measurement,
                    gtsam::noiseModel::Gaussian::SqrtInformation(sqrtInfo));
}

// 设定各个变量节点的初值
gtsam::Values initialEstimate;
for (size_t i = 0; i < vertexes.size(); i++) {
    gtsam::Pose2 v(vertexes[i](0), vertexes[i](1), vertexes[i](2));
    initialEstimate.insert(i, v);
}

// 使用高斯牛顿法进行优化,设定最大迭代次数和迭代终止条件
gtsam::GaussNewtonParams parameters;
parameters.relativeErrorTol = 1e-6;
parameters.maxIterations = 10;
parameters.setVerbosity("SILENT");
gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);

// 进行优化,获得优化后的各个变量节点
gtsam::Values result = optimizer.optimize();

// (可选)将优化的变量节点的均值更新到原来的节点向量中
for (const auto& value: result) {
    gtsam::Pose2 vp = value.value.cast<gtsam::Pose2>();
    vertexes[value.key](0) = vp.x();
    vertexes[value.key](1) = vp.y();
    vertexes[value.key](2) = vp.theta();
}

整体的过程和 G2O 有点类似,都是:

  • 确定要使用的节点类型
  • 初始化图
  • 固定第一个节点初值(包括均值和方差)
  • 添加因子,包括相关的节点(位姿)索引,均值(相对位姿误差观测值)和信息矩阵(作为模型噪声)
  • 添加变量节点初值(待估计位姿)
  • 选择相应的优化器,这里用的是高斯牛顿法
  • 设定优化器参数,包括结束条件,最大迭代次数,输出信息选项等等
  • 进行优化

结果

输出结果如下所示:

initError: 3.08592e+08
Takes :0.250357 Seconds.
FinalError: 10344.7

优化过程使用了 0.25 秒左右,和使用 G2O (自带类型耗时 0.3 秒)差距不大,比 Ceres 快一些。这大概是合理的,因为 Ceres 更加注重于求解通用的最小二乘问题,而 G2O 和 GTSAM 都对 SLAM 相关问题进行了高度定制化。

Built with Hugo
Theme Stack designed by Jimmy