54 measured_(measured), nM_(scale * direction), bias_(bias) {
58 NonlinearFactor::shared_ptr
clone()
const override {
59 return boost::static_pointer_cast<NonlinearFactor>(
60 NonlinearFactor::shared_ptr(
new MagFactor(*
this)));
64 boost::optional<Matrix&> HR = boost::none) {
68 Matrix H = HR->col(2);
78 boost::optional<Matrix&> H = boost::none)
const override {
80 Point3 hx = unrotate(nRb, nM_, H) + bias_;
81 return (hx - measured_);
103 measured_(measured), nM_(scale * direction), bias_(bias) {
107 NonlinearFactor::shared_ptr
clone()
const override {
108 return boost::static_pointer_cast<NonlinearFactor>(
109 NonlinearFactor::shared_ptr(
new MagFactor1(*
this)));
116 boost::optional<Matrix&> H = boost::none)
const override {
119 return (hx - measured_);
139 measured_(measured), bRn_(nRb.inverse()) {
143 NonlinearFactor::shared_ptr
clone()
const override {
144 return boost::static_pointer_cast<NonlinearFactor>(
145 NonlinearFactor::shared_ptr(
new MagFactor2(*
this)));
154 boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
155 boost::none)
const override {
160 return (hx - measured_);
180 measured_(measured), bRn_(nRb.inverse()) {
184 NonlinearFactor::shared_ptr
clone()
const override {
185 return boost::static_pointer_cast<NonlinearFactor>(
186 NonlinearFactor::shared_ptr(
new MagFactor3(*
this)));
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 {
199 Unit3 rotated = bRn_.
rotate(direction, boost::none, H2);
207 *H2 = scale * H * (*H2);
211 return (hx - measured_);
3D rotation represented as a rotation matrix or quaternion
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