diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 12eb0235d..46df47ecb 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/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_; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 6cb84ec85..fb186259a 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/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 f = // + boost::bind(&Camera::pose,_1,boost::none); + Matrix numericalH = numericalDerivative11(&Camera::getPose,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } /* ************************************************************************* */