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 
25 namespace gtsam {
26 
33 class MagFactor: public NoiseModelFactorN<Rot2> {
34 
35  const Point3 measured_;
36  const Point3 nM_;
37  const Point3 bias_;
38 
39 public:
40 
41  // Provide access to Matrix& version of evaluateError:
42  using NoiseModelFactor1<Rot2>::evaluateError;
43 
53  MagFactor(Key key, const Point3& measured, double scale,
54  const Unit3& direction, const Point3& bias,
55  const SharedNoiseModel& model) :
57  measured_(measured), nM_(scale * direction), bias_(bias) {
58  }
59 
62  return std::static_pointer_cast<NonlinearFactor>(
64  }
65 
66  static Point3 unrotate(const Rot2& R, const Point3& p,
68  Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, {});
69  if (HR) {
70  // assign to temporary first to avoid error in Win-Debug mode
71  Matrix H = HR->col(2);
72  *HR = H;
73  }
74  return q;
75  }
76 
80  Vector evaluateError(const Rot2& nRb, OptionalMatrixType H) const override {
81  // measured bM = nRb� * nM + b
82  Point3 hx = unrotate(nRb, nM_, H) + bias_;
83  return (hx - measured_);
84  }
85 };
86 
92 class MagFactor1: public NoiseModelFactorN<Rot3> {
93 
94  const Point3 measured_;
95  const Point3 nM_;
96  const Point3 bias_;
97 
98 public:
99 
100  // Provide access to Matrix& version of evaluateError:
101  using NoiseModelFactor1<Rot3>::evaluateError;
102 
103 
106  const Unit3& direction, const Point3& bias,
107  const SharedNoiseModel& model) :
109  measured_(measured), nM_(scale * direction), bias_(bias) {
110  }
111 
114  return std::static_pointer_cast<NonlinearFactor>(
116  }
117 
121  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
122  // measured bM = nRb� * nM + b
123  Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_;
124  return (hx - measured_);
125  }
126 };
127 
133 class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
134 
136  const Rot3 bRn_;
137 
138 public:
139 
140  // Provide access to Matrix& version of evaluateError:
141  using NoiseModelFactor2<Point3, Point3>::evaluateError;
142 
143 
146  const SharedNoiseModel& model) :
149  }
150 
153  return std::static_pointer_cast<NonlinearFactor>(
155  }
156 
162  Vector evaluateError(const Point3& nM, const Point3& bias,
163  OptionalMatrixType H1, OptionalMatrixType H2) const override {
164  // measured bM = nRb� * nM + b, where b is unknown bias
165  Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
166  if (H2)
167  *H2 = I_3x3;
168  return (hx - measured_);
169  }
170 };
171 
177 class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
178 
180  const Rot3 bRn_;
181 
182 public:
183 
184  // Provide access to Matrix& version of evaluateError:
185  using NoiseModelFactor3<double, Unit3, Point3>::evaluateError;
186 
187 
190  const Rot3& nRb, const SharedNoiseModel& model) :
191  NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
193  }
194 
197  return std::static_pointer_cast<NonlinearFactor>(
199  }
200 
206  Vector evaluateError(const double& scale, const Unit3& direction,
207  const Point3& bias, OptionalMatrixType H1,
208  OptionalMatrixType H2, OptionalMatrixType H3) const override {
209  // measured bM = nRb� * nM + b, where b is unknown bias
210  Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2);
211  Point3 hx = scale * rotated.point3() + bias;
212  if (H1)
213  *H1 = rotated.point3();
214  if (H2) // H2 is 2*2, but we need 3*2
215  {
216  Matrix H;
217  rotated.point3(H);
218  *H2 = scale * H * (*H2);
219  }
220  if (H3)
221  *H3 = I_3x3;
222  return (hx - measured_);
223  }
224 };
225 
226 }
227 
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::MagFactor3::evaluateError
Vector evaluateError(const double &scale, const Unit3 &direction, const Point3 &bias, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override
vector of errors
Definition: MagFactor.h:206
gtsam::NonlinearFactor::shared_ptr
std::shared_ptr< This > shared_ptr
Definition: NonlinearFactor.h:79
gtsam::MagFactor1::bias_
const Point3 bias_
bias
Definition: MagFactor.h:96
inverse
const EIGEN_DEVICE_FUNC InverseReturnType inverse() const
Definition: ArrayCwiseUnaryOps.h:411
gtsam::MagFactor2
Definition: MagFactor.h:133
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H={}) const
Return unit-norm Point3.
Definition: Unit3.cpp:144
biased_x_rotation::bias
const Vector3 bias(1, 2, 3)
gtsam::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition: Rot3.h:178
gtsam::MagFactor2::MagFactor2
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Definition: MagFactor.h:145
measured
Point2 measured(-17, 30)
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
initial::nRb
const Rot3 nRb
Definition: testScenarioRunner.cpp:149
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:39
gtsam::Rot3::unrotate
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Definition: Rot3.cpp:136
Rot2.h
2D rotation
Rot3.h
3D rotation represented as a rotation matrix or quaternion
gtsam::MagFactor1
Definition: MagFactor.h:92
gtsam::MagFactor::unrotate
static Point3 unrotate(const Rot2 &R, const Point3 &p, OptionalMatrixType HR=OptionalNone)
Definition: MagFactor.h:66
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
OptionalNone
#define OptionalNone
Definition: NonlinearFactor.h:50
Eigen::numext::q
EIGEN_DEVICE_FUNC const Scalar & q
Definition: SpecialFunctionsImpl.h:1984
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition: Rot3.h:58
gtsam::MagFactor3::bRn_
const Rot3 bRn_
The assumed known rotation from nav to body.
Definition: MagFactor.h:180
gtsam::MagFactor3::MagFactor3
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Definition: MagFactor.h:189
gtsam::MagFactor
Definition: MagFactor.h:33
gtsam::MagFactor::measured_
const Point3 measured_
The measured magnetometer values.
Definition: MagFactor.h:35
gtsam::NoiseModelFactorN
Definition: NonlinearFactor.h:432
gtsam::MagFactor::evaluateError
Vector evaluateError(const Rot2 &nRb, OptionalMatrixType H) const override
vector of errors
Definition: MagFactor.h:80
gtsam::MagFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:152
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::NoiseModelFactorN< Point3, Point3 >::key1
Key key1() const
Definition: NonlinearFactor.h:732
gtsam::MagFactor3::measured_
const Point3 measured_
The measured magnetometer values.
Definition: MagFactor.h:179
gtsam::MagFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:61
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::MagFactor2::measured_
const Point3 measured_
The measured magnetometer values.
Definition: MagFactor.h:135
gtsam::MagFactor1::nM_
const Point3 nM_
Local magnetic field (mag output units)
Definition: MagFactor.h:95
gtsam::MagFactor::nM_
const Point3 nM_
Local magnetic field (mag output units)
Definition: MagFactor.h:36
gtsam::MagFactor3::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:196
NonlinearFactor.h
Non-linear factor base classes.
gtsam::MagFactor3
Definition: MagFactor.h:177
gtsam::NoiseModelFactorN< Point3, Point3 >::key2
Key key2() const
Definition: NonlinearFactor.h:736
gtsam::Rot2
Definition: Rot2.h:35
gtsam::MagFactor1::measured_
const Point3 measured_
The measured magnetometer values.
Definition: MagFactor.h:94
gtsam
traits
Definition: SFMdata.h:40
gtsam::MagFactor1::evaluateError
Vector evaluateError(const Rot3 &nRb, OptionalMatrixType H) const override
vector of errors
Definition: MagFactor.h:121
gtsam::MagFactor1::MagFactor1
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Definition: MagFactor.h:105
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::MagFactor::MagFactor
MagFactor(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Definition: MagFactor.h:53
gtsam::scale
static double scale(double x, double a, double b, double t1, double t2)
Scale x from [a, b] to [t1, t2].
Definition: Chebyshev.cpp:35
gtsam::NoiseModelFactorN< double, Unit3, Point3 >::key3
Key key3() const
Definition: NonlinearFactor.h:741
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::OptionalMatrixType
Matrix * OptionalMatrixType
Definition: NonlinearFactor.h:56
gtsam::NoiseModelFactorN< Rot2 >::key
Key key() const
Definition: NonlinearFactor.h:583
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition: Unit3.h:42
gtsam::MagFactor2::bRn_
const Rot3 bRn_
The assumed known rotation from nav to body.
Definition: MagFactor.h:136
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::MagFactor::bias_
const Point3 bias_
bias
Definition: MagFactor.h:37
gtsam::MagFactor1::clone
NonlinearFactor::shared_ptr clone() const override
Definition: MagFactor.h:113
gtsam::MagFactor2::evaluateError
Vector evaluateError(const Point3 &nM, const Point3 &bias, OptionalMatrixType H1, OptionalMatrixType H2) const override
vector of errors
Definition: MagFactor.h:162
R
Rot2 R(Rot2::fromAngle(0.1))
HR
#define HR
Definition: mincover.c:26


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:57