diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c934..e4a66fad1 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -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 -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); +template +T interpolate(const T& X, const T& Y, double t, OptionalJacobian Hx = boost::none, OptionalJacobian Hy = boost::none) { + assert(t >= 0 && t <= 1.5); + if (Hx && Hy) { + typedef Eigen::Matrix Jacobian; + typename traits::TangentVector tres; + T tres1; + Jacobian d1; + Jacobian d2; + + tres1 = traits::Between(X, Y, Hx, Hy); + tres = traits::Logmap(tres1, d1); + *Hx = d1 * (*Hx); + *Hy = d1 * (*Hy); + tres1 = traits::Expmap(t * tres, d1); + *Hx = t * d1 * (*Hx); + *Hy = t * d1 * (*Hy); + tres1 = traits::Compose(X, tres1, d1, d2); + *Hx = d1 + d2 * (*Hx); + *Hy = d2 * (*Hy); + return tres1; + } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c183e32ed..b2cdd0c87 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/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 diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 318baab3d..f84d02965 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -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); diff --git a/gtsam_unstable/slam/RollingShutterProjectionFactor.h b/gtsam_unstable/slam/RollingShutterProjectionFactor.h new file mode 100644 index 000000000..69b7e7376 --- /dev/null +++ b/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 +#include +#include + +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 { + 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 + + // 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 Base; + + /// shorthand for this class + typedef RollingShutterProjectionFactor This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr 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& 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) + */ + RollingShutterProjectionFactor(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 ~RollingShutterProjectionFactor() {} + + /// @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))); } + + /** + * 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::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_))); + } + + /// 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; + 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 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_); + 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 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 + 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 \ No newline at end of file