included sensor to body transformation in smartProjectionPoseFactors (with unit test)

gtsam/slam/SmartFactorBase.h

@@ -73,7 +73,7 @@ public:
 
   /**
    * Constructor
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
       body_P_sensor_(body_P_sensor) {
@@ -271,8 +271,13 @@ public:
 
       Vector bi;
       try {
-        bi =
-            -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
+        bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
+        if(body_P_sensor_){
+          Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
+          Matrix J(6, 6);
+          Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
+          Fi = Fi * J;
+        }
       } catch (CheiralityException&) {
         std::cout << "Cheirality exception " << std::endl;
         exit(EXIT_FAILURE);

gtsam/slam/SmartProjectionFactor.h

@@ -120,7 +120,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
@@ -685,7 +685,7 @@ public:
   inline bool isPointBehindCamera() const {
     return cheiralityException_;
   }
-  /** return chirality verbosity */
+  /** return cheirality verbosity */
   inline bool verboseCheirality() const {
     return verboseCheirality_;
   }

gtsam/slam/SmartProjectionPoseFactor.h

@@ -63,7 +63,7 @@ public:
    * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
    * otherwise the factor is simply neglected
    * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
-   * @param body_P_sensor is the transform from body to sensor frame (default identity)
+   * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
@@ -157,6 +157,9 @@ public:
     size_t i=0;
     BOOST_FOREACH(const Key& k, this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
+      if(Base::body_P_sensor_)
+        pose = pose.compose(*(Base::body_P_sensor_));
+
       typename Base::Camera camera(pose, *K_all_[i++]);
       cameras.push_back(camera);
     }

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -292,6 +292,95 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
   if(isDebugTest) tictoc_print_();
 }
 
+/* *************************************************************************/
+TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
+
+  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+  Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
+  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));
+
+  SimpleCamera cam1(cameraPose1, *K); // with camera poses
+  SimpleCamera cam2(cameraPose2, *K);
+  SimpleCamera cam3(cameraPose3, *K);
+
+  // create arbitrary body_Pose_sensor (transforms from sensor to body)
+  Pose3 sensor_to_body =  Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //
+
+  // These are the poses we want to estimate, from camera measurements
+  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
+  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
+  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());
+
+  // three landmarks ~5 meters infront of camera
+  Point3 landmark1(5, 0.5, 1.2);
+  Point3 landmark2(5, -0.5, 1.2);
+  Point3 landmark3(5, 0, 3.0);
+
+  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
+
+  // Project three landmarks into three cameras
+  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
+  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
+
+  // Create smart factors
+  std::vector<Key> views;
+  views.push_back(x1);
+  views.push_back(x2);
+  views.push_back(x3);
+
+  double rankTol = 1;
+  double linThreshold = -1;
+  bool manageDegeneracy = false;
+  bool enableEPI = false;
+
+  SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor1->add(measurements_cam1, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor2->add(measurements_cam2, views, model, K);
+
+  SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol,linThreshold,manageDegeneracy,enableEPI,sensor_to_body));
+  smartFactor3->add(measurements_cam3, views, model, K);
+
+  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
+
+  // Put all factors in factor graph, adding priors
+  NonlinearFactorGraph graph;
+  graph.push_back(smartFactor1);
+  graph.push_back(smartFactor2);
+  graph.push_back(smartFactor3);
+  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
+  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
+
+  // Check errors at ground truth poses
+  Values gtValues;
+  gtValues.insert(x1, bodyPose1);
+  gtValues.insert(x2, bodyPose2);
+  gtValues.insert(x3, bodyPose3);
+  double actualError = graph.error(gtValues);
+  double expectedError = 0.0;
+  DOUBLES_EQUAL(expectedError, actualError, 1e-7)
+
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
+  Values values;
+  values.insert(x1, bodyPose1);
+  values.insert(x2, bodyPose2);
+  // initialize third pose with some noise, we expect it to move back to original pose3
+  values.insert(x3, bodyPose3*noise_pose);
+
+  LevenbergMarquardtParams params;
+  Values result;
+  LevenbergMarquardtOptimizer optimizer(graph, values, params);
+  result = optimizer.optimize();
+
+  // result.print("results of 3 camera, 3 landmark optimization \n");
+  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
+  EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
+}
+
+
 /* *************************************************************************/
 TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
   // cout << " ************************ SmartProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;