simulated2D.h
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 
18 // \callgraph
19 #pragma once
20 
23 #include <gtsam/geometry/Point2.h>
25 
26 // \namespace
27 
28 namespace simulated2D {
29 
30  using namespace gtsam;
31 
32  // Simulated2D robots have no orientation, just a position
33 
37  class Values: public gtsam::Values {
38  private:
39  int nrPoses_, nrPoints_;
40 
41  public:
42  typedef gtsam::Values Base;
43  typedef std::shared_ptr<Point2> sharedPoint;
44 
46  Values() : nrPoses_(0), nrPoints_(0) {
47  }
48 
50  Values(const Base& base) :
51  Base(base), nrPoses_(0), nrPoints_(0) {
52  }
53 
55  void insertPose(Key i, const Point2& p) {
56  insert(i, p);
57  nrPoses_++;
58  }
59 
61  void insertPoint(Key j, const Point2& p) {
62  insert(j, p);
63  nrPoints_++;
64  }
65 
67  int nrPoses() const {
68  return nrPoses_;
69  }
70 
72  int nrPoints() const {
73  return nrPoints_;
74  }
75 
77  Point2 pose(Key i) const {
78  return at<Point2>(i);
79  }
80 
82  Point2 point(Key j) const {
83  return at<Point2>(j);
84  }
85  };
86 
88  inline Point2 prior(const Point2& x) {
89  return x;
90  }
91 
94  if (H) *H = I_2x2;
95  return x;
96  }
97 
99  inline Point2 odo(const Point2& x1, const Point2& x2) {
100  return x2 - x1;
101  }
102 
104  inline Point2 odo(const Point2& x1, const Point2& x2,
107  if (H1) *H1 = -I_2x2;
108  if (H2) *H2 = I_2x2;
109  return x2 - x1;
110  }
111 
113  inline Point2 mea(const Point2& x, const Point2& l) {
114  return l - x;
115  }
116 
118  inline Point2 mea(const Point2& x, const Point2& l, OptionalJacobian<2,2> H1 =
120  if (H1) *H1 = -I_2x2;
121  if (H2) *H2 = I_2x2;
122  return l - x;
123  }
124 
128  template<class VALUE = Point2>
129  class GenericPrior: public NoiseModelFactorN<VALUE> {
130  public:
133  typedef std::shared_ptr<GenericPrior<VALUE> > shared_ptr;
134  typedef VALUE Pose;
135 
136  // Provide access to the Matrix& version of evaluateError:
137  using Base::evaluateError;
138 
139  Pose measured_;
140 
142  GenericPrior(const Pose& z, const SharedNoiseModel& model, Key key) :
143  Base(model, key), measured_(z) {
144  }
145 
147  Vector evaluateError(const Pose& x, OptionalMatrixType H) const override {
148  return (simulated2D::prior(x, H) - measured_);
149  }
150 
151  ~GenericPrior() override {}
152 
155  return std::static_pointer_cast<gtsam::NonlinearFactor>(
157 
158  private:
159 
162 
163 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
164  friend class boost::serialization::access;
166  template<class ARCHIVE>
167  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
168  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
169  ar & BOOST_SERIALIZATION_NVP(measured_);
170  }
171 #endif
172  };
173 
177  template<class VALUE = Point2>
178  class GenericOdometry: public NoiseModelFactorN<VALUE, VALUE> {
179  public:
182  typedef std::shared_ptr<GenericOdometry<VALUE> > shared_ptr;
183  typedef VALUE Pose;
184 
185  // Provide access to the Matrix& version of evaluateError:
186  using Base::evaluateError;
187 
188  Pose measured_;
189 
191  GenericOdometry(const Pose& measured, const SharedNoiseModel& model, Key i1, Key i2) :
192  Base(model, i1, i2), measured_(measured) {
193  }
194 
196  Vector evaluateError(const Pose& x1, const Pose& x2,
197  OptionalMatrixType H1, OptionalMatrixType H2) const override {
198  return (odo(x1, x2, H1, H2) - measured_);
199  }
200 
201  ~GenericOdometry() override {}
202 
205  return std::static_pointer_cast<gtsam::NonlinearFactor>(
207 
208  private:
209 
212 
213 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
214  friend class boost::serialization::access;
216  template<class ARCHIVE>
217  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
218  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
219  ar & BOOST_SERIALIZATION_NVP(measured_);
220  }
221 #endif
222  };
223 
227  template<class POSE, class LANDMARK>
228  class GenericMeasurement: public NoiseModelFactorN<POSE, LANDMARK> {
229  public:
232  typedef std::shared_ptr<GenericMeasurement<POSE, LANDMARK> > shared_ptr;
233  typedef POSE Pose;
234  typedef LANDMARK Landmark;
235 
236  // Provide access to the Matrix& version of evaluateError:
237  using Base::evaluateError;
238 
239  Landmark measured_;
240 
243  Base(model, i, j), measured_(measured) {
244  }
245 
247  Vector evaluateError(const Pose& x1, const Landmark& x2,
248  OptionalMatrixType H1, OptionalMatrixType H2) const override {
249  return (mea(x1, x2, H1, H2) - measured_);
250  }
251 
252  ~GenericMeasurement() override {}
253 
256  return std::static_pointer_cast<gtsam::NonlinearFactor>(
258 
259  private:
260 
263 
264 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
265  friend class boost::serialization::access;
267  template<class ARCHIVE>
268  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
269  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
270  ar & BOOST_SERIALIZATION_NVP(measured_);
271  }
272 #endif
273  };
274 
279 
280  // Specialization of a graph for this example domain
281  // TODO: add functions to add factor types
282  class Graph : public NonlinearFactorGraph {
283  public:
284  Graph() {}
285  };
286 
287 } // namespace simulated2D
288 
290 namespace gtsam {
291 template<class POSE, class LANDMARK>
292 struct traits<simulated2D::GenericMeasurement<POSE, LANDMARK> > : Testable<
293  simulated2D::GenericMeasurement<POSE, LANDMARK> > {
294 };
295 
296 template<>
297 struct traits<simulated2D::Values> : public Testable<simulated2D::Values> {
298 };
299 }
300 
const gtsam::Symbol key('X', 0)
Point2 measured(-17, 30)
std::shared_ptr< GenericOdometry< VALUE > > shared_ptr
Definition: simulated2D.h:182
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:255
GenericPrior()
Default constructor.
Definition: simulated2D.h:161
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:154
GenericOdometry()
Default constructor.
Definition: simulated2D.h:211
Factor Graph consisting of non-linear factors.
std::string serialize(const T &input)
serializes to a string
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
noiseModel::Diagonal::shared_ptr model
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
Vector2 Point2
Definition: Point2.h:32
Pose measured_
prior mean
Definition: simulated2D.h:139
VALUE Pose
shortcut to Pose type
Definition: simulated2D.h:183
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
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
GenericOdometry< VALUE > This
Definition: simulated2D.h:181
Vector evaluateError(const Pose &x1, const Landmark &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error and optionally return derivatives.
Definition: simulated2D.h:247
#define OptionalNone
GenericOdometry(const Pose &measured, const SharedNoiseModel &model, Key i1, Key i2)
Create odometry.
Definition: simulated2D.h:191
GenericPrior< VALUE > This
Definition: simulated2D.h:132
static const Line3 l(Rot3(), 1, 1)
Matrix * OptionalMatrixType
Eigen::VectorXd Vector
Definition: Vector.h:38
void insertPose(Key i, const Point2 &p)
Insert a pose.
Definition: simulated2D.h:55
GenericMeasurement(const Landmark &measured, const SharedNoiseModel &model, Key i, Key j)
Create measurement factor.
Definition: simulated2D.h:242
Vector evaluateError(const Pose &x1, const Pose &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error and optionally return derivatives.
Definition: simulated2D.h:196
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
Pose measured_
odometry measurement
Definition: simulated2D.h:188
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:204
void insertPoint(Key j, const Point2 &p)
Insert a point.
Definition: simulated2D.h:61
Vector evaluateError(const Pose &x, OptionalMatrixType H) const override
Return error and optional derivative.
Definition: simulated2D.h:147
Values()
Constructor.
Definition: simulated2D.h:46
std::shared_ptr< Point2 > sharedPoint
shortcut to shared Point type
Definition: simulated2D.h:43
#define This
traits
Definition: chartTesting.h:28
GenericPrior(const Pose &z, const SharedNoiseModel &model, Key key)
Create generic prior.
Definition: simulated2D.h:142
GenericPrior< Point2 > Prior
Definition: simulated2D.h:276
Point2 point(Key j) const
Return point j.
Definition: simulated2D.h:82
Values(const Base &base)
Copy constructor.
Definition: simulated2D.h:50
Non-linear factor base classes.
std::shared_ptr< GenericPrior< VALUE > > shared_ptr
Definition: simulated2D.h:133
GenericOdometry< Point2 > Odometry
Definition: simulated2D.h:277
std::shared_ptr< GenericMeasurement< POSE, LANDMARK > > shared_ptr
Definition: simulated2D.h:232
Pose3 x1
Definition: testPose3.cpp:663
std::shared_ptr< This > shared_ptr
Landmark measured_
Measurement.
Definition: simulated2D.h:239
NoiseModelFactorN< VALUE > Base
base class
Definition: simulated2D.h:131
NoiseModelFactorN< VALUE, VALUE > Base
base class
Definition: simulated2D.h:180
float * p
GenericMeasurement()
Default constructor.
Definition: simulated2D.h:262
int nrPoses() const
Number of poses.
Definition: simulated2D.h:67
NoiseModelFactorN< POSE, LANDMARK > Base
base class
Definition: simulated2D.h:230
void insert(Key j, const Value &val)
Definition: Values.cpp:155
GenericMeasurement< POSE, LANDMARK > This
Definition: simulated2D.h:231
POSE Pose
shortcut to Pose type
Definition: simulated2D.h:233
Special class for optional Jacobian arguments.
Annotation indicating that a class derives from another given type.
Definition: attr.h:61
VALUE Pose
shortcut to Pose type
Definition: simulated2D.h:134
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
2D Point
LANDMARK Landmark
shortcut to Landmark type
Definition: simulated2D.h:234
int nrPoints() const
Number of points.
Definition: simulated2D.h:72
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::ptrdiff_t j
Point2 pose(Key i) const
Return pose i.
Definition: simulated2D.h:77
Point2 mea(const Point2 &x, const Point2 &l)
measurement between landmark and pose
Definition: simulated2D.h:113
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
gtsam::Values Base
base class
Definition: simulated2D.h:42


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