getPose with derivative (for expressions)

gtsam/geometry/PinholeCamera.h

@@ -151,11 +151,20 @@ public:
     return pose_;
   }
 
-  /// return 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 {
+    if (H) {
+      H->setZero();
+      H->block(0, 0, 6, 6) = I_6x6;
+    }
+    return pose_;
+  }
+
   /// return calibration
   inline Calibration& calibration() {
     return K_;

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
 /* ************************************************************************* */
 TEST( PinholeCamera, constructor)
 {
-  EXPECT(assert_equal( camera.calibration(), K));
-  EXPECT(assert_equal( camera.pose(), pose));
+  EXPECT(assert_equal( K, camera.calibration()));
+  EXPECT(assert_equal( pose, camera.pose()));
+}
+
+//******************************************************************************
+TEST(PinholeCamera, Pose) {
+
+  Matrix actualH;
+  EXPECT(assert_equal(pose, camera.pose(actualH)));
+
+  // Check derivative
+  boost::function<Pose3(Camera)> f = //
+      boost::bind(&Camera::pose,_1,boost::none);
+  Matrix numericalH = numericalDerivative11<Pose3,Camera>(&Camera::getPose,camera);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
 }
 
 /* ************************************************************************* */