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 
140 
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 
189 
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 
240 
243  Base(model, i, j), measured_(measured) {
244  }
245 
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 
simulated2D::Values::pose
Point2 pose(Key i) const
Return pose i.
Definition: simulated2D.h:77
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
simulated2D::GenericOdometry::GenericOdometry
GenericOdometry(const Pose &measured, const SharedNoiseModel &model, Key i1, Key i2)
Create odometry.
Definition: simulated2D.h:191
simulated2D::GenericMeasurement::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:255
simulated2D::Prior
GenericPrior< Point2 > Prior
Definition: simulated2D.h:276
simulated2D::GenericPrior::shared_ptr
std::shared_ptr< GenericPrior< VALUE > > shared_ptr
Definition: simulated2D.h:133
base
Annotation indicating that a class derives from another given type.
Definition: attr.h:64
simulated2D::GenericOdometry::This
GenericOdometry< VALUE > This
Definition: simulated2D.h:181
simulated2D::GenericOdometry
Definition: simulated2D.h:178
simulated2D
Definition: simulated2D.h:28
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
simulated2D::GenericMeasurement::Base
NoiseModelFactorN< POSE, LANDMARK > Base
base class
Definition: simulated2D.h:230
simulated2D::GenericPrior::evaluateError
Vector evaluateError(const Pose &x, OptionalMatrixType H) const override
Return error and optional derivative.
Definition: simulated2D.h:147
simulated2D::GenericPrior::GenericPrior
GenericPrior()
Default constructor.
Definition: simulated2D.h:161
simulated2D::GenericOdometry::Pose
VALUE Pose
shortcut to Pose type
Definition: simulated2D.h:183
simulated2D::GenericMeasurement::measured_
Landmark measured_
Measurement.
Definition: simulated2D.h:239
measured
Point2 measured(-17, 30)
simulated2D::Values::insertPoint
void insertPoint(Key j, const Point2 &p)
Insert a point.
Definition: simulated2D.h:61
simulated2D::GenericPrior::~GenericPrior
~GenericPrior() override
Definition: simulated2D.h:151
simulated2D::GenericPrior::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:154
gtsam::Factor
Definition: Factor.h:70
simulated2D::Values::Base
gtsam::Values Base
base class
Definition: simulated2D.h:42
simulated2D::GenericMeasurement::This
GenericMeasurement< POSE, LANDMARK > This
Definition: simulated2D.h:231
simulated2D::mea
Point2 mea(const Point2 &x, const Point2 &l)
measurement between landmark and pose
Definition: simulated2D.h:113
simulated2D::GenericOdometry::GenericOdometry
GenericOdometry()
Default constructor.
Definition: simulated2D.h:211
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
simulated2D::GenericMeasurement
Definition: simulated2D.h:228
simulated2D::GenericMeasurement::evaluateError
Vector evaluateError(const Pose &x1, const Landmark &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error and optionally return derivatives.
Definition: simulated2D.h:247
simulated2D::Values::sharedPoint
std::shared_ptr< Point2 > sharedPoint
shortcut to shared Point type
Definition: simulated2D.h:43
simulated2D::GenericOdometry::evaluateError
Vector evaluateError(const Pose &x1, const Pose &x2, OptionalMatrixType H1, OptionalMatrixType H2) const override
Evaluate error and optionally return derivatives.
Definition: simulated2D.h:196
simulated2D::Values::nrPoses
int nrPoses() const
Number of poses.
Definition: simulated2D.h:67
Point2.h
2D Point
simulated2D::Graph::Graph
Graph()
Definition: simulated2D.h:284
Eigen::internal::VALUE
@ VALUE
Definition: SpecialFunctionsImpl.h:729
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
simulated2D::GenericMeasurement::Pose
POSE Pose
shortcut to Pose type
Definition: simulated2D.h:233
simulated2D::GenericPrior::Pose
VALUE Pose
shortcut to Pose type
Definition: simulated2D.h:134
simulated2D::GenericPrior::This
GenericPrior< VALUE > This
Definition: simulated2D.h:132
simulated2D::GenericOdometry::~GenericOdometry
~GenericOdometry() override
Definition: simulated2D.h:201
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
simulated2D::GenericMeasurement::shared_ptr
std::shared_ptr< GenericMeasurement< POSE, LANDMARK > > shared_ptr
Definition: simulated2D.h:232
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:49
x1
Pose3 x1
Definition: testPose3.cpp:663
simulated2D::GenericOdometry::Base
NoiseModelFactorN< VALUE, VALUE > Base
base class
Definition: simulated2D.h:180
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
simulated2D::GenericOdometry::clone
gtsam::NonlinearFactor::shared_ptr clone() const override
Definition: simulated2D.h:204
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:431
l
static const Line3 l(Rot3(), 1, 1)
simulated2D::GenericOdometry::shared_ptr
std::shared_ptr< GenericOdometry< VALUE > > shared_ptr
Definition: simulated2D.h:182
simulated2D::GenericOdometry::measured_
Pose measured_
odometry measurement
Definition: simulated2D.h:188
simulated2D::GenericPrior::measured_
Pose measured_
prior mean
Definition: simulated2D.h:139
simulated2D::GenericMeasurement::Landmark
LANDMARK Landmark
shortcut to Landmark type
Definition: simulated2D.h:234
OptionalJacobian.h
Special class for optional Jacobian arguments.
pybind_wrapper_test_script.z
z
Definition: pybind_wrapper_test_script.py:61
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
simulated2D::GenericPrior::GenericPrior
GenericPrior(const Pose &z, const SharedNoiseModel &model, Key key)
Create generic prior.
Definition: simulated2D.h:142
simulated2D::Values::nrPoses_
int nrPoses_
Definition: simulated2D.h:39
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
key
const gtsam::Symbol key('X', 0)
NonlinearFactor.h
Non-linear factor base classes.
simulated2D::prior
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
simulated2D::GenericMeasurement::~GenericMeasurement
~GenericMeasurement() override
Definition: simulated2D.h:252
simulated2D::GenericPrior
Definition: simulated2D.h:129
gtsam
traits
Definition: SFMdata.h:40
gtsam::Testable
Definition: Testable.h:152
gtsam::traits
Definition: Group.h:36
i1
double i1(double x)
Definition: i1.c:150
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
simulated2D::GenericPrior::Base
NoiseModelFactorN< VALUE > Base
base class
Definition: simulated2D.h:131
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::Values
Definition: Values.h:65
simulated2D::GenericMeasurement::GenericMeasurement
GenericMeasurement()
Default constructor.
Definition: simulated2D.h:262
simulated2D::GenericMeasurement::GenericMeasurement
GenericMeasurement(const Landmark &measured, const SharedNoiseModel &model, Key i, Key j)
Create measurement factor.
Definition: simulated2D.h:242
p
float * p
Definition: Tutorial_Map_using.cpp:9
simulated2D::odo
Point2 odo(const Point2 &x1, const Point2 &x2)
odometry between two poses
Definition: simulated2D.h:99
gtsam::Values::insert
void insert(Key j, const Value &val)
Definition: Values.cpp:155
This
#define This
Definition: ActiveSetSolver-inl.h:27
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:55
simulated2D::Values::nrPoints
int nrPoints() const
Number of points.
Definition: simulated2D.h:72
simulated2D::Values::Values
Values()
Constructor.
Definition: simulated2D.h:46
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
simulated2D::Graph
Definition: simulated2D.h:282
x2
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
simulated2D::Values::insertPose
void insertPose(Key i, const Point2 &p)
Insert a pose.
Definition: simulated2D.h:55
simulated2D::Values::Values
Values(const Base &base)
Copy constructor.
Definition: simulated2D.h:50
simulated2D::Odometry
GenericOdometry< Point2 > Odometry
Definition: simulated2D.h:277
simulated2D::Values::point
Point2 point(Key j) const
Return point j.
Definition: simulated2D.h:82


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:25