二维机器人运动
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)