From d4eaa15280275127a7fc053ed971aa5ed0383513 Mon Sep 17 00:00:00 2001 From: yotams Date: Thu, 22 Apr 2021 08:52:28 +0300 Subject: [PATCH 01/50] rolling shutter projection factor --- gtsam/base/Lie.h | 27 ++- gtsam/geometry/Pose3.cpp | 4 + gtsam/geometry/Pose3.h | 9 + .../slam/RollingShutterProjectionFactor.h | 214 ++++++++++++++++++ 4 files changed, 250 insertions(+), 4 deletions(-) create mode 100644 gtsam_unstable/slam/RollingShutterProjectionFactor.h diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c9340..e4a66fad1b 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 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 318baab3d1..f84d029654 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_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 From cd1d4b4df5b214ec49d2adc4e47056f576a20a03 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 14:38:26 -0400 Subject: [PATCH 02/50] added templates for factors --- ...martProjectionPoseFactorRollingShutter.cpp | 125 ++ .../SmartProjectionPoseFactorRollingShutter.h | 369 +++++ ...martProjectionPoseFactorRollingShutter.cpp | 1382 +++++++++++++++++ 3 files changed, 1876 insertions(+) create mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp create mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h create mode 100644 gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 0000000000..7ab28e6142 --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,125 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses and extrinsic calibration + * @author Luca Carlone + * @author Frank Dellaert + */ + +#include + +namespace gtsam { + +SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( + const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params) + : Base(sharedNoiseModel, params) {} // note: no extrinsic specified! + +void SmartStereoProjectionFactorPP::add( + const StereoPoint2& measured, + const Key& w_P_body_key, const Key& body_P_cam_key, + const boost::shared_ptr& K) { + // we index by cameras.. + Base::add(measured, w_P_body_key); + // but we also store the extrinsic calibration keys in the same order + world_P_body_keys_.push_back(w_P_body_key); + body_P_cam_keys_.push_back(body_P_cam_key); + + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end()) + keys_.push_back(body_P_cam_key); // add only unique keys + + K_all_.push_back(K); +} + +void SmartStereoProjectionFactorPP::add( + const std::vector& measurements, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, + const std::vector>& Ks) { + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); + assert(world_P_body_keys.size() == Ks.size()); + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements[i], world_P_body_keys[i]); + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) + keys_.push_back(body_P_cam_keys[i]); // add only unique keys + + world_P_body_keys_.push_back(world_P_body_keys[i]); + body_P_cam_keys_.push_back(body_P_cam_keys[i]); + + K_all_.push_back(Ks[i]); + } +} + +void SmartStereoProjectionFactorPP::add( + const std::vector& measurements, + const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, + const boost::shared_ptr& K) { + assert(world_P_body_keys.size() == measurements.size()); + assert(world_P_body_keys.size() == body_P_cam_keys.size()); + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements[i], world_P_body_keys[i]); + // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared + if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) + keys_.push_back(body_P_cam_keys[i]); // add only unique keys + + world_P_body_keys_.push_back(world_P_body_keys[i]); + body_P_cam_keys_.push_back(body_P_cam_keys[i]); + + K_all_.push_back(K); + } +} + +void SmartStereoProjectionFactorPP::print( + const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << "SmartStereoProjectionFactorPP: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + K_all_[i]->print("calibration = "); + std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; + } + Base::print("", keyFormatter); +} + +bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, + double tol) const { + const SmartStereoProjectionFactorPP* e = + dynamic_cast(&p); + + return e && Base::equals(p, tol) && + body_P_cam_keys_ == e->getExtrinsicPoseKeys(); +} + +double SmartStereoProjectionFactorPP::error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } +} + +SmartStereoProjectionFactorPP::Base::Cameras +SmartStereoProjectionFactorPP::cameras(const Values& values) const { + assert(world_P_body_keys_.size() == K_all_.size()); + assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + Base::Cameras cameras; + for (size_t i = 0; i < world_P_body_keys_.size(); i++) { + Pose3 w_P_body = values.at(world_P_body_keys_[i]); + Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); + } + return cameras; +} + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h new file mode 100644 index 0000000000..40d90d614c --- /dev/null +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -0,0 +1,369 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartStereoProjectionFactorPP.h + * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations + * @author Luca Carlone + * @author Frank Dellaert + */ + +#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 as well as the extrinsic camera calibration (pose of camera wrt body). + * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. + * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * @addtogroup SLAM + */ +class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { + protected: + /// shared pointer to calibration object (one for each camera) + std::vector> K_all_; + + /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view + KeyVector world_P_body_keys_; + + /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) + KeyVector body_P_cam_keys_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// shorthand for base class type + typedef SmartStereoProjectionFactor Base; + + /// shorthand for this class + typedef SmartStereoProjectionFactorPP This; + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) + 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 + */ + SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params = + SmartStereoProjectionParams()); + + /** Virtual destructor */ + ~SmartStereoProjectionFactorPP() override = default; + + /** + * add a new measurement, with a pose key, and an extrinsic pose key + * @param measured is the 3-dimensional location of the projection of a + * single landmark in the a single (stereo) view (the measurement) + * @param world_P_body_key is the key corresponding to the body poses observing the same landmark + * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration + * @param K is the (fixed) camera intrinsic calibration + */ + void add(const StereoPoint2& measured, const Key& world_P_body_key, + const Key& body_P_cam_key, + const boost::shared_ptr& K); + + /** + * Variant of the previous one in which we include a set of measurements + * @param measurements vector of the 3m dimensional location of the projection + * of a single landmark in the m (stereo) view (the measurements) + * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark + * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration + * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * @param Ks vector of intrinsic calibration objects + */ + void add(const std::vector& measurements, + const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const std::vector>& Ks); + + /** + * Variant of the previous one in which we include a set of measurements with + * the same noise and calibration + * @param measurements vector of the 3m dimensional location of the projection + * of a single landmark in the m (stereo) view (the measurements) + * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark + * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration + * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * @param K the (known) camera calibration (same for all measurements) + */ + void add(const std::vector& measurements, + const KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, + const boost::shared_ptr& K); + + /** + * 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; + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; + + /// equals + const KeyVector& getExtrinsicPoseKeys() const { + return body_P_cam_keys_; + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override; + + /** return the calibration object */ + inline std::vector> calibration() const { + return K_all_; + } + + /** + * Collect all cameras involved in this factor + * @param values Values structure which must contain camera poses + * corresponding + * to keys involved in this factor + * @return vector of Values + */ + Base::Cameras cameras(const Values& values) const override; + + /** + * 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 computeJacobiansAndCorrectForMissingMeasurements( + FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { + if (!result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + size_t numViews = measured_.size(); + E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) + b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view + Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; + + for (size_t i = 0; i < numViews; i++) { // for each camera/measurement + Pose3 w_P_body = values.at(world_P_body_keys_.at(i)); + Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); + StereoCamera camera( + w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), + K_all_[i]); + // get jacobians and error vector for current measurement + StereoPoint2 reprojectionError_i = StereoPoint2( + camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); + Eigen::Matrix J; // 3 x 12 + J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) + J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) + // if the right pixel is invalid, fix jacobians + if (std::isnan(measured_.at(i).uR())) + { + J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); + Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); + reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, + reprojectionError_i.v()); + } + // fit into the output structures + Fs.push_back(J); + size_t row = 3 * i; + b.segment(row) = -reprojectionError_i.vector(); + 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 { + + // 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 = keys_.size(); + size_t nrNonuniqueKeys = world_P_body_keys_.size() + + body_P_cam_keys_.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("SmartStereoProjectionHessianFactor: this->" + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + triangulateSafe(cameras(values)); + + if (!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 + > (keys_, Gs, gs, 0.0); + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix F, E; + Vector b; + computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); + + // Whiten using noise model + noiseModel_->WhitenSystem(E, b); + for (size_t i = 0; i < Fs.size(); i++) + Fs[i] = noiseModel_->Whiten(Fs[i]); + + // build augmented Hessian (with last row/column being the information vector) + Matrix3 P; + Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); + + // marginalize point: note - we reuse the standard SchurComplement function + SymmetricBlockMatrix augmentedHessian = + Cameras::SchurComplement<3, Dim>(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_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_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[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 + > (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 (params_.linearizationMode) { + case HESSIAN: + return createHessianFactor(values, lambda); + default: + throw std::runtime_error( + "SmartStereoProjectionFactorPP: 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< + SmartStereoProjectionFactorPP> { +}; + +} // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp new file mode 100644 index 0000000000..c7f220c102 --- /dev/null +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -0,0 +1,1382 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSmartProjectionPoseFactor.cpp + * @brief Unit tests for ProjectionFactor Class + * @author Chris Beall + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + * @date Sept 2013 + */ + +#include "smartFactorScenarios.h" +#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, jacobianQ ) { + + 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.setLinearizationMode(gtsam::JACOBIAN_Q); + + 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 / 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, 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, 3poses_2land_rotation_only_smart_projection_factor ) { + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + // Two different cameras, at the same position, but different rotations + 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, sharedK2); + Camera cam3(pose3, sharedK2); + + Point2Vector measurements_cam1, measurements_cam2; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); + + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK2, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK2, params)); + smartFactor2->add(measurements_cam2, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + 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, pose2 * noise_pose); + values.insert(x3, pose3 * noise_pose); + + // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); + EXPECT(assert_equal(pose3, result.at(x3))); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + + // this test considers a condition in which the cheirality constraint is triggered + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + // Two different cameras, at the same position, but different rotations + 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); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + 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); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + // 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, 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))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) + // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + EXPECT(assert_equal(Pose3(values.at(x3).rotation(), + Point3(0,0,1)), result.at(x3))); +#else + // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation + // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) + EXPECT(assert_equal(pose3, result.at(x3),1e-3)); +#endif +} + +/* *************************************************************************/ +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] +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotation ) { + // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; + + using namespace vanillaPose; + + KeyVector views {x1, x2, x3}; + + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); + smartFactorInstance->add(measurements_cam1, views); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + boost::shared_ptr factor = smartFactorInstance->linearize( + values); + + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(pose_right)); + rotValues.insert(x3, poseDrift.compose(pose_above)); + + boost::shared_ptr factorRot = smartFactorInstance->linearize( + rotValues); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); + + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(pose_right)); + tranValues.insert(x3, poseDrift2.compose(pose_above)); + + boost::shared_ptr factorRotTran = + smartFactorInstance->linearize(tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { + + using namespace vanillaPose2; + + KeyVector views {x1, x2, x3}; + + // All cameras have the same pose so will be degenerate ! + Camera cam2(level_pose, sharedK2); + Camera cam3(level_pose, sharedK2); + + Point2Vector measurements_cam1; + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); + + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); + smartFactor->add(measurements_cam1, views); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + values.insert(x3, cam3.pose()); + + boost::shared_ptr factor = smartFactor->linearize(values); + + Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); + + Values rotValues; + rotValues.insert(x1, poseDrift.compose(level_pose)); + rotValues.insert(x2, poseDrift.compose(level_pose)); + rotValues.insert(x3, poseDrift.compose(level_pose)); + + boost::shared_ptr factorRot = // + smartFactor->linearize(rotValues); + + // Hessian is invariant to rotations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); + + Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), + Point3(10, -4, 5)); + + Values tranValues; + tranValues.insert(x1, poseDrift2.compose(level_pose)); + tranValues.insert(x2, poseDrift2.compose(level_pose)); + tranValues.insert(x3, poseDrift2.compose(level_pose)); + + boost::shared_ptr factorRotTran = smartFactor->linearize( + tranValues); + + // Hessian is invariant to rotations and translations in the nondegenerate case + EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(model, sharedBundlerK, params); + factor.add(measurement1, x1); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3Bundler ) { + + using namespace bundlerPose; + + // three landmarks ~5 meters in front of camera + Point3 landmark3(3, 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); + + KeyVector views {x1, x2, x3}; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); + 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(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(cam3.pose(), result.at(x3), 1e-6)); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { + + using namespace bundlerPose; + + KeyVector views {x1, x2, x3}; + + // Two 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, sharedBundlerK); + Camera cam3(pose3, sharedBundlerK); + + // landmark3 at 3 meters now + Point3 landmark3(3, 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); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor1->add(measurements_cam1, views); + + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor2->add(measurements_cam2, views); + + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedBundlerK, params)); + smartFactor3->add(measurements_cam3, views); + + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, + 0.10); + Point3 positionPrior = Point3(0, 0, 1); + + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + graph.addPrior(x1, cam1.pose(), noisePrior); + graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); + graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); + + // 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))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + + 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_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); + +TEST(SmartProjectionPoseFactor, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor(model, sharedK, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +TEST(SmartProjectionPoseFactor, serialize2) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + Pose3 bts; + SmartFactor factor(model, sharedK, bts, params); + + // insert some measurments + KeyVector key_view; + Point2Vector meas_view; + key_view.push_back(Symbol('x', 1)); + meas_view.push_back(Point2(10, 10)); + factor.add(meas_view, key_view); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 00387b32cdad2bb9007e372a832a7435af13999d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 15:30:53 -0400 Subject: [PATCH 03/50] setting up .h and tests - compiles and tests pass. --- ...martProjectionPoseFactorRollingShutter.cpp | 187 +++---- .../SmartProjectionPoseFactorRollingShutter.h | 474 ++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 529 +----------------- 3 files changed, 356 insertions(+), 834 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp index 7ab28e6142..4e1cbdd469 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -10,116 +10,95 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses and extrinsic calibration + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect * @author Luca Carlone - * @author Frank Dellaert */ -#include +//#include namespace gtsam { -SmartStereoProjectionFactorPP::SmartStereoProjectionFactorPP( - const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params) - : Base(sharedNoiseModel, params) {} // note: no extrinsic specified! -void SmartStereoProjectionFactorPP::add( - const StereoPoint2& measured, - const Key& w_P_body_key, const Key& body_P_cam_key, - const boost::shared_ptr& K) { - // we index by cameras.. - Base::add(measured, w_P_body_key); - // but we also store the extrinsic calibration keys in the same order - world_P_body_keys_.push_back(w_P_body_key); - body_P_cam_keys_.push_back(body_P_cam_key); - - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_key) == keys_.end()) - keys_.push_back(body_P_cam_key); // add only unique keys - - K_all_.push_back(K); -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - assert(world_P_body_keys.size() == Ks.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(Ks[i]); - } -} - -void SmartStereoProjectionFactorPP::add( - const std::vector& measurements, - const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K) { - assert(world_P_body_keys.size() == measurements.size()); - assert(world_P_body_keys.size() == body_P_cam_keys.size()); - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements[i], world_P_body_keys[i]); - // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared - if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) - keys_.push_back(body_P_cam_keys[i]); // add only unique keys - - world_P_body_keys_.push_back(world_P_body_keys[i]); - body_P_cam_keys_.push_back(body_P_cam_keys[i]); - - K_all_.push_back(K); - } -} - -void SmartStereoProjectionFactorPP::print( - const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "SmartStereoProjectionFactorPP: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { - K_all_[i]->print("calibration = "); - std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; - } - Base::print("", keyFormatter); -} - -bool SmartStereoProjectionFactorPP::equals(const NonlinearFactor& p, - double tol) const { - const SmartStereoProjectionFactorPP* e = - dynamic_cast(&p); - - return e && Base::equals(p, tol) && - body_P_cam_keys_ == e->getExtrinsicPoseKeys(); -} - -double SmartStereoProjectionFactorPP::error(const Values& values) const { - if (this->active(values)) { - return this->totalReprojectionError(cameras(values)); - } else { // else of active flag - return 0.0; - } -} - -SmartStereoProjectionFactorPP::Base::Cameras -SmartStereoProjectionFactorPP::cameras(const Values& values) const { - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); - Base::Cameras cameras; - for (size_t i = 0; i < world_P_body_keys_.size(); i++) { - Pose3 w_P_body = values.at(world_P_body_keys_[i]); - Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); - Pose3 w_P_cam = w_P_body.compose(body_P_cam); - cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); - } - return cameras; -} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const std::vector>& Ks) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// assert(world_P_body_keys.size() == Ks.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(Ks[i]); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::add( +// const std::vector& measurements, +// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, +// const boost::shared_ptr& K) { +// assert(world_P_body_keys.size() == measurements.size()); +// assert(world_P_body_keys.size() == body_P_cam_keys.size()); +// for (size_t i = 0; i < measurements.size(); i++) { +// Base::add(measurements[i], world_P_body_keys[i]); +// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared +// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) +// keys_.push_back(body_P_cam_keys[i]); // add only unique keys +// +// world_P_body_keys_.push_back(world_P_body_keys[i]); +// body_P_cam_keys_.push_back(body_P_cam_keys[i]); +// +// K_all_.push_back(K); +// } +//} +// +//void SmartProjectionPoseFactorRollingShutter::print( +// const std::string& s, const KeyFormatter& keyFormatter) const { +// std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; +// for (size_t i = 0; i < K_all_.size(); i++) { +// K_all_[i]->print("calibration = "); +// std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; +// } +// Base::print("", keyFormatter); +//} +// +//bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p, +// double tol) const { +// const SmartProjectionPoseFactorRollingShutter* e = +// dynamic_cast(&p); +// +// return e && Base::equals(p, tol) && +// body_P_cam_keys_ == e->getExtrinsicPoseKeys(); +//} +// +//double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const { +// if (this->active(values)) { +// return this->totalReprojectionError(cameras(values)); +// } else { // else of active flag +// return 0.0; +// } +//} +// +//SmartProjectionPoseFactorRollingShutter::Base::Cameras +//SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const { +// assert(world_P_body_keys_.size() == K_all_.size()); +// assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); +// Base::Cameras cameras; +// for (size_t i = 0; i < world_P_body_keys_.size(); i++) { +// Pose3 w_P_body = values.at(world_P_body_keys_[i]); +// Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); +// Pose3 w_P_cam = w_P_body.compose(body_P_cam); +// cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); +// } +// return cameras; +//} } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 40d90d614c..fe5ccdf7f1 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -10,15 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartStereoProjectionFactorPP.h - * @brief Smart stereo factor on poses (P) and camera extrinsic pose (P) calibrations + * @file SmartProjectionPoseFactorRollingShutter.h + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone - * @author Frank Dellaert */ #pragma once -#include +#include namespace gtsam { /** @@ -33,37 +32,40 @@ namespace gtsam { */ /** - * This factor optimizes the pose of the body as well as the extrinsic camera calibration (pose of camera wrt body). - * Each camera may have its own extrinsic calibration or the same calibration can be shared by multiple cameras. - * This factor requires that values contain the involved poses and extrinsics (both are Pose3 variables). + * 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 */ -class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { +template +class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< + PinholePose > { + protected: - /// shared pointer to calibration object (one for each camera) - std::vector> K_all_; + /// shared pointer to calibration object (one for each observation) + std::vector > K_all_; - /// The keys corresponding to the pose of the body (with respect to an external world frame) for each view - KeyVector world_P_body_keys_; + // 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_; - /// The keys corresponding to the extrinsic pose calibration for each view (pose that transform from camera to body) - KeyVector body_P_cam_keys_; + // interpolation factor (one for each observation) to interpolate between pair of consecutive poses + std::vector gammas_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef PinholePose Camera; /// shorthand for base class type - typedef SmartStereoProjectionFactor Base; + typedef SmartProjectionFactor Base; /// shorthand for this class - typedef SmartStereoProjectionFactorPP This; + typedef SmartProjectionPoseFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose - static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) + static const int Dim = 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 @@ -72,51 +74,67 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * @param Isotropic measurement noise * @param params internal parameters of the smart factors */ - SmartStereoProjectionFactorPP(const SharedNoiseModel& sharedNoiseModel, - const SmartStereoProjectionParams& params = - SmartStereoProjectionParams()); + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) {} /** Virtual destructor */ - ~SmartStereoProjectionFactorPP() override = default; + ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with a pose key, and an extrinsic pose key - * @param measured is the 3-dimensional location of the projection of a - * single landmark in the a single (stereo) view (the measurement) - * @param world_P_body_key is the key corresponding to the body poses observing the same landmark - * @param body_P_cam_key is the key corresponding to the extrinsic camera-to-body pose calibration + * 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 StereoPoint2& measured, const Key& world_P_body_key, - const Key& body_P_cam_key, - const boost::shared_ptr& K); + 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){ + // 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); + } /** - * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * 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 KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const std::vector>& Ks); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const std::vector>& Ks); /** * Variant of the previous one in which we include a set of measurements with - * the same noise and calibration - * @param measurements vector of the 3m dimensional location of the projection - * of a single landmark in the m (stereo) view (the measurements) - * @param w_P_body_keys are the ordered keys corresponding to the body poses observing the same landmark - * @param body_P_cam_keys are the ordered keys corresponding to the extrinsic camera-to-body poses calibration - * (note: elements of this vector do not need to be unique: 2 camera views can share the same calibration) + * the same 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 KeyVector& w_P_body_keys, const KeyVector& body_P_cam_keys, - const boost::shared_ptr& K); +// void add(const std::vector& measurements, +// const std::vector>& world_P_body_key_pairs, +// const std::vector& gammas, +// const boost::shared_ptr& K); /** * print @@ -130,8 +148,8 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override; /// equals - const KeyVector& getExtrinsicPoseKeys() const { - return body_P_cam_keys_; + const std::vector getGammas() const { + return gammas_; } /** @@ -140,18 +158,26 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { double error(const Values& values) const override; /** return the calibration object */ - inline std::vector> calibration() const { + inline std::vector> calibration() const { return K_all_; } /** * Collect all cameras involved in this factor * @param values Values structure which must contain camera poses - * corresponding - * to keys involved in this factor - * @return vector of Values + * corresponding to keys involved in this factor + * @return Cameras */ - Base::Cameras cameras(const Values& values) const override; + typename Base::Cameras cameras(const Values& values) const override { + typename Base::Cameras cameras; + for (const Key& k : this->keys_) { +// const Pose3 world_P_sensor_k = +// Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ +// : values.at(k); +// cameras.emplace_back(world_P_sensor_k, K_); + } + return cameras; + } /** * Compute jacobian F, E and error vector at a given linearization point @@ -161,169 +187,169 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { * 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 computeJacobiansAndCorrectForMissingMeasurements( - FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { - if (!result_) { - throw("computeJacobiansWithTriangulatedPoint"); - } else { // valid result: compute jacobians - size_t numViews = measured_.size(); - E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) - b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view - Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; - - for (size_t i = 0; i < numViews; i++) { // for each camera/measurement - Pose3 w_P_body = values.at(world_P_body_keys_.at(i)); - Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); - StereoCamera camera( - w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), - K_all_[i]); - // get jacobians and error vector for current measurement - StereoPoint2 reprojectionError_i = StereoPoint2( - camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 - J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) - J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) - // if the right pixel is invalid, fix jacobians - if (std::isnan(measured_.at(i).uR())) - { - J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); - Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); - reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, - reprojectionError_i.v()); - } - // fit into the output structures - Fs.push_back(J); - size_t row = 3 * i; - b.segment(row) = -reprojectionError_i.vector(); - E.block<3, 3>(row, 0) = Ei; - } - } - } +// void computeJacobiansAndCorrectForMissingMeasurements( +// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { +// if (!result_) { +// throw("computeJacobiansWithTriangulatedPoint"); +// } else { // valid result: compute jacobians +// size_t numViews = measured_.size(); +// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) +// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view +// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; +// +// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement +// Pose3 w_P_body = values.at(world_P_body_key_pairs_.at(i)); +// Pose3 body_P_cam = values.at(body_P_cam_ this->keys_.at(i)); +// StereoCamera camera( +// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), +// K_all_[i]); +// // get jacobians and error vector for current measurement +// StereoPoint2 reprojectionError_i = StereoPoint2( +// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); +// Eigen::Matrix J; // 3 x 12 +// J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) +// J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) +// // if the right pixel is invalid, fix jacobians +// if (std::isnan(measured_.at(i).uR())) +// { +// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); +// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); +// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, +// reprojectionError_i.v()); +// } +// // fit into the output structures +// Fs.push_back(J); +// size_t row = 3 * i; +// b.segment(row) = -reprojectionError_i.vector(); +// 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 { - - // 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 = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.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("SmartStereoProjectionHessianFactor: this->" - "measured_.size() inconsistent with input"); - - // triangulate 3D point at given linearization point - triangulateSafe(cameras(values)); - - if (!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 - > (keys_, Gs, gs, 0.0); - } - - // compute Jacobian given triangulated 3D Point - FBlocks Fs; - Matrix F, E; - Vector b; - computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); - - // Whiten using noise model - noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) - Fs[i] = noiseModel_->Whiten(Fs[i]); - - // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(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_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_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[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 - > (keys_, augmentedHessianUniqueKeys); - } +// boost::shared_ptr > createHessianFactor( +// const Values& values, const double lambda = 0.0, bool diagonalDamping = +// false) const { +// +// // 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 = world_P_body_key_pairs_.size() +// + body_P_cam_ this->keys_.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("SmartStereoProjectionHessianFactor: this->" +// "measured_.size() inconsistent with input"); +// +// // triangulate 3D point at given linearization point +// triangulateSafe(cameras(values)); +// +// if (!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; +// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); +// +// // Whiten using noise model +// noiseModel_->WhitenSystem(E, b); +// for (size_t i = 0; i < Fs.size(); i++) +// Fs[i] = noiseModel_->Whiten(Fs[i]); +// +// // build augmented Hessian (with last row/column being the information vector) +// Matrix3 P; +// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// +// // marginalize point: note - we reuse the standard SchurComplement function +// SymmetricBlockMatrix augmentedHessian = +// Cameras::SchurComplement<3, Dim>(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) @@ -333,12 +359,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { 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 (params_.linearizationMode) { - case HESSIAN: - return createHessianFactor(values, lambda); + switch (this->params_.linearizationMode) { +// case HESSIAN: +// return createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartStereoProjectionFactorPP: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); } } @@ -361,9 +387,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // end of class declaration /// traits -template<> -struct traits : public Testable< - SmartStereoProjectionFactorPP> { +template +struct traits > : public Testable< + SmartProjectionPoseFactorRollingShutter > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c7f220c102..d4c268b3cc 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,19 +10,17 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactor.cpp - * @brief Unit tests for ProjectionFactor Class - * @author Chris Beall + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class * @author Luca Carlone - * @author Zsolt Kira - * @author Frank Dellaert - * @date Sept 2013 + * @date July 2021 */ -#include "smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include +#include #include #include #include @@ -52,13 +50,13 @@ 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; @@ -66,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { 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; @@ -82,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, params) { using namespace vanillaPose; SmartProjectionParams params; @@ -93,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); } -/* ************************************************************************* */ +/* ************************************************************************* * TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); @@ -105,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noiseless ) { using namespace vanillaPose; @@ -163,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, noisy ) { using namespace vanillaPose; @@ -197,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -/* *************************************************************************/ +/* ************************************************************************* TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; @@ -272,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; @@ -333,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Factors ) { using namespace vanillaPose; @@ -497,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -551,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, jacobianSVD ) { using namespace vanillaPose; @@ -607,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, landmarkDistance ) { using namespace vanillaPose; @@ -666,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { EXPECT(assert_equal(values.at(x3), result.at(x3))); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -732,58 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT(assert_equal(cam3.pose(), result.at(x3))); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, jacobianQ ) { - - 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.setLinearizationMode(gtsam::JACOBIAN_Q); - - 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 / 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, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -830,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -912,144 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6)); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - 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, sharedK2); - Camera cam3(pose3, sharedK2); - - Point2Vector measurements_cam1, measurements_cam2; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - - SmartProjectionParams params; - params.setRankTolerance(50); - params.setDegeneracyMode(gtsam::HANDLE_INFINITY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK2, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK2, params)); - smartFactor2->add(measurements_cam2, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - 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, pose2 * noise_pose); - values.insert(x3, pose3 * noise_pose); - - // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); - EXPECT(assert_equal(pose3, result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { - - // this test considers a condition in which the cheirality constraint is triggered - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - // Two different cameras, at the same position, but different rotations - 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); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - 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); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // 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, 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))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) - // rotation remains the same as the initial guess, but position is fixed by PoseTranslationPrior -#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - EXPECT(assert_equal(Pose3(values.at(x3).rotation(), - Point3(0,0,1)), result.at(x3))); -#else - // if the check is disabled, no cheirality exception if thrown and the pose converges to the right rotation - // with modest accuracy since the configuration is essentially degenerate without the translation due to noise (noise_pose) - EXPECT(assert_equal(pose3, result.at(x3),1e-3)); -#endif -} - -/* *************************************************************************/ +/* ************************************************************************* TEST( SmartProjectionPoseFactor, Hessian ) { using namespace vanillaPose2; @@ -1080,299 +890,6 @@ TEST( SmartProjectionPoseFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotation ) { - // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; - - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); - smartFactorInstance->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactorInstance->linearize( - values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(pose_right)); - rotValues.insert(x3, poseDrift.compose(pose_above)); - - boost::shared_ptr factorRot = smartFactorInstance->linearize( - rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(pose_right)); - tranValues.insert(x3, poseDrift2.compose(pose_above)); - - boost::shared_ptr factorRotTran = - smartFactorInstance->linearize(tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { - - using namespace vanillaPose2; - - KeyVector views {x1, x2, x3}; - - // All cameras have the same pose so will be degenerate ! - Camera cam2(level_pose, sharedK2); - Camera cam3(level_pose, sharedK2); - - Point2Vector measurements_cam1; - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); - smartFactor->add(measurements_cam1, views); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); - - boost::shared_ptr factor = smartFactor->linearize(values); - - Pose3 poseDrift = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); - - Values rotValues; - rotValues.insert(x1, poseDrift.compose(level_pose)); - rotValues.insert(x2, poseDrift.compose(level_pose)); - rotValues.insert(x3, poseDrift.compose(level_pose)); - - boost::shared_ptr factorRot = // - smartFactor->linearize(rotValues); - - // Hessian is invariant to rotations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRot->information(), 1e-7)); - - Pose3 poseDrift2 = Pose3(Rot3::Ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2), - Point3(10, -4, 5)); - - Values tranValues; - tranValues.insert(x1, poseDrift2.compose(level_pose)); - tranValues.insert(x2, poseDrift2.compose(level_pose)); - tranValues.insert(x3, poseDrift2.compose(level_pose)); - - boost::shared_ptr factorRotTran = smartFactor->linearize( - tranValues); - - // Hessian is invariant to rotations and translations in the nondegenerate case - EXPECT(assert_equal(factor->information(), factorRotTran->information(), 1e-7)); -} - -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { - using namespace bundlerPose; - SmartProjectionParams params; - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactor factor(model, sharedBundlerK, params); - factor.add(measurement1, x1); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3Bundler ) { - - using namespace bundlerPose; - - // three landmarks ~5 meters in front of camera - Point3 landmark3(3, 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); - - KeyVector views {x1, x2, x3}; - - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); - 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(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(cam3.pose(), result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { - - using namespace bundlerPose; - - KeyVector views {x1, x2, x3}; - - // Two 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, sharedBundlerK); - Camera cam3(pose3, sharedBundlerK); - - // landmark3 at 3 meters now - Point3 landmark3(3, 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); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor1->add(measurements_cam1, views); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor2->add(measurements_cam2, views); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedBundlerK, params)); - smartFactor3->add(measurements_cam3, views); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, - 0.10); - Point3 positionPrior = Point3(0, 0, 1); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.addPrior(x1, cam1.pose(), noisePrior); - graph.emplace_shared >(x2, positionPrior, noisePriorTranslation); - graph.emplace_shared >(x3, positionPrior, noisePriorTranslation); - - // 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))); - - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - - 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_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); - -TEST(SmartProjectionPoseFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor(model, sharedK, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -TEST(SmartProjectionPoseFactor, serialize2) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - Pose3 bts; - SmartFactor factor(model, sharedK, bts, params); - - // insert some measurments - KeyVector key_view; - Point2Vector meas_view; - key_view.push_back(Symbol('x', 1)); - meas_view.push_back(Point2(10, 10)); - factor.add(meas_view, key_view); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - /* ************************************************************************* */ int main() { TestResult tr; From 82844b541c1fa5d91982482f62739795db77c73e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 16:11:18 -0400 Subject: [PATCH 04/50] put in place initial functions --- ...martProjectionPoseFactorRollingShutter.cpp | 82 ---------- .../SmartProjectionPoseFactorRollingShutter.h | 146 ++++++++++++------ 2 files changed, 102 insertions(+), 126 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp index 4e1cbdd469..c0b22160a7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp @@ -19,86 +19,4 @@ namespace gtsam { - -// -//void SmartProjectionPoseFactorRollingShutter::add( -// const std::vector& measurements, -// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, -// const std::vector>& Ks) { -// assert(world_P_body_keys.size() == measurements.size()); -// assert(world_P_body_keys.size() == body_P_cam_keys.size()); -// assert(world_P_body_keys.size() == Ks.size()); -// for (size_t i = 0; i < measurements.size(); i++) { -// Base::add(measurements[i], world_P_body_keys[i]); -// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared -// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) -// keys_.push_back(body_P_cam_keys[i]); // add only unique keys -// -// world_P_body_keys_.push_back(world_P_body_keys[i]); -// body_P_cam_keys_.push_back(body_P_cam_keys[i]); -// -// K_all_.push_back(Ks[i]); -// } -//} -// -//void SmartProjectionPoseFactorRollingShutter::add( -// const std::vector& measurements, -// const KeyVector& world_P_body_keys, const KeyVector& body_P_cam_keys, -// const boost::shared_ptr& K) { -// assert(world_P_body_keys.size() == measurements.size()); -// assert(world_P_body_keys.size() == body_P_cam_keys.size()); -// for (size_t i = 0; i < measurements.size(); i++) { -// Base::add(measurements[i], world_P_body_keys[i]); -// // pose keys are assumed to be unique (1 observation per time stamp), but calibration can be shared -// if(std::find(keys_.begin(), keys_.end(), body_P_cam_keys[i]) == keys_.end()) -// keys_.push_back(body_P_cam_keys[i]); // add only unique keys -// -// world_P_body_keys_.push_back(world_P_body_keys[i]); -// body_P_cam_keys_.push_back(body_P_cam_keys[i]); -// -// K_all_.push_back(K); -// } -//} -// -//void SmartProjectionPoseFactorRollingShutter::print( -// const std::string& s, const KeyFormatter& keyFormatter) const { -// std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; -// for (size_t i = 0; i < K_all_.size(); i++) { -// K_all_[i]->print("calibration = "); -// std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; -// } -// Base::print("", keyFormatter); -//} -// -//bool SmartProjectionPoseFactorRollingShutter::equals(const NonlinearFactor& p, -// double tol) const { -// const SmartProjectionPoseFactorRollingShutter* e = -// dynamic_cast(&p); -// -// return e && Base::equals(p, tol) && -// body_P_cam_keys_ == e->getExtrinsicPoseKeys(); -//} -// -//double SmartProjectionPoseFactorRollingShutter::error(const Values& values) const { -// if (this->active(values)) { -// return this->totalReprojectionError(cameras(values)); -// } else { // else of active flag -// return 0.0; -// } -//} -// -//SmartProjectionPoseFactorRollingShutter::Base::Cameras -//SmartProjectionPoseFactorRollingShutter::cameras(const Values& values) const { -// assert(world_P_body_keys_.size() == K_all_.size()); -// assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); -// Base::Cameras cameras; -// for (size_t i = 0; i < world_P_body_keys_.size(); i++) { -// Pose3 w_P_body = values.at(world_P_body_keys_[i]); -// Pose3 body_P_cam = values.at(body_P_cam_keys_[i]); -// Pose3 w_P_cam = w_P_body.compose(body_P_cam); -// cameras.push_back(StereoCamera(w_P_cam, K_all_[i])); -// } -// return cameras; -//} - } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index fe5ccdf7f1..840094f79e 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -38,19 +38,22 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< +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_; + /// 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 + /// 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 @@ -77,7 +80,8 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) {} + : Base(sharedNoiseModel, params) { + } /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; @@ -91,24 +95,28 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @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){ + 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)); + 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 + 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); } /** @@ -118,23 +126,55 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @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); + 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 calibration + * 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); + 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 @@ -142,24 +182,38 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override; + 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; + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>(&p); - /// equals - const std::vector getGammas() const { - return gammas_; + 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; - - /** return the calibration object */ - inline std::vector> calibration() const { - return K_all_; + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } } /** @@ -169,12 +223,18 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< * @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 (const Key& k : this->keys_) { -// const Pose3 world_P_sensor_k = -// Base::body_P_sensor_ ? values.at(k) * *Base::body_P_sensor_ -// : values.at(k); -// cameras.emplace_back(world_P_sensor_k, K_); + 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; } @@ -225,7 +285,6 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< // } // } // } - /// 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 = @@ -350,7 +409,6 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< // 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 @@ -379,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); } @@ -388,8 +446,8 @@ class SmartProjectionPoseFactorRollingShutter: public SmartProjectionFactor< /// traits template -struct traits > : public Testable< - SmartProjectionPoseFactorRollingShutter > { +struct traits > : + public Testable > { }; } // namespace gtsam From 1e2a1d259145928efbe8648dbcff6af244891278 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 16:11:34 -0400 Subject: [PATCH 05/50] removed cpp --- ...martProjectionPoseFactorRollingShutter.cpp | 22 ------------------- 1 file changed, 22 deletions(-) delete mode 100644 gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp deleted file mode 100644 index c0b22160a7..0000000000 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - * @author Luca Carlone - */ - -//#include - -namespace gtsam { - -} // \ namespace gtsam From 16d624d4e1dd9f14e4e243cfb1d9abd1d0ca2432 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 17:01:27 -0400 Subject: [PATCH 06/50] now I need to move to testing and interpolation --- .../SmartProjectionPoseFactorRollingShutter.h | 187 +++++++++--------- 1 file changed, 97 insertions(+), 90 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 840094f79e..f1f0e2d719 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -67,9 +67,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 6; ///< Pose3 dimension + 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 Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -247,96 +248,101 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * 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 computeJacobiansAndCorrectForMissingMeasurements( -// FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { -// if (!result_) { -// throw("computeJacobiansWithTriangulatedPoint"); -// } else { // valid result: compute jacobians -// size_t numViews = measured_.size(); -// E = Matrix::Zero(3 * numViews, 3); // a StereoPoint2 for each view (point jacobian) -// b = Vector::Zero(3 * numViews); // a StereoPoint2 for each view -// Matrix dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i, dProject_dPoseCam_i, Ei; -// -// for (size_t i = 0; i < numViews; i++) { // for each camera/measurement -// Pose3 w_P_body = values.at(world_P_body_key_pairs_.at(i)); -// Pose3 body_P_cam = values.at(body_P_cam_ this->keys_.at(i)); -// StereoCamera camera( -// w_P_body.compose(body_P_cam, dPoseCam_dPoseBody_i, dPoseCam_dPoseExt_i), -// K_all_[i]); -// // get jacobians and error vector for current measurement -// StereoPoint2 reprojectionError_i = StereoPoint2( -// camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); -// Eigen::Matrix J; // 3 x 12 -// J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) -// J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) -// // if the right pixel is invalid, fix jacobians -// if (std::isnan(measured_.at(i).uR())) -// { -// J.block<1, 12>(1, 0) = Matrix::Zero(1, 12); -// Ei.block<1, 3>(1, 0) = Matrix::Zero(1, 3); -// reprojectionError_i = StereoPoint2(reprojectionError_i.uL(), 0.0, -// reprojectionError_i.v()); -// } -// // fit into the output structures -// Fs.push_back(J); -// size_t row = 3 * i; -// b.segment(row) = -reprojectionError_i.vector(); -// E.block<3, 3>(row, 0) = Ei; -// } -// } -// } + 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 { -// -// // 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 = world_P_body_key_pairs_.size() -// + body_P_cam_ this->keys_.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("SmartStereoProjectionHessianFactor: this->" -// "measured_.size() inconsistent with input"); -// -// // triangulate 3D point at given linearization point -// triangulateSafe(cameras(values)); -// -// if (!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; -// computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); -// -// // Whiten using noise model -// noiseModel_->WhitenSystem(E, b); -// for (size_t i = 0; i < Fs.size(); i++) -// Fs[i] = noiseModel_->Whiten(Fs[i]); -// + 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; -// Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); +// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // // // marginalize point: note - we reuse the standard SchurComplement function -// SymmetricBlockMatrix augmentedHessian = -// Cameras::SchurComplement<3, Dim>(Fs, E, P, b); -// +// 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); @@ -408,7 +414,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // } // 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 @@ -418,8 +425,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< 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); + case HESSIAN: + return createHessianFactor(values, lambda); default: throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); From a0ca3387fb1b1bee2d87f84f0bb7fdcb6c007b58 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 19 Jul 2021 22:30:32 -0400 Subject: [PATCH 07/50] added interpolation function from shteren1 --- gtsam/base/Lie.h | 27 ++++++++++++- gtsam/geometry/tests/testPose3.cpp | 62 ++++++++++++++++++++++++++++++ 2 files changed, 87 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index fe730c9340..06f63963ec 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 */ @@ -322,8 +323,30 @@ T expm(const Vector& x, int K=7) { * Linear interpolation between X and Y by coefficient t in [0, 1]. */ 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.0); + 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/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; From 4c997e547433a988c6a6c1fb5c06eb220f0806f3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 20:46:29 -0400 Subject: [PATCH 08/50] removed interp from Pose3, starting to take pass on projection factor and test --- ...ShutterProjectionFactor.h => ProjectionFactorRollingShutter.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam_unstable/slam/{RollingShutterProjectionFactor.h => ProjectionFactorRollingShutter.h} (100%) diff --git a/gtsam_unstable/slam/RollingShutterProjectionFactor.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h similarity index 100% rename from gtsam_unstable/slam/RollingShutterProjectionFactor.h rename to gtsam_unstable/slam/ProjectionFactorRollingShutter.h From a204f6d5088f1cea60a70d71e789012f0de63764 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 20:46:43 -0400 Subject: [PATCH 09/50] amended --- gtsam/geometry/Pose3.cpp | 4 - gtsam/geometry/Pose3.h | 9 - .../slam/ProjectionFactorRollingShutter.h | 22 +- .../testProjectionFactorRollingShutter.cpp | 233 ++++++++++++++++++ 4 files changed, 244 insertions(+), 24 deletions(-) create mode 100644 gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b2cdd0c87a..c183e32ed4 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -423,8 +423,4 @@ 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 00c6fa8af0..d76e1b41a1 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -353,15 +353,6 @@ 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_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 69b7e73768..499f1cec3c 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file RollingShutterProjectionFactor.h - * @brief Basic bearing factor from 2D measurement for rolling shutter cameras + * @file ProjectionFactorRollingShutter.h + * @brief Basic projection factor for rolling shutter cameras * @author Yotam Stern */ @@ -34,7 +34,7 @@ namespace gtsam { * @addtogroup SLAM */ - class RollingShutterProjectionFactor: public NoiseModelFactor3 { + class ProjectionFactorRollingShutter: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -53,13 +53,13 @@ namespace gtsam { typedef NoiseModelFactor3 Base; /// shorthand for this class - typedef RollingShutterProjectionFactor This; + typedef ProjectionFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - RollingShutterProjectionFactor() : + ProjectionFactorRollingShutter() : measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { } @@ -74,7 +74,7 @@ namespace gtsam { * @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, + 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), @@ -93,7 +93,7 @@ namespace gtsam { * @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, + 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) : @@ -101,7 +101,7 @@ namespace gtsam { throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~RollingShutterProjectionFactor() {} + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -114,7 +114,7 @@ namespace gtsam { * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RollingShutterProjectionFactor, z = "; + std::cout << s << "ProjectionFactorRollingShutter, z = "; traits::Print(measured_); std::cout << " rolling shutter interpolation param = " << interp_param_; if(this->body_P_sensor_) @@ -141,7 +141,7 @@ namespace gtsam { gtsam::Matrix Hprj; //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - pose = pose_a.interp(interp_param_, pose_b, H1, H2); + pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); try { if(body_P_sensor_) { if(H1 && H2) { @@ -211,4 +211,4 @@ namespace gtsam { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // rolling shutter projection factor -} //namespace gtsam \ No newline at end of file +} //namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp new file mode 100644 index 0000000000..b0fcb23b11 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,233 @@ +/* ---------------------------------------------------------------------------- + + * 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 testProjectionFactorRollingShutter.cpp + * @brief Unit tests for ProjectionFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; +using symbol_shorthand::T; + +//typedef ProjectionFactorPPP TestProjectionFactor; + +///// traits +//namespace gtsam { +//template<> +//struct traits : public Testable { +//}; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, nonStandard ) { +// ProjectionFactorPPP f; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Constructor) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ConstructorWithTransform) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Equals ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, EqualsWithTransform ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Error ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, Pose3(), point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ErrorWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, transform, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Jacobian ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, Pose3(), point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, JacobianWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, body_P_sensor, point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +// +// +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ + From 2812eeb1beed6d9ab7b6bdb33d342bc83dd3c2c3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 21:06:51 -0400 Subject: [PATCH 10/50] pass on projection factor, but looks great overall --- .../slam/ProjectionFactorRollingShutter.h | 366 ++++++++++-------- 1 file changed, 202 insertions(+), 164 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 499f1cec3c..b5bead6216 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 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) {} - - /** 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))); } - - /** - * 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); - } + /** + * 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) { + } - /// 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_))); - } + /** + * 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) { + } - /// 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 { + /** Virtual destructor */ + virtual ~ProjectionFactorRollingShutter() { + } - Pose3 pose; - gtsam::Matrix Hprj; + /// @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))); + } - //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_); + /** + * 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) + && (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 { + + 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 * (*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()); - } - - /** return the measurement */ - const Point2& measured() const { - return 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()); } + return Vector2::Constant(2.0 * K_->fx()); + } - /** return the calibration object */ - inline const boost::shared_ptr calibration() const { - return K_; - } + /** return the measurement */ + const Point2& measured() const { + return measured_; + } - /** returns the rolling shutter interp param*/ - inline double interp_param() const {return interp_param_; } + /** return the calibration object */ + inline const boost::shared_ptr calibration() const { + return K_; + } - /** return verbosity */ - inline bool verboseCheirality() const { return verboseCheirality_; } + /** returns the rolling shutter interp param*/ + inline double interp_param() const { + return interp_param_; + } - /** return flag for throwing cheirality exceptions */ - inline bool throwCheirality() const { return throwCheirality_; } + /** return verbosity */ + inline bool verboseCheirality() const { + return verboseCheirality_; + } - private: + /** return flag for throwing cheirality exceptions */ + inline bool throwCheirality() const { + return throwCheirality_; + } - /// 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 + 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 From 0d1c3f16efb44a07073107a8b9d11e74c1b8145b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 21:29:56 -0400 Subject: [PATCH 11/50] everything working out so far with the tests --- .../slam/ProjectionFactorRollingShutter.h | 5 +- .../testProjectionFactorRollingShutter.cpp | 141 +++++++++--------- 2 files changed, 77 insertions(+), 69 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index b5bead6216..1d06e01a73 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -248,5 +248,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 struct traits : public Testable {}; + }//namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index b0fcb23b11..97e9dd2d13 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file testProjectionFactorRollingShutter.cpp + * @file ProjectionFactorRollingShutterRollingShutter.cpp * @brief Unit tests for ProjectionFactorRollingShutter Class * @author Luca Carlone * @date July 2021 @@ -45,72 +45,77 @@ using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; -//typedef ProjectionFactorPPP TestProjectionFactor; +// Convenience to define common variables across many tests +static Key poseKey1(X(1)); +static Key poseKey2(X(2)); +static Key pointKey(L(1)); +static double interp_params = 0.5; +static Point2 measurement(323.0, 240.0); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Constructor) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Equals ) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, + model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, + model, poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, + model, poseKey1, poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, + model, poseKey1, poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal + } +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + CHECK(assert_equal(factor1, factor2)); + } + { // factors are NOT equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor); + Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K, body_P_sensor2); + CHECK(!assert_equal(factor1, factor2)); + } +} -///// traits -//namespace gtsam { -//template<> -//struct traits : public Testable { -//}; -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, nonStandard ) { -// ProjectionFactorPPP f; -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Constructor) { -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, ConstructorWithTransform) { -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// -// Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Equals ) { -// // Create two identical factors and make sure they're equal -// Point2 measurement(323.0, 240.0); -// -// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); -// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, EqualsWithTransform ) { -// // Create two identical factors and make sure they're equal -// Point2 measurement(323.0, 240.0); -// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// -// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); -// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); -// -// CHECK(assert_equal(factor1, factor2)); -//} -// ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Error ) { +//TEST( ProjectionFactorRollingShutter, Error ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point // Pose3 pose(Rot3(), Point3(0,0,-6)); @@ -127,14 +132,14 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, ErrorWithTransform ) { +//TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); // Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey,transformKey, pointKey, K); // // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); @@ -151,13 +156,13 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, Jacobian ) { +//TEST( ProjectionFactorRollingShutter, Jacobian ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point // Pose3 pose(Rot3(), Point3(0,0,-6)); @@ -178,7 +183,7 @@ using symbol_shorthand::T; // // Verify H2 with numerical derivative // Matrix H2Expected = numericalDerivative32( // std::function( -// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, // std::placeholders::_1, std::placeholders::_2, // std::placeholders::_3, boost::none, boost::none, // boost::none)), @@ -188,14 +193,14 @@ using symbol_shorthand::T; //} // ///* ************************************************************************* */ -//TEST( ProjectionFactorPPP, JacobianWithTransform ) { +//TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { // // Create the factor with a measurement that is 3 pixels off in x // Key poseKey(X(1)); // Key transformKey(T(1)); // Key pointKey(L(1)); // Point2 measurement(323.0, 240.0); // Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); // // // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) // Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); @@ -216,7 +221,7 @@ using symbol_shorthand::T; // // Verify H2 with numerical derivative // Matrix H2Expected = numericalDerivative32( // std::function( -// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, // std::placeholders::_1, std::placeholders::_2, // std::placeholders::_3, boost::none, boost::none, // boost::none)), From a480b2dcfc195731c91923d88f5768dcad89b7c7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 22:05:36 -0400 Subject: [PATCH 12/50] all tests are passing! --- .../testProjectionFactorRollingShutter.cpp | 312 +++++++++++------- 1 file changed, 185 insertions(+), 127 deletions(-) diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 97e9dd2d13..66ed1cf9c6 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -93,144 +93,202 @@ TEST( ProjectionFactorRollingShutter, Equals ) { TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); CHECK(assert_equal(factor1, factor2)); } { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, body_P_sensor); Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, Error ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, Pose3(), point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector2(-3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey,transformKey, pointKey, K); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the error -// Vector actualError(factor.evaluateError(pose, transform, point)); -// -// // The expected error is (-3.0, 0.0) pixels / UnitCovariance -// Vector expectedError = Vector2(-3.0, 0.0); -// -// // Verify we get the expected error -// CHECK(assert_equal(expectedError, actualError, 1e-9)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, Jacobian ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point -// Pose3 pose(Rot3(), Point3(0,0,-6)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual, H3Actual; -// factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); -// -// // The expected Jacobians -// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); -// Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); -// -// // Verify H2 with numerical derivative -// Matrix H2Expected = numericalDerivative32( -// std::function( -// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, -// std::placeholders::_1, std::placeholders::_2, -// std::placeholders::_3, boost::none, boost::none, -// boost::none)), -// pose, Pose3(), point); -// -// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); -//} -// -///* ************************************************************************* */ -//TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { -// // Create the factor with a measurement that is 3 pixels off in x -// Key poseKey(X(1)); -// Key transformKey(T(1)); -// Key pointKey(L(1)); -// Point2 measurement(323.0, 240.0); -// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); -// ProjectionFactorRollingShutter factor(measurement, model, poseKey, transformKey, pointKey, K); -// -// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) -// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); -// Point3 point(0.0, 0.0, 0.0); -// -// // Use the factor to calculate the Jacobians -// Matrix H1Actual, H2Actual, H3Actual; -// factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); -// -// // The expected Jacobians -// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); -// Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); -// -// // Verify the Jacobians are correct -// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); -// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); -// -// // Verify H2 with numerical derivative -// Matrix H2Expected = numericalDerivative32( -// std::function( -// std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, -// std::placeholders::_1, std::placeholders::_2, -// std::placeholders::_3, boost::none, boost::none, -// boost::none)), -// pose, body_P_sensor, point); -// -// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); -// -// -//} +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Error ) { + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose corresponds to the first camera + double t = 0.0; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0,0,-6)); + Pose3 pose2(Rot3(), Point3(0,0,-4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create the factor with a measurement that is 3 pixels off in x + // Camera pose is actually interpolated now + double t = 0.5; + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + + // Set the linearization point + Pose3 pose1(Rot3(), Point3(0,0,-8)); + Pose3 pose2(Rot3(), Point3(0,0,-4)); + Point3 point(0.0, 0.0, 0.0); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is (-3.0, 0.0) pixels / UnitCovariance + Vector expectedError = Vector2(-3.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } + { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + } +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera camera(poseInterp*body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + + // Use the factor to calculate the error + Vector actualError(factor.evaluateError(pose1, pose2, point)); + + // The expected error is zero + Vector expectedError = Vector2(0.0, 0.0); + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, Jacobian ) { + // Create measurement by projecting 3D landmark + double t = 0.3; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} + +/* ************************************************************************* */ +TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { + // Create measurement by projecting 3D landmark + double t = 0.6; + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 poseInterp = interpolate(pose1, pose2, t); + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); + PinholeCamera camera(poseInterp*body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } From 02d2d97a8ef404d67207d14b69697a62b627bd0e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 23:04:52 -0400 Subject: [PATCH 13/50] added nice test on cheirality exception - done with projectionFactorRollingShutter --- .../slam/ProjectionFactorRollingShutter.h | 6 +- .../testProjectionFactorRollingShutter.cpp | 79 +++++++++++++++++++ 2 files changed, 83 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 1d06e01a73..fc7c939a8f 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -18,7 +18,9 @@ #pragma once #include -#include +#include +#include +#include #include namespace gtsam { @@ -189,7 +191,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3(pose1, pose2, t); + PinholeCamera camera(poseInterp, *K); + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + Point2 measured = Point2(0.0,0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true + bool throwCheirality = true; + bool verboseCheirality = true; + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), + CheiralityException); + } + { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + + // Use the factor to calculate the error + Matrix H1Actual, H2Actual, H3Actual; + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + + // The expected error is zero + Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + + // Verify we get the expected error + CHECK(assert_equal(expectedError, actualError, 1e-9)); + CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + } +#else + { + // everything is well defined, hence this matches the test "Jacobian" above: + Point2 measured = camera.project(point); + + // create factor + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + + // Use the factor to calculate the Jacobians + Matrix H1Actual, H2Actual, H3Actual; + factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual); + + // Expected Jacobians via numerical derivatives + Matrix H1Expected = numericalDerivative31( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H2Expected = numericalDerivative32( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + Matrix H3Expected = numericalDerivative33( + std::function( + std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, boost::none, boost::none, boost::none)), + pose1, pose2, point); + + CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); + CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); + CHECK(assert_equal(H3Expected, H3Actual, 1e-5)); + } +#endif +} + + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From 30f304e7332222c8396e0deb6823978c07d7d1dd Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 13:58:47 -0400 Subject: [PATCH 14/50] started serious testing: all tests pass for now --- gtsam/base/Lie.h | 2 +- .../tests/testSmartProjectionPoseFactor.cpp | 2 +- .../SmartProjectionPoseFactorRollingShutter.h | 85 ++++++++++++------- ...martProjectionPoseFactorRollingShutter.cpp | 53 ++++++------ 4 files changed, 84 insertions(+), 58 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 250ee6bcfb..c93456a283 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,7 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta - * @author shteren1 + * @author Yotam Stern */ diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c7f220c102..997c338462 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f1f0e2d719..13ba4e85e6 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -167,12 +167,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< return K_all_; } + /// return (for each observation) the key of the pair of poses from which we interpolate + const std::vector> world_P_body_key_pairs() const { + return world_P_body_key_pairs_; + } + /// return the interpolation factors gammas const std::vector getGammas() const { return gammas_; } - /// return the interpolation factors gammas + /// return the extrinsic camera calibration body_P_sensors const std::vector body_P_sensors() const { return body_P_sensors_; } @@ -192,7 +197,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< 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 = "); + body_P_sensors_[i].print("extrinsic calibration:\n"); + K_all_[i]->print("intrinsic calibration = "); } Base::print("", keyFormatter); } @@ -202,8 +208,30 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< const SmartProjectionPoseFactorRollingShutter* e = dynamic_cast*>(&p); + double keyPairsEqual = true; + if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ + for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + const Key key1own = world_P_body_key_pairs_[k].first; + const Key key1e = e->world_P_body_key_pairs()[k].first; + const Key key2own = world_P_body_key_pairs_[k].second; + const Key key2e = e->world_P_body_key_pairs()[k].second; + if ( !(key1own == key1e) || !(key2own == key2e) ){ + keyPairsEqual = false; break; + } + } + }else{ keyPairsEqual = false; } + + double extrinsicCalibrationEqual = true; + if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ + for(size_t i=0; i< this->body_P_sensors_.size(); i++){ + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ + extrinsicCalibrationEqual = false; break; + } + } + }else{ extrinsicCalibrationEqual = false; } + return e && Base::equals(p, tol) && K_all_ == e->calibration() - && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors(); + && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -249,8 +277,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * 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 { + const Values& values) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); } else { // valid result: compute jacobians @@ -296,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// 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 { + false) const { // 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 @@ -313,29 +340,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< 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]); - +// // 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); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d4c268b3cc..fe662932c2 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactorRollingShutter.cpp - * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class * @author Luca Carlone * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" +//#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -27,6 +27,7 @@ #include #include +using namespace gtsam; using namespace boost::assign; using namespace std::placeholders; @@ -47,17 +48,15 @@ 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; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; -/* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor2) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -65,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor3) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor4) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -81,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, params) { +TEST( SmartProjectionPoseFactorRollingShutter, params) { using namespace vanillaPose; SmartProjectionParams params; double rt = params.getRetriangulationThreshold(); @@ -92,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Equals ) { +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); @@ -104,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noiseless ) { +TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { using namespace vanillaPose; @@ -162,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noisy ) { +TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { using namespace vanillaPose; @@ -196,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { } /* ************************************************************************* -TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -271,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -332,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Factors ) { +TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { using namespace vanillaPose; @@ -496,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -550,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, jacobianSVD ) { +TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { using namespace vanillaPose; @@ -606,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, landmarkDistance ) { +TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { using namespace vanillaPose; @@ -665,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { +TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -731,7 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -778,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, CheckHessian) { +TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -786,7 +785,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { // Two slightly different cameras Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + * 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); @@ -860,7 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Hessian ) { +TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { using namespace vanillaPose2; From 306393a18c1cbbcc9a53378f6552c2effc35d23c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 14:30:55 -0400 Subject: [PATCH 15/50] solidified add and equal --- .../SmartProjectionPoseFactorRollingShutter.h | 8 +- ...martProjectionPoseFactorRollingShutter.cpp | 1562 ++++++++--------- 2 files changed, 785 insertions(+), 785 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 13ba4e85e6..e5730dcd64 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -99,8 +99,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< 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); + // store measurements in base class (note: we manyally add keys below to make sure they are unique + this->measured_.push_back(measured); + // 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)); @@ -113,6 +114,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< == this->keys_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys + // store interpolation factors + gammas_.push_back(gamma); + // store fixed calibration K_all_.push_back(K); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index fe662932c2..87bcbee814 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,7 +16,7 @@ * @date July 2021 */ -//#include "gtsam/slam/tests/smartFactorScenarios.h" +#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -44,8 +44,17 @@ using symbol_shorthand::L; static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); +static Symbol x4('X', 4); +static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), + Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); +static Point2 measurement2(200.0, 220.0); +static Point2 measurement3(320.0, 10.0); +static double interp_factor = 0.5; +static double interp_factor1 = 0.3; +static double interp_factor2 = 0.4; +static double interp_factor3 = 0.5; LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; @@ -55,841 +64,828 @@ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { - using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); + SmartFactorRS factor1(model, params); } -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); - factor1->add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactor factor1(model, sharedK, params); - factor1.add(measurement1, x1); -} - -/* ************************************************************************* * -TEST( SmartProjectionPoseFactorRollingShutter, params) { +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, add) { 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); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); } -/* ************************************************************************* * +/* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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(SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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); + // create fake measurements + std::vector measurements; + measurements.push_back(measurement1); + measurements.push_back(measurement2); + measurements.push_back(measurement3); + + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x4)); + + std::vector> intrinsicCalibrations; + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + intrinsicCalibrations.push_back(sharedK); + + std::vector extrinsicCalibrations; + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + extrinsicCalibrations.push_back(body_P_sensor); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // create by adding a batch of measurements with a bunch of calibrations + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + + // create by adding a batch of measurements with a single calibrations + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); + factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(assert_equal(*factor1, *factor2)); + CHECK(assert_equal(*factor1, *factor3)); } - - { - 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); + { // create slightly different factors (different keys) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); } - - { - // 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); + { // create slightly different factors (different extrinsics) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); + } + { // create slightly different factors (different interp factors) and show equal returns false + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + + CHECK(!assert_equal(*factor1, *factor2)); + CHECK(!assert_equal(*factor1, *factor3)); } } /* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { - - using namespace vanillaPose; - - KeyVector views {x1, x2, x3}; + TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + using namespace vanillaPose; - // 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); + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); - 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)); -} + SmartFactor factor(model, sharedK); + factor.add(level_uv, x1); + factor.add(level_uv_right, x2); -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { + Values values; // it's a pose factor, hence these are poses + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); - using namespace vanillaPose; + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - double excludeLandmarksFutherThanDist = 2; + SmartFactor::Cameras cameras = factor.cameras(values); + double actualError2 = factor.totalReprojectionError(cameras); + EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); - KeyVector views {x1, x2, x3}; + // Calculate expected derivative for point (easiest to check) + std::function f = // + std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + // Calculate using computeEP + Matrix actualE; + factor.triangulateAndComputeE(actualE, values); - // 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); + // get point + boost::optional point = factor.point(); + CHECK(point); - 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))); -} + // calculate numerical derivative with triangulated point + Matrix expectedE = sigma * numericalDerivative11(f, *point); + EXPECT(assert_equal(expectedE, actualE, 1e-7)); -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { + // Calculate using reprojectionError + SmartFactor::Cameras::FBlocks F; + Matrix E; + Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); + EXPECT(assert_equal(expectedE, E, 1e-7)); - using namespace vanillaPose; + 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( SmartProjectionPoseFactorRollingShutter, 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(SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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( SmartProjectionPoseFactorRollingShutter, 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); - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + SmartFactor::shared_ptr smartFactor1( + new SmartFactor(model, sharedK, params)); + smartFactor1->add(measurements_cam1, views); - KeyVector views {x1, x2, x3}; + SmartFactor::shared_ptr smartFactor2( + new SmartFactor(model, sharedK, params)); + smartFactor2->add(measurements_cam2, views); - // add fourth landmark - Point3 landmark4(5, -0.5, 1); + SmartFactor::shared_ptr smartFactor3( + new SmartFactor(model, sharedK, params)); + smartFactor3->add(measurements_cam3, views); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; + SmartFactor::shared_ptr smartFactor4( + new SmartFactor(model, sharedK, params)); + smartFactor4->add(measurements_cam4, views); - // 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 + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - 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))); -} + 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( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { - using namespace vanillaPose2; + using namespace vanillaPose2; - KeyVector views {x1, x2, x3}; + KeyVector views {x1, x2, x3}; - typedef GenericProjectionFactor ProjectionFactor; - NonlinearFactorGraph graph; + 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); + // 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(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); + 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); + 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); + 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); + DOUBLES_EQUAL(48406055, graph.error(values), 1); - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - Values result = optimizer.optimize(); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + Values result = optimizer.optimize(); - DOUBLES_EQUAL(0, graph.error(result), 1e-9); + DOUBLES_EQUAL(0, graph.error(result), 1e-9); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); -} + EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); + } -/* ************************************************************************* -TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { - KeyVector views {x1, x2, x3}; + KeyVector views {x1, x2, x3}; - using namespace vanillaPose; + using namespace vanillaPose; - // Two slightly different cameras - Pose3 pose2 = level_pose + // 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( SmartProjectionPoseFactorRollingShutter, 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] -} - -/* ************************************************************************* */ + 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( SmartProjectionPoseFactorRollingShutter, 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); From 6f8d639ab8c85742563ccccc8637b66232914997 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 14:46:52 -0400 Subject: [PATCH 16/50] finding best way to test RS errors --- ...martProjectionPoseFactorRollingShutter.cpp | 154 ++++++++++++------ 1 file changed, 100 insertions(+), 54 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 87bcbee814..b5bbf9c8b5 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -56,6 +56,20 @@ static double interp_factor1 = 0.3; static double interp_factor2 = 0.4; static double interp_factor3 = 0.5; +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaPoseRS { +typedef PinholePose Camera; +typedef SmartProjectionPoseFactor SmartFactor; +static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); +Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Camera cam1(interp_pose1, sharedK); +Camera cam2(interp_pose2, sharedK); +Camera cam3(interp_pose3, sharedK); +} + LevenbergMarquardtParams lmParams; typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; @@ -154,63 +168,95 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 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); +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { + + using namespace vanillaPoseRS; + +// // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); +// // can be interpolated with interp_factor1 = 0.2: +// Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5)); +// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); +// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera) +// // can be interpolated with interp_factor1 = 0.4: +// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); +// // 2 poses such that pose_above (Third camera 1 meter above the first camera) +// // can be interpolated with interp_factor1 = 0.5: +// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); +// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); +// +// // Project two landmarks into two cameras +// Point2 level_uv = level_camera.project(landmark1); +// Point2 level_uv_right = level_camera_right.project(landmark1); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// SmartFactor factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); +// +// 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 factor(model, sharedK); - factor.add(level_uv, x1); - factor.add(level_uv_right, x2); + /* ************************************************************************* + TEST( SmartProjectionPoseFactorRollingShutter, Jacobians ) { - Values values; // it's a pose factor, hence these are poses - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); + using namespace vanillaPose; - 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); - } + // 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); + + // 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( SmartProjectionPoseFactorRollingShutter, noisy ) { From 5d55e153f019eb0f968df0a6fa5d9d2257f14eaa Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 15:10:10 -0400 Subject: [PATCH 17/50] yay! error test passes! --- .../SmartProjectionPoseFactorRollingShutter.h | 295 +++++++++--------- ...martProjectionPoseFactorRollingShutter.cpp | 49 ++- 2 files changed, 167 insertions(+), 177 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index e5730dcd64..d62d187121 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -39,7 +39,7 @@ namespace gtsam { */ template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< - PinholePose > { +PinholePose > { protected: /// shared pointer to calibration object (one for each observation) @@ -81,7 +81,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { + : Base(sharedNoiseModel, params) { } /** Virtual destructor */ @@ -108,10 +108,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // 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_.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_.end()) this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors @@ -192,7 +192,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { + 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; @@ -238,40 +238,6 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } - /** - * 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 @@ -290,7 +256,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< b = Vector::Zero(2 * numViews); // a Point2 for each view Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement @@ -308,7 +274,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< // get jacobians and error vector for current measurement Point2 reprojectionError_i = Point2( camera.project(*this->result_, dProject_dPoseCam, Ei) - - this->measured_.at(i)); + - this->measured_.at(i)); Eigen::Matrix J; // 2 x 12 J.block(0, 0) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) @@ -342,109 +308,146 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< 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); + "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); + } + + /** + * error calculates the error of the factor. + */ + double error(const Values& values) const override { + if (this->active(values)) { + return this->totalReprojectionError(this->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 { + size_t numViews = this->measured_.size(); + 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 < numViews; i++) { // for each 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]; + Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + Pose3 body_P_cam = body_P_sensors_[i]; + Pose3 w_P_cam = w_P_body.compose(body_P_cam); + std::cout << "id : " << i << std::endl; + w_P_cam.print("w_P_cam\n"); + cameras.emplace_back(w_P_cam, K_all_[i]); + } + return cameras; } /** @@ -466,7 +469,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// linearize boost::shared_ptr linearize(const Values& values) const - override { + override { return linearizeDamped(values); } @@ -485,7 +488,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// traits template struct traits > : - public Testable > { +public Testable > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index b5bbf9c8b5..850ea98fd4 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -60,7 +60,6 @@ static double interp_factor3 = 0.5; // default Cal3_S2 poses with rolling shutter effect namespace vanillaPoseRS { typedef PinholePose Camera; -typedef SmartProjectionPoseFactor SmartFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); @@ -170,38 +169,26 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { - + std::cout << "============================== " << std::endl; using namespace vanillaPoseRS; -// // 2 poses such that level_pose_1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); -// // can be interpolated with interp_factor1 = 0.2: -// Pose3 level_pose1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 5)); -// Pose3 level_pose2 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0)); -// // 2 poses such that pose_right (Second camera 1 meter to the right of first camera) -// // can be interpolated with interp_factor1 = 0.4: -// Pose3 pose_right1 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); -// Pose3 pose_right2 = level_pose * Pose3(Rot3(), Point3(1, 0, 0)); -// // 2 poses such that pose_above (Third camera 1 meter above the first camera) -// // can be interpolated with interp_factor1 = 0.5: -// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); -// Pose3 pose_above1 = level_pose * Pose3(Rot3(), Point3(0, -1, 0)); -// -// // Project two landmarks into two cameras -// Point2 level_uv = level_camera.project(landmark1); -// Point2 level_uv_right = level_camera_right.project(landmark1); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// SmartFactor factor(model); -// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); -// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); -// -// 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); + // Project two landmarks into two cameras + Point2 level_uv = cam1.project(landmark1); + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorId = Pose3::identity(); + + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); + + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); + + double actualError = factor.error(values); + double expectedError = 0.0; + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } /* ************************************************************************* From 4669213618802e07ef1eebb8e2c87a02bec53c5c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 16:19:44 -0400 Subject: [PATCH 18/50] jacobians are good to go! --- .../SmartProjectionPoseFactorRollingShutter.h | 12 +-- ...martProjectionPoseFactorRollingShutter.cpp | 96 ++++++++----------- 2 files changed, 46 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d62d187121..18bb0e7a30 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -264,28 +264,26 @@ PinholePose > { 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 w_P_body = interpolate(w_P_body1, 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]); + 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 + J.block(0, 0, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6) = dProject_dPoseCam * dPoseCam_dInterpPose + J.block(0, 6, ZDim, 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; + E.block(row, 0) = Ei; } } } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 850ea98fd4..1ff62d7c4f 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -45,6 +46,7 @@ static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); +static Symbol l0('L', 0); static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); @@ -167,9 +169,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } +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 + /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { - std::cout << "============================== " << std::endl; +TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -189,63 +196,42 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessError ) { double actualError = factor.error(values); double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, Jacobians ) { - - 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); - // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactor::whitenedError, factor, cameras, std::placeholders::_1); + // Check triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + CHECK(point.valid()); // check triangulated point is valid + CHECK(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark - // Calculate using computeEP + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; 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); - } + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); + CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); + CHECK(actualFs.size() == 2); + + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + CHECK(assert_equal( expectedb1, Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + CHECK(assert_equal( expectedb2, Vector(actualb.segment<2>(2)), 1e-5)); +} - /* ************************************************************************* +/* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { using namespace vanillaPose; From e6ff03f73e36daf988781357aded93894367a9d5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 16:31:45 -0400 Subject: [PATCH 19/50] jacobians and errors are well tested now --- ...martProjectionPoseFactorRollingShutter.cpp | 164 +++++++----------- 1 file changed, 61 insertions(+), 103 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1ff62d7c4f..43049ea61e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -220,7 +220,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - CHECK(assert_equal( expectedb1, Vector(actualb.segment<2>(0)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; @@ -228,117 +229,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - CHECK(assert_equal( expectedb2, Vector(actualb.segment<2>(2)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 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(SmartProjectionPoseFactorRollingShutter, 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); +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; - SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); - smartFactor1.add(measurements_cam1, views); + // Project two landmarks into two cameras + Point2 pixelError(0.5, 1.0); + Point2 level_uv = cam1.project(landmark1) + pixelError; + Point2 level_uv_right = cam2.project(landmark1); + Pose3 body_P_sensorNonId = body_P_sensor; - SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); - smartFactor2.add(measurements_cam2, views); + SmartFactorRS factor(model); + factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); - SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); - smartFactor3.add(measurements_cam3, views); + Values values; // it's a pose factor, hence these are poses + values.insert(x1, level_pose); + values.insert(x2, pose_right); + values.insert(x3, pose_above); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + CHECK(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; - // 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) + // Check Jacobians + // -- actual Jacobians + FBlocks actualFs; + Matrix actualE; + Vector actualb; + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); + CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); + CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); + CHECK(actualFs.size() == 2); - 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); + // -- expected Jacobians from ProjectionFactorsRollingShutter + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + Matrix expectedF11, expectedF12, expectedE1; + Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); + CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - LevenbergMarquardtParams lmParams; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(wTb3, result.at(x3))); - } + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + Matrix expectedF21, expectedF22, expectedE2; + Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); + CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError + CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + + // Check errors + double actualError = factor.error(values); // from smart factor + NonlinearFactorGraph nfg; + nfg.add(factor1); + nfg.add(factor2); + values.insert(l0, landmarkNoisy); + double expectedError = nfg.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { From d4b88ba59ad63bc51ce0147d8478dc5bdcabab30 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 22:46:42 -0400 Subject: [PATCH 20/50] got to the final monster. Now I need to implement createHessian --- .../SmartProjectionPoseFactorRollingShutter.h | 8 +- ...martProjectionPoseFactorRollingShutter.cpp | 104 +++++++++--------- 2 files changed, 56 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 18bb0e7a30..8bc923e208 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -98,7 +98,7 @@ PinholePose > { */ 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) { + const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class (note: we manyally add keys below to make sure they are unique this->measured_.push_back(measured); @@ -131,7 +131,7 @@ PinholePose > { * @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, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& gammas, const std::vector>& Ks, @@ -154,10 +154,10 @@ PinholePose > { * @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, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& gammas, - const boost::shared_ptr& K, const Pose3 body_P_sensor) { + 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() == gammas.size()); for (size_t i = 0; i < measurements.size(); i++) { diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 43049ea61e..d049214248 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -98,7 +98,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { using namespace vanillaPose; // create fake measurements - std::vector measurements; + Point2Vector measurements; measurements.push_back(measurement1); measurements.push_back(measurement2); measurements.push_back(measurement3); @@ -170,7 +170,6 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } 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 @@ -298,68 +297,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { + std::cout << "===================" << std::endl; - using namespace vanillaPose2; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + using namespace vanillaPoseRS; + 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); + // 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); + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); - SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); - smartFactor1->add(measurements_cam1, views); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor1(model); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); - smartFactor3->add(measurements_cam3, views); + SmartFactorRS smartFactor2(model); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + SmartFactorRS smartFactor3(model); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - 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); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - 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); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + 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)); // 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 groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - } + // 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + 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( SmartProjectionPoseFactorRollingShutter, Factors ) { using namespace vanillaPose; From a439cf0f0fe9858852d6a5c519db4da8eccb1d9a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 21 Jul 2021 23:33:42 -0400 Subject: [PATCH 21/50] stuck on compile issue --- .../SmartProjectionPoseFactorRollingShutter.h | 76 +++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 6 ++ 2 files changed, 50 insertions(+), 32 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 8bc923e208..a9f6d6bdf6 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -18,6 +18,9 @@ #pragma once #include +#include +#include +#include namespace gtsam { /** @@ -58,6 +61,8 @@ PinholePose > { EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef PinholePose Camera; + // typedef CameraSet Cameras; + /// shorthand for base class type typedef SmartProjectionFactor Base; @@ -289,13 +294,12 @@ PinholePose > { } /// linearize and return a Hessianfactor that is an approximation of error(p) - boost::shared_ptr > createHessianFactor( + boost::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = false) const { - // 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) + // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), + // hence the number of unique keys may be smaller than 2 * nrMeasurements size_t nrUniqueKeys = this->keys_.size(); size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); @@ -304,37 +308,38 @@ PinholePose > { std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); - if (this->measured_.size() != cameras(values).size()) + if (this->measured_.size() != this->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); - // + std::cout << "got this far.." << std::endl; + + // triangulate 3D point at given linearization point + this->triangulateSafe(this->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; + this->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; + 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); @@ -409,6 +414,13 @@ PinholePose > { // } // return boost::make_shared < RegularHessianFactor // > ( this->keys_, augmentedHessianUniqueKeys); + + // TO REMOVE: + 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); } /** diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d049214248..602ca155e3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -352,6 +352,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) values.insert(x2, pose_right); // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + 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); From d7e8912d6a7c37c3986661033d294a2d2dee8863 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 21:45:26 -0400 Subject: [PATCH 22/50] all pass! --- gtsam/geometry/CameraSet.h | 63 +++++- .../SmartProjectionPoseFactorRollingShutter.h | 191 +++++++++--------- .../slam/SmartStereoProjectionFactorPP.h | 2 +- 3 files changed, 155 insertions(+), 101 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 7d2f818fa0..fcbff020c6 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,14 +142,67 @@ class CameraSet : public std::vector > return ErrorVector(project2(point, Fs, E), measured); } + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + return mySchurComplement<2,3,12>(Fs, E, P, b); + } + + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix mySchurComplement( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + + // a single point is observed in m cameras + size_t m = Fs.size(); + + // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) + size_t M1 = ND * m + 1; + std::vector dims(m + 1); // this also includes the b term + std::fill(dims.begin(), dims.end() - 1, ND); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); + + // Blockwise Schur complement + for (size_t i = 0; i < m; i++) { // for each camera + + const Eigen::Matrix& Fi = Fs[i]; + const auto FiT = Fi.transpose(); + const Eigen::Matrix Ei_P = // + E.block(myZDim * i, 0, myZDim, N) * P; + + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(myZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) + + // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) + augmentedHessian.setDiagonalBlock(i, FiT + * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi)); + + // upper triangular part of the hessian + for (size_t j = i + 1; j < m; j++) { // for each camera + const Eigen::Matrix& Fj = Fs[j]; + + // (DxD) = (Dx2) * ( (2x2) * (2xD) ) + augmentedHessian.setOffDiagonalBlock(i, j, -FiT + * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj)); + } + } // end of for over cameras + + augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); + return augmentedHessian; + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version */ - template // N = 2 or 3, ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplementWithCustomBlocks( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -202,11 +255,11 @@ class CameraSet : public std::vector > template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement(Fs, E, P, b); + return SchurComplementWithCustomBlocks(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void ComputePointCovariance(Eigen::Matrix& P, const Matrix& E, double lambda, bool diagonalDamping = false) { @@ -258,7 +311,7 @@ class CameraSet : public std::vector > * Applies Schur complement (exploiting block structure) to get a smart factor on cameras, * and adds the contribution of the smart factor to a pre-allocated augmented Hessian. */ - template // N = 2 or 3 + template // N = 2 or 3 (point dimension) static void UpdateSchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, const KeyVector& allKeys, const KeyVector& keys, diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index a9f6d6bdf6..472b6348ae 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -17,10 +17,10 @@ #pragma once -#include -#include -#include #include +#include +#include +#include namespace gtsam { /** @@ -60,11 +60,10 @@ PinholePose > { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - typedef PinholePose Camera; - // typedef CameraSet Cameras; - /// shorthand for base class type - typedef SmartProjectionFactor Base; + typedef SmartProjectionFactor > Base; +// typedef PinholePose Camera; +// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -312,8 +311,6 @@ PinholePose > { throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - std::cout << "got this far.." << std::endl; - // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); @@ -336,91 +333,95 @@ PinholePose > { for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // build augmented Hessian (with last row/column being the information vector) - Matrix3 P; - 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); - - // TO REMOVE: - 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); + // and marginalize point: note - we reuse the standard SchurComplement function + // does not work, since assumes convensional 6-dim blocks + // SymmetricBlockMatrix augmentedHessian = + // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); + + SymmetricBlockMatrix augmentedHessian = + Base::Cameras::mySchurComplement(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); // all are poses .. + nonuniqueDims.back() = 1; // except b is a scalar + 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).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + + // 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); + +// // TO REMOVE: +// 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); } /** @@ -470,7 +471,7 @@ PinholePose > { // depending on flag set on construction we may linearize to different linear factors switch (this->params_.linearizationMode) { case HESSIAN: - return createHessianFactor(values, lambda); + return this->createHessianFactor(values, lambda); default: throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); @@ -480,7 +481,7 @@ PinholePose > { /// linearize boost::shared_ptr linearize(const Values& values) const override { - return linearizeDamped(values); + return this->linearizeDamped(values); } private: diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614c..e4d714b0f3 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // marginalize point: note - we reuse the standard SchurComplement function SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(Fs, E, P, b); + Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term From 477dd5b247d0246c6efc609f62580a1d1db8b412 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 21:49:40 -0400 Subject: [PATCH 23/50] all pass! --- gtsam/geometry/CameraSet.h | 58 ++----------------- .../SmartProjectionPoseFactorRollingShutter.h | 9 +-- .../slam/SmartStereoProjectionFactorPP.h | 2 +- 3 files changed, 8 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index fcbff020c6..2b656bd35a 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,57 +142,11 @@ class CameraSet : public std::vector > return ErrorVector(project2(point, Fs, E), measured); } - static SymmetricBlockMatrix mySchurComplement( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, + static SymmetricBlockMatrix SchurComplement312( + const std::vector< Eigen::Matrix, + Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return mySchurComplement<2,3,12>(Fs, E, P, b); - } - - template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix mySchurComplement( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - - // a single point is observed in m cameras - size_t m = Fs.size(); - - // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector) - size_t M1 = ND * m + 1; - std::vector dims(m + 1); // this also includes the b term - std::fill(dims.begin(), dims.end() - 1, ND); - dims.back() = 1; - SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); - - // Blockwise Schur complement - for (size_t i = 0; i < m; i++) { // for each camera - - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(myZDim * i, 0, myZDim, N) * P; - - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(myZDim * i) // F' * b - - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(myZDim * i, 0, myZDim, N).transpose() * Fi)); - - // upper triangular part of the hessian - for (size_t j = i + 1; j < m; j++) { // for each camera - const Eigen::Matrix& Fj = Fs[j]; - - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(myZDim * j, 0, myZDim, N).transpose() * Fj)); - } - } // end of for over cameras - - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; + return SchurComplement<3,12>(Fs, E, P, b); } /** @@ -202,7 +156,7 @@ class CameraSet : public std::vector > * Fixed size version */ template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix SchurComplementWithCustomBlocks( + static SymmetricBlockMatrix SchurComplement( const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { @@ -255,7 +209,7 @@ class CameraSet : public std::vector > template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement(const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplementWithCustomBlocks(Fs, E, P, b); + return SchurComplement(Fs, E, P, b); } /// Computes Point Covariance P, with lambda parameter diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 472b6348ae..3393f7ca4f 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -342,7 +342,7 @@ PinholePose > { // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); SymmetricBlockMatrix augmentedHessian = - Base::Cameras::mySchurComplement(Fs, E, P, b); + Base::Cameras::SchurComplement312(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term @@ -415,13 +415,6 @@ PinholePose > { } return boost::make_shared < RegularHessianFactor > ( this->keys_, augmentedHessianUniqueKeys); - -// // TO REMOVE: -// 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); } /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index e4d714b0f3..40d90d614c 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -250,7 +250,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // marginalize point: note - we reuse the standard SchurComplement function SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplementWithCustomBlocks<3, Dim>(Fs, E, P, b); + Cameras::SchurComplement<3, Dim>(Fs, E, P, b); // now pack into an Hessian factor std::vector dims(nrUniqueKeys + 1); // this also includes the b term From 91a6613d84e786d2cf76f63d1c52bb0025730f31 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 22:31:33 -0400 Subject: [PATCH 24/50] moved common function to cameraSet. commented issues with templated calls to functions in cameraSet --- gtsam/geometry/CameraSet.h | 99 ++++++++++++++++ .../SmartProjectionPoseFactorRollingShutter.h | 112 +++++------------- .../slam/SmartStereoProjectionFactorPP.h | 92 +++----------- 3 files changed, 140 insertions(+), 163 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 2b656bd35a..19ab7ab316 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -200,6 +200,105 @@ class CameraSet : public std::vector > return augmentedHessian; } + /** + * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix + * G = F' * F - F' * E * P * E' * F + * g = F' * (b - E * P * E' * b) + * In this version, we allow for the case where the keys in the Jacobian are organized + * differents from the keys in the output SymmetricBlockMatrix + * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + */ + template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + + size_t nrNonuniqueKeys = jacobianKeys.size(); + size_t nrUniqueKeys = hessianKeys.size(); + + // marginalize point: note - we reuse the standard SchurComplement function + SymmetricBlockMatrix augmentedHessian = SchurComplement(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, NDD); + dims.back() = 1; + SymmetricBlockMatrix augmentedHessianUniqueKeys; + + // here we have to deal with the fact that some blocks may share the same keys + 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, NDD); + nonuniqueDims.back() = 1; + augmentedHessian = SymmetricBlockMatrix( + nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); + + // 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[hessianKeys[k]] = k; + } + + // initialize matrix to zero + augmentedHessianUniqueKeys = SymmetricBlockMatrix( + dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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 = jacobianKeys.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 = jacobianKeys.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 augmentedHessianUniqueKeys; + } + + /** + * non-templated version of function above + */ + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( + const std::vector, + Eigen::aligned_allocator > >& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b, + const KeyVector jacobianKeys, const KeyVector hessianKeys) { + return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, + jacobianKeys, + hessianKeys); + } + /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3393f7ca4f..c35beb0e22 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -62,8 +62,6 @@ PinholePose > { /// shorthand for base class type typedef SmartProjectionFactor > Base; -// typedef PinholePose Camera; -// typedef CameraSet Cameras; /// shorthand for this class typedef SmartProjectionPoseFactorRollingShutter This; @@ -299,27 +297,27 @@ PinholePose > { // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); - size_t nrNonuniqueKeys = 2*world_P_body_key_pairs_.size(); + size_t nrUniqueKeys = this->keys_.size(); // Create structures for Hessian Factors KeyVector js; std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector gs(nrUniqueKeys); + std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); - if (!this->result_) { // failed: return "empty/zero" Hessian + 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); + return boost::make_shared < RegularHessianFactor + > (this->keys_, Gs, gs, 0.0); } // compute Jacobian given triangulated 3D Point @@ -333,88 +331,32 @@ PinholePose > { for (size_t i = 0; i < Fs.size(); i++) Fs[i] = this->noiseModel_->Whiten(Fs[i]); - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // Matrix3 P; + // Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // build augmented Hessian (with last row/column being the information vector) - // and marginalize point: note - we reuse the standard SchurComplement function - // does not work, since assumes convensional 6-dim blocks - // SymmetricBlockMatrix augmentedHessian = - // Base::Cameras::SchurComplement(Fs, E, b, lambda,diagonalDamping); - - SymmetricBlockMatrix augmentedHessian = - Base::Cameras::SchurComplement312(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); // all are poses .. - nonuniqueDims.back() = 1; // except b is a scalar - 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).first); - nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); - } + // 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).first); + nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); + } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( + Fs, E, P, b, nonuniqueKeys, this->keys_); - // 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; - } + // the following unfortunately does not seem to work and causes + // an "reference to overloaded function could not be resolved; did you mean to call it?" error + // SymmetricBlockMatrix augmentedHessianUniqueKeys = + // Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>( + // Fs, E, P, b, nonuniqueKeys, keys_); - // 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); + > (this->keys_, augmentedHessianUniqueKeys); } /** diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 40d90d614c..25be48b0f5 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -61,10 +61,10 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int Dim = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose + static const int DimBlock = 12; ///< Camera dimension: 6 for body pose, 6 for extrinsic pose static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 3; ///< Measurement dimension (for a StereoPoint2 measurement) - typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) + typedef Eigen::Matrix MatrixZD; // F blocks (derivatives wrt camera) typedef std::vector > FBlocks; // vector of F blocks /** @@ -180,7 +180,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // get jacobians and error vector for current measurement StereoPoint2 reprojectionError_i = StereoPoint2( camera.project(*result_, dProject_dPoseCam_i, Ei) - measured_.at(i)); - Eigen::Matrix J; // 3 x 12 + Eigen::Matrix J; // 3 x 12 J.block(0, 0) = dProject_dPoseCam_i * dPoseCam_dPoseBody_i; // (3x6) * (6x6) J.block(0, 6) = dProject_dPoseCam_i * dPoseCam_dPoseExt_i; // (3x6) * (6x6) // if the right pixel is invalid, fix jacobians @@ -209,8 +209,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // 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 = keys_.size(); - size_t nrNonuniqueKeys = world_P_body_keys_.size() - + body_P_cam_keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -246,81 +244,19 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { // build augmented Hessian (with last row/column being the information vector) Matrix3 P; - Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = - Cameras::SchurComplement<3, Dim>(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_keys_.size(); i++) { - nonuniqueKeys.push_back(world_P_body_keys_.at(i)); - nonuniqueKeys.push_back(body_P_cam_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[keys_[k]] = k; - } + Cameras::ComputePointCovariance <3> (P, E, lambda, diagonalDamping); - // 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)); + // 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_keys_.size(); i++) { + nonuniqueKeys.push_back(world_P_body_keys_.at(i)); + nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); } + // but we need to get the augumented hessian wrt the unique keys in key_ + SymmetricBlockMatrix augmentedHessianUniqueKeys = + Cameras::SchurComplementAndRearrangeBlocks<3,DimBlock,DimPose>(Fs,E,P,b, + nonuniqueKeys, keys_); + return boost::make_shared < RegularHessianFactor > (keys_, augmentedHessianUniqueKeys); } From 48a7afa46369ef81dd430022869d328b2976f729 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 22 Jul 2021 22:35:21 -0400 Subject: [PATCH 25/50] removed comments. Code is complete now. Need few more unit tests and we are good to go --- .../slam/SmartProjectionPoseFactorRollingShutter.h | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index c35beb0e22..c5b63250b8 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -332,10 +332,6 @@ PinholePose > { Fs[i] = this->noiseModel_->Whiten(Fs[i]); Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // the following unfortunately does not seem to work and causes - // an "reference to overloaded function could not be resolved; did you mean to call it?" error - // Matrix3 P; - // Base::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); // build augmented Hessian (with last row/column being the information vector) // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) @@ -349,12 +345,6 @@ PinholePose > { Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( Fs, E, P, b, nonuniqueKeys, this->keys_); - // the following unfortunately does not seem to work and causes - // an "reference to overloaded function could not be resolved; did you mean to call it?" error - // SymmetricBlockMatrix augmentedHessianUniqueKeys = - // Base::Cameras::SchurComplementAndRearrangeBlocks<3, DimBlock, DimPose>( - // Fs, E, P, b, nonuniqueKeys, keys_); - return boost::make_shared < RegularHessianFactor > (this->keys_, augmentedHessianUniqueKeys); } From 1c3ff0580be37ec609e8aa9230bc5b2e02ab963b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 00:03:41 -0400 Subject: [PATCH 26/50] removed printout, solved CI issue --- .../slam/SmartProjectionPoseFactorRollingShutter.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index c5b63250b8..b9b43bc18e 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -368,8 +368,10 @@ PinholePose > { */ typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); - assert(world_P_body_keys_.size() == K_all_.size()); - assert(world_P_body_keys_.size() == body_P_cam_keys_.size()); + assert(numViews == K_all_.size()); + assert(numViews == gammas_.size()); + assert(numViews == body_P_sensors_.size()); + assert(numViews == world_P_body_key_pairs_.size()); typename Base::Cameras cameras; for (size_t i = 0; i < numViews; i++) { // for each measurement @@ -379,8 +381,6 @@ PinholePose > { Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); - std::cout << "id : " << i << std::endl; - w_P_cam.print("w_P_cam\n"); cameras.emplace_back(w_P_cam, K_all_[i]); } return cameras; From 934413522de7e260938de735d7d8eaa904b1f85d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:39:13 -0400 Subject: [PATCH 27/50] fixed another test, few more to go --- .../SmartProjectionPoseFactorRollingShutter.h | 18 +- ...martProjectionPoseFactorRollingShutter.cpp | 251 +++++++----------- 2 files changed, 98 insertions(+), 171 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b9b43bc18e..6fb3f5ce1c 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -52,7 +52,7 @@ PinholePose > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector gammas_; + std::vector interp_param_; /// Pose of the camera in the body frame std::vector body_P_sensors_; ///< Pose of the camera in the body frame @@ -117,7 +117,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors - gammas_.push_back(gamma); + interp_param_.push_back(gamma); // store fixed calibration K_all_.push_back(K); @@ -180,7 +180,7 @@ PinholePose > { /// return the interpolation factors gammas const std::vector getGammas() const { - return gammas_; + return interp_param_; } /// return the extrinsic camera calibration body_P_sensors @@ -202,7 +202,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 << " gamma: " << gammas_[i] << std::endl; + std::cout << " gamma: " << interp_param_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -237,7 +237,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; + && interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,7 +264,7 @@ PinholePose > { 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]; + double interpolationFactor = interp_param_[i]; // get interpolated pose: Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); Pose3 body_P_cam = body_P_sensors_[i]; @@ -322,7 +322,7 @@ PinholePose > { // compute Jacobian given triangulated 3D Point FBlocks Fs; - Matrix F, E; + Matrix E; Vector b; this->computeJacobiansWithTriangulatedPoint(Fs, E, b, values); @@ -369,7 +369,7 @@ PinholePose > { typename Base::Cameras cameras(const Values& values) const override { size_t numViews = this->measured_.size(); assert(numViews == K_all_.size()); - assert(numViews == gammas_.size()); + assert(numViews == interp_param_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -377,7 +377,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each 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]; + double interpolationFactor = interp_param_[i]; Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 602ca155e3..a841b753be 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -298,8 +298,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { - std::cout << "===================" << std::endl; +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -365,173 +364,101 @@ TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -/* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 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}; +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { + // here we replicate a test in SmartProjectionPoseFactor by setting interpolation + // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) + // Note: this is a quite extreme test since in typical camera you would not have more than + // 1 measurement per landmark at each interpolated pose + using namespace vanillaPose; - SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); - smartFactor1->add(measurements_cam1, views); + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); - 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); - } + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); - { - 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); - } + // 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)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + 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(); - { - // 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); - } - } + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values); + 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); +} /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - + std::cout << "===================" << std::endl; using namespace vanillaPose; KeyVector views {x1, x2, x3}; From aeb1d35dd6407350d2d997c670befddf618de368 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:47:07 -0400 Subject: [PATCH 28/50] fixed test with lmk distance --- ...martProjectionPoseFactorRollingShutter.cpp | 152 ++++++------------ 1 file changed, 51 insertions(+), 101 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a841b753be..c2ebd49fba 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -456,121 +456,71 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { - std::cout << "===================" << std::endl; - 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( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 2; +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { + using namespace vanillaPoseRS; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - KeyVector views {x1, x2, x3}; + // 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); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - // 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); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + 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); + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); + 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); + NonlinearFactorGraph graph; + graph.push_back(smartFactor1); + graph.push_back(smartFactor2); + graph.push_back(smartFactor3); + 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)); // 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); + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + 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))); - } + // 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( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { + std::cout << "===================" << std::endl; using namespace vanillaPose; From a7b7770310acaa84a7ef047cebd7e183290560d1 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 15:50:16 -0400 Subject: [PATCH 29/50] test with EPI fixed --- ...martProjectionPoseFactorRollingShutter.cpp | 64 ++++++++++++++++++- 1 file changed, 63 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c2ebd49fba..e501b9d492 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -456,6 +456,68 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { + using namespace vanillaPoseRS; + 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 inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + double excludeLandmarksFutherThanDist = 1e10; //very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model,params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor2(model,params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + + SmartFactorRS smartFactor3(model,params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + + 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +} + /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { using namespace vanillaPoseRS; @@ -511,7 +573,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { // initialize third pose with some noise, we expect it to move back to original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); From 9c288d90cec950b95e0e882e2e7986170249b244 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 17:48:08 -0400 Subject: [PATCH 30/50] working on testing + cosmetic improvements to print for smart factors --- gtsam/slam/SmartFactorBase.h | 2 +- gtsam/slam/SmartProjectionFactor.h | 2 +- ...martProjectionPoseFactorRollingShutter.cpp | 128 ++++++++++-------- 3 files changed, 72 insertions(+), 60 deletions(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 0b0308c5c8..3802831411 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -178,7 +178,7 @@ class SmartFactorBase: public NonlinearFactor { DefaultKeyFormatter) const override { std::cout << s << "SmartFactorBase, z = \n"; for (size_t k = 0; k < measured_.size(); ++k) { - std::cout << "measurement, p = " << measured_[k] << "\t"; + std::cout << "measurement " << k<<", px = \n" << measured_[k] << "\n"; noiseModel_->print("noise model = "); } if(body_P_sensor_) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 475a9e8298..f67ca0740b 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -101,7 +101,7 @@ class SmartProjectionFactor: public SmartFactorBase { void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionFactor\n"; - std::cout << "linearizationMode:\n" << params_.linearizationMode + std::cout << "linearizationMode: " << params_.linearizationMode << std::endl; std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index e501b9d492..802ddbf439 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -319,14 +319,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS smartFactor1(model); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -457,7 +457,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -519,7 +519,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { using namespace vanillaPoseRS; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -581,70 +581,82 @@ TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { } /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { - std::cout << "===================" << std::endl; - - using namespace vanillaPose; +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { + std::cout << "===================" << std::endl; + using namespace vanillaPoseRS; + // add fourth landmark + Point3 landmark4(5, -0.5, 1); - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + // Project 4 landmarks into 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 - KeyVector views {x1, x2, x3}; + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - // add fourth landmark - Point3 landmark4(5, -0.5, 1); + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, - measurements_cam4; + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - // 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.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); - SmartProjectionParams params; - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + SmartFactorRS smartFactor1(model, params); + smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(model, sharedK, params)); - smartFactor1->add(measurements_cam1, views); + SmartFactorRS smartFactor2(model, params); + smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(model, sharedK, params)); - smartFactor2->add(measurements_cam2, views); + SmartFactorRS smartFactor3(model, params); + smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(model, sharedK, params)); - smartFactor3->add(measurements_cam3, views); + SmartFactorRS smartFactor4(model, params); + smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK); - SmartFactor::shared_ptr smartFactor4( - new SmartFactor(model, sharedK, params)); - smartFactor4->add(measurements_cam4, views); + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); - 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); + Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.1, 0.1, 0.1)); // smaller noise + Values values; + values.insert(x1, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - values.insert(x3, cam3.pose()); + // Optimization should correct 3rd pose + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - // 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))); + smartFactor1.print("smartFactor1"); + smartFactor2.print("smartFactor2"); + smartFactor3.print("smartFactor3"); + smartFactor4.print("smartFactor4"); } /* ************************************************************************* From 81526e8917b5cd256b0021c473d16de7d70c0741 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 17:57:24 -0400 Subject: [PATCH 31/50] fixed another test. --- .../SmartProjectionPoseFactorRollingShutter.h | 1 - ...martProjectionPoseFactorRollingShutter.cpp | 31 ++++++++----------- 2 files changed, 13 insertions(+), 19 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 6fb3f5ce1c..03c44dbe90 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -319,7 +319,6 @@ PinholePose > { return boost::make_shared < RegularHessianFactor > (this->keys_, Gs, gs, 0.0); } - // compute Jacobian given triangulated 3D Point FBlocks Fs; Matrix E; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 802ddbf439..8b2bc26ddc 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -580,9 +580,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista EXPECT(assert_equal(values.at(x3), result.at(x3))); } - /* ************************************************************************* + /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { - std::cout << "===================" << std::endl; using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -607,7 +606,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -617,17 +616,17 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model,params)); + smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model,params)); + smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model,params)); + smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor4(model, params); - smartFactor4.add(measurements_cam4, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model,params)); + smartFactor4->add(measurements_cam4, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -639,8 +638,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3::identity(); // Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), -// Point3(0.1, 0.1, 0.1)); // smaller noise + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); @@ -652,15 +651,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - - smartFactor1.print("smartFactor1"); - smartFactor2.print("smartFactor2"); - smartFactor3.print("smartFactor3"); - smartFactor4.print("smartFactor4"); } /* ************************************************************************* TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { + std::cout << "===================" << std::endl; using namespace vanillaPose2; From 5350e3463ef8168653f2f16f241443eae851ce1d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 18:58:43 -0400 Subject: [PATCH 32/50] done with tests, now I only have to rename gamma to keep consistency with the projection factors RS --- ...martProjectionPoseFactorRollingShutter.cpp | 353 ++++++++---------- 1 file changed, 154 insertions(+), 199 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 8b2bc26ddc..1cfe44d848 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp - * @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class + * @file testSmartProjectionPoseFactorRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class * @author Luca Carlone * @date July 2021 */ @@ -301,12 +301,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -320,13 +320,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -384,18 +384,18 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); - Point2Vector measurements_cam1; + Point2Vector measurements_lmk1; // Project 2 landmarks into 2 cameras - measurements_cam1.push_back(cam1.project(landmark1)); - measurements_cam1.push_back(cam2.project(landmark1)); + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactor1->add(measurements_cam1[0], x1, x2, interp_factor, sharedKSimple, + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactor1->add(measurements_cam1[1], x1, x2, interp_factor, sharedKSimple, + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, body_P_sensorId); SmartFactor::Cameras cameras; @@ -459,12 +459,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -486,13 +486,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setEnableEPI(true); SmartFactorRS smartFactor1(model,params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor2(model,params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor3(model,params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -521,12 +521,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { /* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { using namespace vanillaPoseRS; - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // 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); + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs std::vector> key_pairs; @@ -548,13 +548,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setEnableEPI(false); SmartFactorRS smartFactor1(model,params); - smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor2(model,params); - smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); SmartFactorRS smartFactor3(model,params); - smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -580,19 +580,20 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista EXPECT(assert_equal(values.at(x3), result.at(x3))); } - /* *************************************************************************/ +/* *************************************************************************/ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); - Point2Vector measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; // Project 4 landmarks into 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 + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); + measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -616,17 +617,17 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model,params)); - smartFactor1->add(measurements_cam1, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model,params)); - smartFactor2->add(measurements_cam2, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model,params)); - smartFactor3->add(measurements_cam3, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model,params)); - smartFactor4->add(measurements_cam4, key_pairs, interp_factors, sharedK); + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -651,170 +652,124 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); - } - - /* ************************************************************************* - TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { - std::cout << "===================" << std::endl; - - 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( SmartProjectionPoseFactorRollingShutter, 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); +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - // 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))); + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; - 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)); + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - 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( SmartProjectionPoseFactorRollingShutter, Hessian ) { + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); - using namespace vanillaPose2; + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); - KeyVector views {x1, x2}; + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - // 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); + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + 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))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + CHECK(point.valid()); // check triangulated point is valid + + // Use the factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 3, 6 * 3); + Matrix E = Matrix::Zero(2 * 3, 3); + Vector b = Vector::Zero(6); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + // whiten + F = (1/sigma) * F; + E = (1/sigma) * E; + b = (1/sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} - 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); From 1f07142b5bfdbb22e12f1ff559d666aae9d6e897 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 19:03:23 -0400 Subject: [PATCH 33/50] renamed params. need one last test --- .../SmartProjectionPoseFactorRollingShutter.h | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 03c44dbe90..4d0d6d9e64 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -52,7 +52,7 @@ PinholePose > { std::vector> world_P_body_key_pairs_; /// interpolation factor (one for each observation) to interpolate between pair of consecutive poses - std::vector interp_param_; + std::vector interp_params_; /// Pose of the camera in the body frame std::vector body_P_sensors_; ///< Pose of the camera in the body frame @@ -95,11 +95,11 @@ PinholePose > { * 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 interp_param in [0,1] is the interpolation factor, such that if interp_param = 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 Key& world_P_body_key2, const double& interp_param, const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurements in base class (note: we manyally add keys below to make sure they are unique this->measured_.push_back(measured); @@ -117,7 +117,7 @@ PinholePose > { this->keys_.push_back(world_P_body_key2); // add only unique keys // store interpolation factors - interp_param_.push_back(gamma); + interp_params_.push_back(interp_param); // store fixed calibration K_all_.push_back(K); @@ -135,15 +135,15 @@ PinholePose > { */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& gammas, + const std::vector& interp_params, 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() == interp_params.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], + world_P_body_key_pairs[i].second, interp_params[i], Ks[i], body_P_sensors[i]); } } @@ -158,13 +158,13 @@ PinholePose > { */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, - const std::vector& gammas, + const std::vector& interp_params, 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() == gammas.size()); + assert(world_P_body_key_pairs.size() == interp_params.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); + world_P_body_key_pairs[i].second, interp_params[i], K, body_P_sensor); } } @@ -178,9 +178,9 @@ PinholePose > { return world_P_body_key_pairs_; } - /// return the interpolation factors gammas - const std::vector getGammas() const { - return interp_param_; + /// return the interpolation factors interp_params + const std::vector interp_params() const { + return interp_params_; } /// return the extrinsic camera calibration body_P_sensors @@ -202,7 +202,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 << " gamma: " << interp_param_[i] << std::endl; + std::cout << " interp_param: " << interp_params_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -237,7 +237,7 @@ PinholePose > { }else{ extrinsicCalibrationEqual = false; } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && interp_param_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; + && interp_params_ == e->interp_params() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,7 +264,7 @@ PinholePose > { 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 = interp_param_[i]; + double interpolationFactor = interp_params_[i]; // get interpolated pose: Pose3 w_P_body = interpolate(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); Pose3 body_P_cam = body_P_sensors_[i]; @@ -368,7 +368,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_param_.size()); + assert(numViews == interp_params_.size()); assert(numViews == body_P_sensors_.size()); assert(numViews == world_P_body_key_pairs_.size()); @@ -376,7 +376,7 @@ PinholePose > { for (size_t i = 0; i < numViews; i++) { // for each 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 = interp_param_[i]; + double interpolationFactor = interp_params_[i]; Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); Pose3 body_P_cam = body_P_sensors_[i]; Pose3 w_P_cam = w_P_body.compose(body_P_cam); From a10d495611e6bb875971b88185175f2aa3fcd4fe Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:23:01 -0400 Subject: [PATCH 34/50] extra cleanup --- .../SmartProjectionPoseFactorRollingShutter.h | 117 ++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 4 +- 2 files changed, 65 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4d0d6d9e64..5c7f1a54e8 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -17,9 +17,6 @@ #pragma once -#include -#include -#include #include namespace gtsam { @@ -35,8 +32,8 @@ namespace gtsam { */ /** - * 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 + * This factor optimizes two consecutive poses of the body assuming a rolling shutter model of the camera with given readout time. + * The factor requires that values contain (for each pixel observation) two consecutive camera poses * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ @@ -55,7 +52,7 @@ PinholePose > { std::vector interp_params_; /// Pose of the camera in the body frame - std::vector body_P_sensors_; ///< Pose of the camera in the body frame + std::vector body_P_sensors_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -72,7 +69,7 @@ PinholePose > { 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 Eigen::Matrix MatrixZD; // F blocks (derivatives wrt block of 2 poses) typedef std::vector > FBlocks; // vector of F blocks /** @@ -90,48 +87,50 @@ PinholePose > { ~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 interp_param in [0,1] is the interpolation factor, such that if interp_param = 0 the interpolated pose is the same as world_P_body_key - * @param K is the (fixed) camera intrinsic calibration + * add a new measurement, with 2 pose keys, interpolation factor, camera (intrinsic and extrinsic) calibration, and observed pixel. + * @param measured 2-dimensional location of the projection of a + * 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 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 boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { - // store measurements in base class (note: we manyally add keys below to make sure they are unique + // store measurements in base class this->measured_.push_back(measured); - // but we also store the extrinsic calibration keys in the same order + // store the pair of keys for each measurement, 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()) + // also store keys in the keys_ vector: these 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()) + 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 interpolation factors + // store interpolation factor interp_params_.push_back(interp_param); - // store fixed calibration + // store fixed intrinsic calibration K_all_.push_back(K); - // store extrinsics of the camera + // store fixed 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 + * Variant of the previous "add" function in which we include multiple 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 + * @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 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, @@ -149,12 +148,15 @@ PinholePose > { } /** - * Variant of the previous one in which we include a set of measurements with - * the same (intrinsic and extrinsic) calibration + * Variant of the previous "add" function in which we include multiple 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) + * @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 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, @@ -173,7 +175,7 @@ PinholePose > { return K_all_; } - /// return (for each observation) the key of the pair of poses from which we interpolate + /// return (for each observation) the keys of the pair of poses from which we interpolate const std::vector> world_P_body_key_pairs() const { return world_P_body_key_pairs_; } @@ -245,7 +247,7 @@ PinholePose > { * @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, + * respect to both body poses we interpolate from), 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, @@ -256,19 +258,20 @@ PinholePose > { 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 + // intermediate Jacobians 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); + 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]; // get interpolated pose: - Pose3 w_P_body = interpolate(w_P_body1, 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); + 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]; + const 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 @@ -297,14 +300,14 @@ PinholePose > { // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation), // hence the number of unique keys may be smaller than 2 * nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); + size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys // Create structures for Hessian Factors KeyVector js; std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector < Vector > gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) + if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); @@ -312,12 +315,17 @@ PinholePose > { this->triangulateSafe(this->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); + if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { + 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); + } else { + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + } } // compute Jacobian given triangulated 3D Point FBlocks Fs; @@ -332,14 +340,15 @@ PinholePose > { Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // build augmented Hessian (with last row/column being the information vector) - // these are the keys that correspond to the blocks in augmentedHessian (output of SchurComplement) + // Collect all the key pairs: these are the keys that correspond to the blocks in Fs (on which we apply the Schur Complement) 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).first); nonuniqueKeys.push_back(world_P_body_key_pairs_.at(i).second); } - // but we need to get the augumented hessian wrt the unique keys in key_ + + // Build augmented Hessian (with last row/column being the information vector) + // Note: we need to get the augumented hessian wrt the unique keys in key_ SymmetricBlockMatrix augmentedHessianUniqueKeys = Base::Cameras::SchurComplementAndRearrangeBlocks_3_12_6( Fs, E, P, b, nonuniqueKeys, this->keys_); @@ -374,12 +383,12 @@ PinholePose > { typename Base::Cameras cameras; for (size_t i = 0; i < numViews; i++) { // for each 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); + 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]; - Pose3 w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - Pose3 body_P_cam = body_P_sensors_[i]; - Pose3 w_P_cam = w_P_body.compose(body_P_cam); + 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); cameras.emplace_back(w_P_cam, K_all_[i]); } return cameras; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1cfe44d848..a89a490e50 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -481,7 +481,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); @@ -612,7 +612,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); From e838d011a6223fd0f693ea2dba28ac9f53053e1d Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:48:16 -0400 Subject: [PATCH 35/50] added timing test --- ...martProjectionPoseFactorRollingShutter.cpp | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a89a490e50..f75e2fea22 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -27,6 +27,7 @@ #include #include #include +#define DISABLE_TIMING using namespace gtsam; using namespace boost::assign; @@ -769,6 +770,69 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } +#ifndef DISABLE_TIMING +#include +// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 1000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, + body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SF_RS_LINEARIZE); + smartFactorRS->linearize(values); + gttoc_(SF_RS_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(RS_LINEARIZE); + smartFactor->linearize(values); + gttoc_(RS_LINEARIZE); + } + tictoc_print_(); +} +#endif + /* ************************************************************************* */ int main() { TestResult tr; From e4f1bb1bd0df65f8ed333cdb4c331a39aacae211 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 23 Jul 2021 22:56:37 -0400 Subject: [PATCH 36/50] CHECK -> EXPECT --- ...martProjectionPoseFactorRollingShutter.cpp | 76 +++++++++---------- 1 file changed, 38 insertions(+), 38 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index f75e2fea22..75aaf4ac12 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -138,8 +138,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(assert_equal(*factor1, *factor2)); - CHECK(assert_equal(*factor1, *factor3)); + EXPECT(assert_equal(*factor1, *factor2)); + EXPECT(assert_equal(*factor1, *factor3)); } { // create slightly different factors (different keys) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -147,8 +147,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } { // create slightly different factors (different extrinsics) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -156,8 +156,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } { // create slightly different factors (different interp factors) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -165,8 +165,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - CHECK(!assert_equal(*factor1, *factor2)); - CHECK(!assert_equal(*factor1, *factor3)); + EXPECT(!assert_equal(*factor1, *factor2)); + EXPECT(!assert_equal(*factor1, *factor3)); } } @@ -200,8 +200,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - CHECK(point.valid()); // check triangulated point is valid - CHECK(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians @@ -209,28 +209,28 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { Matrix actualE; Vector actualb; factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); - CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); - CHECK(actualFs.size() == 2); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ @@ -256,7 +256,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - CHECK(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -265,28 +265,28 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { Matrix actualE; Vector actualb; factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - CHECK(actualE.rows() == 4); CHECK(actualE.cols() == 3); - CHECK(actualb.rows() == 4); CHECK(actualb.cols() == 1); - CHECK(actualFs.size() == 2); + EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - CHECK(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - CHECK(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - CHECK(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - CHECK(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); + EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); + EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); + EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - CHECK(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors double actualError = factor.error(values); // from smart factor @@ -404,11 +404,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { cameras.push_back(cam2); // Make sure triangulation works - CHECK(smartFactor1->triangulateSafe(cameras)); - CHECK(!smartFactor1->isDegenerate()); - CHECK(!smartFactor1->isPointBehindCamera()); + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); boost::optional p = smartFactor1->point(); - CHECK(p); + EXPECT(p); EXPECT(assert_equal(landmark1, *p)); VectorValues zeroDelta; @@ -703,7 +703,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // linearization point for the 3D point smartFactor1->triangulateSafe(smartFactor1->cameras(values)); TriangulationResult point = smartFactor1->point(); - CHECK(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid // Use the factor to calculate the Jacobians Matrix F = Matrix::Zero(2 * 3, 6 * 3); From 98340420407e4896fac02a8d97c0c20dd9a62876 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 00:15:33 -0400 Subject: [PATCH 37/50] test still in progress; removed a tmp function --- gtsam/geometry/CameraSet.h | 7 --- gtsam/geometry/tests/testCameraSet.cpp | 87 ++++++++++++++++++++++++++ 2 files changed, 87 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 19ab7ab316..f0bfffb58f 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -142,13 +142,6 @@ class CameraSet : public std::vector > return ErrorVector(project2(point, Fs, E), measured); } - static SymmetricBlockMatrix SchurComplement312( - const std::vector< Eigen::Matrix, - Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { - return SchurComplement<3,12>(Fs, E, P, b); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 761ef3a8ca..e706b326b2 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -125,6 +125,93 @@ TEST(CameraSet, Pinhole) { EXPECT(assert_equal(actualE, E)); } +/* ************************************************************************* */ +TEST(CameraSet, SchurComplementAndRearrangeBlocks) { + typedef PinholePose Camera; + typedef CameraSet Set; + typedef Point2Vector ZZ; + + KeyVector nonuniqueKeys; + nonuniqueKeys.push_back(0); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(0); + + KeyVector uniqueKeys; + uniqueKeys.push_back(0); + uniqueKeys.push_back(1); + uniqueKeys.push_back(2); + + // this is the (block) Jacobian with respect to the nonuniqueKeys + std::vector, + Eigen::aligned_allocator > > Fs; + Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1) + Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2) + Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0) + Matrix E = Matrix::Identity(6,3) + Matrix::Ones(6,3); + Matrix34 Et = E.transpose(); + Matrix P = (Et * E).inverse(); + Vector b = 5*Vector::Ones(6); + +// { // SchurComplement +// // Actual +// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b); +// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); +// +// // Expected +// Matrix F = Matrix::Zero(6,3*12); +// F.block<2,12>(0,0) = Fs[0]; +// F.block<2,12>(2,12) = Fs[1]; +// F.block<2,12>(4,24) = Fs[2]; +// +// std::cout << "E \n" << E << std::endl; +// std::cout << "P \n" << P << std::endl; +// std::cout << "F \n" << F << std::endl; +// +// Matrix Ft = F.transpose(); +// Matrix H = Ft * F - Ft * E * P * Et * F; +// Vector v = Ft * (b - E * P * Et * b); +// Matrix expectedAugmentedHessian = Matrix::Zero(3*12+1, 3*12+1); +// expectedAugmentedHessian.block<36,36>(0,0) = H; +// expectedAugmentedHessian.block<36,1>(0,36) = v; +// expectedAugmentedHessian.block<1,36>(36,0) = v.transpose(); +// expectedAugmentedHessian(36,36) = b.squaredNorm(); +// +// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); +// } + +// { // SchurComplementAndRearrangeBlocks +// // Actual +// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>( +// Fs,E,P,b,nonuniqueKeys,uniqueKeys); +// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); +// +// // Expected +// // we first need to build the Jacobian F according to unique keys +// Matrix F = Matrix::Zero(6,18); +// F.block<2,6>(0,0) = Fs[0].block<2,6>(0,0); +// F.block<2,6>(0,6) = Fs[0].block<2,6>(0,6); +// F.block<2,6>(2,6) = Fs[1].block<2,6>(0,0); +// F.block<2,6>(2,12) = Fs[1].block<2,6>(0,6); +// F.block<2,6>(4,12) = Fs[2].block<2,6>(0,0); +// F.block<2,6>(4,0) = Fs[2].block<2,6>(0,6); +// +// std::cout << "P \n" << P << std::endl; +// std::cout << "F \n" << F << std::endl; +// +// Matrix Ft = F.transpose(); +// Matrix34 Et = E.transpose(); +// Vector v = Ft * (b - E * P * Et * b); +// Matrix H = Ft * F - Ft * E * P * Et * F; +// Matrix expectedAugmentedHessian(19, 19); +// expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); +// +// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); +// } +} + /* ************************************************************************* */ #include TEST(CameraSet, Stereo) { From 2f03e588fce0727e5895634dd500a66286fd684c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 00:47:17 -0400 Subject: [PATCH 38/50] fixed last test - this is good to go! --- gtsam/geometry/tests/testCameraSet.cpp | 156 ++++++++++++------------- 1 file changed, 77 insertions(+), 79 deletions(-) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e706b326b2..9118c8899f 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -17,6 +17,7 @@ */ #include +#include #include #include #include @@ -127,89 +128,86 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { - typedef PinholePose Camera; + typedef PinholePose Camera; typedef CameraSet Set; - typedef Point2Vector ZZ; - - KeyVector nonuniqueKeys; - nonuniqueKeys.push_back(0); - nonuniqueKeys.push_back(1); - nonuniqueKeys.push_back(1); - nonuniqueKeys.push_back(2); - nonuniqueKeys.push_back(2); - nonuniqueKeys.push_back(0); - - KeyVector uniqueKeys; - uniqueKeys.push_back(0); - uniqueKeys.push_back(1); - uniqueKeys.push_back(2); // this is the (block) Jacobian with respect to the nonuniqueKeys - std::vector, - Eigen::aligned_allocator > > Fs; - Fs.push_back(1 * Matrix::Ones(2,12)); // corresponding to key pair (0,1) - Fs.push_back(2 * Matrix::Ones(2,12)); // corresponding to key pair (1,2) - Fs.push_back(3 * Matrix::Ones(2,12)); // corresponding to key pair (2,0) - Matrix E = Matrix::Identity(6,3) + Matrix::Ones(6,3); - Matrix34 Et = E.transpose(); + std::vector, + Eigen::aligned_allocator > > Fs; + Fs.push_back(1 * Matrix::Ones(2, 12)); // corresponding to key pair (0,1) + Fs.push_back(2 * Matrix::Ones(2, 12)); // corresponding to key pair (1,2) + Fs.push_back(3 * Matrix::Ones(2, 12)); // corresponding to key pair (2,0) + Matrix E = 4 * Matrix::Identity(6, 3) + Matrix::Ones(6, 3); + E(0, 0) = 3; + E(1, 1) = 2; + E(2, 2) = 5; + Matrix Et = E.transpose(); Matrix P = (Et * E).inverse(); - Vector b = 5*Vector::Ones(6); - -// { // SchurComplement -// // Actual -// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3,12>(Fs,E,P,b); -// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); -// -// // Expected -// Matrix F = Matrix::Zero(6,3*12); -// F.block<2,12>(0,0) = Fs[0]; -// F.block<2,12>(2,12) = Fs[1]; -// F.block<2,12>(4,24) = Fs[2]; -// -// std::cout << "E \n" << E << std::endl; -// std::cout << "P \n" << P << std::endl; -// std::cout << "F \n" << F << std::endl; -// -// Matrix Ft = F.transpose(); -// Matrix H = Ft * F - Ft * E * P * Et * F; -// Vector v = Ft * (b - E * P * Et * b); -// Matrix expectedAugmentedHessian = Matrix::Zero(3*12+1, 3*12+1); -// expectedAugmentedHessian.block<36,36>(0,0) = H; -// expectedAugmentedHessian.block<36,1>(0,36) = v; -// expectedAugmentedHessian.block<1,36>(36,0) = v.transpose(); -// expectedAugmentedHessian(36,36) = b.squaredNorm(); -// -// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); -// } - -// { // SchurComplementAndRearrangeBlocks -// // Actual -// SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplementAndRearrangeBlocks<3,12,6>( -// Fs,E,P,b,nonuniqueKeys,uniqueKeys); -// Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); -// -// // Expected -// // we first need to build the Jacobian F according to unique keys -// Matrix F = Matrix::Zero(6,18); -// F.block<2,6>(0,0) = Fs[0].block<2,6>(0,0); -// F.block<2,6>(0,6) = Fs[0].block<2,6>(0,6); -// F.block<2,6>(2,6) = Fs[1].block<2,6>(0,0); -// F.block<2,6>(2,12) = Fs[1].block<2,6>(0,6); -// F.block<2,6>(4,12) = Fs[2].block<2,6>(0,0); -// F.block<2,6>(4,0) = Fs[2].block<2,6>(0,6); -// -// std::cout << "P \n" << P << std::endl; -// std::cout << "F \n" << F << std::endl; -// -// Matrix Ft = F.transpose(); -// Matrix34 Et = E.transpose(); -// Vector v = Ft * (b - E * P * Et * b); -// Matrix H = Ft * F - Ft * E * P * Et * F; -// Matrix expectedAugmentedHessian(19, 19); -// expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); -// -// EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); -// } + Vector b = 5 * Vector::Ones(6); + + { // SchurComplement + // Actual + SymmetricBlockMatrix augmentedHessianBM = Set::SchurComplement<3, 12>(Fs, E, + P, b); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); + + // Expected + Matrix F = Matrix::Zero(6, 3 * 12); + F.block<2, 12>(0, 0) = 1 * Matrix::Ones(2, 12); + F.block<2, 12>(2, 12) = 2 * Matrix::Ones(2, 12); + F.block<2, 12>(4, 24) = 3 * Matrix::Ones(2, 12); + + Matrix Ft = F.transpose(); + Matrix H = Ft * F - Ft * E * P * Et * F; + Vector v = Ft * (b - E * P * Et * b); + Matrix expectedAugmentedHessian = Matrix::Zero(3 * 12 + 1, 3 * 12 + 1); + expectedAugmentedHessian.block<36, 36>(0, 0) = H; + expectedAugmentedHessian.block<36, 1>(0, 36) = v; + expectedAugmentedHessian.block<1, 36>(36, 0) = v.transpose(); + expectedAugmentedHessian(36, 36) = b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } + + { // SchurComplementAndRearrangeBlocks + KeyVector nonuniqueKeys; + nonuniqueKeys.push_back(0); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(1); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(2); + nonuniqueKeys.push_back(0); + + KeyVector uniqueKeys; + uniqueKeys.push_back(0); + uniqueKeys.push_back(1); + uniqueKeys.push_back(2); + + // Actual + SymmetricBlockMatrix augmentedHessianBM = + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(Fs, E, P, b, + nonuniqueKeys, + uniqueKeys); + Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); + + // Expected + // we first need to build the Jacobian F according to unique keys + Matrix F = Matrix::Zero(6, 18); + F.block<2, 6>(0, 0) = Fs[0].block<2, 6>(0, 0); + F.block<2, 6>(0, 6) = Fs[0].block<2, 6>(0, 6); + F.block<2, 6>(2, 6) = Fs[1].block<2, 6>(0, 0); + F.block<2, 6>(2, 12) = Fs[1].block<2, 6>(0, 6); + F.block<2, 6>(4, 12) = Fs[2].block<2, 6>(0, 0); + F.block<2, 6>(4, 0) = Fs[2].block<2, 6>(0, 6); + + Matrix Ft = F.transpose(); + Vector v = Ft * (b - E * P * Et * b); + Matrix H = Ft * F - Ft * E * P * Et * F; + Matrix expectedAugmentedHessian(19, 19); + expectedAugmentedHessian << H, v, v.transpose(), b.squaredNorm(); + + EXPECT(assert_equal(expectedAugmentedHessian, actualAugmentedHessian)); + } } /* ************************************************************************* */ From 5e0a6b45c4e4bbb684e3e5a078f0289042e83fcb Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 24 Jul 2021 10:11:54 -0400 Subject: [PATCH 39/50] fixing ci issue --- gtsam/geometry/tests/testCameraSet.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index 9118c8899f..e583c0e80e 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -128,7 +128,7 @@ TEST(CameraSet, Pinhole) { /* ************************************************************************* */ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { - typedef PinholePose Camera; + typedef PinholePose Camera; typedef CameraSet Set; // this is the (block) Jacobian with respect to the nonuniqueKeys @@ -185,7 +185,7 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks<3, 12, 6>(Fs, E, P, b, + Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); From cbd6a2a01c1ba857b2570bf69936884dd9e5ded5 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 19:36:15 -0400 Subject: [PATCH 40/50] now using MakeJacobian --- gtsam/base/Lie.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index c93456a283..1e19bfc0eb 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -324,13 +324,12 @@ T expm(const Vector& x, int K=7) { */ template 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) { + typename MakeOptionalJacobian::type Hx = boost::none, + typename MakeOptionalJacobian::type 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; + typename MakeJacobian::type 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); From 9f19077217e31187282c718e6a57226f5fd396c7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 19:54:23 -0400 Subject: [PATCH 41/50] simplified jacobian computation, improved comments --- gtsam/base/Lie.h | 24 +++++++++--------------- gtsam/geometry/CameraSet.h | 5 +++-- 2 files changed, 12 insertions(+), 17 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 1e19bfc0eb..48d7bf5312 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -329,21 +329,15 @@ T interpolate(const T& X, const T& Y, double t, assert(t >= 0.0 && t <= 1.5); if (Hx || Hy) { typename traits::TangentVector log_Xinv_Y; - typename MakeJacobian::type 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; + typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; + + T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity + log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); + Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); + Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity + + if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if(Hy) *Hy = t * exp_H * log_H; return Xinv_Y; } return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index f0bfffb58f..3fc77bb2e2 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -198,8 +198,9 @@ class CameraSet : public std::vector > * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * In this version, we allow for the case where the keys in the Jacobian are organized - * differents from the keys in the output SymmetricBlockMatrix - * In particular: each block of the Jacobian captures 2 poses (useful for rolling shutter and extrinsic calibration) + * differently from the keys in the output SymmetricBlockMatrix + * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) + * such that F keeps the block structure that makes the Schur complement trick fast. */ template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( From a3ee695b85f92eb7487e568aa59f45f55b2080ca Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 20:05:19 -0400 Subject: [PATCH 42/50] reformatted using google style --- .../slam/ProjectionFactorRollingShutter.h | 39 ++++++++++++------- 1 file changed, 24 insertions(+), 15 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index fc7c939a8f..6ab16f4a05 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -36,7 +36,8 @@ namespace gtsam { * @addtogroup SLAM */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter : public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -80,10 +81,11 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = boost::none) + boost::optional body_P_sensor = + boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), @@ -107,11 +109,12 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, bool throwCheirality, bool verboseCheirality, - boost::optional body_P_sensor = boost::none) + boost::optional body_P_sensor = + boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), @@ -160,8 +163,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 H1 = boost::none, + 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 { @@ -171,8 +175,10 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 camera(pose.compose(*body_P_sensor_, HbodySensor), *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + 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) @@ -184,14 +190,15 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 camera(pose, *K_); - Point2 reprojectionError(camera.project(point, Hprj, H3, boost::none) - measured_); + 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) { + } catch (CheiralityException& e) { if (H1) *H1 = Matrix::Zero(2, 6); if (H2) @@ -252,6 +259,8 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 struct traits : public Testable {}; +template<> struct traits : public Testable< + ProjectionFactorRollingShutter> { +}; -}//namespace gtsam +} //namespace gtsam From 0a8ebdf8cd027e93eef7e38ab1b3c8d2f62ce6d7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Fri, 13 Aug 2021 21:42:09 -0400 Subject: [PATCH 43/50] renamed interp param to alpha, improved comments, added cpp --- gtsam/base/Lie.h | 5 +- .../slam/ProjectionFactorRollingShutter.cpp | 73 +++++++++++++++++ .../slam/ProjectionFactorRollingShutter.h | 78 ++++--------------- .../SmartProjectionPoseFactorRollingShutter.h | 40 +++++----- 4 files changed, 112 insertions(+), 84 deletions(-) create mode 100644 gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 48d7bf5312..34422edd75 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 0000000000..5fc1c05ebe --- /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 6ab16f4a05..5827a538ce 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); From 37b001307e14bf859b8dcb2b9902f8d700243bcb Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 11:29:34 -0400 Subject: [PATCH 44/50] plot twist: templating new factor on CAMERA --- .../SmartProjectionPoseFactorRollingShutter.h | 18 +- ...martProjectionPoseFactorRollingShutter.cpp | 209 +++++++++++++++++- 2 files changed, 218 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index dbe734a2c7..524943a3f4 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -37,9 +37,11 @@ namespace gtsam { * from which the pixel observation pose can be interpolated. * @addtogroup SLAM */ -template -class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< -PinholePose > { +template +class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { + + public: + typedef typename CAMERA::CalibrationType CALIBRATION; protected: /// shared pointer to calibration object (one for each observation) @@ -213,8 +215,8 @@ PinholePose > { /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>(&p); + const SmartProjectionPoseFactorRollingShutter* e = + dynamic_cast*>(&p); double keyPairsEqual = true; if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ @@ -430,9 +432,9 @@ PinholePose > { // end of class declaration /// traits -template -struct traits > : -public Testable > { +template +struct traits > : +public Testable > { }; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 75aaf4ac12..bf8a8c4abc 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -73,7 +73,7 @@ Camera cam3(interp_pose3, sharedK); } LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; /* ************************************************************************* */ TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { @@ -770,6 +770,213 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark + // at a single pose, a setup that occurs in multi-camera systems + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); + key_pairs.push_back(std::make_pair(x1, x2)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + interp_factors.push_back(interp_factor1); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + 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))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(8); + + // create projection factors rolling shutter + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, + model, x1, x2, l0, sharedK); + Matrix H1Actual, H2Actual, H3Actual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(0, 0) = H1Actual; + F.block<2, 6>(0, 6) = H2Actual; + E.block<2, 3>(0, 0) = H3Actual; + + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, + model, x2, x3, l0, sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(2, 6) = H1Actual; + F.block<2, 6>(2, 12) = H2Actual; + E.block<2, 3>(2, 0) = H3Actual; + + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, + model, x3, x1, l0, sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(4, 12) = H1Actual; + F.block<2, 6>(4, 0) = H2Actual; + E.block<2, 3>(4, 0) = H3Actual; + + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, + model, x1, x2, l0, sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + F.block<2, 6>(6, 0) = H1Actual; + F.block<2, 6>(6, 6) = H2Actual; + E.block<2, 3>(6, 0) = H3Actual; + + // whiten + F = (1/sigma) * F; + E = (1/sigma) * E; + b = (1/sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactorsRS; + nfg_projFactorsRS.add(factor11); + nfg_projFactorsRS.add(factor12); + nfg_projFactorsRS.add(factor13); + nfg_projFactorsRS.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactorsRS.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { + + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1,x2)); + key_pairs.push_back(std::make_pair(x2,x3)); + key_pairs.push_back(std::make_pair(x3,x1)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + // For first factor, we create redundant measurement (taken by the same keys as factor 1, to + // make sure the redundancy in the keys does not create problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + std::vector interp_factors_redundant = interp_factors; + interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); + + 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + 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, level_pose); + values.insert(x2, pose_right); + // initialize third pose with some noise, we expect it to move back to original pose_above + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + 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-5)); +} + #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) From f712d6215086b8508062df29658337af62ee3d11 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 16:30:25 -0400 Subject: [PATCH 45/50] Added override --- .../slam/ProjectionFactorRollingShutter.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 5827a538ce..ed56ad8f36 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -129,7 +129,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3 (gtsam::NonlinearFactor::shared_ptr(new This(*this))); } @@ -140,7 +140,7 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3measured_.size() != + this->cameras(values).size()) // 1 observation per interpolated camera + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(this->cameras(values)); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { - 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); + for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); + for (Vector& v : gs) v = Vector::Zero(DimPose); + return boost::make_shared>(this->keys_, + Gs, gs, 0.0); } else { - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } // compute Jacobian given triangulated 3D Point From 2b3709ec73496f74e98b11b26024ccfc2a35fede Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:19:53 -0400 Subject: [PATCH 47/50] Got rid of SchurComplementAndRearrangeBlocks_3_12_6 --- gtsam/geometry/CameraSet.h | 13 ------------- gtsam/geometry/tests/testCameraSet.cpp | 5 ++--- .../slam/SmartProjectionPoseFactorRollingShutter.h | 2 +- 3 files changed, 3 insertions(+), 17 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index b8fbb42008..58122e33e8 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -286,19 +286,6 @@ class CameraSet : public std::vector > return augmentedHessianUniqueKeys; } - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_12_6( - const std::vector, - Eigen::aligned_allocator > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - return SchurComplementAndRearrangeBlocks<3,12,6>(Fs, E, P, b, - jacobianKeys, - hessianKeys); - } - /** * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix * G = F' * F - F' * E * P * E' * F diff --git a/gtsam/geometry/tests/testCameraSet.cpp b/gtsam/geometry/tests/testCameraSet.cpp index e583c0e80e..144f28314c 100644 --- a/gtsam/geometry/tests/testCameraSet.cpp +++ b/gtsam/geometry/tests/testCameraSet.cpp @@ -185,9 +185,8 @@ TEST(CameraSet, SchurComplementAndRearrangeBlocks) { // Actual SymmetricBlockMatrix augmentedHessianBM = - Set::SchurComplementAndRearrangeBlocks_3_12_6(Fs, E, P, b, - nonuniqueKeys, - uniqueKeys); + Set::SchurComplementAndRearrangeBlocks<3, 12, 6>( + Fs, E, P, b, nonuniqueKeys, uniqueKeys); Matrix actualAugmentedHessian = augmentedHessianBM.selfadjointView(); // Expected diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 3624e3f8e0..236ae6c953 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -363,7 +363,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); return boost::make_shared < RegularHessianFactor From d0505d4bc3767b79aebfc28c7fc59258f309f90c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:20:08 -0400 Subject: [PATCH 48/50] Check equals not assert_equal --- ...stSmartProjectionPoseFactorRollingShutter.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index bf8a8c4abc..a7441e170e 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -138,8 +138,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(assert_equal(*factor1, *factor2)); - EXPECT(assert_equal(*factor1, *factor3)); + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); } { // create slightly different factors (different keys) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -147,8 +147,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different extrinsics) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -156,8 +156,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different interp factors) and show equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); @@ -165,8 +165,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - EXPECT(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } } From bafcde9ee195216a0b4bae1381bfa74e59f45fad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:36:14 -0400 Subject: [PATCH 49/50] Google-style formatting in new files. --- gtsam/geometry/CameraSet.h | 41 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 173 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 230 ++++---- .../testProjectionFactorRollingShutter.cpp | 252 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 498 +++++++++++------- 6 files changed, 694 insertions(+), 539 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 58122e33e8..143d4bc3c3 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -218,48 +218,52 @@ class CameraSet : public std::vector > size_t nrNonuniqueKeys = jacobianKeys.size(); size_t nrUniqueKeys = hessianKeys.size(); - // marginalize point: note - we reuse the standard SchurComplement function - SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs,E,P,b); + // Marginalize point: note - we reuse the standard SchurComplement function. + SymmetricBlockMatrix augmentedHessian = SchurComplement(Fs, E, P, b); - // now pack into an Hessian factor - std::vector dims(nrUniqueKeys + 1); // this also includes the b term + // Pack into an Hessian factor, allow space for b term. + std::vector dims(nrUniqueKeys + 1); std::fill(dims.begin(), dims.end() - 1, NDD); dims.back() = 1; SymmetricBlockMatrix augmentedHessianUniqueKeys; - // here we have to deal with the fact that some blocks may share the same keys - if (nrUniqueKeys == nrNonuniqueKeys) { // if there is 1 calibration key per camera + // Deal with the fact that some blocks may share the same keys. + if (nrUniqueKeys == nrNonuniqueKeys) { + // Case when 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 + } else { + // When multiple cameras share a calibration we have to rearrange + // the results of the Schur complement matrix. + std::vector nonuniqueDims(nrNonuniqueKeys + 1); // includes b std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, NDD); nonuniqueDims.back() = 1; augmentedHessian = SymmetricBlockMatrix( nonuniqueDims, Matrix(augmentedHessian.selfadjointView())); - // get map from key to location in the new augmented Hessian matrix (the one including only unique keys) + // 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[hessianKeys[k]] = k; } - // initialize matrix to zero + // Initialize matrix to zero. augmentedHessianUniqueKeys = SymmetricBlockMatrix( dims, Matrix::Zero(NDD * nrUniqueKeys + 1, NDD * 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) + // 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 = jacobianKeys.at(i); - // update information vector + // Update information vector. augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i], nrUniqueKeys, augmentedHessian.aboveDiagonalBlock(i, nrNonuniqueKeys)); - // update blocks + // Update blocks. for (size_t j = i; j < nrNonuniqueKeys; j++) { // cols Key key_j = jacobianKeys.at(j); if (i == j) { @@ -273,13 +277,14 @@ class CameraSet : public std::vector > } else { augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i], - augmentedHessian.aboveDiagonalBlock(i, j) - + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); + augmentedHessian.aboveDiagonalBlock(i, j) + + augmentedHessian.aboveDiagonalBlock(i, j).transpose()); } } } } - // update bottom right element of the matrix + + // Update bottom right element of the matrix. augmentedHessianUniqueKeys.updateDiagonalBlock( nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); } diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp index 5fc1c05ebe..c92a13daf5 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.cpp @@ -23,7 +23,6 @@ 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; @@ -32,12 +31,10 @@ Vector ProjectionFactorRollingShutter::evaluateError( 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); + 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_); @@ -45,29 +42,23 @@ Vector ProjectionFactorRollingShutter::evaluateError( } } 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); + 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 (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()); + << 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 +} // namespace gtsam diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index ed56ad8f36..c92653c13e 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -17,41 +17,47 @@ #pragma once -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { /** - * 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 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. + * 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 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 */ -class ProjectionFactorRollingShutter : public NoiseModelFactor3 { +class ProjectionFactorRollingShutter + : public NoiseModelFactor3 { protected: - // Keep a copy of measurement and calibration for I/O - Point2 measured_; ///< 2D measurement - double alpha_; ///< interpolation parameter in [0,1] corresponding to the point2 measurement + Point2 measured_; ///< 2D measurement + double alpha_; ///< 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 + 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) + 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; @@ -66,72 +72,72 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3& K, - boost::optional body_P_sensor = - boost::none) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, 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), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(false), - verboseCheirality_(false) { - } + verboseCheirality_(false) {} /** * Constructor with exception-handling flags - * @param measured is the 2-dimensional pixel location of point in the image (the measurement) + * @param measured is the 2-dimensional pixel location of point in the image + * (the measurement) * @param alpha in [0,1] 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) + * @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 alpha, - 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) + ProjectionFactorRollingShutter( + const Point2& measured, double alpha, 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), alpha_(alpha), K_(K), body_P_sensor_(body_P_sensor), throwCheirality_(throwCheirality), - verboseCheirality_(verboseCheirality) { - } + verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~ProjectionFactorRollingShutter() { - } + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); } /** @@ -139,8 +145,9 @@ class ProjectionFactorRollingShutter : public NoiseModelFactor3>& world_P_body_key_pairs, const std::vector& alphas, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector& body_P_sensors) { assert(world_P_body_key_pairs.size() == measurements.size()); assert(world_P_body_key_pairs.size() == alphas.size()); assert(world_P_body_key_pairs.size() == Ks.size()); @@ -151,20 +169,24 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor>& world_P_body_key_pairs, const std::vector& alphas, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + 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() == alphas.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -174,39 +196,37 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor> calibration() const { + const std::vector>& calibration() const { return K_all_; } - /// return (for each observation) the keys of the pair of poses from which we interpolate - const std::vector> world_P_body_key_pairs() const { + /// return (for each observation) the keys of the pair of poses from which we + /// interpolate + const std::vector>& world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector alphas() const { - return alphas_; - } + const std::vector& alphas() const { return alphas_; } /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } + 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 { + 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; + << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; std::cout << " pose2 key: " - << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; + << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); @@ -217,17 +237,20 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; - if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ - for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + if (this->world_P_body_key_pairs_.size() == + e->world_P_body_key_pairs().size()) { + for (size_t k = 0; k < this->world_P_body_key_pairs_.size(); k++) { const Key key1own = world_P_body_key_pairs_[k].first; const Key key1e = e->world_P_body_key_pairs()[k].first; const Key key2own = world_P_body_key_pairs_[k].second; const Key key2e = e->world_P_body_key_pairs()[k].second; - if ( !(key1own == key1e) || !(key2own == key2e) ){ - keyPairsEqual = false; break; + if (!(key1own == key1e) || !(key2own == key2e)) { + keyPairsEqual = false; + break; } } } else { @@ -235,18 +258,19 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactorbody_P_sensors_.size() == e->body_P_sensors().size()){ - for(size_t i=0; i< this->body_P_sensors_.size(); i++){ - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){ - extrinsicCalibrationEqual = false; break; + if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { + for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { + if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { + extrinsicCalibrationEqual = false; + break; } } } else { extrinsicCalibrationEqual = false; } - return e && Base::equals(p, tol) && K_all_ == e->calibration() - && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && K_all_ == e->calibration() && + alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -264,12 +288,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactormeasured_.size(); - E = Matrix::Zero(2 * numViews, 3); // a Point2 for each view (point jacobian) + E = Matrix::Zero(2 * numViews, + 3); // a Point2 for each view (point jacobian) b = Vector::Zero(2 * numViews); // a Point2 for each view // intermediate Jacobians Eigen::Matrix dProject_dPoseCam; Eigen::Matrix dInterpPose_dPoseBody1, - dInterpPose_dPoseBody2, dPoseCam_dInterpPose; + dInterpPose_dPoseBody2, dPoseCam_dInterpPose; Eigen::Matrix Ei; for (size_t i = 0; i < numViews; i++) { // for each camera/measurement @@ -285,14 +310,16 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor 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)); + 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, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) - J.block(0, 6, ZDim, 6) = dProject_dPoseCam * dPoseCam_dInterpPose - * dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) + J.block(0, 0, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody1; // (2x6) * (6x6) * (6x6) + J.block(0, 6, ZDim, 6) = + dProject_dPoseCam * dPoseCam_dInterpPose * + dInterpPose_dPoseBody2; // (2x6) * (6x6) * (6x6) // fit into the output structures Fs.push_back(J); @@ -353,21 +380,23 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor( Fs, E, P, b, nonuniqueKeys, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared>( + this->keys_, augmentedHessianUniqueKeys); } /** @@ -376,7 +405,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactoractive(values)) { return this->totalReprojectionError(this->cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -396,10 +425,13 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor(world_P_body_key_pairs_[i].first); - const Pose3& w_P_body2 = values.at(world_P_body_key_pairs_[i].second); + 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 = alphas_[i]; - const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); + 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); cameras.emplace_back(w_P_cam, K_all_[i]); @@ -408,44 +440,46 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor linearizeDamped( const Values& values, const double lambda = 0.0) const { - // depending on flag set on construction we may linearize to different linear factors + // depending on flag set on construction we may linearize to different + // linear factors switch (this->params_.linearizationMode) { case HESSIAN: return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization mode"); + "SmartProjectionPoseFactorRollingShutter: unknown linearization " + "mode"); } } /// linearize - boost::shared_ptr linearize(const Values& values) const - override { + boost::shared_ptr linearize( + const Values& values) const override { return this->linearizeDamped(values); } private: /// Serialization function friend class boost::serialization::access; - template + template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(K_all_); } - }; // end of class declaration /// traits -template -struct traits > : -public Testable > { -}; +template +struct traits> + : public Testable> {}; } // namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp index 943e350d40..161c9aa55b 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -16,34 +16,33 @@ * @date July 2021 */ -#include +#include #include -#include -#include +#include #include #include -#include -#include #include - -#include +#include +#include +#include +#include using namespace std::placeholders; using namespace std; using namespace gtsam; // make a realistic calibration matrix -static double fov = 60; // degrees -static size_t w=640,h=480; -static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); +static double fov = 60; // degrees +static size_t w = 640, h = 480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov, w, h)); // Create a noise model for the pixel error static SharedNoiseModel model(noiseModel::Unit::Create(2)); // Convenience for named keys -using symbol_shorthand::X; using symbol_shorthand::L; using symbol_shorthand::T; +using symbol_shorthand::X; // Convenience to define common variables across many tests static Key poseKey1(X(1)); @@ -51,74 +50,84 @@ static Key poseKey2(X(2)); static Key pointKey(L(1)); static double interp_params = 0.5; static Point2 measurement(323.0, 240.0); -static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +static Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), + Point3(0.25, -0.10, 1.0)); /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Constructor) { - ProjectionFactorRollingShutter factor(measurement, interp_params, model, poseKey1, poseKey2, pointKey, K); +TEST(ProjectionFactorRollingShutter, Constructor) { + ProjectionFactorRollingShutter factor(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ConstructorWithTransform) { +TEST(ProjectionFactorRollingShutter, ConstructorWithTransform) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Equals ) { - { // factors are equal - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); +TEST(ProjectionFactorRollingShutter, Equals) { + { // factors are equal + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal (keys are different) - ProjectionFactorRollingShutter factor1(measurement, interp_params, - model, poseKey1, poseKey2, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, interp_params, - model, poseKey1, poseKey1, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (keys are different) + ProjectionFactorRollingShutter factor1(measurement, interp_params, model, + poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, interp_params, model, + poseKey1, poseKey1, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } - { // factors are NOT equal (different interpolation) - ProjectionFactorRollingShutter factor1(measurement, 0.1, - model, poseKey1, poseKey1, pointKey, K); - ProjectionFactorRollingShutter factor2(measurement, 0.5, - model, poseKey1, poseKey2, pointKey, K); - CHECK(!assert_equal(factor1, factor2)); // not equal + { // factors are NOT equal (different interpolation) + ProjectionFactorRollingShutter factor1(measurement, 0.1, model, poseKey1, + poseKey1, pointKey, K); + ProjectionFactorRollingShutter factor2(measurement, 0.5, model, poseKey1, + poseKey2, pointKey, K); + CHECK(!assert_equal(factor1, factor2)); // not equal } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, EqualsWithTransform ) { - { // factors are equal +TEST(ProjectionFactorRollingShutter, EqualsWithTransform) { + { // factors are equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + poseKey1, poseKey2, pointKey, K, + body_P_sensor); CHECK(assert_equal(factor1, factor2)); } - { // factors are NOT equal + { // factors are NOT equal ProjectionFactorRollingShutter factor1(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); - Pose3 body_P_sensor2(Rot3::RzRyRx(0.0, 0.0, 0.0), Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor + poseKey1, poseKey2, pointKey, K, + body_P_sensor); + Pose3 body_P_sensor2( + Rot3::RzRyRx(0.0, 0.0, 0.0), + Point3(0.25, -0.10, 1.0)); // rotation different from body_P_sensor ProjectionFactorRollingShutter factor2(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor2); + poseKey1, poseKey2, pointKey, K, + body_P_sensor2); CHECK(!assert_equal(factor1, factor2)); } } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Error ) { +TEST(ProjectionFactorRollingShutter, Error) { { // Create the factor with a measurement that is 3 pixels off in x // Camera pose corresponds to the first camera double t = 0.0; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-6)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -6)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -134,11 +143,12 @@ TEST( ProjectionFactorRollingShutter, Error ) { // Create the factor with a measurement that is 3 pixels off in x // Camera pose is actually interpolated now double t = 0.5; - ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measurement, t, model, poseKey1, + poseKey2, pointKey, K); // Set the linearization point - Pose3 pose1(Rot3(), Point3(0,0,-8)); - Pose3 pose2(Rot3(), Point3(0,0,-4)); + Pose3 pose1(Rot3(), Point3(0, 0, -8)); + Pose3 pose2(Rot3(), Point3(0, 0, -4)); Point3 point(0.0, 0.0, 0.0); // Use the factor to calculate the error @@ -153,15 +163,16 @@ TEST( ProjectionFactorRollingShutter, Error ) { { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -175,19 +186,20 @@ TEST( ProjectionFactorRollingShutter, Error ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { +TEST(ProjectionFactorRollingShutter, ErrorWithTransform) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the error Vector actualError(factor.evaluateError(pose1, pose2, point)); @@ -200,18 +212,19 @@ TEST( ProjectionFactorRollingShutter, ErrorWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, Jacobian ) { +TEST(ProjectionFactorRollingShutter, Jacobian) { // Create measurement by projecting 3D landmark double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -222,22 +235,25 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -245,19 +261,20 @@ TEST( ProjectionFactorRollingShutter, Jacobian ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { +TEST(ProjectionFactorRollingShutter, JacobianWithTransform) { // Create measurement by projecting 3D landmark double t = 0.6; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); - Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0.2,0.1)); - PinholeCamera camera(poseInterp*body_P_sensor3, *K); - Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera + Pose3 body_P_sensor3(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0.2, 0.1)); + PinholeCamera camera(poseInterp * body_P_sensor3, *K); + Point3 point(0.0, 0.0, 5.0); // 5 meters in front of the camera Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, body_P_sensor3); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, + pointKey, K, body_P_sensor3); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -268,22 +285,25 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -291,41 +311,48 @@ TEST( ProjectionFactorRollingShutter, JacobianWithTransform ) { } /* ************************************************************************* */ -TEST( ProjectionFactorRollingShutter, cheirality ) { +TEST(ProjectionFactorRollingShutter, cheirality) { // Create measurement by projecting 3D landmark behind camera double t = 0.3; - Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0,0,0)); - Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0,0,1)); + Pose3 pose1(Rot3::RzRyRx(0.1, 0.0, 0.1), Point3(0, 0, 0)); + Pose3 pose2(Rot3::RzRyRx(-0.1, -0.1, 0.0), Point3(0, 0, 1)); Pose3 poseInterp = interpolate(pose1, pose2, t); PinholeCamera camera(poseInterp, *K); - Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera + Point3 point(0.0, 0.0, -5.0); // 5 meters behind the camera #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION - Point2 measured = Point2(0.0,0.0); // project would throw an exception - { // check that exception is thrown if we set throwCheirality = true + Point2 measured = Point2(0.0, 0.0); // project would throw an exception + { // check that exception is thrown if we set throwCheirality = true bool throwCheirality = true; bool verboseCheirality = true; - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); CHECK_EXCEPTION(factor.evaluateError(pose1, pose2, point), CheiralityException); } - { // check that exception is NOT thrown if we set throwCheirality = false, and outputs are correct - bool throwCheirality = false; // default - bool verboseCheirality = false; // default - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K, throwCheirality, verboseCheirality); + { // check that exception is NOT thrown if we set throwCheirality = false, + // and outputs are correct + bool throwCheirality = false; // default + bool verboseCheirality = false; // default + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K, + throwCheirality, verboseCheirality); // Use the factor to calculate the error Matrix H1Actual, H2Actual, H3Actual; - Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, H2Actual, H3Actual)); + Vector actualError(factor.evaluateError(pose1, pose2, point, H1Actual, + H2Actual, H3Actual)); // The expected error is zero - Vector expectedError = Vector2::Constant(2.0 * K->fx()); // this is what we return when point is behind camera + Vector expectedError = Vector2::Constant( + 2.0 * K->fx()); // this is what we return when point is behind camera // Verify we get the expected error CHECK(assert_equal(expectedError, actualError, 1e-9)); - CHECK(assert_equal(Matrix::Zero(2,6), H1Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,6), H2Actual, 1e-5)); - CHECK(assert_equal(Matrix::Zero(2,3), H3Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H1Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 6), H2Actual, 1e-5)); + CHECK(assert_equal(Matrix::Zero(2, 3), H3Actual, 1e-5)); } #else { @@ -333,7 +360,8 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { Point2 measured = camera.project(point); // create factor - ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, poseKey2, pointKey, K); + ProjectionFactorRollingShutter factor(measured, t, model, poseKey1, + poseKey2, pointKey, K); // Use the factor to calculate the Jacobians Matrix H1Actual, H2Actual, H3Actual; @@ -344,22 +372,25 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H2Expected = numericalDerivative32( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); Matrix H3Expected = numericalDerivative33( std::function( std::bind(&ProjectionFactorRollingShutter::evaluateError, &factor, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3, boost::none, boost::none, boost::none)), - pose1, pose2, point); + std::placeholders::_3, boost::none, boost::none, + boost::none)), + pose1, pose2, point); CHECK(assert_equal(H1Expected, H1Actual, 1e-5)); CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); @@ -368,8 +399,9 @@ TEST( ProjectionFactorRollingShutter, cheirality ) { #endif } - /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index a7441e170e..0b94d2c3fa 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -16,17 +16,19 @@ * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" -#include -#include -#include -#include -#include +#include #include #include -#include +#include +#include +#include +#include +#include + #include #include + +#include "gtsam/slam/tests/smartFactorScenarios.h" #define DISABLE_TIMING using namespace gtsam; @@ -39,8 +41,8 @@ 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; +using symbol_shorthand::X; // tests data static Symbol x1('X', 1); @@ -48,8 +50,8 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Symbol x4('X', 4); static Symbol l0('L', 0); -static Pose3 body_P_sensor = Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), - Point3(0.1, 0.0, 0.0)); +static Pose3 body_P_sensor = + Pose3(Rot3::Ypr(-0.1, 0.2, -0.2), Point3(0.1, 0.0, 0.0)); static Point2 measurement1(323.0, 240.0); static Point2 measurement2(200.0, 220.0); @@ -64,38 +66,39 @@ static double interp_factor3 = 0.5; namespace vanillaPoseRS { typedef PinholePose Camera; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); -Pose3 interp_pose1 = interpolate(level_pose,pose_right,interp_factor1); -Pose3 interp_pose2 = interpolate(pose_right,pose_above,interp_factor2); -Pose3 interp_pose3 = interpolate(pose_above,level_pose,interp_factor3); +Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); +Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); +Pose3 interp_pose3 = interpolate(pose_above, level_pose, interp_factor3); Camera cam1(interp_pose1, sharedK); Camera cam2(interp_pose2, sharedK); Camera cam3(interp_pose3, sharedK); -} +} // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; -typedef SmartProjectionPoseFactorRollingShutter< PinholePose > SmartFactorRS; +typedef SmartProjectionPoseFactorRollingShutter> + SmartFactorRS; /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { +TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, params); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, add) { +TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPose; SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); } /* ************************************************************************* */ -TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { +TEST(SmartProjectionPoseFactorRollingShutter, Equals) { using namespace vanillaPose; // create fake measurements @@ -104,10 +107,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { measurements.push_back(measurement2); measurements.push_back(measurement3); - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x4)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x4)); std::vector> intrinsicCalibrations; intrinsicCalibrations.push_back(sharedK); @@ -126,13 +129,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, extrinsicCalibrations); + factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, + extrinsicCalibrations); // create by adding a batch of measurements with a single calibrations SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); - { // create equal factors and show equal returns true + { // create equal factors and show equal returns true SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); @@ -141,28 +145,34 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create slightly different factors (different keys) and show equal returns false + { // create slightly different factors (different keys) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x2, interp_factor2, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different extrinsics) and show equal returns false + { // create slightly different factors (different extrinsics) and show equal + // returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor*body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor2, sharedK, + body_P_sensor * body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } - { // create slightly different factors (different interp factors) and show equal returns false + { // create slightly different factors (different interp factors) and show + // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, body_P_sensor); // different! + factor1->add(measurement2, x2, x3, interp_factor1, sharedK, + body_P_sensor); // different! factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); EXPECT(!factor1->equals(*factor2)); @@ -170,13 +180,16 @@ TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation pose is interpolated -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 +static const int DimBlock = 12; ///< size of the variable stacking 2 poses from + ///< which the observation pose is interpolated +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 /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { using namespace vanillaPoseRS; // Project two landmarks into two cameras @@ -188,7 +201,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -200,41 +213,56 @@ TEST( SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians ) { // Check triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal(landmark1, *point)); // check triangulation result matches expected 3D landmark + EXPECT(point.valid()); // check triangulated point is valid + EXPECT(assert_equal( + landmark1, + *point)); // check triangulation result matches expected 3D landmark // Check Jacobians // -- actual Jacobians FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorId); + Vector expectedb1 = factor1.evaluateError( + level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, body_P_sensorId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = factor2.evaluateError( + pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { // also includes non-identical extrinsic calibration using namespace vanillaPoseRS; @@ -246,9 +274,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { SmartFactorRS factor(model); factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorNonId); + factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, + body_P_sensorNonId); - Values values; // it's a pose factor, hence these are poses + Values values; // it's a pose factor, hence these are poses values.insert(x1, level_pose); values.insert(x2, pose_right); values.insert(x3, pose_above); @@ -256,7 +285,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { // Perform triangulation factor.triangulateSafe(factor.cameras(values)); TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid + EXPECT(point.valid()); // check triangulated point is valid Point3 landmarkNoisy = *point; // Check Jacobians @@ -264,32 +293,48 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { FBlocks actualFs; Matrix actualE; Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, values); - EXPECT(actualE.rows() == 4); EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); EXPECT(actualb.cols() == 1); + factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, + values); + EXPECT(actualE.rows() == 4); + EXPECT(actualE.cols() == 3); + EXPECT(actualb.rows() == 4); + EXPECT(actualb.cols() == 1); EXPECT(actualFs.size() == 2); // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, x2, l0, sharedK, body_P_sensorNonId); + ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, + x2, l0, sharedK, body_P_sensorNonId); Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, expectedF12, expectedE1); - EXPECT(assert_equal( expectedF11, Matrix(actualFs[0].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF12, Matrix(actualFs[0].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE1, Matrix(actualE.block(0,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, x2, x3, l0, sharedK, body_P_sensorNonId); + Vector expectedb1 = + factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, + expectedF12, expectedE1); + EXPECT( + assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); + + ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, + x2, x3, l0, sharedK, + body_P_sensorNonId); Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, expectedF22, expectedE2); - EXPECT(assert_equal( expectedF21, Matrix(actualFs[1].block(0,0,2,6)), 1e-5)); - EXPECT(assert_equal( expectedF22, Matrix(actualFs[1].block(0,6,2,6)), 1e-5)); - EXPECT(assert_equal( expectedE2, Matrix(actualE.block(2,0,2,3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus reprojectionError - EXPECT(assert_equal( expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); + Vector expectedb2 = + factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, + expectedF22, expectedE2); + EXPECT( + assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); + EXPECT( + assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); + EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); + // by definition computeJacobiansWithTriangulatedPoint returns minus + // reprojectionError + EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); // Check errors - double actualError = factor.error(values); // from smart factor + double actualError = factor.error(values); // from smart factor NonlinearFactorGraph nfg; nfg.add(factor1); nfg.add(factor2); @@ -299,8 +344,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { - +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -310,10 +354,10 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); @@ -344,20 +388,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { groundTruth.insert(x3, pose_above); 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/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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - 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))); + EXPECT( // check that the pose is actually noisy + 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); @@ -366,11 +412,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { - // here we replicate a test in SmartProjectionPoseFactor by setting interpolation - // factors to 0 and 1 (such that the rollingShutter measurements falls back to standard pixel measurements) - // Note: this is a quite extreme test since in typical camera you would not have more than - // 1 measurement per landmark at each interpolated pose +TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { + // here we replicate a test in SmartProjectionPoseFactor by setting + // interpolation factors to 0 and 1 (such that the rollingShutter measurements + // falls back to standard pixel measurements) Note: this is a quite extreme + // test since in typical camera you would not have more than 1 measurement per + // landmark at each interpolated pose using namespace vanillaPose; // Default cameras for simple derivatives @@ -423,7 +470,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { perturbedDelta.insert(x2, delta); double expectedError = 2500; - // After eliminating the point, A1 and A2 contain 2-rank information on cameras: + // 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; @@ -449,8 +497,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { Values values; values.insert(x1, pose1); values.insert(x2, pose2); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values); EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); EXPECT(assert_equal(expected, *actual, 1e-6)); EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); @@ -458,7 +506,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -478,7 +526,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - double excludeLandmarksFutherThanDist = 1e10; //very large + double excludeLandmarksFutherThanDist = 1e10; // very large SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); @@ -486,13 +534,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -509,7 +557,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -520,7 +569,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI ) { } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDistance ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_landmarkDistance) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -548,13 +598,13 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model,params); + SmartFactorRS smartFactor1(model, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor2(model,params); + SmartFactorRS smartFactor2(model, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - SmartFactorRS smartFactor3(model,params); + SmartFactorRS smartFactor3(model, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -571,10 +621,12 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - // All factors are disabled (due to the distance threshold) and pose should remain where it is + // All factors are disabled (due to the distance threshold) and pose should + // remain where it is Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); @@ -582,7 +634,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_landmarkDista } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlierRejection ) { +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_dynamicOutlierRejection) { using namespace vanillaPoseRS; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -594,7 +647,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = measurements_lmk4.at(0) + Point2(10, 10); // add outlier + measurements_lmk4.at(0) = + measurements_lmk4.at(0) + Point2(10, 10); // add outlier // create inputs std::vector> key_pairs; @@ -608,7 +662,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie interp_factors.push_back(interp_factor3); double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 3; // max 3 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error SmartProjectionParams params; params.setRankTolerance(1.0); @@ -640,12 +695,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie graph.addPrior(x1, level_pose, noisePrior); graph.addPrior(x2, pose_right, noisePrior); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, 0.01)); // smaller noise, otherwise outlier rejection will kick in + Pose3 noise_pose = Pose3( + Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), + Point3(0.01, 0.01, + 0.01)); // smaller noise, otherwise outlier rejection will kick in Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); // Optimization should correct 3rd pose @@ -656,8 +714,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_dynamicOutlie } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter) { - +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -683,10 +741,15 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - 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))); + 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))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -695,8 +758,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -714,46 +777,52 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, model, x1, x2, l0, sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -771,9 +840,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { - // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark - // at a single pose, a setup that occurs in multi-camera systems +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel + // measurements of the same landmark at a single pose, a setup that occurs in + // multi-camera systems using namespace vanillaPoseRS; Point2Vector measurements_lmk1; @@ -783,7 +854,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // create redundant measurements: Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement // create inputs std::vector> key_pairs; @@ -799,17 +871,23 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, + sharedK); 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, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise to get a nontrivial linearization point + // initialize third pose with some noise to get a nontrivial linearization + // point values.insert(x3, pose_above * noise_pose); EXPECT( // check that the pose is actually noisy - 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))); + 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))); // linearization point for the poses Pose3 pose1 = level_pose; @@ -818,8 +896,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli // ==== check Hessian of smartFactor1 ===== // -- compute actual Hessian - boost::shared_ptr linearfactor1 = smartFactor1->linearize( - values); + boost::shared_ptr linearfactor1 = + smartFactor1->linearize(values); Matrix actualHessian = linearfactor1->information(); // -- compute expected Hessian from manual Schur complement from Jacobians @@ -828,62 +906,74 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli TriangulationResult point = smartFactor1->point(); EXPECT(point.valid()); // check triangulated point is valid - // Use standard ProjectionFactorRollingShutter factor to calculate the Jacobians + // Use standard ProjectionFactorRollingShutter factor to calculate the + // Jacobians Matrix F = Matrix::Zero(2 * 4, 6 * 3); Matrix E = Matrix::Zero(2 * 4, 3); Vector b = Vector::Zero(8); // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], interp_factor1, - model, x1, x2, l0, sharedK); + ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], + interp_factor1, model, x1, x2, l0, + sharedK); Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(0, 0) = H1Actual; F.block<2, 6>(0, 6) = H2Actual; E.block<2, 3>(0, 0) = H3Actual; - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], + interp_factor2, model, x2, x3, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(2, 6) = H1Actual; F.block<2, 6>(2, 12) = H2Actual; E.block<2, 3>(2, 0) = H3Actual; - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], + interp_factor3, model, x3, x1, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(4, 12) = H1Actual; F.block<2, 6>(4, 0) = H2Actual; E.block<2, 3>(4, 0) = H3Actual; - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], interp_factor1, - model, x1, x2, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, H2Actual, H3Actual); + ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], + interp_factor1, model, x1, x2, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, + H2Actual, H3Actual); F.block<2, 6>(6, 0) = H1Actual; F.block<2, 6>(6, 6) = H2Actual; E.block<2, 3>(6, 0) = H3Actual; // whiten - F = (1/sigma) * F; - E = (1/sigma) * E; - b = (1/sigma) * b; + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; //* G = F' * F - F' * E * P * E' * F Matrix P = (E.transpose() * E).inverse(); - Matrix expectedHessian = F.transpose() * F - - (F.transpose() * E * P * E.transpose() * F); + Matrix expectedHessian = + F.transpose() * F - (F.transpose() * E * P * E.transpose() * F); EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); // ==== check Information vector of smartFactor1 ===== GaussianFactorGraph gfg; gfg.add(linearfactor1); Matrix actualHessian_v2 = gfg.hessian().first; - EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + EXPECT(assert_equal(actualHessian_v2, actualHessian, + 1e-6)); // sanity check on hessian // -- compute actual information vector Vector actualInfoVector = gfg.hessian().second; - // -- compute expected information vector from manual Schur complement from Jacobians + // -- compute expected information vector from manual Schur complement from + // Jacobians //* g = F' * (b - E * P * E' * b) Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); @@ -902,8 +992,8 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessianComparedToProjFactorsRolli } /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionPoseFactorRollingShutter, + optimization_3poses_measurementsFromSamePose) { using namespace vanillaPoseRS; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -913,27 +1003,32 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector> key_pairs; - key_pairs.push_back(std::make_pair(x1,x2)); - key_pairs.push_back(std::make_pair(x2,x3)); - key_pairs.push_back(std::make_pair(x3,x1)); + std::vector> key_pairs; + key_pairs.push_back(std::make_pair(x1, x2)); + key_pairs.push_back(std::make_pair(x2, x3)); + key_pairs.push_back(std::make_pair(x3, x1)); std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - // For first factor, we create redundant measurement (taken by the same keys as factor 1, to - // make sure the redundancy in the keys does not create problems) + // For first factor, we create redundant measurement (taken by the same keys + // as factor 1, to make sure the redundancy in the keys does not create + // problems) Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; - measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement - std::vector> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back(key_pairs.at(0)); // we readd the first pair of keys + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + std::vector> key_pairs_redundant = key_pairs; + key_pairs_redundant.push_back( + key_pairs.at(0)); // we readd the first pair of keys std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back(interp_factors.at(0));// we readd the first interp factor + interp_factors_redundant.push_back( + interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant, sharedK); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant, sharedK); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); @@ -956,20 +1051,22 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF groundTruth.insert(x3, pose_above); 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/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 + Point3(0.1, 0.1, 0.1)); // smaller noise Values values; values.insert(x1, level_pose); values.insert(x2, pose_right); - // initialize third pose with some noise, we expect it to move back to original pose_above + // initialize third pose with some noise, we expect it to move back to + // original pose_above values.insert(x3, pose_above * noise_pose); - EXPECT( // check that the pose is actually noisy - 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))); + EXPECT( // check that the pose is actually noisy + 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); @@ -980,11 +1077,11 @@ TEST( SmartProjectionPoseFactorRollingShutter, optimization_3poses_measurementsF #ifndef DISABLE_TIMING #include // -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +// children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionPoseFactorRollingShutter, timing ) { - +TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1007,14 +1104,14 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { size_t nrTests = 1000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, + sharedKSimple, body_P_sensorId); Values values; values.insert(x1, pose1); @@ -1024,7 +1121,7 @@ TEST( SmartProjectionPoseFactorRollingShutter, timing ) { gttoc_(SF_RS_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1046,4 +1143,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From 9798bfa8151788b31e225be9acce0ca4c4d30b10 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:46:12 -0400 Subject: [PATCH 50/50] Cleaned up interpolate --- gtsam/base/Lie.h | 28 ++++++++++++++-------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 34422edd75..ac7c2a9a5b 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -320,28 +320,28 @@ T expm(const Vector& x, int K=7) { } /** - * 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. + * 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. */ template T interpolate(const T& X, const T& Y, double t, typename MakeOptionalJacobian::type Hx = boost::none, typename MakeOptionalJacobian::type Hy = boost::none) { - if (Hx || Hy) { - typename traits::TangentVector log_Xinv_Y; typename MakeJacobian::type between_H_x, log_H, exp_H, compose_H_x; - - T Xinv_Y = traits::Between(X, Y, between_H_x); // between_H_y = identity - log_Xinv_Y = traits::Logmap(Xinv_Y, log_H); - Xinv_Y = traits::Expmap(t * log_Xinv_Y, exp_H); - Xinv_Y = traits::Compose(X, Xinv_Y, compose_H_x); // compose_H_xinv_y = identity - - if(Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; - if(Hy) *Hy = t * exp_H * log_H; - return Xinv_Y; + const T between = + traits::Between(X, Y, between_H_x); // between_H_y = identity + typename traits::TangentVector delta = traits::Logmap(between, log_H); + const T Delta = traits::Expmap(t * delta, exp_H); + const T result = traits::Compose( + X, Delta, compose_H_x); // compose_H_xinv_y = identity + + if (Hx) *Hx = compose_H_x + t * exp_H * log_H * between_H_x; + if (Hy) *Hy = t * exp_H * log_H; + return result; } - return traits::Compose(X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); + return traits::Compose( + X, traits::Expmap(t * traits::Logmap(traits::Between(X, Y)))); } /**