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
 
 * -------------------------------------------------------------------------- */
 
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
 
namespace gtsam {
 
class MagFactor: public NoiseModelFactor1<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) :
      NoiseModelFactor1<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 NoiseModelFactor1<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) :
      NoiseModelFactor1<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 NoiseModelFactor2<Point3, Point3> {
 
  const Point3 measured_; 
  const Rot3 bRn_; 
 
public:
 
  MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
      const SharedNoiseModel& model) :
      NoiseModelFactor2<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 NoiseModelFactor3<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) :
      NoiseModelFactor3<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_);
  }
};
 
}