CalibratedCamera now derived from PinholeBase

gtsam/geometry/CalibratedCamera.cpp

@@ -55,33 +55,13 @@ Point2 PinholeBase::project_to_camera(const Point3& P,
 }
 
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Pose3& pose) :
+Point3 PinholeBase::backproject_from_camera(const Point2& p,
-    pose_(pose) {
-}
-
-/* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Vector &v) :
-    pose_(Pose3::Expmap(v)) {
-}
-
-/* ************************************************************************* */
-Point2 CalibratedCamera::project_to_camera(const Point3& P,
-    OptionalJacobian<2, 3> H1) {
-  if (H1) {
-    double d = 1.0 / P.z(), d2 = d * d;
-    *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
-  }
-  return Point2(P.x() / P.z(), P.y() / P.z());
-}
-
-/* ************************************************************************* */
-Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
     const double scale) {
   return Point3(p.x() * scale, p.y() * scale, scale);
 }
 
 /* ************************************************************************* */
-Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) {
+Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
   const double st = sin(pose2.theta()), ct = cos(pose2.theta());
   const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
   const Rot3 wRc(x, y, z);
@@ -90,12 +70,7 @@ Pose3 CalibratedCamera::LevelPose(const Pose2& pose2, double height) {
 }
 
 /* ************************************************************************* */
-CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
+Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
-  return CalibratedCamera(LevelPose(pose2, height));
-}
-
-/* ************************************************************************* */
-Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
     const Point3& upVector) {
   Point3 zc = target - eye;
   zc = zc / zc.norm();
@@ -105,6 +80,11 @@ Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
   return Pose3(Rot3(xc, yc, zc), eye);
 }
 
+/* ************************************************************************* */
+CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
+  return CalibratedCamera(LevelPose(pose2, height));
+}
+
 /* ************************************************************************* */
 CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
     const Point3& target, const Point3& upVector) {
@@ -120,7 +100,7 @@ Point2 CalibratedCamera::project(const Point3& point,
   Matrix3 Dpoint_;
   Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
 #else
-  Point3 q = pose_.transform_to(point);
+  Point3 q = pose().transform_to(point);
 #endif
   Point2 intrinsic = project_to_camera(q);
 
@@ -145,7 +125,7 @@ Point2 CalibratedCamera::project(const Point3& point,
       *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d
           * v;
     if (Dpoint) {
-      const Matrix3 R(pose_.rotation().matrix());
+      const Matrix3 R(pose().rotation().matrix());
       Matrix23 Dpoint_;
       Dpoint_ << 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)

gtsam/geometry/CalibratedCamera.h

@@ -44,6 +44,30 @@ private:
 
 public:
 
+  /// @name Static functions
+  /// @{
+
+  /**
+   * Create a level pose at the given 2D pose and height
+   * @param K the calibration
+   * @param pose2 specifies the location and viewing direction
+   * (theta 0 = looking in direction of positive X axis)
+   * @param height camera height
+   */
+  static Pose3 LevelPose(const Pose2& pose2, double height);
+
+  /**
+   * Create a camera pose at the given eye position looking at a target point in the scene
+   * with the specified up direction vector.
+   * @param eye specifies the camera position
+   * @param target the point to look at
+   * @param upVector specifies the camera up direction vector,
+   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+   */
+  static Pose3 LookatPose(const Point3& eye, const Point3& target,
+      const Point3& upVector);
+
+  /// @}
   /// @name Standard Constructors
   /// @{
 
@@ -102,6 +126,11 @@ public:
   static Point2 project_to_camera(const Point3& P, //
       OptionalJacobian<2, 3> Dpoint = boost::none);
 
+  /**
+   * backproject a 2-dimensional point to a 3-dimension point
+   */
+  static Point3 backproject_from_camera(const Point2& p, const double scale);
+
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
@@ -195,9 +224,7 @@ private:
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT CalibratedCamera {
+class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
-private:
-  Pose3 pose_; // 6DOF pose
 
 public:
 
@@ -213,21 +240,14 @@ public:
   }
 
   /// construct with pose
-  explicit CalibratedCamera(const Pose3& pose);
+  explicit CalibratedCamera(const Pose3& pose) :
+      PinholeBase(pose) {
+  }
 
   /// @}
   /// @name Named Constructors
   /// @{
 
-  /**
-   * Create a level pose at the given 2D pose and height
-   * @param K the calibration
-   * @param pose2 specifies the location and viewing direction
-   * (theta 0 = looking in direction of positive X axis)
-   * @param height camera height
-   */
-  static Pose3 LevelPose(const Pose2& pose2, double height);
-
   /**
    * Create a level camera at the given 2D pose and height
    * @param pose2 specifies the location and viewing direction
@@ -236,17 +256,6 @@ public:
    */
   static CalibratedCamera Level(const Pose2& pose2, double height);
 
-  /**
-   * Create a camera pose at the given eye position looking at a target point in the scene
-   * with the specified up direction vector.
-   * @param eye specifies the camera position
-   * @param target the point to look at
-   * @param upVector specifies the camera up direction vector,
-   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
-   */
-  static Pose3 LookatPose(const Point3& eye, const Point3& target,
-      const Point3& upVector);
-
   /**
    * Create a camera at the given eye position looking at a target point in the scene
    * with the specified up direction vector.
@@ -263,19 +272,8 @@ public:
   /// @{
 
   /// construct from vector
-  explicit CalibratedCamera(const Vector &v);
+  explicit CalibratedCamera(const Vector &v) :
-
+      PinholeBase(v) {
-  /// @}
-  /// @name Testable
-  /// @{
-
-  virtual void print(const std::string& s = "") const {
-    pose_.print(s);
-  }
-
-  /// check equality to another camera
-  bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
-    return pose_.equals(camera.pose(), tol);
   }
 
   /// @}
@@ -286,11 +284,6 @@ public:
   virtual ~CalibratedCamera() {
   }
 
-  /// return pose
-  inline const Pose3& pose() const {
-    return pose_;
-  }
-
   /// @}
   /// @name Manifold
   /// @{
@@ -311,10 +304,6 @@ public:
     return 6;
   }
 
-  /* ************************************************************************* */
-  // measurement functions and derivatives
-  /* ************************************************************************* */
-
   /// @}
   /// @name Transformations and mesaurement functions
   /// @{
@@ -330,43 +319,6 @@ public:
       OptionalJacobian<2, 6> Dpose = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
-  /**
-   * projects a 3-dimensional point in camera coordinates into the
-   * camera and returns a 2-dimensional point, no calibration applied
-   * With optional 2by3 derivative
-   */
-  static Point2 project_to_camera(const Point3& cameraPoint,
-      OptionalJacobian<2, 3> H1 = boost::none);
-
-  /**
-   * backproject a 2-dimensional point to a 3-dimension point
-   */
-  static Point3 backproject_from_camera(const Point2& p, const double scale);
-
-  /**
-   * Calculate range to a landmark
-   * @param point 3D location of landmark
-   * @param H1 optionally computed Jacobian with respect to pose
-   * @param H2 optionally computed Jacobian with respect to the 3D point
-   * @return range (double)
-   */
-  double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
-      OptionalJacobian<1, 3> H2 = boost::none) const {
-    return pose_.range(point, H1, H2);
-  }
-
-  /**
-   * Calculate range to another pose
-   * @param pose Other SO(3) pose
-   * @param H1 optionally computed Jacobian with respect to pose
-   * @param H2 optionally computed Jacobian with respect to the 3D point
-   * @return range (double)
-   */
-  double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
-      OptionalJacobian<1, 6> H2 = boost::none) const {
-    return pose_.range(pose, H1, H2);
-  }
-
   /**
    * Calculate range to another camera
    * @param camera Other camera
@@ -376,12 +328,13 @@ public:
    */
   double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
       boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
-    return pose_.range(camera.pose_, H1, H2);
+    return pose().range(camera.pose(), H1, H2);
   }
 
+  /// @}
+
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 
@@ -389,7 +342,9 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(pose_);
+    ar
+        & boost::serialization::make_nvp("PinholeBase",
+            boost::serialization::base_object<PinholeBase>(*this));
   }
 
   /// @}