ProjectionFactorPPP.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/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>

namespace gtsam {

  template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
  class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
  protected:

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

    // verbosity handling for Cheirality Exceptions
    bool throwCheirality_; 
    bool verboseCheirality_; 

  public:

    typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;

    typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;

    typedef boost::shared_ptr<This> shared_ptr;

  ProjectionFactorPPP() :
      measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
  }

    ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
        Key poseKey, Key transformKey,  Key pointKey,
        const boost::shared_ptr<CALIBRATION>& K) :
          Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
          throwCheirality_(false), verboseCheirality_(false) {}

    ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
        Key poseKey, Key transformKey, Key pointKey,
        const boost::shared_ptr<CALIBRATION>& K,
        bool throwCheirality, bool verboseCheirality) :
          Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
          throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}

    virtual ~ProjectionFactorPPP() {}

    virtual NonlinearFactor::shared_ptr clone() const {
      return boost::static_pointer_cast<NonlinearFactor>(
          NonlinearFactor::shared_ptr(new This(*this))); }

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

    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 Pose3& pose, const Pose3& transform, const Point3& point,
        boost::optional<Matrix&> H1 = boost::none,
        boost::optional<Matrix&> H2 = boost::none,
        boost::optional<Matrix&> H3 = boost::none) const {
      try {
          if(H1 || H2 || H3) {
            Matrix H0, H02;
            PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
            Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
            *H2 = *H1 * H02;
            *H1 = *H1 * H0;
            return reprojectionError;
          } else {
            PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
            return camera.project(point, H1, H3, boost::none) - measured_;
          }
      } catch( CheiralityException& e) {
        if (H1) *H1 = Matrix::Zero(2,6);
        if (H2) *H2 = Matrix::Zero(2,6);
        if (H3) *H3 = Matrix::Zero(2,3);
        if (verboseCheirality_)
          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
        if (throwCheirality_)
          throw e;
      }
      return Vector::Ones(2) * 2.0 * K_->fx();
    }

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

    inline const boost::shared_ptr<CALIBRATION> 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_BASE_OBJECT_NVP(Base);
      ar & BOOST_SERIALIZATION_NVP(measured_);
      ar & BOOST_SERIALIZATION_NVP(K_);
      ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
      ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
    }
  };

  template<class POSE, class LANDMARK, class CALIBRATION>
  struct traits<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > :
      public Testable<ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> > {
  };

} // \ namespace gtsam