Merged in feature/PinholePose (pull request #108)

Revamped Pinhole Cameras

.cproject

@@ -1019,6 +1019,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testPinholePose.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testPinholePose.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1285,7 +1293,6 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1325,7 +1332,6 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1333,7 +1339,6 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1437,6 +1442,7 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1739,6 +1745,7 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1746,6 +1753,7 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1753,6 +1761,7 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1760,6 +1769,7 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1951,7 +1961,6 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2103,6 +2112,7 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2110,6 +2120,7 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2157,6 +2168,7 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2164,6 +2176,7 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2171,6 +2184,7 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2186,6 +2200,7 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3305,6 +3320,7 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3312,6 +3328,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -3319,6 +3336,7 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

examples/Pose2SLAMExample_g2o.cpp

@@ -20,6 +20,7 @@
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
 

examples/Pose2SLAMExample_lago.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <fstream>
 
 using namespace std;

gtsam/discrete/DecisionTree-inl.h

@@ -462,15 +462,17 @@ namespace gtsam {
   // cannot just create a root Choice node on the label: if the label is not the
   // highest label, we need to do a complicated and expensive recursive call.
   template<typename L, typename Y> template<typename Iterator>
-  typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(
+  typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(Iterator begin,
-      Iterator begin, Iterator end, const L& label) const {
+      Iterator end, const L& label) const {
 
     // find highest label among branches
     boost::optional<L> highestLabel;
     boost::optional<size_t> nrChoices;
     for (Iterator it = begin; it != end; it++) {
-      if (it->root_->isLeaf()) continue;
+      if (it->root_->isLeaf())
-      boost::shared_ptr<const Choice> c = boost::dynamic_pointer_cast<const Choice> (it->root_);
+        continue;
+      boost::shared_ptr<const Choice> c =
+          boost::dynamic_pointer_cast<const Choice>(it->root_);
       if (!highestLabel || c->label() > *highestLabel) {
         highestLabel.reset(c->label());
         nrChoices.reset(c->nrChoices());
@@ -478,15 +480,15 @@ namespace gtsam {
     }
 
     // if label is already in correct order, just put together a choice on label
-    if (!highestLabel || label > *highestLabel) {
+    if (!highestLabel || !nrChoices || label > *highestLabel) {
       boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
       for (Iterator it = begin; it != end; it++)
         choiceOnLabel->push_back(it->root_);
       return Choice::Unique(choiceOnLabel);
-    }
+    } else {
-
       // Set up a new choice on the highest label
-    boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices));
+      boost::shared_ptr<Choice> choiceOnHighestLabel(
+          new Choice(*highestLabel, *nrChoices));
       // now, for all possible values of highestLabel
       for (size_t index = 0; index < *nrChoices; index++) {
         // make a new set of functions for composing by iterating over the given
@@ -503,6 +505,7 @@ namespace gtsam {
       }
       return Choice::Unique(choiceOnHighestLabel);
     }
+  }
 
   /*********************************************************************************/
   // "create" is a bit of a complicated thing, but very useful.
@@ -667,9 +670,10 @@ namespace gtsam {
   void DecisionTree<L, Y>::dot(const std::string& name, bool showZero) const {
     std::ofstream os((name + ".dot").c_str());
     dot(os, showZero);
-    system(
+    int result = system(
         ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str());
-  }
+    if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed");
+}
 
 /*********************************************************************************/
 

gtsam/geometry/CalibratedCamera.cpp

@@ -19,88 +19,131 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 
+using namespace std;
+
 namespace gtsam {
 
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Pose3& pose) :
+Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
-    pose_(pose) {
+  // 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;
+  return Dpn_pose;
 }
 
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Vector &v) :
+Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
-    pose_(Pose3::Expmap(v)) {
+  // optimized version of derivatives, see CalibratedCamera.nb
+  const double u = pn.x(), v = pn.y();
+  Matrix23 Dpn_point;
+  Dpn_point << //
+      Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
+  /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
+  Dpn_point *= d;
+  return Dpn_point;
 }
 
 /* ************************************************************************* */
-Point2 CalibratedCamera::project_to_camera(const Point3& P,
+Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
-    OptionalJacobian<2,3> H1) {
+  const double st = sin(pose2.theta()), ct = cos(pose2.theta());
-  if (H1) {
+  const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
-    double d = 1.0 / P.z(), d2 = d * d;
+  const Rot3 wRc(x, y, z);
-    *H1 <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
+  const Point3 t(pose2.x(), pose2.y(), height);
+  return Pose3(wRc, t);
+}
+
+/* ************************************************************************* */
+Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
+    const Point3& upVector) {
+  Point3 zc = target - eye;
+  zc = zc / zc.norm();
+  Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
+  xc = xc / xc.norm();
+  Point3 yc = zc.cross(xc);
+  return Pose3(Rot3(xc, yc, zc), eye);
+}
+
+/* ************************************************************************* */
+bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
+  return pose_.equals(camera.pose(), tol);
+}
+
+/* ************************************************************************* */
+void PinholeBase::print(const string& s) const {
+  pose_.print(s + ".pose");
+}
+
+/* ************************************************************************* */
+const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
+  if (H) {
+    H->setZero();
+    H->block(0, 0, 6, 6) = I_6x6;
   }
-  return Point2(P.x() / P.z(), P.y() / P.z());
+  return pose_;
 }
 
 /* ************************************************************************* */
-Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
+Point2 PinholeBase::project_to_camera(const Point3& pc,
-    const double scale) {
+    OptionalJacobian<2, 3> Dpoint) {
-  return Point3(p.x() * scale, p.y() * scale, scale);
+  double d = 1.0 / pc.z();
+  const double u = pc.x() * d, v = pc.y() * d;
+  if (Dpoint)
+    *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
+  return Point2(u, v);
+}
+
+/* ************************************************************************* */
+pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
+  const Point3 pc = pose().transform_to(pw);
+  const Point2 pn = project_to_camera(pc);
+  return make_pair(pn, pc.z() > 0);
+}
+
+/* ************************************************************************* */
+Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 3> Dpoint) const {
+
+  Matrix3 Rt; // calculated by transform_to if needed
+  const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  if (q.z() <= 0)
+  throw CheiralityException();
+#endif
+  const Point2 pn = project_to_camera(q);
+
+  if (Dpose || Dpoint) {
+    const double d = 1.0 / q.z();
+    if (Dpose)
+      *Dpose = PinholeBase::Dpose(pn, d);
+    if (Dpoint)
+      *Dpoint = PinholeBase::Dpoint(pn, d, Rt);
+  }
+  return pn;
+}
+
+/* ************************************************************************* */
+Point3 PinholeBase::backproject_from_camera(const Point2& p,
+    const double depth) {
+  return Point3(p.x() * depth, p.y() * depth, depth);
 }
 
 /* ************************************************************************* */
 CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
-  double st = sin(pose2.theta()), ct = cos(pose2.theta());
+  return CalibratedCamera(LevelPose(pose2, height));
-  Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
+}
-  Rot3 wRc(x, y, z);
+
-  Point3 t(pose2.x(), pose2.y(), height);
+/* ************************************************************************* */
-  Pose3 pose3(wRc, t);
+CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
-  return CalibratedCamera(pose3);
+    const Point3& target, const Point3& upVector) {
+  return CalibratedCamera(LookatPose(eye, target, upVector));
 }
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
-    OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
+    OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
-
+  return project2(point, Dcamera, Dpoint);
-#ifdef CALIBRATEDCAMERA_CHAIN_RULE
-  Matrix36 Dpose_;
-  Matrix3 Dpoint_;
-  Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
-#else
-  Point3 q = pose_.transform_to(point);
-#endif
-  Point2 intrinsic = project_to_camera(q);
-
-  // Check if point is in front of camera
-  if (q.z() <= 0)
-    throw CheiralityException();
-
-  if (Dpose || Dpoint) {
-#ifdef CALIBRATEDCAMERA_CHAIN_RULE
-    // just implement chain rule
-    if(Dpose && Dpoint) {
-      Matrix23 H;
-      project_to_camera(q,H);
-      if (Dpose) *Dpose = H * (*Dpose_);
-      if (Dpoint) *Dpoint = H * (*Dpoint_);
-    }
-#else
-    // optimized version, see CalibratedCamera.nb
-    const double z = q.z(), d = 1.0 / z;
-    const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
-    if (Dpose)
-      *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());
-      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) - v * R(2, 2);
-      *Dpoint = d * Dpoint_;
-    }
-#endif
-  }
-  return intrinsic;
 }
 
 /* ************************************************************************* */

gtsam/geometry/CalibratedCamera.h

@@ -19,10 +19,11 @@
 #pragma once
 
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
+class Point2;
+
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
@@ -31,6 +32,146 @@ 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
+
+protected:
+
+  /// @name Derivatives
+  /// @{
+
+  /**
+   * Calculate Jacobian with respect to pose
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   */
+  static Matrix26 Dpose(const Point2& pn, double d);
+
+  /**
+   * Calculate Jacobian with respect to point
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Rt transposed rotation matrix
+   */
+  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
+
+  /// @}
+
+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
+  /// @{
+
+  /** 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
+  /// @{
+
+  /// return pose, constant version
+  const Pose3& pose() const {
+    return pose_;
+  }
+
+  /// return pose, with derivative
+  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /**
+   * Project from 3D point in camera coordinates into image
+   * Does *not* throw a CheiralityException, even if pc behind image plane
+   * @param pc point in camera coordinates
+   */
+  static Point2 project_to_camera(const Point3& pc, //
+      OptionalJacobian<2, 3> Dpoint = boost::none);
+
+  /// Project a point into the image and check depth
+  std::pair<Point2, bool> projectSafe(const Point3& pw) const;
+
+  /**
+   * Project point into the image
+   * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  static Point3 backproject_from_camera(const Point2& p, const double depth);
+
+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
@@ -38,13 +179,13 @@ public:
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT CalibratedCamera {
+class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
-private:
-  Pose3 pose_; // 6DOF pose
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
@@ -54,26 +195,40 @@ public:
   }
 
   /// construct with pose
-  explicit CalibratedCamera(const Pose3& pose);
+  explicit CalibratedCamera(const Pose3& pose) :
+      PinholeBase(pose) {
+  }
+
+  /// @}
+  /// @name Named Constructors
+  /// @{
+
+  /**
+   * Create a level camera at the given 2D pose and height
+   * @param pose2 specifies the location and viewing direction
+   * @param height specifies the height of the camera (along the positive Z-axis)
+   * (theta 0 = looking in direction of positive X axis)
+   */
+  static CalibratedCamera Level(const Pose2& pose2, double height);
+
+  /**
+   * Create a camera 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 CalibratedCamera Lookat(const Point3& eye, const Point3& target,
+      const Point3& upVector);
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
   /// 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);
   }
 
   /// @}
@@ -84,19 +239,6 @@ public:
   virtual ~CalibratedCamera() {
   }
 
-  /// return pose
-  inline const Pose3& pose() const {
-    return pose_;
-  }
-
-  /**
-   * Create a level camera at the given 2D pose and height
-   * @param pose2 specifies the location and viewing direction
-   * @param height specifies the height of the camera (along the positive Z-axis)
-   * (theta 0 = looking in direction of positive X axis)
-   */
-  static CalibratedCamera Level(const Pose2& pose2, double height);
-
   /// @}
   /// @name Manifold
   /// @{
@@ -107,87 +249,68 @@ public:
   /// Return canonical coordinate
   Vector localCoordinates(const CalibratedCamera& T2) const;
 
-  /// Lie group dimensionality
+  /// @deprecated
   inline size_t dim() const {
     return 6;
   }
 
-  /// Lie group dimensionality
+  /// @deprecated
   inline static size_t Dim() {
     return 6;
   }
 
-  /* ************************************************************************* */
-  // measurement functions and derivatives
-  /* ************************************************************************* */
-
   /// @}
   /// @name Transformations and mesaurement functions
   /// @{
-  /**
-   * This function receives the camera pose and the landmark location and
-   * returns the location the point is supposed to appear in the image
-   * @param point a 3D point to be projected
-   * @param Dpose the optionally computed Jacobian with respect to pose
-   * @param Dpoint the optionally computed Jacobian with respect to the 3D point
-   * @return the intrinsic coordinates of the projected point
-   */
-  Point2 project(const Point3& point,
-      OptionalJacobian<2, 6> Dpose = boost::none,
-      OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /**
-   * projects a 3-dimensional point in camera coordinates into the
+   * @deprecated
-   * camera and returns a 2-dimensional point, no calibration applied
+   * Use project2, which is more consistently named across Pinhole cameras
-   * With optional 2by3 derivative
    */
-  static Point2 project_to_camera(const Point3& cameraPoint,
+  Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
-      OptionalJacobian<2, 3> H1 = boost::none);
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
-  /**
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-   * backproject a 2-dimensional point to a 3-dimension point
+  Point3 backproject(const Point2& pn, double depth) const {
-   */
+    return pose().transform_from(backproject_from_camera(pn, depth));
-  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,
+  double range(const Point3& point,
-      OptionalJacobian<1, 3> H2 = boost::none) const {
+      OptionalJacobian<1, 6> Dcamera = boost::none,
-    return pose_.range(point, H1, H2);
+      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 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,
+  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
-      OptionalJacobian<1, 6> H2 = boost::none) const {
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
-    return pose_.range(pose, H1, H2);
+    return this->pose().range(pose, Dcamera, Dpose);
   }
 
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @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 CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
+  double range(const CalibratedCamera& camera, //
-      boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
+      OptionalJacobian<1, 6> H1 = boost::none, //
-    return pose_.range(camera.pose_, H1, H2);
+      OptionalJacobian<1, 6> H2 = boost::none) const {
+    return pose().range(camera.pose(), H1, H2);
   }
 
+  /// @}
+
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 
@@ -195,17 +318,22 @@ 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));
   }
 
   /// @}
 };
 
 template<>
-struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
+};
 
 template<>
-struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+struct traits<const CalibratedCamera> : public internal::Manifold<
+    CalibratedCamera> {
+};
 
 }
 

gtsam/geometry/PinholeCamera.h

@@ -18,32 +18,32 @@
 
 #pragma once
 
-#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholePose.h>
-#include <gtsam/geometry/Pose2.h>
-#include <cmath>
 
 namespace gtsam {
 
 /**
  * A pinhole camera class that has a Pose3 and a Calibration.
+ * Use PinholePose if you will not be optimizing for Calibration
  * @addtogroup geometry
  * \nosubgrouping
  */
 template<typename Calibration>
-class PinholeCamera {
+class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
 private:
-  Pose3 pose_;
-  Calibration K_;
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
+  typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
+  Calibration K_; ///< Calibration, part of class now
 
   // Get dimensions of calibration type at compile time
   static const int DimK = FixedDimension<Calibration>::value;
 
 public:
 
-  enum { dimension = 6 + DimK };
+  enum {
+    dimension = 6 + DimK
+  }; ///< Dimension depends on calibration
 
   /// @name Standard Constructors
   /// @{
@@ -54,12 +54,12 @@ public:
 
   /** constructor with pose */
   explicit PinholeCamera(const Pose3& pose) :
-      pose_(pose) {
+      Base(pose) {
   }
 
   /** constructor with pose and calibration */
   PinholeCamera(const Pose3& pose, const Calibration& K) :
-      pose_(pose), K_(K) {
+      Base(pose), K_(K) {
   }
 
   /// @}
@@ -75,12 +75,7 @@ public:
    */
   static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
       double height) {
-    const double st = sin(pose2.theta()), ct = cos(pose2.theta());
+    return PinholeCamera(Base::LevelPose(pose2, height), K);
-    const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
-    const Rot3 wRc(x, y, z);
-    const Point3 t(pose2.x(), pose2.y(), height);
-    const Pose3 pose3(wRc, t);
-    return PinholeCamera(pose3, K);
   }
 
   /// PinholeCamera::level with default calibration
@@ -99,28 +94,23 @@ public:
    */
   static PinholeCamera Lookat(const Point3& eye, const Point3& target,
       const Point3& upVector, const Calibration& K = Calibration()) {
-    Point3 zc = target - eye;
+    return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
-    zc = zc / zc.norm();
-    Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
-    xc = xc / xc.norm();
-    Point3 yc = zc.cross(xc);
-    Pose3 pose3(Rot3(xc, yc, zc), eye);
-    return PinholeCamera(pose3, K);
   }
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  explicit PinholeCamera(const Vector &v) {
+  /// Init from vector, can be 6D (default calibration) or dim
-    pose_ = Pose3::Expmap(v.head(6));
+  explicit PinholeCamera(const Vector &v) :
-    if (v.size() > 6) {
+      Base(v.head<6>()) {
-      K_ = Calibration(v.tail(DimK));
+    if (v.size() > 6)
-    }
+      K_ = Calibration(v.tail<DimK>());
   }
 
+  /// Init from Vector and calibration
   PinholeCamera(const Vector &v, const Vector &K) :
-      pose_(Pose3::Expmap(v)), K_(K) {
+      Base(v), K_(K) {
   }
 
   /// @}
@@ -128,14 +118,14 @@ public:
   /// @{
 
   /// assert equality up to a tolerance
-  bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
+  bool equals(const Base &camera, double tol = 1e-9) const {
-    return pose_.equals(camera.pose(), tol)
+    const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
-        && K_.equals(camera.calibration(), tol);
+    return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
   }
 
   /// print
   void print(const std::string& s = "PinholeCamera") const {
-    pose_.print(s + ".pose");
+    Base::print(s);
     K_.print(s + ".calibration");
   }
 
@@ -147,31 +137,21 @@ public:
   }
 
   /// return pose
-  inline Pose3& pose() {
+  const Pose3& pose() const {
-    return pose_;
+    return Base::pose();
-  }
-
-  /// return pose, constant version
-  inline const Pose3& pose() const {
-    return pose_;
   }
 
   /// return pose, with derivative
-  inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
+  const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
     if (H) {
       H->setZero();
       H->block(0, 0, 6, 6) = I_6x6;
     }
-    return pose_;
+    return Base::pose();
   }
 
   /// return calibration
-  inline Calibration& calibration() {
+  const Calibration& calibration() const {
-    return K_;
-  }
-
-  /// return calibration
-  inline const Calibration& calibration() const {
     return K_;
   }
 
@@ -179,13 +159,13 @@ public:
   /// @name Manifold
   /// @{
 
-  /// Manifold dimension
+  /// @deprecated
-  inline size_t dim() const {
+  size_t dim() const {
     return dimension;
   }
 
-  /// Manifold dimension
+  /// @deprecated
-  inline static size_t Dim() {
+  static size_t Dim() {
     return dimension;
   }
 
@@ -194,9 +174,9 @@ public:
   /// move a cameras according to d
   PinholeCamera retract(const Vector& d) const {
     if ((size_t) d.size() == 6)
-      return PinholeCamera(pose().retract(d), calibration());
+      return PinholeCamera(this->pose().retract(d), calibration());
     else
-      return PinholeCamera(pose().retract(d.head(6)),
+      return PinholeCamera(this->pose().retract(d.head(6)),
           calibration().retract(d.tail(calibration().dim())));
   }
 
@@ -204,7 +184,7 @@ public:
   VectorK6 localCoordinates(const PinholeCamera& T2) const {
     VectorK6 d;
     // TODO: why does d.head<6>() not compile??
-    d.head(6) = pose().localCoordinates(T2.pose());
+    d.head(6) = this->pose().localCoordinates(T2.pose());
     d.tail(DimK) = calibration().localCoordinates(T2.calibration());
     return d;
   }
@@ -218,32 +198,6 @@ public:
   /// @name Transformations and measurement functions
   /// @{
 
-  /**
-   * projects a 3-dimensional point in camera coordinates into the
-   * camera and returns a 2-dimensional point, no calibration applied
-   * @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) {
-#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);
-  }
-
-  /// Project a point into the image and check depth
-  inline std::pair<Point2, bool> projectSafe(const Point3& pw) const {
-    const Point3 pc = pose_.transform_to(pw);
-    const Point2 pn = project_to_camera(pc);
-    return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
-  }
-
   typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
 
   /** project a point from world coordinate to the image
@@ -252,31 +206,25 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
+  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
-    // Transform to camera coordinates and check cheirality
+    // project to normalized coordinates
-    const Point3 pc = pose_.transform_to(pw);
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
 
-    // Project to normalized image coordinates
+    // uncalibrate to pixel coordinates
-    const Point2 pn = project_to_camera(pc);
-
-    if (Dpose || Dpoint) {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
     Matrix2 Dpi_pn;
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+    const Point2 pi = calibration().uncalibrate(pn, Dcal,
+        Dpose || Dpoint ? &Dpi_pn : 0);
 
-      // chain the Jacobian matrices
+    // If needed, apply chain rule
     if (Dpose)
-        calculateDpose(pn, d, Dpi_pn, *Dpose);
+      *Dpose = Dpi_pn * *Dpose;
     if (Dpoint)
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
+      *Dpoint = Dpi_pn * *Dpoint;
+
     return pi;
-    } else
-      return K_.uncalibrate(pn, Dcal);
   }
 
   /** project a point at infinity from world coordinate to the image
@@ -285,20 +233,19 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 projectPointAtInfinity(const Point3& pw,
+  Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
-      OptionalJacobian<2, 6> Dpose = boost::none,
+      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
-      OptionalJacobian<2, 2> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
     if (!Dpose && !Dpoint && !Dcal) {
-      const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
+      const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
-      const Point2 pn = project_to_camera(pc); // project the point to the camera
+      const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
       return K_.uncalibrate(pn);
     }
 
     // world to camera coordinate
     Matrix3 Dpc_rot, Dpc_point;
-    const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
+    const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
 
     Matrix36 Dpc_pose;
     Dpc_pose.setZero();
@@ -306,7 +253,7 @@ public:
 
     // camera to normalized image coordinate
     Matrix23 Dpn_pc; // 2*3
-    const Point2 pn = project_to_camera(pc, Dpn_pc);
+    const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
 
     // uncalibration
     Matrix2 Dpi_pn; // 2*2
@@ -323,65 +270,40 @@ public:
 
   /** project a point from world coordinate to the image, fixed Jacobians
    *  @param pw is a point in the world coordinate
-   *  @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
-   *  @param Dpoint is the Jacobian w.r.t. point3
    */
   Point2 project2(
       const Point3& pw, //
       OptionalJacobian<2, dimension> Dcamera = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none) const {
 
-    const Point3 pc = pose_.transform_to(pw);
+    // project to normalized coordinates
-    const Point2 pn = project_to_camera(pc);
+    Matrix26 Dpose;
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
 
-    if (!Dcamera && !Dpoint) {
+    // uncalibrate to pixel coordinates
-      return K_.uncalibrate(pn);
-    } else {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
     Matrix2K Dcal;
     Matrix2 Dpi_pn;
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+    const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
+        Dcamera || Dpoint ? &Dpi_pn : 0);
+
+    // If needed, calculate derivatives
+    if (Dcamera)
+      *Dcamera << Dpi_pn * Dpose, Dcal;
+    if (Dpoint)
+      *Dpoint = Dpi_pn * (*Dpoint);
 
-      if (Dcamera) { // TODO why does leftCols<6>() not compile ??
-        calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
-        (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
-      }
-      if (Dpoint) {
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
-      }
     return pi;
   }
-  }
-
-  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-  inline Point3 backproject(const Point2& p, double depth) const {
-    const Point2 pn = K_.calibrate(p);
-    const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
-    return pose_.transform_from(pc);
-  }
-
-  /// backproject a 2-dimensional point to a 3-dimensional point at infinity
-  inline Point3 backprojectPointAtInfinity(const Point2& p) const {
-    const Point2 pn = K_.calibrate(p);
-    const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
-    return pose_.rotation().rotate(pc);
-  }
 
   /**
    * 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(
+  double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
-      const Point3& point, //
+      boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
-      OptionalJacobian<1, dimension> Dcamera = boost::none,
-      OptionalJacobian<1, 3> Dpoint = boost::none) const {
     Matrix16 Dpose_;
-    double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
+    double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
     if (Dcamera)
       *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
     return result;
@@ -390,16 +312,12 @@ public:
   /**
    * 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(
+  double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
-      const Pose3& pose, //
+      boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
-      OptionalJacobian<1, dimension> Dcamera = boost::none,
-      OptionalJacobian<1, 6> Dpose = boost::none) const {
     Matrix16 Dpose_;
-    double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
+    double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
     if (Dcamera)
       *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
     return result;
@@ -408,26 +326,21 @@ public:
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
   template<class CalibrationB>
-  double range(
+  double range(const PinholeCamera<CalibrationB>& camera,
-      const PinholeCamera<CalibrationB>& camera, //
       OptionalJacobian<1, dimension> Dcamera = boost::none,
-//      OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
+      boost::optional<Matrix&> Dother = boost::none) const {
-       boost::optional<Matrix&> Dother =
-          boost::none) const {
     Matrix16 Dcamera_, Dother_;
-    double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
+    double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
         Dother ? &Dother_ : 0);
     if (Dcamera) {
       Dcamera->resize(1, 6 + DimK);
       *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
     }
     if (Dother) {
-      Dother->resize(1, 6+CalibrationB::dimension);
+      Dother->resize(1, 6 + CalibrationB::dimension);
       Dother->setZero();
       Dother->block(0, 0, 1, 6) = Dother_;
     }
@@ -437,12 +350,9 @@ public:
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
-  double range(
+  double range(const CalibratedCamera& camera,
-      const CalibratedCamera& camera, //
       OptionalJacobian<1, dimension> Dcamera = boost::none,
       OptionalJacobian<1, 6> Dother = boost::none) const {
     return range(camera.pose(), Dcamera, Dother);
@@ -450,65 +360,26 @@ public:
 
 private:
 
-  /**
-   * 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;
-  }
-
   /** Serialization function */
   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("PinholeBaseK",
+            boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
 
 };
 
+template<typename Calibration>
+struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
+    PinholeCamera<Calibration> > {
+};
 
 template<typename Calibration>
-struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
+struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
-
+    PinholeCamera<Calibration> > {
-template<typename Calibration>
+};
-struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
 
 } // \ gtsam

gtsam/geometry/PinholePose.h

@@ -0,0 +1,344 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   PinholePose.h
+ * @brief  Pinhole camera with known calibration
+ * @author Yong-Dian Jian
+ * @author Frank Dellaert
+ * @date   Feb 20, 2015
+ */
+
+#pragma once
+
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+/**
+ * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+template<typename Calibration>
+class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
+
+  GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
+
+public  :
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholeBaseK() {
+  }
+
+  /** constructor with pose */
+  explicit PinholeBaseK(const Pose3& pose) :
+  PinholeBase(pose) {
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  explicit PinholeBaseK(const Vector &v) :
+  PinholeBase(v) {
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~PinholeBaseK() {
+  }
+
+  /// return calibration
+  virtual const Calibration& calibration() const = 0;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /// Project a point into the image and check depth
+  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;
+  }
+
+  /** project a point from world coordinate to the image, fixed Jacobians
+   *  @param pw is a point in the world coordinates
+   */
+  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none) 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, boost::none,
+        Dpose || Dpoint ? &Dpi_pn : 0);
+
+    // If needed, apply chain rule
+    if (Dpose) *Dpose = Dpi_pn * (*Dpose);
+    if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
+
+    return pi;
+  }
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  Point3 backproject(const Point2& p, double depth) const {
+    const Point2 pn = calibration().calibrate(p);
+    return pose().transform_from(backproject_from_camera(pn, depth));
+  }
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at infinity
+  Point3 backprojectPointAtInfinity(const Point2& p) const {
+    const Point2 pn = calibration().calibrate(p);
+    const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
+    return pose().rotation().rotate(pc);
+  }
+
+  /**
+   * Calculate range to a landmark
+   * @param point 3D location of 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
+   * @return range (double)
+   */
+  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
+    return this->pose().range(pose, Dcamera, Dpose);
+  }
+
+  /**
+   * Calculate range to a CalibratedCamera
+   * @param camera Other camera
+   * @return range (double)
+   */
+  double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
+      boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
+    return pose().range(camera.pose(), Dcamera, Dother);
+  }
+
+  /**
+   * Calculate range to a PinholePoseK derived class
+   * @param camera Other camera
+   * @return range (double)
+   */
+  template<class CalibrationB>
+  double range(const PinholeBaseK<CalibrationB>& camera,
+      OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 6> Dother = boost::none) const {
+    return pose().range(camera.pose(), Dcamera, Dother);
+  }
+
+private:
+
+  /** Serialization function */
+  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));
+  }
+
+};
+// end of class PinholeBaseK
+
+/**
+ * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
+ * Instead of using this class, one might consider calibrating the measurements
+ * and using CalibratedCamera, which would then be faster.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+template<typename Calibration>
+class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
+
+private:
+
+  typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
+  boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
+
+public:
+
+  enum {
+    dimension = 6
+  }; ///< There are 6 DOF to optimize for
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholePose() {
+  }
+
+  /** constructor with pose, uses default calibration */
+  explicit PinholePose(const Pose3& pose) :
+      Base(pose), K_(new Calibration()) {
+  }
+
+  /** constructor with pose and calibration */
+  PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
+      Base(pose), K_(K) {
+  }
+
+  /// @}
+  /// @name Named Constructors
+  /// @{
+
+  /**
+   * Create a level camera 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 PinholePose Level(const boost::shared_ptr<Calibration>& K,
+      const Pose2& pose2, double height) {
+    return PinholePose(Base::LevelPose(pose2, height), K);
+  }
+
+  /// PinholePose::level with default calibration
+  static PinholePose Level(const Pose2& pose2, double height) {
+    return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
+  }
+
+  /**
+   * Create a camera 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
+   * @param K optional calibration parameter
+   */
+  static PinholePose Lookat(const Point3& eye, const Point3& target,
+      const Point3& upVector, const boost::shared_ptr<Calibration>& K =
+          boost::make_shared<Calibration>()) {
+    return PinholePose(Base::LookatPose(eye, target, upVector), K);
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /// Init from 6D vector
+  explicit PinholePose(const Vector &v) :
+      Base(v), K_(new Calibration()) {
+  }
+
+  /// Init from Vector and calibration
+  PinholePose(const Vector &v, const Vector &K) :
+      Base(v), K_(new Calibration(K)) {
+  }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  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);
+  }
+
+  /// print
+  void print(const std::string& s = "PinholePose") const {
+    Base::print(s);
+    K_->print(s + ".calibration");
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~PinholePose() {
+  }
+
+  /// return calibration
+  virtual const Calibration& calibration() const {
+    return *K_;
+  }
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  /// @deprecated
+  size_t dim() const {
+    return 6;
+  }
+
+  /// @deprecated
+  static size_t Dim() {
+    return 6;
+  }
+
+  /// move a cameras according to d
+  PinholePose retract(const Vector6& d) const {
+    return PinholePose(Base::pose().retract(d), K_);
+  }
+
+  /// return canonical coordinate
+  Vector6 localCoordinates(const PinholePose& p) const {
+    return Base::pose().localCoordinates(p.Base::pose());
+  }
+
+  /// for Canonical
+  static PinholePose identity() {
+    return PinholePose(); // assumes that the default constructor is valid
+  }
+
+  /// @}
+
+private:
+
+  /** Serialization function */
+  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_);
+  }
+
+};
+// 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

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -29,6 +29,7 @@ using namespace gtsam;
 
 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
 
+// Camera situated at 0.5 meters high, looking down
 static const Pose3 pose1((Matrix)(Matrix(3,3) <<
               1., 0., 0.,
               0.,-1., 0.,
@@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) {
 }
 
 /* ************************************************************************* */
-static Point2 project2(const Pose3& pose, const Point3& point) {
+static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
-  return CalibratedCamera(pose).project(point);
+  return camera.project(point);
 }
 
 TEST( CalibratedCamera, Dproject_point_pose)
 {
   Matrix Dpose, Dpoint;
   Point2 result = camera.project(point1, Dpose, Dpoint);
-  Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
+  Matrix numerical_pose  = numericalDerivative21(project2, camera, point1);
-  Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
+  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
   CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
   CHECK(assert_equal(Point2(-.16,  .16), result));
   CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject_point_pose2)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project2, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
 
-

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -15,12 +15,14 @@
  * @brief test PinholeCamera class
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
@@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2)
   EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
 }
 
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( PinholeCamera, Dproject3)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Camera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project4, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 static double range0(const Camera& camera, const Point3& point) {
   return camera.range(point);

gtsam/geometry/tests/testPinholePose.cpp

@@ -0,0 +1,259 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   testPinholePose.cpp
+ * @author Frank Dellaert
+ * @brief  test PinholePose class
+ * @date   Feb 20, 2015
+ */
+
+#include <gtsam/geometry/PinholePose.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+typedef PinholePose<Cal3_S2> Camera;
+
+static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
+
+static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
+static const Camera camera(pose, K);
+
+static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
+static const Camera camera1(pose1, K);
+
+static const Point3 point1(-0.08,-0.08, 0.0);
+static const Point3 point2(-0.08, 0.08, 0.0);
+static const Point3 point3( 0.08, 0.08, 0.0);
+static const Point3 point4( 0.08,-0.08, 0.0);
+
+static const Point3 point1_inf(-0.16,-0.16, -1.0);
+static const Point3 point2_inf(-0.16, 0.16, -1.0);
+static const Point3 point3_inf( 0.16, 0.16, -1.0);
+static const Point3 point4_inf( 0.16,-0.16, -1.0);
+
+/* ************************************************************************* */
+TEST( PinholePose, constructor)
+{
+  EXPECT(assert_equal( pose, camera.pose()));
+}
+
+//******************************************************************************
+TEST(PinholeCamera, Pose) {
+
+  Matrix actualH;
+  EXPECT(assert_equal(pose, camera.getPose(actualH)));
+
+  // Check derivative
+  boost::function<Pose3(Camera)> f = //
+      boost::bind(&Camera::getPose,_1,boost::none);
+  Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, lookat)
+{
+  // Create a level camera, looking in Y-direction
+  Point3 C(10.0,0.0,0.0);
+  Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
+
+  // expected
+  Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
+  Pose3 expected(Rot3(xc,yc,zc),C);
+  EXPECT(assert_equal( camera.pose(), expected));
+
+  Point3 C2(30.0,0.0,10.0);
+  Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
+
+  Matrix R = camera2.pose().rotation().matrix();
+  Matrix I = trans(R)*R;
+  EXPECT(assert_equal(I, eye(3)));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, project)
+{
+  EXPECT(assert_equal( camera.project2(point1), Point2(-100,  100) ));
+  EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) ));
+  EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) ));
+  EXPECT(assert_equal( camera.project2(point4), Point2( 100,  100) ));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backproject)
+{
+  EXPECT(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
+  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
+  EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
+  EXPECT(assert_equal( camera.backproject(Point2( 100,  100), 0.5),  point4));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backprojectInfinity)
+{
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)),  point3_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100,  100)),  point4_inf));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backproject2)
+{
+  Point3 origin;
+  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
+  Camera camera(Pose3(rot, origin), K);
+
+  Point3 actual = camera.backproject(Point2(), 1.);
+  Point3 expected(0., 1., 0.);
+  pair<Point2, bool> x = camera.projectSafe(expected);
+
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(Point2(), x.first));
+  EXPECT(x.second);
+}
+
+/* ************************************************************************* */
+static Point2 project3(const Pose3& pose, const Point3& point,
+    const Cal3_S2::shared_ptr& cal) {
+  return Camera(pose, cal).project2(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, Dproject)
+{
+  Matrix Dpose, Dpoint;
+  Point2 result = camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative31(project3, pose, point1, K);
+  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
+  EXPECT(assert_equal(Point2(-100,  100), result));
+  EXPECT(assert_equal(numerical_pose,  Dpose,  1e-7));
+  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+static Point2 project4(const Camera& camera, const Point3& point) {
+  return camera.project2(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, Dproject2)
+{
+  Matrix Dcamera, Dpoint;
+  Point2 result = camera.project2(point1, Dcamera, Dpoint);
+  Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
+  Matrix Hexpected2  = numericalDerivative22(project4, camera, point1);
+  EXPECT(assert_equal(result, Point2(-100,  100) ));
+  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
+  EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
+}
+
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject3)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Camera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project4, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+static double range0(const Camera& camera, const Point3& point) {
+  return camera.range(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range0) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(point1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
+  Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
+  EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
+      1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static double range1(const Camera& camera, const Pose3& pose) {
+  return camera.range(pose);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range1) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(pose1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
+  Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+typedef PinholePose<Cal3Bundler> Camera2;
+static const boost::shared_ptr<Cal3Bundler> K2 =
+    boost::make_shared<Cal3Bundler>(625, 1e-3, 1e-3);
+static const Camera2 camera2(pose1, K2);
+static double range2(const Camera& camera, const Camera2& camera2) {
+  return camera.range<Cal3Bundler>(camera2);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range2) {
+  Matrix D1; Matrix D2;
+  double result = camera.range<Cal3Bundler>(camera2, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
+  Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static const CalibratedCamera camera3(pose1);
+static double range3(const Camera& camera, const CalibratedCamera& camera3) {
+  return camera.range(camera3);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range3) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(camera3, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
+  Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -16,8 +16,6 @@
  * @date Feb 7, 2012
  */
 
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/EssentialMatrix.h>

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -15,13 +15,13 @@
  * @brief test SimpleCamera class
  */
 
-#include <cmath>
+#include <gtsam/geometry/SimpleCamera.h>
-#include <iostream>
+#include <gtsam/geometry/Pose2.h>
-
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <CppUnitLite/TestHarness.h>
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;

gtsam/nonlinear/tests/testSerializationNonlinear.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Bundler.h>

gtsam/slam/expressions.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/expressions.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>

gtsam/slam/tests/testDataset.cpp

@@ -15,16 +15,16 @@
  * @author  Richard Roberts, Luca Carlone
  */
 
-#include <CppUnitLite/TestHarness.h>
-
-#include <boost/algorithm/string.hpp>
-
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/inference/Symbol.h>
 
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <boost/algorithm/string.hpp>
+
+#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam::symbol_shorthand;
 using namespace std;

gtsam/slam/tests/testLago.cpp

@@ -23,6 +23,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>

tests/testManifold.cpp

@@ -17,9 +17,8 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/base/VectorSpace.h>