Loading...
Searching...
No Matches
MagFactor.h
Go to the documentation of this file.
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
class MagFactor: public NoiseModelFactorN<Rot2> {
const Point3 measured_;
const Point3 nM_;
const Point3 bias_;
public:
MagFactor(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) :
NoiseModelFactorN<Rot2>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) {
}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor(*this)));
}
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);
*HR = H;
}
return q;
}
Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const override {
// measured bM = nRb� * nM + b
Point3 hx = unrotate(nRb, nM_, H) + bias_;
return (hx - measured_);
}
};
class MagFactor1: public NoiseModelFactorN<Rot3> {
const Point3 measured_;
const Point3 nM_;
const Point3 bias_;
public:
MagFactor1(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) :
NoiseModelFactorN<Rot3>(model, key), //
measured_(measured), nM_(scale * direction), bias_(bias) {
}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this)));
}
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const override {
// measured bM = nRb� * nM + b
Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_);
}
};
class MagFactor2: public NoiseModelFactorN<Point3, Point3> {
const Point3 measured_;
const Rot3 bRn_;
public:
MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
const SharedNoiseModel& model) :
NoiseModelFactorN<Point3, Point3>(model, key1, key2), //
measured_(measured), bRn_(nRb.inverse()) {
}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor2(*this)));
}
Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const override {
// measured bM = nRb� * nM + b, where b is unknown bias
Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H2)
*H2 = I_3x3;
return (hx - measured_);
}
};
class MagFactor3: public NoiseModelFactorN<double, Unit3, Point3> {
const Point3 measured_;
const Rot3 bRn_;
public:
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactorN<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) {
}
NonlinearFactor::shared_ptr clone() const override {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor3(*this)));
}
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 {
// measured bM = nRb� * nM + b, where b is unknown bias
Unit3 rotated = bRn_.rotate(direction, boost::none, H2);
Point3 hx = scale * rotated.point3() + bias;
if (H1)
*H1 = rotated.point3();
if (H2) // H2 is 2*2, but we need 3*2
{
Matrix H;
rotated.point3(H);
*H2 = scale * H * (*H2);
}
if (H3)
*H3 = I_3x3;
return (hx - measured_);
}
};
}
Rot3.h
3D rotation represented as a rotation matrix or quaternion
Rot2.h
2D rotation
NonlinearFactor.h
Non-linear factor base classes.
gtsam
Global functions in a separate testing namespace.
Definition chartTesting.h:28
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:36
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
gtsam::Rot2
Rotation matrix NOTE: the angle theta is in radians unless explicitly stated.
Definition Rot2.h:36
gtsam::Rot3
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Definition Rot3.h:58
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::Rot3::Yaw
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
Definition Rot3.h:174
gtsam::Unit3
Represents a 3D point on a unit sphere.
Definition Unit3.h:43
gtsam::Unit3::point3
Point3 point3(OptionalJacobian< 3, 2 > H=boost::none) const
Return unit-norm Point3.
Definition Unit3.cpp:144
gtsam::MagFactor::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:58
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:50
gtsam::MagFactor::evaluateError
Vector evaluateError(const Rot2 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:77
gtsam::MagFactor1::MagFactor1
MagFactor1(Key key, const Point3 &measured, double scale, const Unit3 &direction, const Point3 &bias, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:99
gtsam::MagFactor1::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:107
gtsam::MagFactor1::evaluateError
Vector evaluateError(const Rot3 &nRb, boost::optional< Matrix & > H=boost::none) const override
vector of errors
Definition MagFactor.h:115
gtsam::MagFactor2::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:143
gtsam::MagFactor2::MagFactor2
MagFactor2(Key key1, Key key2, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:136
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:153
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:194
gtsam::MagFactor3::MagFactor3
MagFactor3(Key key1, Key key2, Key key3, const Point3 &measured, const Rot3 &nRb, const SharedNoiseModel &model)
Constructor.
Definition MagFactor.h:177
gtsam::MagFactor3::clone
NonlinearFactor::shared_ptr clone() const override
Definition MagFactor.h:184
gtsam::NoiseModelFactorN< Rot2 >::NoiseModelFactorN
NoiseModelFactorN()
Definition NonlinearFactor.h:469
gtsam::NoiseModelFactorN< Rot2 >::key
Key key() const
Definition NonlinearFactor.h:518