Put PinholeBase in CalibratedCamera

gtsam/geometry/CalibratedCamera.cpp

@@ -21,6 +21,39 @@
 
 namespace gtsam {
 
+/* ************************************************************************* */
+bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
+  return pose_.equals(camera.pose(), tol);
+}
+
+/* ************************************************************************* */
+void PinholeBase::print(const std::string& s) const {
+  pose_.print(s + ".pose");
+}
+
+/* ************************************************************************* */
+const Pose3& PinholeBase::pose(OptionalJacobian<6, 6> H) const {
+  if (H) {
+    H->setZero();
+    H->block(0, 0, 6, 6) = I_6x6;
+  }
+  return pose_;
+}
+
+/* ************************************************************************* */
+Point2 PinholeBase::project_to_camera(const Point3& P,
+    OptionalJacobian<2, 3> Dpoint) {
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  if (P.z() <= 0)
+  throw CheiralityException();
+#endif
+  double d = 1.0 / P.z();
+  const double u = P.x() * d, v = P.y() * d;
+  if (Dpoint)
+    *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
+  return Point2(u, v);
+}
+
 /* ************************************************************************* */
 CalibratedCamera::CalibratedCamera(const Pose3& pose) :
     pose_(pose) {

gtsam/geometry/CalibratedCamera.h

@@ -19,11 +19,10 @@
 #pragma once
 
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
-class Point2;
-
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
@@ -32,6 +31,163 @@ public:
   }
 };
 
+/**
+ * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT PinholeBase {
+
+private:
+
+  Pose3 pose_; ///< 3D pose of camera
+
+public:
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholeBase() {
+  }
+
+  /** constructor with pose */
+  explicit PinholeBase(const Pose3& pose) :
+      pose_(pose) {
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  explicit PinholeBase(const Vector &v) :
+      pose_(Pose3::Expmap(v)) {
+  }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const PinholeBase &camera, double tol = 1e-9) const;
+
+  /// print
+  void print(const std::string& s = "PinholeBase") const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~PinholeBase() {
+  }
+
+  /// return pose, constant version
+  const Pose3& pose() const {
+    return pose_;
+  }
+
+  /// return pose, with derivative
+  const Pose3& pose(OptionalJacobian<6, 6> H) const;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /**
+   * projects a 3-dimensional point in camera coordinates into the
+   * camera and returns a 2-dimensional point
+   * @param P A point in camera coordinates
+   * @param Dpoint is the 2*3 Jacobian w.r.t. P
+   */
+  static Point2 project_to_camera(const Point3& P, //
+      OptionalJacobian<2, 3> Dpoint = boost::none);
+
+  /**
+   * Calculate range to a landmark
+   * @param point 3D location of landmark
+   * @param Dcamera the optionally computed Jacobian with respect to pose
+   * @param Dpoint the optionally computed Jacobian with respect to the landmark
+   * @return range (double)
+   */
+  double range(
+      const Point3& point, //
+      OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 3> Dpoint = boost::none) const {
+    return pose_.range(point, Dcamera, Dpoint);
+  }
+
+  /**
+   * Calculate range to another pose
+   * @param pose Other SO(3) pose
+   * @param Dcamera the optionally computed Jacobian with respect to pose
+   * @param Dpose2 the optionally computed Jacobian with respect to the other pose
+   * @return range (double)
+   */
+  double range(
+      const Pose3& pose, //
+      OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
+    return pose_.range(pose, Dcamera, Dpose);
+  }
+
+protected:
+
+  /**
+   * Calculate Jacobian with respect to pose
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Dpi_pn derivative of uncalibrate with respect to pn
+   * @param Dpose Output argument, can be matrix or block, assumed right size !
+   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
+   */
+  template<typename Derived>
+  static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
+      Eigen::MatrixBase<Derived> const & Dpose) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    double uv = u * v, uu = u * u, vv = v * v;
+    Matrix26 Dpn_pose;
+    Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
+    assert(Dpose.rows()==2 && Dpose.cols()==6);
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
+        Dpi_pn * Dpn_pose;
+  }
+
+  /**
+   * Calculate Jacobian with respect to point
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Dpi_pn derivative of uncalibrate with respect to pn
+   * @param Dpoint Output argument, can be matrix or block, assumed right size !
+   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
+   */
+  template<typename Derived>
+  static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
+      const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    Matrix23 Dpn_point;
+    Dpn_point << //
+        R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
+    /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
+    Dpn_point *= d;
+    assert(Dpoint.rows()==2 && Dpoint.cols()==3);
+    const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
+        Dpi_pn * Dpn_point;
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(pose_);
+  }
+
+};
+// end of class PinholeBase
+
 /**
  * A Calibrated camera class [R|-R't], calibration K=I.
  * If calibration is known, it is more computationally efficient