CalibratedCamera.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/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/dllexport.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/nvp.hpp>
#endif
 
namespace gtsam {
 
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<CheiralityException> {
public:
  CheiralityException()
    : CheiralityException(std::numeric_limits<Key>::max()) {}
 
  CheiralityException(Key j)
    : ThreadsafeException<CheiralityException>("CheiralityException"),
      j_(j) {}
 
  Key nearbyVariable() const {return j_;}
 
private:
  Key j_;
};
 
class GTSAM_EXPORT PinholeBase {
 
public:
 
  typedef Rot3 Rotation;
  typedef Point3 Translation;
 
  typedef Point2 Measurement;
  typedef Point2Vector MeasurementVector;
 
private:
 
  Pose3 pose_; 
 
protected:
 
 
  static Matrix26 Dpose(const Point2& pn, double d);
 
  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
 
 
public:
 
 
  static Pose3 LevelPose(const Pose2& pose2, double height);
 
  static Pose3 LookatPose(const Point3& eye, const Point3& target,
      const Point3& upVector);
 
 
  PinholeBase() {}
 
  explicit PinholeBase(const Pose3& pose) : pose_(pose) {}
 
 
  explicit PinholeBase(const Vector& v) : pose_(Pose3::Expmap(v)) {}
 
  virtual ~PinholeBase() = default;
 
 
  bool equals(const PinholeBase &camera, double tol = 1e-9) const;
 
  virtual void print(const std::string& s = "PinholeBase") const;
 
 
  const Pose3& pose() const {
    return pose_;
  }
 
  const Rot3& rotation() const {
    return pose_.rotation();
  }
 
  const Point3& translation() const {
    return pose_.translation();
  }
 
  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
 
 
  static Point2 Project(const Point3& pc, //
      OptionalJacobian<2, 3> Dpoint = {});
 
  static Point2 Project(const Unit3& pc, //
      OptionalJacobian<2, 2> Dpoint = {});
 
  std::pair<Point2, bool> projectSafe(const Point3& pw) const;
 
  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
      {}, OptionalJacobian<2, 3> Dpoint = {}) const;
 
  Point2 project2(const Unit3& point,
      OptionalJacobian<2, 6> Dpose = {},
      OptionalJacobian<2, 2> Dpoint = {}) const;
 
  static Point3 BackprojectFromCamera(const Point2& p, const double depth,
                                      OptionalJacobian<3, 2> Dpoint = {},
                                      OptionalJacobian<3, 1> Ddepth = {});
 
 
  inline static std::pair<size_t, size_t> translationInterval() {
    return {3, 5};
  }
 
 
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_NVP(pose_);
  }
#endif
};
// end of class PinholeBase
 
class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
 
public:
 
  enum {
    dimension = 6
  };
 
 
  CalibratedCamera() {
  }
 
  explicit CalibratedCamera(const Pose3& pose) :
      PinholeBase(pose) {
  }
 
 
  // Create CalibratedCamera, with derivatives
  static CalibratedCamera Create(const Pose3& pose,
                                 OptionalJacobian<dimension, 6> H1 = {}) {
    if (H1)
      *H1 << I_6x6;
    return CalibratedCamera(pose);
  }
 
  static CalibratedCamera Level(const Pose2& pose2, double height);
 
  static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
      const Point3& upVector);
 
 
  explicit CalibratedCamera(const Vector &v) :
      PinholeBase(v) {
  }
 
 
  CalibratedCamera retract(const Vector& d) const;
 
  Vector localCoordinates(const CalibratedCamera& T2) const;
 
  void print(const std::string& s = "CalibratedCamera") const override {
    PinholeBase::print(s);
  }
 
  inline size_t dim() const {
    return dimension;
  }
 
  inline static size_t Dim() {
    return dimension;
  }
 
 
  Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
      {}, OptionalJacobian<2, 3> Dpoint = {}) const;
 
  Point3 backproject(const Point2& pn, double depth,
                     OptionalJacobian<3, 6> Dresult_dpose = {},
                     OptionalJacobian<3, 2> Dresult_dp = {},
                     OptionalJacobian<3, 1> Dresult_ddepth = {}) const {
 
    Matrix32 Dpoint_dpn;
    Matrix31 Dpoint_ddepth;
    const Point3 point = BackprojectFromCamera(pn, depth,
                                                 Dresult_dp ? &Dpoint_dpn : 0,
                                                 Dresult_ddepth ? &Dpoint_ddepth : 0);
 
    Matrix33 Dresult_dpoint;
    const Point3 result = pose().transformFrom(point, Dresult_dpose,
                                                       (Dresult_ddepth ||
                                                        Dresult_dp) ? &Dresult_dpoint : 0);
 
    if (Dresult_dp)
      *Dresult_dp = Dresult_dpoint * Dpoint_dpn;
    if (Dresult_ddepth)
      *Dresult_ddepth = Dresult_dpoint * Dpoint_ddepth;
 
    return result;
  }
 
  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> H1 = {}, //
      OptionalJacobian<1, 6> H2 = {}) const {
    return pose().range(camera.pose(), H1, H2);
  }
 
 
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
 
};
 
// manifold traits
template <>
struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
 
template <>
struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
 
// range traits, used in RangeFactor
template <typename T>
struct Range<CalibratedCamera, T> : HasRange<CalibratedCamera, T, double> {};
 
}  // namespace gtsam