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/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/BinaryJacobianFactor.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/SymmetricBlockMatrix.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/timing.h>

#include <boost/none.hpp>
#include <boost/optional/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <iostream>
#include <string>

namespace boost {
namespace serialization {
class access;
} /* namespace serialization */
} /* namespace boost */

namespace gtsam {

template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {

  GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA);
  GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK);

  static const int DimC = FixedDimension<CAMERA>::value;
  static const int DimL = FixedDimension<LANDMARK>::value;
  typedef Eigen::Matrix<double, 2, DimC> JacobianC;
  typedef Eigen::Matrix<double, 2, DimL> JacobianL;

protected:

  Point2 measured_; 

public:

  typedef GeneralSFMFactor<CAMERA, LANDMARK> This;
  typedef NoiseModelFactor2<CAMERA, LANDMARK> Base;

  // 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);
    traits<Point2>::Print(measured_, 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) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
  }

  Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
      boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
    try {
      return camera.project2(point,H1,H2) - measured_;
    }
    catch( CheiralityException& e) {
      if (H1) *H1 = JacobianC::Zero();
      if (H2) *H2 = JacobianL::Zero();
      // TODO warn if verbose output asked for
      return Z_2x1;
    }
  }

  boost::shared_ptr<GaussianFactor> linearize(const Values& values) const {
    // Only linearize if the factor is active
    if (!this->active(values)) return boost::shared_ptr<JacobianFactor>();

    const Key key1 = this->key1(), key2 = this->key2();
    JacobianC H1;
    JacobianL H2;
    Vector2 b;
    try {
      const CAMERA& camera = values.at<CAMERA>(key1);
      const LANDMARK& point = values.at<LANDMARK>(key2);
      b = measured() - camera.project2(point, H1, H2);
    } catch (CheiralityException& e) {
      H1.setZero();
      H2.setZero();
      b.setZero();
      // TODO warn if verbose output asked for
    }

    // Whiten the system if needed
    const SharedNoiseModel& noiseModel = this->get_noiseModel();
    if (noiseModel && !noiseModel->isUnit()) {
      // TODO: implement WhitenSystem for fixed size matrices and include
      // above
      H1 = noiseModel->Whiten(H1);
      H2 = noiseModel->Whiten(H2);
      b = noiseModel->Whiten(b);
    }

    // Create new (unit) noiseModel, preserving constraints if applicable
    SharedDiagonal model;
    if (noiseModel && noiseModel->isConstrained()) {
      model = boost::static_pointer_cast<noiseModel::Constrained>(noiseModel)->unit();
    }

    return boost::make_shared<BinaryJacobianFactor<2, DimC, DimL> >(key1, H1, key2, H2, b, model);
  }

  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 CAMERA, class LANDMARK>
struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
    GeneralSFMFactor<CAMERA, LANDMARK> > {
};

template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {

  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION);
  static const int DimK = FixedDimension<CALIBRATION>::value;

protected:

  Point2 measured_; 

public:

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

  // 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);
    traits<Point2>::Print(measured_, 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) && traits<Point2>::Equals(this->measured_, 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);
      return camera.project(point, H1, H2, H3) - measured_;
    }
    catch( CheiralityException& e) {
      if (H1) *H1 = Matrix::Zero(2, 6);
      if (H2) *H2 = Matrix::Zero(2, 3);
      if (H3) *H3 = Matrix::Zero(2, DimK);
      std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2())
      << " behind Camera " << DefaultKeyFormatter(this->key1()) << std::endl;
    }
    return Z_2x1;
  }

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

template<class CALIBRATION>
struct traits<GeneralSFMFactor2<CALIBRATION> > : Testable<
    GeneralSFMFactor2<CALIBRATION> > {
};

} //namespace