PinholeCamera.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/PinholePose.h>
#include <gtsam/geometry/BearingRange.h>

namespace gtsam {

template<typename Calibration>
class PinholeCamera: public PinholeBaseK<Calibration> {

public:

  typedef Point2 Measurement;
  typedef Point2Vector MeasurementVector;

private:

  typedef PinholeBaseK<Calibration> Base; 
  Calibration K_; 

  // Get dimensions of calibration type at compile time
  static const int DimK = FixedDimension<Calibration>::value;

public:

  enum {
    dimension = 6 + DimK
  }; 


  PinholeCamera() {
  }

  explicit PinholeCamera(const Pose3& pose) :
      Base(pose) {
  }

  PinholeCamera(const Pose3& pose, const Calibration& K) :
      Base(pose), K_(K) {
  }


  static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
      double height) {
    return PinholeCamera(Base::LevelPose(pose2, height), K);
  }

  static PinholeCamera Level(const Pose2& pose2, double height) {
    return PinholeCamera::Level(Calibration(), pose2, height);
  }

  static PinholeCamera Lookat(const Point3& eye, const Point3& target,
      const Point3& upVector, const Calibration& K = Calibration()) {
    return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
  }

  // Create PinholeCamera, with derivatives
  static PinholeCamera Create(const Pose3& pose, const Calibration &K,
      OptionalJacobian<dimension, 6> H1 = {}, //
      OptionalJacobian<dimension, DimK> H2 = {}) {
    typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
    if (H1)
      *H1 << I_6x6, MatrixK6::Zero();
    typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
    typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
    if (H2)
      *H2 << Matrix6K::Zero(), MatrixK::Identity();
    return PinholeCamera(pose,K);
  }


  explicit PinholeCamera(const Vector &v) :
      Base(v.head<6>()) {
    if (v.size() > 6)
      K_ = Calibration(v.tail<DimK>());
  }

  PinholeCamera(const Vector &v, const Vector &K) :
      Base(v), K_(K) {
  }


  bool equals(const Base &camera, double tol = 1e-9) const {
    const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
    return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
  }

  void print(const std::string& s = "PinholeCamera") const override {
    Base::print(s);
    K_.print(s + ".calibration");
  }


  ~PinholeCamera() override {
  }

  const Pose3& pose() const {
    return Base::pose();
  }

  const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
    if (H) {
      H->setZero();
      H->template block<6, 6>(0, 0) = I_6x6;
    }
    return Base::pose();
  }

  const Calibration& calibration() const override {
    return K_;
  }


  size_t dim() const {
    return dimension;
  }

  static size_t Dim() {
    return dimension;
  }

  typedef Eigen::Matrix<double, dimension, 1> VectorK6;

  PinholeCamera retract(const Vector& d) const {
    if ((size_t) d.size() == 6)
      return PinholeCamera(this->pose().retract(d), calibration());
    else
      return PinholeCamera(this->pose().retract(d.head<6>()),
          calibration().retract(d.tail(calibration().dim())));
  }

  VectorK6 localCoordinates(const PinholeCamera& T2) const {
    VectorK6 d;
    d.template head<6>() = this->pose().localCoordinates(T2.pose());
    d.template tail<DimK>() = calibration().localCoordinates(T2.calibration());
    return d;
  }

  static PinholeCamera Identity() {
    return PinholeCamera(); // assumes that the default constructor is valid
  }


  typedef Eigen::Matrix<double, 2, DimK> Matrix2K;

  template<class POINT>
  Point2 _project2(const POINT& pw, OptionalJacobian<2, dimension> Dcamera,
      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint) const {
    // We just call 3-derivative version in Base
    if (Dcamera){
      Matrix26 Dpose;
      Eigen::Matrix<double, 2, DimK> Dcal;
      const Point2 pi = Base::project(pw, Dpose, Dpoint, Dcal);
      *Dcamera << Dpose, Dcal;
      return pi;
    } else {
      return Base::project(pw, {}, Dpoint, {});
    }
  }

  Point2 project2(const Point3& pw, OptionalJacobian<2, dimension> Dcamera =
      {}, OptionalJacobian<2, 3> Dpoint = {}) const {
    return _project2(pw, Dcamera, Dpoint);
  }

  Point2 project2(const Unit3& pw, OptionalJacobian<2, dimension> Dcamera =
      {}, OptionalJacobian<2, 2> Dpoint = {}) const {
    return _project2(pw, Dcamera, Dpoint);
  }

  double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
      {}, OptionalJacobian<1, 3> Dpoint = {}) const {
    Matrix16 Dpose_;
    double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
    if (Dcamera)
      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
    return result;
  }

  double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
      {}, OptionalJacobian<1, 6> Dpose = {}) const {
    Matrix16 Dpose_;
    double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
    if (Dcamera)
      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
    return result;
  }

  template<class CalibrationB>
  double range(const PinholeCamera<CalibrationB>& camera,
      OptionalJacobian<1, dimension> Dcamera = {},
      OptionalJacobian<1, 6 + CalibrationB::dimension> Dother = {}) const {
    Matrix16 Dcamera_, Dother_;
    double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
        Dother ? &Dother_ : 0);
    if (Dcamera) {
      *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
    }
    if (Dother) {
      Dother->setZero();
      Dother->template block<1, 6>(0, 0) = Dother_;
    }
    return result;
  }

  double range(const CalibratedCamera& camera,
      OptionalJacobian<1, dimension> Dcamera = {},
      OptionalJacobian<1, 6> Dother = {}) const {
    return range(camera.pose(), Dcamera, Dother);
  }

  Matrix34 cameraProjectionMatrix() const {
    return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
  }

  Vector defaultErrorWhenTriangulatingBehindCamera() const {
    return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());
  }

private:

#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION

  friend class boost::serialization::access;
  template<class Archive>
  void serialize(Archive & ar, const unsigned int /*version*/) {
    ar
        & boost::serialization::make_nvp("PinholeBaseK",
            boost::serialization::base_object<Base>(*this));
    ar & BOOST_SERIALIZATION_NVP(K_);
  }
#endif

public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};

// manifold traits

template <typename Calibration>
struct traits<PinholeCamera<Calibration> >
    : public internal::Manifold<PinholeCamera<Calibration> > {};

template <typename Calibration>
struct traits<const PinholeCamera<Calibration> >
    : public internal::Manifold<PinholeCamera<Calibration> > {};

// range traits, used in RangeFactor
template <typename Calibration, typename T>
struct Range<PinholeCamera<Calibration>, T> : HasRange<PinholeCamera<Calibration>, T, double> {};

}  // \ gtsam