diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 46df47ecb..3b4858a4a 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -108,6 +108,20 @@ public: return PinholeCamera(pose3, K); } + // Create PinholeCamera, with derivatives + static PinholeCamera Create(const Pose3& pose, const Calibration &K, + OptionalJacobian H1 = boost::none, // + OptionalJacobian H2 = boost::none) { + typedef Eigen::Matrix MatrixK6; + if (H1) + *H1 << I_6x6, MatrixK6::Zero(); + typedef Eigen::Matrix Matrix6K; + typedef Eigen::Matrix MatrixK; + if (H2) + *H2 << Matrix6K::Zero(), MatrixK::Identity(); + return PinholeCamera(pose,K); + } + /// @} /// @name Advanced Constructors /// @{ diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 20f7a3231..fd041ef38 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -55,6 +55,21 @@ TEST( PinholeCamera, constructor) EXPECT(assert_equal( pose, camera.pose())); } +//****************************************************************************** +TEST(PinholeCamera, Create) { + + Matrix actualH1, actualH2; + EXPECT(assert_equal(camera, Camera::Create(pose,K, actualH1, actualH2))); + + // Check derivative + boost::function f = // + boost::bind(Camera::Create,_1,_2,boost::none,boost::none); + Matrix numericalH1 = numericalDerivative21(f,pose,K); + EXPECT(assert_equal(numericalH1, actualH1, 1e-9)); + Matrix numericalH2 = numericalDerivative22(f,pose,K); + EXPECT(assert_equal(numericalH2, actualH2, 1e-8)); +} + //****************************************************************************** TEST(PinholeCamera, Pose) {