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

namespace gtsam {

template<typename CALIBRATION>
class PinholeBaseK: public PinholeBase {

private:

  GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)

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

public:

  typedef CALIBRATION CalibrationType;


  PinholeBaseK() {
  }

  explicit PinholeBaseK(const Pose3& pose) :
  PinholeBase(pose) {
  }


  explicit PinholeBaseK(const Vector& v) : PinholeBase(v) {}


  virtual const CALIBRATION& calibration() const = 0;


  std::pair<Point2, bool> projectSafe(const Point3& pw) const {
    std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
    pn.first = calibration().uncalibrate(pn.first);
    return pn;
  }


  template <class POINT>
  Point2 _project(const POINT& pw, OptionalJacobian<2, 6> Dpose,
      OptionalJacobian<2, FixedDimension<POINT>::value> Dpoint,
      OptionalJacobian<2, DimK> Dcal) const {

    // project to normalized coordinates
    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);

    // uncalibrate to pixel coordinates
    Matrix2 Dpi_pn;
    const Point2 pi = calibration().uncalibrate(pn, Dcal,
        Dpose || Dpoint ? &Dpi_pn : 0);

    // If needed, apply chain rule
    if (Dpose)
      *Dpose = Dpi_pn * *Dpose;
    if (Dpoint)
      *Dpoint = Dpi_pn * *Dpoint;

    return pi;
  }

  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 3> Dpoint = {},
      OptionalJacobian<2, DimK> Dcal = {}) const {
    return _project(pw, Dpose, Dpoint, Dcal);
  }

  Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 3> Dpoint = {},
      OptionalJacobian<2, DimK> Dcal = {}) const {
    return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
  }

  Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {},
      OptionalJacobian<2, DimK> Dcal = {}) const {
    return _project(pw, Dpose, Dpoint, Dcal);
  }

  Point3 backproject(const Point2& p, double depth,
                     OptionalJacobian<3, 6> Dresult_dpose = {},
                     OptionalJacobian<3, 2> Dresult_dp = {},
                     OptionalJacobian<3, 1> Dresult_ddepth = {},
                     OptionalJacobian<3, DimK> Dresult_dcal = {}) const {
    typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
    Matrix2K Dpn_dcal;
    Matrix22 Dpn_dp;
    const Point2 pn = calibration().calibrate(p, Dresult_dcal ? &Dpn_dcal : 0,
                                                 Dresult_dp ? &Dpn_dp : 0);
    Matrix32 Dpoint_dpn;
    Matrix31 Dpoint_ddepth;
    const Point3 point = BackprojectFromCamera(pn, depth,
                                               (Dresult_dp || Dresult_dcal) ? &Dpoint_dpn : 0,
                                               Dresult_ddepth ? &Dpoint_ddepth : 0);
    Matrix33 Dresult_dpoint;
    const Point3 result = pose().transformFrom(point, Dresult_dpose,
                                                    (Dresult_ddepth ||
                                                     Dresult_dp     ||
                                                     Dresult_dcal) ? &Dresult_dpoint : 0);
    if (Dresult_dcal)
       *Dresult_dcal = Dresult_dpoint * Dpoint_dpn * Dpn_dcal;  // (3x3)*(3x2)*(2xDimK)
    if (Dresult_dp)
       *Dresult_dp =   Dresult_dpoint * Dpoint_dpn * Dpn_dp;    // (3x3)*(3x2)*(2x2)
    if (Dresult_ddepth)
       *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;        // (3x3)*(3x1)

    return result;
  }

  Unit3 backprojectPointAtInfinity(const Point2& p) const {
    const Point2 pn = calibration().calibrate(p);
    const Unit3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
    return pose().rotation().rotate(pc);
  }

  double range(const Point3& point,
      OptionalJacobian<1, 6> Dcamera = {},
      OptionalJacobian<1, 3> Dpoint = {}) const {
    return pose().range(point, Dcamera, Dpoint);
  }

  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = {},
      OptionalJacobian<1, 6> Dpose = {}) const {
    return this->pose().range(pose, Dcamera, Dpose);
  }

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

  template<class CalibrationB>
  double range(const PinholeBaseK<CalibrationB>& camera,
      OptionalJacobian<1, 6> Dcamera = {},
      OptionalJacobian<1, 6> Dother = {}) const {
    return pose().range(camera.pose(), Dcamera, Dother);
  }

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("PinholeBase",
        boost::serialization::base_object<PinholeBase>(*this));
  }
#endif

public:
  GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
// end of class PinholeBaseK

template<typename CALIBRATION>
class PinholePose: public PinholeBaseK<CALIBRATION> {

private:

  typedef PinholeBaseK<CALIBRATION> Base; 
  std::shared_ptr<CALIBRATION> K_; 

public:

  enum {
    dimension = 6
  }; 


  PinholePose() {
  }

  explicit PinholePose(const Pose3& pose) :
      Base(pose), K_(new CALIBRATION()) {
  }

  PinholePose(const Pose3& pose, const std::shared_ptr<CALIBRATION>& K) :
      Base(pose), K_(K) {
  }


  static PinholePose Level(const std::shared_ptr<CALIBRATION>& K,
      const Pose2& pose2, double height) {
    return PinholePose(Base::LevelPose(pose2, height), K);
  }

  static PinholePose Level(const Pose2& pose2, double height) {
    return PinholePose::Level(std::make_shared<CALIBRATION>(), pose2, height);
  }

  static PinholePose Lookat(const Point3& eye, const Point3& target,
      const Point3& upVector, const std::shared_ptr<CALIBRATION>& K =
          std::make_shared<CALIBRATION>()) {
    return PinholePose(Base::LookatPose(eye, target, upVector), K);
  }


  explicit PinholePose(const Vector &v) :
      Base(v), K_(new CALIBRATION()) {
  }

  PinholePose(const Vector &v, const Vector &K) :
      Base(v), K_(new CALIBRATION(K)) {
  }

  // Init from Pose3 and calibration
  PinholePose(const Pose3 &pose, const Vector &K) :
      Base(pose), K_(new CALIBRATION(K)) {
  }


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

  friend std::ostream& operator<<(std::ostream &os, const PinholePose& camera) {
    os << "{R: " << camera.pose().rotation().rpy().transpose();
    os << ", t: " << camera.pose().translation().transpose();
    if (!camera.K_) os << ", K: none";
    else     os << ", K: " << *camera.K_;
    os << "}";
    return os;
  }

  void print(const std::string& s = "PinholePose") const override {
    Base::print(s);
    if (!K_)
      std::cout << "s No calibration given" << std::endl;
    else
      K_->print(s + ".calibration");
  }


  ~PinholePose() override {
  }

  const std::shared_ptr<CALIBRATION>& sharedCalibration() const {
    return K_;
  }

  const CALIBRATION& calibration() const override {
    return *K_;
  }

  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 3> Dpoint = {}) const {
    return Base::project(pw, Dpose, Dpoint);
  }

  Point2 project2(const Unit3& pw, OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {}) const {
    return Base::project(pw, Dpose, Dpoint);
  }


  size_t dim() const {
    return 6;
  }

  static size_t Dim() {
    return 6;
  }

  PinholePose retract(const Vector6& d) const {
    return PinholePose(Base::pose().retract(d), K_);
  }

  Vector6 localCoordinates(const PinholePose& p) const {
    return Base::pose().localCoordinates(p.Base::pose());
  }

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

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

  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
};
// end of class PinholePose

template<typename CALIBRATION>
struct traits<PinholePose<CALIBRATION> > : public internal::Manifold<
    PinholePose<CALIBRATION> > {
};

template<typename CALIBRATION>
struct traits<const PinholePose<CALIBRATION> > : public internal::Manifold<
    PinholePose<CALIBRATION> > {
};

} // \ gtsam