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/base/MatrixConstants.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NoiseModelFactorN.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
 
namespace gtsam {

class MagFactor: public NoiseModelFactorN<Rot2> {
 
  const Point3 measured_; 
  const Point3 nM_; 
  const Point3 bias_; 
 
public:
 
  // Provide access to Matrix& version of evaluateError:
  using NoiseModelFactor1<Rot2>::evaluateError;

  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 std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new MagFactor(*this)));
  }
 
  static Point3 unrotate(const Rot2& R, const Point3& p,
      OptionalMatrixType HR = OptionalNone) {
    Point3 q = Rot3::Yaw(R.theta()).unrotate(p, HR, {});
    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, OptionalMatrixType H) 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:
 
  // Provide access to Matrix& version of evaluateError:
  using NoiseModelFactor1<Rot3>::evaluateError;
 

  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 std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new MagFactor1(*this)));
  }

  Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override {
    // measured bM = nRb� * nM + b
    Point3 hx = nRb.unrotate(nM_, H, OptionalNone) + bias_;
    return (hx - measured_);
  }
};

class MagFactor2 : public NoiseModelFactorT<Vector3, Point3, Point3> {
 
  const Point3 measured_; 
  const Rot3 bRn_; 
 
public:
 
  // Provide access to Matrix& version of evaluateError:
  using Base = NoiseModelFactorT<Vector3, Point3, Point3>;
  using Base::evaluateError;
 

  MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
      const SharedNoiseModel& model) :
      Base(model, key1, key2), //
      measured_(measured), bRn_(nRb.inverse()) {
  }

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new MagFactor2(*this)));
  }

  Vector3 evaluateError(const Point3& nM, const Point3& bias,
                        OptionalMatrixType H1,
                        OptionalMatrixType H2) const override {
    // measured bM = nRb� * nM + b, where b is unknown bias
    Point3 hx = bRn_.rotate(nM, OptionalNone, H1) + bias;
    if (H2)
      *H2 = I_3x3;
    return (hx - measured_);
  }
};

class MagFactor3
    : public NoiseModelFactorT<Vector3, double, Unit3, Point3> {
 
  const Point3 measured_; 
  const Rot3 bRn_; 
 
public:
 
  // Provide access to Matrix& version of evaluateError:
  using Base = NoiseModelFactorT<Vector3, double, Unit3, Point3>;
  using Base::evaluateError;
 

  MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
      const Rot3& nRb, const SharedNoiseModel& model) :
      Base(model, key1, key2, key3), //
      measured_(measured), bRn_(nRb.inverse()) {
  }

  NonlinearFactor::shared_ptr clone() const override {
    return std::static_pointer_cast<NonlinearFactor>(
        NonlinearFactor::shared_ptr(new MagFactor3(*this)));
  }

  Vector3 evaluateError(const double& scale, const Unit3& direction,
      const Point3& bias, OptionalMatrixType H1,
      OptionalMatrixType H2, OptionalMatrixType H3) const override {
    // measured bM = nRb� * nM + b, where b is unknown bias
    Unit3 rotated = bRn_.rotate(direction, OptionalNone, 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_);
  }
};
 
}