StereoFactor.h

/* ----------------------------------------------------------------------------
 
 * 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/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/StereoCamera.h>
 
namespace gtsam {
 
template<class POSE, class LANDMARK>
class GenericStereoFactor: public NoiseModelFactor2<POSE, LANDMARK> {
private:
 
  // Keep a copy of measurement and calibration for I/O
  StereoPoint2 measured_;                      
  Cal3_S2Stereo::shared_ptr K_;                
  boost::optional<POSE> body_P_sensor_;        
 
  // verbosity handling for Cheirality Exceptions
  bool throwCheirality_;                       
  bool verboseCheirality_;                     
 
public:
 
  // shorthand for base class type
  typedef NoiseModelFactor2<POSE, LANDMARK> Base;             
  typedef GenericStereoFactor<POSE, LANDMARK> This;           
  typedef boost::shared_ptr<GenericStereoFactor> shared_ptr;  
  typedef POSE CamPose;                                       
 
  GenericStereoFactor() : K_(new Cal3_S2Stereo(444, 555, 666, 777, 888, 1.0)),
      throwCheirality_(false), verboseCheirality_(false) {}
 
  GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
      Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
      boost::optional<POSE> body_P_sensor = boost::none) :
    Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
    throwCheirality_(false), verboseCheirality_(false) {}
 
  GenericStereoFactor(const StereoPoint2& measured, const SharedNoiseModel& model,
      Key poseKey, Key landmarkKey, const Cal3_S2Stereo::shared_ptr& K,
      bool throwCheirality, bool verboseCheirality,
      boost::optional<POSE> body_P_sensor = boost::none) :
    Base(model, poseKey, landmarkKey), measured_(measured), K_(K), body_P_sensor_(body_P_sensor),
    throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
 
  ~GenericStereoFactor() override {}
 
  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, keyFormatter);
    measured_.print(s + ".z");
    if(this->body_P_sensor_)
      this->body_P_sensor_->print("  sensor pose in body frame: ");
  }
 
  bool equals(const NonlinearFactor& f, double tol = 1e-9) const override {
    const GenericStereoFactor* e = dynamic_cast<const GenericStereoFactor*> (&f);
    return e
        && Base::equals(f)
        && measured_.equals(e->measured_, tol)
        && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
  }
 
  Vector evaluateError(const Pose3& pose, const Point3& point,
      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
    try {
      if(body_P_sensor_) {
        if(H1) {
          gtsam::Matrix H0;
          StereoCamera stereoCam(pose.compose(*body_P_sensor_, H0), K_);
          StereoPoint2 reprojectionError(stereoCam.project(point, H1, H2) - measured_);
          *H1 = *H1 * H0;
          return reprojectionError.vector();
        } else {
          StereoCamera stereoCam(pose.compose(*body_P_sensor_), K_);
          return (stereoCam.project(point, H1, H2) - measured_).vector();
        }
      } else {
        StereoCamera stereoCam(pose, K_);
        return (stereoCam.project(point, H1, H2) - measured_).vector();
      }
    } catch(StereoCheiralityException& e) {
      if (H1) *H1 = Matrix::Zero(3,6);
      if (H2) *H2 = Z_3x3;
      if (verboseCheirality_)
      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
          " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
      if (throwCheirality_)
        throw StereoCheiralityException(this->key2());
    }
    return Vector3::Constant(2.0 * K_->fx());
  }
 
  const StereoPoint2& measured() const {
    return measured_;
  }
 
  inline const Cal3_S2Stereo::shared_ptr calibration() const {
    return K_;
  }
 
  inline bool verboseCheirality() const { return verboseCheirality_; }
 
  inline bool throwCheirality() const { return throwCheirality_; }
 
private:
  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar & boost::serialization::make_nvp("NoiseModelFactor2",
        boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(measured_);
    ar & BOOST_SERIALIZATION_NVP(K_);
    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
    ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
    ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
  }
};
 
template<class T1, class T2>
struct traits<GenericStereoFactor<T1, T2> > : public Testable<GenericStereoFactor<T1, T2> > {};
 
} // \ namespace gtsam