LocalizationExample.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
26 // We will use Pose2 variables (x, y, theta) to represent the robot positions
27 #include <gtsam/geometry/Pose2.h>
28 
29 // We will use simple integer Keys to refer to the robot poses.
30 #include <gtsam/inference/Key.h>
31 
32 // As in OdometryExample.cpp, we use a BetweenFactor to model odometry measurements.
34 
35 // We add all facors to a Nonlinear Factor Graph, as our factors are nonlinear.
37 
38 // The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
39 // nonlinear functions around an initial linearization point, then solve the linear system
40 // to update the linearization point. This happens repeatedly until the solver converges
41 // to a consistent set of variable values. This requires us to specify an initial guess
42 // for each variable, held in a Values container.
43 #include <gtsam/nonlinear/Values.h>
44 
45 // Finally, once all of the factors have been added to our factor graph, we will want to
46 // solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
47 // GTSAM includes several nonlinear optimizers to perform this step. Here we will use the
48 // standard Levenberg-Marquardt solver
50 
51 // Once the optimized values have been calculated, we can also calculate the marginal covariance
52 // of desired variables
54 
55 using namespace std;
56 using namespace gtsam;
57 
58 // Before we begin the example, we must create a custom unary factor to implement a
59 // "GPS-like" functionality. Because standard GPS measurements provide information
60 // only on the position, and not on the orientation, we cannot use a simple prior to
61 // properly model this measurement.
62 //
63 // The factor will be a unary factor, affect only a single system variable. It will
64 // also use a standard Gaussian noise model. Hence, we will derive our new factor from
65 // the NoiseModelFactorN.
67 
68 class UnaryFactor: public NoiseModelFactorN<Pose2> {
69  // The factor will hold a measurement consisting of an (X,Y) location
70  // We could this with a Point2 but here we just use two doubles
71  double mx_, my_;
72 
73  public:
74 
75  // Provide access to Matrix& version of evaluateError:
76  using NoiseModelFactor1<Pose2>::evaluateError;
77 
79  typedef std::shared_ptr<UnaryFactor> shared_ptr;
80 
81  // The constructor requires the variable key, the (X, Y) measurement value, and the noise model
82  UnaryFactor(Key j, double x, double y, const SharedNoiseModel& model):
83  NoiseModelFactorN<Pose2>(model, j), mx_(x), my_(y) {}
84 
85  ~UnaryFactor() override {}
86 
87  // Using the NoiseModelFactorN base class there are two functions that must be overridden.
88  // The first is the 'evaluateError' function. This function implements the desired measurement
89  // function, returning a vector of errors when evaluated at the provided variable value. It
90  // must also calculate the Jacobians for this measurement function, if requested.
91  Vector evaluateError(const Pose2& q, OptionalMatrixType H) const override {
92  // The measurement function for a GPS-like measurement h(q) which predicts the measurement (m) is h(q) = q, q = [qx qy qtheta]
93  // The error is then simply calculated as E(q) = h(q) - m:
94  // error_x = q.x - mx
95  // error_y = q.y - my
96  // Node's orientation reflects in the Jacobian, in tangent space this is equal to the right-hand rule rotation matrix
97  // H = [ cos(q.theta) -sin(q.theta) 0 ]
98  // [ sin(q.theta) cos(q.theta) 0 ]
99  const Rot2& R = q.rotation();
100  if (H) (*H) = (gtsam::Matrix(2, 3) << R.c(), -R.s(), 0.0, R.s(), R.c(), 0.0).finished();
101  return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
102  }
103 
104  // The second is a 'clone' function that allows the factor to be copied. Under most
105  // circumstances, the following code that employs the default copy constructor should
106  // work fine.
108  return std::static_pointer_cast<gtsam::NonlinearFactor>(
110 
111  // Additionally, we encourage you the use of unit testing your custom factors,
112  // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
113  // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
114 }; // UnaryFactor
115 
116 
117 int main(int argc, char** argv) {
118  // 1. Create a factor graph container and add factors to it
120 
121  // 2a. Add odometry factors
122  // For simplicity, we will use the same noise model for each odometry factor
123  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
124  // Create odometry (Between) factors between consecutive poses
127 
128  // 2b. Add "GPS-like" measurements
129  // We will use our custom UnaryFactor for this.
130  auto unaryNoise =
131  noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
135  graph.print("\nFactor Graph:\n"); // print
136 
137  // 3. Create the data structure to hold the initialEstimate estimate to the solution
138  // For illustrative purposes, these have been deliberately set to incorrect values
139  Values initialEstimate;
140  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
141  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
142  initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
143  initialEstimate.print("\nInitial Estimate:\n"); // print
144 
145  // 4. Optimize using Levenberg-Marquardt optimization. The optimizer
146  // accepts an optional set of configuration parameters, controlling
147  // things like convergence criteria, the type of linear system solver
148  // to use, and the amount of information displayed during optimization.
149  // Here we will use the default set of parameters. See the
150  // documentation for the full set of parameters.
151  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
152  Values result = optimizer.optimize();
153  result.print("Final Result:\n");
154 
155  // 5. Calculate and print marginal covariances for all variables
157  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
158  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
159  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
160 
161  return 0;
162 }
H
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
Definition: gnuplot_common_settings.hh:74
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:78
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::Vector2
Eigen::Vector2d Vector2
Definition: Vector.h:42
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
x
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Definition: gnuplot_common_settings.hh:12
gtsam::Marginals
Definition: Marginals.h:32
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::Marginals::marginalCovariance
Matrix marginalCovariance(Key variable) const
Definition: Marginals.cpp:133
UnaryFactor::~UnaryFactor
~UnaryFactor() override
Definition: LocalizationExample.cpp:85
Key.h
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
BetweenFactor.h
gtsam::Rot2::s
double s() const
Definition: Rot2.h:202
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
UnaryFactor::evaluateError
Vector evaluateError(const Pose2 &q, OptionalMatrixType H) const override
Definition: LocalizationExample.cpp:91
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
y
Scalar * y
Definition: level1_cplx_impl.h:124
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Rot2
Definition: Rot2.h:35
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
std
Definition: BFloat16.h:88
odometryNoise
auto odometryNoise
Definition: doc/Code/OdometryExample.cpp:11
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
gtsam::Rot2::c
double c() const
Definition: Rot2.h:197
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
main
int main(int argc, char **argv)
Definition: LocalizationExample.cpp:117
marginals
Marginals marginals(graph, result)
UnaryFactor
Definition: LocalizationFactor.cpp:1
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
UnaryFactor::shared_ptr
std::shared_ptr< UnaryFactor > shared_ptr
shorthand for a smart pointer to a factor
Definition: LocalizationExample.cpp:79
UnaryFactor::UnaryFactor
UnaryFactor(Key j, double x, double y, const SharedNoiseModel &model)
Definition: LocalizationExample.cpp:82
ceres::Vector
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
Definition: gtsam/3rdparty/ceres/eigen.h:38
R
Rot2 R(Rot2::fromAngle(0.1))
Values.h
A non-templated config holding any types of Manifold-group elements.
UnaryFactor::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: LocalizationExample.cpp:107
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::Pose2
Definition: Pose2.h:39
unaryNoise
auto unaryNoise
Definition: LocalizationExample2.cpp:2
gtsam::BetweenFactor
Definition: BetweenFactor.h:40
gtsam::NonlinearFactorGraph::print
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: NonlinearFactorGraph.cpp:55


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:09