gtsam  4.1.0
gtsam
TSAMFactors.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 
19 #pragma once
20 
21 #include <gtsam/geometry/Pose2.h>
23 
24 namespace gtsam {
25 
29 class DeltaFactor: public NoiseModelFactor2<Pose2, Point2> {
30 
31 public:
32  typedef DeltaFactor This;
34  typedef boost::shared_ptr<This> shared_ptr;
35 
36 private:
37  Point2 measured_;
38 
39 public:
40 
42  DeltaFactor(Key i, Key j, const Point2& measured,
43  const SharedNoiseModel& model) :
44  Base(model, i, j), measured_(measured) {
45  }
46 
48  Vector evaluateError(const Pose2& pose, const Point2& point,
49  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
50  boost::none) const override {
51  return pose.transformTo(point, H1, H2) - measured_;
52  }
53 };
54 
58 class DeltaFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Point2> {
59 
60 public:
61  typedef DeltaFactorBase This;
63  typedef boost::shared_ptr<This> shared_ptr;
64 
65 private:
66  Point2 measured_;
67 
68 public:
69 
71  DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2& measured,
72  const SharedNoiseModel& model) :
73  Base(model, b1, i, b2, j), measured_(measured) {
74  }
75 
77  Vector evaluateError(const Pose2& base1, const Pose2& pose,
78  const Pose2& base2, const Point2& point, //
79  boost::optional<Matrix&> H1 = boost::none, //
80  boost::optional<Matrix&> H2 = boost::none, //
81  boost::optional<Matrix&> H3 = boost::none, //
82  boost::optional<Matrix&> H4 = boost::none) const override {
83  if (H1 || H2 || H3 || H4) {
84  // TODO use fixed-size matrices
85  Matrix D_pose_g_base1, D_pose_g_pose;
86  Pose2 pose_g = base1.compose(pose, D_pose_g_base1, D_pose_g_pose);
87  Matrix D_point_g_base2, D_point_g_point;
88  Point2 point_g = base2.transformFrom(point, D_point_g_base2,
89  D_point_g_point);
90  Matrix D_e_pose_g, D_e_point_g;
91  Point2 d = pose_g.transformTo(point_g, D_e_pose_g, D_e_point_g);
92  if (H1)
93  *H1 = D_e_pose_g * D_pose_g_base1;
94  if (H2)
95  *H2 = D_e_pose_g * D_pose_g_pose;
96  if (H3)
97  *H3 = D_e_point_g * D_point_g_base2;
98  if (H4)
99  *H4 = D_e_point_g * D_point_g_point;
100  return d - measured_;
101  } else {
102  Pose2 pose_g = base1.compose(pose);
103  Point2 point_g = base2.transformFrom(point);
104  Point2 d = pose_g.transformTo(point_g);
105  return d - measured_;
106  }
107  }
108 };
109 
113 class OdometryFactorBase: public NoiseModelFactor4<Pose2, Pose2, Pose2, Pose2> {
114 
115 public:
116  typedef OdometryFactorBase This;
118  typedef boost::shared_ptr<This> shared_ptr;
119 
120 private:
121  Pose2 measured_;
122 
123 public:
124 
126  OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2& measured,
127  const SharedNoiseModel& model) :
128  Base(model, b1, i, b2, j), measured_(measured) {
129  }
130 
132  Vector evaluateError(const Pose2& base1, const Pose2& pose1,
133  const Pose2& base2, const Pose2& pose2, //
134  boost::optional<Matrix&> H1 = boost::none, //
135  boost::optional<Matrix&> H2 = boost::none, //
136  boost::optional<Matrix&> H3 = boost::none, //
137  boost::optional<Matrix&> H4 = boost::none) const override {
138  if (H1 || H2 || H3 || H4) {
139  // TODO use fixed-size matrices
140  Matrix D_pose1_g_base1, D_pose1_g_pose1;
141  Pose2 pose1_g = base1.compose(pose1, D_pose1_g_base1, D_pose1_g_pose1);
142  Matrix D_pose2_g_base2, D_pose2_g_pose2;
143  Pose2 pose2_g = base2.compose(pose2, D_pose2_g_base2, D_pose2_g_pose2);
144  Matrix D_e_pose1_g, D_e_pose2_g;
145  Pose2 d = pose1_g.between(pose2_g, D_e_pose1_g, D_e_pose2_g);
146  if (H1)
147  *H1 = D_e_pose1_g * D_pose1_g_base1;
148  if (H2)
149  *H2 = D_e_pose1_g * D_pose1_g_pose1;
150  if (H3)
151  *H3 = D_e_pose2_g * D_pose2_g_base2;
152  if (H4)
153  *H4 = D_e_pose2_g * D_pose2_g_pose2;
154  return measured_.localCoordinates(d);
155  } else {
156  Pose2 pose1_g = base1.compose(pose1);
157  Pose2 pose2_g = base2.compose(pose2);
158  Pose2 d = pose1_g.between(pose2_g);
159  return measured_.localCoordinates(d);
160  }
161  }
162 };
163 
164 }
165 
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::OdometryFactorBase::OdometryFactorBase
OdometryFactorBase(Key b1, Key i, Key b2, Key j, const Pose2 &measured, const SharedNoiseModel &model)
Constructor.
Definition: TSAMFactors.h:126
gtsam::Pose2
Definition: Pose2.h:36
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::LieGroup::localCoordinates
TangentVector localCoordinates(const Class &g) const
localCoordinates as required by manifold concept: finds tangent vector between *this and g
Definition: Lie.h:135
Pose2.h
2D Pose
NonlinearFactor.h
Non-linear factor base classes.
gtsam::OdometryFactorBase::evaluateError
Vector evaluateError(const Pose2 &base1, const Pose2 &pose1, const Pose2 &base2, const Pose2 &pose2, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:132
gtsam::DeltaFactor
DeltaFactor: relative 2D measurement between Pose2 and Point2.
Definition: TSAMFactors.h:29
gtsam::Pose2::transformTo
GTSAM_EXPORT Point2 transformTo(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in pose coordinate frame.
Definition: Pose2.cpp:201
gtsam::Point2
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
gtsam::DeltaFactorBase::DeltaFactorBase
DeltaFactorBase(Key b1, Key i, Key b2, Key j, const Point2 &measured, const SharedNoiseModel &model)
Constructor.
Definition: TSAMFactors.h:71
gtsam::DeltaFactor::evaluateError
Vector evaluateError(const Pose2 &pose, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:48
gtsam::DeltaFactor::DeltaFactor
DeltaFactor(Key i, Key j, const Point2 &measured, const SharedNoiseModel &model)
Constructor.
Definition: TSAMFactors.h:42
gtsam::Factor
This is the base class for all factor types.
Definition: Factor.h:55
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::Pose2::transformFrom
GTSAM_EXPORT Point2 transformFrom(const Point2 &point, OptionalJacobian< 2, 3 > Dpose=boost::none, OptionalJacobian< 2, 2 > Dpoint=boost::none) const
Return point coordinates in global frame.
Definition: Pose2.cpp:212
gtsam::OdometryFactorBase
OdometryFactorBase: Pose2 odometry, with Basenodes.
Definition: TSAMFactors.h:113
gtsam::NoiseModelFactor4
A convenient base class for creating your own NoiseModelFactor with 4 variables.
Definition: NonlinearFactor.h:507
gtsam::DeltaFactorBase::evaluateError
Vector evaluateError(const Pose2 &base1, const Pose2 &pose, const Pose2 &base2, const Point2 &point, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none, boost::optional< Matrix & > H4=boost::none) const override
Evaluate measurement error h(x)-z.
Definition: TSAMFactors.h:77
gtsam::DeltaFactorBase
DeltaFactorBase: relative 2D measurement between Pose2 and Point2, with Basenodes.
Definition: TSAMFactors.h:58