Merge pull request #826 from shteren1/rolling_factor

rolling shutter projection factor

gtsam/base/Lie.h

@@ -320,13 +320,13 @@ T expm(const Vector& x, int K=7) {
 }
 
 /**
- * Linear interpolation between X and Y by coefficient t in [0, 1].
+ * Linear interpolation between X and Y by coefficient t in [0, 1.5] (t>1 implies extrapolation), with optional jacobians.
  */
 template <typename T>
 T interpolate(const T& X, const T& Y, double t,
               OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hx = boost::none,
               OptionalJacobian< traits<T>::dimension, traits<T>::dimension > Hy = boost::none) {
-  assert(t >= 0.0 && t <= 1.0);
+  assert(t >= 0.0 && t <= 1.5);
   if (Hx || Hy) {
     typedef Eigen::Matrix<double, traits<T>::dimension, traits<T>::dimension> Jacobian;
     typename traits<T>::TangentVector log_Xinv_Y;

gtsam/geometry/Pose3.cpp

@@ -423,4 +423,8 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   return os;
 }
 
+Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) {
+  return X.interp(t, Y, Hx, Hy);
+}
+
 } // namespace gtsam

gtsam/geometry/Pose3.h

@@ -353,6 +353,15 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
     return std::make_pair(0, 2);
   }
 
+  /**
+   * @brief Spherical Linear interpolation between *this and other
+   * @param s a value between 0 and 1.5
+   * @param other final point of iterpolation geodesic on manifold
+   * @param Hx jacobian of the interpolation on this
+   & @param Hy jacobian of the interpolation on other
+   */
+  Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const;
+
   /// Output stream operator
   GTSAM_EXPORT
   friend std::ostream &operator<<(std::ostream &os, const Pose3& p);

gtsam_unstable/slam/RollingShutterProjectionFactor.h

@@ -0,0 +1,214 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file RollingShutterProjectionFactor.h
+ * @brief Basic bearing factor from 2D measurement for rolling shutter cameras
+ * @author Yotam Stern
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/SimpleCamera.h>
+#include <boost/optional.hpp>
+
+namespace gtsam {
+
+  /**
+   * Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
+   * i.e. the main building block for visual SLAM.
+   * this version takes rolling shutter information into account like so: consider camera A (pose A) and camera B, and Point2 from camera A.
+   * 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
+   * corresponding to the time of exposure of the row in the camera it 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
+   * the corresponding landmark to Point2.
+   * @addtogroup SLAM
+   */
+
+  class RollingShutterProjectionFactor: 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
+    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
+
+    // verbosity handling for Cheirality Exceptions
+    bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
+    bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
+
+  public:
+
+    /// shorthand for base class type
+    typedef NoiseModelFactor3<Pose3, Pose3, Point3> Base;
+
+    /// shorthand for this class
+    typedef RollingShutterProjectionFactor This;
+
+    /// shorthand for a smart pointer to a factor
+    typedef boost::shared_ptr<This> shared_ptr;
+
+    /// Default constructor
+  RollingShutterProjectionFactor() :
+      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 interp_param is the rolling shutter parameter for the measurement
+     * @param model is the standard deviation
+     * @param poseKey_a is the index of the first camera
+     * @param poseKey_b is the index of the second camera
+     * @param pointKey is the index 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)
+     */
+    RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model,
+        Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr<Cal3_S2>& K,
+        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 interp_param is the rolling shutter parameter for the measurement
+     * @param model is the standard deviation
+     * @param poseKey_a is the index of the first camera
+     * @param poseKey_b is the index of the second camera
+     * @param pointKey is the index 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)
+     */
+    RollingShutterProjectionFactor(const Point2& measured, double interp_param, 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) :
+          Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor),
+          throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
+
+    /** Virtual destructor */
+    virtual ~RollingShutterProjectionFactor() {}
+
+    /// @return a deep copy of this factor
+    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+          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 {
+      std::cout << s << "RollingShutterProjectionFactor, z = ";
+      traits<Point2>::Print(measured_);
+      std::cout << " rolling shutter interpolation param = " << interp_param_;
+      if(this->body_P_sensor_)
+        this->body_P_sensor_->print("  sensor pose in body frame: ");
+      Base::print("", keyFormatter);
+    }
+
+    /// equals
+    virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
+      const This *e = dynamic_cast<const This*>(&p);
+      return e
+          && 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_)));
+    }
+
+    /// 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 {
+
+      Pose3 pose;
+      gtsam::Matrix Hprj;
+
+      //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2);
+      pose = pose_a.interp(interp_param_, pose_b, H1, H2);
+      try {
+        if(body_P_sensor_) {
+          if(H1 && H2) {
+            gtsam::Matrix H0;
+            PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_, H0), *K_);
+            Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
+            *H1 = Hprj * H0 * (*H1);
+            *H2 = Hprj * H0 * (*H2);
+            return reprojectionError;
+          } else {
+            PinholeCamera<Cal3_S2> camera(pose.compose(*body_P_sensor_), *K_);
+            return camera.project(point, Hprj, H3, boost::none) - measured_;
+          }
+        } else {
+          PinholeCamera<Cal3_S2> camera(pose, *K_);
+          Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_);
+          if (H1) *H1 = Hprj * (*H1);
+          if (H2) *H2 = Hprj * (*H2);
+          return reprojectionError;
+        }
+      } catch( CheiralityException& e) {
+        if (H1) *H1 = Matrix::Zero(2,6);
+        if (H2) *H2 = Matrix::Zero(2,6);
+        if (H3) *H3 = Matrix::Zero(2,3);
+        if (verboseCheirality_)
+          std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
+              " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
+        if (throwCheirality_)
+          throw CheiralityException(this->key2());
+      }
+      return Vector2::Constant(2.0 * K_->fx());
+    }
+
+    /** return the measurement */
+    const Point2& measured() const {
+      return measured_;
+    }
+
+    /** return the calibration object */
+    inline const boost::shared_ptr<Cal3_S2> calibration() const {
+      return K_;
+    }
+
+    /** returns the rolling shutter interp param*/
+    inline double interp_param() const {return interp_param_; }
+
+    /** return verbosity */
+    inline bool verboseCheirality() const { return verboseCheirality_; }
+
+    /** return flag for throwing cheirality exceptions */
+    inline bool throwCheirality() const { return throwCheirality_; }
+
+  private:
+
+    /// Serialization function
+    friend class boost::serialization::access;
+    template<class ARCHIVE>
+    void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
+      ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
+      ar & BOOST_SERIALIZATION_NVP(measured_);
+      ar & BOOST_SERIALIZATION_NVP(interp_param_);
+      ar & BOOST_SERIALIZATION_NVP(K_);
+      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+      ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
+      ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
+    }
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  }; // rolling shutter projection factor
+} //namespace gtsam