GeneralSFMFactor.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/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <iostream>

namespace gtsam {

  template <class CAMERA, class LANDMARK>
  class GeneralSFMFactor:  public NoiseModelFactor2<CAMERA, LANDMARK> {
  protected:
    Point2 measured_;      

  public:

    typedef CAMERA Cam;                        
    typedef GeneralSFMFactor<CAMERA, LANDMARK> This;  
    typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;  
    typedef Point2 Measurement;              

    // shorthand for a smart pointer to a factor
    typedef boost::shared_ptr<This> shared_ptr;

    GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) :
      Base(model, cameraKey, landmarkKey), measured_(measured) {}

    GeneralSFMFactor():measured_(0.0,0.0) {}               
    GeneralSFMFactor(const Point2 & p):measured_(p) {}      
    GeneralSFMFactor(double x, double y):measured_(x,y) {} 

    virtual ~GeneralSFMFactor() {} 

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

    void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
      Base::print(s, keyFormatter);
      measured_.print(s + ".z");
    }

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

    Vector evaluateError(const Cam& camera,  const Point3& point,
        boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {

      try {
        Point2 reprojError(camera.project2(point,H1,H2) - measured_);
        return reprojError.vector();
      }
      catch( CheiralityException& e) {
        if (H1) *H1 = zeros(2, camera.dim());
        if (H2) *H2 = zeros(2, point.dim());
        std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
                              << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
        return zero(2);
      }
    }

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

  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_);
    }
  };

  template <class CALIBRATION>
  class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
  protected:
    Point2 measured_;     

  public:

    typedef GeneralSFMFactor2<CALIBRATION> This;
    typedef PinholeCamera<CALIBRATION> Camera;                  
    typedef NoiseModelFactor3<Pose3, Point3, CALIBRATION> Base; 
    typedef Point2 Measurement;                                 

    // shorthand for a smart pointer to a factor
    typedef boost::shared_ptr<This> shared_ptr;

    GeneralSFMFactor2(const Point2& measured, const SharedNoiseModel& model, Key poseKey, Key landmarkKey, Key calibKey) :
      Base(model, poseKey, landmarkKey, calibKey), measured_(measured) {}
    GeneralSFMFactor2():measured_(0.0,0.0) {}              

    virtual ~GeneralSFMFactor2() {} 

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

    void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
      Base::print(s, keyFormatter);
      measured_.print(s + ".z");
    }

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

    Vector evaluateError(const Pose3& pose3, const Point3& point, const CALIBRATION &calib,
                         boost::optional<Matrix&> H1=boost::none,
                         boost::optional<Matrix&> H2=boost::none,
                         boost::optional<Matrix&> H3=boost::none) const
    {
      try {
        Camera camera(pose3,calib);
        Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
        return reprojError.vector();
      }
      catch( CheiralityException& e) {
        if (H1) *H1 = zeros(2, pose3.dim());
        if (H2) *H2 = zeros(2, point.dim());
        if (H3) *H3 = zeros(2, calib.dim());
        std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
                              << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
      }
      return zero(2);
    }

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

  private:
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
      ar & boost::serialization::make_nvp("NoiseModelFactor3",
          boost::serialization::base_object<Base>(*this));
      ar & BOOST_SERIALIZATION_NVP(measured_);
    }
  };



} //namespace