put sensor_T_body in SmartProjectionFactor

gtsam/slam/SmartProjectionFactor.h

@@ -74,6 +74,10 @@ protected:
   const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
   /// @}
 
+  /// @name Pose of the camera in the body frame
+  const boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
+  /// @}
+
 public:
 
   /// shorthand for a smart pointer to a factor
@@ -94,14 +98,16 @@ public:
       double rankTolerance = 1, DegeneracyMode degeneracyMode =
           IGNORE_DEGENERACY, bool enableEPI = false,
       double landmarkDistanceThreshold = 1e10,
-      double dynamicOutlierRejectionThreshold = -1) :
+      double dynamicOutlierRejectionThreshold = -1,
+      boost::optional<Pose3> body_P_sensor = boost::none) :
       linearizationMode_(linearizationMode), //
       degeneracyMode_(degeneracyMode), //
       parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
           dynamicOutlierRejectionThreshold), //
       result_(TriangulationResult::Degenerate()), //
       retriangulationThreshold_(1e-5), //
-      throwCheirality_(false), verboseCheirality_(false) {
+      throwCheirality_(false), verboseCheirality_(false),
+      body_P_sensor_(body_P_sensor){
   }
 
   /** Virtual destructor */
@@ -119,6 +125,8 @@ public:
     std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl;
     std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
     std::cout << "result:\n" << result_ << std::endl;
+    if(body_P_sensor_)
+      body_P_sensor_->print("body_P_sensor_:\n");
     Base::print("", keyFormatter);
   }
 
@@ -468,6 +476,13 @@ public:
     return throwCheirality_;
   }
 
+  Pose3 body_P_sensor() const{
+    if(body_P_sensor_)
+      return *body_P_sensor_;
+    else
+      return Pose3(); // if unspecified, the transformation is the identity
+  }
+
 private:
 
   /// Serialization function

gtsam/slam/SmartProjectionPoseFactor.h

@@ -49,7 +49,6 @@ private:
 protected:
 
   boost::shared_ptr<CALIBRATION> K_; ///< calibration object (one for all cameras)
-  boost::optional<Pose3> body_P_sensor_; ///< Pose of the camera in the body frame
 
 public:
 
@@ -71,7 +70,7 @@ public:
       LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
         Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, landmarkDistanceThreshold,
-            dynamicOutlierRejectionThreshold), K_(K),  body_P_sensor_(body_P_sensor) {}
+            dynamicOutlierRejectionThreshold, body_P_sensor), K_(K) {}
 
   /** Virtual destructor */
   virtual ~SmartProjectionPoseFactor() {}
@@ -84,8 +83,6 @@ public:
   void print(const std::string& s = "", const KeyFormatter& keyFormatter =
       DefaultKeyFormatter) const {
     std::cout << s << "SmartProjectionPoseFactor, z = \n ";
-    if(body_P_sensor_)
-      body_P_sensor_->print("body_P_sensor_:\n");
     Base::print("", keyFormatter);
   }
 
@@ -129,13 +126,6 @@ public:
     return K_;
   }
 
-  Pose3 body_P_sensor() const{
-    if(body_P_sensor_)
-      return *body_P_sensor_;
-    else
-      return Pose3(); // if unspecified, the transformation is the identity
-  }
-
   /**
    * Collect all cameras involved in this factor
    * @param values Values structure which must contain camera poses corresponding
@@ -146,8 +136,8 @@ public:
     typename Base::Cameras cameras;
     BOOST_FOREACH(const Key& k, this->keys_) {
       Pose3 pose = values.at<Pose3>(k);
-      if(body_P_sensor_)
+      if(Base::body_P_sensor_)
-        pose = pose.compose(*(body_P_sensor_));
+        pose = pose.compose(*(Base::body_P_sensor_));
 
       Camera camera(pose, K_);
       cameras.push_back(camera);