pass on projection factor, but looks great overall

gtsam_unstable/slam/ProjectionFactorRollingShutter.h

@@ -23,23 +23,23 @@
 
 namespace gtsam {
 
-  /**
+/**
-   * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
+ * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here.
-   * i.e. the main building block for visual SLAM.
+ * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B,
-   * this version takes rolling shutter information into account like so: consider camera A (pose A) and camera B, and Point2 from camera A.
+ * and a Point2 measurement taken starting at time A using a rolling shutter camera.
-   * camera A has timestamp t_A for the exposure time of its first row, and so does camera B t_B, Point2 has timestamp t_p according to the timestamp
+ * Pose A has timestamp t_A, and Pose B has timestamp t_B. The Point2 measurement has timestamp t_p (with t_A <= t_p <= t_B)
-   * corresponding to the time of exposure of the row in the camera it belongs to.
+ * corresponding to the time of exposure of the row of the image the pixel belongs to.
-   * let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by the interp_param to project
+ * Let us define the interp_param = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by
-   * the corresponding landmark to Point2.
+ * the interp_param to project the corresponding landmark to Point2.
  * @addtogroup SLAM
  */
 
-  class ProjectionFactorRollingShutter: public NoiseModelFactor3<Pose3, Pose3, Point3> {
+class ProjectionFactorRollingShutter : public NoiseModelFactor3<Pose3, Pose3, Point3> {
  protected:
 
   // Keep a copy of measurement and calibration for I/O
   Point2 measured_;                   ///< 2D measurement
-    double interp_param_;              ///< interpolation parameter corresponding to the point2 measured
+  double interp_param_;  ///< interpolation parameter in [0,1] corresponding to the point2 measurement
   boost::shared_ptr<Cal3_S2> K_;  ///< shared pointer to calibration object
   boost::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
 
@@ -59,65 +59,87 @@ namespace gtsam {
   typedef boost::shared_ptr<This> shared_ptr;
 
   /// Default constructor
-  ProjectionFactorRollingShutter() :
+  ProjectionFactorRollingShutter()
-      measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) {
+      : measured_(0, 0),
+        interp_param_(0),
+        throwCheirality_(false),
+        verboseCheirality_(false) {
   }
 
   /**
    * Constructor
-     * @param measured is the 2 dimensional location of point in image (the measurement)
+   * @param measured is the 2-dimensional pixel location of point in the image (the measurement)
    * @param interp_param is the rolling shutter parameter for the measurement
-     * @param model is the standard deviation
+   * @param model is the noise model
-     * @param poseKey_a is the index of the first camera
+   * @param poseKey_a is the key of the first camera
-     * @param poseKey_b is the index of the second camera
+   * @param poseKey_b is the key of the second camera
-     * @param pointKey is the index of the landmark
+   * @param pointKey is the key of the landmark
    * @param K shared pointer to the constant calibration
    * @param body_P_sensor is the transform from body to sensor frame (default identity)
    */
-    ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model,
+  ProjectionFactorRollingShutter(const Point2& measured, double interp_param,
-        Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr<Cal3_S2>& K,
+                                 const SharedNoiseModel& model,
-        boost::optional<Pose3> body_P_sensor = boost::none) :
+                                 Key poseKey_a, Key poseKey_b, Key pointKey,
-          Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
+                                 const boost::shared_ptr<Cal3_S2>& K,
-          throwCheirality_(false), verboseCheirality_(false) {}
+                                 boost::optional<Pose3> body_P_sensor = boost::none)
+      : Base(model, poseKey_a, poseKey_b, pointKey),
+        measured_(measured),
+        interp_param_(interp_param),
+        K_(K),
+        body_P_sensor_(body_P_sensor),
+        throwCheirality_(false),
+        verboseCheirality_(false) {
+  }
 
   /**
    * Constructor with exception-handling flags
-     * @param measured is the 2 dimensional location of point in image (the measurement)
+   * @param measured is the 2-dimensional pixel location of point in the image (the measurement)
    * @param interp_param is the rolling shutter parameter for the measurement
-     * @param model is the standard deviation
+   * @param model is the noise model
-     * @param poseKey_a is the index of the first camera
+   * @param poseKey_a is the key of the first camera
-     * @param poseKey_b is the index of the second camera
+   * @param poseKey_b is the key of the second camera
-     * @param pointKey is the index of the landmark
+   * @param pointKey is the key of the landmark
    * @param K shared pointer to the constant calibration
    * @param throwCheirality determines whether Cheirality exceptions are rethrown
    * @param verboseCheirality determines whether exceptions are printed for Cheirality
    * @param body_P_sensor is the transform from body to sensor frame  (default identity)
    */
-    ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model,
+  ProjectionFactorRollingShutter(const Point2& measured, double interp_param,
-        Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr<Cal3_S2>& K,
+                                 const SharedNoiseModel& model,
+                                 Key poseKey_a, Key poseKey_b, Key pointKey,
+                                 const boost::shared_ptr<Cal3_S2>& K,
                                  bool throwCheirality, bool verboseCheirality,
-        boost::optional<Pose3> body_P_sensor = boost::none) :
+                                 boost::optional<Pose3> body_P_sensor = boost::none)
-          Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
+      : Base(model, poseKey_a, poseKey_b, pointKey),
-          throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
+        measured_(measured),
+        interp_param_(interp_param),
+        K_(K),
+        body_P_sensor_(body_P_sensor),
+        throwCheirality_(throwCheirality),
+        verboseCheirality_(verboseCheirality) {
+  }
 
   /** Virtual destructor */
-    virtual ~ProjectionFactorRollingShutter() {}
+  virtual ~ProjectionFactorRollingShutter() {
+  }
 
   /// @return a deep copy of this factor
   virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+    return boost::static_pointer_cast < gtsam::NonlinearFactor
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+        > (gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 
   /**
    * print
    * @param s optional string naming the factor
    * @param keyFormatter optional formatter useful for printing Symbols
    */
-    void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+                 DefaultKeyFormatter) const {
     std::cout << s << "ProjectionFactorRollingShutter, z = ";
     traits<Point2>::Print(measured_);
     std::cout << " rolling shutter interpolation param = " << interp_param_;
-      if(this->body_P_sensor_)
+    if (this->body_P_sensor_)
       this->body_P_sensor_->print("  sensor pose in body frame: ");
     Base::print("", keyFormatter);
   }
@@ -125,50 +147,59 @@ namespace gtsam {
   /// equals
   virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
     const This *e = dynamic_cast<const This*>(&p);
-      return e
+    return e && Base::equals(p, tol) && (interp_param_ == e->interp_param())
-          && Base::equals(p, tol)
-          && (interp_param_ == e->interp_param())
         && traits<Point2>::Equals(this->measured_, e->measured_, tol)
         && this->K_->equals(*e->K_, tol)
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+        && (this->throwCheirality_ == e->throwCheirality_)
+        && (this->verboseCheirality_ == e->verboseCheirality_)
+        && ((!body_P_sensor_ && !e->body_P_sensor_)
+            || (body_P_sensor_ && e->body_P_sensor_
+                && body_P_sensor_->equals(*e->body_P_sensor_)));
   }
 
   /// Evaluate error h(x)-z and optionally derivatives
   Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::none) const {
+                       boost::optional<Matrix&> H1 = boost::none,
+                       boost::optional<Matrix&> H2 = boost::none,
+                       boost::optional<Matrix&> H3 = boost::none) const {
 
-      Pose3 pose;
-      gtsam::Matrix Hprj;
-
-      //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2);
-      pose = interpolate<Pose3>(pose_a, pose_b, interp_param_, H1, H2);
     try {
-        if(body_P_sensor_) {
+      Pose3 pose = interpolate<Pose3>(pose_a, pose_b, interp_param_, H1, H2);
-          if(H1 && H2) {
+      gtsam::Matrix Hprj;
-            gtsam::Matrix H0;
+      if (body_P_sensor_) {
-            PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_, H0), *K_);
+        if (H1 || H2 || H3) {
+          gtsam::Matrix HbodySensor;
+          PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_, HbodySensor), *K_);
           Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
-            *H1 = Hprj * H0 * (*H1);
+          if (H1)
-            *H2 = Hprj * H0 * (*H2);
+            *H1 = Hprj * HbodySensor * (*H1);
+          if (H2)
+            *H2 = Hprj * HbodySensor * (*H2);
           return reprojectionError;
         } else {
           PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
-            return camera.project(point, Hprj, H3, boost::none) - measured_;
+          return camera.project(point) - measured_;
         }
       } else {
         PinholeCamera<Cal3_S2> camera(pose, *K_);
         Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
-          if (H1) *H1 = Hprj * (*H1);
+        if (H1)
-          if (H2) *H2 = Hprj * (*H2);
+          *H1 = Hprj * (*H1);
+        if (H2)
+          *H2 = Hprj * (*H2);
         return reprojectionError;
       }
-      } catch( CheiralityException& e) {
+    } catch (CheiralityException& e) {
-        if (H1) *H1 = Matrix::Zero(2,6);
+      if (H1)
-        if (H2) *H2 = Matrix::Zero(2,6);
+        *H1 = Matrix::Zero(2, 6);
-        if (H3) *H3 = Matrix::Zero(2,3);
+      if (H2)
+        *H2 = Matrix::Zero(2, 6);
+      if (H3)
+        *H3 = Matrix::Zero(2, 3);
       if (verboseCheirality_)
-          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+        std::cout << e.what() << ": Landmark "
-              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+            << DefaultKeyFormatter(this->key2()) << " moved behind camera "
+            << DefaultKeyFormatter(this->key1()) << std::endl;
       if (throwCheirality_)
         throw CheiralityException(this->key2());
     }
@@ -186,13 +217,19 @@ namespace gtsam {
   }
 
   /** returns the rolling shutter interp param*/
-    inline double interp_param() const {return interp_param_; }
+  inline double interp_param() const {
+    return interp_param_;
+  }
 
   /** return verbosity */
-    inline bool verboseCheirality() const { return verboseCheirality_; }
+  inline bool verboseCheirality() const {
+    return verboseCheirality_;
+  }
 
   /** return flag for throwing cheirality exceptions */
-    inline bool throwCheirality() const { return throwCheirality_; }
+  inline bool throwCheirality() const {
+    return throwCheirality_;
+  }
 
  private:
 
@@ -210,5 +247,6 @@ namespace gtsam {
   }
  public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-  }; // rolling shutter projection factor
+};
-} //namespace gtsam
+// rolling shutter projection factor
+}//namespace gtsam