diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 499f1cec3..b5bead621 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -23,192 +23,230 @@ 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 - */ +/** + * Non-linear factor for 2D projection measurement obtained using a rolling shutter camera. The calibration is known here. + * This version takes rolling shutter information into account as follows: consider two consecutive poses A and B, + * and a Point2 measurement taken starting at time A using a rolling shutter camera. + * 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 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 the corresponding landmark to Point2. + * @addtogroup SLAM + */ - class ProjectionFactorRollingShutter: public NoiseModelFactor3 { - protected: +class ProjectionFactorRollingShutter : public NoiseModelFactor3 { + 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 K_; ///< shared pointer to calibration object - boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + // Keep a copy of measurement and calibration for I/O + Point2 measured_; ///< 2D measurement + double interp_param_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + boost::shared_ptr K_; ///< shared pointer to calibration object + boost::optional 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) + // 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: + public: - /// shorthand for base class type - typedef NoiseModelFactor3 Base; + /// shorthand for base class type + typedef NoiseModelFactor3 Base; - /// shorthand for this class - typedef ProjectionFactorRollingShutter This; + /// shorthand for this class + typedef ProjectionFactorRollingShutter This; - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; - /// Default constructor - ProjectionFactorRollingShutter() : - measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { + /// Default constructor + ProjectionFactorRollingShutter() + : 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) - */ - ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, - Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, - boost::optional 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 + * @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 noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @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, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + boost::optional 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) - */ - ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, - Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, - bool throwCheirality, bool verboseCheirality, - boost::optional 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) {} + /** + * Constructor with exception-handling flags + * @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 noise model + * @param poseKey_a is the key of the first camera + * @param poseKey_b is the key of the second camera + * @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, + Key poseKey_a, Key poseKey_b, Key pointKey, + const boost::shared_ptr& K, + bool throwCheirality, bool verboseCheirality, + boost::optional 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 ~ProjectionFactorRollingShutter() {} + /** Virtual destructor */ + virtual ~ProjectionFactorRollingShutter() { + } - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + /// @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 << "ProjectionFactorRollingShutter, z = "; - traits::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); - } + /** + * 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 << "ProjectionFactorRollingShutter, z = "; + traits::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(&p); - return e - && Base::equals(p, tol) - && (interp_param_ == e->interp_param()) - && traits::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_))); - } + /// equals + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && (interp_param_ == e->interp_param()) + && traits::Equals(this->measured_, e->measured_, tol) + && this->K_->equals(*e->K_, tol) + && (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 H1 = boost::none, boost::optional H2 = boost::none, boost::optional H3 = boost::none) const { + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const { - Pose3 pose; + try { + Pose3 pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); gtsam::Matrix Hprj; - - //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - try { - if(body_P_sensor_) { - if(H1 && H2) { - gtsam::Matrix H0; - PinholeCamera 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 camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point, Hprj, H3, boost::none) - measured_; - } - } else { - PinholeCamera camera(pose, *K_); + if (body_P_sensor_) { + if (H1 || H2 || H3) { + gtsam::Matrix HbodySensor; + PinholeCamera camera(pose.compose(*body_P_sensor_, HbodySensor), *K_); Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); - if (H1) *H1 = Hprj * (*H1); - if (H2) *H2 = Hprj * (*H2); + if (H1) + *H1 = Hprj * HbodySensor * (*H1); + if (H2) + *H2 = Hprj * HbodySensor * (*H2); return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - measured_; } - } 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()); + } else { + PinholeCamera 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; } - return Vector2::Constant(2.0 * K_->fx()); + } 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 measurement */ + const Point2& measured() const { + return measured_; + } - /** return the calibration object */ - inline const boost::shared_ptr calibration() const { - return K_; - } + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } - /** returns the rolling shutter interp param*/ - inline double interp_param() const {return interp_param_; } + /** returns the rolling shutter interp param*/ + inline double interp_param() const { + return interp_param_; + } - /** return verbosity */ - inline bool verboseCheirality() const { return verboseCheirality_; } + /** return verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { return throwCheirality_; } + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } - private: + private: - /// Serialization function - friend class boost::serialization::access; - template - 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 + /// Serialization function + friend class boost::serialization::access; + template + 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