InvDepthFactor3.h

Go to the documentation of this file.

#pragma once

#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/geometry/InvDepthCamera3.h>

namespace gtsam {

template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:

  // Keep a copy of measurement and calibration for I/O
  Point2 measured_;                
  boost::shared_ptr<Cal3_S2> K_;  

public:

  typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;

  typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;

  typedef boost::shared_ptr<This> shared_ptr;

  InvDepthFactor3() :
      measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
  }

  InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
      const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
        Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}

  virtual ~InvDepthFactor3() {}

  void print(const std::string& s = "InvDepthFactor3",
      const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
    Base::print(s, keyFormatter);
    traits<Point2>::Print(measured_, s + ".z");
  }

  virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
    const This *e = dynamic_cast<const This*>(&p);
    return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
  }

  Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
      boost::optional<Matrix&> H1=boost::none,
      boost::optional<Matrix&> H2=boost::none,
      boost::optional<Matrix&> H3=boost::none) const {
    try {
      InvDepthCamera3<Cal3_S2> camera(pose, K_);
      return camera.project(point, invDepth, H1, H2, H3) - measured_;
    } catch( CheiralityException& e) {
      if (H1) *H1 = Matrix::Zero(2,6);
      if (H2) *H2 = Matrix::Zero(2,5);
      if (H3) *H2 = Matrix::Zero(2,1);
      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
          " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
      return Vector::Ones(2) * 2.0 * K_->fx();
    }
    return (Vector(1) << 0.0).finished();
  }

  const Point2& imagePoint() const {
    return measured_;
  }

  inline const Cal3_S2::shared_ptr calibration() const {
    return K_;
  }

private:

  friend class boost::serialization::access;
  template<class ARCHIVE>
  void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
    ar & BOOST_SERIALIZATION_NVP(measured_);
    ar & BOOST_SERIALIZATION_NVP(K_);
  }
};
} // \ namespace gtsam