[代码实践] GTSAM 学习记录（二）：3D 位姿图优化

简介

在之前的博客中（GTSAM 学习记录（一）），我结合 2D 激光 SLAM 简单介绍了一下 GTSAM 库的基本用法，这篇博客主要是在 3D 激光 SLAM 框架中基于 GTSAM 实现激光里程计，GNSS 以及回环约束在 KITTI 数据集下进行图优化。

简单介绍了一下优化过程中涉及的节点和边。首先是节点，由于是纯激光 SLAM （没有 IMU 和视觉等），因此优化变量只有载体的位姿。因此这个图也是个位姿图。边包括三种：激光里程计产生的前后两帧的相对位姿约束，GNSS 对位姿节点的先验约束以及回环检测产生的轨迹中形成回环的两帧的相对位姿约束。如下图所示，三角形为载体位姿，红边为激光里程计对相邻两帧的位姿约束，蓝边为 GNSS 对某一帧的的位置约束（不包含姿态），绿边为回环检测到的某两帧之间的位姿约束。

关于非线性优化的原理可以参考：最小二乘问题和非线性优化。在这个问题中，图优化需要做到以下几步：

添加节点（位姿）和边（位姿或位置约素）为优化做准备
优化过程，对每条边计算误差以及对优化变量的雅可比矩阵，这里的边有两个类型（两帧节点之间的相对位姿观测，以及对单个节点的位置观测），我们需要对这两种边的误差以及雅可比计算方式进行推导
选择合适的优化策略，如 LM 或者 Dogleg 法，利用每个观测的误差和雅可比矩阵构建正规方程 $\boldsymbol{H}\Delta\boldsymbol{x} = \boldsymbol{b}$
对上述方程选择合适的方式进行求解，例如 QR 分解、Cholesky 分解等
按照需要添加鲁棒核函数，不过这里基本都是位姿之间的约束，不涉及特征，因此误匹配的情况应该比较少

接下来看 GTSAM 如何实现上述过程。

节点和边

在 GTSAM 学习记录（三）：常见李群李代数及其函数的导数和微分里面已经比较详细地将 GTSAM 中的误差和雅可比求导进行了整理，GTSAM 的文档中，也对相关的类有比较清晰的介绍。因此这里不对自带的类进行赘述。

优化中只会涉及一个节点类型，即：gtsam::Pose3。此外还有两个因子类型：BetweenFactor 和 <gtsam::PoseTranslationPrior 分别对应相对位姿约束和平移约束。

下面是添加节点和边的函数定义：GTSAM 将优化初值和图区分存放，节点初值存放在 gtsam::Values 中，因子加入 gtsam::NonlinearFactorGraph 中。

void GTSamGraphOptimizer::AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) {
    gtsam::Key key = estimates_.size();
    gtsam::Pose3 p(pose.matrix());
    estimates_.insert(key, p);

    // 由于 GTSAM 没有提供固定节点的功能，因此对需要固定的节点提供一个信息因子很大的先验约束来防止它被更新
    // for fixed vertex, add a prior with pretty small noise
    if (need_fix) {
        gtsam::Vector6 fixed_noise;
        fixed_noise << 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6;
        gtsam::noiseModel::Diagonal::shared_ptr prior_noise = gtsam::noiseModel::Diagonal::Sigmas(fixed_noise);
        graph_.addPrior(key, p, prior_noise);
    }
}

void GTSamGraphOptimizer::AddSe3Edge(int vertex_index1,
                                     int vertex_index2,
                                     const Eigen::Isometry3d &relative_pose,
                                     const Eigen::Vector3d &translation_noise,
                                     const Eigen::Vector3d &rotation_noise) {
    gtsam::Pose3 measurement(relative_pose.matrix());

    Eigen::Matrix<double, 6, 1> noise;
    noise << rotation_noise, translation_noise;
    gtsam::noiseModel::Diagonal::shared_ptr noise_model = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector6(noise));

    // 按需加入核函数
    if (need_robust_kernel_) {
        gtsam::noiseModel::Robust::shared_ptr robust_loss;
        if (robust_kernel_name_ == "Huber") {
            robust_loss = gtsam::noiseModel::Robust::Create(
                gtsam::noiseModel::mEstimator::Huber::Create(robust_kernel_size_), noise_model);
        }
        graph_.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
            vertex_index1, vertex_index2, measurement, robust_loss);
    } else {
        graph_.emplace_shared<gtsam::BetweenFactor<gtsam::Pose3>>(
            vertex_index1, vertex_index2, measurement, noise_model);
    }
}

void GTSamGraphOptimizer::AddSe3PriorXYZEdge(int se3_vertex_index,
                                             const Eigen::Vector3d &xyz,
                                             const Eigen::Vector3d &translation_noise) {
    Eigen::MatrixXd information_matrix = CalculateDiagMatrix(translation_noise);
    gtsam::Matrix3 information(information_matrix.matrix());
    gtsam::Point3 measurement(xyz.x(), xyz.y(), xyz.z());
    graph_.emplace_shared<gtsam::PoseTranslationPrior<gtsam::Pose3>>(
        se3_vertex_index, measurement, gtsam::noiseModel::Gaussian::Information(information));
}

优化过程

优化过程需要先设置好相关参数，以 LM 法为例：

gtsam::LevenbergMarquardtParams params;
params.setVerbosity("SILENT"); // 打印优化选项
params.setLinearSolverType("MULTIFRONTAL_CHOLESKY"); // 线性求解器选项
estimates_ = gtsam::LevenbergMarquardtOptimizer(graph_, estimates_, params).optimize(); // 优化，并更新初值