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
12/**
13 * @file simulated2D.h
14 * @brief measurement functions and derivatives for simulated 2D robot
15 * @author Frank Dellaert
16 */
17
18// \callgraph
19#pragma once
20
21#include <gtsam/geometry/Pose2.h>
22#include <gtsam/nonlinear/Values.h>
23#include <gtsam/nonlinear/NonlinearFactor.h>
24#include <gtsam/nonlinear/NonlinearFactorGraph.h>
25#include "gtsam/base/OptionalJacobian.h"
26
27// \namespace
28namespace simulated2DOriented {
29
30 using namespace gtsam;
31
32 /// Specialized Values structure with syntactic sugar for
33 /// compatibility with matlab
34 class Values: public gtsam::Values {
35 int nrPoses_, nrPoints_;
36 public:
37 Values() : nrPoses_(0), nrPoints_(0) {}
38
39 /// insert a pose
40 void insertPose(Key i, const Pose2& p) {
41 insert(j: i, val: p);
42 nrPoses_++;
43 }
44
45 /// insert a landmark
46 void insertPoint(Key j, const Point2& p) {
47 insert(j, val: p);
48 nrPoints_++;
49 }
50
51 int nrPoses() const { return nrPoses_; } ///< nr of poses
52 int nrPoints() const { return nrPoints_; } ///< nr of landmarks
53
54 Pose2 pose(Key i) const { return at<Pose2>(j: i); } ///< get a pose
55 Point2 point(Key j) const { return at<Point2>(j); } ///< get a landmark
56 };
57
58 //TODO:: point prior is not implemented right now
59
60 /// Prior on a single pose
61 inline Pose2 prior(const Pose2& x) {
62 return x;
63 }
64
65 /// Prior on a single pose, optional derivative version
66 Pose2 prior(const Pose2& x, OptionalJacobian<3,3> H = OptionalNone) {
67 if (H) *H = I_3x3;
68 return x;
69 }
70
71 /// odometry between two poses
72 inline Pose2 odo(const Pose2& x1, const Pose2& x2) {
73 return x1.between(g: x2);
74 }
75
76 /// odometry between two poses, optional derivative version
77 Pose2 odo(const Pose2& x1, const Pose2& x2, OptionalJacobian<3,3> H1 =
78 OptionalNone, OptionalJacobian<3,3> H2 = OptionalNone) {
79 return x1.between(g: x2, H1, H2);
80 }
81
82 /// Unary factor encoding a soft prior on a vector
83 template<class VALUE = Pose2>
84 struct GenericPosePrior: public NoiseModelFactorN<VALUE> {
85
86 Pose2 measured_; ///< measurement
87
88 /// Create generic pose prior
89 GenericPosePrior(const Pose2& measured, const SharedNoiseModel& model, Key key) :
90 NoiseModelFactorN<VALUE>(model, key), measured_(measured) {
91 }
92
93 /// Evaluate error and optionally derivative
94 Vector evaluateError(const Pose2& x, OptionalMatrixType H =
95 OptionalNone) const {
96 return measured_.localCoordinates(g: prior(x, H));
97 }
98
99 };
100
101 /**
102 * Binary factor simulating "odometry" between two Vectors
103 */
104 template<class VALUE = Pose2>
105 struct GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
106 Pose2 measured_; ///< Between measurement for odometry factor
107
108 typedef GenericOdometry<VALUE> This;
109
110 // Provide access to the Matrix& version of evaluateError:
111 using NoiseModelFactor2<VALUE, VALUE>::evaluateError;
112
113 /**
114 * Creates an odometry factor between two poses
115 */
116 GenericOdometry(const Pose2& measured, const SharedNoiseModel& model,
117 Key i1, Key i2) :
118 NoiseModelFactorN<VALUE, VALUE>(model, i1, i2), measured_(measured) {
119 }
120
121 ~GenericOdometry() override {}
122
123 /// Evaluate error and optionally derivative
124 Vector evaluateError(const VALUE& x1, const VALUE& x2,
125 OptionalMatrixType H1, OptionalMatrixType H2) const override {
126 return measured_.localCoordinates(odo(x1, x2, H1, H2));
127 }
128
129 /// @return a deep copy of this factor
130 gtsam::NonlinearFactor::shared_ptr clone() const override {
131 return std::static_pointer_cast<gtsam::NonlinearFactor>(
132 r: gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
133
134 };
135
136 typedef GenericOdometry<Pose2> Odometry;
137
138 /// Graph specialization for syntactic sugar use with matlab
139 class Graph : public NonlinearFactorGraph {
140 public:
141 Graph() {}
142 // TODO: add functions to add factors
143 };
144
145} // namespace simulated2DOriented
146