GTSAM入门学习

二维机器人运动

x1、x2、x3分别为机器人的三次位姿，f为每个节点间的关系，其中o为里程计的意思。

#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>

int main(int argc, char** argv)
{
    
        NonlinearFactorGraph graph;
        gtsam::Pose2 priorMean(0.0, 0.0, 0.0);
        noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
        graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
        gtsam::Pose2 odometry(2.0, 0, 0);
        noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
        graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
        graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
        Values initial;
        initial.insert(1, Pose2(0.5, 0.0, 0.2));
        initial.insert(2, Pose2(2.3, 0.1, -0.2));
        initial.insert(3, Pose2(4.1, 0.1, 0.1));
        Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
        result.at(1).print();
        result.at(2).print();
        result.at(3).print();
}

• NonlinearFactorGraph graph创建一个因子图优化对象
• gtsam::Pose2 priorMean(0.0, 0.0, 0.0) 创建二维先验位姿（x, y, yaw）
• line 3～4创建先验位姿的噪声，并把先验位姿加入因子图优化
• gtsam::Pose2 odometry(2.0, 0, 0) 创建一个delta的里程计对象，x方向为2米，y为0米，yaw为0弧度
• line 6～8 创建里程计的噪声，并把里程计作为两个节点间的变换关系(BetweenFactor<Pose2>)传入因子图优化对象内
• line 9~12 定义三个节点的初始位姿
• line13 开始优化，把优化结果存入result
• 最后打印优化结果

可以看到我们三个节点的初始位姿分别为：
(0.5, 0.0, 0.2)
(2.3, 0.1, -0.2)
(4.1, 0.1, 0.1)

经过优化后的打印结果为:
(7.46978e-16, -5.34409e-16, -1.78382e-16)
(2, -1.09237e-15, -2.48671e-16)
(4, -1.70076e-15, -2.50944e-16)