From 73007fe0483c81f72b05220b8d0a45b8eb18a363 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 7 Jul 2020 19:24:38 -0400
Subject: [PATCH] test SmartFactor when body_P_sensor is passed in

---
 gtsam/slam/SmartFactorBase.h             |  7 ++--
 gtsam/slam/tests/testSmartFactorBase.cpp | 42 +++++++++++++++++++++---
 2 files changed, 41 insertions(+), 8 deletions(-)

diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h
index 717a9c1f2..3bedf599f 100644
--- a/gtsam/slam/SmartFactorBase.h
+++ b/gtsam/slam/SmartFactorBase.h
@@ -206,11 +206,12 @@ protected:
       boost::optional<Matrix&> E = boost::none) const {
     Vector ue = cameras.reprojectionError(point, measured_, Fs, E);
     if (body_P_sensor_ && Fs) {
-      const Pose3 sensor_P_body = body_P_sensor_->inverse();
+      const Pose3 sensor_T_body = body_P_sensor_->inverse();
       for (size_t i = 0; i < Fs->size(); i++) {
-        const Pose3 w_Pose_body = cameras[i].pose() * sensor_P_body;
+        const Pose3 world_T_body = cameras[i].pose() * sensor_T_body;
         Matrix J(6, 6);
-        const Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+        // Call compose to compute Jacobian
+        world_T_body.compose(*body_P_sensor_, J);
         Fs->at(i) = Fs->at(i) * J;
       }
     }
diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp
index f69f4c113..bb04ad56d 100644
--- a/gtsam/slam/tests/testSmartFactorBase.cpp
+++ b/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,38 @@ 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_T_body = Pose3();
+  // Camera coordinates in world frame.
+  Pose3 wTc = world_T_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));
+  EXPECT(assert_equal(expectedFs, Fs[0])); // We only have the jacobian for the 1 camera
+  EXPECT(assert_equal(expectedE, E));
+}
+
 /* ************************************************************************* */
 #include <gtsam/geometry/StereoCamera.h>