doc/Code/OdometryExample.cpp
Go to the documentation of this file.
1 // Create an empty nonlinear factor graph
2 NonlinearFactorGraph graph;
3 
4 // Add a Gaussian prior on pose x_1
5 Pose2 priorMean(0.0, 0.0, 0.0);
6 auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
7 graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
8 
9 // Add two odometry factors
10 Pose2 odometry(2.0, 0.0, 0.0);
11 auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
12 graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
13 graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
Eigen::Vector3d Vector3
Definition: Vector.h:43
NonlinearFactorGraph graph
auto odometryNoise
Pose2 priorMean(0.0, 0.0, 0.0)
auto priorNoise
Pose2 odometry(2.0, 0.0, 0.0)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:58