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_);
  }
};

}