doc/Code/Pose2SLAMExample.cpp
Go to the documentation of this file.
1 NonlinearFactorGraph graph;
2 noiseModel::Diagonal::shared_ptr priorNoise =
3  noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
4 graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
5 
6 // Add odometry factors
7 noiseModel::Diagonal::shared_ptr model =
8  noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
9 graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
10 graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
11 graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
12 graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
13 
14 // Add the loop closure constraint
15 graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
16 
Eigen::Vector3d Vector3
Definition: Vector.h:43
noiseModel::Diagonal::shared_ptr model
noiseModel::Diagonal::shared_ptr priorNoise
NonlinearFactorGraph graph


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:14