4. 构建带路标点的因子图

任务目标

在因子图框架中引入路标点（Landmark）节点，并添加相机/传感器对路标点的观测约束，构建包含机器人位姿节点、路标点节点及两类因子（里程计因子、观测因子）的完整SLAM因子图。本案例使用GTSAM库实现，最终输出因子图的dot文件与关键统计信息，用于验证图结构正确性。

环境依赖

GTSAM 4.x（推荐4.2及以上）
C++17编译器（如g++ 9+）
CMake 3.16+（可选，直接编译亦可）

核心实现

#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/KeyFormatter.h>
#include <fstream>
#include <iostream>

using namespace gtsam;
using symbol_shorthand::X;  // 位姿节点: X1, X2, ...
using symbol_shorthand::L;  // 路标节点: L1, L2, ...

int main() {
    // 1. 构建因子图
    NonlinearFactorGraph graph;

    // 2. 添加里程计因子（连接相邻位姿）
    // 机器人从X1出发，依次运动到X2、X3
    Pose2 odometry1(1.0, 0.0, 0.0);   // X1 -> X2: 沿x轴前进1m
    Pose2 odometry2(0.0, 1.0, 0.0);   // X2 -> X3: 沿y轴前进1m
    auto noise_odom = noiseModel::Diagonal::Sigmas(
        (Vector(3) << 0.1, 0.1, 0.05).finished());

    graph.add(BetweenFactor<Pose2>(X(1), X(2), odometry1, noise_odom));
    graph.add(BetweenFactor<Pose2>(X(2), X(3), odometry2, noise_odom));

    // 3. 添加路标点观测因子（BearingRange: 方位角+距离）
    // 路标点L1被X1、X2观测；路标点L2被X2、X3观测
    auto noise_br = noiseModel::Diagonal::Sigmas(
        (Vector(2) << 0.05, 0.1).finished());  // 角度噪声0.05rad, 距离噪声0.1m

    // 真实路标位置（用于生成观测值，实际SLAM中来自传感器）
    Point2 landmark1_true(2.0, 1.0);
    Point2 landmark2_true(1.5, 2.5);

    // 从X1观测L1: 假设X1在原点(0,0)
    Pose2 x1_pose(0.0, 0.0, 0.0);
    Rot2 bearing1 = x1_pose.rotation().rotate(
        (landmark1_true - x1_pose.translation()).angle());
    double range1 = (landmark1_true - x1_pose.translation()).norm();
    graph.add(BearingRangeFactor<Pose2, Point2>(
        X(1), L(1), bearing1, range1, noise_br));

    // 从X2观测L1和L2: X2在(1,0)
    Pose2 x2_pose(1.0, 0.0, 0.0);
    Rot2 bearing2_l1 = x2_pose.rotation().rotate(
        (landmark1_true - x2_pose.translation()).angle());
    double range2_l1 = (landmark1_true - x2_pose.translation()).norm();
    graph.add(BearingRangeFactor<Pose2, Point2>(
        X(2), L(1), bearing2_l1, range2_l1, noise_br));

    Rot2 bearing2_l2 = x2_pose.rotation().rotate(
        (landmark2_true - x2_pose.translation()).angle());
    double range2_l2 = (landmark2_true - x2_pose.translation()).norm();
    graph.add(BearingRangeFactor<Pose2, Point2>(
        X(2), L(2), bearing2_l2, range2_l2, noise_br));

    // 从X3观测L2: X3在(1,1)
    Pose2 x3_pose(1.0, 1.0, 0.0);
    Rot2 bearing3 = x3_pose.rotation().rotate(
        (landmark2_true - x3_pose.translation()).angle());
    double range3 = (landmark2_true - x3_pose.translation()).norm();
    graph.add(BearingRangeFactor<Pose2, Point2>(
        X(3), L(2), bearing3, range3, noise_br));

    // 4. 添加先验因子（固定第一个位姿，消除自由度）
    auto noise_prior = noiseModel::Diagonal::Sigmas(
        (Vector(3) << 0.01, 0.01, 0.01).finished());
    graph.addPrior(X(1), x1_pose, noise_prior);

    // 5. 设置初始值（用于后续优化，本案例仅验证图结构）
    Values initial;
    initial.insert(X(1), x1_pose);
    initial.insert(X(2), x2_pose);
    initial.insert(X(3), x3_pose);
    initial.insert(L(1), Point2(1.8, 0.8));  // 带噪声的初始估计
    initial.insert(L(2), Point2(1.2, 2.2));

    // 6. 输出因子图结构（dot格式）
    std::ofstream dot_file("landmark_factor_graph.dot");
    graph.saveGraph(dot_file, initial);
    dot_file.close();

    // 7. 打印图统计信息
    std::cout << "因子图统计:" << std::endl;
    std::cout << "  因子总数: " << graph.size() << std::endl;
    std::cout << "  位姿节点数: 3 (X1, X2, X3)" << std::endl;
    std::cout << "  路标节点数: 2 (L1, L2)" << std::endl;
    std::cout << "  里程计因子: 2" << std::endl;
    std::cout << "  观测因子: 4" << std::endl;
    std::cout << "  先验因子: 1" << std::endl;
    std::cout << "  图结构已保存至 landmark_factor_graph.dot" << std::endl;

    // 8. 可选：执行优化验证图结构可解
    LevenbergMarquardtOptimizer optimizer(graph, initial);
    Values result = optimizer.optimize();
    std::cout << "\n优化后路标点估计:" << std::endl;
    std::cout << "  L1: " << result.at<Point2>(L(1)).transpose() << std::endl;
    std::cout << "  L2: " << result.at<Point2>(L(2)).transpose() << std::endl;

    return 0;
}

编译与运行

# 编译（假设GTSAM已安装）
g++ -std=c++17 landmark_graph.cpp -o landmark_graph \
    $(pkg-config --cflags --libs gtsam)

# 运行
./landmark_graph

验证方法

图结构验证：程序输出因子总数应为7（2里程计+4观测+1先验），节点数5（3位姿+2路标）。
可视化验证：打开生成的landmark_factor_graph.dot文件，应看到X1-X2-X3通过里程计因子（黑色边）串联，L1连接X1和X2，L2连接X2和X3，观测因子以不同颜色边表示。
可解性验证：优化器应正常收敛，优化后L1接近真实值(2.0, 1.0)，L2接近(1.5, 2.5)。由于观测噪声设置较小且初始值接近，误差应在0.1m量级内。

预期输出（确定性部分）

因子图统计:
  因子总数: 7
  位姿节点数: 3 (X1, X2, X3)
  路标节点数: 2 (L1, L2)
  里程计因子: 2
  观测因子: 4
  先验因子: 1
  图结构已保存至 landmark_factor_graph.dot

优化后的路标点数值取决于GTSAM的迭代收敛过程，属于数值计算结果而非确定性输出，此处不预先给出具体数值。

图中蓝色圆形表示机器人位姿节点（X1~X3），红色圆形表示路标点节点（L1、L2）。黑色实线为里程计因子，连接相邻位姿；彩色虚线为BearingRange观测因子，连接位姿与路标点。X1处附加先验因子固定全局参考系。该图展示了SLAM中位姿-路标联合估计的典型拓扑结构。

📚 课程目录