Create, with derivatives

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<dimension, 6> H1 = boost::none, //
+      OptionalJacobian<dimension, DimK> H2 = boost::none) {
+    typedef Eigen::Matrix<double, DimK, 6> MatrixK6;
+    if (H1)
+      *H1 << I_6x6, MatrixK6::Zero();
+    typedef Eigen::Matrix<double, 6, DimK> Matrix6K;
+    typedef Eigen::Matrix<double, DimK, DimK> MatrixK;
+    if (H2)
+      *H2 << Matrix6K::Zero(), MatrixK::Identity();
+    return PinholeCamera(pose,K);
+  }
+
   /// @}
   /// @name Advanced Constructors
   /// @{

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<Camera(Pose3,Cal3_S2)> f = //
+      boost::bind(Camera::Create,_1,_2,boost::none,boost::none);
+  Matrix numericalH1 = numericalDerivative21<Camera,Pose3,Cal3_S2>(f,pose,K);
+  EXPECT(assert_equal(numericalH1, actualH1, 1e-9));
+  Matrix numericalH2 = numericalDerivative22<Camera,Pose3,Cal3_S2>(f,pose,K);
+  EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
+}
+
 //******************************************************************************
 TEST(PinholeCamera, Pose) {