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 
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
priorNoise
noiseModel::Diagonal::shared_ptr priorNoise
Definition: doc/Code/Pose2SLAMExample.cpp:2
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
M_PI_2
#define M_PI_2
Definition: mconf.h:118
graph
NonlinearFactorGraph graph
Definition: doc/Code/Pose2SLAMExample.cpp:1


gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:36