From bafcde9ee195216a0b4bae1381bfa74e59f45fad Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 28 Aug 2021 17:36:14 -0400 Subject: [PATCH] Google-style formatting in new files. --- gtsam/geometry/CameraSet.h | 41 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 173 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 230 ++++---- .../testProjectionFactorRollingShutter.cpp | 258 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 494 +++++++++++------- 6 files changed, 695 insertions(+), 540 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 58122e33e..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -218,48 +218,52 @@ public: 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 @@ public: } 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 5fc1c05eb..c92a13daf 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 ed56ad8f3..c92653c13 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 943e350d4..161c9aa55 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, ConstructorWithTransform) { +TEST(ProjectionFactorRollingShutter, Constructor) { ProjectionFactorRollingShutter factor(measurement, interp_params, model, - poseKey1, poseKey2, pointKey, K, body_P_sensor); + 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); +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 (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 a7441e170..0b94d2c3f 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)); + 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); + 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)); + 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); + 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); } /* ************************************************************************* */ -