diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 48d7bf531..34422edd7 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,13 +320,14 @@ T expm(const Vector& x, int K=7) { } /** - * Linear interpolation between X and Y by coefficient t in [0, 1.5] (t>1 implies extrapolation), with optional jacobians. + * Linear interpolation between X and Y by coefficient t (typically t \in [0,1], + * but can also be used to extrapolate before pose X or after pose Y), with optional jacobians. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - assert(t >= 0.0 && t <= 1.5); + if (Hx || Hy) { typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..5fc1c05eb --- /dev/null +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -0,0 +1,73 @@ +/* ---------------------------------------------------------------------------- + + * 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 ProjectionFactorRollingShutter.cpp + * @brief Basic projection factor for rolling shutter cameras + * @author Yotam Stern + */ + +#include + +namespace gtsam { + +Vector ProjectionFactorRollingShutter::evaluateError( + const Pose3& pose_a, const Pose3& pose_b, const Point3& point, + boost::optional H1, boost::optional H2, + boost::optional H3) const { + + try { + Pose3 pose = interpolate(pose_a, pose_b, alpha_, H1, H2); + gtsam::Matrix Hprj; + 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 * HbodySensor * (*H1); + if (H2) + *H2 = Hprj * HbodySensor * (*H2); + return reprojectionError; + } else { + PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); + return camera.project(point) - 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()); +} + +} //namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 6ab16f4a0..5827a538c 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -31,8 +31,8 @@ namespace gtsam { * 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. + * Let us define the alpha = (t_p - t_A) / (t_B - t_A), we will use the pose interpolated between A and B by + * the alpha to project the corresponding landmark to Point2. * @addtogroup SLAM */ @@ -42,7 +42,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 K_; ///< shared pointer to calibration object boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -64,7 +64,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, @@ -88,7 +88,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, @@ -117,7 +117,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3(&p); - return e && Base::equals(p, tol) && (interp_param_ == e->interp_param()) + return e && Base::equals(p, tol) && (alpha_ == e->alpha()) && traits::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol) && (this->throwCheirality_ == e->throwCheirality_) @@ -167,53 +167,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 H1 = boost::none, boost::optional H2 = boost::none, - boost::optional H3 = boost::none) const { - - try { - Pose3 pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - gtsam::Matrix Hprj; - 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 * HbodySensor * (*H1); - if (H2) - *H2 = Hprj * HbodySensor * (*H2); - return reprojectionError; - } else { - PinholeCamera camera(pose.compose(*body_P_sensor_), *K_); - return camera.project(point) - 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()); - } + boost::optional H3 = boost::none) const; /** return the measurement */ const Point2& measured() const { @@ -226,8 +180,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector interp_params_; + std::vector alphas_; /// Pose of the camera in the body frame std::vector body_P_sensors_; @@ -92,12 +92,12 @@ PinholePose > { * single landmark in a single view (the measurement), interpolated from the 2 poses * @param world_P_body_key1 key corresponding to the first body poses (time <= time pixel is acquired) * @param world_P_body_key2 key corresponding to the second body poses (time >= time pixel is acquired) - * @param interp_param interpolation factor in [0,1], such that if interp_param = 0 the interpolated pose is the same as world_P_body_key1 + * @param alpha interpolation factor in [0,1], such that if alpha = 0 the interpolated pose is the same as world_P_body_key1 * @param K (fixed) camera intrinsic calibration * @param body_P_sensor (fixed) camera extrinsic calibration */ void add(const Point2& measured, const Key& world_P_body_key1, - const Key& world_P_body_key2, const double& interp_param, + const Key& world_P_body_key2, const double& alpha, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class this->measured_.push_back(measured); @@ -113,7 +113,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factor - interp_params_.push_back(interp_param); + alphas_.push_back(alpha); // store fixed intrinsic calibration K_all_.push_back(K); @@ -128,21 +128,21 @@ PinholePose > { * of a single landmark in the m views (the measurements) * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated - * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * @param Ks vector of (fixed) intrinsic calibration objects * @param body_P_sensors vector of (fixed) extrinsic calibration objects */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& interp_params, + const std::vector& alphas, const std::vector>& Ks, const std::vector body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == interp_params.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, interp_params[i], Ks[i], + world_P_body_key_pairs[i].second, alphas[i], Ks[i], body_P_sensors[i]); } } @@ -154,19 +154,19 @@ PinholePose > { * of a single landmark in the m views (the measurements) * @param world_P_body_key_pairs vector where the i-th element contains a pair of keys corresponding * to the pair of poses from which the observation pose for the i0-th measurement can be interpolated - * @param interp_params vector of interpolation params, one for each measurement (in the same order) + * @param alphas vector of interpolation params (in [0,1]), one for each measurement (in the same order) * @param K (fixed) camera intrinsic calibration (same for all measurements) * @param body_P_sensor (fixed) camera extrinsic calibration (same for all measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& interp_params, + const std::vector& alphas, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == interp_params.size()); + assert(world_P_body_key_pairs.size() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor); + world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); } } @@ -180,9 +180,9 @@ PinholePose > { return world_P_body_key_pairs_; } - /// return the interpolation factors interp_params - const std::vector interp_params() const { - return interp_params_; + /// return the interpolation factors alphas + const std::vector alphas() const { + return alphas_; } /// return the extrinsic camera calibration body_P_sensors @@ -204,7 +204,7 @@ PinholePose > { << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; - std::cout << " interp_param: " << interp_params_[i] << std::endl; + std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -239,7 +239,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual; + && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -267,7 +267,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each camera/measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_params_[i]; + double interpolationFactor = alphas_[i]; // get interpolated pose: const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); const Pose3& body_P_cam = body_P_sensors_[i]; @@ -377,7 +377,7 @@ PinholePose > { typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); assert(numViews == K_all_.size()); - assert(numViews == interp_params_.size()); + assert(numViews == alphas_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -385,7 +385,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); - double interpolationFactor = interp_params_[i]; + double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& w_P_cam = w_P_body.compose(body_P_cam);