Merge pull request #379 from borglab/fix/smartfactor_body_P_sensor

Test body_P_sensor in SmartFactor

gtsam/geometry/PinholePose.h

@@ -107,9 +107,9 @@ public:
 
     // If needed, apply chain rule
     if (Dpose)
-    *Dpose = Dpi_pn * *Dpose;
+      *Dpose = Dpi_pn * *Dpose;
     if (Dpoint)
-    *Dpoint = Dpi_pn * *Dpoint;
+      *Dpoint = Dpi_pn * *Dpoint;
 
     return pi;
   }

gtsam/slam/SmartFactorBase.h

@@ -207,10 +207,18 @@ protected:
     Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
     if (body_P_sensor_ && Fs) {
       const Pose3 sensor_P_body = body_P_sensor_->inverse();
+      constexpr int camera_dim = traits<CAMERA>::dimension;
+      constexpr int pose_dim = traits<Pose3>::dimension;
+
       for (size_t i = 0; i < Fs->size(); i++) {
-        const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
-        Matrix J(6, 6);
-        const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+        const Pose3 world_P_body = cameras[i].pose() * sensor_P_body;
+        Eigen::Matrix<double, camera_dim, camera_dim> J;
+        J.setZero();
+        Eigen::Matrix<double, pose_dim, pose_dim> H;
+        // Call compose to compute Jacobian for camera extrinsics
+        world_P_body.compose(*body_P_sensor_, H);
+        // Assign extrinsics part of the Jacobian
+        J.template block<pose_dim, pose_dim>(0, 0) = H;
         Fs->at(i) = Fs->at(i) * J;
       }
     }

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -37,11 +37,11 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
 public:
   typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
   PinholeFactor() {}
-  PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
-  }
-  virtual double error(const Values& values) const {
-    return 0.0;
-  }
+  PinholeFactor(const SharedNoiseModel& sharedNoiseModel,
+                boost::optional<Pose3> body_P_sensor = boost::none,
+                size_t expectedNumberCameras = 10)
+      : Base(sharedNoiseModel, body_P_sensor, expectedNumberCameras) {}
+  virtual double error(const Values& values) const { return 0.0; }
   virtual boost::shared_ptr<GaussianFactor> linearize(
       const Values& values) const {
     return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
@@ -60,6 +60,40 @@ TEST(SmartFactorBase, Pinhole) {
   EXPECT_LONGS_EQUAL(2 * 2, f.dim());
 }
 
+TEST(SmartFactorBase, PinholeWithSensor) {
+  Pose3 body_P_sensor(Rot3(), Point3(1, 0, 0));
+  PinholeFactor f = PinholeFactor(unit2, body_P_sensor);
+  EXPECT(assert_equal<Pose3>(f.body_P_sensor(), body_P_sensor));
+
+  PinholeFactor::Cameras cameras;
+  // Assume body at origin.
+  Pose3 world_P_body = Pose3();
+  // Camera coordinates in world frame.
+  Pose3 wTc = world_P_body * body_P_sensor;
+  cameras.push_back(PinholeCamera<Cal3Bundler>(wTc));
+  
+  // Simple point to project slightly off image center
+  Point3 p(0, 0, 10);
+  Point2 measurement = cameras[0].project(p);
+  f.add(measurement, 1);
+
+  PinholeFactor::Cameras::FBlocks Fs;
+  Matrix E;
+  Vector error = f.unwhitenedError<Point3>(cameras, p, Fs, E);
+
+  Vector expectedError = Vector::Zero(2);  
+  Matrix29 expectedFs;
+  expectedFs << -0.001, -1.00001, 0, -0.1, 0, -0.01, 0, 0, 0, 1, 0, 0, 0, -0.1, 0, 0, 0, 0;
+  Matrix23 expectedE;
+  expectedE << 0.1, 0, 0.01, 0, 0.1, 0;
+
+  EXPECT(assert_equal(error, expectedError));
+  // We only have the jacobian for the 1 camera
+  // Use of a lower tolerance value due to compiler precision mismatch.
+  EXPECT(assert_equal(expectedFs, Fs[0], 1e-3));
+  EXPECT(assert_equal(expectedE, E));
+}
+
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>