diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c9340..250ee6bcfb 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta + * @author shteren1 */ @@ -319,11 +320,33 @@ 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 -T interpolate(const T& X, const T& Y, double t) { - assert(t >= 0 && t <= 1); +T interpolate(const T& X, const T& Y, double t, + OptionalJacobian< traits::dimension, traits::dimension > Hx = boost::none, + OptionalJacobian< traits::dimension, traits::dimension > Hy = boost::none) { + assert(t >= 0.0 && t <= 1.5); + if (Hx || Hy) { + typedef Eigen::Matrix::dimension, traits::dimension> Jacobian; + typename traits::TangentVector log_Xinv_Y; + Jacobian Hx_tmp, Hy_tmp, H1, H2; + + T Xinv_Y = traits::Between(X, Y, Hx_tmp, Hy_tmp); + log_Xinv_Y = traits::Logmap(Xinv_Y, H1); + Hx_tmp = H1 * Hx_tmp; + Hy_tmp = H1 * Hy_tmp; + Xinv_Y = traits::Expmap(t * log_Xinv_Y, H1); + Hx_tmp = t * H1 * Hx_tmp; + Hy_tmp = t * H1 * Hy_tmp; + Xinv_Y = traits::Compose(X, Xinv_Y, H1, H2); + Hx_tmp = H1 + H2 * Hx_tmp; + Hy_tmp = H2 * Hy_tmp; + + if(Hx) *Hx = Hx_tmp; + if(Hy) *Hy = Hy_tmp; + return Xinv_Y; + } 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 c183e32ed4..b2cdd0c87a 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 d76e1b41a1..00c6fa8af0 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -353,6 +353,15 @@ class GTSAM_EXPORT Pose3: public LieGroup { 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/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 11b8791d46..7c1fa81e6b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -1046,6 +1046,68 @@ TEST(Pose3, interpolate) { EXPECT(assert_equal(expected2, T2.interpolateRt(T3, t))); } +/* ************************************************************************* */ +Pose3 testing_interpolate(const Pose3& t1, const Pose3& t2, double gamma) { return interpolate(t1,t2,gamma); } + +TEST(Pose3, interpolateJacobians) { + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(1, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0.5, -0.207107, 0)); // note: different from test above: this is full Pose3 interpolation + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::identity(), Point3(1, 0, 0)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::identity(), Point3(0.3, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X = Pose3::identity(); + Pose3 Y(Rot3::Rz(M_PI_2), Point3(0, 0, 0)); + double t = 0.5; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + EXPECT(assert_equal(expectedPoseInterp, interpolate(X, Y, t, actualJacobianX, actualJacobianY), 1e-5)); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } + { + Pose3 X(Rot3::Ypr(0.1,0.2,0.3), Point3(10, 5, -2)); + Pose3 Y(Rot3::Ypr(1.1,-2.2,-0.3), Point3(-5, 1, 1)); + double t = 0.3; + Pose3 expectedPoseInterp(Rot3::Rz(M_PI_4), Point3(0, 0, 0)); + Matrix actualJacobianX, actualJacobianY; + interpolate(X, Y, t, actualJacobianX, actualJacobianY); + + Matrix expectedJacobianX = numericalDerivative31(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianX,actualJacobianX,1e-6)); + + Matrix expectedJacobianY = numericalDerivative32(testing_interpolate, X, Y, t); + EXPECT(assert_equal(expectedJacobianY,actualJacobianY,1e-6)); + } +} + /* ************************************************************************* */ TEST(Pose3, Create) { Matrix63 actualH1, actualH2; diff --git a/gtsam_unstable/slam/RollingShutterProjectionFactor.h b/gtsam_unstable/slam/RollingShutterProjectionFactor.h new file mode 100644 index 0000000000..69b7e73768 --- /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 diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 0000000000..f1f0e2d719 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,460 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time + * @author Luca Carlone + */ + +#pragma once + +#include + +namespace gtsam { +/** + * + * @addtogroup SLAM + * + * If you are using the factor, please cite: + * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, + * Eliminating conditionally independent sets in factor graphs: + * a unifying perspective based on smart factors, + * Int. Conf. on Robotics and Automation (ICRA), 2014. + */ + +/** + * This factor optimizes the pose of the body assuming a rolling shutter model of the camera with given readout time. + * This factor requires that values contain (for each pixel observation) consecutive camera poses + * from which the pixel observation pose can be interpolated. + * @addtogroup SLAM + */ +template +class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< + PinholePose > { + + protected: + /// shared pointer to calibration object (one for each observation) + std::vector > K_all_; + + /// The keys of the pose of the body (with respect to an external world frame): two consecutive poses for each observation + std::vector> world_P_body_key_pairs_; + + /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses + std::vector gammas_; + + /// Pose of the camera in the body frame + std::vector body_P_sensors_; ///< Pose of the camera in the body frame + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef PinholePose Camera; + /// shorthand for base class type + typedef SmartProjectionFactor Base; + + /// shorthand for this class + typedef SmartProjectionPoseFactorRollingShutter This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef std::vector > FBlocks; // vector of F blocks + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + } + + /** Virtual destructor */ + ~SmartProjectionPoseFactorRollingShutter() override = default; + + /** + * add a new measurement, with 2 pose keys, camera calibration, and observed pixel. + * @param measured is the 2-dimensional location of the projection of a + * single landmark in the a single view (the measurement), interpolated from the 2 poses + * @param world_P_body_key1 is the key corresponding to the first body poses (time <= time pixel is acquired) + * @param world_P_body_key2 is the key corresponding to the second body poses (time >= time pixel is acquired) + * @param gamma in [0,1] is the interpolation factor, such that if gamma = 0 the interpolated pose is the same as world_P_body_key + * @param K is the (fixed) camera intrinsic calibration + */ + void add(const Point2& measured, const Key& world_P_body_key1, + const Key& world_P_body_key2, const double& gamma, + const boost::shared_ptr& K, const Pose3 body_P_sensor) { + // store measurements in base class (note: we only store the first key there) + Base::add(measured, world_P_body_key1); + // but we also store the extrinsic calibration keys in the same order + world_P_body_key_pairs_.push_back( + std::make_pair(world_P_body_key1, world_P_body_key2)); + + // pose keys are assumed to be unique, so we avoid duplicates here + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key1) + == this->keys_.end()) + this->keys_.push_back(world_P_body_key1); // add only unique keys + if (std::find(this->keys_.begin(), this->keys_.end(), world_P_body_key2) + == this->keys_.end()) + this->keys_.push_back(world_P_body_key2); // add only unique keys + + // store fixed calibration + K_all_.push_back(K); + + // store extrinsics of the camera + body_P_sensors_.push_back(body_P_sensor); + } + + /** + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated + * @param Ks vector of intrinsic calibration objects + */ + void add(const std::vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& gammas, + 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() == gammas.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, gammas[i], Ks[i], + body_P_sensors[i]); + } + } + + /** + * Variant of the previous one in which we include a set of measurements with + * the same (intrinsic and extrinsic) calibration + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m views (the measurements) + * @param world_P_body_key_pairs vector of (1 for each view) containing the pair of poses from which each view can be interpolated + * @param K the (known) camera calibration (same for all measurements) + */ + void add(const std::vector& measurements, + const std::vector>& world_P_body_key_pairs, + const std::vector& gammas, + const boost::shared_ptr& K, const Pose3 body_P_sensor) { + assert(world_P_body_key_pairs.size() == measurements.size()); + assert(world_P_body_key_pairs.size() == gammas.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, gammas[i], K, body_P_sensor); + } + } + + /// return the calibration object + inline std::vector> calibration() const { + return K_all_; + } + + /// return the interpolation factors gammas + const std::vector getGammas() const { + return gammas_; + } + + /// return the interpolation factors gammas + const std::vector body_P_sensors() const { + return body_P_sensors_; + } + + /** + * 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 override { + std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + std::cout << "-- Measurement nr " << i << std::endl; + std::cout << " pose1 key: " + << 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 << " gamma: " << gammas_[i] << std::endl; + K_all_[i]->print("calibration = "); + } + Base::print("", keyFormatter); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>(&p); + + return e && Base::equals(p, tol) && K_all_ == e->calibration() + && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors(); + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + assert(world_P_body_keys_.size() == K_all_.size()); + assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + typename Base::Cameras cameras; + for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { + Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = gammas_[i]; + // get interpolated pose: + Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, interpolationFactor); + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, K_all_[i]); + } + return cameras; + } + + /** + * Compute jacobian F, E and error vector at a given linearization point + * @param values Values structure which must contain camera poses + * corresponding to keys involved in this factor + * @return Return arguments are the camera jacobians Fs (including the jacobian with + * respect to both the body pose and extrinsic pose), the point Jacobian E, + * and the error vector b. Note that the jacobians are computed for a given point. + */ + void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, + const Values& values) const + override { + if (!this->result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + size_t numViews = this->measured_.size(); + E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + b = Vector::Zero(2 * numViews); // a Point2 for each view + Eigen::Matrix dProject_dPoseCam; + Eigen::Matrix dInterpPose_dPoseBody1, + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + Eigen::Matrix Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + Pose3 w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + Pose3 w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + double interpolationFactor = gammas_[i]; + // get interpolated pose: + std::cout << "TODO: need to add proper interpolation and Jacobians here" << std::endl; + Pose3 w_P_body = w_P_body1.interpolateRt(w_P_body2, + interpolationFactor); /*dInterpPose_dPoseBody1, dInterpPose_dPoseBody2 */ + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + PinholeCamera camera(w_P_cam, K_all_[i]); + + // get jacobians and error vector for current measurement + Point2 reprojectionError_i = Point2( + camera.project(*this->result_, dProject_dPoseCam, Ei) + - this->measured_.at(i)); + Eigen::Matrix J; // 2 x 12 + J.block(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose + * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6) = dProject_dPoseCam * dPoseCam_dInterpPose + * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + + // fit into the output structures + Fs.push_back(J); + size_t row = 2 * i; + b.segment(row) = -reprojectionError_i; + E.block<3, 3>(row, 0) = Ei; + } + } + } + + /// linearize and return a Hessianfactor that is an approximation of error(p) + boost::shared_ptr > createHessianFactor( + const Values& values, const double lambda = 0.0, bool diagonalDamping = + false) const override { + + // we may have multiple cameras sharing the same extrinsic cals, hence the number + // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we + // have a body key and an extrinsic calibration key for each measurement) + size_t nrUniqueKeys = this->keys_.size(); + size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); + + // Create structures for Hessian Factors + KeyVector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); + + if (this->measured_.size() != cameras(values).size()) + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + triangulateSafe(cameras(values)); + + if (!this->result_) { // failed: return "empty/zero" Hessian + for (Matrix& m : Gs) + m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) + v = Vector::Zero(DimPose); + return boost::make_shared < RegularHessianFactor + > ( this->keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + + // Whiten using noise model + this->noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = this->noiseModel_->Whiten(Fs[i]); + +// // build augmented Hessian (with last row/column being the information vector) +// Matrix3 P; +// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// +// // marginalize point: note - we reuse the standard SchurComplement function +// SymmetricBlockMatrix augmentedHessian = This::Cameras::SchurComplement<2,DimBlock>(Fs, E, P, b); + +// // now pack into an Hessian factor +// std::vector dims(nrUniqueKeys + 1); // this also includes the b term +// std::fill(dims.begin(), dims.end() - 1, 6); +// dims.back() = 1; +// SymmetricBlockMatrix augmentedHessianUniqueKeys; +// +// // here we have to deal with the fact that some cameras may share the same extrinsic key +// if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix(augmentedHessian.selfadjointView())); +// } else { // if multiple cameras share a calibration we have to rearrange +// // the results of the Schur complement matrix +// std::vector nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term +// std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); +// nonuniqueDims.back() = 1; +// augmentedHessian = SymmetricBlockMatrix( +// nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); +// +// // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) +// KeyVector nonuniqueKeys; +// for (size_t i = 0; i < world_P_body_key_pairs_.size(); i++) { +// nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i)); +// nonuniqueKeys.push_back(body_P_cam_ this->keys_.at(i)); +// } +// +// // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) +// std::map keyToSlotMap; +// for (size_t k = 0; k < nrUniqueKeys; k++) { +// keyToSlotMap[ this->keys_[k]] = k; +// } +// +// // initialize matrix to zero +// augmentedHessianUniqueKeys = SymmetricBlockMatrix( +// dims, Matrix::Zero(6 * nrUniqueKeys + 1, 6 * nrUniqueKeys + 1)); +// +// // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian) +// // and populates an Hessian that only includes the unique keys (that is what we want to return) +// for (size_t i = 0; i < nrNonuniqueKeys; i++) { // rows +// Key key_i = nonuniqueKeys.at(i); +// +// // update information vector +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], nrUniqueKeys, +// augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); +// +// // update blocks +// for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols +// Key key_j = nonuniqueKeys.at(j); +// if (i == j) { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], augmentedHessian.diagonalBlock(i)); +// } else { // (i < j) +// if (keyToSlotMap[key_i] != keyToSlotMap[key_j]) { +// augmentedHessianUniqueKeys.updateOffDiagonalBlock( +// keyToSlotMap[key_i], keyToSlotMap[key_j], +// augmentedHessian.aboveDiagonalBlock(i, j)); +// } else { +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// keyToSlotMap[key_i], +// augmentedHessian.aboveDiagonalBlock(i, j) +// + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); +// } +// } +// } +// } +// // update bottom right element of the matrix +// augmentedHessianUniqueKeys.updateDiagonalBlock( +// nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); +// } +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, augmentedHessianUniqueKeys); + } + + /** + * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) + * @param values Values structure which must contain camera poses and extrinsic pose for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped( + const Values& values, const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (this->params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(values, lambda); + default: + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); + } + } + + /// linearize + boost::shared_ptr linearize(const Values& values) const + override { + return linearizeDamped(values); + } + + 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(K_all_); + } + +}; +// end of class declaration + +/// traits +template +struct traits > : + public Testable > { +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 0000000000..d4c268b3cc --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,899 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include "gtsam/slam/tests/smartFactorScenarios.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace boost::assign; +using namespace std::placeholders; + +static const double rankTol = 1.0; +// Create a noise model for the pixel error +static const double sigma = 0.1; +static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; + +// tests data +static Symbol x1('X', 1); +static Symbol x2('X', 2); +static Symbol x3('X', 3); + +static Point2 measurement1(323.0, 240.0); + +LevenbergMarquardtParams lmParams; +// Make more verbose like so (in tests): +// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, Constructor) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); +} + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, Constructor2) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, params); +} + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, Constructor3) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); +} + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, Constructor4) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(model, sharedK, params); + factor1.add(measurement1, x1); +} + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, params) { + using namespace vanillaPose; + SmartProjectionParams params; + double rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); + params.setRetriangulationThreshold(1e-3); + rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); +} + +/* ************************************************************************* * +TEST( SmartProjectionPoseFactor, Equals ) { + using namespace vanillaPose; + SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); + factor1->add(measurement1, x1); + + SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK)); + factor2->add(measurement1, x1); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, noiseless ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); + + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); + + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); + + // get point + boost::optional point = factor.point(); + CHECK(point); + + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); + + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); + + EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7)); + + // Calculate using computeJacobians + Vector b; + SmartFactor::FBlocks Fs; + factor.computeJacobians(Fs, E, b, cameras, *point); + double actualError3 = b.squaredNorm(); + EXPECT(assert_equal(expectedE, E, 1e-7)); + EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, noisy ) { + + using namespace vanillaPose; + + // Project two landmarks into two cameras + Point2 pixelError(0.2, 0.2); + Point2 level_uv = level_camera.project(landmark1) + pixelError; + Point2 level_uv_right = level_camera_right.project(landmark1); + + Values values; + values.insert(x1, cam1.pose()); + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + values.insert(x2, pose_right.compose(noise_pose)); + + SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); + factor->add(level_uv, x1); + factor->add(level_uv_right, x2); + + double actualError1 = factor->error(values); + + SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + +/* ************************************************************************* +TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body = body_T_sensor.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body; + Pose3 wTb2 = cam2.pose() * sensor_T_body; + Pose3 wTb3 = cam3.pose() * sensor_T_body; + + // three landmarks ~5 meters infront of camera + Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + // Create smart factors + KeyVector views {x1, x2, x3}; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); + smartFactor1.add(measurements_cam1, views); + + SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); + smartFactor2.add(measurements_cam2, views); + + SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); + smartFactor3.add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + // Put all factors in factor graph, adding priors + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, wTb1, noisePrior); + graph.addPrior(x2, wTb2, noisePrior); + + // Check errors at ground truth poses + Values gtValues; + gtValues.insert(x1, wTb1); + gtValues.insert(x2, wTb2); + gtValues.insert(x3, wTb3); + double actualError = graph.error(gtValues); + double expectedError = 0.0; + DOUBLES_EQUAL(expectedError, actualError, 1e-7) + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); + Values values; + values.insert(x1, wTb1); + values.insert(x2, wTb2); + // initialize third pose with some noise, we expect it to move back to + // original pose3 + values.insert(x3, wTb3 * noise_pose); + + LevenbergMarquardtParams lmParams; + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(wTb3, result.at(x3))); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + KeyVector views; + views.push_back(x1); + views.push_back(x2); + views.push_back(x3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values groundTruth; + groundTruth.insert(x1, cam1.pose()); + groundTruth.insert(x2, cam2.pose()); + groundTruth.insert(x3, cam3.pose()); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0, -0.0314107591, 0.99950656, -0.99950656, -0.0313952598, + -0.000986635786, 0.0314107591, -0.999013364, -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, Factors ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + Rot3 R; + static Cal3_S2::shared_ptr sharedK(new Cal3_S2(100, 100, 0, 0, 0)); + Camera cam1(Pose3(R, Point3(0, 0, 0)), sharedK), cam2( + Pose3(R, Point3(1, 0, 0)), sharedK); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_cam1; + + // Project 2 landmarks into 2 cameras + measurements_cam1.push_back(cam1.project(landmark1)); + measurements_cam1.push_back(cam2.project(landmark1)); + + // Create smart factors + KeyVector views {x1, x2}; + + SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + CHECK(smartFactor1->triangulateSafe(cameras)); + CHECK(!smartFactor1->isDegenerate()); + CHECK(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + CHECK(p); + EXPECT(assert_equal(landmark1, *p)); + + VectorValues zeroDelta; + Vector6 delta; + delta.setZero(); + zeroDelta.insert(x1, delta); + zeroDelta.insert(x2, delta); + + VectorValues perturbedDelta; + delta.setOnes(); + perturbedDelta.insert(x1, delta); + perturbedDelta.insert(x2, delta); + double expectedError = 2500; + + // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + Matrix16 A1, A2; + A1 << -10, 0, 0, 0, 1, 0; + A2 << 10, 0, 1, 0, -1, 0; + A1 *= 10. / sigma; + A2 *= 10. / sigma; + Matrix expectedInformation; // filled below + { + // createHessianFactor + Matrix66 G11 = 0.5 * A1.transpose() * A1; + Matrix66 G12 = 0.5 * A1.transpose() * A2; + Matrix66 G22 = 0.5 * A2.transpose() * A2; + + Vector6 g1; + g1.setZero(); + Vector6 g2; + g2.setZero(); + + double f = 0; + + RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); + expectedInformation = expected.information(); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(cameras, 0.0); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual, 1e-6)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + Matrix26 F1; + F1.setZero(); + F1(0, 1) = -100; + F1(0, 3) = -10; + F1(1, 0) = 100; + F1(1, 4) = -10; + Matrix26 F2; + F2.setZero(); + F2(0, 1) = -101; + F2(0, 3) = -10; + F2(0, 5) = -1; + F2(1, 0) = 100; + F2(1, 2) = 10; + F2(1, 4) = -10; + Matrix E(4, 3); + E.setZero(); + E(0, 0) = 10; + E(1, 1) = 10; + E(2, 0) = 10; + E(2, 2) = 1; + E(3, 1) = 10; + SmartFactor::FBlocks Fs = list_of(F1)(F2); + Vector b(4); + b.setZero(); + + // Create smart factors + KeyVector keys; + keys.push_back(x1); + keys.push_back(x2); + + // createJacobianQFactor + SharedIsotropic n = noiseModel::Isotropic::Sigma(4, sigma); + Matrix3 P = (E.transpose() * E).inverse(); + JacobianFactorQ<6, 2> expectedQ(keys, Fs, E, P, b, n); + EXPECT(assert_equal(expectedInformation, expectedQ.information(), 1e-6)); + + boost::shared_ptr > actualQ = + smartFactor1->createJacobianQFactor(cameras, 0.0); + CHECK(actualQ); + EXPECT(assert_equal(expectedInformation, actualQ->information(), 1e-6)); + EXPECT(assert_equal(expectedQ, *actualQ)); + EXPECT_DOUBLES_EQUAL(0, actualQ->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actualQ->error(perturbedDelta), 1e-6); + + // Whiten for RegularImplicitSchurFactor (does not have noise model) + model->WhitenSystem(E, b); + Matrix3 whiteP = (E.transpose() * E).inverse(); + Fs[0] = model->Whiten(Fs[0]); + Fs[1] = model->Whiten(Fs[1]); + + // createRegularImplicitSchurFactor + RegularImplicitSchurFactor expected(keys, Fs, E, whiteP, b); + + boost::shared_ptr > actual = + smartFactor1->createRegularImplicitSchurFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } + + { + // createJacobianSVDFactor + Vector1 b; + b.setZero(); + double s = sigma * sin(M_PI_4); + SharedIsotropic n = noiseModel::Isotropic::Sigma(4 - 3, sigma); + JacobianFactor expected(x1, s * A1, x2, s * A2, b, n); + EXPECT(assert_equal(expectedInformation, expected.information(), 1e-6)); + + boost::shared_ptr actual = + smartFactor1->createJacobianSVDFactor(cameras, 0.0); + CHECK(actual); + EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); + EXPECT(assert_equal(expected, *actual)); + EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); + } +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656, + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), Point3(0.1, -0.1, 1.9)), + values.at(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, jacobianSVD ) { + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(model, sharedK, params); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, landmarkDistance ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 2; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, pose_above * noise_pose); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(values.at(x3), result.at(x3))); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + // add fourth landmark + Point3 landmark4(5, -0.5, 1); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, + measurements_cam4; + + // Project 4 landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); + + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, params)); + smartFactor4->add(measurements_cam4, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.push_back(smartFactor4); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.addPrior(x2, cam2.pose(), noisePrior); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + // All factors are disabled and pose should remain where it is + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3))); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + typedef GenericProjectionFactor ProjectionFactor; + NonlinearFactorGraph graph; + + // Project three landmarks into three cameras + graph.emplace_shared(cam1.project(landmark1), model, x1, L(1), sharedK2); + graph.emplace_shared(cam2.project(landmark1), model, x2, L(1), sharedK2); + graph.emplace_shared(cam3.project(landmark1), model, x3, L(1), sharedK2); + + graph.emplace_shared(cam1.project(landmark2), model, x1, L(2), sharedK2); + graph.emplace_shared(cam2.project(landmark2), model, x2, L(2), sharedK2); + graph.emplace_shared(cam3.project(landmark2), model, x3, L(2), sharedK2); + + graph.emplace_shared(cam1.project(landmark3), model, x1, L(3), sharedK2); + graph.emplace_shared(cam2.project(landmark3), model, x2, L(3), sharedK2); + graph.emplace_shared(cam3.project(landmark3), model, x3, L(3), sharedK2); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + graph.addPrior(x1, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above * noise_pose); + values.insert(L(1), landmark1); + values.insert(L(2), landmark2); + values.insert(L(3), landmark3); + + DOUBLES_EQUAL(48406055, graph.error(values), 1); + + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + + DOUBLES_EQUAL(0, graph.error(result), 1e-9); + + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, CheckHessian) { + + KeyVector views {x1, x2, x3}; + + using namespace vanillaPose; + + // Two slightly different cameras + Pose3 pose2 = level_pose + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + Camera cam2(pose2, sharedK); + Camera cam3(pose3, sharedK); + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + + SmartProjectionParams params; + params.setRankTolerance(10); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + + // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose3 * noise_pose); + EXPECT( + assert_equal( + Pose3( + Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), + Point3(0.0897734171, -0.110201006, 0.901022872)), + values.at(x3))); + + boost::shared_ptr factor1 = smartFactor1->linearize(values); + boost::shared_ptr factor2 = smartFactor2->linearize(values); + boost::shared_ptr factor3 = smartFactor3->linearize(values); + + Matrix CumulativeInformation = factor1->information() + factor2->information() + + factor3->information(); + + boost::shared_ptr GaussianGraph = graph.linearize( + values); + Matrix GraphInformation = GaussianGraph->hessian().first; + + // Check Hessian + EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6)); + + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + factor3->augmentedInformation(); + + // Check Information vector + Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector + + // Check Hessian + EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); +} + +/* ************************************************************************* +TEST( SmartProjectionPoseFactor, Hessian ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2}; + + // Project three landmarks into 2 cameras + Point2 cam1_uv1 = cam1.project(landmark1); + Point2 cam2_uv1 = cam2.project(landmark1); + Point2Vector measurements_cam1; + measurements_cam1.push_back(cam1_uv1); + measurements_cam1.push_back(cam2_uv1); + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); + smartFactor1->add(measurements_cam1, views); + + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), + Point3(0.5, 0.1, 0.3)); + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr factor = smartFactor1->linearize(values); + + // compute triangulation from linearization point + // compute reprojection errors (sum squared) + // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance) + // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +