rolling shutter projection factor
							parent
							
								
									301f7fe1a3
								
							
						
					
					
						commit
						d4eaa15280
					
				|  | @ -319,11 +319,30 @@ T expm(const Vector& x, int K=7) { | |||
| } | ||||
| 
 | ||||
| /**
 | ||||
|  * Linear interpolation between X and Y by coefficient t in [0, 1]. | ||||
|  * Linear interpolation and some extrapolation between X and Y by coefficient t in [0, 1.5], optinal jacobians calculations. | ||||
|  */ | ||||
| template <typename T> | ||||
| T interpolate(const T& X, const T& Y, double t) { | ||||
|   assert(t >= 0 && t <= 1); | ||||
| template <typename T, int N> | ||||
| T interpolate(const T& X, const T& Y, double t, OptionalJacobian<N, N> Hx = boost::none, OptionalJacobian<N, N> Hy = boost::none) { | ||||
|   assert(t >= 0 && t <= 1.5); | ||||
|   if (Hx && Hy) { | ||||
|     typedef Eigen::Matrix<double, N, N> Jacobian; | ||||
|     typename traits<T>::TangentVector tres; | ||||
|     T tres1; | ||||
|     Jacobian d1; | ||||
|     Jacobian d2; | ||||
| 
 | ||||
|     tres1 = traits<T>::Between(X, Y, Hx, Hy); | ||||
|     tres = traits<T>::Logmap(tres1, d1); | ||||
|     *Hx = d1 * (*Hx); | ||||
|     *Hy = d1 * (*Hy); | ||||
|     tres1 = traits<T>::Expmap(t * tres, d1); | ||||
|     *Hx = t * d1 * (*Hx); | ||||
|     *Hy = t * d1 * (*Hy); | ||||
|     tres1 = traits<T>::Compose(X, tres1, d1, d2); | ||||
|     *Hx = d1 + d2 * (*Hx); | ||||
|     *Hy = d2 * (*Hy); | ||||
|     return tres1; | ||||
|   } | ||||
|   return traits<T>::Compose(X, traits<T>::Expmap(t * traits<T>::Logmap(traits<T>::Between(X, Y)))); | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -353,6 +353,15 @@ public: | |||
|     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); | ||||
|  |  | |||
|  | @ -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
 | ||||
		Loading…
	
		Reference in New Issue