gtsam  4.1.0
gtsam
MagFactor.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 
20 #include <gtsam/geometry/Rot2.h>
21 #include <gtsam/geometry/Rot3.h>
22 
23 namespace gtsam {
24 
31 class MagFactor: public NoiseModelFactor1<Rot2> {
32 
33  const Point3 measured_;
34  const Point3 nM_;
35  const Point3 bias_;
36 
37 public:
38 
48  MagFactor(Key key, const Point3& measured, double scale,
49  const Unit3& direction, const Point3& bias,
50  const SharedNoiseModel& model) :
51  NoiseModelFactor1<Rot2>(model, key), //
52  measured_(measured), nM_(scale * direction), bias_(bias) {
53  }
54 
56  NonlinearFactor::shared_ptr clone() const override {
57  return boost::static_pointer_cast<NonlinearFactor>(
58  NonlinearFactor::shared_ptr(new MagFactor(*this)));
59  }
60 
61  static Point3 unrotate(const Rot2& R, const Point3& p,
62  boost::optional<Matrix&> HR = boost::none) {
63  Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
64  if (HR) {
65  // assign to temporary first to avoid error in Win-Debug mode
66  Matrix H = HR->col(2);
67  *HR = H;
68  }
69  return q;
70  }
71 
75  Vector evaluateError(const Rot2& nRb,
76  boost::optional<Matrix&> H = boost::none) const override {
77  // measured bM = nRb� * nM + b
78  Point3 hx = unrotate(nRb, nM_, H) + bias_;
79  return (hx - measured_);
80  }
81 };
82 
88 class MagFactor1: public NoiseModelFactor1<Rot3> {
89 
90  const Point3 measured_;
91  const Point3 nM_;
92  const Point3 bias_;
93 
94 public:
95 
97  MagFactor1(Key key, const Point3& measured, double scale,
98  const Unit3& direction, const Point3& bias,
99  const SharedNoiseModel& model) :
100  NoiseModelFactor1<Rot3>(model, key), //
101  measured_(measured), nM_(scale * direction), bias_(bias) {
102  }
103 
105  NonlinearFactor::shared_ptr clone() const override {
106  return boost::static_pointer_cast<NonlinearFactor>(
107  NonlinearFactor::shared_ptr(new MagFactor1(*this)));
108  }
109 
113  Vector evaluateError(const Rot3& nRb,
114  boost::optional<Matrix&> H = boost::none) const override {
115  // measured bM = nRb� * nM + b
116  Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
117  return (hx - measured_);
118  }
119 };
120 
126 class MagFactor2: public NoiseModelFactor2<Point3, Point3> {
127 
128  const Point3 measured_;
129  const Rot3 bRn_;
130 
131 public:
132 
134  MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
135  const SharedNoiseModel& model) :
136  NoiseModelFactor2<Point3, Point3>(model, key1, key2), //
137  measured_(measured), bRn_(nRb.inverse()) {
138  }
139 
141  NonlinearFactor::shared_ptr clone() const override {
142  return boost::static_pointer_cast<NonlinearFactor>(
143  NonlinearFactor::shared_ptr(new MagFactor2(*this)));
144  }
145 
151  Vector evaluateError(const Point3& nM, const Point3& bias,
152  boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
153  boost::none) const override {
154  // measured bM = nRb� * nM + b, where b is unknown bias
155  Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
156  if (H2)
157  *H2 = I_3x3;
158  return (hx - measured_);
159  }
160 };
161 
167 class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
168 
169  const Point3 measured_;
170  const Rot3 bRn_;
171 
172 public:
173 
175  MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
176  const Rot3& nRb, const SharedNoiseModel& model) :
177  NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
178  measured_(measured), bRn_(nRb.inverse()) {
179  }
180 
182  NonlinearFactor::shared_ptr clone() const override {
183  return boost::static_pointer_cast<NonlinearFactor>(
184  NonlinearFactor::shared_ptr(new MagFactor3(*this)));
185  }
186 
192  Vector evaluateError(const double& scale, const Unit3& direction,
193  const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
194  boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
195  boost::none) const override {
196  // measured bM = nRb� * nM + b, where b is unknown bias
197  Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
198  Point3 hx = scale * rotated.point3() + bias;
199  if (H1)
200  *H1 = rotated.point3();
201  if (H2) // H2 is 2*2, but we need 3*2
202  {
203  Matrix H;
204  rotated.point3(H);
205  *H2 = scale * H * (*H2);
206  }
207  if (H3)
208  *H3 = I_3x3;
209  return (hx - measured_);
210  }
211 };
212 
213 }
214 
gtsam::MagFactor1::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:105
gtsam::Rot2
Definition: Rot2.h:35
gtsam::MagFactor3
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition: MagFactor.h:167
gtsam::NoiseModelFactor3
A convenient base class for creating your own NoiseModelFactor with 3 variables.
Definition: NonlinearFactor.h:430
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from rotated coordinate frame to world
Definition: Rot3M.cpp:149
gtsam::MagFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:56
gtsam::MagFactor1
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition: MagFactor.h:88
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::NoiseModelFactor1
A convenient base class for creating your own NoiseModelFactor with 1 variable.
Definition: NonlinearFactor.h:271
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::MagFactor3::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:182
gtsam::MagFactor::MagFactor
MagFactor(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor of factor that estimates nav to body rotation bRn.
Definition: MagFactor.h:48
gtsam::NoiseModelFactor2< Point3, Point3 >::key1
Key key1() const
methods to retrieve both keys
Definition: NonlinearFactor.h:387
gtsam::MagFactor
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition: MagFactor.h:31
gtsam::NoiseModelFactor3< double, Unit3, Point3 >::key1
Key key1() const
methods to retrieve keys
Definition: NonlinearFactor.h:464
gtsam::MagFactor1::MagFactor1
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:97
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::MagFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:141
gtsam::MagFactor1::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: MagFactor.h:113
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1=boost::none, OptionalJacobian< 3, 3 > H2=boost::none) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
gtsam::MagFactor2::MagFactor2
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:134
gtsam::Unit3::point3
GTSAM_EXPORT Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition: Unit3.cpp:136
NonlinearFactor.h
Non-linear factor base classes.
gtsam::Rot3
Definition: Rot3.h:59
gtsam::MagFactor::evaluateError
Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: MagFactor.h:75
gtsam::MagFactor2::evaluateError
Vector evaluateError(const Point3 &nM, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none) const override
vector of errors
Definition: MagFactor.h:151
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
Rot2.h
2D rotation
gtsam::MagFactor2
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition: MagFactor.h:126
gtsam::NoiseModelFactor2
A convenient base class for creating your own NoiseModelFactor with 2 variables.
Definition: NonlinearFactor.h:355
gtsam::MagFactor3::evaluateError
Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 &bias, boost::optional< Matrix & > H1=boost::none, boost::optional< Matrix & > H2=boost::none, boost::optional< Matrix & > H3=boost::none) const override
vector of errors
Definition: MagFactor.h:192
gtsam::Point3
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:35
gtsam::MagFactor3::MagFactor3
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:175
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:177