gtsam 4.2.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
19#pragma once
20
22#include <gtsam/geometry/Rot2.h>
23#include <gtsam/geometry/Rot3.h>
24
25namespace gtsam {
26
33class MagFactor: public NoiseModelFactorN<Rot2> {
34
35 const Point3 measured_;
36 const Point3 nM_;
37 const Point3 bias_;
38
39public:
40
50 MagFactor(Key key, const Point3& measured, double scale,
51 const Unit3& direction, const Point3& bias,
52 const SharedNoiseModel& model) :
53 NoiseModelFactorN<Rot2>(model, key), //
54 measured_(measured), nM_(scale * direction), bias_(bias) {
55 }
56
58 NonlinearFactor::shared_ptr clone() const override {
59 return boost::static_pointer_cast<NonlinearFactor>(
60 NonlinearFactor::shared_ptr(new MagFactor(*this)));
61 }
62
63 static Point3 unrotate(const Rot2& R, const Point3& p,
64 boost::optional<Matrix&> HR = boost::none) {
65 Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
66 if (HR) {
67 // assign to temporary first to avoid error in Win-Debug mode
68 Matrix H = HR->col(2);
69 *HR = H;
70 }
71 return q;
72 }
73
77 Vector evaluateError(const Rot2& nRb,
78 boost::optional<Matrix&> H = boost::none) const override {
79 // measured bM = nRb� * nM + b
80 Point3 hx = unrotate(nRb, nM_, H) + bias_;
81 return (hx - measured_);
82 }
83};
84
90class MagFactor1: public NoiseModelFactorN<Rot3> {
91
92 const Point3 measured_;
93 const Point3 nM_;
94 const Point3 bias_;
95
96public:
97
99 MagFactor1(Key key, const Point3& measured, double scale,
100 const Unit3& direction, const Point3& bias,
101 const SharedNoiseModel& model) :
102 NoiseModelFactorN<Rot3>(model, key), //
103 measured_(measured), nM_(scale * direction), bias_(bias) {
104 }
105
107 NonlinearFactor::shared_ptr clone() const override {
108 return boost::static_pointer_cast<NonlinearFactor>(
109 NonlinearFactor::shared_ptr(new MagFactor1(*this)));
110 }
111
115 Vector evaluateError(const Rot3& nRb,
116 boost::optional<Matrix&> H = boost::none) const override {
117 // measured bM = nRb� * nM + b
118 Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
119 return (hx - measured_);
120 }
121};
122
128class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
129
130 const Point3 measured_;
131 const Rot3 bRn_;
132
133public:
134
136 MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
137 const SharedNoiseModel& model) :
138 NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
139 measured_(measured), bRn_(nRb.inverse()) {
140 }
141
143 NonlinearFactor::shared_ptr clone() const override {
144 return boost::static_pointer_cast<NonlinearFactor>(
145 NonlinearFactor::shared_ptr(new MagFactor2(*this)));
146 }
147
153 Vector evaluateError(const Point3& nM, const Point3& bias,
154 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
155 boost::none) const override {
156 // measured bM = nRb� * nM + b, where b is unknown bias
157 Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
158 if (H2)
159 *H2 = I_3x3;
160 return (hx - measured_);
161 }
162};
163
169class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
170
171 const Point3 measured_;
172 const Rot3 bRn_;
173
174public:
175
177 MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
178 const Rot3& nRb, const SharedNoiseModel& model) :
179 NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
180 measured_(measured), bRn_(nRb.inverse()) {
181 }
182
184 NonlinearFactor::shared_ptr clone() const override {
185 return boost::static_pointer_cast<NonlinearFactor>(
186 NonlinearFactor::shared_ptr(new MagFactor3(*this)));
187 }
188
194 Vector evaluateError(const double& scale, const Unit3& direction,
195 const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
196 boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
197 boost::none) const override {
198 // measured bM = nRb� * nM + b, where b is unknown bias
199 Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
200 Point3 hx = scale * rotated.point3() + bias;
201 if (H1)
202 *H1 = rotated.point3();
203 if (H2) // H2 is 2*2, but we need 3*2
204 {
205 Matrix H;
206 rotated.point3(H);
207 *H2 = scale * H * (*H2);
208 }
209 if (H3)
210 *H3 = I_3x3;
211 return (hx - measured_);
212 }
213};
214
215}
216
3D rotation represented as a rotation matrix or quaternion
2D rotation
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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:36
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition: Rot2.h:36
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
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
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:174
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
Represents a 3D point on a unit sphere.
Definition: Unit3.h:43
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition: MagFactor.h:33
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:58
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:50
Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: MagFactor.h:77
Factor to estimate rotation given magnetometer reading This version uses model measured bM = scale * ...
Definition: MagFactor.h:90
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:99
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:107
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition: MagFactor.h:115
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition: MagFactor.h:128
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:143
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:136
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:153
Factor to calibrate local Earth magnetic field as well as magnetometer bias This version uses model m...
Definition: MagFactor.h:169
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:194
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition: MagFactor.h:177
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:184
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition: NonlinearFactor.h:400
Key key() const
Returns a key.
Definition: NonlinearFactor.h:518