RotateFactor.h

/*
 * @file RotateFactor.cpp
 * @brief RotateFactor class
 * @author Frank Dellaert
 * @date December 17, 2013
 */
 
#pragma once
 
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot3.h>
 
namespace gtsam {
 
class RotateFactor: public NoiseModelFactor1<Rot3> {
 
  Point3 p_, z_; 
 
  typedef NoiseModelFactor1<Rot3> Base;
  typedef RotateFactor This;
 
public:
 
  RotateFactor(Key key, const Rot3& P, const Rot3& Z,
      const SharedNoiseModel& model) :
      Base(model, key), p_(Rot3::Logmap(P)), z_(Rot3::Logmap(Z)) {
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
  void print(const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "RotateFactor:]\n";
    std::cout << "p: " << p_.transpose() << std::endl;
    std::cout << "z: " << z_.transpose() << std::endl;
  }
 
  Vector evaluateError(const Rot3& R,
      boost::optional<Matrix&> H = boost::none) const override {
    // predict p_ as q = R*z_, derivative H will be filled if not none
    Point3 q = R.rotate(z_,H);
    // error is just difference, and note derivative of that wrpt q is I3
    return (Vector(3) << q.x()-p_.x(), q.y()-p_.y(), q.z()-p_.z()).finished();
  }
 
};
 
class RotateDirectionsFactor: public NoiseModelFactor1<Rot3> {
 
  Unit3 i_p_, c_z_; 
 
  typedef NoiseModelFactor1<Rot3> Base;
  typedef RotateDirectionsFactor This;
 
public:
 
  RotateDirectionsFactor(Key key, const Unit3& i_p, const Unit3& c_z,
      const SharedNoiseModel& model) :
      Base(model, key), i_p_(i_p), c_z_(c_z) {
  }
 
  static Rot3 Initialize(const Unit3& i_p, const Unit3& c_z) {
    gtsam::Quaternion iRc;
    // setFromTwoVectors sets iRc to (a) quaternion which transform c_z into i_p
    iRc.setFromTwoVectors(c_z.unitVector(), i_p.unitVector());
    return Rot3(iRc);
  }
 
  gtsam::NonlinearFactor::shared_ptr clone() const override {
    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
        gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
  void print(const std::string& s = "",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override {
    Base::print(s);
    std::cout << "RotateDirectionsFactor:" << std::endl;
    i_p_.print("p");
    c_z_.print("z");
  }
 
  Vector evaluateError(const Rot3& iRc, boost::optional<Matrix&> H = boost::none) const override {
    Unit3 i_q = iRc * c_z_;
    Vector error = i_p_.error(i_q, H);
    if (H) {
      Matrix DR;
      iRc.rotate(c_z_, DR);
      *H = (*H) * DR;
    }
    return error;
  }
 
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
}  // namespace gtsam