Added two static functions and Lookat named constructor. Will be called in Pinhole* classes to avoid copy/paste

gtsam/geometry/CalibratedCamera.cpp

@@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project_to_camera(const Point3& P,
-    OptionalJacobian<2,3> H1) {
+    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;
+    *H1 << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }
@@ -47,19 +47,40 @@ Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
   return Point3(p.x() * scale, p.y() * scale, scale);
 }
 
+/* ************************************************************************* */
+Pose3 CalibratedCamera::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);
+  const Point3 t(pose2.x(), pose2.y(), height);
+  return Pose3(wRc, t);
+}
+
 /* ************************************************************************* */
 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);
+Pose3 CalibratedCamera::LookatPose(const Point3& eye, const Point3& target,
-  return CalibratedCamera(pose3);
+    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);
+}
+
+/* ************************************************************************* */
+CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
+    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> Dpose, OptionalJacobian<2, 3> Dpoint) const {
 
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
   Matrix36 Dpose_;
@@ -88,14 +109,14 @@ Point2 CalibratedCamera::project(const Point3& point,
     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),
+      *Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d
-          -uv, -u, 0., -d, d * v;
+          * 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),
+      Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0)
-              R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
+          - u * R(2, 2), R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1)
-              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
+          - v * R(2, 2);
       *Dpoint = d * Dpoint_;
     }
 #endif

gtsam/geometry/CalibratedCamera.h

@@ -44,7 +44,9 @@ private:
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
@@ -56,6 +58,49 @@ public:
   /// construct with pose
   explicit CalibratedCamera(const Pose3& 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
+   * @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 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.
+   * @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
   /// @{
@@ -89,14 +134,6 @@ public:
     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
   /// @{
@@ -202,10 +239,13 @@ private:
 };
 
 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> {
+};
 
 }