From be556354682c151feebf206de0f8155c2478c0e9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 24 Aug 2021 13:36:45 -0400 Subject: [PATCH 001/110] starting to make templates for smart projection factors uniform (all on cameras) --- gtsam/slam/SmartProjectionPoseFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index c7b1d5424..8a70c5ec3 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -41,7 +41,7 @@ namespace gtsam { * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ -template +template class SmartProjectionPoseFactor: public SmartProjectionFactor< PinholePose > { From cf3cf396838cbe7b8dbb2e0ae4feae10a48be2bb Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 24 Aug 2021 20:35:19 -0400 Subject: [PATCH 002/110] investigating potential templating alternatives --- gtsam/slam/SmartProjectionPoseFactor.h | 54 +++++++++++++++++++------- 1 file changed, 39 insertions(+), 15 deletions(-) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 8a70c5ec3..5abfcc312 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionPoseFactor.h + * @file SmartProjectionPoseFactorC.h * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall @@ -42,13 +42,13 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactor: public SmartProjectionFactor< - PinholePose > { +class SmartProjectionPoseFactorC: public SmartProjectionFactor { private: - typedef PinholePose Camera; - typedef SmartProjectionFactor Base; - typedef SmartProjectionPoseFactor This; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactorC This; + typedef CAMERA Camera; + typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -62,7 +62,7 @@ public: /** * Default constructor, only for serialization */ - SmartProjectionPoseFactor() {} + SmartProjectionPoseFactorC() {} /** * Constructor @@ -70,7 +70,7 @@ public: * @param K (fixed) calibration, assumed to be the same for all cameras * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactor( + SmartProjectionPoseFactorC( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const SmartProjectionParams& params = SmartProjectionParams()) @@ -84,17 +84,17 @@ public: * @param body_P_sensor pose of the camera in the body frame (optional) * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactor( + SmartProjectionPoseFactorC( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const boost::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) - : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { + : SmartProjectionPoseFactorC(sharedNoiseModel, K, params) { this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ - ~SmartProjectionPoseFactor() override { + ~SmartProjectionPoseFactorC() override { } /** @@ -104,7 +104,7 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionPoseFactor, z = \n "; + std::cout << s << "SmartProjectionPoseFactorC, z = \n "; Base::print("", keyFormatter); } @@ -161,9 +161,33 @@ public: // end of class declaration /// traits -template -struct traits > : public Testable< - SmartProjectionPoseFactor > { +template +struct traits > : public Testable< + SmartProjectionPoseFactorC > { }; +// legacy smart factor class, only templated on calibration and assuming pinhole +template using SmartProjectionPoseFactor = SmartProjectionPoseFactorC< PinholePose >; + +//template +//using SmartProjectionPoseFactor = SmartProjectionPoseFactorC< PinholePose >; + +//template +//struct SmartProjectionPoseFactor{ +// typedef SmartProjectionPoseFactorC< PinholePose >; +//}; + +//template +//class SmartProjectionPoseFactor{ +// typedef SmartProjectionPoseFactorC< PinholePose >; +//}; + +//typedef typename CAMERA::CalibrationType CALIBRATION; +//template +//class SmartProjectionPoseFactor: public SmartProjectionPoseFactorC< > { +// public: +// private: +//}; +// end + } // \ namespace gtsam From db2a9151e5c35ca84c150852d4d6a8f5c29afe21 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 24 Aug 2021 20:56:40 -0400 Subject: [PATCH 003/110] don't like it - going to create a different class --- gtsam/slam/SmartProjectionPoseFactor.h | 45 +++++-------------- .../slam/SmartStereoProjectionFactor.h | 2 +- 2 files changed, 13 insertions(+), 34 deletions(-) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 5abfcc312..8731eaec4 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionPoseFactorC.h + * @file SmartProjectionFactorP.h * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall @@ -42,11 +42,11 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionPoseFactorC: public SmartProjectionFactor { +class SmartProjectionFactorP: public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; - typedef SmartProjectionPoseFactorC This; + typedef SmartProjectionFactorP This; typedef CAMERA Camera; typedef typename CAMERA::CalibrationType CALIBRATION; @@ -62,7 +62,7 @@ public: /** * Default constructor, only for serialization */ - SmartProjectionPoseFactorC() {} + SmartProjectionFactorP() {} /** * Constructor @@ -70,7 +70,7 @@ public: * @param K (fixed) calibration, assumed to be the same for all cameras * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactorC( + SmartProjectionFactorP( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const SmartProjectionParams& params = SmartProjectionParams()) @@ -84,17 +84,17 @@ public: * @param body_P_sensor pose of the camera in the body frame (optional) * @param params parameters for the smart projection factors */ - SmartProjectionPoseFactorC( + SmartProjectionFactorP( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const boost::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) - : SmartProjectionPoseFactorC(sharedNoiseModel, K, params) { + : SmartProjectionFactorP(sharedNoiseModel, K, params) { this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ - ~SmartProjectionPoseFactorC() override { + ~SmartProjectionFactorP() override { } /** @@ -104,7 +104,7 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionPoseFactorC, z = \n "; + std::cout << s << "SmartProjectionFactorP, z = \n "; Base::print("", keyFormatter); } @@ -162,32 +162,11 @@ public: /// traits template -struct traits > : public Testable< - SmartProjectionPoseFactorC > { +struct traits > : public Testable< + SmartProjectionFactorP > { }; // legacy smart factor class, only templated on calibration and assuming pinhole -template using SmartProjectionPoseFactor = SmartProjectionPoseFactorC< PinholePose >; - -//template -//using SmartProjectionPoseFactor = SmartProjectionPoseFactorC< PinholePose >; - -//template -//struct SmartProjectionPoseFactor{ -// typedef SmartProjectionPoseFactorC< PinholePose >; -//}; - -//template -//class SmartProjectionPoseFactor{ -// typedef SmartProjectionPoseFactorC< PinholePose >; -//}; - -//typedef typename CAMERA::CalibrationType CALIBRATION; -//template -//class SmartProjectionPoseFactor: public SmartProjectionPoseFactorC< > { -// public: -// private: -//}; -// end +template using SmartProjectionPoseFactor = SmartProjectionFactorP< PinholePose >; } // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 52fd99356..e361dddac 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file SmartStereoProjectionFactor.h - * @brief Smart stereo factor on StereoCameras (pose + calibration) + * @brief Smart stereo factor on StereoCameras (pose) * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert From 8d9ede82853eb20230d72f03e02dd0e17c1ea86e Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 20:35:18 -0400 Subject: [PATCH 004/110] reverted all changes back to master --- gtsam/slam/SmartProjectionPoseFactor.h | 35 ++++++++++++-------------- 1 file changed, 16 insertions(+), 19 deletions(-) diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 8731eaec4..c7b1d5424 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionFactorP.h + * @file SmartProjectionPoseFactor.h * @brief Smart factor on poses, assuming camera calibration is fixed * @author Luca Carlone * @author Chris Beall @@ -41,14 +41,14 @@ namespace gtsam { * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM */ -template -class SmartProjectionFactorP: public SmartProjectionFactor { +template +class SmartProjectionPoseFactor: public SmartProjectionFactor< + PinholePose > { private: - typedef SmartProjectionFactor Base; - typedef SmartProjectionFactorP This; - typedef CAMERA Camera; - typedef typename CAMERA::CalibrationType CALIBRATION; + typedef PinholePose Camera; + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactor This; protected: @@ -62,7 +62,7 @@ public: /** * Default constructor, only for serialization */ - SmartProjectionFactorP() {} + SmartProjectionPoseFactor() {} /** * Constructor @@ -70,7 +70,7 @@ public: * @param K (fixed) calibration, assumed to be the same for all cameras * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( + SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const SmartProjectionParams& params = SmartProjectionParams()) @@ -84,17 +84,17 @@ public: * @param body_P_sensor pose of the camera in the body frame (optional) * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( + SmartProjectionPoseFactor( const SharedNoiseModel& sharedNoiseModel, const boost::shared_ptr K, const boost::optional body_P_sensor, const SmartProjectionParams& params = SmartProjectionParams()) - : SmartProjectionFactorP(sharedNoiseModel, K, params) { + : SmartProjectionPoseFactor(sharedNoiseModel, K, params) { this->body_P_sensor_ = body_P_sensor; } /** Virtual destructor */ - ~SmartProjectionFactorP() override { + ~SmartProjectionPoseFactor() override { } /** @@ -104,7 +104,7 @@ public: */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionFactorP, z = \n "; + std::cout << s << "SmartProjectionPoseFactor, z = \n "; Base::print("", keyFormatter); } @@ -161,12 +161,9 @@ public: // end of class declaration /// traits -template -struct traits > : public Testable< - SmartProjectionFactorP > { +template +struct traits > : public Testable< + SmartProjectionPoseFactor > { }; -// legacy smart factor class, only templated on calibration and assuming pinhole -template using SmartProjectionPoseFactor = SmartProjectionFactorP< PinholePose >; - } // \ namespace gtsam From 432921e3627c857b2c60cb00449dcbc2bf7e2efa Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 20:35:31 -0400 Subject: [PATCH 005/110] and created a new factor instead - moving to testing now --- gtsam/slam/SmartProjectionFactorP.h | 216 +++ .../slam/tests/testSmartProjectionFactorP.cpp | 1382 +++++++++++++++++ 2 files changed, 1598 insertions(+) create mode 100644 gtsam/slam/SmartProjectionFactorP.h create mode 100644 gtsam/slam/tests/testSmartProjectionFactorP.cpp diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h new file mode 100644 index 000000000..030ec65f2 --- /dev/null +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -0,0 +1,216 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartProjectionFactorP.h + * @brief Smart factor on poses, assuming camera calibration is fixed. + * Same as SmartProjectionPoseFactor, except: + * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) + * - it admits a different calibration for each measurement (i.e., it can model a multi-camera system) + * @author Luca Carlone + * @author Chris Beall + * @author Zsolt Kira + */ + +#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 assumes that camera calibration is fixed (but each camera + * measurement can have a different extrinsic and intrinsic calibration). + * The factor only constrains poses (variable dimension is 6). + * This factor requires that values contains the involved poses (Pose3). + * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! + * If the calibration should be optimized, as well, use SmartProjectionFactor instead! + * @addtogroup SLAM + */ +template +class SmartProjectionFactorP: public SmartProjectionFactor { + +private: + typedef SmartProjectionFactor Base; + typedef SmartProjectionFactorP This; + typedef CAMERA Camera; + typedef typename CAMERA::CalibrationType CALIBRATION; + +protected: + + /// shared pointer to calibration object (one for each observation) + std::vector > K_all_; + + /// Pose of the camera in the body frame (one for each observation) + std::vector body_P_sensors_; + +public: + + /// shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptr; + + /// Default constructor, only for serialization + SmartProjectionFactorP() {} + + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param params parameters for the smart projection factors + */ + SmartProjectionFactorP( + const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + } + + /** Virtual destructor */ + ~SmartProjectionFactorP() override { + } + + /** + * add a new measurement, corresponding to an observation from pose "poseKey" whose camera + * has intrinsic calibration K and extrinsic calibration body_P_sensor. + * @param measured 2-dimensional location of the projection of a + * single landmark in a single view (the measurement) + * @param poseKey key corresponding to the body pose of the camera taking the measurement + * @param K (fixed) camera intrinsic calibration + * @param body_P_sensor (fixed) camera extrinsic calibration + */ + void add(const Point2& measured, const Key& poseKey, + const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + // store measurement and key + this->measured_.push_back(measured); + this->keys_.push_back(key); + // store fixed intrinsic calibration + K_all_.push_back(K); + // store fixed extrinsics of the camera + body_P_sensors_.push_back(body_P_sensor); + } + + /** + * 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 poseKeys keys corresponding to the body poses of the cameras taking the measurements + * @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& poseKeys, + const std::vector>& Ks, + const std::vector body_P_sensors) { + assert(poseKeys.size() == measurements.size()); + assert(poseKeys.size() == Ks.size()); + assert(poseKeys.size() == body_P_sensors.size()); + for (size_t i = 0; i < measurements.size(); i++) { + add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); + } + } + + /// return the calibration object + inline std::vector> calibration() const { + return K_all_; + } + + /// return the extrinsic camera calibration 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 { + std::cout << s << "SmartProjectionFactorP: \n "; + for (size_t i = 0; i < K_all_.size(); i++) { + std::cout << "-- Measurement nr " << i << std::endl; + body_P_sensors_[i].print("extrinsic calibration:\n"); + K_all_[i]->print("intrinsic calibration = "); + } + Base::print("", keyFormatter); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This *e = dynamic_cast(&p); + 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() + && 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 vector of cameras + */ + typename Base::Cameras cameras(const Values& values) const override { + typename Base::Cameras cameras; + for (const Key& k : this->keys_) { + const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3 world_P_sensor_k = values.at(k) * body_P_cam; + cameras.emplace_back(world_P_sensor_k, K_all_[i]); + } + return cameras; + } + + 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_); + } + +}; +// end of class declaration + +/// traits +template +struct traits > : public Testable< + SmartProjectionFactorP > { +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp new file mode 100644 index 000000000..c7f220c10 --- /dev/null +++ b/gtsam/slam/tests/testSmartProjectionFactorP.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 37dfb0c3dad40b17c5c108c2728fc3672ebb9010 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 21:05:56 -0400 Subject: [PATCH 006/110] compiles, testing now --- gtsam/slam/SmartProjectionFactorP.h | 9 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../slam/tests/testSmartProjectionFactorP.cpp | 2647 ++++++++--------- 3 files changed, 1329 insertions(+), 1328 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 030ec65f2..1d362d3d5 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -98,7 +98,7 @@ public: const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { // store measurement and key this->measured_.push_back(measured); - this->keys_.push_back(key); + this->keys_.push_back(poseKey); // store fixed intrinsic calibration K_all_.push_back(K); // store fixed extrinsics of the camera @@ -186,9 +186,9 @@ public: */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - for (const Key& k : this->keys_) { + for (const Key& i : this->keys_) { const Pose3& body_P_cam = body_P_sensors_[i]; - const Pose3 world_P_sensor_k = values.at(k) * body_P_cam; + const Pose3 world_P_sensor_k = values.at(i) * body_P_cam; cameras.emplace_back(world_P_sensor_k, K_all_[i]); } return cameras; @@ -201,7 +201,8 @@ public: template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensors_); } }; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 4abc59305..1a64efc5c 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include #include diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index c7f220c10..dd2d769e6 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -10,17 +10,16 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactor.cpp - * @brief Unit tests for ProjectionFactor Class + * @file testSmartProjectionFactorP.cpp + * @brief Unit tests for SmartProjectionFactorP Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira * @author Frank Dellaert - * @date Sept 2013 + * @date August 2021 */ #include "smartFactorScenarios.h" -#include #include #include #include @@ -52,1326 +51,1326 @@ 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)); -} +///* ************************************************************************* */ +//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() { From df82ca1b0c09e3e01c63107012a3cd149e4fa013 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 21:24:37 -0400 Subject: [PATCH 007/110] fixed bug --- gtsam/slam/SmartProjectionFactorP.h | 9 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../slam/tests/testSmartProjectionFactorP.cpp | 337 +++++++++--------- 3 files changed, 169 insertions(+), 178 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 1d362d3d5..34a8aa06a 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -186,10 +186,11 @@ public: */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - for (const Key& i : this->keys_) { - const Pose3& body_P_cam = body_P_sensors_[i]; - const Pose3 world_P_sensor_k = values.at(i) * body_P_cam; - cameras.emplace_back(world_P_sensor_k, K_all_[i]); + for (size_t i = 0; i < this->keys_.size(); i++) { + const Pose3& body_P_cam_i = body_P_sensors_[i]; + const Pose3 world_P_sensor_i = values.at(this->keys_[i]) + * body_P_cam_i; + cameras.emplace_back(world_P_sensor_i, K_all_[i]); } return cameras; } diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 1a64efc5c..1a16485e0 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -70,6 +70,7 @@ SmartProjectionParams params; namespace vanillaPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionFactorP SmartFactorP; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index dd2d769e6..1f5457fb8 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -51,87 +51,76 @@ 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); +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, Constructor) { + using namespace vanillaPose; + SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); +} + +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, Constructor2) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, Constructor3) { + using namespace vanillaPose; + SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); + factor1->add(measurement1, x1, sharedK); +} + +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, Constructor4) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, params); + factor1.add(measurement1, x1, sharedK); +} + +/* ************************************************************************* */ +TEST( SmartProjectionFactorP, Equals ) { + using namespace vanillaPose; + SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); + factor1->add(measurement1, x1, sharedK); + + SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); + factor2->add(measurement1, x1, sharedK); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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); + + SmartFactorP factor(model); + factor.add(level_uv, x1, sharedK); + factor.add(level_uv_right, x2, sharedK); + + 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); + +// SmartFactorP::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); +// std::bind(&SmartFactorP::whitenedError, factor, cameras, std::placeholders::_1); // // // Calculate using computeEP // Matrix actualE; @@ -146,7 +135,7 @@ LevenbergMarquardtParams lmParams; // EXPECT(assert_equal(expectedE, actualE, 1e-7)); // // // Calculate using reprojectionError -// SmartFactor::Cameras::FBlocks F; +// SmartFactorP::Cameras::FBlocks F; // Matrix E; // Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); // EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -155,15 +144,15 @@ LevenbergMarquardtParams lmParams; // // // Calculate using computeJacobians // Vector b; -// SmartFactor::FBlocks Fs; +// SmartFactorP::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 ) { +//TEST( SmartProjectionFactorP, noisy ) { // // using namespace vanillaPose; // @@ -178,13 +167,13 @@ LevenbergMarquardtParams lmParams; // Point3(0.5, 0.1, 0.3)); // values.insert(x2, pose_right.compose(noise_pose)); // -// SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK)); +// SmartFactorP::shared_ptr factor(new SmartFactorP(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)); +// SmartFactorP::shared_ptr factor2(new SmartFactorP(model, sharedK)); // Point2Vector measurements; // measurements.push_back(level_uv); // measurements.push_back(level_uv_right); @@ -197,7 +186,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { +//TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { // using namespace vanillaPose; // // // create arbitrary body_T_sensor (transforms from sensor to body) @@ -227,13 +216,13 @@ LevenbergMarquardtParams lmParams; // params.setDegeneracyMode(IGNORE_DEGENERACY); // params.setEnableEPI(false); // -// SmartFactor smartFactor1(model, sharedK, body_T_sensor, params); +// SmartFactorP smartFactor1(model, sharedK, body_T_sensor, params); // smartFactor1.add(measurements_cam1, views); // -// SmartFactor smartFactor2(model, sharedK, body_T_sensor, params); +// SmartFactorP smartFactor2(model, sharedK, body_T_sensor, params); // smartFactor2.add(measurements_cam2, views); // -// SmartFactor smartFactor3(model, sharedK, body_T_sensor, params); +// SmartFactorP smartFactor3(model, sharedK, body_T_sensor, params); // smartFactor3.add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -272,7 +261,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { +//TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { // // using namespace vanillaPose2; // Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -287,13 +276,13 @@ LevenbergMarquardtParams lmParams; // views.push_back(x2); // views.push_back(x3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2)); +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK2)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2)); +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedK2)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -333,7 +322,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, Factors ) { +//TEST( SmartProjectionFactorP, Factors ) { // // using namespace vanillaPose; // @@ -355,10 +344,10 @@ LevenbergMarquardtParams lmParams; // // Create smart factors // KeyVector views {x1, x2}; // -// SmartFactor::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); +// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::Cameras cameras; +// SmartFactorP::Cameras cameras; // cameras.push_back(cam1); // cameras.push_back(cam2); // @@ -435,7 +424,7 @@ LevenbergMarquardtParams lmParams; // E(2, 0) = 10; // E(2, 2) = 1; // E(3, 1) = 10; -// SmartFactor::FBlocks Fs = list_of(F1)(F2); +// SmartFactorP::FBlocks Fs = list_of(F1)(F2); // Vector b(4); // b.setZero(); // @@ -497,7 +486,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { +//TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { // // using namespace vanillaPose; // @@ -510,13 +499,13 @@ LevenbergMarquardtParams lmParams; // projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK)); +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK)); +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK)); +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedK)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -551,7 +540,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, jacobianSVD ) { +//TEST( SmartProjectionFactorP, jacobianSVD ) { // // using namespace vanillaPose; // @@ -569,18 +558,18 @@ LevenbergMarquardtParams lmParams; // params.setLinearizationMode(gtsam::JACOBIAN_SVD); // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // params.setEnableEPI(false); -// SmartFactor factor1(model, sharedK, params); +// SmartFactorP factor1(model, sharedK, params); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -607,7 +596,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, landmarkDistance ) { +//TEST( SmartProjectionFactorP, landmarkDistance ) { // // using namespace vanillaPose; // @@ -629,16 +618,16 @@ LevenbergMarquardtParams lmParams; // params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); // params.setEnableEPI(false); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -666,7 +655,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { +//TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { // // using namespace vanillaPose; // @@ -693,20 +682,20 @@ LevenbergMarquardtParams lmParams; // params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); // params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // smartFactor3->add(measurements_cam3, views); // -// SmartFactor::shared_ptr smartFactor4( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor4( +// new SmartFactorP(model, sharedK, params)); // smartFactor4->add(measurements_cam4, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -732,7 +721,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, jacobianQ ) { +//TEST( SmartProjectionFactorP, jacobianQ ) { // // using namespace vanillaPose; // @@ -748,16 +737,16 @@ LevenbergMarquardtParams lmParams; // SmartProjectionParams params; // params.setLinearizationMode(gtsam::JACOBIAN_Q); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -783,7 +772,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { +//TEST( SmartProjectionFactorP, 3poses_projection_factor ) { // // using namespace vanillaPose2; // @@ -830,7 +819,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, CheckHessian) { +//TEST( SmartProjectionFactorP, CheckHessian) { // // KeyVector views {x1, x2, x3}; // @@ -853,16 +842,16 @@ LevenbergMarquardtParams lmParams; // SmartProjectionParams params; // params.setRankTolerance(10); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); // HESSIAN, by default +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); // HESSIAN, by default +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); // HESSIAN, by default +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default // smartFactor3->add(measurements_cam3, views); // // NonlinearFactorGraph graph; @@ -912,7 +901,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ) { +//TEST( SmartProjectionFactorP, 3poses_2land_rotation_only_smart_projection_factor ) { // using namespace vanillaPose2; // // KeyVector views {x1, x2, x3}; @@ -933,12 +922,12 @@ LevenbergMarquardtParams lmParams; // params.setRankTolerance(50); // params.setDegeneracyMode(gtsam::HANDLE_INFINITY); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK2, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK2, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK2, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK2, params)); // smartFactor2->add(measurements_cam2, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -966,7 +955,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { +//TEST( SmartProjectionFactorP, 3poses_rotation_only_smart_projection_factor ) { // // // this test considers a condition in which the cheirality constraint is triggered // using namespace vanillaPose; @@ -991,16 +980,16 @@ LevenbergMarquardtParams lmParams; // params.setRankTolerance(10); // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedK, params)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1049,7 +1038,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, Hessian ) { +//TEST( SmartProjectionFactorP, Hessian ) { // // using namespace vanillaPose2; // @@ -1062,7 +1051,7 @@ LevenbergMarquardtParams lmParams; // measurements_cam1.push_back(cam1_uv1); // measurements_cam1.push_back(cam2_uv1); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2)); +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2)); // smartFactor1->add(measurements_cam1, views); // // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), @@ -1080,8 +1069,8 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, HessianWithRotation ) { -// // cout << " ************************ SmartProjectionPoseFactor: rotated Hessian **********************" << endl; +//TEST( SmartProjectionFactorP, HessianWithRotation ) { +// // cout << " ************************ SmartProjectionFactorP: rotated Hessian **********************" << endl; // // using namespace vanillaPose; // @@ -1091,7 +1080,7 @@ LevenbergMarquardtParams lmParams; // // projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); // -// SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK)); +// SmartFactorP::shared_ptr smartFactorInstance(new SmartFactorP(model, sharedK)); // smartFactorInstance->add(measurements_cam1, views); // // Values values; @@ -1131,7 +1120,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { +//TEST( SmartProjectionFactorP, HessianWithRotationDegenerate ) { // // using namespace vanillaPose2; // @@ -1144,7 +1133,7 @@ LevenbergMarquardtParams lmParams; // Point2Vector measurements_cam1; // projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); // -// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2)); +// SmartFactorP::shared_ptr smartFactor(new SmartFactorP(model, sharedK2)); // smartFactor->add(measurements_cam1, views); // // Values values; @@ -1183,16 +1172,16 @@ LevenbergMarquardtParams lmParams; //} // ///* ************************************************************************* */ -//TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { +//TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { // using namespace bundlerPose; // SmartProjectionParams params; // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactor factor(model, sharedBundlerK, params); +// SmartFactorP factor(model, sharedBundlerK, params); // factor.add(measurement1, x1); //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, Cal3Bundler ) { +//TEST( SmartProjectionFactorP, Cal3Bundler ) { // // using namespace bundlerPose; // @@ -1208,13 +1197,13 @@ LevenbergMarquardtParams lmParams; // // KeyVector views {x1, x2, x3}; // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK)); +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedBundlerK)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK)); +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedBundlerK)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK)); +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, sharedBundlerK)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1248,7 +1237,7 @@ LevenbergMarquardtParams lmParams; //} // ///* *************************************************************************/ -//TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { +//TEST( SmartProjectionFactorP, Cal3BundlerRotationOnly ) { // // using namespace bundlerPose; // @@ -1275,16 +1264,16 @@ LevenbergMarquardtParams lmParams; // params.setRankTolerance(10); // params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); // -// SmartFactor::shared_ptr smartFactor1( -// new SmartFactor(model, sharedBundlerK, params)); +// SmartFactorP::shared_ptr smartFactor1( +// new SmartFactorP(model, sharedBundlerK, params)); // smartFactor1->add(measurements_cam1, views); // -// SmartFactor::shared_ptr smartFactor2( -// new SmartFactor(model, sharedBundlerK, params)); +// SmartFactorP::shared_ptr smartFactor2( +// new SmartFactorP(model, sharedBundlerK, params)); // smartFactor2->add(measurements_cam2, views); // -// SmartFactor::shared_ptr smartFactor3( -// new SmartFactor(model, sharedBundlerK, params)); +// SmartFactorP::shared_ptr smartFactor3( +// new SmartFactorP(model, sharedBundlerK, params)); // smartFactor3->add(measurements_cam3, views); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1340,25 +1329,25 @@ LevenbergMarquardtParams lmParams; //BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); //BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // -//TEST(SmartProjectionPoseFactor, serialize) { +//TEST(SmartProjectionFactorP, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// SmartFactor factor(model, sharedK, params); +// SmartFactorP factor(model, sharedK, params); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); // EXPECT(equalsBinary(factor)); //} // -//TEST(SmartProjectionPoseFactor, serialize2) { +//TEST(SmartProjectionFactorP, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); // Pose3 bts; -// SmartFactor factor(model, sharedK, bts, params); +// SmartFactorP factor(model, sharedK, bts, params); // // // insert some measurments // KeyVector key_view; From 492c2b8561f7b8307a256c1edcd3b2f38a2e2ba4 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 21:39:57 -0400 Subject: [PATCH 008/110] progress on tests --- gtsam/slam/SmartProjectionFactorP.h | 51 +++--- .../slam/tests/testSmartProjectionFactorP.cpp | 146 +++++++++--------- 2 files changed, 106 insertions(+), 91 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 34a8aa06a..a215890bc 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -46,15 +46,15 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionFactorP: public SmartProjectionFactor { +class SmartProjectionFactorP : public SmartProjectionFactor { -private: + private: typedef SmartProjectionFactor Base; typedef SmartProjectionFactorP This; typedef CAMERA Camera; typedef typename CAMERA::CalibrationType CALIBRATION; -protected: + protected: /// shared pointer to calibration object (one for each observation) std::vector > K_all_; @@ -62,22 +62,23 @@ protected: /// Pose of the camera in the body frame (one for each observation) std::vector body_P_sensors_; -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionFactorP() {} + SmartProjectionFactorP() { + } /** * Constructor * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param params parameters for the smart projection factors */ - SmartProjectionFactorP( - const SharedNoiseModel& sharedNoiseModel, - const SmartProjectionParams& params = SmartProjectionParams()) + SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel, + const SmartProjectionParams& params = + SmartProjectionParams()) : Base(sharedNoiseModel, params) { } @@ -95,7 +96,8 @@ public: * @param body_P_sensor (fixed) camera extrinsic calibration */ void add(const Point2& measured, const Key& poseKey, - const boost::shared_ptr& K, const Pose3 body_P_sensor = Pose3::identity()) { + const boost::shared_ptr& K, const Pose3 body_P_sensor = + Pose3::identity()) { // store measurement and key this->measured_.push_back(measured); this->keys_.push_back(poseKey); @@ -113,15 +115,17 @@ public: * @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& poseKeys, + void add(const Point2Vector& measurements, const std::vector& poseKeys, const std::vector>& Ks, - const std::vector body_P_sensors) { + const std::vector body_P_sensors = std::vector()) { assert(poseKeys.size() == measurements.size()); assert(poseKeys.size() == Ks.size()); - assert(poseKeys.size() == body_P_sensors.size()); for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); + if (poseKeys.size() == body_P_sensors.size()) { + add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); + } else { + add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics + } } } @@ -141,7 +145,7 @@ public: * @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 << "SmartProjectionFactorP: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; @@ -155,13 +159,16 @@ public: bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); 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; + 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; } + } else { + extrinsicCalibrationEqual = false; + } return e && Base::equals(p, tol) && K_all_ == e->calibration() && extrinsicCalibrationEqual; @@ -173,7 +180,7 @@ public: double error(const Values& values) const override { if (this->active(values)) { return this->totalReprojectionError(cameras(values)); - } else { // else of active flag + } else { // else of active flag return 0.0; } } @@ -215,4 +222,4 @@ struct traits > : public Testable< SmartProjectionFactorP > { }; -} // \ namespace gtsam +} // \ namespace gtsam diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 1f5457fb8..3ad4d62b6 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -114,77 +114,85 @@ TEST( SmartProjectionFactorP, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -// SmartFactorP::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(&SmartFactorP::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 -// SmartFactorP::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; -// SmartFactorP::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); + SmartFactorP::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(&SmartFactorP::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 + SmartFactorP::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; + SmartFactorP::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( SmartProjectionFactorP, 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)); + + SmartFactorP::shared_ptr factor(new SmartFactorP(model)); + factor->add(level_uv, x1, sharedK); + factor->add(level_uv_right, x2, sharedK); + + double actualError1 = factor->error(values); + + SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + std::vector body_P_sensors; + body_P_sensors.push_back(Pose3::identity()); + body_P_sensors.push_back(Pose3::identity()); + + KeyVector views {x1, x2}; + + factor2->add(measurements, views, sharedKs, body_P_sensors); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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)); -// -// SmartFactorP::shared_ptr factor(new SmartFactorP(model, sharedK)); -// factor->add(level_uv, x1); -// factor->add(level_uv_right, x2); -// -// double actualError1 = factor->error(values); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(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(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { // using namespace vanillaPose; From 7b399a363a9b6b64476d72bdb481cd3cb1d64db9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 22:22:21 -0400 Subject: [PATCH 009/110] removed line --- gtsam/slam/SmartFactorBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 0b0308c5c..6d493b533 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -393,7 +393,6 @@ protected: F.block(ZDim * i, Dim * i) = Fs.at(i); } - Pose3 body_P_sensor() const{ if(body_P_sensor_) return *body_P_sensor_; From b5236232772bdfcc0e327c155b0ff68bebbf3cf3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 22:22:53 -0400 Subject: [PATCH 010/110] still challenging to parse extrinsics --- gtsam/slam/SmartProjectionFactorP.h | 3 +- .../slam/tests/testSmartProjectionFactorP.cpp | 166 +++++++++--------- 2 files changed, 88 insertions(+), 81 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index a215890bc..495557c83 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -51,7 +51,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; typedef SmartProjectionFactorP This; - typedef CAMERA Camera; typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -63,6 +62,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::vector body_P_sensors_; public: + typedef CAMERA Camera; + typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 3ad4d62b6..4ba15264d 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -182,92 +182,98 @@ TEST( SmartProjectionFactorP, noisy ) { sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - std::vector body_P_sensors; - body_P_sensors.push_back(Pose3::identity()); - body_P_sensors.push_back(Pose3::identity()); - KeyVector views {x1, x2}; - factor2->add(measurements, views, sharedKs, body_P_sensors); + factor2->add(measurements, views, sharedKs); double actualError2 = factor2->error(values); DOUBLES_EQUAL(actualError1, actualError2, 1e-7); } -///* *************************************************************************/ -//TEST(SmartProjectionFactorP, 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); -// -// SmartFactorP smartFactor1(model, sharedK, body_T_sensor, params); -// smartFactor1.add(measurements_cam1, views); -// -// SmartFactorP smartFactor2(model, sharedK, body_T_sensor, params); -// smartFactor2.add(measurements_cam2, views); -// -// SmartFactorP 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(SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + std::vector body_T_sensors; + body_T_sensors.push_back(body_T_sensor); + body_T_sensors.push_back(body_T_sensor); + body_T_sensors.push_back(body_T_sensor); + + SmartFactorP smartFactor1(model, params); + smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); + + SmartFactorP smartFactor2(model, params); + smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); + + SmartFactorP smartFactor3(model, params); + smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);; + + 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( SmartProjectionFactorP, 3poses_smart_projection_factor ) { // From d004a8cd1e69b2c6f394f6e949bcd07273f3a52a Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 25 Aug 2021 23:36:39 -0400 Subject: [PATCH 011/110] tests finally passing! --- gtsam/slam/SmartProjectionFactorP.h | 127 ++++++++++++++++-- .../slam/tests/testSmartProjectionFactorP.cpp | 1 + 2 files changed, 117 insertions(+), 11 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 495557c83..6dd413346 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -53,6 +53,9 @@ class SmartProjectionFactorP : public SmartProjectionFactor { typedef SmartProjectionFactorP This; typedef typename CAMERA::CalibrationType CALIBRATION; + static const int DimPose = 6; ///< Pose3 dimension + static const int ZDim = 2; ///< Measurement dimension (Point2) + protected: /// shared pointer to calibration object (one for each observation) @@ -64,6 +67,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { public: typedef CAMERA Camera; typedef CameraSet Cameras; + typedef Eigen::Matrix MatrixZD; // F blocks + typedef std::vector > FBlocks; // vector of F blocks /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -175,17 +180,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { && 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 @@ -203,6 +197,117 @@ class SmartProjectionFactorP : public SmartProjectionFactor { return cameras; } + /** + * 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; + } + } + + /** + * 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 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, + const Cameras& cameras) const { + if (!this->result_) { + throw("computeJacobiansWithTriangulatedPoint"); + } else { // valid result: compute jacobians + b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); + for (size_t i = 0; i < Fs.size(); i++) { + const Pose3 sensor_P_body = body_P_sensors_[i].inverse(); + const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; + Eigen::Matrix H; + world_P_body.compose(body_P_sensors_[i], H); + Fs.at(i) = Fs.at(i) * H; + } + } + } + + /// 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 { + + size_t nrKeys = this->keys_.size(); + Cameras cameras = this->cameras(values); + + // Create structures for Hessian Factors + KeyVector js; + std::vector < Matrix > Gs(nrKeys * (nrKeys + 1) / 2); + std::vector < Vector > gs(nrKeys); + + if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera + throw std::runtime_error("SmartProjectionFactorP: " + "measured_.size() inconsistent with input"); + + // triangulate 3D point at given linearization point + this->triangulateSafe(cameras); + + 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); + } else { + throw std::runtime_error( + "SmartProjectionFactorP: " + "only supported degeneracy mode is ZERO_ON_DEGENERACY"); + } + } + + // compute Jacobian given triangulated 3D Point + FBlocks Fs; + Matrix E; + Vector b; + this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); + + // 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 + SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b); + return boost::make_shared < RegularHessianFactor + > (this->keys_, augmentedHessian); + } + + /** + * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for LM) + * @param values Values structure which must contain camera poses and extrinsic pose for this factor + * @return a Gaussian factor + */ + boost::shared_ptr linearizeDamped( + const Values& values, const double lambda = 0.0) const { + // depending on flag set on construction we may linearize to different linear factors + switch (this->params_.linearizationMode) { + case HESSIAN: + return this->createHessianFactor(values, lambda); + default: + throw std::runtime_error( + "SmartProjectioFactorP: unknown linearization mode"); + } + } + + /// linearize + boost::shared_ptr linearize(const Values& values) const + override { + return this->linearizeDamped(values); + } + private: /// Serialization function diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 4ba15264d..816ba6b8a 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -271,6 +271,7 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { Values result; LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); +// graph.print("graph\n"); EXPECT(assert_equal(wTb3, result.at(x3))); } From fe75554862b6cd535786c6ce393d2516d65e6a69 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 11:56:16 -0400 Subject: [PATCH 012/110] added capability to use multiple measurements from the same pose. unfortunately still had to define a non-templated function from cameraSet --- gtsam/geometry/CameraSet.h | 13 +++++++++ gtsam/slam/SmartProjectionFactorP.h | 44 ++++++++++++++++++++++------- 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index 3fc77bb2e..c09eba9bb 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -293,6 +293,19 @@ public: hessianKeys); } + /** + * non-templated version of function above + */ + static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_6_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,6,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/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 6dd413346..ccc67df44 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -15,6 +15,7 @@ * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) * - it admits a different calibration for each measurement (i.e., it can model a multi-camera system) + * - it allows multiple observations from the same pose/key (again, to model a multi-camera system) * @author Luca Carlone * @author Chris Beall * @author Zsolt Kira @@ -58,6 +59,9 @@ class SmartProjectionFactorP : public SmartProjectionFactor { protected: + /// vector of keys (one for each observation) with potentially repeated keys + std::vector nonUniqueKeys_; + /// shared pointer to calibration object (one for each observation) std::vector > K_all_; @@ -106,7 +110,12 @@ class SmartProjectionFactorP : public SmartProjectionFactor { Pose3::identity()) { // store measurement and key this->measured_.push_back(measured); - this->keys_.push_back(poseKey); + this->nonUniqueKeys_.push_back(poseKey); + + // 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(), poseKey) == this->keys_.end()) + this->keys_.push_back(poseKey); // add only unique keys + // store fixed intrinsic calibration K_all_.push_back(K); // store fixed extrinsics of the camera @@ -145,6 +154,11 @@ class SmartProjectionFactorP : public SmartProjectionFactor { return body_P_sensors_; } + /// return (for each observation) the (possibly non unique) keys involved in the measurements + const std::vector nonUniqueKeys() const { + return nonUniqueKeys_; + } + /** * print * @param s optional string naming the factor @@ -155,6 +169,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::cout << s << "SmartProjectionFactorP: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; + std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; body_P_sensors_[i].print("extrinsic calibration:\n"); K_all_[i]->print("intrinsic calibration = "); } @@ -177,7 +192,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } return e && Base::equals(p, tol) && K_all_ == e->calibration() - && extrinsicCalibrationEqual; + && nonUniqueKeys_ == e->nonUniqueKeys() && extrinsicCalibrationEqual; } /** @@ -188,9 +203,9 @@ class SmartProjectionFactorP : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - for (size_t i = 0; i < this->keys_.size(); i++) { + for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { const Pose3& body_P_cam_i = body_P_sensors_[i]; - const Pose3 world_P_sensor_i = values.at(this->keys_[i]) + const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) * body_P_cam_i; cameras.emplace_back(world_P_sensor_i, K_all_[i]); } @@ -237,13 +252,16 @@ class SmartProjectionFactorP : public SmartProjectionFactor { const Values& values, const double lambda = 0.0, bool diagonalDamping = false) const { - size_t nrKeys = this->keys_.size(); + // we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose), + // hence the number of unique keys may be smaller than nrMeasurements + size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys + Cameras cameras = this->cameras(values); // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrKeys * (nrKeys + 1) / 2); - std::vector < Vector > gs(nrKeys); + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera throw std::runtime_error("SmartProjectionFactorP: " @@ -279,10 +297,16 @@ class SmartProjectionFactorP : public SmartProjectionFactor { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } - // build augmented hessian - SymmetricBlockMatrix augmentedHessian = Cameras::SchurComplement(Fs, E, b); + Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + + // 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_6_6( + Fs, E, P, b, nonUniqueKeys_, this->keys_); + return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessian); + > (this->keys_, augmentedHessianUniqueKeys); } /** From 050d64bdca2e05e85bee15c82971a6cb8dc03fa2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 12:30:52 -0400 Subject: [PATCH 013/110] all tests pass except one on serialization --- gtsam/slam/SmartProjectionFactorP.h | 1 + gtsam/slam/tests/smartFactorScenarios.h | 2 + .../slam/tests/testSmartProjectionFactorP.cpp | 1658 ++++++----------- 3 files changed, 576 insertions(+), 1085 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index ccc67df44..559a294ac 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -340,6 +340,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { void serialize(ARCHIVE & ar, const unsigned int /*version*/) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(K_all_); + ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_); ar & BOOST_SERIALIZATION_NVP(body_P_sensors_); } diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 1a16485e0..7421f73af 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -84,6 +84,7 @@ Camera cam3(pose_above, sharedK); namespace vanillaPose2 { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionFactorP SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -113,6 +114,7 @@ typedef GeneralSFMFactor SFMFactor; namespace bundlerPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; +typedef SmartProjectionFactorP SmartFactorP; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 816ba6b8a..9ce3e56ac 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -275,1101 +275,589 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK2)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK2)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(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( SmartProjectionFactorP, 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}; -// -// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model, sharedK); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::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; -// SmartFactorP::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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedK)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedK)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(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( SmartProjectionFactorP, 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); -// SmartFactorP factor1(model, sharedK, params); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(model, sharedK, params)); -// smartFactor3->add(measurements_cam3, views); -// -// SmartFactorP::shared_ptr smartFactor4( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); // HESSIAN, by default -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK2, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(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( SmartProjectionFactorP, HessianWithRotation ) { -// // cout << " ************************ SmartProjectionFactorP: 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); -// -// SmartFactorP::shared_ptr smartFactorInstance(new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor(new SmartFactorP(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( SmartProjectionFactorP, ConstructorWithCal3Bundler) { -// using namespace bundlerPose; -// SmartProjectionParams params; -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactorP factor(model, sharedBundlerK, params); -// factor.add(measurement1, x1); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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}; -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, sharedBundlerK)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, sharedBundlerK)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(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( SmartProjectionFactorP, 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); -// -// SmartFactorP::shared_ptr smartFactor1( -// new SmartFactorP(model, sharedBundlerK, params)); -// smartFactor1->add(measurements_cam1, views); -// -// SmartFactorP::shared_ptr smartFactor2( -// new SmartFactorP(model, sharedBundlerK, params)); -// smartFactor2->add(measurements_cam2, views); -// -// SmartFactorP::shared_ptr smartFactor3( -// new SmartFactorP(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(SmartProjectionFactorP, serialize) { -// using namespace vanillaPose; -// using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor(model, sharedK, params); -// -// EXPECT(equalsObj(factor)); -// EXPECT(equalsXML(factor)); -// EXPECT(equalsBinary(factor)); -//} -// +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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); + + std::vector> sharedK2s; + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedK2s); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedK2s); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedK2s); + + 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( SmartProjectionFactorP, 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}; + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::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(); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr > actual = + smartFactor1->createHessianFactor(values, 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); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, dynamicOutlierRejection ) { + + using namespace vanillaPose; + + double excludeLandmarksFutherThanDist = 1e10; + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + + KeyVector views {x1, x2, x3}; + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // 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::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); + smartFactor3->add(measurements_cam3, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor4( + new SmartFactorP(model, params)); + smartFactor4->add(measurements_cam4, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + SmartFactorP::shared_ptr smartFactor1( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor2( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, sharedKs); + + SmartFactorP::shared_ptr smartFactor3( + new SmartFactorP(model, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, sharedKs); + + 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( SmartProjectionFactorP, 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); + + std::vector> sharedK2s; + sharedK2s.push_back(sharedK2); + sharedK2s.push_back(sharedK2); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedK2s); + + 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( SmartProjectionFactorP, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactorP factor(model, params); + factor.add(measurement1, x1, sharedBundlerK); +} + +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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}; + + std::vector> sharedBundlerKs; + sharedBundlerKs.push_back(sharedBundlerK); + sharedBundlerKs.push_back(sharedBundlerK); + sharedBundlerKs.push_back(sharedBundlerK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_cam1, views, sharedBundlerKs); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_cam2, views, sharedBundlerKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_cam3, views, sharedBundlerKs); + + 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)); +} + +/* ************************************************************************* */ +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(SmartProjectionFactorP, serialize) { + using namespace vanillaPose; + using namespace gtsam::serializationTestHelpers; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor(model, params); + + EXPECT(equalsObj(factor)); + EXPECT(equalsXML(factor)); + EXPECT(equalsBinary(factor)); +} + +// SERIALIZATION TEST FAILS: to be fixed //TEST(SmartProjectionFactorP, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// Pose3 bts; -// SmartFactorP factor(model, sharedK, bts, params); +// SmartFactorP factor(model, params); // -// // insert some measurments +// // insert some measurements // KeyVector key_view; // Point2Vector meas_view; +// std::vector> sharedKs; +// +// // key_view.push_back(Symbol('x', 1)); // meas_view.push_back(Point2(10, 10)); -// factor.add(meas_view, key_view); +// sharedKs.push_back(sharedK); +// factor.add(meas_view, key_view, sharedKs); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); From 9479bddf81cd4548ba2333e4b233be398e247526 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 12:52:12 -0400 Subject: [PATCH 014/110] passing tough test - nice! --- .../slam/tests/testSmartProjectionFactorP.cpp | 402 +++++++++++++++--- 1 file changed, 336 insertions(+), 66 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index 9ce3e56ac..a6a90dbbc 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -106,7 +106,7 @@ TEST( SmartProjectionFactorP, noiseless ) { factor.add(level_uv, x1, sharedK); factor.add(level_uv_right, x2, sharedK); - 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -119,8 +119,9 @@ TEST( SmartProjectionFactorP, noiseless ) { EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) - std::function f = // - std::bind(&SmartFactorP::whitenedError, factor, cameras, std::placeholders::_1); + std::function f = // + std::bind(&SmartFactorP::whitenedError, factor, cameras, + std::placeholders::_1); // Calculate using computeEP Matrix actualE; @@ -164,7 +165,7 @@ TEST( SmartProjectionFactorP, noisy ) { 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)); + Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); SmartFactorP::shared_ptr factor(new SmartFactorP(model)); @@ -178,11 +179,11 @@ TEST( SmartProjectionFactorP, noisy ) { measurements.push_back(level_uv); measurements.push_back(level_uv_right); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; factor2->add(measurements, views, sharedKs); double actualError2 = factor2->error(values); @@ -194,7 +195,8 @@ TEST(SmartProjectionFactorP, 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)); + 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(); @@ -213,14 +215,14 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; SmartProjectionParams params; params.setRankTolerance(1.0); params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -237,7 +239,8 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); SmartFactorP smartFactor3(model, params); - smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors);; + smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); + ; const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -291,7 +294,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { views.push_back(x2); views.push_back(x3); - std::vector> sharedK2s; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); @@ -322,7 +325,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { // 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -332,8 +335,9 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { 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))); + -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); @@ -362,13 +366,14 @@ TEST( SmartProjectionFactorP, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared(model); + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + > (model); smartFactor1->add(measurements_cam1, views, sharedKs); SmartFactorP::Cameras cameras; @@ -401,7 +406,7 @@ TEST( SmartProjectionFactorP, Factors ) { A2 << 10, 0, 1, 0, -1, 0; A1 *= 10. / sigma; A2 *= 10. / sigma; - Matrix expectedInformation; // filled below + Matrix expectedInformation; // filled below { // createHessianFactor Matrix66 G11 = 0.5 * A1.transpose() * A1; @@ -422,8 +427,8 @@ TEST( SmartProjectionFactorP, Factors ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr > actual = - smartFactor1->createHessianFactor(values, 0.0); + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values, 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); @@ -436,7 +441,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -445,7 +450,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -470,7 +475,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { // 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -480,8 +485,9 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { 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)), + -0.0313952598, -0.000986635786, 0.0314107591, -0.999013364, + -0.0313952598), + Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; @@ -497,7 +503,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { double excludeLandmarksFutherThanDist = 2; - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -506,7 +512,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -518,16 +524,13 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); smartFactor1->add(measurements_cam1, views, sharedKs); - SmartFactorP::shared_ptr smartFactor2( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); smartFactor2->add(measurements_cam2, views, sharedKs); - SmartFactorP::shared_ptr smartFactor3( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); smartFactor3->add(measurements_cam3, views, sharedKs); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -541,7 +544,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { // 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -560,11 +563,11 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -580,7 +583,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { 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 + measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartProjectionParams params; params.setLinearizationMode(gtsam::HESSIAN); @@ -588,20 +591,16 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); smartFactor1->add(measurements_cam1, views, sharedKs); - SmartFactorP::shared_ptr smartFactor2( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); smartFactor2->add(measurements_cam2, views, sharedKs); - SmartFactorP::shared_ptr smartFactor3( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); smartFactor3->add(measurements_cam3, views, sharedKs); - SmartFactorP::shared_ptr smartFactor4( - new SmartFactorP(model, params)); + SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); smartFactor4->add(measurements_cam4, views, sharedKs); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -629,7 +628,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { /* *************************************************************************/ TEST( SmartProjectionFactorP, CheckHessian) { - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; using namespace vanillaPose; @@ -647,7 +646,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); @@ -656,16 +655,13 @@ TEST( SmartProjectionFactorP, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactorP::shared_ptr smartFactor1( - new SmartFactorP(model, params)); // HESSIAN, by default + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, sharedKs); - SmartFactorP::shared_ptr smartFactor2( - new SmartFactorP(model, params)); // HESSIAN, by default + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, sharedKs); - SmartFactorP::shared_ptr smartFactor3( - new SmartFactorP(model, params)); // HESSIAN, by default + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, sharedKs); NonlinearFactorGraph graph; @@ -675,7 +671,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { // 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -685,8 +681,8 @@ TEST( SmartProjectionFactorP, CheckHessian) { assert_equal( Pose3( Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265, - -0.130426831, -0.0115837907, 0.130819108, -0.98278564, - -0.130455917), + -0.130426831, -0.0115837907, 0.130819108, -0.98278564, + -0.130455917), Point3(0.0897734171, -0.110201006, 0.901022872)), values.at(x3))); @@ -708,7 +704,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { + factor2->augmentedInformation() + factor3->augmentedInformation(); // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + 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)); @@ -719,7 +715,7 @@ TEST( SmartProjectionFactorP, Hessian ) { using namespace vanillaPose2; - KeyVector views {x1, x2}; + KeyVector views { x1, x2 }; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -728,7 +724,7 @@ TEST( SmartProjectionFactorP, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - std::vector> sharedK2s; + std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; sharedK2s.push_back(sharedK2); sharedK2s.push_back(sharedK2); @@ -736,7 +732,7 @@ TEST( SmartProjectionFactorP, Hessian ) { smartFactor1->add(measurements_cam1, views, sharedK2s); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + Point3(0.5, 0.1, 0.3)); Values values; values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); @@ -773,9 +769,9 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views {x1, x2, x3}; + KeyVector views { x1, x2, x3 }; - std::vector> sharedBundlerKs; + std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; sharedBundlerKs.push_back(sharedBundlerK); sharedBundlerKs.push_back(sharedBundlerK); sharedBundlerKs.push_back(sharedBundlerK); @@ -800,7 +796,7 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { // 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, cam1.pose()); values.insert(x2, cam2.pose()); @@ -810,8 +806,9 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { 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))); + -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); @@ -819,6 +816,279 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); } +#include +typedef GenericProjectionFactor TestProjectionFactor; +static Symbol l0('L', 0); +/* *************************************************************************/ +TEST( SmartProjectionFactorP, hessianComparedToProjFactors_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 vanillaPose; + 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 keys; + keys.push_back(x1); + keys.push_back(x2); + keys.push_back(x3); + keys.push_back(x1); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); + + 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 ProjectionFactor factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(2 * 4); + + // create projection factors rolling shutter + TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, + sharedK); + Matrix HPoseActual, HEActual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(0, 0) = HPoseActual; + E.block<2, 3>(0, 0) = HEActual; + + TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, + HEActual); + F.block<2, 6>(2, 6) = HPoseActual; + E.block<2, 3>(2, 0) = HEActual; + + TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, + HEActual); + F.block<2, 6>(4, 12) = HPoseActual; + E.block<2, 3>(4, 0) = HEActual; + + TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(6, 0) = HPoseActual; + E.block<2, 3>(6, 0) = HEActual; + + // 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_projFactors; + nfg_projFactors.add(factor11); + nfg_projFactors.add(factor12); + nfg_projFactors.add(factor13); + nfg_projFactors.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactors.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +///* *************************************************************************/ +//TEST( SmartProjectionFactorP, 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) +////| -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 + /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); From 7d80f52b1247204d910cdd07c91be79c68749dd2 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 26 Aug 2021 13:31:09 -0400 Subject: [PATCH 015/110] done with all tests --- gtsam/slam/SmartProjectionFactorP.h | 3 + .../slam/tests/testSmartProjectionFactorP.cpp | 271 +++++++++--------- 2 files changed, 137 insertions(+), 137 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 559a294ac..929ec41f7 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -90,6 +90,9 @@ class SmartProjectionFactorP : public SmartProjectionFactor { const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; } /** Virtual destructor */ diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionFactorP.cpp index a6a90dbbc..4591dc08e 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactorP.cpp @@ -950,144 +950,141 @@ TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSameP EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); } -///* *************************************************************************/ -//TEST( SmartProjectionFactorP, 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)); -//} +/* *************************************************************************/ +TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { -//#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 + using namespace vanillaPose; + 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 keys; + keys.push_back(x1); + keys.push_back(x2); + keys.push_back(x3); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // 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 keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; + sharedKs_redundant.push_back(sharedK);// we readd the first calibration + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); + smartFactor2->add(measurements_lmk2, keys, sharedKs); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); + smartFactor3->add(measurements_lmk3, keys, sharedKs); + + 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 +// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor +//-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) +//| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionFactorP, 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, sharedKSimple, body_P_sensorId); + smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartFactorP_LINEARIZE); + smartFactorP->linearize(values); + gttoc_(SmartFactorP_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_(SmartPoseFactor_LINEARIZE); + smartFactor->linearize(values); + gttoc_(SmartPoseFactor_LINEARIZE); + } + tictoc_print_(); +} +#endif /* ************************************************************************* */ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); From e9641ba26b1214529af00dc82515ab4be6eb1636 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 28 Aug 2021 20:09:24 -0400 Subject: [PATCH 016/110] Merge branch 'develop' into feature/cameraTemplateForAllSmartFactors # Conflicts: # gtsam/geometry/CameraSet.h --- doc/CMakeLists.txt | 25 +- gtsam/CMakeLists.txt | 1 + gtsam/base/Lie.h | 26 +- gtsam/basis/Basis.h | 507 +++++++++++++++++ gtsam/basis/BasisFactors.h | 424 +++++++++++++++ gtsam/basis/CMakeLists.txt | 6 + gtsam/basis/Chebyshev.cpp | 98 ++++ gtsam/basis/Chebyshev.h | 109 ++++ gtsam/basis/Chebyshev2.cpp | 205 +++++++ gtsam/basis/Chebyshev2.h | 148 +++++ gtsam/basis/FitBasis.h | 99 ++++ gtsam/basis/Fourier.h | 108 ++++ gtsam/basis/ParameterMatrix.h | 215 ++++++++ gtsam/basis/basis.i | 146 +++++ gtsam/basis/tests/CMakeLists.txt | 1 + gtsam/basis/tests/testChebyshev.cpp | 236 ++++++++ gtsam/basis/tests/testChebyshev2.cpp | 435 +++++++++++++++ gtsam/basis/tests/testFourier.cpp | 254 +++++++++ gtsam/basis/tests/testParameterMatrix.cpp | 145 +++++ gtsam/geometry/CameraSet.h | 165 +++--- gtsam/geometry/tests/testCameraSet.cpp | 5 +- gtsam/nonlinear/FunctorizedFactor.h | 2 +- .../nonlinear/tests/testFunctorizedFactor.cpp | 140 ++++- gtsam/slam/SmartProjectionFactorP.h | 2 +- .../slam/ProjectionFactorRollingShutter.cpp | 39 +- .../slam/ProjectionFactorRollingShutter.h | 187 ++++--- .../SmartProjectionPoseFactorRollingShutter.h | 301 ++++++----- .../testProjectionFactorRollingShutter.cpp | 258 +++++---- ...martProjectionPoseFactorRollingShutter.cpp | 510 +++++++++++------- python/CMakeLists.txt | 1 + python/gtsam/preamble/basis.h | 12 + python/gtsam/specializations/basis.h | 12 + 32 files changed, 4132 insertions(+), 690 deletions(-) create mode 100644 gtsam/basis/Basis.h create mode 100644 gtsam/basis/BasisFactors.h create mode 100644 gtsam/basis/CMakeLists.txt create mode 100644 gtsam/basis/Chebyshev.cpp create mode 100644 gtsam/basis/Chebyshev.h create mode 100644 gtsam/basis/Chebyshev2.cpp create mode 100644 gtsam/basis/Chebyshev2.h create mode 100644 gtsam/basis/FitBasis.h create mode 100644 gtsam/basis/Fourier.h create mode 100644 gtsam/basis/ParameterMatrix.h create mode 100644 gtsam/basis/basis.i create mode 100644 gtsam/basis/tests/CMakeLists.txt create mode 100644 gtsam/basis/tests/testChebyshev.cpp create mode 100644 gtsam/basis/tests/testChebyshev2.cpp create mode 100644 gtsam/basis/tests/testFourier.cpp create mode 100644 gtsam/basis/tests/testParameterMatrix.cpp create mode 100644 python/gtsam/preamble/basis.h create mode 100644 python/gtsam/specializations/basis.h diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7c43a8989..2218addcf 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -22,18 +22,19 @@ if (GTSAM_BUILD_DOCS) # GTSAM core subfolders set(gtsam_doc_subdirs - gtsam/base - gtsam/discrete - gtsam/geometry - gtsam/inference - gtsam/linear - gtsam/navigation - gtsam/nonlinear - gtsam/sam - gtsam/sfm - gtsam/slam - gtsam/smart - gtsam/symbolic + gtsam/base + gtsam/basis + gtsam/discrete + gtsam/geometry + gtsam/inference + gtsam/linear + gtsam/navigation + gtsam/nonlinear + gtsam/sam + gtsam/sfm + gtsam/slam + gtsam/smart + gtsam/symbolic gtsam ) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index 71daf0653..e2f2ad828 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -5,6 +5,7 @@ project(gtsam LANGUAGES CXX) # The following variable is the master list of subdirs to add set (gtsam_subdirs base + basis geometry inference symbolic diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 34422edd7..ac7c2a9a5 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; + 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 - 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; + 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)))); } /** diff --git a/gtsam/basis/Basis.h b/gtsam/basis/Basis.h new file mode 100644 index 000000000..d8bd28c1a --- /dev/null +++ b/gtsam/basis/Basis.h @@ -0,0 +1,507 @@ +/* ---------------------------------------------------------------------------- + + * 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 Basis.h + * @brief Compute an interpolating basis + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +/** + * This file supports creating continuous functions `f(x;p)` as a linear + * combination of `basis functions` such as the Fourier basis on SO(2) or a set + * of Chebyshev polynomials on [-1,1]. + * + * In the expression `f(x;p)` the variable `x` is + * the continuous argument at which the function is evaluated, and `p` are + * the parameters which are coefficients of the different basis functions, + * e.g. p = [4; 3; 2] => 4 + 3x + 2x^2 for a polynomial. + * However, different parameterizations are also possible. + + * The `Basis` class below defines a number of functors that can be used to + * evaluate `f(x;p)` at a given `x`, and these functors also calculate + * the Jacobian of `f(x;p)` with respect to the parameters `p`. + * This is actually the most important calculation, as it will allow GTSAM + * to optimize over the parameters `p`. + + * This functionality is implemented using the `CRTP` or "Curiously recurring + * template pattern" C++ idiom, which is a meta-programming technique in which + * the derived class is passed as a template argument to `Basis`. + * The DERIVED class is assumed to satisfy a C++ concept, + * i.e., we expect it to define the following types and methods: + + - type `Parameters`: the parameters `p` in f(x;p) + - `CalculateWeights(size_t N, double x, double a=default, double b=default)` + - `DerivativeWeights(size_t N, double x, double a=default, double b=default)` + + where `Weights` is an N*1 row vector which defines the basis values for the + polynomial at the specified point `x`. + + E.g. A Fourier series would give the following: + - `CalculateWeights` -> For N=5, the values for the bases: + [1, cos(x), sin(x), cos(2x), sin(2x)] + - `DerivativeWeights` -> For N=5, these are: + [0, -sin(x), cos(x), -2sin(2x), 2cos(x)] + + Note that for a pseudo-spectral basis (as in Chebyshev2), the weights are + instead the values for the Barycentric interpolation formula, since the values + at the polynomial points (e.g. Chebyshev points) define the bases. + */ + +namespace gtsam { + +using Weights = Eigen::Matrix; /* 1xN vector */ + +/** + * @brief Function for computing the kronecker product of the 1*N Weight vector + * `w` with the MxM identity matrix `I` efficiently. + * The main reason for this is so we don't need to use Eigen's Unsupported + * library. + * + * @tparam M Size of the identity matrix. + * @param w The weights of the polynomial. + * @return Mx(M*N) kronecker product [w(0)*I, w(1)*I, ..., w(N-1)*I] + */ +template +Matrix kroneckerProductIdentity(const Weights& w) { + Matrix result(M, w.cols() * M); + result.setZero(); + + for (int i = 0; i < w.cols(); i++) { + result.block(0, i * M, M, M).diagonal().array() = w(i); + } + return result; +} + +/// CRTP Base class for function bases +template +class GTSAM_EXPORT Basis { + public: + /** + * Calculate weights for all x in vector X. + * Returns M*N matrix where M is the size of the vector X, + * and N is the number of basis functions. + */ + static Matrix WeightMatrix(size_t N, const Vector& X) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i)); + return W; + } + + /** + * @brief Calculate weights for all x in vector X, with interval [a,b]. + * + * @param N The number of basis functions. + * @param X The vector for which to compute the weights. + * @param a The lower bound for the interval range. + * @param b The upper bound for the interval range. + * @return Returns M*N matrix where M is the size of the vector X. + */ + static Matrix WeightMatrix(size_t N, const Vector& X, double a, double b) { + Matrix W(X.size(), N); + for (int i = 0; i < X.size(); i++) + W.row(i) = DERIVED::CalculateWeights(N, X(i), a, b); + return W; + } + + /** + * An instance of an EvaluationFunctor calculates f(x;p) at a given `x`, + * applied to Parameters `p`. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific N*1 vector of Parameters, returns the scalar + * value of the function at x, possibly with Jacobians wrpt the parameters. + */ + class EvaluationFunctor { + protected: + Weights weights_; + + public: + /// For serialization + EvaluationFunctor() {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x) + : weights_(DERIVED::CalculateWeights(N, x)) {} + + /// Constructor with interval [a,b] + EvaluationFunctor(size_t N, double x, double a, double b) + : weights_(DERIVED::CalculateWeights(N, x, a, b)) {} + + /// Regular 1D evaluation + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + if (H) *H = weights_; + return (weights_ * p)(0); + } + + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian<-1, -1> H = boost::none) const { + return apply(p, H); // might call apply in derived + } + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * VectorEvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + */ + template + class VectorEvaluationFunctor : protected EvaluationFunctor { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorEvaluationFunctor() {} + + /// Default Constructor + VectorEvaluationFunctor(size_t N, double x) : EvaluationFunctor(N, x) { + calculateJacobian(); + } + + /// Constructor, with interval [a,b] + VectorEvaluationFunctor(size_t N, double x, double a, double b) + : EvaluationFunctor(N, x, a, b) { + calculateJacobian(); + } + + /// M-dimensional evaluation + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + + /// c++ sugar + VectorM operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * VectorComponentFunctor computes the N-vector value for a specific row + * component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class VectorComponentFunctor : public EvaluationFunctor { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorEvaluationFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < EvaluationFunctor::weights_.size(); j++) + H_(0, rowIndex_ + j * M) = EvaluationFunctor::weights_(j); + } + + public: + /// For serialization + VectorComponentFunctor() {} + + /// Construct with row index + VectorComponentFunctor(size_t N, size_t i, double x) + : EvaluationFunctor(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + VectorComponentFunctor(size_t N, size_t i, double x, double a, double b) + : EvaluationFunctor(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + + /// Calculate component of component rowIndex_ of P + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * EvaluationFunctor::weights_.transpose(); + } + + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Manifold EvaluationFunctor at a given x, applied to ParameterMatrix. + * This functor is used to evaluate a parameterized function at a given scalar + * value x. When given a specific M*N parameters, returns an M-vector the M + * corresponding functions at x, possibly with Jacobians wrpt the parameters. + * + * The difference with the VectorEvaluationFunctor is that after computing the + * M*1 vector xi=F(x;P), with x a scalar and P the M*N parameter vector, we + * also retract xi back to the T manifold. + * For example, if T==Rot3, then we first compute a 3-vector xi using x and P, + * and then map that 3-vector xi back to the Rot3 manifold, yielding a valid + * 3D rotation. + */ + template + class ManifoldEvaluationFunctor + : public VectorEvaluationFunctor::dimension> { + enum { M = traits::dimension }; + using Base = VectorEvaluationFunctor; + + public: + /// For serialization + ManifoldEvaluationFunctor() {} + + /// Default Constructor + ManifoldEvaluationFunctor(size_t N, double x) : Base(N, x) {} + + /// Constructor, with interval [a,b] + ManifoldEvaluationFunctor(size_t N, double x, double a, double b) + : Base(N, x, a, b) {} + + /// Manifold evaluation + T apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + // Interpolate the M-dimensional vector to yield a vector in tangent space + Eigen::Matrix xi = Base::operator()(P, H); + + // Now call retract with this M-vector, possibly with derivatives + Eigen::Matrix D_result_xi; + T result = T::ChartAtOrigin::Retract(xi, H ? &D_result_xi : 0); + + // Finally, if derivatives are asked, apply chain rule where H is Mx(M*N) + // derivative of interpolation and D_result_xi is MxM derivative of + // retract. + if (H) *H = D_result_xi * (*H); + + // and return a T + return result; + } + + /// c++ sugar + T operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); // might call apply in derived + } + }; + + /// Base class for functors below that calculate derivative weights + class DerivativeFunctorBase { + protected: + Weights weights_; + + public: + /// For serialization + DerivativeFunctorBase() {} + + DerivativeFunctorBase(size_t N, double x) + : weights_(DERIVED::DerivativeWeights(N, x)) {} + + DerivativeFunctorBase(size_t N, double x, double a, double b) + : weights_(DERIVED::DerivativeWeights(N, x, a, b)) {} + + void print(const std::string& s = "") const { + std::cout << s << (s != "" ? " " : "") << weights_ << std::endl; + } + }; + + /** + * An instance of a DerivativeFunctor calculates f'(x;p) at a given `x`, + * applied to Parameters `p`. + * When given a scalar value x and a specific N*1 vector of Parameters, + * this functor returns the scalar derivative value of the function at x, + * possibly with Jacobians wrpt the parameters. + */ + class DerivativeFunctor : protected DerivativeFunctorBase { + public: + /// For serialization + DerivativeFunctor() {} + + DerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) {} + + DerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) {} + + double apply(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + if (H) *H = this->weights_; + return (this->weights_ * p)(0); + } + /// c++ sugar + double operator()(const typename DERIVED::Parameters& p, + OptionalJacobian H = boost::none) const { + return apply(p, H); // might call apply in derived + } + }; + + /** + * VectorDerivativeFunctor at a given x, applied to ParameterMatrix. + * + * This functor is used to evaluate the derivatives of a parameterized + * function at a given scalar value x. When given a specific M*N parameters, + * returns an M-vector the M corresponding function derivatives at x, possibly + * with Jacobians wrpt the parameters. + */ + template + class VectorDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using VectorM = Eigen::Matrix; + using Jacobian = Eigen::Matrix; + Jacobian H_; + + /** + * Calculate the `M*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H =[ w(0) 0 w(1) 0 w(2) 0 + * 0 w(0) 0 w(1) 0 w(2) ] + * i.e., the Kronecker product of weights_ with the MxM identity matrix. + */ + void calculateJacobian() { + H_ = kroneckerProductIdentity(this->weights_); + } + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// For serialization + VectorDerivativeFunctor() {} + + /// Default Constructor + VectorDerivativeFunctor(size_t N, double x) : DerivativeFunctorBase(N, x) { + calculateJacobian(); + } + + /// Constructor, with optional interval [a,b] + VectorDerivativeFunctor(size_t N, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b) { + calculateJacobian(); + } + + VectorM apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.matrix() * this->weights_.transpose(); + } + /// c++ sugar + VectorM operator()( + const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + /** + * Given a M*N Matrix of M-vectors at N polynomial points, an instance of + * ComponentDerivativeFunctor computes the N-vector derivative for a specific + * row component of the M-vectors at all the polynomial points. + * + * This component is specified by the row index i, with 0 + class ComponentDerivativeFunctor : protected DerivativeFunctorBase { + protected: + using Jacobian = Eigen::Matrix; + size_t rowIndex_; + Jacobian H_; + + /* + * Calculate the `1*(M*N)` Jacobian of this functor with respect to + * the M*N parameter matrix `P`. + * We flatten assuming column-major order, e.g., if N=3 and M=2, we have + * H=[w(0) 0 w(1) 0 w(2) 0] for rowIndex==0 + * H=[0 w(0) 0 w(1) 0 w(2)] for rowIndex==1 + * i.e., one row of the Kronecker product of weights_ with the + * MxM identity matrix. See also VectorDerivativeFunctor. + */ + void calculateJacobian(size_t N) { + H_.setZero(1, M * N); + for (int j = 0; j < this->weights_.size(); j++) + H_(0, rowIndex_ + j * M) = this->weights_(j); + } + + public: + /// For serialization + ComponentDerivativeFunctor() {} + + /// Construct with row index + ComponentDerivativeFunctor(size_t N, size_t i, double x) + : DerivativeFunctorBase(N, x), rowIndex_(i) { + calculateJacobian(N); + } + + /// Construct with row index and interval + ComponentDerivativeFunctor(size_t N, size_t i, double x, double a, double b) + : DerivativeFunctorBase(N, x, a, b), rowIndex_(i) { + calculateJacobian(N); + } + /// Calculate derivative of component rowIndex_ of F + double apply(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + if (H) *H = H_; + return P.row(rowIndex_) * this->weights_.transpose(); + } + /// c++ sugar + double operator()(const ParameterMatrix& P, + OptionalJacobian H = boost::none) const { + return apply(P, H); + } + }; + + // Vector version for MATLAB :-( + static double Derivative(double x, const Vector& p, // + OptionalJacobian H = boost::none) { + return DerivativeFunctor(x)(p.transpose(), H); + } +}; + +} // namespace gtsam diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h new file mode 100644 index 000000000..0b3d4c1a0 --- /dev/null +++ b/gtsam/basis/BasisFactors.h @@ -0,0 +1,424 @@ +/* ---------------------------------------------------------------------------- + + * 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 BasisFactors.h + * @brief Factor definitions for various Basis functors. + * @author Varun Agrawal + * @date July 4, 2020 + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Factor for enforcing the scalar value of the polynomial BASIS + * representation at `x` is the same as the measurement `z` when using a + * pseudo-spectral parameterization. + * + * @tparam BASIS The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT EvaluationFactor : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + EvaluationFactor() {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new EvaluationFactor object + * + * @param key Symbol for value to optimize. + * @param z The measurement value. + * @param model Noise model + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} + + virtual ~EvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (M, N) is equal to a vector-valued measurement at the same point, + when + * using a pseudo-spectral parameterization. + * + * This factors tries to enforce the basis function evaluation `f(x;p)` to take + * on the value `z` at location `x`, providing a gradient on the parameters p. + * In a probabilistic estimation context, `z` is known as a measurement, and the + * parameterized basis function can be seen as a + * measurement prediction function. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector. + */ +template +class GTSAM_EXPORT VectorEvaluationFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorEvaluationFactor() {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new VectorEvaluationFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorEvaluationFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, + typename BASIS::template VectorEvaluationFunctor(N, x, a, b)) {} + + virtual ~VectorEvaluationFactor() {} +}; + +/** + * Unary factor for enforcing BASIS polynomial evaluation on a ParameterMatrix + * of size (P, N) is equal to specified measurement at the same point, when + * using a pseudo-spectral parameterization. + * + * This factor is similar to `VectorEvaluationFactor` with the key difference + * being that it only enforces the constraint for a single scalar in the vector, + * indexed by `i`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the fixed-size vector. + * + * Example: + * VectorComponentFactor controlPrior(key, measured, model, + * N, i, t, a, b); + * where N is the degree and i is the component index. + */ +template +class GTSAM_EXPORT VectorComponentFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + + public: + VectorComponentFactor() {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x) + : Base(key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x)) {} + + /** + * @brief Construct a new VectorComponentFactor object. + * + * @param key The key to the ParameterMatrix object used to represent the + * polynomial. + * @param z The scalar value at a specified index `i` of the full measurement + * vector. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate 0the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorComponentFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, size_t i, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template VectorComponentFunctor

(N, i, x, a, b)) { + } + + virtual ~VectorComponentFactor() {} +}; + +/** + * For a measurement value of type T i.e. `T z = g(x)`, this unary + * factor enforces that the polynomial basis, when evaluated at `x`, gives us + * the same `z`, i.e. `T z = g(x) = f(x;p)`. + * + * This is done via computations on the tangent space of the + * manifold of T. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param T: Object type which is synthesized by the provided functor. + * + * Example: + * ManifoldEvaluationFactor rotationFactor(key, measurement, + * model, N, x, a, b); + * + * where `x` is the value (e.g. timestep) at which the rotation was evaluated. + */ +template +class GTSAM_EXPORT ManifoldEvaluationFactor + : public FunctorizedFactor::dimension>> { + private: + using Base = FunctorizedFactor::dimension>>; + + public: + ManifoldEvaluationFactor() {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x)) {} + + /** + * @brief Construct a new ManifoldEvaluationFactor object. + * + * @param key Key for the state matrix parameterizing the function to estimate + * via the BASIS. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which the estimated function is evaluated. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ManifoldEvaluationFactor(Key key, const T &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base( + key, z, model, + typename BASIS::template ManifoldEvaluationFunctor(N, x, a, b)) { + } + + virtual ~ManifoldEvaluationFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point`x` is equal to the scalar measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + */ +template +class GTSAM_EXPORT DerivativeFactor + : public FunctorizedFactor { + private: + using Base = FunctorizedFactor; + + public: + DerivativeFactor() {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x)) {} + + /** + * @brief Construct a new DerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + DerivativeFactor(Key key, const double &z, const SharedNoiseModel &model, + const size_t N, double x, double a, double b) + : Base(key, z, model, typename BASIS::DerivativeFunctor(N, x, a, b)) {} + + virtual ~DerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial at a specified point `x` is equal to the vector value `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param M: Size of the evaluated state vector derivative. + */ +template +class GTSAM_EXPORT VectorDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template VectorDerivativeFunctor; + + public: + VectorDerivativeFactor() {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x) + : Base(key, z, model, Func(N, x)) {} + + /** + * @brief Construct a new VectorDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The measurement value. + * @param model The noise model. + * @param N The degree of the polynomial. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + VectorDerivativeFactor(Key key, const Vector &z, + const SharedNoiseModel &model, const size_t N, + double x, double a, double b) + : Base(key, z, model, Func(N, x, a, b)) {} + + virtual ~VectorDerivativeFactor() {} +}; + +/** + * A unary factor which enforces the evaluation of the derivative of a BASIS + * polynomial is equal to the scalar value at a specific index `i` of a + * vector-valued measurement `z`. + * + * @param BASIS: The basis class to use e.g. Chebyshev2 + * @param P: Size of the control component derivative. + */ +template +class GTSAM_EXPORT ComponentDerivativeFactor + : public FunctorizedFactor> { + private: + using Base = FunctorizedFactor>; + using Func = typename BASIS::template ComponentDerivativeFunctor

; + + public: + ComponentDerivativeFactor() {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x) + : Base(key, z, model, Func(N, i, x)) {} + + /** + * @brief Construct a new ComponentDerivativeFactor object. + * + * @param key The key to the ParameterMatrix which represents the basis + * polynomial. + * @param z The scalar measurement value at a specific index `i` of the full + * measurement vector. + * @param model The degree of the polynomial. + * @param N The degree of the polynomial. + * @param i The index for the evaluated vector to give us the desired scalar + * value. + * @param x The point at which to evaluate the basis polynomial. + * @param a Lower bound for the polynomial. + * @param b Upper bound for the polynomial. + */ + ComponentDerivativeFactor(Key key, const double &z, + const SharedNoiseModel &model, const size_t N, + size_t i, double x, double a, double b) + : Base(key, z, model, Func(N, i, x, a, b)) {} + + virtual ~ComponentDerivativeFactor() {} +}; + +} // namespace gtsam diff --git a/gtsam/basis/CMakeLists.txt b/gtsam/basis/CMakeLists.txt new file mode 100644 index 000000000..203fd96a2 --- /dev/null +++ b/gtsam/basis/CMakeLists.txt @@ -0,0 +1,6 @@ +# Install headers +file(GLOB basis_headers "*.h") +install(FILES ${basis_headers} DESTINATION include/gtsam/basis) + +# Build tests +add_subdirectory(tests) diff --git a/gtsam/basis/Chebyshev.cpp b/gtsam/basis/Chebyshev.cpp new file mode 100644 index 000000000..3b5825fc3 --- /dev/null +++ b/gtsam/basis/Chebyshev.cpp @@ -0,0 +1,98 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev.cpp + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +/** + * @brief Scale x from [a, b] to [t1, t2] + * + * ((b'-a') * (x - a) / (b - a)) + a' + * + * @param x Value to scale to new range. + * @param a Original lower limit. + * @param b Original upper limit. + * @param t1 New lower limit. + * @param t2 New upper limit. + * @return double + */ +static double scale(double x, double a, double b, double t1, double t2) { + return ((t2 - t1) * (x - a) / (b - a)) + t1; +} + +Weights Chebyshev1Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Tx(1, N); + + x = scale(x, a, b, -1, 1); + + Tx(0) = 1; + Tx(1) = x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Tx(i) = 2 * x * Tx(i - 1) - Tx(i - 2); + } + return Tx; +} + +Weights Chebyshev1Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + Weights weights = Weights::Zero(N); + for (size_t n = 1; n < N; n++) { + weights(n) = n * Ux(n - 1); + } + return weights; +} + +Weights Chebyshev2Basis::CalculateWeights(size_t N, double x, double a, + double b) { + Weights Ux(N); + + x = scale(x, a, b, -1, 1); + + Ux(0) = 1; + Ux(1) = 2 * x; + for (size_t i = 2; i < N; i++) { + // instead of cos(i*acos(x)), this recurrence is much faster + Ux(i) = 2 * x * Ux(i - 1) - Ux(i - 2); + } + return Ux; +} + +Weights Chebyshev2Basis::DerivativeWeights(size_t N, double x, double a, + double b) { + Weights Tx = Chebyshev1Basis::CalculateWeights(N + 1, x, a, b); + Weights Ux = Chebyshev2Basis::CalculateWeights(N, x, a, b); + + Weights weights(N); + + x = scale(x, a, b, -1, 1); + if (x == -1 || x == 1) { + throw std::runtime_error( + "Derivative of Chebyshev2 Basis does not exist at range limits."); + } + + for (size_t n = 0; n < N; n++) { + weights(n) = ((n + 1) * Tx(n + 1) - x * Ux(n)) / (x * x - 1); + } + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h new file mode 100644 index 000000000..d16ccfaac --- /dev/null +++ b/gtsam/basis/Chebyshev.h @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev.h + * @brief Chebyshev basis decompositions + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include + +#include + +namespace gtsam { + +/** + * Basis of Chebyshev polynomials of the first kind + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#First_kind + * These are typically denoted with the symbol T_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + */ +struct Chebyshev1Basis : Basis { + using Parameters = Eigen::Matrix; + + Parameters parameters_; + + /** + * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * The derivative weights are pre-multiplied to the polynomial Parameters. + * + * From Wikipedia we have D[T_n(x),x] = n*U_{n-1}(x) + * I.e. the derivative fo a first kind cheb is just a second kind cheb + * So, we define a second kind basis here of order N-1 + * Note that it has one less weight. + * + * The Parameters pertain to 1st kind chebs up to order N-1 + * But of course the first one (order 0) is constant, so omit that weight. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev1Basis + +/** + * Basis of Chebyshev polynomials of the second kind. + * https://en.wikipedia.org/wiki/Chebyshev_polynomials#Second_kind + * These are typically denoted with the symbol U_n, where n is the degree. + * The parameter N is the number of coefficients, i.e., N = n+1. + * In contrast to the templates in Chebyshev2, the classes below specify + * basis functions, weighted combinations of which are used to approximate + * functions. In this sense, they are like the sines and cosines of the Fourier + * basis. + */ +struct Chebyshev2Basis : Basis { + using Parameters = Eigen::Matrix; + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * @brief Evaluate Chebyshev derivative at x. + * + * @param N Degree of the polynomial. + * @param x Point to evaluate polynomial at. + * @param a Lower limit of polynomial (default=-1). + * @param b Upper limit of polynomial (default=1). + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); +}; // Chebyshev2Basis + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp new file mode 100644 index 000000000..44876b6e9 --- /dev/null +++ b/gtsam/basis/Chebyshev2.cpp @@ -0,0 +1,205 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.cpp + * @brief Chebyshev parameterizations on Chebyshev points of second kind + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#include + +namespace gtsam { + +Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weights(N); + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weights.setZero(); + weights(j) = 1; + return weights; + } + distances(j) = dj; + } + + // Beginning of interval, j = 0, x(0) = a + weights(0) = 0.5 / distances(0); + + // All intermediate points j=1:N-2 + double d = weights(0), s = -1; // changes sign s at every iteration + for (size_t j = 1; j < N - 1; j++, s = -s) { + weights(j) = s / distances(j); + d += weights(j); + } + + // End of interval, j = N-1, x(N-1) = b + weights(N - 1) = 0.5 * s / distances(N - 1); + d += weights(N - 1); + + // normalize + return weights / d; +} + +Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { + // Allocate space for weights + Weights weightDerivatives(N); + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + // We start by getting distances from x to all Chebyshev points + // as well as getting smallest distance + Weights distances(N); + + for (size_t j = 0; j < N; j++) { + const double dj = + x - Point(N, j, a, b); // only thing that depends on [a,b] + if (std::abs(dj) < 1e-10) { + // exceptional case: x coincides with a Chebyshev point + weightDerivatives.setZero(); + // compute the jth row of the differentiation matrix for this point + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + for (size_t k = 0; k < N; k++) { + if (j == 0 && k == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = -(cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (j == N - 1 && k == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + weightDerivatives(k) = (cj * (N - 1) * (N - 1) + 1) / 6.0; + } else if (k == j) { + double xj = Point(N, j); + double xj2 = xj * xj; + weightDerivatives(k) = -0.5 * xj / (1 - xj2); + } else { + double xj = Point(N, j); + double xk = Point(N, k); + double ck = (k == 0 || k == N - 1) ? 2. : 1.; + t = ((j + k) % 2) == 0 ? 1 : -1; + weightDerivatives(k) = (cj / ck) * t / (xj - xk); + } + } + return 2 * weightDerivatives / (b - a); + } + distances(j) = dj; + } + + // This section of code computes the derivative of + // the Barycentric Interpolation weights formula by applying + // the chain rule on the original formula. + + // g and k are multiplier terms which represent the derivatives of + // the numerator and denominator + double g = 0, k = 0; + double w = 1; + + for (size_t j = 0; j < N; j++) { + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + w = 1.0; + } + + t = (j % 2 == 0) ? 1 : -1; + + double c = t / distances(j); + g += w * c; + k += (w * c / distances(j)); + } + + double s = 1; // changes sign s at every iteration + double g2 = g * g; + + for (size_t j = 0; j < N; j++, s = -s) { + // Beginning of interval, j = 0, x0 = -1.0 and end of interval, j = N-1, + // x0 = 1.0 + if (j == 0 || j == N - 1) { + w = 0.5; + } else { + // All intermediate points j=1:N-2 + w = 1.0; + } + weightDerivatives(j) = (w * -s / (g * distances(j) * distances(j))) - + (w * -s * k / (g2 * distances(j))); + } + + return weightDerivatives; +} + +Chebyshev2::DiffMatrix Chebyshev2::DifferentiationMatrix(size_t N, double a, + double b) { + DiffMatrix D(N, N); + if (N == 1) { + D(0, 0) = 1; + return D; + } + + // toggle variable so we don't need to use `pow` for -1 + double t = -1; + + for (size_t i = 0; i < N; i++) { + double xi = Point(N, i); + double ci = (i == 0 || i == N - 1) ? 2. : 1.; + for (size_t j = 0; j < N; j++) { + if (i == 0 && j == 0) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = -(ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == N - 1 && j == N - 1) { + // we reverse the sign since we order the cheb points from -1 to 1 + D(i, j) = (ci * (N - 1) * (N - 1) + 1) / 6.0; + } else if (i == j) { + double xi2 = xi * xi; + D(i, j) = -xi / (2 * (1 - xi2)); + } else { + double xj = Point(N, j); + double cj = (j == 0 || j == N - 1) ? 2. : 1.; + t = ((i + j) % 2) == 0 ? 1 : -1; + D(i, j) = (ci / cj) * t / (xi - xj); + } + } + } + // scale the matrix to the range + return D / ((b - a) / 2.0); +} + +Weights Chebyshev2::IntegrationWeights(size_t N, double a, double b) { + // Allocate space for weights + Weights weights(N); + size_t K = N - 1, // number of intervals between N points + K2 = K * K; + weights(0) = 0.5 * (b - a) / (K2 + K % 2 - 1); + weights(N - 1) = weights(0); + + size_t last_k = K / 2 + K % 2 - 1; + + for (size_t i = 1; i <= N - 2; ++i) { + double theta = i * M_PI / K; + weights(i) = (K % 2 == 0) ? 1 - cos(K * theta) / (K2 - 1) : 1; + + for (size_t k = 1; k <= last_k; ++k) + weights(i) -= 2 * cos(2 * k * theta) / (4 * k * k - 1); + weights(i) *= (b - a) / K; + } + + return weights; +} + +} // namespace gtsam diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h new file mode 100644 index 000000000..28590961d --- /dev/null +++ b/gtsam/basis/Chebyshev2.h @@ -0,0 +1,148 @@ +/* ---------------------------------------------------------------------------- + + * 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 Chebyshev2.h + * @brief Pseudo-spectral parameterization for Chebyshev polynomials of the + * second kind. + * + * In a pseudo-spectral case, rather than the parameters acting as + * weights for the bases polynomials (as in Chebyshev2Basis), here the + * parameters are the *values* at a specific set of points in the interval, the + * "Chebyshev points". These values uniquely determine the polynomial that + * interpolates them at the Chebyshev points. + * + * This is different from Chebyshev.h since it leverage ideas from + * pseudo-spectral optimization, i.e. we don't decompose into basis functions, + * rather estimate function parameters that enforce function nodes at Chebyshev + * points. + * + * Please refer to Agrawal21icra for more details. + * + * @author Varun Agrawal, Jing Dong, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * Chebyshev Interpolation on Chebyshev points of the second kind + * Note that N here, the number of points, is one less than N from + * 'Approximation Theory and Approximation Practice by L. N. Trefethen (pg.42)'. + */ +class GTSAM_EXPORT Chebyshev2 : public Basis { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + using Base = Basis; + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /// Specific Chebyshev point + static double Point(size_t N, int j) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N > 1 ? (N - 1) : 1); + // We add -PI so that we get values ordered from -1 to +1 + // sin(- M_PI_2 + dtheta*j); also works + return cos(-M_PI + dtheta * j); + } + + /// Specific Chebyshev point, within [a,b] interval + static double Point(size_t N, int j, double a, double b) { + assert(j >= 0 && size_t(j) < N); + const double dtheta = M_PI / (N - 1); + // We add -PI so that we get values ordered from -1 to +1 + return a + (b - a) * (1. + cos(-M_PI + dtheta * j)) / 2; + } + + /// All Chebyshev points + static Vector Points(size_t N) { + Vector points(N); + for (size_t j = 0; j < N; j++) points(j) = Point(N, j); + return points; + } + + /// All Chebyshev points, within [a,b] interval + static Vector Points(size_t N, double a, double b) { + Vector points = Points(N); + const double T1 = (a + b) / 2, T2 = (b - a) / 2; + points = T1 + (T2 * points).array(); + return points; + } + + /** + * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) + * These weights implement barycentric interpolation at a specific x. + * More precisely, f(x) ~ [w0;...;wN] * [f0;...;fN], where the fj are the + * values of the function f at the Chebyshev points. As such, for a given x we + * obtain a linear map from parameter vectors f to interpolated values f(x). + * Optional [a,b] interval can be specified as well. + */ + static Weights CalculateWeights(size_t N, double x, double a = -1, + double b = 1); + + /** + * Evaluate derivative of barycentric weights. + * This is easy and efficient via the DifferentiationMatrix. + */ + static Weights DerivativeWeights(size_t N, double x, double a = -1, + double b = 1); + + /// compute D = differentiation matrix, Trefethen00book p.53 + /// when given a parameter vector f of function values at the Chebyshev + /// points, D*f are the values of f'. + /// https://people.maths.ox.ac.uk/trefethen/8all.pdf Theorem 8.4 + static DiffMatrix DifferentiationMatrix(size_t N, double a = -1, + double b = 1); + + /** + * Evaluate Clenshaw-Curtis integration weights. + * Trefethen00book, pg 128, clencurt.m + * Note that N in clencurt.m is 1 less than our N + * K = N-1; + theta = pi*(0:K)'/K; + w = zeros(1,N); ii = 2:K; v = ones(K-1, 1); + if mod(K,2) == 0 + w(1) = 1/(K^2-1); w(N) = w(1); + for k=1:K/2-1, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + v = v - cos(K*theta(ii))/(K^2-1); + else + w(1) = 1/K^2; w(N) = w(1); + for k=1:K/2, v = v-2*cos(2*k*theta(ii))/(4*k^2-1); end + end + w(ii) = 2*v/K; + + */ + static Weights IntegrationWeights(size_t N, double a = -1, double b = 1); + + /** + * Create matrix of values at Chebyshev points given vector-valued function. + */ + template + static Matrix matrix(boost::function(double)> f, + size_t N, double a = -1, double b = 1) { + Matrix Xmat(M, N); + for (size_t j = 0; j < N; j++) { + Xmat.col(j) = f(Point(N, j, a, b)); + } + return Xmat; + } +}; // \ Chebyshev2 + +} // namespace gtsam diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h new file mode 100644 index 000000000..f5cb99bd7 --- /dev/null +++ b/gtsam/basis/FitBasis.h @@ -0,0 +1,99 @@ +/* ---------------------------------------------------------------------------- + + * 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 FitBasis.h + * @date July 4, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Fit a Basis using least-squares + */ + +/* + * Concept needed for LS. Parameters = Coefficients | Values + * - Parameters, Jacobian + * - PredictFactor(double x)(Parameters p, OptionalJacobian<1,N> H) + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +/// Our sequence representation is a map of {x: y} values where y = f(x) +using Sequence = std::map; +/// A sample is a key-value pair from a sequence. +using Sample = std::pair; + +/** + * Class that does regression via least squares + * Example usage: + * size_t N = 3; + * auto fit = FitBasis(data_points, noise_model, N); + * Vector coefficients = fit.parameters(); + * + * where `data_points` are a map from `x` to `y` values indicating a function + * mapping at specific points, `noise_model` is the gaussian noise model, and + * `N` is the degree of the polynomial basis used to fit the function. + */ +template +class FitBasis { + public: + using Parameters = typename Basis::Parameters; + + private: + Parameters parameters_; + + public: + /// Create nonlinear FG from Sequence + static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence, + const SharedNoiseModel& model, + size_t N) { + NonlinearFactorGraph graph; + for (const Sample sample : sequence) { + graph.emplace_shared>(0, sample.second, model, N, + sample.first); + } + return graph; + } + + /// Create linear FG from Sequence + static GaussianFactorGraph::shared_ptr LinearGraph( + const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); + Values values; + values.insert(0, Parameters::Zero(N)); + GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); + return gfg; + } + + /** + * @brief Construct a new FitBasis object. + * + * @param sequence map of x->y values for a function, a.k.a. y = f(x). + * @param model The noise model to use. + * @param N The degree of the polynomial to fit. + */ + FitBasis(const Sequence& sequence, const SharedNoiseModel& model, size_t N) { + GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model, N); + VectorValues solution = gfg->optimize(); + parameters_ = solution.at(0); + } + + /// Return Fourier coefficients + Parameters parameters() const { return parameters_; } +}; + +} // namespace gtsam diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h new file mode 100644 index 000000000..6943150d7 --- /dev/null +++ b/gtsam/basis/Fourier.h @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 Fourier.h + * @brief Fourier decomposition, see e.g. + * http://mathworld.wolfram.com/FourierSeries.html + * @author Varun Agrawal, Frank Dellaert + * @date July 4, 2020 + */ + +#pragma once + +#include + +namespace gtsam { + +/// Fourier basis +class GTSAM_EXPORT FourierBasis : public Basis { + public: + using Parameters = Eigen::Matrix; + using DiffMatrix = Eigen::Matrix; + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x) { + Weights b(N); + b[0] = 1; + for (size_t i = 1, n = 1; i < N; i += 2, n++) { + b[i] = cos(n * x); + b[i + 1] = sin(n * x); + } + return b; + } + + /** + * @brief Evaluate Real Fourier Weights of size N in interval [a, b], + * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights CalculateWeights(size_t N, double x, double a, double b) { + // TODO(Varun) How do we enforce an interval for Fourier series? + return CalculateWeights(N, x); + } + + /** + * Compute D = differentiation matrix. + * Given coefficients c of a Fourier series c, D*c are the values of c'. + */ + static DiffMatrix DifferentiationMatrix(size_t N) { + DiffMatrix D = DiffMatrix::Zero(N, N); + double k = 1; + for (size_t i = 1; i < N; i += 2) { + D(i, i + 1) = k; // sin'(k*x) = k*cos(k*x) + D(i + 1, i) = -k; // cos'(k*x) = -k*sin(k*x) + k += 1; + } + + return D; + } + + /** + * @brief Get weights at a given x that calculate the derivative. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x) { + return CalculateWeights(N, x) * DifferentiationMatrix(N); + } + + /** + * @brief Get derivative weights at a given x that calculate the derivative, + in the interval [a, b]. + * + * @param N The degree of the polynomial to use. + * @param x The point at which to compute the derivaive weights. + * @param a Lower bound of interval. + * @param b Upper bound of interval. + * @return Weights + */ + static Weights DerivativeWeights(size_t N, double x, double a, double b) { + return CalculateWeights(N, x, a, b) * DifferentiationMatrix(N); + } + +}; // FourierBasis + +} // namespace gtsam diff --git a/gtsam/basis/ParameterMatrix.h b/gtsam/basis/ParameterMatrix.h new file mode 100644 index 000000000..df2d9f62e --- /dev/null +++ b/gtsam/basis/ParameterMatrix.h @@ -0,0 +1,215 @@ +/* ---------------------------------------------------------------------------- + + * 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 ParamaterMatrix.h + * @brief Define ParameterMatrix class which is used to store values at + * interpolation points. + * @author Varun Agrawal, Frank Dellaert + * @date September 21, 2020 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +/** + * A matrix abstraction of MxN values at the Basis points. + * This class serves as a wrapper over an Eigen matrix. + * @tparam M: The dimension of the type you wish to evaluate. + * @param N: the number of Basis points (e.g. Chebyshev points of the second + * kind). + */ +template +class ParameterMatrix { + using MatrixType = Eigen::Matrix; + + private: + MatrixType matrix_; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + enum { dimension = Eigen::Dynamic }; + + /** + * Create ParameterMatrix using the number of basis points. + * @param N: The number of basis points (the columns). + */ + ParameterMatrix(const size_t N) : matrix_(M, N) { matrix_.setZero(); } + + /** + * Create ParameterMatrix from an MxN Eigen Matrix. + * @param matrix: An Eigen matrix used to initialze the ParameterMatrix. + */ + ParameterMatrix(const MatrixType& matrix) : matrix_(matrix) {} + + /// Get the number of rows. + size_t rows() const { return matrix_.rows(); } + + /// Get the number of columns. + size_t cols() const { return matrix_.cols(); } + + /// Get the underlying matrix. + MatrixType matrix() const { return matrix_; } + + /// Return the tranpose of the underlying matrix. + Eigen::Matrix transpose() const { return matrix_.transpose(); } + + /** + * Get the matrix row specified by `index`. + * @param index: The row index to retrieve. + */ + Eigen::Matrix row(size_t index) const { + return matrix_.row(index); + } + + /** + * Set the matrix row specified by `index`. + * @param index: The row index to set. + */ + auto row(size_t index) -> Eigen::Block { + return matrix_.row(index); + } + + /** + * Get the matrix column specified by `index`. + * @param index: The column index to retrieve. + */ + Eigen::Matrix col(size_t index) const { + return matrix_.col(index); + } + + /** + * Set the matrix column specified by `index`. + * @param index: The column index to set. + */ + auto col(size_t index) -> Eigen::Block { + return matrix_.col(index); + } + + /** + * Set all matrix coefficients to zero. + */ + void setZero() { matrix_.setZero(); } + + /** + * Add a ParameterMatrix to another. + * @param other: ParameterMatrix to add. + */ + ParameterMatrix operator+(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ + other.matrix()); + } + + /** + * Add a MxN-sized vector to the ParameterMatrix. + * @param other: Vector which is reshaped and added. + */ + ParameterMatrix operator+( + const Eigen::Matrix& other) const { + // This form avoids a deep copy and instead typecasts `other`. + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ + other_); + } + + /** + * Subtract a ParameterMatrix from another. + * @param other: ParameterMatrix to subtract. + */ + ParameterMatrix operator-(const ParameterMatrix& other) const { + return ParameterMatrix(matrix_ - other.matrix()); + } + + /** + * Subtract a MxN-sized vector from the ParameterMatrix. + * @param other: Vector which is reshaped and subracted. + */ + ParameterMatrix operator-( + const Eigen::Matrix& other) const { + Eigen::Map other_(other.data(), M, cols()); + return ParameterMatrix(matrix_ - other_); + } + + /** + * Multiply ParameterMatrix with an Eigen matrix. + * @param other: Eigen matrix which should be multiplication compatible with + * the ParameterMatrix. + */ + MatrixType operator*(const Eigen::Matrix& other) const { + return matrix_ * other; + } + + /// @name Vector Space requirements, following LieMatrix + /// @{ + + /** + * Print the ParameterMatrix. + * @param s: The prepend string to add more contextual info. + */ + void print(const std::string& s = "") const { + std::cout << (s == "" ? s : s + " ") << matrix_ << std::endl; + } + + /** + * Check for equality up to absolute tolerance. + * @param other: The ParameterMatrix to check equality with. + * @param tol: The absolute tolerance threshold. + */ + bool equals(const ParameterMatrix& other, double tol = 1e-8) const { + return gtsam::equal_with_abs_tol(matrix_, other.matrix(), tol); + } + + /// Returns dimensionality of the tangent space + inline size_t dim() const { return matrix_.size(); } + + /// Convert to vector form, is done row-wise + inline Vector vector() const { + using RowMajor = Eigen::Matrix; + Vector result(matrix_.size()); + Eigen::Map(&result(0), rows(), cols()) = matrix_; + return result; + } + + /** Identity function to satisfy VectorSpace traits. + * + * NOTE: The size at compile time is unknown so this identity is zero + * length and thus not valid. + */ + inline static ParameterMatrix identity() { + // throw std::runtime_error( + // "ParameterMatrix::identity(): Don't use this function"); + return ParameterMatrix(0); + } + + /// @} +}; + +// traits for ParameterMatrix +template +struct traits> + : public internal::VectorSpace> {}; + +/* ************************************************************************* */ +// Stream operator that takes a ParameterMatrix. Used for printing. +template +inline std::ostream& operator<<(std::ostream& os, + const ParameterMatrix& parameterMatrix) { + os << parameterMatrix.matrix(); + return os; +} + +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/basis/basis.i b/gtsam/basis/basis.i new file mode 100644 index 000000000..8f06fd2e1 --- /dev/null +++ b/gtsam/basis/basis.i @@ -0,0 +1,146 @@ +//************************************************************************* +// basis +//************************************************************************* + +namespace gtsam { + +// TODO(gerry): add all the Functors to the Basis interfaces, e.g. +// `EvaluationFunctor` + +#include + +class FourierBasis { + static Vector CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); + + static Matrix DifferentiationMatrix(size_t N); + static Vector DerivativeWeights(size_t N, double x); +}; + +#include + +class Chebyshev1Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector X); +}; + +class Chebyshev2Basis { + static Matrix CalculateWeights(size_t N, double x); + static Matrix WeightMatrix(size_t N, Vector x); +}; + +#include +class Chebyshev2 { + static double Point(size_t N, int j); + static double Point(size_t N, int j, double a, double b); + + static Vector Points(size_t N); + static Vector Points(size_t N, double a, double b); + + static Matrix WeightMatrix(size_t N, Vector X); + static Matrix WeightMatrix(size_t N, Vector X, double a, double b); + + static Matrix CalculateWeights(size_t N, double x, double a, double b); + static Matrix DerivativeWeights(size_t N, double x, double a, double b); + static Matrix IntegrationWeights(size_t N, double a, double b); + static Matrix DifferentiationMatrix(size_t N, double a, double b); + + // TODO Needs OptionalJacobian + // static double Derivative(double x, Vector f); +}; + +#include + +template +class ParameterMatrix { + ParameterMatrix(const size_t N); + ParameterMatrix(const Matrix& matrix); + + Matrix matrix() const; + + void print(const string& s = "") const; +}; + +#include + +template +virtual class EvaluationFactor : gtsam::NoiseModelFactor { + EvaluationFactor(); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + EvaluationFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +template +virtual class VectorEvaluationFactor : gtsam::NoiseModelFactor { + VectorEvaluationFactor(); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + VectorEvaluationFactor(gtsam::Key key, const Vector& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(Varun) Better way to support arbitrary dimensions? +// Especially if users mainly do `pip install gtsam` for the Python wrapper. +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D3; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D4; +typedef gtsam::VectorEvaluationFactor + VectorEvaluationFactorChebyshev2D12; + +template +virtual class VectorComponentFactor : gtsam::NoiseModelFactor { + VectorComponentFactor(); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x); + VectorComponentFactor(gtsam::Key key, const double z, + const gtsam::noiseModel::Base* model, const size_t N, + size_t i, double x, double a, double b); +}; + +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D3; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D4; +typedef gtsam::VectorComponentFactor + VectorComponentFactorChebyshev2D12; + +template +virtual class ManifoldEvaluationFactor : gtsam::NoiseModelFactor { + ManifoldEvaluationFactor(); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x); + ManifoldEvaluationFactor(gtsam::Key key, const T& z, + const gtsam::noiseModel::Base* model, const size_t N, + double x, double a, double b); +}; + +// TODO(gerry): Add `DerivativeFactor`, `VectorDerivativeFactor`, and +// `ComponentDerivativeFactor` + +#include +template +class FitBasis { + FitBasis(const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + + static gtsam::NonlinearFactorGraph NonlinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + static gtsam::GaussianFactorGraph::shared_ptr LinearGraph( + const std::map& sequence, + const gtsam::noiseModel::Base* model, size_t N); + Parameters parameters() const; +}; + +} // namespace gtsam diff --git a/gtsam/basis/tests/CMakeLists.txt b/gtsam/basis/tests/CMakeLists.txt new file mode 100644 index 000000000..63cad0be6 --- /dev/null +++ b/gtsam/basis/tests/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTestsGlob(basis "test*.cpp" "" "gtsam") diff --git a/gtsam/basis/tests/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp new file mode 100644 index 000000000..64c925886 --- /dev/null +++ b/gtsam/basis/tests/testChebyshev.cpp @@ -0,0 +1,236 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +const size_t N = 3; + +//****************************************************************************** +TEST(Chebyshev, Chebyshev1) { + using Synth = Chebyshev1Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 3 * x + 2 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Chebyshev2) { + using Synth = Chebyshev2Basis::EvaluationFunctor; + Vector c(N); + double x; + c << 12, 3, 1; + x = -1.0; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = -0.5; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); + x = 0.3; + EXPECT_DOUBLES_EQUAL(12 + 6 * x + 4 * x * x - 1, Synth(N, x)(c), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev, Evaluation) { + Chebyshev1Basis::EvaluationFunctor fx(N, 0.5); + Vector c(N); + c << 3, 5, -12; + EXPECT_DOUBLES_EQUAL(11.5, fx(c), 1e-9); +} + +//****************************************************************************** +#include +#include +TEST(Chebyshev, Expression) { + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + + // Let's pretend we have 6 GPS measurements (we just do x coordinate) + // at times + const size_t m = 6; + Vector t(m); + t << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + Vector x(m); + x << -0.7, -0.4, 0.1, 0.3, 0.7, 0.9; + + for (size_t i = 0; i < m; i++) { + graph.emplace_shared>(key, x(i), model, N, + t(i)); + } + + // Solve + Values initial; + initial.insert(key, Vector::Zero(N)); // initial does not matter + + // ... and optimize + GaussNewtonParams parameters; + GaussNewtonOptimizer optimizer(graph, initial, parameters); + Values result = optimizer.optimize(); + + // Check + Vector expected(N); + expected << 0, 1, 0; + Vector actual_c = result.at(key); + EXPECT(assert_equal(expected, actual_c)); + + // Calculate and print covariances + Marginals marginals(graph, result); + Matrix3 cov = marginals.marginalCovariance(key); + EXPECT_DOUBLES_EQUAL(0.626, cov(1, 1), 1e-3); + + // Predict x at time 1.0 + Chebyshev1Basis::EvaluationFunctor f(N, 1.0); + Matrix H; + double actual = f(actual_c, H); + EXPECT_DOUBLES_EQUAL(1.0, actual, 1e-9); + + // Calculate predictive variance on prediction + double actual_variance_on_prediction = (H * cov * H.transpose())(0); + EXPECT_DOUBLES_EQUAL(1.1494, actual_variance_on_prediction, 1e-4); +} + +//****************************************************************************** +TEST(Chebyshev, Decomposition) { + const size_t M = 16; + + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < M; i++) { + double x = ((double)i / M); // - 0.99; + double y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, N); + + // Check + Vector expected = Vector::Zero(N); + expected(1) = 1; + EXPECT(assert_equal(expected, (Vector)actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev1, Derivative) { + Vector c(N); + c << 12, 3, 2; + + Weights D; + + double x = -1.0; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-5, (D * c)(0), 1e-9); + + x = -0.5; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(-1, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev1Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(5.4, (D * c)(0), 1e-9); +} + +//****************************************************************************** +Vector3 f(-6, 1, 0.5); + +double proxy1(double x, size_t N) { + return Chebyshev1Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev1, Derivative2) { + const double x = 0.5; + auto D = Chebyshev1Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy1, x, N); + // regression + EXPECT_DOUBLES_EQUAL(2, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(2, (D * f)(0), 1e-9); +} + +//****************************************************************************** +TEST(Chebyshev2, Derivative) { + Vector c(N); + c << 12, 6, 2; + + Weights D; + + double x = -1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + x = 1.0; + CHECK_EXCEPTION(Chebyshev2Basis::DerivativeWeights(N, x), std::runtime_error); + + x = -0.5; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(4, (D * c)(0), 1e-9); + + x = 0.3; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(16.8, (D * c)(0), 1e-9); + + x = 0.75; + D = Chebyshev2Basis::DerivativeWeights(N, x); + // regression + EXPECT_DOUBLES_EQUAL(24, (D * c)(0), 1e-9); + + x = 10; + D = Chebyshev2Basis::DerivativeWeights(N, x, 0, 20); + // regression + EXPECT_DOUBLES_EQUAL(12, (D * c)(0), 1e-9); +} + +//****************************************************************************** +double proxy2(double x, size_t N) { + return Chebyshev2Basis::EvaluationFunctor(N, x)(Vector(f)); +} + +TEST(Chebyshev2, Derivative2) { + const double x = 0.5; + auto D = Chebyshev2Basis::DerivativeWeights(N, x); + + Matrix numeric_dTdx = + numericalDerivative21(proxy2, x, N); + // regression + EXPECT_DOUBLES_EQUAL(4, numeric_dTdx(0, 0), 1e-9); + EXPECT_DOUBLES_EQUAL(4, (D * f)(0), 1e-9); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp new file mode 100644 index 000000000..4cee70daf --- /dev/null +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -0,0 +1,435 @@ +/* ---------------------------------------------------------------------------- + + * 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 testChebyshev.cpp + * @date July 4, 2020 + * @author Varun Agrawal + * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral + * methods + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; +using namespace boost::placeholders; + +noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1); + +const size_t N = 32; + +//****************************************************************************** +TEST(Chebyshev2, Point) { + static const int N = 5; + auto points = Chebyshev2::Points(N); + Vector expected(N); + expected << -1., -sqrt(2.) / 2., 0., sqrt(2.) / 2., 1.; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // Check symmetry + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 0), -Chebyshev2::Point(N, 4), tol); + EXPECT_DOUBLES_EQUAL(Chebyshev2::Point(N, 1), -Chebyshev2::Point(N, 3), tol); +} + +//****************************************************************************** +TEST(Chebyshev2, PointInInterval) { + static const int N = 5; + auto points = Chebyshev2::Points(N, 0, 20); + Vector expected(N); + expected << 0., 1. - sqrt(2.) / 2., 1., 1. + sqrt(2.) / 2., 2.; + expected *= 10.0; + static const double tol = 1e-15; // changing this reveals errors + EXPECT_DOUBLES_EQUAL(expected(0), points(0), tol); + EXPECT_DOUBLES_EQUAL(expected(1), points(1), tol); + EXPECT_DOUBLES_EQUAL(expected(2), points(2), tol); + EXPECT_DOUBLES_EQUAL(expected(3), points(3), tol); + EXPECT_DOUBLES_EQUAL(expected(4), points(4), tol); + + // all at once + Vector actual = Chebyshev2::Points(N, 0, 20); + CHECK(assert_equal(expected, actual)); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {0, 2}, {1, 6}}, 0.5] +TEST(Chebyshev2, Interpolate2) { + size_t N = 3; + Chebyshev2::EvaluationFunctor fx(N, 0.5); + Vector f(N); + f << 4, 2, 6; + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{0, 4}, {1, 2}, {2, 6}}, 1.5] +TEST(Chebyshev2, Interpolate2_Interval) { + Chebyshev2::EvaluationFunctor fx(3, 1.5, 0, 2); + Vector3 f(4, 2, 6); + EXPECT_DOUBLES_EQUAL(3.25, fx(f), 1e-9); +} + +//****************************************************************************** +// InterpolatingPolynomial[{{-1, 4}, {-Sqrt[2]/2, 2}, {0, 6}, {Sqrt[2]/2,3}, {1, +// 3}}, 0.5] +TEST(Chebyshev2, Interpolate5) { + Chebyshev2::EvaluationFunctor fx(5, 0.5); + Vector f(5); + f << 4, 2, 6, 3, 3; + EXPECT_DOUBLES_EQUAL(4.34283, fx(f), 1e-5); +} + +//****************************************************************************** +// Interpolating vectors +TEST(Chebyshev2, InterpolateVector) { + double t = 30, a = 0, b = 100; + const size_t N = 3; + // Create 2x3 matrix with Vectors at Chebyshev points + ParameterMatrix<2> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + + // Check value + Vector expected(2); + expected << t, 0; + Eigen::Matrix actualH(2, 2 * N); + + Chebyshev2::VectorEvaluationFunctor<2> fx(N, t, a, b); + EXPECT(assert_equal(expected, fx(X, actualH), 1e-9)); + + // Check derivative + boost::function)> f = boost::bind( + &Chebyshev2::VectorEvaluationFunctor<2>::operator(), fx, _1, boost::none); + Matrix numericalH = + numericalDerivative11, 2 * N>(f, X); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); +} + +//****************************************************************************** +TEST(Chebyshev2, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = (double)i / 16. - 0.99, y = x; + sequence[x] = y; + } + + // Do Chebyshev Decomposition + FitBasis actual(sequence, model, 3); + + // Check + Vector expected(3); + expected << -1, 0, 1; + EXPECT(assert_equal(expected, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DifferentiationMatrix3) { + // Trefethen00book, p.55 + const size_t N = 3; + Matrix expected(N, N); + // Differentiation matrix computed from Chebfun + expected << 1.5000, -2.0000, 0.5000, // + 0.5000, -0.0000, -0.5000, // + -0.5000, 2.0000, -1.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal(expected, actual, 1e-4)); +} + +//****************************************************************************** +TEST(Chebyshev2, DerivativeMatrix6) { + // Trefethen00book, p.55 + const size_t N = 6; + Matrix expected(N, N); + expected << 8.5000, -10.4721, 2.8944, -1.5279, 1.1056, -0.5000, // + 2.6180, -1.1708, -2.0000, 0.8944, -0.6180, 0.2764, // + -0.7236, 2.0000, -0.1708, -1.6180, 0.8944, -0.3820, // + 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // + -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // + 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; + // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // This was verified with chebfun + expected = -expected; + + Matrix actual = Chebyshev2::DifferentiationMatrix(N); + EXPECT(assert_equal((Matrix)expected, actual, 1e-4)); +} + +// test function for CalculateWeights and DerivativeWeights +double f(double x) { + // return 3*(x**3) - 2*(x**2) + 5*x - 11 + return 3.0 * pow(x, 3) - 2.0 * pow(x, 2) + 5.0 * x - 11; +} + +// its derivative +double fprime(double x) { + // return 9*(x**2) - 4*(x) + 5 + return 9.0 * pow(x, 2) - 4.0 * x + 5.0; +} + +//****************************************************************************** +TEST(Chebyshev2, CalculateWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376; + Weights weights1 = Chebyshev2::CalculateWeights(N, x1); + Weights weights2 = Chebyshev2::CalculateWeights(N, x2); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + EXPECT_DOUBLES_EQUAL(f(x2), weights2 * fvals, 1e-8); +} + +TEST(Chebyshev2, CalculateWeights2) { + double a = 0, b = 10, x1 = 7, x2 = 4.12; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights weights1 = Chebyshev2::CalculateWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(f(x1), weights1 * fvals, 1e-8); + + Weights weights2 = Chebyshev2::CalculateWeights(N, x2, a, b); + double expected2 = f(x2); // 185.454784 + double actual2 = weights2 * fvals; + EXPECT_DOUBLES_EQUAL(expected2, actual2, 1e-8); +} + +TEST(Chebyshev2, DerivativeWeights) { + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i)); + } + double x1 = 0.7, x2 = -0.376, x3 = 0.0; + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-9); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-9); + + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-9); + + // test if derivative calculation and cheb point is correct + double x4 = Chebyshev2::Point(N, 3); + Weights dWeights4 = Chebyshev2::DerivativeWeights(N, x4); + EXPECT_DOUBLES_EQUAL(fprime(x4), dWeights4 * fvals, 1e-9); +} + +TEST(Chebyshev2, DerivativeWeights2) { + double x1 = 5, x2 = 4.12, a = 0, b = 10; + + Eigen::Matrix fvals(N); + for (size_t i = 0; i < N; i++) { + fvals(i) = f(Chebyshev2::Point(N, i, a, b)); + } + + Weights dWeights1 = Chebyshev2::DerivativeWeights(N, x1, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x1), dWeights1 * fvals, 1e-8); + + Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); + + // test if derivative calculation and cheb point is correct + double x3 = Chebyshev2::Point(N, 3, a, b); + Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); + EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeightsDifferentiationMatrix) { + const size_t N6 = 6; + double x1 = 0.311; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Weights expected = Chebyshev2::CalculateWeights(N6, x1) * D6; + Weights actual = Chebyshev2::DerivativeWeights(N6, x1); + EXPECT(assert_equal(expected, actual, 1e-12)); + + double a = -3, b = 8, x2 = 5.05; + Matrix D6_2 = Chebyshev2::DifferentiationMatrix(N6, a, b); + Weights expected1 = Chebyshev2::CalculateWeights(N6, x2, a, b) * D6_2; + Weights actual1 = Chebyshev2::DerivativeWeights(N6, x2, a, b); + EXPECT(assert_equal(expected1, actual1, 1e-12)); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights6) { + const size_t N6 = 6; + Matrix D6 = Chebyshev2::DifferentiationMatrix(N6); + Chebyshev2::Parameters x = Chebyshev2::Points(N6); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N6), Vector(D6 * x))); +} + +//****************************************************************************** +// Check two different ways to calculate the derivative weights +TEST(Chebyshev2, DerivativeWeights7) { + const size_t N7 = 7; + Matrix D7 = Chebyshev2::DifferentiationMatrix(N7); + Chebyshev2::Parameters x = Chebyshev2::Points(N7); // ramp with slope 1 + EXPECT(assert_equal(Vector::Ones(N7), Vector(D7 * x))); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +Vector6 f3_at_6points = (Vector6() << 4, 2, 6, 2, 4, 3).finished(); +double proxy3(double x) { + return Chebyshev2::EvaluationFunctor(6, x)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6) { + // Check Derivative evaluation at point x=0.2 + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy3, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Assert that derivative also works in non-standard interval [0,3] +double proxy4(double x) { + return Chebyshev2::EvaluationFunctor(6, x, 0, 3)(f3_at_6points); +} + +TEST(Chebyshev2, Derivative6_03) { + // Check Derivative evaluation at point x=0.2, in interval [0,3] + + // calculate expected values by numerical derivative of synthesis + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy4, x); + + // Calculate derivatives at Chebyshev points using D3, interpolate + Matrix D6 = Chebyshev2::DifferentiationMatrix(6, 0, 3); + Vector derivative_at_points = D6 * f3_at_6points; + Chebyshev2::EvaluationFunctor fx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivative_at_points), 1e-8); + + // Do directly + Chebyshev2::DerivativeFunctor dfdx(6, x, 0, 3); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(f3_at_6points), 1e-8); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor +TEST(Chebyshev2, VectorDerivativeFunctor) { + const size_t N = 3, M = 2; + const double x = 0.2; + using VecD = Chebyshev2::VectorDerivativeFunctor; + VecD fx(N, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(M, M * N); + EXPECT(assert_equal(Vector::Zero(M), (Vector)fx(X, actualH), 1e-8)); + + // Test Jacobian + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +// Test VectorDerivativeFunctor with polynomial function +TEST(Chebyshev2, VectorDerivativeFunctor2) { + const size_t N = 64, M = 1, T = 15; + using VecD = Chebyshev2::VectorDerivativeFunctor; + + const Vector points = Chebyshev2::Points(N, 0, T); + + // Assign the parameter matrix + Vector values(N); + for (size_t i = 0; i < N; ++i) { + values(i) = f(points(i)); + } + ParameterMatrix X(values); + + // Evaluate the derivative at the chebyshev points using + // VectorDerivativeFunctor. + for (size_t i = 0; i < N; ++i) { + VecD d(N, points(i), 0, T); + Vector1 Dx = d(X); + EXPECT_DOUBLES_EQUAL(fprime(points(i)), Dx(0), 1e-6); + } + + // Test Jacobian at the first chebyshev point. + Matrix actualH(M, M * N); + VecD vecd(N, points(0), 0, T); + vecd(X, actualH); + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&VecD::operator(), vecd, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-6)); +} + +//****************************************************************************** +// Test ComponentDerivativeFunctor +TEST(Chebyshev2, ComponentDerivativeFunctor) { + const size_t N = 6, M = 2; + const double x = 0.2; + using CompFunc = Chebyshev2::ComponentDerivativeFunctor; + size_t row = 1; + CompFunc fx(N, row, x, 0, 3); + ParameterMatrix X(N); + Matrix actualH(1, M * N); + EXPECT_DOUBLES_EQUAL(0, fx(X, actualH), 1e-8); + + Matrix expectedH = numericalDerivative11, M * N>( + boost::bind(&CompFunc::operator(), fx, _1, boost::none), X); + EXPECT(assert_equal(expectedH, actualH, 1e-7)); +} + +//****************************************************************************** +TEST(Chebyshev2, IntegralWeights) { + const size_t N7 = 7; + Vector actual = Chebyshev2::IntegrationWeights(N7); + Vector expected = (Vector(N7) << 0.0285714285714286, 0.253968253968254, + 0.457142857142857, 0.520634920634921, 0.457142857142857, + 0.253968253968254, 0.0285714285714286) + .finished(); + EXPECT(assert_equal(expected, actual)); + + const size_t N8 = 8; + Vector actual2 = Chebyshev2::IntegrationWeights(N8); + Vector expected2 = (Vector(N8) << 0.0204081632653061, 0.190141007218208, + 0.352242423718159, 0.437208405798326, 0.437208405798326, + 0.352242423718159, 0.190141007218208, 0.0204081632653061) + .finished(); + EXPECT(assert_equal(expected2, actual2)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testFourier.cpp b/gtsam/basis/tests/testFourier.cpp new file mode 100644 index 000000000..7a53cfcc9 --- /dev/null +++ b/gtsam/basis/tests/testFourier.cpp @@ -0,0 +1,254 @@ +/* ---------------------------------------------------------------------------- + + * 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 testFourier.cpp + * @date July 4, 2020 + * @author Frank Dellaert, Varun Agrawal + * @brief Unit tests for Fourier Basis Decompositions with Expressions + */ + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +auto model = noiseModel::Unit::Create(1); + +// Coefficients for testing, respectively 3 and 7 parameter Fourier basis. +// They correspond to best approximation of TestFunction(x) +const Vector k3Coefficients = (Vector3() << 1.5661, 1.2717, 1.2717).finished(); +const Vector7 k7Coefficients = + (Vector7() << 1.5661, 1.2717, 1.2717, -0.0000, 0.5887, -0.0943, 0.0943) + .finished(); + +// The test-function used below +static double TestFunction(double x) { return exp(sin(x) + cos(x)); } + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctor) { + // fx(0) takes coefficients c to calculate the value f(c;x==0) + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients), 1e-9); +} + +//****************************************************************************** +TEST(Basis, BasisEvaluationFunctorDerivative) { + // If we give the H argument, we get the derivative of fx(0) wrpt coefficients + // Needs to be Matrix so it can be used by OptionalJacobian. + Matrix H(1, 3); + FourierBasis::EvaluationFunctor fx(3, 0); + EXPECT_DOUBLES_EQUAL(k3Coefficients[0] + k3Coefficients[1], + fx(k3Coefficients, H), 1e-9); + + Matrix13 expectedH(1, 1, 0); + EXPECT(assert_equal(expectedH, H)); +} + +//****************************************************************************** +TEST(Basis, Manual) { + const size_t N = 3; + + // We will create a linear factor graph + GaussianFactorGraph graph; + + // We create an unknown Vector expression for the coefficients + Key key(1); + + // We will need values below to test the Jacobians + Values values; + values.insert(key, Vector::Zero(N)); // value does not really matter + + // At 16 different samples points x, check Predict_ expression + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, N); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A, b); + graph.add(linearFactor); + + // Create factor to predict value at x + EvaluationFactor predictFactor(key, desiredValue, model, N, + x); + + // Check expression Jacobians + EXPECT_CORRECT_FACTOR_JACOBIANS(predictFactor, values, 1e-5, 1e-9); + + auto linearizedFactor = predictFactor.linearize(values); + auto linearizedJacobianFactor = + boost::dynamic_pointer_cast(linearizedFactor); + CHECK(linearizedJacobianFactor); // makes sure it's indeed a JacobianFactor + EXPECT(assert_equal(linearFactor, *linearizedJacobianFactor, 1e-9)); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)k3Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, EvaluationFactor) { + // Check fitting a function with a 7-parameter Fourier basis + + // Create linear factor graph + NonlinearFactorGraph graph; + Key key(1); + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, desiredValue = TestFunction(x); + graph.emplace_shared>(key, desiredValue, + model, 7, x); + } + + // Solve FourierFactorGraph + Values values; + values.insert(key, Vector::Zero(7)); + GaussianFactorGraph::shared_ptr lfg = graph.linearize(values); + VectorValues actual = lfg->optimize(); + + EXPECT(assert_equal((Vector)k7Coefficients, actual.at(key), 1e-4)); +} + +//****************************************************************************** +TEST(Basis, WeightMatrix) { + // The WeightMatrix creates an m*n matrix, where m is the number of sample + // points, and n is the number of parameters. + Matrix expected(2, 3); + expected.row(0) << 1, cos(1), sin(1); + expected.row(1) << 1, cos(2), sin(2); + Vector2 X(1, 2); + Matrix actual = FourierBasis::WeightMatrix(3, X); + EXPECT(assert_equal(expected, actual, 1e-9)); +} + +//****************************************************************************** +TEST(Basis, Decomposition) { + // Create example sequence + Sequence sequence; + for (size_t i = 0; i < 16; i++) { + double x = i * M_PI / 8, y = TestFunction(x); + sequence[x] = y; + } + + // Do Fourier Decomposition + FitBasis actual(sequence, model, 3); + + // Check + EXPECT(assert_equal((Vector)k3Coefficients, actual.parameters(), 1e-4)); +} + +//****************************************************************************** +// Check derivative in two different ways: numerical and using D on f +double proxy(double x) { + return FourierBasis::EvaluationFunctor(7, x)(k7Coefficients); +} + +TEST(Basis, Derivative7) { + // Check Derivative evaluation at point x=0.2 + + // Calculate expected values by numerical derivative of proxy. + const double x = 0.2; + Matrix numeric_dTdx = numericalDerivative11(proxy, x); + + // Calculate derivatives at Chebyshev points using D7, interpolate + Matrix D7 = FourierBasis::DifferentiationMatrix(7); + Vector derivativeCoefficients = D7 * k7Coefficients; + FourierBasis::EvaluationFunctor fx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), fx(derivativeCoefficients), 1e-8); + + // Do directly + FourierBasis::DerivativeFunctor dfdx(7, x); + EXPECT_DOUBLES_EQUAL(numeric_dTdx(0, 0), dfdx(k7Coefficients), 1e-8); +} + +//****************************************************************************** +TEST(Basis, VecDerivativeFunctor) { + using DotShape = typename FourierBasis::VectorDerivativeFunctor<2>; + const size_t N = 3; + + // MATLAB example, Dec 27 2019, commit 014eded5 + double h = 2 * M_PI / 16; + Vector2 dotShape(0.5556, -0.8315); // at h/2 + DotShape dotShapeFunction(N, h / 2); + Matrix23 theta_mat = (Matrix32() << 0, 0, 0.7071, 0.7071, 0.7071, -0.7071) + .finished() + .transpose(); + ParameterMatrix<2> theta(theta_mat); + EXPECT(assert_equal(Vector(dotShape), dotShapeFunction(theta), 1e-4)); +} + +//****************************************************************************** +// Suppose we want to parameterize a periodic function with function values at +// specific times, rather than coefficients. Can we do it? This would be a +// generalization of the Fourier transform, and constitute a "pseudo-spectral" +// parameterization. One way to do this is to establish hard constraints that +// express the relationship between the new parameters and the coefficients. +// For example, below I'd like the parameters to be the function values at +// X = {0.1,0.2,0.3}, rather than a 3-vector of coefficients. +// Because the values f(X) = at these points can be written as f(X) = W(X)*c, +// we can simply express the coefficents c as c=inv(W(X))*f, which is a +// generalized Fourier transform. That also means we can create factors with the +// unknown f-values, as done manually below. +TEST(Basis, PseudoSpectral) { + // We will create a linear factor graph + GaussianFactorGraph graph; + + const size_t N = 3; + const Key key(1); + + // The correct values at X = {0.1,0.2,0.3} are simply W*c + const Vector X = (Vector3() << 0.1, 0.2, 0.3).finished(); + const Matrix W = FourierBasis::WeightMatrix(N, X); + const Vector expected = W * k3Coefficients; + + // Check those values are indeed correct values of Fourier approximation + using Eval = FourierBasis::EvaluationFunctor; + EXPECT_DOUBLES_EQUAL(Eval(N, 0.1)(k3Coefficients), expected(0), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.2)(k3Coefficients), expected(1), 1e-9); + EXPECT_DOUBLES_EQUAL(Eval(N, 0.3)(k3Coefficients), expected(2), 1e-9); + + // Calculate "inverse Fourier transform" matrix + const Matrix invW = W.inverse(); + + // At 16 different samples points x, add a factor on fExpr + for (size_t i = 0; i < 16; i++) { + const double x = i * M_PI / 8; + const double desiredValue = TestFunction(x); + + // Manual JacobianFactor + Matrix A(1, 3); + A << 1, cos(x), sin(x); + Vector b(1); + b << desiredValue; + JacobianFactor linearFactor(key, A * invW, b); + graph.add(linearFactor); + } + + // Solve linear graph + VectorValues actual = graph.optimize(); + EXPECT(assert_equal((Vector)expected, actual.at(key), 1e-4)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/basis/tests/testParameterMatrix.cpp b/gtsam/basis/tests/testParameterMatrix.cpp new file mode 100644 index 000000000..ec62e8eea --- /dev/null +++ b/gtsam/basis/tests/testParameterMatrix.cpp @@ -0,0 +1,145 @@ +/* ---------------------------------------------------------------------------- + + * 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 testParameterMatrix.cpp + * @date Sep 22, 2020 + * @author Varun Agrawal, Frank Dellaert + * @brief Unit tests for ParameterMatrix.h + */ + +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +using Parameters = Chebyshev2::Parameters; + +const size_t M = 2, N = 5; + +//****************************************************************************** +TEST(ParameterMatrix, Constructor) { + ParameterMatrix<2> actual1(3); + ParameterMatrix<2> expected1(Matrix::Zero(2, 3)); + EXPECT(assert_equal(expected1, actual1)); + + ParameterMatrix<2> actual2(Matrix::Ones(2, 3)); + ParameterMatrix<2> expected2(Matrix::Ones(2, 3)); + EXPECT(assert_equal(expected2, actual2)); + EXPECT(assert_equal(expected2.matrix(), actual2.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Dimensions) { + ParameterMatrix params(N); + EXPECT_LONGS_EQUAL(params.rows(), M); + EXPECT_LONGS_EQUAL(params.cols(), N); + EXPECT_LONGS_EQUAL(params.dim(), M * N); +} + +//****************************************************************************** +TEST(ParameterMatrix, Getters) { + ParameterMatrix params(N); + + Matrix expectedMatrix = Matrix::Zero(2, 5); + EXPECT(assert_equal(expectedMatrix, params.matrix())); + + Matrix expectedMatrixTranspose = Matrix::Zero(5, 2); + EXPECT(assert_equal(expectedMatrixTranspose, params.transpose())); + + ParameterMatrix p2(Matrix::Ones(M, N)); + Vector expectedRowVector = Vector::Ones(N); + for (size_t r = 0; r < M; ++r) { + EXPECT(assert_equal(p2.row(r), expectedRowVector)); + } + + ParameterMatrix p3(2 * Matrix::Ones(M, N)); + Vector expectedColVector = 2 * Vector::Ones(M); + for (size_t c = 0; c < M; ++c) { + EXPECT(assert_equal(p3.col(c), expectedColVector)); + } +} + +//****************************************************************************** +TEST(ParameterMatrix, Setters) { + ParameterMatrix params(Matrix::Zero(M, N)); + Matrix expected = Matrix::Zero(M, N); + + // row + params.row(0) = Vector::Ones(N); + expected.row(0) = Vector::Ones(N); + EXPECT(assert_equal(expected, params.matrix())); + + // col + params.col(2) = Vector::Ones(M); + expected.col(2) = Vector::Ones(M); + + EXPECT(assert_equal(expected, params.matrix())); + + // setZero + params.setZero(); + expected.setZero(); + EXPECT(assert_equal(expected, params.matrix())); +} + +//****************************************************************************** +TEST(ParameterMatrix, Addition) { + ParameterMatrix params(Matrix::Ones(M, N)); + ParameterMatrix expected(2 * Matrix::Ones(M, N)); + + // Add vector + EXPECT(assert_equal(expected, params + Vector::Ones(M * N))); + // Add another ParameterMatrix + ParameterMatrix actual = params + ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Subtraction) { + ParameterMatrix params(2 * Matrix::Ones(M, N)); + ParameterMatrix expected(Matrix::Ones(M, N)); + + // Subtract vector + EXPECT(assert_equal(expected, params - Vector::Ones(M * N))); + // Subtract another ParameterMatrix + ParameterMatrix actual = params - ParameterMatrix(Matrix::Ones(M, N)); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +TEST(ParameterMatrix, Multiplication) { + ParameterMatrix params(Matrix::Ones(M, N)); + Matrix multiplier = 2 * Matrix::Ones(N, 2); + Matrix expected = 2 * N * Matrix::Ones(M, 2); + EXPECT(assert_equal(expected, params * multiplier)); +} + +//****************************************************************************** +TEST(ParameterMatrix, VectorSpace) { + ParameterMatrix params(Matrix::Ones(M, N)); + // vector + EXPECT(assert_equal(Vector::Ones(M * N), params.vector())); + // identity + EXPECT(assert_equal(ParameterMatrix::identity(), + ParameterMatrix(Matrix::Zero(M, 0)))); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index c09eba9bb..143d4bc3c 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -147,113 +147,123 @@ public: * G = F' * F - F' * E * P * E' * F * g = F' * (b - E * P * E' * b) * Fixed size version - */ - template // N = 2 or 3 (point dimension), ND is the camera dimension - static SymmetricBlockMatrix SchurComplement( - const std::vector< Eigen::Matrix, Eigen::aligned_allocator< Eigen::Matrix > >& Fs, - const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + */ + template // N = 2 or 3 (point dimension), ND is the camera dimension + static SymmetricBlockMatrix SchurComplement( + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, + const Matrix& E, const Eigen::Matrix& P, const Vector& b) { + // a single point is observed in m cameras + size_t m = Fs.size(); - // 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)); - // 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 - // 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(ZDim * i, 0, ZDim, N) * P; - const Eigen::Matrix& Fi = Fs[i]; - const auto FiT = Fi.transpose(); - const Eigen::Matrix Ei_P = // - E.block(ZDim * i, 0, ZDim, N) * P; + // D = (Dx2) * ZDim + augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * i) // F' * b + - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1) - // D = (Dx2) * ZDim - augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment(ZDim * 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(ZDim * i, 0, ZDim, N).transpose() * Fi)); - // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) ) - augmentedHessian.setDiagonalBlock(i, FiT - * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, 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]; - // 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(ZDim * j, 0, ZDim, N).transpose() * Fj)); + } + } // end of for over cameras - // (DxD) = (Dx2) * ( (2x2) * (2xD) ) - augmentedHessian.setOffDiagonalBlock(i, j, -FiT - * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); - } - } // end of for over cameras - - augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); - return augmentedHessian; - } + 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) - * In this version, we allow for the case where the keys in the Jacobian are organized - * 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. + * In this version, we allow for the case where the keys in the Jacobian are + * organized 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. + * + * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is + * the Hessian block dimension */ - template // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension + template static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( - const std::vector, - Eigen::aligned_allocator > >& Fs, + const std::vector< + Eigen::Matrix, + Eigen::aligned_allocator>>& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b, - const KeyVector jacobianKeys, const KeyVector hessianKeys) { - + 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); + // 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) { @@ -267,45 +277,20 @@ 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)); } 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); - } - - /** - * non-templated version of function above - */ - static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks_3_6_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,6,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 e583c0e80..144f28314 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/nonlinear/FunctorizedFactor.h b/gtsam/nonlinear/FunctorizedFactor.h index 691ab8ac8..e1f8ece8d 100644 --- a/gtsam/nonlinear/FunctorizedFactor.h +++ b/gtsam/nonlinear/FunctorizedFactor.h @@ -110,7 +110,7 @@ class GTSAM_EXPORT FunctorizedFactor : public NoiseModelFactor1 { bool equals(const NonlinearFactor &other, double tol = 1e-9) const override { const FunctorizedFactor *e = dynamic_cast *>(&other); - return e && Base::equals(other, tol) && + return e != nullptr && Base::equals(other, tol) && traits::Equals(this->measured_, e->measured_, tol); } /// @} diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index b0ec5e722..14a14fc19 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -20,8 +20,12 @@ #include #include #include +#include +#include +#include #include #include +#include #include using namespace std; @@ -60,7 +64,7 @@ class ProjectionFunctor { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; } @@ -255,18 +259,148 @@ TEST(FunctorizedFactor, Lambda2) { if (H1) { H1->resize(x.size(), A.size()); *H1 << I_3x3, I_3x3, I_3x3; - } + } if (H2) *H2 = A; return A * x; }; // FunctorizedFactor factor(key, measurement, model, lambda); - auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, model2, lambda); + auto factor = MakeFunctorizedFactor2(keyA, keyx, measurement, + model2, lambda); Vector error = factor.evaluateError(A, x); EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } +const size_t N = 2; + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = noiseModel::Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + const size_t M = 4; + + double measured = 0; + auto model = noiseModel::Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index 929ec41f7..b2076cc14 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -305,7 +305,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { // 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_6_6( + Base::Cameras::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); return boost::make_shared < RegularHessianFactor 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 5827a538c..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 - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast < gtsam::NonlinearFactor - > (gtsam::NonlinearFactor::shared_ptr(new This(*this))); + gtsam::NonlinearFactor::shared_ptr clone() const override { + 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()); @@ -150,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++) { @@ -173,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 = "); @@ -216,41 +237,50 @@ 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{ keyPairsEqual = false; } + } 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; + 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; } + } 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; } /** * 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 body poses we interpolate from), the point Jacobian E, - * and the error vector b. Note that the jacobians are computed for a given point. + * @return Return arguments are the camera jacobians Fs (including the + * jacobian with 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, const Values& values) const { @@ -258,33 +288,38 @@ 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 - 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); + auto w_P_body1 = values.at(world_P_body_key_pairs_[i].first); + auto w_P_body2 = values.at(world_P_body_key_pairs_[i].second); 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]; - const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); + auto w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor, + dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); + auto body_P_cam = body_P_sensors_[i]; + auto 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)); + 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); @@ -296,37 +331,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor > createHessianFactor( - const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const { - - // 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(); // note: by construction, keys_ only contains unique keys + boost::shared_ptr> createHessianFactor( + const Values& values, const double lambda = 0.0, + bool diagonalDamping = false) const { + // 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(); // 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); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "measured_.size() inconsistent with input"); + if (this->measured_.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 @@ -342,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); } /** @@ -365,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; } } @@ -385,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]); @@ -397,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 bf8a8c4ab..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,57 +129,67 @@ 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); 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 + { // 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(!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 + { // 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(!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 + { // 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(!assert_equal(*factor1, *factor2)); - EXPECT(!assert_equal(*factor1, *factor3)); + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); } } -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); } /* ************************************************************************* */ - diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index bdda15acb..a5fdc80a6 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -61,6 +61,7 @@ set(interface_headers ${PROJECT_SOURCE_DIR}/gtsam/slam/slam.i ${PROJECT_SOURCE_DIR}/gtsam/sfm/sfm.i ${PROJECT_SOURCE_DIR}/gtsam/navigation/navigation.i + ${PROJECT_SOURCE_DIR}/gtsam/basis/basis.i ) diff --git a/python/gtsam/preamble/basis.h b/python/gtsam/preamble/basis.h new file mode 100644 index 000000000..d07a75f6f --- /dev/null +++ b/python/gtsam/preamble/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `PYBIND11_MAKE_OPAQUE` will mark the type as "opaque" for the pybind11 + * automatic STL binding, such that the raw objects can be accessed in Python. + * Without this they will be automatically converted to a Python object, and all + * mutations on Python side will not be reflected on C++. + */ diff --git a/python/gtsam/specializations/basis.h b/python/gtsam/specializations/basis.h new file mode 100644 index 000000000..da8842eaf --- /dev/null +++ b/python/gtsam/specializations/basis.h @@ -0,0 +1,12 @@ +/* Please refer to: + * https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html + * These are required to save one copy operation on Python calls. + * + * NOTES + * ================= + * + * `py::bind_vector` and similar machinery gives the std container a Python-like + * interface, but without the `` copying mechanism. Combined + * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, + * and saves one copy operation. + */ From 0b9c368acaebf5bfa4a1e5a64f6301289fffec27 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:33:13 -0400 Subject: [PATCH 017/110] Removed types defined in Base class --- gtsam/slam/SmartProjectionFactorP.h | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionFactorP.h index b2076cc14..b01420446 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionFactorP.h @@ -71,8 +71,6 @@ class SmartProjectionFactorP : public SmartProjectionFactor { public: typedef CAMERA Camera; typedef CameraSet Cameras; - typedef Eigen::Matrix MatrixZD; // F blocks - typedef std::vector > FBlocks; // vector of F blocks /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -234,7 +232,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * 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, + void computeJacobiansWithTriangulatedPoint(typename Base::FBlocks& Fs, + Matrix& E, Vector& b, const Cameras& cameras) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); @@ -289,7 +288,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } // compute Jacobian given triangulated 3D Point - FBlocks Fs; + typename Base::FBlocks Fs; Matrix E; Vector b; this->computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); @@ -300,7 +299,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } - Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); + const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); // 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_ From e0946dfc8653b007658eb212f4ff381ea8644f47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:33:50 -0400 Subject: [PATCH 018/110] Documented linear factors better. --- gtsam/slam/JacobianFactorSVD.h | 55 +++++++++++++------------ gtsam/slam/RegularImplicitSchurFactor.h | 41 ++++++++++++++---- 2 files changed, 61 insertions(+), 35 deletions(-) diff --git a/gtsam/slam/JacobianFactorSVD.h b/gtsam/slam/JacobianFactorSVD.h index bc906d24e..f6bc1dd8c 100644 --- a/gtsam/slam/JacobianFactorSVD.h +++ b/gtsam/slam/JacobianFactorSVD.h @@ -9,20 +9,21 @@ namespace gtsam { /** - * JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis + * JacobianFactor for Schur complement that uses the "Nullspace Trick" by + * Mourikis et al. * * This trick is equivalent to the Schur complement, but can be faster. - * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, - * is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., - * The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) - * where Enull is an m x (m-3) matrix - * Then Enull'*E*dp = 0, and + * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are + * poses, is multiplied by Enull, a matrix that spans the left nullspace of E, + * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * + * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. * - * The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. - * Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) - * Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult + * The code below assumes that F is block diagonal and is given as a vector of + * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for + * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as + * a 1x2 * 2x6 multiplication. */ template class JacobianFactorSVD: public RegularJacobianFactor { @@ -37,10 +38,10 @@ public: JacobianFactorSVD() { } - /// Empty constructor with keys - JacobianFactorSVD(const KeyVector& keys, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + /// Empty constructor with keys. + JacobianFactorSVD(const KeyVector& keys, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { Matrix zeroMatrix = Matrix::Zero(0, D); Vector zeroVector = Vector::Zero(0); std::vector QF; @@ -51,18 +52,21 @@ public: } /** - * @brief Constructor - * Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) - * and a reduced point derivative, Enull - * and creates a reduced-rank Jacobian factor on the CameraSet + * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank + * Jacobian factor on the CameraSet. * - * @Fblocks: + * @param keys keys associated with F blocks. + * @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F + * @param Enull a reduced point derivative + * @param b right-hand side + * @param model noise model */ - JacobianFactorSVD(const KeyVector& keys, - const std::vector >& Fblocks, const Matrix& Enull, - const Vector& b, // - const SharedDiagonal& model = SharedDiagonal()) : - Base() { + JacobianFactorSVD( + const KeyVector& keys, + const std::vector >& Fblocks, + const Matrix& Enull, const Vector& b, + const SharedDiagonal& model = SharedDiagonal()) + : Base() { size_t numKeys = Enull.rows() / ZDim; size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? // PLAIN nullptr SPACE TRICK @@ -74,9 +78,8 @@ public: QF.reserve(numKeys); for (size_t k = 0; k < Fblocks.size(); ++k) { Key key = keys[k]; - QF.push_back( - KeyMatrix(key, - (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k])); + QF.emplace_back( + key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]); } JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); } diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 2ed6aa491..b4a341719 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -1,6 +1,6 @@ /** * @file RegularImplicitSchurFactor.h - * @brief A new type of linear factor (GaussianFactor), which is subclass of GaussianFactor + * @brief A subclass of GaussianFactor specialized to structureless SFM. * @author Frank Dellaert * @author Luca Carlone */ @@ -20,6 +20,20 @@ namespace gtsam { /** * RegularImplicitSchurFactor + * + * A specialization of a GaussianFactor to structure-less SFM, which is very + * fast in a conjugate gradient (CG) solver. Specifically, as measured in + * timeSchurFactors.cpp, it stays very fast for an increasing number of cameras. + * The magic is in multiplyHessianAdd, which does the Hessian-vector multiply at + * the core of CG, and implements + * y += F'*alpha*(I - E*P*E')*F*x + * where + * - F is the 2mx6m Jacobian of the m 2D measurements wrpt m 6DOF poses + * - E is the 2mx3 Jacabian of the m 2D measurements wrpt a 3D point + * - P is the covariance on the point + * The equation above implicitly executes the Schur complement by removing the + * information E*P*E' from the Hessian. It is also very fast as we do not use + * the full 6m*6m F matrix, but rather only it's m 6x6 diagonal blocks. */ template class RegularImplicitSchurFactor: public GaussianFactor { @@ -38,9 +52,10 @@ protected: static const int ZDim = traits::dimension; ///< Measurement dimension typedef Eigen::Matrix MatrixZD; ///< type of an F block - typedef Eigen::Matrix MatrixDD; ///< camera hessian + typedef Eigen::Matrix MatrixDD; ///< camera Hessian + typedef std::vector > FBlocks; - const std::vector > FBlocks_; ///< All ZDim*D F blocks (one for each camera) + FBlocks FBlocks_; ///< All ZDim*D F blocks (one for each camera) const Matrix PointCovariance_; ///< the 3*3 matrix P = inv(E'E) (2*2 if degenerate) const Matrix E_; ///< The 2m*3 E Jacobian with respect to the point const Vector b_; ///< 2m-dimensional RHS vector @@ -52,17 +67,25 @@ public: } /// Construct from blocks of F, E, inv(E'*E), and RHS vector b - RegularImplicitSchurFactor(const KeyVector& keys, - const std::vector >& FBlocks, const Matrix& E, const Matrix& P, - const Vector& b) : - GaussianFactor(keys), FBlocks_(FBlocks), PointCovariance_(P), E_(E), b_(b) { - } + + /** + * @brief Construct a new RegularImplicitSchurFactor object. + * + * @param keys keys corresponding to cameras + * @param Fs All ZDim*D F blocks (one for each camera) + * @param E Jacobian of measurements wrpt point. + * @param P point covariance matrix + * @param b RHS vector + */ + RegularImplicitSchurFactor(const KeyVector& keys, const FBlocks& Fs, + const Matrix& E, const Matrix& P, const Vector& b) + : GaussianFactor(keys), FBlocks_(Fs), PointCovariance_(P), E_(E), b_(b) {} /// Destructor ~RegularImplicitSchurFactor() override { } - std::vector >& FBlocks() const { + const FBlocks& Fs() const { return FBlocks_; } From 4ef234bbbb485cdee26576ae0ecc0995812d91fe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:46:53 -0400 Subject: [PATCH 019/110] Formatting and better documentation --- gtsam/slam/ProjectionFactor.h | 17 +- gtsam/slam/SmartFactorBase.h | 155 ++++++++++-------- gtsam/slam/SmartProjectionFactor.h | 106 +++++++----- .../slam/SmartStereoProjectionFactor.h | 22 +-- .../slam/SmartStereoProjectionFactorPP.h | 8 +- 5 files changed, 173 insertions(+), 135 deletions(-) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index ada304f27..169fe8a0a 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -11,7 +11,7 @@ /** * @file ProjectionFactor.h - * @brief Basic bearing factor from 2D measurement + * @brief Reprojection of a LANDMARK to a 2D point. * @author Chris Beall * @author Richard Roberts * @author Frank Dellaert @@ -22,17 +22,20 @@ #include #include +#include +#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. + * Non-linear factor for a constraint derived from a 2D measurement. + * The calibration is known here. + * The main building block for visual SLAM. * @addtogroup SLAM */ - template + template class GenericProjectionFactor: public NoiseModelFactor2 { protected: @@ -57,9 +60,9 @@ namespace gtsam { typedef boost::shared_ptr shared_ptr; /// Default constructor - GenericProjectionFactor() : - measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { - } + GenericProjectionFactor() : + measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) { + } /** * Constructor diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e7584a4da..f815200ce 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -37,12 +37,14 @@ namespace gtsam { /** - * @brief Base class for smart factors + * @brief Base class for smart factors. * This base class has no internal point, but it has a measurement, noise model * and an optional sensor pose. - * This class mainly computes the derivatives and returns them as a variety of factors. - * The methods take a Cameras argument, which should behave like PinholeCamera, and - * the value of a point, which is kept in the base class. + * This class mainly computes the derivatives and returns them as a variety of + * factors. The methods take a CameraSet argument and the value of a + * point, which is kept in the derived class. + * + * @tparam CAMERA should behave like a set of PinholeCamera. */ template class SmartFactorBase: public NonlinearFactor { @@ -64,19 +66,20 @@ protected: /** * As of Feb 22, 2015, the noise model is the same for all measurements and * is isotropic. This allows for moving most calculations of Schur complement - * etc to be moved to CameraSet very easily, and also agrees pragmatically + * etc. to be easily moved to CameraSet, and also agrees pragmatically * with what is normally done. */ SharedIsotropic noiseModel_; /** - * 2D measurement and noise model for each of the m views - * We keep a copy of measurements for I/O and computing the error. + * Measurements for each of the m views. + * We keep a copy of the measurements for I/O and computing the error. * The order is kept the same as the keys that we use to create the factor. */ ZVector measured_; - boost::optional body_P_sensor_; ///< Pose of the camera in the body frame + boost::optional + body_P_sensor_; ///< Pose of the camera in the body frame // Cache for Fblocks, to avoid a malloc ever time we re-linearize mutable FBlocks Fs; @@ -84,16 +87,16 @@ protected: public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW - /// shorthand for a smart pointer to a factor + /// shorthand for a smart pointer to a factor. typedef boost::shared_ptr shared_ptr; - /// We use the new CameraSte data structure to refer to a set of cameras + /// The CameraSet data structure is used to refer to a set of cameras. typedef CameraSet Cameras; - /// Default Constructor, for serialization + /// Default Constructor, for serialization. SmartFactorBase() {} - /// Constructor + /// Construct with given noise model and optional arguments. SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none, size_t expectedNumberCameras = 10) @@ -111,12 +114,12 @@ protected: noiseModel_ = sharedIsotropic; } - /// Virtual destructor, subclasses from NonlinearFactor + /// Virtual destructor, subclasses from NonlinearFactor. ~SmartFactorBase() override { } /** - * Add a new measurement and pose/camera key + * Add a new measurement and pose/camera key. * @param measured is the 2m dimensional projection of a single landmark * @param key is the index corresponding to the camera observing the landmark */ @@ -129,9 +132,7 @@ protected: this->keys_.push_back(key); } - /** - * Add a bunch of measurements, together with the camera keys - */ + /// Add a bunch of measurements, together with the camera keys. void add(const ZVector& measurements, const KeyVector& cameraKeys) { assert(measurements.size() == cameraKeys.size()); for (size_t i = 0; i < measurements.size(); i++) { @@ -140,8 +141,8 @@ protected: } /** - * Adds an entire SfM_track (collection of cameras observing a single point). - * The noise is assumed to be the same for all measurements + * Add an entire SfM_track (collection of cameras observing a single point). + * The noise is assumed to be the same for all measurements. */ template void add(const SFM_TRACK& trackToAdd) { @@ -151,17 +152,13 @@ protected: } } - /// get the dimension (number of rows!) of the factor - size_t dim() const override { - return ZDim * this->measured_.size(); - } + /// Return the dimension (number of rows!) of the factor. + size_t dim() const override { return ZDim * this->measured_.size(); } - /** return the measurements */ - const ZVector& measured() const { - return measured_; - } + /// Return the 2D measurements (ZDim, in general). + const ZVector& measured() const { return measured_; } - /// Collect all cameras: important that in key order + /// Collect all cameras: important that in key order. virtual Cameras cameras(const Values& values) const { Cameras cameras; for(const Key& k: this->keys_) @@ -188,25 +185,30 @@ protected: /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const This *e = dynamic_cast(&p); - - bool areMeasurementsEqual = true; - for (size_t i = 0; i < measured_.size(); i++) { - if (traits::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false) - areMeasurementsEqual = false; - break; + if (const This* e = dynamic_cast(&p)) { + // Check that all measurements are the same. + for (size_t i = 0; i < measured_.size(); i++) { + if (!traits::Equals(this->measured_.at(i), e->measured_.at(i), tol)) + return false; + } + // If so, check base class. + return Base::equals(p, tol); + } else { + return false; } - return e && Base::equals(p, tol) && areMeasurementsEqual; } /// Compute reprojection errors [h(x)-z] = [cameras.project(p)-z] and - /// derivatives + /// derivatives. This is the error before the noise model is applied. template Vector unwhitenedError( const Cameras& cameras, const POINT& point, boost::optional Fs = boost::none, // boost::optional E = boost::none) const { - Vector ue = cameras.reprojectionError(point, measured_, Fs, E); + // Reproject, with optional derivatives. + Vector error = cameras.reprojectionError(point, measured_, Fs, E); + + // Apply chain rule if body_P_sensor_ is given. if (body_P_sensor_ && Fs) { const Pose3 sensor_P_body = body_P_sensor_->inverse(); constexpr int camera_dim = traits::dimension; @@ -224,52 +226,60 @@ protected: Fs->at(i) = Fs->at(i) * J; } } - correctForMissingMeasurements(cameras, ue, Fs, E); - return ue; + + // Correct the Jacobians in case some measurements are missing. + correctForMissingMeasurements(cameras, error, Fs, E); + + return error; } /** - * This corrects the Jacobians for the case in which some pixel measurement is missing (nan) - * In practice, this does not do anything in the monocular case, but it is implemented in the stereo version + * This corrects the Jacobians for the case in which some 2D measurement is + * missing (nan). In practice, this does not do anything in the monocular + * case, but it is implemented in the stereo version. */ - virtual void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, - boost::optional E = boost::none) const {} + virtual void correctForMissingMeasurements( + const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const {} /** - * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - z] - * Noise model applied + * Calculate vector of re-projection errors [h(x)-z] = [cameras.project(p) - + * z], with the noise model applied. */ template Vector whitenedError(const Cameras& cameras, const POINT& point) const { - Vector e = cameras.reprojectionError(point, measured_); + Vector error = cameras.reprojectionError(point, measured_); if (noiseModel_) - noiseModel_->whitenInPlace(e); - return e; + noiseModel_->whitenInPlace(error); + return error; } - /** Calculate the error of the factor. - * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. - * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model - * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. - * Will be used in "error(Values)" function required by NonlinearFactor base class + /** + * Calculate the error of the factor. + * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of + * Gaussian. In this class, we take the raw prediction error \f$ h(x)-z \f$, + * ask the noise model to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and + * then multiply by 0.5. Will be used in "error(Values)" function required by + * NonlinearFactor base class */ template double totalReprojectionError(const Cameras& cameras, const POINT& point) const { - Vector e = whitenedError(cameras, point); - return 0.5 * e.dot(e); + Vector error = whitenedError(cameras, point); + return 0.5 * error.dot(error); } - /// Computes Point Covariance P from E - static Matrix PointCov(Matrix& E) { + /// Computes Point Covariance P from the "point Jacobian" E. + static Matrix PointCov(const Matrix& E) { return (E.transpose() * E).inverse(); } /** - * Compute F, E, and b (called below in both vanilla and SVD versions), where - * F is a vector of derivatives wrpt the cameras, and E the stacked derivatives - * with respect to the point. The value of cameras/point are passed as parameters. - * TODO: Kill this obsolete method + * Compute F, E, and b (called below in both vanilla and SVD versions), where + * F is a vector of derivatives wrpt the cameras, and E the stacked + * derivatives with respect to the point. The value of cameras/point are + * passed as parameters. */ template void computeJacobians(FBlocks& Fs, Matrix& E, Vector& b, @@ -281,7 +291,11 @@ protected: b = -unwhitenedError(cameras, point, Fs, E); } - /// SVD version + /** + * SVD version that produces smaller Jacobian matrices by doing an SVD + * decomposition on E, and returning the left nulkl-space of E. + * See JacobianFactorSVD for more documentation. + * */ template void computeJacobiansSVD(FBlocks& Fs, Matrix& Enull, Vector& b, const Cameras& cameras, const POINT& point) const { @@ -291,14 +305,14 @@ protected: static const int N = FixedDimension::value; // 2 (Unit3) or 3 (Point3) - // Do SVD on A + // Do SVD on A. Eigen::JacobiSVD svd(E, Eigen::ComputeFullU); - Vector s = svd.singularValues(); size_t m = this->keys_.size(); Enull = svd.matrixU().block(0, N, ZDim * m, ZDim * m - N); // last ZDim*m-N columns } - /// Linearize to a Hessianfactor + /// Linearize to a Hessianfactor. + // TODO(dellaert): Not used/tested anywhere and not properly whitened. boost::shared_ptr > createHessianFactor( const Cameras& cameras, const Point3& point, const double lambda = 0.0, bool diagonalDamping = false) const { @@ -351,9 +365,7 @@ protected: P, b); } - /** - * Return Jacobians as JacobianFactorQ - */ + /// Return Jacobians as JacobianFactorQ. boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0, bool diagonalDamping = false) const { @@ -368,8 +380,8 @@ protected: } /** - * Return Jacobians as JacobianFactorSVD - * TODO lambda is currently ignored + * Return Jacobians as JacobianFactorSVD. + * TODO(dellaert): lambda is currently ignored */ boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, const Point3& point, double lambda = 0.0) const { @@ -393,6 +405,7 @@ protected: F.block(ZDim * i, Dim * i) = Fs.at(i); } + // Return sensor pose. Pose3 body_P_sensor() const{ if(body_P_sensor_) return *body_P_sensor_; diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index f67ca0740..9fb9c6905 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -61,10 +61,11 @@ protected: /// @name Caching triangulation /// @{ mutable TriangulationResult result_; ///< result from triangulateSafe - mutable std::vector > cameraPosesTriangulation_; ///< current triangulation poses + mutable std::vector > + cameraPosesTriangulation_; ///< current triangulation poses /// @} -public: + public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; @@ -116,21 +117,31 @@ public: && Base::equals(p, tol); } - /// Check if the new linearization point is the same as the one used for previous triangulation + /** + * @brief Check if the new linearization point is the same as the one used for + * previous triangulation. + * + * @param cameras + * @return true if we need to re-triangulate. + */ bool decideIfTriangulate(const Cameras& cameras) const { - // several calls to linearize will be done from the same linearization point, hence it is not needed to re-triangulate - // Note that this is not yet "selecting linearization", that will come later, and we only check if the - // current linearization is the "same" (up to tolerance) w.r.t. the last time we triangulated the point + // Several calls to linearize will be done from the same linearization + // point, hence it is not needed to re-triangulate. Note that this is not + // yet "selecting linearization", that will come later, and we only check if + // the current linearization is the "same" (up to tolerance) w.r.t. the last + // time we triangulated the point. size_t m = cameras.size(); bool retriangulate = false; - // if we do not have a previous linearization point or the new linearization point includes more poses + // Definitely true if we do not have a previous linearization point or the + // new linearization point includes more poses. if (cameraPosesTriangulation_.empty() || cameras.size() != cameraPosesTriangulation_.size()) retriangulate = true; + // Otherwise, check poses against cache. if (!retriangulate) { for (size_t i = 0; i < cameras.size(); i++) { if (!cameras[i].pose().equals(cameraPosesTriangulation_[i], @@ -141,7 +152,8 @@ public: } } - if (retriangulate) { // we store the current poses used for triangulation + // Store the current poses used for triangulation if we will re-triangulate. + if (retriangulate) { cameraPosesTriangulation_.clear(); cameraPosesTriangulation_.reserve(m); for (size_t i = 0; i < m; i++) @@ -149,10 +161,15 @@ public: cameraPosesTriangulation_.push_back(cameras[i].pose()); } - return retriangulate; // if we arrive to this point all poses are the same and we don't need re-triangulation + return retriangulate; } - /// triangulateSafe + /** + * @brief Call gtsam::triangulateSafe iff we need to re-triangulate. + * + * @param cameras + * @return TriangulationResult + */ TriangulationResult triangulateSafe(const Cameras& cameras) const { size_t m = cameras.size(); @@ -166,17 +183,21 @@ public: return result_; } - /// triangulate + /** + * @brief Possibly re-triangulate before calculating Jacobians. + * + * @param cameras + * @return true if we could safely triangulate + */ bool triangulateForLinearize(const Cameras& cameras) const { triangulateSafe(cameras); // imperative, might reset result_ return bool(result_); } - /// linearize returns a Hessianfactor that is an approximation of error(p) + /// Create a Hessianfactor that is an approximation of error(p). boost::shared_ptr > createHessianFactor( - const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping = - false) const { - + const Cameras& cameras, const double lambda = 0.0, + bool diagonalDamping = false) const { size_t numKeys = this->keys_.size(); // Create structures for Hessian Factors KeyVector js; @@ -184,39 +205,38 @@ public: std::vector gs(numKeys); if (this->measured_.size() != cameras.size()) - throw std::runtime_error("SmartProjectionHessianFactor: this->measured_" - ".size() inconsistent with input"); + throw std::runtime_error( + "SmartProjectionHessianFactor: this->measured_" + ".size() inconsistent with input"); triangulateSafe(cameras); if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { // failed: return"empty" Hessian - for(Matrix& m: Gs) - m = Matrix::Zero(Base::Dim, Base::Dim); - for(Vector& v: gs) - v = Vector::Zero(Base::Dim); + for (Matrix& m : Gs) m = Matrix::Zero(Base::Dim, Base::Dim); + for (Vector& v : gs) v = Vector::Zero(Base::Dim); return boost::make_shared >(this->keys_, - Gs, gs, 0.0); + Gs, gs, 0.0); } // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). - std::vector > Fblocks; + typename Base::FBlocks Fs; Matrix E; Vector b; - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); // Whiten using noise model - Base::whitenJacobians(Fblocks, E, b); + Base::whitenJacobians(Fs, E, b); // build augmented hessian - SymmetricBlockMatrix augmentedHessian = // - Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping); + SymmetricBlockMatrix augmentedHessian = // + Cameras::SchurComplement(Fs, E, b, lambda, diagonalDamping); - return boost::make_shared >(this->keys_, - augmentedHessian); + return boost::make_shared >( + this->keys_, augmentedHessian); } - // create factor + // Create RegularImplicitSchurFactor factor. boost::shared_ptr > createRegularImplicitSchurFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -226,7 +246,7 @@ public: return boost::shared_ptr >(); } - /// create factor + /// Create JacobianFactorQ factor. boost::shared_ptr > createJacobianQFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -236,13 +256,13 @@ public: return boost::make_shared >(this->keys_); } - /// Create a factor, takes values + /// Create JacobianFactorQ factor, takes values. boost::shared_ptr > createJacobianQFactor( const Values& values, double lambda) const { return createJacobianQFactor(this->cameras(values), lambda); } - /// different (faster) way to compute Jacobian factor + /// Different (faster) way to compute a JacobianFactorSVD factor. boost::shared_ptr createJacobianSVDFactor( const Cameras& cameras, double lambda) const { if (triangulateForLinearize(cameras)) @@ -252,19 +272,19 @@ public: return boost::make_shared >(this->keys_); } - /// linearize to a Hessianfactor + /// Linearize to a Hessianfactor. virtual boost::shared_ptr > linearizeToHessian( const Values& values, double lambda = 0.0) const { return createHessianFactor(this->cameras(values), lambda); } - /// linearize to an Implicit Schur factor + /// Linearize to an Implicit Schur factor. virtual boost::shared_ptr > linearizeToImplicit( const Values& values, double lambda = 0.0) const { return createRegularImplicitSchurFactor(this->cameras(values), lambda); } - /// linearize to a JacobianfactorQ + /// Linearize to a JacobianfactorQ. virtual boost::shared_ptr > linearizeToJacobian( const Values& values, double lambda = 0.0) const { return createJacobianQFactor(this->cameras(values), lambda); @@ -334,7 +354,7 @@ public: /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate void computeJacobiansWithTriangulatedPoint( - std::vector >& Fblocks, Matrix& E, Vector& b, + typename Base::FBlocks& Fs, Matrix& E, Vector& b, const Cameras& cameras) const { if (!result_) { @@ -342,32 +362,32 @@ public: // TODO check flag whether we should do this Unit3 backProjected = cameras[0].backprojectPointAtInfinity( this->measured_.at(0)); - Base::computeJacobians(Fblocks, E, b, cameras, backProjected); + Base::computeJacobians(Fs, E, b, cameras, backProjected); } else { // valid result: just return Base version - Base::computeJacobians(Fblocks, E, b, cameras, *result_); + Base::computeJacobians(Fs, E, b, cameras, *result_); } } /// Version that takes values, and creates the point bool triangulateAndComputeJacobians( - std::vector >& Fblocks, Matrix& E, Vector& b, + typename Base::FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); + computeJacobiansWithTriangulatedPoint(Fs, E, b, cameras); return nonDegenerate; } /// takes values bool triangulateAndComputeJacobiansSVD( - std::vector >& Fblocks, Matrix& Enull, Vector& b, + typename Base::FBlocks& Fs, Matrix& Enull, Vector& b, const Values& values) const { Cameras cameras = this->cameras(values); bool nonDegenerate = triangulateForLinearize(cameras); if (nonDegenerate) - Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_); + Base::computeJacobiansSVD(Fs, Enull, b, cameras, *result_); return nonDegenerate; } diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index e361dddac..88e112998 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -447,23 +447,23 @@ public: } /** - * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + * This corrects the Jacobians and error vector for the case in which the + * right 2D measurement in the monocular camera is missing (nan). */ - void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, - boost::optional Fs = boost::none, - boost::optional E = boost::none) const override - { + void correctForMissingMeasurements( + const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const override { // when using stereo cameras, some of the measurements might be missing: - for(size_t i=0; i < cameras.size(); i++){ + for (size_t i = 0; i < cameras.size(); i++) { const StereoPoint2& z = measured_.at(i); - if(std::isnan(z.uR())) // if the right pixel is invalid + if (std::isnan(z.uR())) // if the right 2D measurement is invalid { - if(Fs){ // delete influence of right point on jacobian Fs + if (Fs) { // delete influence of right point on jacobian Fs MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); // set the corresponding entry of vector ue to zero diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 25be48b0f..ce6df15cb 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -33,9 +33,11 @@ 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 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 { From 372ae27a5e66b9c0b53c9ec5f09189562c07f151 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 16:47:32 -0400 Subject: [PATCH 020/110] Added two ReadMe files to document the plethora of smart factors. --- gtsam/slam/ReadMe.md | 64 +++++++++++++++++++++++++++++++++++ gtsam_unstable/slam/ReadMe.md | 39 +++++++++++++++++++++ 2 files changed, 103 insertions(+) create mode 100644 gtsam/slam/ReadMe.md create mode 100644 gtsam_unstable/slam/ReadMe.md diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md new file mode 100644 index 000000000..22a5778f2 --- /dev/null +++ b/gtsam/slam/ReadMe.md @@ -0,0 +1,64 @@ +# SLAM Factors + +## GenericProjectionFactor (defined in ProjectionFactor.h) + +Non-linear factor for a constraint derived from a 2D measurement. +The calibration is assumed known and passed in the constructor. +The main building block for visual SLAM. + +Templated on +- `POSE`, default `Pose3` +- `LANDMARK`, default `Point3` +- `CALIBRATION`, default `Cal3_S2` + +## SmartFactors + +These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. + +### SmartFactorBase + +This is the base class for smart factors, templated on a `CAMERA` type. +It has no internal point, but it saves the measurements, keeps a noise model, and an optional sensor pose. + +### SmartProjectionFactor + +Also templated on `CAMERA`. Triangulates a 3D point and keeps an estimate of it around. +This factor operates with monocular cameras, and is used to optimize the camera pose +*and* calibration for each camera, and requires variables of type `CAMERA` in values. + +If the calibration is fixed use `SmartProjectionPoseFactor` instead! + + +### SmartProjectionPoseFactor + +Derives from `SmartProjectionFactor` but is templated on a `CALIBRATION` type, setting `CAMERA = PinholePose`. +This factor assumes that the camera calibration is fixed and the same for all cameras involved in this factor. +The factor only constrains poses. + +If the calibration should be optimized, as well, use `SmartProjectionFactor` instead! + +### SmartProjectionFactorP + +Same as `SmartProjectionPoseFactor`, except: +- it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; +- it admits a different calibration for each measurement, i.e., it can model a multi-camera system; +- it allows multiple observations from the same pose/key, again, to model a multi-camera system. + +TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. + +### RegularImplicitSchurFactor + +A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. +It is produced by calling `createRegularImplicitSchurFactor` in `SmartFactorBase` or `SmartProjectionFactor`. + +### JacobianFactorQ + +A RegularJacobianFactor that uses some badly documented reduction on the Jacobians. + +### JacobianFactorQR + +A RegularJacobianFactor that eliminates a point using sequential elimination. + +### JacobianFactorQR + +A RegularJacobianFactor that uses the "Nullspace Trick" by Mourikis et al. See the documentation in the file, which *is* well documented. \ No newline at end of file diff --git a/gtsam_unstable/slam/ReadMe.md b/gtsam_unstable/slam/ReadMe.md new file mode 100644 index 000000000..78d53090a --- /dev/null +++ b/gtsam_unstable/slam/ReadMe.md @@ -0,0 +1,39 @@ +# SLAM Factors + +## SmartFactors + +These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. + +### SmartRangeFactor + +An experiment in creating a structure-less 2D range-SLAM factor with range-only measurements. +It uses a sophisticated `triangulate` logic based on circle intersections. + +### SmartStereoProjectionFactor + +Version of `SmartProjectionFactor` for stereo observations, specializes SmartFactorBase for `CAMERA == StereoCamera`. + +TODO: a lot of commented out code and could move a lot to .cpp file. + +### SmartStereoProjectionPoseFactor + +Derives from `SmartStereoProjectionFactor` but adds an array of `Cal3_S2Stereo` calibration objects . + +TODO: Again, as no template arguments, we could move a lot to .cpp file. + +### SmartStereoProjectionFactorPP + +Similar `SmartStereoProjectionPoseFactor` but *additionally* adds an array of body_P_cam poses. The dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. + +TODO: See above, same issues as `SmartStereoProjectionPoseFactor`. + +### SmartProjectionPoseFactorRollingShutter + +Is templated on a `CAMERA` type and derives from `SmartProjectionFactor`. + +This factor optimizes two consecutive poses of a body assuming a rolling +shutter model of the camera with given readout time. The factor requires that +values contain (for each 2D observation) two consecutive camera poses from +which the 2D observation pose can be interpolated. + +TODO: the dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. Also, possibly a lot of copy/paste computation of things that (should) happen in base class. \ No newline at end of file From c0262b074c04e8e3ca75e66c01b08a0dc79bb641 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 29 Aug 2021 19:38:20 -0400 Subject: [PATCH 021/110] Address review comments, docs only. --- gtsam/slam/ProjectionFactor.h | 3 ++- gtsam/slam/ReadMe.md | 8 +++++++- gtsam/slam/SmartFactorBase.h | 2 +- gtsam_unstable/slam/ReadMe.md | 1 + 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/ProjectionFactor.h b/gtsam/slam/ProjectionFactor.h index 169fe8a0a..42dba8bd0 100644 --- a/gtsam/slam/ProjectionFactor.h +++ b/gtsam/slam/ProjectionFactor.h @@ -35,7 +35,8 @@ namespace gtsam { * The main building block for visual SLAM. * @addtogroup SLAM */ - template + template class GenericProjectionFactor: public NoiseModelFactor2 { protected: diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index 22a5778f2..c43f0769a 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -2,7 +2,7 @@ ## GenericProjectionFactor (defined in ProjectionFactor.h) -Non-linear factor for a constraint derived from a 2D measurement. +Non-linear factor that minimizes the re-projection error with respect to a 2D measurement. The calibration is assumed known and passed in the constructor. The main building block for visual SLAM. @@ -14,6 +14,7 @@ Templated on ## SmartFactors These are "structure-less" factors, i.e., rather than introducing a new variable for an observed 3D point or landmark, a single factor is created that provides a multi-view constraint on several poses and/or cameras. +While one typically adds multiple GenericProjectionFactors (one for each observation of a landmark), a SmartFactor collects all measurements for a landmark, i.e., the factor graph contains 1 smart factor per landmark. ### SmartFactorBase @@ -46,6 +47,11 @@ Same as `SmartProjectionPoseFactor`, except: TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. +## Linearized Smart Factors + +The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above. + + ### RegularImplicitSchurFactor A specialization of a GaussianFactor to structure-less SFM, which is very fast in a conjugate gradient (CG) solver. diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index f815200ce..ddf56b289 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -44,7 +44,7 @@ namespace gtsam { * factors. The methods take a CameraSet argument and the value of a * point, which is kept in the derived class. * - * @tparam CAMERA should behave like a set of PinholeCamera. + * @tparam CAMERA should behave like a PinholeCamera. */ template class SmartFactorBase: public NonlinearFactor { diff --git a/gtsam_unstable/slam/ReadMe.md b/gtsam_unstable/slam/ReadMe.md index 78d53090a..9aa0fed78 100644 --- a/gtsam_unstable/slam/ReadMe.md +++ b/gtsam_unstable/slam/ReadMe.md @@ -24,6 +24,7 @@ TODO: Again, as no template arguments, we could move a lot to .cpp file. ### SmartStereoProjectionFactorPP Similar `SmartStereoProjectionPoseFactor` but *additionally* adds an array of body_P_cam poses. The dimensions seem to be hardcoded and the types defined in the SmartFactorBase have been re-defined. +The body_P_cam poses are optimized here! TODO: See above, same issues as `SmartStereoProjectionPoseFactor`. From 19850425b8e4e3b30c8d4a7ea21974ed83484d8c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 10 Sep 2021 11:02:14 -0400 Subject: [PATCH 022/110] clean up and refactoring to use custom preintegration params --- gtsam/navigation/navigation.i | 2 + python/gtsam/examples/ImuFactorExample.py | 48 ++++++++++++------- .../gtsam/examples/PreintegrationExample.py | 38 ++++++++------- 3 files changed, 56 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 48a5a35de..7a879c3ef 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -88,6 +88,8 @@ virtual class PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { PreintegrationParams(Vector n_gravity); + gtsam::Vector n_gravity; + static gtsam::PreintegrationParams* MakeSharedD(double g); static gtsam::PreintegrationParams* MakeSharedU(double g); static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 86613234d..d6e6793f2 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -10,20 +10,19 @@ A script validating and demonstrating the ImuFactor inference. Author: Frank Dellaert, Varun Agrawal """ -# pylint: disable=no-name-in-module,unused-import,arguments-differ +# pylint: disable=no-name-in-module,unused-import,arguments-differ,import-error,wrong-import-order from __future__ import print_function import argparse import math +import gtsam import matplotlib.pyplot as plt import numpy as np -from mpl_toolkits.mplot3d import Axes3D - -import gtsam from gtsam.symbol_shorthand import B, V, X from gtsam.utils.plot import plot_pose3 +from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample @@ -51,12 +50,23 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) + g = 9.81 + params = gtsam.PreintegrationParams.MakeSharedU(g) + + # Some arbitrary noise sigmas + gyro_sigma = 1e-3 + accel_sigma = 1e-3 + I_3x3 = np.eye(3) + params.setGyroscopeCovariance(gyro_sigma**2 * I_3x3) + params.setAccelerometerCovariance(accel_sigma**2 * I_3x3) + params.setIntegrationCovariance(1e-7**2 * I_3x3) + dt = 1e-2 super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], - bias, dt) + bias, params, dt) def addPrior(self, i, graph): - """Add priors at time step `i`.""" + """Add a prior on the navigation state at time `i`.""" state = self.scenario.navState(i) graph.push_back( gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise)) @@ -71,21 +81,27 @@ class ImuFactorExample(PreintegrationExample): result = optimizer.optimize() return result - def plot(self, result): - """Plot resulting poses.""" + def plot(self, + values, + title="Estimated Trajectory", + fignum=POSES_FIG + 1, + show=False): + """Plot poses in values.""" i = 0 - while result.exists(X(i)): - pose_i = result.atPose3(X(i)) - plot_pose3(POSES_FIG + 1, pose_i, 1) + while values.exists(X(i)): + pose_i = values.atPose3(X(i)) + plot_pose3(fignum, pose_i, 1) i += 1 - plt.title("Estimated Trajectory") + plt.title(title) - gtsam.utils.plot.set_axes_equal(POSES_FIG + 1) + gtsam.utils.plot.set_axes_equal(fignum) - print("Bias Values", result.atConstantBias(BIAS_KEY)) + print("Bias Values", values.atConstantBias(BIAS_KEY)) plt.ioff() - plt.show() + + if show: + plt.show() def run(self, T=12, compute_covariances=False, verbose=True): """Main runner.""" @@ -173,7 +189,7 @@ class ImuFactorExample(PreintegrationExample): print("Covariance on vel {}:\n{}\n".format( i, marginals.marginalCovariance(V(i)))) - self.plot(result) + self.plot(result, show=True) if __name__ == '__main__': diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 458248c30..f38ff7bc9 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -5,9 +5,13 @@ All Rights Reserved See LICENSE for the license information -A script validating the Preintegration of IMU measurements +A script validating the Preintegration of IMU measurements. + +Authors: Frank Dellaert, Varun Agrawal. """ +# pylint: disable=invalid-name,unused-import,wrong-import-order + import math import gtsam @@ -21,22 +25,20 @@ POSES_FIG = 2 class PreintegrationExample(object): - + """Base class for all preintegration examples.""" @staticmethod def defaultParams(g): """Create default parameters with Z *up* and realistic noise parameters""" params = gtsam.PreintegrationParams.MakeSharedU(g) kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW kAccelSigma = 0.1 / 60 # 10 cm VRW - params.setGyroscopeCovariance( - kGyroSigma ** 2 * np.identity(3, float)) - params.setAccelerometerCovariance( - kAccelSigma ** 2 * np.identity(3, float)) - params.setIntegrationCovariance( - 0.0000001 ** 2 * np.identity(3, float)) + params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float)) + params.setAccelerometerCovariance(kAccelSigma**2 * + np.identity(3, float)) + params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float)) return params - def __init__(self, twist=None, bias=None, dt=1e-2): + def __init__(self, twist=None, bias=None, params=None, dt=1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -58,9 +60,11 @@ class PreintegrationExample(object): self.labels = list('xyz') self.colors = list('rgb') - # Create runner - self.g = 10 # simple gravity constant - self.params = self.defaultParams(self.g) + if params: + self.params = params + else: + # Default params with simple gravity constant + self.params = self.defaultParams(g=10) if bias is not None: self.actualBias = bias @@ -69,13 +73,15 @@ class PreintegrationExample(object): gyroBias = np.array([0, 0, 0]) self.actualBias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - self.runner = gtsam.ScenarioRunner( - self.scenario, self.params, self.dt, self.actualBias) + # Create runner + self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt, + self.actualBias) fig, self.axes = plt.subplots(4, 3) fig.set_tight_layout(True) def plotImu(self, t, measuredOmega, measuredAcc): + """Plot IMU measurements.""" plt.figure(IMU_FIG) # plot angular velocity @@ -109,7 +115,7 @@ class PreintegrationExample(object): ax.set_xlabel('specific force ' + label) def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): - # plot ground truth pose, as well as prediction from integrated IMU measurements + """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) t = actualPose.translation() @@ -122,7 +128,7 @@ class PreintegrationExample(object): plt.pause(time_interval) def run(self, T=12): - # simulate the loop + """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) measuredAcc = self.runner.measuredSpecificForce(t) From b49bd123f428009699a3ed17518deb1cc07e6b13 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 19:08:54 -0400 Subject: [PATCH 023/110] renamed smartProjectionFactorP -> smartProjectionRigFactor --- gtsam/slam/ReadMe.md | 2 +- ...onFactorP.h => SmartProjectionRigFactor.h} | 22 ++++----- gtsam/slam/tests/smartFactorScenarios.h | 8 ++-- ...P.cpp => testSmartProjectionRigFactor.cpp} | 48 +++++++++---------- 4 files changed, 40 insertions(+), 40 deletions(-) rename gtsam/slam/{SmartProjectionFactorP.h => SmartProjectionRigFactor.h} (95%) rename gtsam/slam/tests/{testSmartProjectionFactorP.cpp => testSmartProjectionRigFactor.cpp} (96%) diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index c43f0769a..d216b9181 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -38,7 +38,7 @@ The factor only constrains poses. If the calibration should be optimized, as well, use `SmartProjectionFactor` instead! -### SmartProjectionFactorP +### SmartProjectionRigFactor Same as `SmartProjectionPoseFactor`, except: - it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; diff --git a/gtsam/slam/SmartProjectionFactorP.h b/gtsam/slam/SmartProjectionRigFactor.h similarity index 95% rename from gtsam/slam/SmartProjectionFactorP.h rename to gtsam/slam/SmartProjectionRigFactor.h index b01420446..26cbffe7c 100644 --- a/gtsam/slam/SmartProjectionFactorP.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file SmartProjectionFactorP.h + * @file SmartProjectionRigFactor.h * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) @@ -47,11 +47,11 @@ namespace gtsam { * @addtogroup SLAM */ template -class SmartProjectionFactorP : public SmartProjectionFactor { +class SmartProjectionRigFactor : public SmartProjectionFactor { private: typedef SmartProjectionFactor Base; - typedef SmartProjectionFactorP This; + typedef SmartProjectionRigFactor This; typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension @@ -76,7 +76,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionFactorP() { + SmartProjectionRigFactor() { } /** @@ -84,7 +84,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { * @param sharedNoiseModel isotropic noise model for the 2D feature measurements * @param params parameters for the smart projection factors */ - SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel, + SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { @@ -94,7 +94,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionFactorP() override { + ~SmartProjectionRigFactor() override { } /** @@ -167,7 +167,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { - std::cout << s << "SmartProjectionFactorP: \n "; + std::cout << s << "SmartProjectionRigFactor: \n "; for (size_t i = 0; i < K_all_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; @@ -266,7 +266,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { std::vector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera - throw std::runtime_error("SmartProjectionFactorP: " + throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point @@ -282,7 +282,7 @@ class SmartProjectionFactorP : public SmartProjectionFactor { > (this->keys_, Gs, gs, 0.0); } else { throw std::runtime_error( - "SmartProjectionFactorP: " + "SmartProjectionRigFactor: " "only supported degeneracy mode is ZERO_ON_DEGENERACY"); } } @@ -351,8 +351,8 @@ class SmartProjectionFactorP : public SmartProjectionFactor { /// traits template -struct traits > : public Testable< - SmartProjectionFactorP > { +struct traits > : public Testable< + SmartProjectionRigFactor > { }; } // \ namespace gtsam diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index 7421f73af..f35bdc950 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -18,11 +18,11 @@ #pragma once #include -#include #include #include #include #include +#include "../SmartProjectionRigFactor.h" using namespace std; using namespace gtsam; @@ -70,7 +70,7 @@ SmartProjectionParams params; namespace vanillaPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -84,7 +84,7 @@ Camera cam3(pose_above, sharedK); namespace vanillaPose2 { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -114,7 +114,7 @@ typedef GeneralSFMFactor SFMFactor; namespace bundlerPose { typedef PinholePose Camera; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionFactorP SmartFactorP; +typedef SmartProjectionRigFactor SmartFactorP; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionFactorP.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp similarity index 96% rename from gtsam/slam/tests/testSmartProjectionFactorP.cpp rename to gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4591dc08e..ef330cdd0 100644 --- a/gtsam/slam/tests/testSmartProjectionFactorP.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionFactorP.cpp - * @brief Unit tests for SmartProjectionFactorP Class + * @file testSmartProjectionRigFactor.cpp + * @brief Unit tests for SmartProjectionRigFactor Class * @author Chris Beall * @author Luca Carlone * @author Zsolt Kira @@ -52,13 +52,13 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor) { +TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor2) { +TEST( SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -66,14 +66,14 @@ TEST( SmartProjectionFactorP, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor3) { +TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Constructor4) { +TEST( SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -82,7 +82,7 @@ TEST( SmartProjectionFactorP, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, Equals ) { +TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); factor1->add(measurement1, x1, sharedK); @@ -94,7 +94,7 @@ TEST( SmartProjectionFactorP, Equals ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noiseless ) { +TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; @@ -153,7 +153,7 @@ TEST( SmartProjectionFactorP, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, noisy ) { +TEST( SmartProjectionRigFactor, noisy ) { using namespace vanillaPose; @@ -191,7 +191,7 @@ TEST( SmartProjectionFactorP, noisy ) { } /* *************************************************************************/ -TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -279,7 +279,7 @@ TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -346,7 +346,7 @@ TEST( SmartProjectionFactorP, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Factors ) { +TEST( SmartProjectionRigFactor, Factors ) { using namespace vanillaPose; @@ -437,7 +437,7 @@ TEST( SmartProjectionFactorP, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -497,7 +497,7 @@ TEST( SmartProjectionFactorP, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, landmarkDistance ) { +TEST( SmartProjectionRigFactor, landmarkDistance ) { using namespace vanillaPose; @@ -558,7 +558,7 @@ TEST( SmartProjectionFactorP, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { +TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -626,7 +626,7 @@ TEST( SmartProjectionFactorP, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, CheckHessian) { +TEST( SmartProjectionRigFactor, CheckHessian) { KeyVector views { x1, x2, x3 }; @@ -711,7 +711,7 @@ TEST( SmartProjectionFactorP, CheckHessian) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Hessian ) { +TEST( SmartProjectionRigFactor, Hessian ) { using namespace vanillaPose2; @@ -746,7 +746,7 @@ TEST( SmartProjectionFactorP, Hessian ) { } /* ************************************************************************* */ -TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -755,7 +755,7 @@ TEST( SmartProjectionFactorP, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionFactorP, Cal3Bundler ) { +TEST( SmartProjectionRigFactor, Cal3Bundler ) { using namespace bundlerPose; @@ -820,7 +820,7 @@ TEST( SmartProjectionFactorP, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSamePose) { +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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 @@ -951,7 +951,7 @@ TEST( SmartProjectionFactorP, hessianComparedToProjFactors_measurementsFromSameP } /* *************************************************************************/ -TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1033,7 +1033,7 @@ TEST( SmartProjectionFactorP, optimization_3poses_measurementsFromSamePose ) { //| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) //| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionFactorP, timing ) { +TEST( SmartProjectionRigFactor, timing ) { using namespace vanillaPose; @@ -1095,7 +1095,7 @@ BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropi BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); -TEST(SmartProjectionFactorP, serialize) { +TEST(SmartProjectionRigFactor, serialize) { using namespace vanillaPose; using namespace gtsam::serializationTestHelpers; SmartProjectionParams params; @@ -1108,7 +1108,7 @@ TEST(SmartProjectionFactorP, serialize) { } // SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionFactorP, serialize2) { +//TEST(SmartProjectionRigFactor, serialize2) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; From a5db090fb4d01d16287b0a38f43322387c260d04 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:00:25 -0400 Subject: [PATCH 024/110] modernized factor --- gtsam/slam/SmartProjectionFactor.h | 1 + gtsam/slam/SmartProjectionRigFactor.h | 121 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../tests/testSmartProjectionRigFactor.cpp | 2130 +++++++++-------- 4 files changed, 1119 insertions(+), 1134 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 9fb9c6905..f9c101cb8 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -71,6 +71,7 @@ protected: typedef boost::shared_ptr shared_ptr; /// shorthand for a set of cameras + typedef CAMERA Camera; typedef CameraSet Cameras; /** diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 26cbffe7c..600e48e8d 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -14,11 +14,10 @@ * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) - * - it admits a different calibration for each measurement (i.e., it can model a multi-camera system) + * - it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system) * - it allows multiple observations from the same pose/key (again, to model a multi-camera system) * @author Luca Carlone - * @author Chris Beall - * @author Zsolt Kira + * @author Frank Dellaert */ #pragma once @@ -34,7 +33,6 @@ namespace gtsam { * 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. - * */ /** @@ -60,13 +58,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { protected: /// vector of keys (one for each observation) with potentially repeated keys - std::vector nonUniqueKeys_; + FastVector nonUniqueKeys_; - /// shared pointer to calibration object (one for each observation) - std::vector > K_all_; + /// cameras in the rig (fixed poses wrt body + fixed intrinsics) + typename Base::Cameras cameraRig_; - /// Pose of the camera in the body frame (one for each observation) - std::vector body_P_sensors_; + /// vector of camera Ids (one for each observation), identifying which camera took the measurement + FastVector cameraIds_; public: typedef CAMERA Camera; @@ -82,12 +80,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /** * Constructor * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig * @param params parameters for the smart projection factors */ SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, + const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -98,17 +98,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** - * add a new measurement, corresponding to an observation from pose "poseKey" whose camera - * has intrinsic calibration K and extrinsic calibration body_P_sensor. + * add a new measurement, corresponding to an observation from pose "poseKey" + * and taken from the camera in the rig identified by "cameraId" * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param K (fixed) camera intrinsic calibration - * @param body_P_sensor (fixed) camera extrinsic calibration + * @param cameraId ID of the camera in the rig taking the measurement */ - void add(const Point2& measured, const Key& poseKey, - const boost::shared_ptr& K, const Pose3 body_P_sensor = - Pose3::identity()) { + void add(const Point2& measured, const Key& poseKey, const size_t cameraId) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); @@ -117,10 +114,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (std::find(this->keys_.begin(), this->keys_.end(), poseKey) == this->keys_.end()) this->keys_.push_back(poseKey); // add only unique keys - // store fixed intrinsic calibration - K_all_.push_back(K); - // store fixed extrinsics of the camera - body_P_sensors_.push_back(body_P_sensor); + // store id of the camera taking the measurement + cameraIds_.push_back(cameraId); } /** @@ -128,38 +123,32 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param Ks vector of (fixed) intrinsic calibration objects - * @param body_P_sensors vector of (fixed) extrinsic calibration objects + * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) */ - void add(const Point2Vector& measurements, const std::vector& poseKeys, - const std::vector>& Ks, - const std::vector body_P_sensors = std::vector()) { + void add(const Point2Vector& measurements, const FastVector& poseKeys, + const FastVector& cameraIds) { assert(poseKeys.size() == measurements.size()); - assert(poseKeys.size() == Ks.size()); + assert(poseKeys.size() == cameraIds.size()); for (size_t i = 0; i < measurements.size(); i++) { - if (poseKeys.size() == body_P_sensors.size()) { - add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]); - } else { - add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics - } + add(measurements[i], poseKeys[i], cameraIds[i]); } } - /// return the calibration object - inline std::vector> calibration() const { - return K_all_; - } - - /// return the extrinsic camera calibration body_P_sensors - const std::vector body_P_sensors() const { - return body_P_sensors_; - } - /// return (for each observation) the (possibly non unique) keys involved in the measurements - const std::vector nonUniqueKeys() const { + const FastVector nonUniqueKeys() const { return nonUniqueKeys_; } + /// return the calibration object + inline Cameras cameraRig() const { + return cameraRig_; + } + + /// return the calibration object + inline FastVector cameraIds() const { + return cameraIds_; + } + /** * print * @param s optional string naming the factor @@ -168,11 +157,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionRigFactor: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { + for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; - body_P_sensors_[i].print("extrinsic calibration:\n"); - K_all_[i]->print("intrinsic calibration = "); + std::cout << "cameraId: " << cameraIds_[i] << std::endl; + cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -180,35 +169,26 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This *e = dynamic_cast(&p); - 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() - && nonUniqueKeys_ == e->nonUniqueKeys() && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) + && nonUniqueKeys_ == e->nonUniqueKeys() + && cameraRig_.equals(e->cameraRig()); +// && cameraIds_ == e->cameraIds(); } /** * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding + * @param values Values structure which must contain body poses corresponding * to keys involved in this factor * @return vector of cameras */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3& body_P_cam_i = body_P_sensors_[i]; + const Pose3& body_P_cam_i = cameraRig_[i].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) * body_P_cam_i; - cameras.emplace_back(world_P_sensor_i, K_all_[i]); + cameras.emplace_back(world_P_sensor_i, + make_shared(cameraRig_[i].calibration())); } return cameras; } @@ -240,10 +220,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3 sensor_P_body = body_P_sensors_[i].inverse(); + const Pose3 body_P_sensor = cameraRig_[i].pose(); + const Pose3 sensor_P_body = body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Eigen::Matrix H; - world_P_body.compose(body_P_sensors_[i], H); + world_P_body.compose(body_P_sensor, H); Fs.at(i) = Fs.at(i) * H; } } @@ -262,8 +243,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { // Create structures for Hessian Factors KeyVector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + FastVector < Vector > gs(nrUniqueKeys); if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera throw std::runtime_error("SmartProjectionRigFactor: " @@ -324,7 +305,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectioFactorP: unknown linearization mode"); + "SmartProjectionRigFactor: unknown linearization mode"); } } @@ -341,9 +322,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { 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_NVP(nonUniqueKeys_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensors_); + ar & BOOST_SERIALIZATION_NVP(cameraRig_); + ar & BOOST_SERIALIZATION_NVP(cameraIds_); } }; diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index f35bdc950..bd4fb0642 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -69,6 +69,7 @@ SmartProjectionParams params; // default Cal3_S2 poses namespace vanillaPose { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index ef330cdd0..19a1a9f19 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -54,1082 +54,1084 @@ LevenbergMarquardtParams lmParams; /* ************************************************************************* */ TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); } -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor2) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor1(model, params); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor3) { - using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); - factor1->add(measurement1, x1, sharedK); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor4) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor1(model, params); - factor1.add(measurement1, x1, sharedK); -} - -/* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Equals ) { - using namespace vanillaPose; - SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); - factor1->add(measurement1, x1, sharedK); - - SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); - factor2->add(measurement1, x1, sharedK); - - CHECK(assert_equal(*factor1, *factor2)); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 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); - - SmartFactorP factor(model); - factor.add(level_uv, x1, sharedK); - factor.add(level_uv_right, x2, sharedK); - - 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); - - SmartFactorP::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(&SmartFactorP::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 - SmartFactorP::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; - SmartFactorP::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( SmartProjectionRigFactor, 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)); - - SmartFactorP::shared_ptr factor(new SmartFactorP(model)); - factor->add(level_uv, x1, sharedK); - factor->add(level_uv_right, x2, sharedK); - - double actualError1 = factor->error(values); - - SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); - Point2Vector measurements; - measurements.push_back(level_uv); - measurements.push_back(level_uv_right); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - KeyVector views { x1, x2 }; - - factor2->add(measurements, views, sharedKs); - double actualError2 = factor2->error(values); - DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - std::vector body_T_sensors; - body_T_sensors.push_back(body_T_sensor); - body_T_sensors.push_back(body_T_sensor); - body_T_sensors.push_back(body_T_sensor); - - SmartFactorP smartFactor1(model, params); - smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); - - SmartFactorP smartFactor2(model, params); - smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); - - SmartFactorP smartFactor3(model, params); - smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); - ; - - 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(); -// graph.print("graph\n"); - EXPECT(assert_equal(wTb3, result.at(x3))); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedK2s); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedK2s); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedK2s); - - 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( SmartProjectionRigFactor, 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 }; - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP - > (model); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::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(); - - Values values; - values.insert(x1, cam1.pose()); - values.insert(x2, cam2.pose()); - - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values, 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); - } -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - 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( SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - 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( SmartProjectionRigFactor, dynamicOutlierRejection ) { - - using namespace vanillaPose; - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error - - KeyVector views { x1, x2, x3 }; - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - // 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::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); - smartFactor3->add(measurements_cam3, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); - smartFactor4->add(measurements_cam4, views, sharedKs); - - 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( SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartProjectionParams params; - params.setRankTolerance(10); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor1->add(measurements_cam1, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor2->add(measurements_cam2, views, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default - smartFactor3->add(measurements_cam3, views, sharedKs); - - 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( SmartProjectionRigFactor, 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); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; - sharedK2s.push_back(sharedK2); - sharedK2s.push_back(sharedK2); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedK2s); - - 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( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { - using namespace bundlerPose; - SmartProjectionParams params; - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - SmartFactorP factor(model, params); - factor.add(measurement1, x1, sharedBundlerK); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 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 }; - - std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; - sharedBundlerKs.push_back(sharedBundlerK); - sharedBundlerKs.push_back(sharedBundlerK); - sharedBundlerKs.push_back(sharedBundlerK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_cam1, views, sharedBundlerKs); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_cam2, views, sharedBundlerKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_cam3, views, sharedBundlerKs); - - 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)); -} - -#include -typedef GenericProjectionFactor TestProjectionFactor; -static Symbol l0('L', 0); -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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 vanillaPose; - 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 keys; - keys.push_back(x1); - keys.push_back(x2); - keys.push_back(x3); - keys.push_back(x1); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); - - 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 ProjectionFactor factor to calculate the Jacobians - Matrix F = Matrix::Zero(2 * 4, 6 * 3); - Matrix E = Matrix::Zero(2 * 4, 3); - Vector b = Vector::Zero(2 * 4); - - // create projection factors rolling shutter - TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, - sharedK); - Matrix HPoseActual, HEActual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); - F.block<2, 6>(0, 0) = HPoseActual; - E.block<2, 3>(0, 0) = HEActual; - - TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, - sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, - HEActual); - F.block<2, 6>(2, 6) = HPoseActual; - E.block<2, 3>(2, 0) = HEActual; - - TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, - sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, - HEActual); - F.block<2, 6>(4, 12) = HPoseActual; - E.block<2, 3>(4, 0) = HEActual; - - TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, - sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); - F.block<2, 6>(6, 0) = HPoseActual; - E.block<2, 3>(6, 0) = HEActual; - - // 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_projFactors; - nfg_projFactors.add(factor11); - nfg_projFactors.add(factor12); - nfg_projFactors.add(factor13); - nfg_projFactors.add(factor14); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactors.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { - - using namespace vanillaPose; - 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 keys; - keys.push_back(x1); - keys.push_back(x2); - keys.push_back(x3); - - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - - // 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 keys_redundant = keys; - keys_redundant.push_back(keys.at(0)); // we readd the first key - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; - sharedKs_redundant.push_back(sharedK);// we readd the first calibration - - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); - smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); - - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); - smartFactor2->add(measurements_lmk2, keys, sharedKs); - - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); - smartFactor3->add(measurements_lmk3, keys, sharedKs); - - 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 -// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor -//-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) -//| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) -/* *************************************************************************/ -TEST( SmartProjectionRigFactor, 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, sharedKSimple, body_P_sensorId); - smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(SmartFactorP_LINEARIZE); - smartFactorP->linearize(values); - gttoc_(SmartFactorP_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_(SmartPoseFactor_LINEARIZE); - smartFactor->linearize(values); - gttoc_(SmartPoseFactor_LINEARIZE); - } - tictoc_print_(); -} -#endif - -/* ************************************************************************* */ -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(SmartProjectionRigFactor, serialize) { - using namespace vanillaPose; - using namespace gtsam::serializationTestHelpers; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartFactorP factor(model, params); - - EXPECT(equalsObj(factor)); - EXPECT(equalsXML(factor)); - EXPECT(equalsBinary(factor)); -} - -// SERIALIZATION TEST FAILS: to be fixed -//TEST(SmartProjectionRigFactor, serialize2) { +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor2) { +// using namespace vanillaPose; +// SmartProjectionParams params; +// params.setRankTolerance(rankTol); +// SmartFactorP factor1(model, params); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor3) { +// using namespace vanillaPose; +// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); +// factor1->add(measurement1, x1, sharedK); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Constructor4) { +// using namespace vanillaPose; +// SmartProjectionParams params; +// params.setRankTolerance(rankTol); +// SmartFactorP factor1(model, params); +// factor1.add(measurement1, x1, sharedK); +//} +// +///* ************************************************************************* */ +//TEST( SmartProjectionRigFactor, Equals ) { +// using namespace vanillaPose; +// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); +// factor1->add(measurement1, x1, sharedK); +// +// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); +// factor2->add(measurement1, x1, sharedK); +// +// CHECK(assert_equal(*factor1, *factor2)); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 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); +// +// SmartFactorP factor(model); +// factor.add(level_uv, x1, sharedK); +// factor.add(level_uv_right, x2, sharedK); +// +// 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); +// +// SmartFactorP::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(&SmartFactorP::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 +// SmartFactorP::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; +// SmartFactorP::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( SmartProjectionRigFactor, 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)); +// +// SmartFactorP::shared_ptr factor(new SmartFactorP(model)); +// factor->add(level_uv, x1, sharedK); +// factor->add(level_uv_right, x2, sharedK); +// +// double actualError1 = factor->error(values); +// +// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); +// Point2Vector measurements; +// measurements.push_back(level_uv); +// measurements.push_back(level_uv_right); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// KeyVector views { x1, x2 }; +// +// factor2->add(measurements, views, sharedKs); +// double actualError2 = factor2->error(values); +// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// std::vector body_T_sensors; +// body_T_sensors.push_back(body_T_sensor); +// body_T_sensors.push_back(body_T_sensor); +// body_T_sensors.push_back(body_T_sensor); +// +// SmartFactorP smartFactor1(model, params); +// smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); +// +// SmartFactorP smartFactor2(model, params); +// smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); +// +// SmartFactorP smartFactor3(model, params); +// smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); +// ; +// +// 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(); +//// graph.print("graph\n"); +// EXPECT(assert_equal(wTb3, result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedK2s); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedK2s); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedK2s); +// +// 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( SmartProjectionRigFactor, 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 }; +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP +// > (model); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::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(); +// +// Values values; +// values.insert(x1, cam1.pose()); +// values.insert(x2, cam2.pose()); +// +// boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 +// ->createHessianFactor(values, 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); +// } +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// 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( SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::JACOBIAN_SVD); +// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(false); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// 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( SmartProjectionRigFactor, dynamicOutlierRejection ) { +// +// using namespace vanillaPose; +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error +// +// KeyVector views { x1, x2, x3 }; +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// // 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::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); +// smartFactor4->add(measurements_cam4, views, sharedKs); +// +// 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( SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartProjectionParams params; +// params.setRankTolerance(10); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor1->add(measurements_cam1, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor2->add(measurements_cam2, views, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default +// smartFactor3->add(measurements_cam3, views, sharedKs); +// +// 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( SmartProjectionRigFactor, 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); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; +// sharedK2s.push_back(sharedK2); +// sharedK2s.push_back(sharedK2); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedK2s); +// +// 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( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { +// using namespace bundlerPose; +// SmartProjectionParams params; +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// SmartFactorP factor(model, params); +// factor.add(measurement1, x1, sharedBundlerK); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 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 }; +// +// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; +// sharedBundlerKs.push_back(sharedBundlerK); +// sharedBundlerKs.push_back(sharedBundlerK); +// sharedBundlerKs.push_back(sharedBundlerK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_cam1, views, sharedBundlerKs); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_cam2, views, sharedBundlerKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_cam3, views, sharedBundlerKs); +// +// 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)); +//} +// +//#include +//typedef GenericProjectionFactor TestProjectionFactor; +//static Symbol l0('L', 0); +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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 vanillaPose; +// 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 keys; +// keys.push_back(x1); +// keys.push_back(x2); +// keys.push_back(x3); +// keys.push_back(x1); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); +// +// 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 ProjectionFactor factor to calculate the Jacobians +// Matrix F = Matrix::Zero(2 * 4, 6 * 3); +// Matrix E = Matrix::Zero(2 * 4, 3); +// Vector b = Vector::Zero(2 * 4); +// +// // create projection factors rolling shutter +// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, +// sharedK); +// Matrix HPoseActual, HEActual; +// // note: b is minus the reprojection error, cf the smart factor jacobian computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(0, 0) = HPoseActual; +// E.block<2, 3>(0, 0) = HEActual; +// +// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, +// sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(2, 6) = HPoseActual; +// E.block<2, 3>(2, 0) = HEActual; +// +// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, +// sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(4, 12) = HPoseActual; +// E.block<2, 3>(4, 0) = HEActual; +// +// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, +// sharedK); +// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, +// HEActual); +// F.block<2, 6>(6, 0) = HPoseActual; +// E.block<2, 3>(6, 0) = HEActual; +// +// // 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_projFactors; +// nfg_projFactors.add(factor11); +// nfg_projFactors.add(factor12); +// nfg_projFactors.add(factor13); +// nfg_projFactors.add(factor14); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactors.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { +// +// using namespace vanillaPose; +// 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 keys; +// keys.push_back(x1); +// keys.push_back(x2); +// keys.push_back(x3); +// +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// sharedKs.push_back(sharedK); +// +// // 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 keys_redundant = keys; +// keys_redundant.push_back(keys.at(0)); // we readd the first key +// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; +// sharedKs_redundant.push_back(sharedK);// we readd the first calibration +// +// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); +// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); +// +// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); +// smartFactor2->add(measurements_lmk2, keys, sharedKs); +// +// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); +// smartFactor3->add(measurements_lmk3, keys, sharedKs); +// +// 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 +//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor +////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) +////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) +////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) +///* *************************************************************************/ +//TEST( SmartProjectionRigFactor, 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, sharedKSimple, body_P_sensorId); +// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SmartFactorP_LINEARIZE); +// smartFactorP->linearize(values); +// gttoc_(SmartFactorP_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_(SmartPoseFactor_LINEARIZE); +// smartFactor->linearize(values); +// gttoc_(SmartPoseFactor_LINEARIZE); +// } +// tictoc_print_(); +//} +//#endif +// +///* ************************************************************************* */ +//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(SmartProjectionRigFactor, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); // SmartFactorP factor(model, params); // -// // insert some measurements -// KeyVector key_view; -// Point2Vector meas_view; -// std::vector> sharedKs; -// -// -// key_view.push_back(Symbol('x', 1)); -// meas_view.push_back(Point2(10, 10)); -// sharedKs.push_back(sharedK); -// factor.add(meas_view, key_view, sharedKs); -// // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); // EXPECT(equalsBinary(factor)); //} +// +//// SERIALIZATION TEST FAILS: to be fixed +////TEST(SmartProjectionRigFactor, serialize2) { +//// using namespace vanillaPose; +//// using namespace gtsam::serializationTestHelpers; +//// SmartProjectionParams params; +//// params.setRankTolerance(rankTol); +//// SmartFactorP factor(model, params); +//// +//// // insert some measurements +//// KeyVector key_view; +//// Point2Vector meas_view; +//// std::vector> sharedKs; +//// +//// +//// key_view.push_back(Symbol('x', 1)); +//// meas_view.push_back(Point2(10, 10)); +//// sharedKs.push_back(sharedK); +//// factor.add(meas_view, key_view, sharedKs); +//// +//// EXPECT(equalsObj(factor)); +//// EXPECT(equalsXML(factor)); +//// EXPECT(equalsBinary(factor)); +////} /* ************************************************************************* */ int main() { From 4c10be9808983c4c4533b780fecf503cefc4ddf9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:12:46 -0400 Subject: [PATCH 025/110] fixed equal --- gtsam/slam/SmartProjectionRigFactor.h | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 600e48e8d..07efd775c 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -58,13 +58,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { protected: /// vector of keys (one for each observation) with potentially repeated keys - FastVector nonUniqueKeys_; + KeyVector nonUniqueKeys_; /// cameras in the rig (fixed poses wrt body + fixed intrinsics) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement - FastVector cameraIds_; + KeyVector cameraIds_; public: typedef CAMERA Camera; @@ -125,7 +125,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) */ - void add(const Point2Vector& measurements, const FastVector& poseKeys, + void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { assert(poseKeys.size() == measurements.size()); assert(poseKeys.size() == cameraIds.size()); @@ -135,7 +135,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return (for each observation) the (possibly non unique) keys involved in the measurements - const FastVector nonUniqueKeys() const { + const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } @@ -145,7 +145,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return the calibration object - inline FastVector cameraIds() const { + inline KeyVector cameraIds() const { return cameraIds_; } @@ -171,8 +171,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() - && cameraRig_.equals(e->cameraRig()); -// && cameraIds_ == e->cameraIds(); + && cameraRig_.equals(e->cameraRig()) + && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } /** From 117f0d1f45506085e39aab5aad4a9cc2226d3bd4 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:47:44 -0400 Subject: [PATCH 026/110] fixing tests --- gtsam/slam/SmartProjectionRigFactor.h | 14 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../tests/testSmartProjectionRigFactor.cpp | 580 +++++++++--------- 3 files changed, 301 insertions(+), 294 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 07efd775c..3d7f7f626 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -64,7 +64,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement - KeyVector cameraIds_; + FastVector cameraIds_; public: typedef CAMERA Camera; @@ -145,7 +145,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// return the calibration object - inline KeyVector cameraIds() const { + inline FastVector cameraIds() const { return cameraIds_; } @@ -184,11 +184,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3& body_P_cam_i = cameraRig_[i].pose(); + const Key cameraId = cameraIds_[i]; + const Pose3& body_P_cam_i = cameraRig_[cameraId].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) * body_P_cam_i; cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[i].calibration())); + make_shared(cameraRig_[cameraId].calibration())); } return cameras; } @@ -220,7 +221,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3 body_P_sensor = cameraRig_[i].pose(); + const Key cameraId = cameraIds_[i]; + const Pose3 body_P_sensor = cameraRig_[cameraId].pose(); const Pose3 sensor_P_body = body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; Eigen::Matrix H; @@ -242,7 +244,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Cameras cameras = this->cameras(values); // Create structures for Hessian Factors - KeyVector js; + FastVector js; FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); FastVector < Vector > gs(nrUniqueKeys); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index bd4fb0642..a9d0c43f7 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -84,6 +84,7 @@ Camera cam3(pose_above, sharedK); // default Cal3_S2 poses namespace vanillaPose2 { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionRigFactor SmartFactorP; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 19a1a9f19..d5206818e 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -45,6 +45,10 @@ static Symbol x1('X', 1); static Symbol x2('X', 2); static Symbol x3('X', 3); +Key cameraId1 = 0; // first camera +Key cameraId2 = 1; +Key cameraId3 = 2; + static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; @@ -59,294 +63,294 @@ TEST( SmartProjectionRigFactor, Constructor) { SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); } -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor2) { -// using namespace vanillaPose; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor1(model, params); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor3) { -// using namespace vanillaPose; -// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); -// factor1->add(measurement1, x1, sharedK); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Constructor4) { -// using namespace vanillaPose; -// SmartProjectionParams params; -// params.setRankTolerance(rankTol); -// SmartFactorP factor1(model, params); -// factor1.add(measurement1, x1, sharedK); -//} -// -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, Equals ) { -// using namespace vanillaPose; -// SmartFactorP::shared_ptr factor1(new SmartFactorP(model)); -// factor1->add(measurement1, x1, sharedK); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); -// factor2->add(measurement1, x1, sharedK); -// -// CHECK(assert_equal(*factor1, *factor2)); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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); -// -// SmartFactorP factor(model); -// factor.add(level_uv, x1, sharedK); -// factor.add(level_uv_right, x2, sharedK); -// -// 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); -// -// SmartFactorP::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(&SmartFactorP::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 -// SmartFactorP::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; -// SmartFactorP::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( SmartProjectionRigFactor, 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)); -// -// SmartFactorP::shared_ptr factor(new SmartFactorP(model)); -// factor->add(level_uv, x1, sharedK); -// factor->add(level_uv_right, x2, sharedK); -// -// double actualError1 = factor->error(values); -// -// SmartFactorP::shared_ptr factor2(new SmartFactorP(model)); -// Point2Vector measurements; -// measurements.push_back(level_uv); -// measurements.push_back(level_uv_right); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// KeyVector views { x1, x2 }; -// -// factor2->add(measurements, views, sharedKs); -// double actualError2 = factor2->error(values); -// DOUBLES_EQUAL(actualError1, actualError2, 1e-7); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// std::vector body_T_sensors; -// body_T_sensors.push_back(body_T_sensor); -// body_T_sensors.push_back(body_T_sensor); -// body_T_sensors.push_back(body_T_sensor); -// -// SmartFactorP smartFactor1(model, params); -// smartFactor1.add(measurements_cam1, views, sharedKs, body_T_sensors); -// -// SmartFactorP smartFactor2(model, params); -// smartFactor2.add(measurements_cam2, views, sharedKs, body_T_sensors); -// -// SmartFactorP smartFactor3(model, params); -// smartFactor3.add(measurements_cam3, views, sharedKs, body_T_sensors); -// ; -// -// 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(); -//// graph.print("graph\n"); -// EXPECT(assert_equal(wTb3, result.at(x3))); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedK2s); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedK2s); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedK2s); -// -// 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( SmartProjectionRigFactor, Constructor2) { + using namespace vanillaPose; + Cameras cameraRig; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, cameraRig, params); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor3) { + using namespace vanillaPose; + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + factor1->add(measurement1, x1, cameraId1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor4) { + using namespace vanillaPose; + Cameras cameraRig; + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactorP factor1(model, cameraRig, params); + factor1.add(measurement1, x1, cameraId1); +} + +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Equals ) { + using namespace vanillaPose; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + factor1->add(measurement1, x1, cameraId1); + + SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig)); + factor2->add(measurement1, x1, cameraId1); + + CHECK(assert_equal(*factor1, *factor2)); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, noiseless ) { + + using namespace vanillaPose; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + // Project two landmarks into two cameras + Point2 level_uv = level_camera.project(landmark1); + Point2 level_uv_right = level_camera_right.project(landmark1); + + SmartFactorP factor(model, cameraRig); + factor.add(level_uv, x1, cameraId1); // both taken from the same camera + factor.add(level_uv_right, x2, cameraId1); + + 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); + + SmartFactorP::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(&SmartFactorP::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 + SmartFactorP::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; + SmartFactorP::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( SmartProjectionRigFactor, noisy ) { + + using namespace vanillaPose; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + // 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)); + + SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig)); + factor->add(level_uv, x1, cameraId1); + factor->add(level_uv_right, x2, cameraId1); + + double actualError1 = factor->error(values); + + // create other factor by passing multiple measurements + SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig)); + + Point2Vector measurements; + measurements.push_back(level_uv); + measurements.push_back(level_uv_right); + + KeyVector views { x1, x2 }; + FastVector cameraIds { 0, 0 }; + + factor2->add(measurements, views, cameraIds); + double actualError2 = factor2->error(values); + DOUBLES_EQUAL(actualError1, actualError2, 1e-7); +} + +/* *************************************************************************/ +TEST(SmartProjectionRigFactor, 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)); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_T_sensor, sharedK) ); + + // 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 }; + FastVector cameraIds { 0, 0, 0 }; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartFactorP smartFactor1(model, cameraRig, params); + smartFactor1.add(measurements_cam1, views, cameraIds); + + SmartFactorP smartFactor2(model, cameraRig, params); + smartFactor2.add(measurements_cam2, views, cameraIds); + + SmartFactorP smartFactor3(model, cameraRig, params); + smartFactor3.add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { + + using namespace vanillaPose2; + Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + + // 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}; + FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, Factors ) { // From eb82878044396bfd2d025274965e1190c6131bac Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 21:51:44 -0400 Subject: [PATCH 027/110] halfway there --- .../tests/testSmartProjectionRigFactor.cpp | 306 +++++++++--------- 1 file changed, 155 insertions(+), 151 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index d5206818e..fcdf3e2a2 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -351,157 +351,161 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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 }; -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP -// > (model); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::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(); -// -// Values values; -// values.insert(x1, cam1.pose()); -// values.insert(x2, cam2.pose()); -// -// boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 -// ->createHessianFactor(values, 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); -// } -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// 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( SmartProjectionRigFactor, 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 + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + + KeyVector views { x1, x2 }; + FastVector cameraIds { 0, 0 }; + + SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + > (model,cameraRig); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::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(); + + Values values; + values.insert(x1, cam1.pose()); + values.insert(x2, cam2.pose()); + + boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 + ->createHessianFactor(values, 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); + } +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, 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); + + std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + sharedKs.push_back(sharedK); + + // create smart factor + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, landmarkDistance ) { // From fa4de18742e1b11c6742ae50cfff6c96b3df4a21 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 22:00:09 -0400 Subject: [PATCH 028/110] working on tests --- gtsam/slam/SmartProjectionRigFactor.h | 6 +- .../tests/testSmartProjectionRigFactor.cpp | 495 +++++++++--------- 2 files changed, 250 insertions(+), 251 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 3d7f7f626..2ec2ed86f 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -127,8 +127,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { - assert(poseKeys.size() == measurements.size()); - assert(poseKeys.size() == cameraIds.size()); + if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){ + throw std::runtime_error("SmartProjectionRigFactor: " + "trying to add inconsistent inputs"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], poseKeys[i], cameraIds[i]); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index fcdf3e2a2..4c6b33655 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -506,255 +506,252 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::JACOBIAN_SVD); -// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(false); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// 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( SmartProjectionRigFactor, dynamicOutlierRejection ) { -// -// using namespace vanillaPose; -// -// double excludeLandmarksFutherThanDist = 1e10; -// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error -// -// KeyVector views { x1, x2, x3 }; -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// // 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::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params)); -// smartFactor4->add(measurements_cam4, views, sharedKs); -// -// 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( SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartProjectionParams params; -// params.setRankTolerance(10); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor1->add(measurements_cam1, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor2->add(measurements_cam2, views, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default -// smartFactor3->add(measurements_cam3, views, sharedKs); -// -// 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( SmartProjectionRigFactor, 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); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s; -// sharedK2s.push_back(sharedK2); -// sharedK2s.push_back(sharedK2); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedK2s); -// -// 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( SmartProjectionRigFactor, 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); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, 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::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params)); + smartFactor4->add(measurements_cam4, views, cameraIds); + + 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( SmartProjectionRigFactor, 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); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0}; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, 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); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + FastVector cameraIds { 0, 0 }; + + SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + 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( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { // using namespace bundlerPose; From 3758fdaa5d7e92d5b45f3b9defe075afce6d46d9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 23:10:05 -0400 Subject: [PATCH 029/110] all tests work except serialization --- gtsam/slam/SmartProjectionRigFactor.h | 25 +- gtsam/slam/tests/smartFactorScenarios.h | 8 +- .../tests/testSmartProjectionRigFactor.cpp | 754 +++++++++--------- .../SmartProjectionPoseFactorRollingShutter.h | 6 +- 4 files changed, 395 insertions(+), 398 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 2ec2ed86f..581859b1f 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** * This factor assumes that camera calibration is fixed (but each camera * measurement can have a different extrinsic and intrinsic calibration). - * The factor only constrains poses (variable dimension is 6). + * The factor only constrains poses (variable dimension is 6 for each pose). * This factor requires that values contains the involved poses (Pose3). * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead! @@ -60,7 +60,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement @@ -185,13 +185,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; + cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3& body_P_cam_i = cameraRig_[cameraId].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) - * body_P_cam_i; + * cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[cameraId].calibration())); + make_shared(cameraRig_[ cameraIds_[i] ].calibration())); } return cameras; } @@ -223,10 +222,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3 body_P_sensor = cameraRig_[cameraId].pose(); - const Pose3 sensor_P_body = body_P_sensor.inverse(); - const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; + const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); + const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); Fs.at(i) = Fs.at(i) * H; @@ -246,11 +243,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Cameras cameras = this->cameras(values); // Create structures for Hessian Factors - FastVector js; - FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - FastVector < Vector > gs(nrUniqueKeys); + std::vector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector < Vector > gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera + if (this->measured_.size() != cameras.size()) // 1 observation per camera throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index a9d0c43f7..b17ffdac6 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -71,7 +71,7 @@ namespace vanillaPose { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -86,7 +86,7 @@ namespace vanillaPose2 { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -99,6 +99,7 @@ Camera cam3(pose_above, sharedK2); // Cal3Bundler cameras namespace bundler { typedef PinholeCamera Camera; +typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); Camera level_camera(level_pose, K); @@ -115,8 +116,9 @@ typedef GeneralSFMFactor SFMFactor; // Cal3Bundler poses namespace bundlerPose { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4c6b33655..db077525b 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -60,7 +60,7 @@ TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ @@ -69,7 +69,7 @@ TEST( SmartProjectionRigFactor, Constructor2) { Cameras cameraRig; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } @@ -88,7 +88,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); factor1.add(measurement1, x1, cameraId1); } @@ -98,10 +98,10 @@ TEST( SmartProjectionRigFactor, Equals ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); - SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); @@ -119,7 +119,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactorP factor(model, cameraRig); + SmartRigFactor factor(model, cameraRig); factor.add(level_uv, x1, cameraId1); // both taken from the same camera factor.add(level_uv_right, x2, cameraId1); @@ -131,13 +131,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactorP::Cameras cameras = factor.cameras(values); + SmartRigFactor::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(&SmartFactorP::whitenedError, factor, cameras, + std::bind(&SmartRigFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP @@ -153,7 +153,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { EXPECT(assert_equal(expectedE, actualE, 1e-7)); // Calculate using reprojectionError - SmartFactorP::Cameras::FBlocks F; + SmartRigFactor::Cameras::FBlocks F; Matrix E; Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -162,7 +162,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { // Calculate using computeJacobians Vector b; - SmartFactorP::FBlocks Fs; + SmartRigFactor::FBlocks Fs; factor.computeJacobians(Fs, E, b, cameras, *point); double actualError3 = b.squaredNorm(); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -188,14 +188,14 @@ TEST( SmartProjectionRigFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); @@ -244,13 +244,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactorP smartFactor1(model, cameraRig, params); + SmartRigFactor smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_cam1, views, cameraIds); - SmartFactorP smartFactor2(model, cameraRig, params); + SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views, cameraIds); - SmartFactorP smartFactor3(model, cameraRig, params); + SmartRigFactor smartFactor3(model, cameraRig, params); smartFactor3.add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -305,13 +305,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { KeyVector views {x1,x2,x3}; FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -378,11 +378,11 @@ TEST( SmartProjectionRigFactor, Factors ) { KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor > (model,cameraRig); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::Cameras cameras; + SmartRigFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -465,13 +465,13 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -533,13 +533,13 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -599,16 +599,16 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -662,13 +662,13 @@ TEST( SmartProjectionRigFactor, CheckHessian) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -735,7 +735,7 @@ TEST( SmartProjectionRigFactor, Hessian ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), @@ -752,347 +752,340 @@ TEST( SmartProjectionRigFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { -// using namespace bundlerPose; -// SmartProjectionParams params; -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactorP factor(model, params); -// factor.add(measurement1, x1, sharedBundlerK); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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 }; -// -// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedBundlerKs); -// -// 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)); -//} -// -//#include -//typedef GenericProjectionFactor TestProjectionFactor; -//static Symbol l0('L', 0); -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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 vanillaPose; -// 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 keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// keys.push_back(x1); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); -// -// 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 ProjectionFactor factor to calculate the Jacobians -// Matrix F = Matrix::Zero(2 * 4, 6 * 3); -// Matrix E = Matrix::Zero(2 * 4, 3); -// Vector b = Vector::Zero(2 * 4); -// -// // create projection factors rolling shutter -// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, -// sharedK); -// Matrix HPoseActual, HEActual; -// // note: b is minus the reprojection error, cf the smart factor jacobian computation -// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(0, 0) = HPoseActual; -// E.block<2, 3>(0, 0) = HEActual; -// -// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, -// sharedK); -// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(2, 6) = HPoseActual; -// E.block<2, 3>(2, 0) = HEActual; -// -// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, -// sharedK); -// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(4, 12) = HPoseActual; -// E.block<2, 3>(4, 0) = HEActual; -// -// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, -// sharedK); -// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(6, 0) = HPoseActual; -// E.block<2, 3>(6, 0) = HEActual; -// -// // 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_projFactors; -// nfg_projFactors.add(factor11); -// nfg_projFactors.add(factor12); -// nfg_projFactors.add(factor13); -// nfg_projFactors.add(factor14); -// values.insert(l0, *point); -// -// double actualError = smartFactor1->error(values); -// double expectedError = nfg_projFactors.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { -// -// using namespace vanillaPose; -// 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 keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// // 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 keys_redundant = keys; -// keys_redundant.push_back(keys.at(0)); // we readd the first key -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; -// sharedKs_redundant.push_back(sharedK);// we readd the first calibration -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_lmk2, keys, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_lmk3, keys, sharedKs); -// -// 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 -//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor -////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) -////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) -////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, 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, sharedKSimple, body_P_sensorId); -// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SmartFactorP_LINEARIZE); -// smartFactorP->linearize(values); -// gttoc_(SmartFactorP_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_(SmartPoseFactor_LINEARIZE); -// smartFactor->linearize(values); -// gttoc_(SmartPoseFactor_LINEARIZE); -// } -// tictoc_print_(); -//} -//#endif -// +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartRigFactor factor(model, cameraRig, params); + factor.add(measurement1, x1, cameraId1); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, 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 }; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + FastVector cameraIds { 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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)); +} + +#include +typedef GenericProjectionFactor TestProjectionFactor; +static Symbol l0('L', 0); +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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 vanillaPose; + 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 keys { x1, x2, x3, x1}; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); + + 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 ProjectionFactor factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(2 * 4); + + // create projection factors rolling shutter + TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, + sharedK); + Matrix HPoseActual, HEActual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(0, 0) = HPoseActual; + E.block<2, 3>(0, 0) = HEActual; + + TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, + HEActual); + F.block<2, 6>(2, 6) = HPoseActual; + E.block<2, 3>(2, 0) = HEActual; + + TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, + HEActual); + F.block<2, 6>(4, 12) = HPoseActual; + E.block<2, 3>(4, 0) = HEActual; + + TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(6, 0) = HPoseActual; + E.block<2, 3>(6, 0) = HEActual; + + // 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_projFactors; + nfg_projFactors.add(factor11); + nfg_projFactors.add(factor12); + nfg_projFactors.add(factor13); + nfg_projFactors.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactors.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { + + using namespace vanillaPose; + 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 keys { x1, x2, x3 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0 }; + FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + + // 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 keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_lmk2, keys, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_lmk3, keys, cameraIds); + + 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 +// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, 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(); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + + // 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 = 10000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); + smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartRigFactor_LINEARIZE); + smartFactorP->linearize(values); + gttoc_(SmartRigFactor_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_(SmartPoseFactor_LINEARIZE); + smartFactor->linearize(values); + gttoc_(SmartPoseFactor_LINEARIZE); + } + tictoc_print_(); +} +#endif + ///* ************************************************************************* */ //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); @@ -1107,7 +1100,11 @@ TEST( SmartProjectionRigFactor, Hessian ) { // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// SmartFactorP factor(model, params); +// +// Cameras cameraRig; // single camera in the rig +// cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); +// +// SmartRigFactor factor(model, cameraRig, params); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); @@ -1120,14 +1117,13 @@ TEST( SmartProjectionRigFactor, Hessian ) { //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); -//// SmartFactorP factor(model, params); +//// SmartRigFactor factor(model, params); //// //// // insert some measurements //// KeyVector key_view; //// Point2Vector meas_view; //// std::vector> sharedKs; //// -//// //// key_view.push_back(Symbol('x', 1)); //// meas_view.push_back(Point2(10, 10)); //// sharedKs.push_back(sharedK); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 7660ff236..d58aea148 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -341,19 +341,21 @@ class SmartProjectionPoseFactorRollingShutter this->keys_ .size(); // note: by construction, keys_ only contains unique keys + typename Base::Cameras cameras = this->cameras(values); + // Create structures for Hessian Factors KeyVector js; std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); if (this->measured_.size() != - this->cameras(values).size()) // 1 observation per interpolated camera + cameras.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)); + this->triangulateSafe(cameras); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { From 4a88574f6749063ed917ae3c86c38080b98f5303 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 23:16:28 -0400 Subject: [PATCH 030/110] added extra test with multi cam --- .../tests/testSmartProjectionRigFactor.cpp | 87 +++++++++++++++++++ 1 file changed, 87 insertions(+) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index db077525b..fb3f3c461 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -288,6 +288,93 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { EXPECT(assert_equal(wTb3, result.at(x3))); } +/* *************************************************************************/ +TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { + using namespace vanillaPose; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), + Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3::identity(); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_T_sensor1, sharedK) ); + cameraRig.push_back( Camera(body_T_sensor2, sharedK) ); + cameraRig.push_back( Camera(body_T_sensor3, sharedK) ); + + // These are the poses we want to estimate, from camera measurements + const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); + const Pose3 sensor_T_body2 = body_T_sensor2.inverse(); + const Pose3 sensor_T_body3 = body_T_sensor3.inverse(); + Pose3 wTb1 = cam1.pose() * sensor_T_body1; + Pose3 wTb2 = cam2.pose() * sensor_T_body2; + Pose3 wTb3 = cam3.pose() * sensor_T_body3; + + // 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 }; + FastVector cameraIds { 0, 1, 2 }; + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setEnableEPI(false); + + SmartRigFactor smartFactor1(model, cameraRig, params); + smartFactor1.add(measurements_cam1, views, cameraIds); + + SmartRigFactor smartFactor2(model, cameraRig, params); + smartFactor2.add(measurements_cam2, views, cameraIds); + + SmartRigFactor smartFactor3(model, cameraRig, params); + smartFactor3.add(measurements_cam3, views, cameraIds); + + 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( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { From fe59f3db19a0b56a18d560e482c48890bfbd8258 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 03:56:25 -0400 Subject: [PATCH 031/110] AdjointMap jacobians for Pose3 --- gtsam/geometry/Pose3.cpp | 36 ++++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 10 +++++---- gtsam/geometry/tests/testPose3.cpp | 30 +++++++++++++++++++++++++ 3 files changed, 72 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index c183e32ed..93d6248e6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -63,6 +63,42 @@ Matrix6 Pose3::AdjointMap() const { return adj; } +/* ************************************************************************* */ +// Calculate AdjointMap applied to xi_b, with Jacobians +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, + OptionalJacobian<6, 6> H_xib) const { + // Ad * xi = [ R 0 * [w + // [t]R R ] v] + + // Declarations and aliases + Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; + Vector6 result; + auto Rw = result.head<3>(); + const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + + // Calculations + Rw = R_.rotate(w, Rw_H_R, Rw_H_w); + const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v); + const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw); + result.tail<3>() = pRw + Rv; + pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason + + // Jacobians + if (H_this) { + *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) + .finished(); + } + if (H_xib) { + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) + .finished(); + } + + // Return + return result; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index d76e1b41a..9a8a7e939 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -148,12 +148,14 @@ public: Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect /** - * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a + * body-fixed velocity, transforming it to the spatial frame * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + * Note that H_xib = AdjointMap() */ - Vector6 Adjoint(const Vector6& xi_b) const { - return AdjointMap() * xi_b; - } /// FIXME Not tested - marked as incorrect + Vector6 Adjoint(const Vector6& xi_b, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_xib = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 7c1fa81e6..9fde4c2c1 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -145,6 +145,36 @@ TEST(Pose3, Adjoint_full) EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); } +/* ************************************************************************* */ +// Check Adjoint numerical derivatives +TEST(Pose3, Adjoint_jacobians) +{ + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function Adjoint_proxy = + [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + + gtsam::Vector6 xi = + (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + T.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.Adjoint(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi); + expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat) From bcafdb75573bcaff421be29fd104a4cd911ab6ae Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 04:04:28 -0400 Subject: [PATCH 032/110] only compute jacobians when needed --- gtsam/geometry/Pose3.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 93d6248e6..2f4fa4da3 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -67,9 +67,8 @@ Matrix6 Pose3::AdjointMap() const { // Calculate AdjointMap applied to xi_b, with Jacobians Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_xib) const { - // Ad * xi = [ R 0 * [w - // [t]R R ] v] - + // Ad * xi = [ R 0 . [w + // [t]R R ] v] // Declarations and aliases Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; Vector6 result; @@ -77,14 +76,16 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); // Calculations - Rw = R_.rotate(w, Rw_H_R, Rw_H_w); - const Vector3 Rv = R_.rotate(v, Rv_H_R, Rv_H_v); - const Vector3 pRw = cross(t_, Rw, pRw_H_t, pRw_H_Rw); + Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); + const Vector3 Rv = + R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); + const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, + H_this || H_xib ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; - pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason // Jacobians if (H_this) { + pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); From ecf53a68543248ac0ca210654759349941f6973d Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 04:58:16 -0400 Subject: [PATCH 033/110] AdjointTranspose --- gtsam/geometry/Pose3.cpp | 36 ++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 5 ++++ gtsam/geometry/tests/testPose3.cpp | 41 +++++++++++++++++++++++++++++- 3 files changed, 81 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 2f4fa4da3..daa889c0d 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -100,6 +100,42 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, return result; } +/* ************************************************************************* */ +/// The dual version of Adjoint +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, + OptionalJacobian<6, 6> H_x) const { + // Ad^T * xi = [ R^T R^T.[-t] . [w + // 0 R^T ] v] + // Declarations and aliases + Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv; + Vector6 result; + const Vector3 &w = x.head<3>(), &v = x.tail<3>(); + auto Rv = result.tail<3>(); + + // Calculations + const Vector3 Rw = + R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); + Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); + const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v); + const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv); + result.head<3>() = Rw - Rtv; + + // Jacobians + if (H_this) { + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t + /* */ Rv_H_R, /* */ Z_3x3) + .finished(); + } + if (H_x) { + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // + /* */ Z_3x3, Rv_H_v) + .finished(); + } + + // Return + return result; +} + /* ************************************************************************* */ Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 9a8a7e939..cff5fd231 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -156,6 +156,11 @@ public: Vector6 Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this = boost::none, OptionalJacobian<6, 6> H_xib = boost::none) const; + + /// The dual version of Adjoint + Vector6 AdjointTranspose(const Vector6& x, + OptionalJacobian<6, 6> H_this = boost::none, + OptionalJacobian<6, 6> H_x = boost::none) const; /** * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 9fde4c2c1..722087934 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -151,7 +151,11 @@ TEST(Pose3, Adjoint_jacobians) { Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; gtsam::Vector6 xi = (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); @@ -175,6 +179,41 @@ TEST(Pose3, Adjoint_jacobians) EXPECT(assert_equal(expectedH2, actualH2)); } +/* ************************************************************************* */ +// Check AdjointTranspose and jacobians +TEST(Pose3, AdjointTranspose) +{ + Matrix6 actualH1, actualH2, expectedH1, expectedH2; + std::function AdjointTranspose_proxy = + [&](const Pose3& T, const Vector6& xi) { + Vector6 res1 = T.AdjointMap().transpose() * xi, + res2 = T.AdjointTranspose(xi); + EXPECT(assert_equal(res1, res2)); + return res1; + }; + + gtsam::Vector6 xi = + (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + T.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T2.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH2, actualH2)); + + T3.AdjointTranspose(xi, actualH1, actualH2); + expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); + expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); + EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH2, actualH2)); +} + /* ************************************************************************* */ // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) TEST(Pose3, Adjoint_hat) From ab1b926dcd92c49004b293f79bf84762b093fb18 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 05:03:00 -0400 Subject: [PATCH 034/110] only compute intermediate jacobians when needed --- gtsam/geometry/Pose3.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index daa889c0d..27a5cf557 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -80,7 +80,7 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, - H_this || H_xib ? &pRw_H_Rw : nullptr); + (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians @@ -116,8 +116,10 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); - const Vector3 tv = cross(t_, v, tv_H_t, tv_H_v); - const Vector3 Rtv = R_.unrotate(tv, Rtv_H_R, Rtv_H_tv); + const Vector3 tv = + cross(t_, v, H_this ? &tv_H_t : nullptr, H_x ? &tv_H_v : nullptr); + const Vector3 Rtv = R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr, + (H_this || H_x) ? &Rtv_H_tv : nullptr); result.head<3>() = Rw - Rtv; // Jacobians From f7fed8b5f50218a289a97e65b104465e1f1aca7b Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Mon, 4 Oct 2021 11:19:19 -0400 Subject: [PATCH 035/110] review comment: move check outside lambda --- gtsam/geometry/tests/testPose3.cpp | 36 +++++++++++++++++------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 722087934..fe527ab2e 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -149,16 +149,17 @@ TEST(Pose3, Adjoint_full) // Check Adjoint numerical derivatives TEST(Pose3, Adjoint_jacobians) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation sanity check + EQUALITY(static_cast(T.AdjointMap() * xi), T.Adjoint(xi)); + EQUALITY(static_cast(T2.AdjointMap() * xi), T2.Adjoint(xi)); + EQUALITY(static_cast(T3.AdjointMap() * xi), T3.Adjoint(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap() * xi, res2 = T.Adjoint(xi); - EXPECT(assert_equal(res1, res2)); - return res1; - }; - - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + [&](const Pose3& T, const Vector6& xi) { return T.AdjointMap() * xi; }; T.Adjoint(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); @@ -183,18 +184,23 @@ TEST(Pose3, Adjoint_jacobians) // Check AdjointTranspose and jacobians TEST(Pose3, AdjointTranspose) { + Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); + + // Check evaluation + EQUALITY(static_cast(T.AdjointMap().transpose() * xi), + T.AdjointTranspose(xi)); + EQUALITY(static_cast(T2.AdjointMap().transpose() * xi), + T2.AdjointTranspose(xi)); + EQUALITY(static_cast(T3.AdjointMap().transpose() * xi), + T3.AdjointTranspose(xi)); + + // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function AdjointTranspose_proxy = [&](const Pose3& T, const Vector6& xi) { - Vector6 res1 = T.AdjointMap().transpose() * xi, - res2 = T.AdjointTranspose(xi); - EXPECT(assert_equal(res1, res2)); - return res1; + return T.AdjointMap().transpose() * xi; }; - gtsam::Vector6 xi = - (gtsam::Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished(); - T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); From 222380ce485a06c179e1cba2d25598ecb079a8e3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 4 Oct 2021 20:29:40 -0400 Subject: [PATCH 036/110] added more comments here and there --- gtsam/slam/SmartProjectionRigFactor.h | 35 +++++++++++++++------------ 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 581859b1f..0246d6327 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -36,10 +36,10 @@ namespace gtsam { */ /** - * This factor assumes that camera calibration is fixed (but each camera - * measurement can have a different extrinsic and intrinsic calibration). - * The factor only constrains poses (variable dimension is 6 for each pose). - * This factor requires that values contains the involved poses (Pose3). + * This factor assumes that camera calibration is fixed (but each measurement + * can be taken by a different camera in the rig, hence can have a different + * extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension + * is 6 for each pose). This factor requires that values contains the involved poses (Pose3). * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead! * @addtogroup SLAM @@ -53,7 +53,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 2; ///< Measurement dimension (Point2) + static const int ZDim = 2; ///< Measurement dimension protected: @@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement FastVector cameraIds_; public: @@ -84,10 +84,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param params parameters for the smart projection factors */ SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Cameras& cameraRig, - const SmartProjectionParams& params = - SmartProjectionParams()) - : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { + const Cameras& cameraRig, + const SmartProjectionParams& params = + SmartProjectionParams()) + : Base(sharedNoiseModel, params), + cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -123,7 +124,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measurements vector of the 2m dimensional location of the projection * of a single landmark in the m views (the measurements) * @param poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as measurements) + * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds) { @@ -185,12 +186,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - cameras.reserve(nonUniqueKeys_.size()); // preallocate + cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) - * cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i - cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[ cameraIds_[i] ].calibration())); + const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body + * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + cameras.emplace_back( + world_P_sensor_i, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } From 7988a7050f26e448e94e5ca532251af01d4188dc Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 4 Oct 2021 21:33:26 -0400 Subject: [PATCH 037/110] changing API for rolling shutter --- .../SmartProjectionPoseFactorRollingShutter.h | 160 +- ...martProjectionPoseFactorRollingShutter.cpp | 2026 +++++++++-------- 2 files changed, 1092 insertions(+), 1094 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d58aea148..cf1526673 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -11,8 +11,7 @@ /** * @file SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect with - * given readout time + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone */ @@ -43,7 +42,10 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - public: + + private: + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactorRollingShutter This; typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -58,24 +60,23 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// Pose of the camera in the body frame - std::vector body_P_sensors_; + /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics) + typename Base::Cameras cameraRig_; + + /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement + FastVector cameraIds_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// shorthand for base class type - typedef SmartProjectionFactor> Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactorRollingShutter This; + typedef CAMERA Camera; + typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = - 12; ///< size of the variable stacking 2 poses from which the observation - ///< pose is interpolated + static const int 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 @@ -89,9 +90,30 @@ class SmartProjectionPoseFactorRollingShutter * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, + const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) {} + : Base(sharedNoiseModel, params), + cameraRig_(cameraRig) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + } + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, const Camera& camera, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; @@ -112,8 +134,7 @@ class SmartProjectionPoseFactorRollingShutter */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, - const boost::shared_ptr& K, - const Pose3& body_P_sensor = Pose3::identity()) { + const size_t cameraId = 0) { // store measurements in base class this->measured_.push_back(measured); @@ -133,11 +154,8 @@ class SmartProjectionPoseFactorRollingShutter // store interpolation factor alphas_.push_back(alpha); - // store fixed intrinsic calibration - K_all_.push_back(K); - - // store fixed extrinsics of the camera - body_P_sensors_.push_back(body_P_sensor); + // store id of the camera taking the measurement + cameraIds_.push_back(cameraId); } /** @@ -156,61 +174,39 @@ class SmartProjectionPoseFactorRollingShutter void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, 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() == alphas.size()); - assert(world_P_body_key_pairs.size() == Ks.size()); + const FastVector& cameraIds = FastVector()) { + + if (world_P_body_key_pairs.size() != measurements.size() + || world_P_body_key_pairs.size() != alphas.size() + || world_P_body_key_pairs.size() != cameraIds.size()) { + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "trying to add inconsistent inputs"); + } 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, alphas[i], Ks[i], - body_P_sensors[i]); + world_P_body_key_pairs[i].second, alphas[i], + cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified } } - /** - * 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 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 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& 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() == 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, alphas[i], K, body_P_sensor); - } - } - - /// return the calibration object - 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 { + 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_; } + /// return the calibration object + inline Cameras cameraRig() const { + return cameraRig_; + } + + /// return the calibration object + inline FastVector cameraIds() const { + return cameraIds_; + } /** * print @@ -228,8 +224,8 @@ class SmartProjectionPoseFactorRollingShutter std::cout << " pose2 key: " << 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 = "); + std::cout << "cameraId: " << cameraIds_[i] << std::endl; + cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -237,8 +233,7 @@ class SmartProjectionPoseFactorRollingShutter /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>( - &p); + dynamic_cast*>(&p); double keyPairsEqual = true; if (this->world_P_body_key_pairs_.size() == @@ -257,20 +252,9 @@ class SmartProjectionPoseFactorRollingShutter 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() && - alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual + && cameraRig_.equals(e->cameraRig()) + && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } /** @@ -305,9 +289,9 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - auto body_P_cam = body_P_sensors_[i]; + auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto 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, cameraRig_[cameraIds_[i]].calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = @@ -434,9 +418,10 @@ class SmartProjectionPoseFactorRollingShutter 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& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, K_all_[i]); + cameras.emplace_back(w_P_cam, make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -474,7 +459,6 @@ class SmartProjectionPoseFactorRollingShutter 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 diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 0b94d2c3f..6c28cbda5 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -61,10 +61,13 @@ static double interp_factor1 = 0.3; static double interp_factor2 = 0.4; static double interp_factor3 = 0.5; +static size_t cameraId1 = 0; + /* ************************************************************************* */ // default Cal3_S2 poses with rolling shutter effect namespace vanillaPoseRS { typedef PinholePose Camera; +typedef CameraSet Cameras; 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); @@ -80,21 +83,23 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { + using namespace vanillaPoseRS; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorRS factor1(model, params); + SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { - using namespace vanillaPose; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ @@ -112,1030 +117,1039 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { 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); + std::vector cameraIds{0, 0, 0}; + + Cameras cameraRig; + cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + // 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); + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + factor2->add(measurements, key_pairs, interp_factors, cameraIds); // 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); + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + factor3->add(measurements, key_pairs, interp_factors, cameraIds); - { // 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); - - 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)); - 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); - - EXPECT(!factor1->equals(*factor2)); - EXPECT(!factor1->equals(*factor3)); - } +// { // create equal factors and show equal returns true +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1); +// factor1->add(measurement2, x2, x3, interp_factor2); +// factor1->add(measurement3, x3, x4, interp_factor3); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurements, key_pairs, interp_factors); +// +// 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, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// 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)); - 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); + Cameras cameraRig2; + cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); 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)); - 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); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } } -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) { - using namespace vanillaPoseRS; - - // 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); - - // 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 - - // 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); - 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); - 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)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { - // also includes non-identical extrinsic calibration - using namespace vanillaPoseRS; - - // 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; - - 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); - - 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); - - // Perform triangulation - factor.triangulateSafe(factor.cameras(values)); - TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - Point3 landmarkNoisy = *point; - - // 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); - 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); - 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)); - - // 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, optimization_3poses) { - 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); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, 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-6)); -} - -/* *************************************************************************/ -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 - 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)); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - 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_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - EXPECT(smartFactor1->triangulateSafe(cameras)); - EXPECT(!smartFactor1->isDegenerate()); - EXPECT(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - EXPECT(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(); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - 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); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { - 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); - - double excludeLandmarksFutherThanDist = 1e10; // very large - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(true); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - 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); - - 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, - optimization_3poses_landmarkDistance) { - 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); - - double excludeLandmarksFutherThanDist = 2; - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - 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); - - 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 (due to the distance threshold) 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, - optimization_3poses_dynamicOutlierRejection) { - using namespace vanillaPoseRS; - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, - measurements_lmk4; - // Project 4 landmarks into cameras - 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; - 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; - double dynamicOutlierRejectionThreshold = - 3; // max 3 pixel of average reprojection error - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - params.setEnableEPI(false); - - 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_lmk2, 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_lmk4, 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.push_back(smartFactor4); - 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 - 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, - hessianComparedToProjFactorsRollingShutter) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - - // 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); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, 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 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); -} - -/* *************************************************************************/ -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) -//| -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; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - 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); - - 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; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); - smartFactor->add(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 +//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) { +// using namespace vanillaPoseRS; +// +// // 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); +// +// // 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 +// +// // 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); +// 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); +// 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)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { +// // also includes non-identical extrinsic calibration +// using namespace vanillaPoseRS; +// +// // 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; +// +// 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); +// +// 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); +// +// // Perform triangulation +// factor.triangulateSafe(factor.cameras(values)); +// TriangulationResult point = factor.point(); +// EXPECT(point.valid()); // check triangulated point is valid +// Point3 landmarkNoisy = *point; +// +// // 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); +// 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); +// 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)); +// +// // 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, optimization_3poses) { +// 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); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, 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-6)); +//} +// +///* *************************************************************************/ +//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 +// 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)); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// 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_lmk1[1], x1, x2, interp_factor, sharedKSimple, +// body_P_sensorId); +// +// SmartFactor::Cameras cameras; +// cameras.push_back(cam1); +// cameras.push_back(cam2); +// +// // Make sure triangulation works +// EXPECT(smartFactor1->triangulateSafe(cameras)); +// EXPECT(!smartFactor1->isDegenerate()); +// EXPECT(!smartFactor1->isPointBehindCamera()); +// boost::optional p = smartFactor1->point(); +// EXPECT(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(); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// 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); +// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { +// 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); +// +// double excludeLandmarksFutherThanDist = 1e10; // very large +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(true); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// 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); +// +// 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, +// optimization_3poses_landmarkDistance) { +// 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); +// +// double excludeLandmarksFutherThanDist = 2; +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(false); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// 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); +// +// 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 (due to the distance threshold) 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, +// optimization_3poses_dynamicOutlierRejection) { +// using namespace vanillaPoseRS; +// // add fourth landmark +// Point3 landmark4(5, -0.5, 1); +// +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, +// measurements_lmk4; +// // Project 4 landmarks into cameras +// 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; +// 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; +// double dynamicOutlierRejectionThreshold = +// 3; // max 3 pixel of average reprojection error +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); +// params.setEnableEPI(false); +// +// 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_lmk2, 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_lmk4, 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.push_back(smartFactor4); +// 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 +// 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, +// hessianComparedToProjFactorsRollingShutter) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// +// // 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); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, 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 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); +//} +// +///* *************************************************************************/ +//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) +////| -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; i < nrTests; i++) { +// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// 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); +// +// 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; i < nrTests; i++) { +// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); +// smartFactor->add(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() { From 4fd6c2cb5d12cec2ff7e9b9184d2e2fe1fd78b15 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:23:16 -0400 Subject: [PATCH 038/110] bug fix - finalizing last few tests --- .../SmartProjectionPoseFactorRollingShutter.h | 3 +- ...martProjectionPoseFactorRollingShutter.cpp | 1133 ++++++++--------- 2 files changed, 567 insertions(+), 569 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index cf1526673..09d13f0e5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -178,7 +178,8 @@ class SmartProjectionPoseFactorRollingShutter if (world_P_body_key_pairs.size() != measurements.size() || world_P_body_key_pairs.size() != alphas.size() - || world_P_body_key_pairs.size() != cameraIds.size()) { + || (world_P_body_key_pairs.size() != cameraIds.size() + && cameraIds.size() != 0)) { // cameraIds.size()=0 is default throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6c28cbda5..e93c00ba8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -135,41 +135,41 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); -// { // create equal factors and show equal returns true -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); -// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); -// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create equal factors and show equal returns true (use default cameraId) -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1); -// factor1->add(measurement2, x2, x3, interp_factor2); -// factor1->add(measurement3, x3, x4, interp_factor3); -// -// EXPECT(factor1->equals(*factor2)); -// EXPECT(factor1->equals(*factor3)); -// } -// { // create equal factors and show equal returns true (use default cameraId) -// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); -// factor1->add(measurements, key_pairs, interp_factors); -// -// 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, cameraRig)); -// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); -// factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! -// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); -// -// EXPECT(!factor1->equals(*factor2)); -// EXPECT(!factor1->equals(*factor3)); -// } + { // create equal factors and show equal returns true + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create equal factors and show equal returns true (use default cameraId) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1); + factor1->add(measurement2, x2, x3, interp_factor2); + factor1->add(measurement3, x3, x4, interp_factor3); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create equal factors and show equal returns true (use default cameraId) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurements, key_pairs, interp_factors); + + EXPECT(factor1->equals(*factor2)); + EXPECT(factor1->equals(*factor3)); + } + { // create slightly different factors (different keys) and show equal + // returns false (use default cameraIds) + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); + + EXPECT(!factor1->equals(*factor2)); + EXPECT(!factor1->equals(*factor3)); + } { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; @@ -194,539 +194,536 @@ 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 -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { -// using namespace vanillaPoseRS; -// -// // 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); -// -// // 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 -// -// // 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); -// 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); -// 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)); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { -// // also includes non-identical extrinsic calibration -// using namespace vanillaPoseRS; -// -// // 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; -// -// 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); -// -// 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); -// -// // Perform triangulation -// factor.triangulateSafe(factor.cameras(values)); -// TriangulationResult point = factor.point(); -// EXPECT(point.valid()); // check triangulated point is valid -// Point3 landmarkNoisy = *point; -// -// // 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); -// 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); -// 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)); -// -// // 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, optimization_3poses) { -// 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); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, 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-6)); -//} -// -///* *************************************************************************/ -//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 -// 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)); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// double interp_factor = 0; // equivalent to measurement taken at pose 1 -// 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_lmk1[1], x1, x2, interp_factor, sharedKSimple, -// body_P_sensorId); -// -// SmartFactor::Cameras cameras; -// cameras.push_back(cam1); -// cameras.push_back(cam2); -// -// // Make sure triangulation works -// EXPECT(smartFactor1->triangulateSafe(cameras)); -// EXPECT(!smartFactor1->isDegenerate()); -// EXPECT(!smartFactor1->isPointBehindCamera()); -// boost::optional p = smartFactor1->point(); -// EXPECT(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(); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// 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); -// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -//} -// -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { -// 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); -// -// double excludeLandmarksFutherThanDist = 1e10; // very large -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(true); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// 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); -// -// 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, -// optimization_3poses_landmarkDistance) { -// 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); -// -// double excludeLandmarksFutherThanDist = 2; -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setEnableEPI(false); -// -// SmartFactorRS smartFactor1(model, params); -// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor2(model, params); -// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); -// -// SmartFactorRS smartFactor3(model, params); -// 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); -// -// 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 (due to the distance threshold) 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, -// optimization_3poses_dynamicOutlierRejection) { -// using namespace vanillaPoseRS; -// // add fourth landmark -// Point3 landmark4(5, -0.5, 1); -// -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, -// measurements_lmk4; -// // Project 4 landmarks into cameras -// 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; -// 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; -// double dynamicOutlierRejectionThreshold = -// 3; // max 3 pixel of average reprojection error -// -// SmartProjectionParams params; -// params.setRankTolerance(1.0); -// params.setLinearizationMode(gtsam::HESSIAN); -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); -// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); -// params.setEnableEPI(false); -// -// 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_lmk2, 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_lmk4, 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.push_back(smartFactor4); -// 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 -// 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)); -//} -// +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) { + using namespace vanillaPoseRS; + + // 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, Camera(body_P_sensorId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + 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); + + // 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 + + // 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); + 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); + 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)); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { + // also includes non-identical extrinsic calibration + using namespace vanillaPoseRS; + + // 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; + + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + factor.add(level_uv, x1, x2, interp_factor1); + factor.add(level_uv_right, x2, x3, interp_factor2); + + 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); + + // Perform triangulation + factor.triangulateSafe(factor.cameras(values)); + TriangulationResult point = factor.point(); + EXPECT(point.valid()); // check triangulated point is valid + Point3 landmarkNoisy = *point; + + // 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); + 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); + 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)); + + // 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, optimization_3poses) { + 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); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + 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-6)); +} + +/* *************************************************************************/ +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 + 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)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); + + SmartFactor::Cameras cameras; + cameras.push_back(cam1); + cameras.push_back(cam2); + + // Make sure triangulation works + EXPECT(smartFactor1->triangulateSafe(cameras)); + EXPECT(!smartFactor1->isDegenerate()); + EXPECT(!smartFactor1->isPointBehindCamera()); + boost::optional p = smartFactor1->point(); + EXPECT(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(); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + 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); + EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +} + +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { + 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); + + double excludeLandmarksFutherThanDist = 1e10; // very large + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(true); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); + + 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, + optimization_3poses_landmarkDistance) { + 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); + + double excludeLandmarksFutherThanDist = 2; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); + + 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); + + // 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(); + 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_lmk1, measurements_lmk2, measurements_lmk3, + measurements_lmk4; + // Project 4 landmarks into cameras + 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; + 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; + double dynamicOutlierRejectionThreshold = + 3; // max 3 pixel of average reprojection error + + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + params.setEnableEPI(false); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); + + 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); + + 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 + 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, // hessianComparedToProjFactorsRollingShutter) { From 0797eae9a8d55db35f3cbe32702bea2b7178618f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:32:02 -0400 Subject: [PATCH 039/110] all tests are passing! --- ...martProjectionPoseFactorRollingShutter.cpp | 842 +++++++++--------- 1 file changed, 419 insertions(+), 423 deletions(-) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index e93c00ba8..34e2678c3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -724,429 +724,425 @@ TEST(SmartProjectionPoseFactorRollingShutter, EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } -///* *************************************************************************/ -//TEST(SmartProjectionPoseFactorRollingShutter, -// hessianComparedToProjFactorsRollingShutter) { -// using namespace vanillaPoseRS; -// Point2Vector measurements_lmk1; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// -// // 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); -// -// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); -// smartFactor1->add(measurements_lmk1, 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 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); -//} -// -///* *************************************************************************/ -//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) -////| -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; i < nrTests; i++) { -// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); -// double interp_factor = 0; // equivalent to measurement taken at pose 1 -// 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); -// -// 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; i < nrTests; i++) { -// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); -// smartFactor->add(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 +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, + hessianComparedToProjFactorsRollingShutter) { + using namespace vanillaPoseRS; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // 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); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); + + 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 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); +} + +/* *************************************************************************/ +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, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); + + 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, Camera(Pose3::identity(),sharedK))); + smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, + interp_factors_redundant); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); + + 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.21 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 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 = 10000; + + for (size_t i = 0; i < nrTests; i++) { + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + double interp_factor = 0; // equivalent to measurement taken at pose 1 + smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); + interp_factor = 1; // equivalent to measurement taken at pose 2 + smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor); + + 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; i < nrTests; i++) { + SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + smartFactor->add(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() { From 823a7e7be09bcf160c0cf4f469493a6f91b27654 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 22:53:36 -0400 Subject: [PATCH 040/110] done with tests --- ...martProjectionPoseFactorRollingShutter.cpp | 72 +++++++++++++++++++ 1 file changed, 72 insertions(+) diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 34e2678c3..c4c670de3 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -424,6 +424,78 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { + 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); + + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensor, sharedK)); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1}); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1}); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1}); + + 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); // pose above is the pose of the camera + 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-6)); +} + /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // here we replicate a test in SmartProjectionPoseFactor by setting From f0745a9c286c4104647302779ac4442ded799703 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 23:10:45 -0400 Subject: [PATCH 041/110] now I only need to fix comments in rolling shutter factor --- gtsam/slam/SmartProjectionRigFactor.h | 35 ++++++++++++++---- .../tests/testSmartProjectionRigFactor.cpp | 36 +++++++++++-------- .../SmartProjectionPoseFactorRollingShutter.h | 5 +++ 3 files changed, 56 insertions(+), 20 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 0246d6327..3cae1ea64 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -94,6 +94,23 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Base::params_.linearizationMode = gtsam::HESSIAN; } + /** + * Constructor + * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param camera single camera (fixed poses wrt body and intrinsics) + * @param params parameters for the smart projection factors + */ + SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, + const Camera& camera, + const SmartProjectionParams& params = + SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionRigFactor() override { } @@ -104,9 +121,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param cameraId ID of the camera in the rig taking the measurement + * @param cameraId ID of the camera in the rig taking the measurement (default 0) */ - void add(const Point2& measured, const Key& poseKey, const size_t cameraId) { + void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); @@ -127,13 +144,19 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, - const FastVector& cameraIds) { - if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){ + const FastVector& cameraIds = FastVector()) { + if (poseKeys.size() != measurements.size() + || (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { throw std::runtime_error("SmartProjectionRigFactor: " - "trying to add inconsistent inputs"); + "trying to add inconsistent inputs"); + } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionRigFactor: " + "camera rig includes multiple camera but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], cameraIds[i]); + add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); } } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index fb3f3c461..a2a30e89a 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -92,6 +92,15 @@ TEST( SmartProjectionRigFactor, Constructor4) { factor1.add(measurement1, x1, cameraId1); } +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, Constructor5) { + using namespace vanillaPose; + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + factor1.add(measurement1, x1, cameraId1); +} + /* ************************************************************************* */ TEST( SmartProjectionRigFactor, Equals ) { using namespace vanillaPose; @@ -105,6 +114,11 @@ TEST( SmartProjectionRigFactor, Equals ) { factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); + + SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default + + CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ @@ -112,16 +126,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, cameraRig); - factor.add(level_uv, x1, cameraId1); // both taken from the same camera - factor.add(level_uv_right, x2, cameraId1); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); + factor.add(level_uv, x1); // both taken from the same camera + factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses values.insert(x1, cam1.pose()); @@ -245,13 +256,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views, cameraIds); + smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); - smartFactor2.add(measurements_cam2, views, cameraIds); + smartFactor2.add(measurements_cam2, views); SmartRigFactor smartFactor3(model, cameraRig, params); - smartFactor3.add(measurements_cam3, views, cameraIds); + smartFactor3.add(measurements_cam3, views); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -459,15 +470,12 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model,cameraRig); - smartFactor1->add(measurements_cam1, views, cameraIds); + > (model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 09d13f0e5..f3f8900f5 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -183,6 +183,11 @@ class SmartProjectionPoseFactorRollingShutter throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } + if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "camera rig includes multiple camera but add did not input cameraIds"); + } 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, alphas[i], From 737dcf65e4c1488b1e605faf081fc94310476d5c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 5 Oct 2021 23:29:20 -0400 Subject: [PATCH 042/110] all set here! --- .../SmartProjectionPoseFactorRollingShutter.h | 29 ++----- ...martProjectionPoseFactorRollingShutter.cpp | 85 +++++++++++++++++++ 2 files changed, 94 insertions(+), 20 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f3f8900f5..84e3437a7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -49,9 +49,6 @@ class SmartProjectionPoseFactorRollingShutter typedef typename CAMERA::CalibrationType CALIBRATION; 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_; @@ -60,10 +57,10 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement + /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement FastVector cameraIds_; public: @@ -87,6 +84,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -102,6 +100,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param camera single camera (fixed poses wrt body and intrinsics) * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -114,13 +113,11 @@ class SmartProjectionPoseFactorRollingShutter cameraRig_.push_back(camera); } - /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with 2 pose keys, interpolation factor, camera - * (intrinsic and extrinsic) calibration, and observed pixel. + * add a new measurement, with 2 pose keys, interpolation factor, and cameraId * @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 <= @@ -129,8 +126,7 @@ class SmartProjectionPoseFactorRollingShutter * >= time pixel is acquired) * @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 + * @param cameraId ID of the camera taking the measurement (default 0) */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, @@ -168,8 +164,7 @@ class SmartProjectionPoseFactorRollingShutter * for the i0-th measurement can be interpolated * @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 + * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, @@ -223,7 +218,7 @@ class SmartProjectionPoseFactorRollingShutter 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++) { + for (size_t i = 0; i < cameraIds_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; @@ -409,14 +404,8 @@ class SmartProjectionPoseFactorRollingShutter * @return Cameras */ typename Base::Cameras cameras(const Values& values) const override { - size_t numViews = this->measured_.size(); - assert(numViews == K_all_.size()); - assert(numViews == alphas_.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 + for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c4c670de3..6a3e31dd7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -496,6 +496,91 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { + using namespace vanillaPoseRS; + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), + Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3::identity(); + + Camera camera1(interp_pose1*body_T_sensor1, sharedK); + Camera camera2(interp_pose2*body_T_sensor2, sharedK); + Camera camera3(interp_pose3*body_T_sensor3, sharedK); + + // Project three landmarks into three cameras + projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, 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); + + Cameras cameraRig; + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + + 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); // pose above is the pose of the camera + 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-4)); +} + /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // here we replicate a test in SmartProjectionPoseFactor by setting From f97e55b85beaf9bd9a6cac6bb6ac28ba6c514316 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 21:05:18 -0400 Subject: [PATCH 043/110] update proxy functions to use the Adjoint and AdjointTranpose methods --- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index fe527ab2e..94e2e558b 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -159,7 +159,7 @@ TEST(Pose3, Adjoint_jacobians) // Check jacobians Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function Adjoint_proxy = - [&](const Pose3& T, const Vector6& xi) { return T.AdjointMap() * xi; }; + [&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); }; T.Adjoint(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi); @@ -198,7 +198,7 @@ TEST(Pose3, AdjointTranspose) Matrix6 actualH1, actualH2, expectedH1, expectedH2; std::function AdjointTranspose_proxy = [&](const Pose3& T, const Vector6& xi) { - return T.AdjointMap().transpose() * xi; + return T.AdjointTranspose(xi); }; T.AdjointTranspose(xi, actualH1, actualH2); From 2de623befccc6cce0b064737053e3ca4d9a72389 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 21:50:07 -0400 Subject: [PATCH 044/110] add docs explaining why pRw_H_t is the same as Rw_H_R --- gtsam/geometry/Pose3.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 27a5cf557..484fb9ca9 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -79,13 +79,19 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - const Vector3 pRw = cross(t_, Rw, H_this ? &pRw_H_t : nullptr, - (H_this || H_xib) ? &pRw_H_Rw : nullptr); + // Since we use the Point3 version of cross, the jacobian of pRw wrpt t + // (pRw_H_t) needs special treatment as detailed below. + const Vector3 pRw = + cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { - pRw_H_t = Rw_H_R; // This is needed to pass the unit tests for some reason + // By applying the chain rule to the matrix-matrix product of [t]R, we can + // compute a simplified derivative which is the same as Rw_H_R. Details: + // https://github.com/borglab/gtsam/pull/885#discussion_r726591370 + pRw_H_t = Rw_H_R; + *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); From 561037f073f075a99fbf60c8e666e3278e20f423 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Oct 2021 23:22:37 -0400 Subject: [PATCH 045/110] make threshold more lenient --- gtsam/geometry/tests/testPose3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 94e2e558b..f0f2c0ccd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,7 +204,7 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); @@ -216,7 +216,7 @@ TEST(Pose3, AdjointTranspose) T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); } From 859c5f8d07abcd5f933bbb540b6281f3f30d04bb Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 00:13:05 -0400 Subject: [PATCH 046/110] added new Python examples using iSAM2 --- python/gtsam/examples/Pose2ISAM2Example.py | 153 +++++++++++++++++++ python/gtsam/examples/Pose3ISAM2Example.py | 165 +++++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 python/gtsam/examples/Pose2ISAM2Example.py create mode 100644 python/gtsam/examples/Pose3ISAM2Example.py diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py new file mode 100644 index 000000000..ba2fbb283 --- /dev/null +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -0,0 +1,153 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Pose SLAM example using iSAM2 in the 2D plane. +Author: Jerred Chen, Yusuf Ali +Modelled after: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) +""" + +from __future__ import print_function +import math +import numpy as np +from numpy.random import multivariate_normal as mult_gauss +import gtsam +import matplotlib.pyplot as plt +import gtsam.utils.plot as gtsam_plot + +def Pose2SLAM_ISAM2_plot(graph, current_estimate): + """ + Plots incremental state of the robot for 2D Pose SLAM using iSAM2 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + fig = plt.figure(0) + axes = fig.gca() + plt.cla() + + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose2(0, current_estimate.atPose2(i), 0.5, marginals.marginalCovariance(i)) + i += 1 + + plt.axis('equal') + axes.set_xlim(-1, 5) + axes.set_ylim(-1, 3) + plt.pause(1) + return marginals + + +def Pose2SLAM_ISAM2_example(): + plt.ion() + + def vector3(x, y, z): + """Create 3d double numpy array.""" + return np.array([x, y, z], dtype=float) + + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + + # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + # The ground truth odometry measurements (without noise) of the robot during the trajectory. + odom_arr = [(2, 0, 0), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2)] + + # The initial estimates for robot poses 2-5. Pose 1 is initialized by the prior. + # To demonstrate iSAM2 incremental optimization, poor initializations were intentionally inserted. + pose_est = [gtsam.Pose2(2.3, 0.1, -0.2), + gtsam.Pose2(4.1, 0.1, math.pi/2), + gtsam.Pose2(4.0, 2.0, math.pi), + gtsam.Pose2(2.1, 2.1, -math.pi/2), + gtsam.Pose2(1.9, -0.2, 0.2)] + + graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) + initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) + + def determine_loop_closure(odom, current_estimate, xy_tol=0.6, theta_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) + measurement in the body frame. + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xy_tol: (double) Optional argument for the x-y measurement tolerance. + theta_tol: (double) Optional argument for the theta measurement tolerance. + ### Returns: + k: (int) The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose2(i+1) + rotated_odom = prev_est.rotation().matrix() @ odom[:2] + curr_xy = np.array([prev_est.x() + rotated_odom[0], + prev_est.y() + rotated_odom[1]]) + curr_theta = prev_est.theta() + odom[2] + for k in range(1, i+1): + pose_xy = np.array([current_estimate.atPose2(k).x(), + current_estimate.atPose2(k).y()]) + pose_theta = current_estimate.atPose2(k).theta() + if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ + (abs(pose_theta - curr_theta) <= theta_tol): + return k + + current_estimate = None + for i in range(len(odom_arr)): + + odom_x, odom_y, odom_theta = odom_arr[i] + # Odometry measurement that is received by the robot and corrupted by gaussian noise + odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state + loop = determine_loop_closure(odom, current_estimate) + if loop: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + initial_estimate.insert(i + 2, pose_est[i]) + # Incremental update to iSAM2's internal Baye's tree, which only optimizes upon the added variables + # as well as any affected variables + isam.update(graph, initial_estimate) + # Another iSAM2 update can be performed for additional optimization + isam.update() + current_estimate = isam.calculateEstimate() + print("*"*50) + print(f"Inference after State {i+1}:") + print(current_estimate) + marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) + initial_estimate.clear() + + for i in range(1, len(odom_arr)+1): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") + + plt.ioff() + plt.show() + + +if __name__ == "__main__": + Pose2SLAM_ISAM2_example() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py new file mode 100644 index 000000000..26d82ae33 --- /dev/null +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -0,0 +1,165 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Pose SLAM example using iSAM2 in 3D space. +Author: Jerred Chen +Modelled after version by: + - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) +""" +from __future__ import print_function +import gtsam +from gtsam import Pose3, Rot3 +from gtsam.symbol_shorthand import X +import gtsam.utils.plot as gtsam_plot +import numpy as np +from numpy import cos, sin, pi +from numpy.random import multivariate_normal as mult_gauss +import matplotlib.pyplot as plt + + +def Pose3SLAM_ISAM2_plot(graph, current_estimate): + """ + Plots incremental state of the robot for 3D Pose SLAM using iSAM2 + Author: Jerred Chen + Based on version by: + - Ellon Paiva (Python), + - Duy Nguyen Ta and Frank Dellaert (MATLAB) + """ + marginals = gtsam.Marginals(graph, current_estimate) + + fig = plt.figure(0) + axes = fig.gca(projection='3d') + plt.cla() + + i = 0 + while current_estimate.exists(X(i)): + gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), 10, + marginals.marginalCovariance(X(i))) + i += 1 + + axes.set_xlim3d(-30, 45) + axes.set_ylim3d(-30, 45) + axes.set_zlim3d(-30, 45) + plt.pause(1) + + return marginals + + +def createPoses(): + """ + Creates ground truth poses of the robot. + """ + P0 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + P1 = np.array([[0, -1, 0, 15], + [1, 0, 0, 15], + [0, 0, 1, 20], + [0, 0, 0, 1]]) + P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30], + [0, 1, 0, 30], + [-sin(pi/4), 0, cos(pi/4), 30], + [0, 0, 0, 1]]) + P3 = np.array([[0, 1, 0, 30], + [0, 0, -1, 0], + [-1, 0, 0, -15], + [0, 0, 0, 1]]) + P4 = np.array([[-1, 0, 0, 0], + [0, -1, 0, -10], + [0, 0, 1, -10], + [0, 0, 0, 1]]) + P5 = P0[:] + + return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] + +def Pose3_ISAM2_example(): + """ + """ + plt.ion() + + def vector6(x, y, z, a, b, c): + """Create 6d double numpy array.""" + return np.array([x, y, z, a, b, c], dtype=float) + + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + + poses = createPoses() + + parameters = gtsam.ISAM2Params() + parameters.setRelinearizeThreshold(0.1) + parameters.setRelinearizeSkip(1) + isam = gtsam.ISAM2(parameters) + + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() + # Add prior factor to the first pose + graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) + initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + + def determine_loop_closure(odom, current_estimate, xyz_tol=0.6, rot_tol=0.3): + """ + Simple brute force approach which iterates through previous states + and checks for loop closure. + ### Parameters: + odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation + measurement in the body frame, [roll, pitch, yaw, x, y, z] + current_estimate: (gtsam.Values) The current estimates computed by iSAM2. + xyz_tol: (double) Optional argument for the translational tolerance. + rot_tol: (double) Optional argument for the rotational tolerance. + ### Returns: + k: (int) The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + rot = Rot3.RzRyRx(odom[:3]) + odom_tf = Pose3(rot, odom[3:6].reshape(-1,1)) + prev_est = current_estimate.atPose3(X(i-1)) + curr_est = prev_est.transformPoseFrom(odom_tf) + for k in range(i): + pose = current_estimate.atPose3(X(k)) + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): + return k + + current_estimate = None + for i in range(1, len(poses)): + # The odometry "ground truth" measurement between poses + odom_tf = poses[i-1].transformPoseTo(poses[i]) + odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() + odom_rpy = odom_tf.rotation().rpy() + # Odometry measurement that is received by the robot and corrupted by gaussian noise + measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) + loop = determine_loop_closure(measurement, current_estimate) + if loop is not None: + graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + else: + graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(i), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + # Intentionally insert poor initializations + initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( + gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + isam.update(graph, initial_estimate) + isam.update() + current_estimate = isam.calculateEstimate() + print("*"*50) + print(f"Inference after State {i}:") + print(current_estimate) + marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) + initial_estimate.clear() + i = 0 + while current_estimate.exists(X(i)): + print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") + i += 1 + + plt.ioff() + plt.show() + +if __name__ == '__main__': + Pose3_ISAM2_example() From 47c45c633f607790940859589292c03d35f4ceb4 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 13 Oct 2021 11:03:40 -0400 Subject: [PATCH 047/110] Added minor comments for documentation --- python/gtsam/examples/Pose2ISAM2Example.py | 7 +++++-- python/gtsam/examples/Pose3ISAM2Example.py | 22 +++++++++++++++++++--- 2 files changed, 24 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index ba2fbb283..6710dc706 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -48,6 +48,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): + """ + """ plt.ion() def vector3(x, y, z): @@ -93,6 +95,7 @@ def Pose2SLAM_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred ### Parameters: odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) measurement in the body frame. @@ -119,7 +122,7 @@ def Pose2SLAM_ISAM2_example(): current_estimate = None for i in range(len(odom_arr)): - + # The "ground truth" change between poses odom_x, odom_y, odom_theta = odom_arr[i] # Odometry measurement that is received by the robot and corrupted by gaussian noise odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) @@ -141,7 +144,7 @@ def Pose2SLAM_ISAM2_example(): print(current_estimate) marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() - + # Print the final covariance matrix for each pose after completing inference for i in range(1, len(odom_arr)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 26d82ae33..a15424d04 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -10,6 +10,7 @@ Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen Modelled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) + - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ from __future__ import print_function import gtsam @@ -80,6 +81,8 @@ def createPoses(): def Pose3_ISAM2_example(): """ + Perform 3D SLAM given ground truth poses as well as simple + loop closure detection. """ plt.ion() @@ -87,19 +90,27 @@ def Pose3_ISAM2_example(): """Create 6d double numpy array.""" return np.array([x, y, z, a, b, c], dtype=float) + # Although this example only uses linear measurements and Gaussian noise models, it is important + # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example + # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + # Create the ground truth poses of the robot trajectory. poses = createPoses() + # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() - # Add prior factor to the first pose + initial_estimate = gtsam.Values() + + # Add prior factor to the first pose with intentionally poor initial estimate graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) @@ -108,6 +119,7 @@ def Pose3_ISAM2_example(): """ Simple brute force approach which iterates through previous states and checks for loop closure. + Author: Jerred Chen ### Parameters: odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation measurement in the body frame, [roll, pitch, yaw, x, y, z] @@ -131,13 +143,14 @@ def Pose3_ISAM2_example(): current_estimate = None for i in range(1, len(poses)): - # The odometry "ground truth" measurement between poses + # The "ground truth" change between poses odom_tf = poses[i-1].transformPoseTo(poses[i]) odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() odom_rpy = odom_tf.rotation().rpy() # Odometry measurement that is received by the robot and corrupted by gaussian noise measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) loop = determine_loop_closure(measurement, current_estimate) + # If loop closure is detected, then adds a constraint between existing states in the factor graph if loop is not None: graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) else: @@ -145,7 +158,9 @@ def Pose3_ISAM2_example(): # Intentionally insert poor initializations initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) + # Performs iSAM2 incremental update based on newly added factors isam.update(graph, initial_estimate) + # Additional iSAM2 optimization isam.update() current_estimate = isam.calculateEstimate() print("*"*50) @@ -153,6 +168,7 @@ def Pose3_ISAM2_example(): print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() + # Print the final covariance matrix for each pose after completing inference i = 0 while current_estimate.exists(X(i)): print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") From 00c541aca660268e79834b8ab90f3782896482be Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 14 Oct 2021 13:42:21 -0400 Subject: [PATCH 048/110] adjusted docstrings to match google style guide --- python/gtsam/examples/Pose2ISAM2Example.py | 33 ++++++++--------- python/gtsam/examples/Pose3ISAM2Example.py | 43 ++++++++++------------ 2 files changed, 35 insertions(+), 41 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 6710dc706..5336bc34e 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -22,9 +22,8 @@ import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot def Pose2SLAM_ISAM2_plot(graph, current_estimate): - """ - Plots incremental state of the robot for 2D Pose SLAM using iSAM2 - Author: Jerred Chen + """Plots incremental state of the robot for 2D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -48,7 +47,8 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): def Pose2SLAM_ISAM2_example(): - """ + """Perform 2D SLAM given the ground truth changes in pose as well as + simple loop closure detection. """ plt.ion() @@ -91,20 +91,19 @@ def Pose2SLAM_ISAM2_example(): graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - def determine_loop_closure(odom, current_estimate, xy_tol=0.6, theta_tol=0.3): - """ - Simple brute force approach which iterates through previous states + def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + xy_tol=0.6, theta_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states and checks for loop closure. - Author: Jerred - ### Parameters: - odom: (numpy.ndarray) 1x3 vector representing noisy odometry (x, y, theta) - measurement in the body frame. - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xy_tol: (double) Optional argument for the x-y measurement tolerance. - theta_tol: (double) Optional argument for the theta measurement tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + Args: + odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + xy_tol: Optional argument for the x-y measurement tolerance. + theta_tol: Optional argument for the theta measurement tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. """ if current_estimate: prev_est = current_estimate.atPose2(i+1) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index a15424d04..3fcdcd7ec 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -24,9 +24,8 @@ import matplotlib.pyplot as plt def Pose3SLAM_ISAM2_plot(graph, current_estimate): - """ - Plots incremental state of the robot for 3D Pose SLAM using iSAM2 - Author: Jerred Chen + """Plots incremental state of the robot for 3D Pose SLAM using iSAM2 + Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) @@ -52,9 +51,7 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate): def createPoses(): - """ - Creates ground truth poses of the robot. - """ + """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], @@ -80,10 +77,8 @@ def createPoses(): return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] def Pose3_ISAM2_example(): - """ - Perform 3D SLAM given ground truth poses as well as simple - loop closure detection. - """ + """Perform 3D SLAM given ground truth poses as well as simple + loop closure detection.""" plt.ion() def vector6(x, y, z, a, b, c): @@ -115,20 +110,20 @@ def Pose3_ISAM2_example(): initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - def determine_loop_closure(odom, current_estimate, xyz_tol=0.6, rot_tol=0.3): - """ - Simple brute force approach which iterates through previous states + def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + xyz_tol=0.6, rot_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states and checks for loop closure. - Author: Jerred Chen - ### Parameters: - odom: (numpy.ndarray) 1x6 vector representing noisy odometry transformation - measurement in the body frame, [roll, pitch, yaw, x, y, z] - current_estimate: (gtsam.Values) The current estimates computed by iSAM2. - xyz_tol: (double) Optional argument for the translational tolerance. - rot_tol: (double) Optional argument for the rotational tolerance. - ### Returns: - k: (int) The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. + + Args: + odom: Vector representing noisy odometry transformation measurement in the body frame, + where the vector represents [roll, pitch, yaw, x, y, z]. + current_estimate: The current estimates computed by iSAM2. + xyz_tol: Optional argument for the translational tolerance. + rot_tol: Optional argument for the rotational tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. """ if current_estimate: rot = Rot3.RzRyRx(odom[:3]) @@ -164,7 +159,7 @@ def Pose3_ISAM2_example(): isam.update() current_estimate = isam.calculateEstimate() print("*"*50) - print(f"Inference after State {i}:") + print(f"Inference after State {i}:\n") print(current_estimate) marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) initial_estimate.clear() From aac05f238996e952eefee5655e5eab6709ad7f28 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Fri, 15 Oct 2021 14:46:17 -0400 Subject: [PATCH 049/110] Revert "Revert "replace deprecated tbb functionality"" This reverts commit f819b1a03f74f289f50b96125610026618601a2f. --- gtsam/base/treeTraversal-inst.h | 5 +- .../treeTraversal/parallelTraversalTasks.h | 70 +++++++------------ 2 files changed, 29 insertions(+), 46 deletions(-) diff --git a/gtsam/base/treeTraversal-inst.h b/gtsam/base/treeTraversal-inst.h index 7a88f72eb..30cec3b9a 100644 --- a/gtsam/base/treeTraversal-inst.h +++ b/gtsam/base/treeTraversal-inst.h @@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData, // Typedefs typedef typename FOREST::Node Node; - tbb::task::spawn_root_and_wait( - internal::CreateRootTask(forest.roots(), rootData, visitorPre, - visitorPost, problemSizeThreshold)); + internal::CreateRootTask(forest.roots(), rootData, visitorPre, + visitorPost, problemSizeThreshold); #else DepthFirstForest(forest, rootData, visitorPre, visitorPost); #endif diff --git a/gtsam/base/treeTraversal/parallelTraversalTasks.h b/gtsam/base/treeTraversal/parallelTraversalTasks.h index 87d5b0d4c..dc1b45906 100644 --- a/gtsam/base/treeTraversal/parallelTraversalTasks.h +++ b/gtsam/base/treeTraversal/parallelTraversalTasks.h @@ -22,7 +22,7 @@ #include #ifdef GTSAM_USE_TBB -#include // tbb::task, tbb::task_list +#include // tbb::task_group #include // tbb::scalable_allocator namespace gtsam { @@ -34,7 +34,7 @@ namespace gtsam { /* ************************************************************************* */ template - class PreOrderTask : public tbb::task + class PreOrderTask { public: const boost::shared_ptr& treeNode; @@ -42,28 +42,30 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; bool makeNewTasks; - bool isPostOrderPhase; + // Keep track of order phase across multiple calls to the same functor + mutable bool isPostOrderPhase; PreOrderTask(const boost::shared_ptr& treeNode, const boost::shared_ptr& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, - bool makeNewTasks = true) + tbb::task_group& tg, bool makeNewTasks = true) : treeNode(treeNode), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), problemSizeThreshold(problemSizeThreshold), + tg(tg), makeNewTasks(makeNewTasks), isPostOrderPhase(false) {} - tbb::task* execute() override + void operator()() const { if(isPostOrderPhase) { // Run the post-order visitor since this task was recycled to run the post-order visitor (void) visitorPost(treeNode, *myData); - return nullptr; } else { @@ -71,14 +73,10 @@ namespace gtsam { { if(!treeNode->children.empty()) { - // Allocate post-order task as a continuation - isPostOrderPhase = true; - recycle_as_continuation(); - bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); - tbb::task* firstChild = 0; - tbb::task_list childTasks; + // If we have child tasks, start subtasks and wait for them to complete + tbb::task_group ctg; for(const boost::shared_ptr& child: treeNode->children) { // Process child in a subtask. Important: Run visitorPre before calling @@ -86,37 +84,30 @@ namespace gtsam { // allocated an extra child, this causes a TBB error. boost::shared_ptr childData = boost::allocate_shared( tbb::scalable_allocator(), visitorPre(child, *myData)); - tbb::task* childTask = - new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, - problemSizeThreshold, overThreshold); - if (firstChild) - childTasks.push_back(*childTask); - else - firstChild = childTask; + ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost, + problemSizeThreshold, ctg, overThreshold)); } + ctg.wait(); - // If we have child tasks, start subtasks and wait for them to complete - set_ref_count((int)treeNode->children.size()); - spawn(childTasks); - return firstChild; + // Allocate post-order task as a continuation + isPostOrderPhase = true; + tg.run(*this); } else { // Run the post-order visitor in this task if we have no children (void) visitorPost(treeNode, *myData); - return nullptr; } } else { // Process this node and its children in this task processNodeRecursively(treeNode, *myData); - return nullptr; } } } - void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) + void processNodeRecursively(const boost::shared_ptr& node, DATA& myData) const { for(const boost::shared_ptr& child: node->children) { @@ -131,7 +122,7 @@ namespace gtsam { /* ************************************************************************* */ template - class RootTask : public tbb::task + class RootTask { public: const ROOTS& roots; @@ -139,38 +130,31 @@ namespace gtsam { VISITOR_PRE& visitorPre; VISITOR_POST& visitorPost; int problemSizeThreshold; + tbb::task_group& tg; RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, - int problemSizeThreshold) : + int problemSizeThreshold, tbb::task_group& tg) : roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), - problemSizeThreshold(problemSizeThreshold) {} + problemSizeThreshold(problemSizeThreshold), tg(tg) {} - tbb::task* execute() override + void operator()() const { typedef PreOrderTask PreOrderTask; // Create data and tasks for our children - tbb::task_list tasks; for(const boost::shared_ptr& root: roots) { boost::shared_ptr rootData = boost::allocate_shared(tbb::scalable_allocator(), visitorPre(root, myData)); - tasks.push_back(*new(allocate_child()) - PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold)); + tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); } - // Set TBB ref count - set_ref_count(1 + (int) roots.size()); - // Spawn tasks - spawn_and_wait_for_all(tasks); - // Return nullptr - return nullptr; } }; template - RootTask& - CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) + void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold) { typedef RootTask RootTask; - return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); - } + tbb::task_group tg; + tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg)); + } } From 33e16aa7d2ba47239ec59439da997238d642b8bd Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 01:01:02 -0400 Subject: [PATCH 050/110] correct jacobians --- gtsam/geometry/Pose3.cpp | 21 ++++++++++++--------- 1 file changed, 12 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 484fb9ca9..5a3b20782 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -79,22 +79,23 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - // Since we use the Point3 version of cross, the jacobian of pRw wrpt t - // (pRw_H_t) needs special treatment as detailed below. const Vector3 pRw = - cross(t_, Rw, boost::none, (H_this || H_xib) ? &pRw_H_Rw : nullptr); + cross(t_, Rw, pRw_H_t, (H_this || H_xib) ? &pRw_H_Rw : nullptr); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { - // By applying the chain rule to the matrix-matrix product of [t]R, we can - // compute a simplified derivative which is the same as Rw_H_R. Details: - // https://github.com/borglab/gtsam/pull/885#discussion_r726591370 - pRw_H_t = Rw_H_R; - *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) .finished(); + /* This is the "full" calculation: + Matrix36 R_H_this, t_H_this; + rotation(R_H_this); // I_3x3, Z_3x3 + translation(t_H_this); // Z_3x3, R + (*H_this) *= (Matrix6() << R_H_this, t_H_this).finished(); + */ + // But we can simplify those calculations since it's mostly I and Z: + H_this->bottomRightCorner<3, 3>() *= R_.matrix(); } if (H_xib) { *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // @@ -130,9 +131,11 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, // Jacobians if (H_this) { - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rv_H_R, // -Rtv_H_tv * tv_H_t + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, -Rtv_H_tv * tv_H_t, /* */ Rv_H_R, /* */ Z_3x3) .finished(); + // See Adjoint(xi) jacobian calculation for why we multiply by R + H_this->topRightCorner<3, 3>() *= R_.matrix(); } if (H_x) { *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // From 908ba707062fdbf698101ab1d4a29f84697ddf39 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:34:13 -0400 Subject: [PATCH 051/110] documentation --- doc/math.lyx | 179 +++++++++++++++++++++++++++++++++++++++++++++++++++ doc/math.pdf | Bin 261727 -> 264527 bytes 2 files changed, 179 insertions(+) diff --git a/doc/math.lyx b/doc/math.lyx index 2533822a7..f14ebc66f 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5084,6 +5084,185 @@ reference "ex:projection" \end_layout +\begin_layout Subsection +Derivative of Adjoint +\end_layout + +\begin_layout Standard +Consider +\begin_inset Formula $f:SE(3)\rightarrow\mathbb{R}^{6}$ +\end_inset + + is defined as +\begin_inset Formula $f(T)=Ad_{T}y$ +\end_inset + + for some constant +\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}$ +\end_inset + +. + Defining +\begin_inset Formula $\xi=\begin{bmatrix}\omega\\ +v +\end{bmatrix}$ +\end_inset + + for the derivative notation (w.r.t. + pose +\begin_inset Formula $T$ +\end_inset + +): +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix} +\] + +\end_inset + +Then we can compute +\begin_inset Formula $f'(T)$ +\end_inset + + by considering the rotation and translation separately. + To reduce confusion with +\begin_inset Formula $\omega_{y},v_{y}$ +\end_inset + +, we will use +\begin_inset Formula $R,t$ +\end_inset + + to denote the rotation and translation of +\begin_inset Formula $T\exp\xi$ +\end_inset + +. +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial\omega}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}=\frac{\partial}{\partial\omega}\begin{bmatrix}R\omega_{y}\\{} +[t]_{\times}R\omega_{y}+Rv_{y} +\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times}\\ +-[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial t}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega\\ +v +\end{bmatrix}=\frac{\partial}{\partial t}\begin{bmatrix}R\omega_{y}\\{} +[t]_{\times}R\omega_{y}+Rv_{y} +\end{bmatrix}=\begin{bmatrix}0\\ +-[R\omega_{y}] +\end{bmatrix} +\] + +\end_inset + +Applying chain rule for the translation, +\begin_inset Formula $\frac{\partial}{\partial v}=\frac{\partial}{\partial t}\frac{\partial t}{\partial v}$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{} +[t]_{\times}R & R +\end{bmatrix}\begin{bmatrix}\omega_{y}\\ +v_{y} +\end{bmatrix}=\begin{bmatrix}0\\ +-[R\omega_{y}]_{\times} +\end{bmatrix}R=\begin{bmatrix}0\\ +-[R\omega_{y}]_{\times}R +\end{bmatrix}=\begin{bmatrix}0\\ +-R[\omega_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + +The simplification +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $[R\omega_{y}]_{\times}R=R[\omega_{y}]_{\times}$ +\end_inset + + can be derived by considering the result for when +\begin_inset Formula $\omega_{y}$ +\end_inset + + is each of the 3 standard basis vectors +\begin_inset Formula $\hat{e}_{i}$ +\end_inset + +: +\begin_inset Formula $-[R\hat{e}_{i}]_{\times}R$ +\end_inset + +. +\end_layout + +\begin_layout Standard +Now putting together the rotation and translation: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times} & 0\\ +-[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times} +\end{bmatrix} +\] + +\end_inset + + +\end_layout + +\begin_layout Standard +We can apply a similar procedure to compute the derivative of +\begin_inset Formula $Ad_{T}^{T}y$ +\end_inset + +. +\end_layout + \begin_layout Subsection Instantaneous Velocity \end_layout diff --git a/doc/math.pdf b/doc/math.pdf index 8dc7270f1139ce7e246a63925d29302f3afad2aa..06dec078d5fb590fe6dfe74dcdc13aace1a22328 100644 GIT binary patch delta 127232 zcmZshQ)8G7lx}0&YGXEbW81cECyn0Nwr$&Xlg74fJ2~H++nL|6Z`Rt++WtVEPC$th z0cA~fyalHQHZ^tS@x?KGw)L4}cOj3ap4Ak|l;DJ{)?gHJJskUBYx=|tB-Zt}ojDlK zZ>KIY4A8Y;%~CfXyN`36`A6VksjC$9I8h}Q0>U691qET}giw*hERG@m7?NE<8fJ|m z8WvY1p-tiBS2^@x!Li>IRfv{l&(x8eYgr%`;)x*uF|H_U@JelE;4=7VFU0%0raR+C zOjzQ`_7~`1hys81cjXlj<*}ZuB1u1_Jes4#_Hcsceo9DC2{$;wwb6o*@Zh5m#l8By zgWx4$=z)ZdP$S4jWxH6s zQ6JO;qC>602hx;-gbpBtI1sB1W~uHlB_T?|2IAFlCaa`F(T9d^aY!|h!V_l{V9dhg z4VIyLh6zw}>nM;2Lup19RZv|(i;2*@)B73(@zGb1P%<<4A$_V$67qwSqqoE~7B|4w zBVI$q)B?E!Bxc0aA^fW#O(D{&jDr{rdDoeMD##U5?>Q(&$+&T-FU}?>v_4{5xfSLG zr#MxV1E-l&`6Kov;=1|5sXf$RLJ20?gPIQ9-$iI|z1gyt!w97Fp zm?`IYJf!T?8z@Eh2>u#+kp3_{o|EZtk6mI*Xb1ODj{Tg(r291`zjcx;D@l8#EYYiTc<6_Kl|J67VV6_xTl;02my!Z9YWe_y z9lS@*p4OXlmfEZ>-!1(-NxFzI?|j$L!+hw0smRkjN!R|$*7{l2sIBE)pB zr?0$#oxRi9-Km2N?teC5S4Ni_iQ}}M+*>mL($#DIZ+`O5y(z|@mN1NIUL>X&N*+=-W=Uw2Y_X>{(6UPrJ78Nt3==|6?l)xh(Ot zJB~49FV++lx5W8k=Bk0|llQqW6DCdVb*tIyB9>I7J@@8)wGQIi^S@hPd`<6BZ(u`@ zmsU^?HFcj?e?WzroI0Akk|oXGvWtc{t_V{0Iz8oD6zSLH?)LXSRExne^9T!}ct1nWNR2DQjnnV|yMxeC}?_e4C4Ort3|A{18|k`Y^YH zW(|_-JxiGEneeG5&-BUe+)7~krQYv|ut##Q$MQ$&G1T;pa+=2eT8(JBYXJ~$sQ^HC z#T}~>Zz<8i*q9P)Au&NY69vMk5-%Y!0Cvla7Qp~*R&bYD7vl8?jqZjM2`96oueYB) zWKx6mrG2ad99VQe;zN&bv#%AlH-MKx_X-WgsGz*zv#&Z5DH56&DGc%gmFlbKN}#59 zQP)R2^0D@*6mK96!+LYu*1~^Tweva9{yfmfVyC;~f}RFy!Ir3Hirk%N zJzm7YGZvux+oCl?p`!>@Ns6gbLQ)9}31JA_NVY7nWTypy*(8VgOxbn`kpII<;Xh(G zB3DUz!Num}Knxuor+P@r4RK775q6S-7nyKE8(3|I2{FWYOUQ(3!iD6ERg=%QXiaJh z3DNW*3962kMl;$it-1%`f;mMHgI`}vhyo-wh#){1P(s3O(WKrpl0A9A8V;UDa1xp{ z@SIwH2QN^eJW!<38AU|Sn(~{mF`ycgMJ&XL%FtxGJUOwua=#E4bpdlnO^_mJb@ zSU=2en^%`9H=jVrRQHd0Rc(D3C%3*EN4$8JIt#x$SyEq2@T-908*gmCE9H$(-b8vh zutp8dnVteNkpL2Q^+F5-rq(_@x9uqmcRN!M9CANq@g0NjIb=3JgkNKC`7Ei1INMilbCrAX(ygn(Byz;NKw+oO>+!E) zJ{&ZfiQ@U4BjQf}0muO(*yMXx;_{oYLw!?S5|9i%PLnIvU>L&ao6f3iI1$Wm!qvZp zDSa@?hMLd=%m#VU{4vVXtN129ZB5*xNE`dR8i>9wR}VnX7NXBm7MwwRg=inGvtUdK zmkWhgK^CVT?AJ?;>a2Q2H^E-VlN!&Z-Q1Re zit_xT_-Ll^_NUM7G!N zyA`ygRb`tsUS)n|Zi7uxkh?hBS3-4M)aqYi_EpS6?KvT}a1~(nhq1CIVr`v~mk{Ip zdv-z3_&v?KWz2jWC!!v^?1$$QNZiKURocDB~uOL zUf-hnEmx-*w5O%5&n@SQ&`fZL85mSz#DJt))-XXV&nTSeD*mOFq}xYVe;2x`ORIRb z{>YG#_^7!=`y~ZM;mN-zONE?UmRN>;Hbe+ohDXFz`>w9^=i~`O% zSWhN3Z5gaHoM>*1(Q;ME_J!t?=mt*2f8X#ohiI&9Z*XiJnX~b!-CGtGO^pV-sT*PR&h=gAp}V5aWjNkPF?pupLmarY}8YK?)xzvxol*Dc_^9tXhJ{IibS zu?8e-R|n;dTKNv$C0+iqS$&I^H*xeNk@o2 zr7u2mjr*?7Z#l{DR|GeV;0`P^bg+P;=2~xFm`g_yhR=ap)gk_vFLW*b*9(fH@#>T) z{XR$X*bFX`7FaOwv~kLjrc;-E<-zT^;p4~VM``$3JpKoPo=6I7oU78n-YDQhRwhoL zc&Vi|^Xz=N75Cmclm0tXwo+6I)JKN>lVay3;aiO5M_V`bZ$KM^v=(6h!iURF=U$Kj zr}ehaYl|{ff29q=@3tIaF?#==dw=lTC5$bZ<*kC6WNqb{>T!3_H+ByU*X3svvPfOZ zMKT0+KJ~ne@Zn7L&n$i0<$QUbZ-n`tDW=vr??*fJnBRx$a_C?c_Oa6`(JGUIe^bbP zfNyX_VAo)b zr0rPuFtcxHNXTcl1dgIs#mz#}Oj!_!)NT$oZU%F&_&=bxP-^}DKjLsOC(a;Yg0lP% z;sBf4lFsX5SUuZ?VHwr8U7(gVxTFCA-Dl)alhw?V&+bd;oVVoMN6Lsau3 zCGtKRHbjF8WygK;NX3GPNJV5rmB=J^u0vr__a~pE{k~%6hl#BQ7B8!_n7y879)PV#Cov6q0Ss8fw}*4Ru6Ke)hpe} zizeGL3K%R9(SsN??miLL=qGhtZJvuFQ(``OHPt9K=2G?;GGUEg^2s#b;&alp*Eo;tjoapQXi_8_01#U=DQIj{dy+Bh3`wI zsy|gJ(%&u{%il-*c9eK7PmY?2bk$t^Hj$5f3XR`N)z`ec$<9190_Qh{Zake9dUO}+ zJKoXz#9ln-Bs(5mZoBIEod_>ZB2}js6a>p+9UC@Jhs+vFMvXdei*~^KB2fF#T6f2k zmd`DJV7Kn{tY8zk`Ewcmc(l?=6SJR>aFfF@a*!g;^%culEAwds%Ks?t(VwpN9ov>w zvH%1$+rRqHulu2j9WyO|KBtTnJk&Xy!QN^Tsa`%KXx~Q4dCxA+8gri*NVfnHYtP6H zH#pqkGfW02R0$HPMFkKvb^wGaQ6#VC*=pqv9^E@lE51fB)=`0n`n=}h6eRs{z08xd z73AN|`Iq-SAKsBr^1^B*IucH1H3w;)x$ zt}CBHyO;m$_CjrMsS-MQ$))?{f!{sLQJ{YgE>|-`GMXD1?&&}j)&V(`4c?KEQuv#<;bnB!R7I1~T5YFF^W*SU-v{{>UzWvKN&=!BMj<{U!y;H8| zcdyl_0FN5Z3QN#CaSG_P1+fk*#FsD3aYeM2gw)#eD-XCNsw;1^^r4>)^?obAk9M2z z9k_r~TJN*t&|ih~Ml@c%$~P4-%to{eMGwk8%lsfwB)EV_^iN${$&%gj2E6XK@hh!X zTtt4qpu9BiU;jy>=RiV|PMlR3*_+0^II&lmMK1tXx&C zv+7?eYz3lO3#@TOPypf2n0I5t%0H%F?VE72B}(nucC{#Ihjl}~C`Ytau0^ux6fKduI_{WaY7Skh&%BIGFo zL7uSE?f8m-=UizieKc=CJ&~3;Y#E&3L?f6C!3cvLuVcxgCKCXgt-x*Rq4C}q`d zUaIXG*9MpZnBPfAkFZx6zXzov6PLo9)7SEO^Cs8EtJ%G(ba2K*WrH%F1I#oN1EOrl zAfKvhjke8ZdhjPb`;5kkEetckU@!6|ET(l=?qAZsvuu|l_j~;3tg37@KSzJ~ygHH$ zw;@-D&9)WPR+b`IeYcQ)6R+2!*f>ok6d8uypaAIkAE-z3a}G3WnP`)$8~(ayNa|rH z3T<;EMOHiJz)JGF&stwLnJrQPz6;=^`rP~U$NC(BDt(%-e~XwtFt-k4sG}P3p+<+B zbsV&>NH7YV%A%KxXJO%(`fkw!^0wh|J80S~uk%;R;UNSBYb{2RqYQ64d6>4CG29x+9Yy z@tLWcyaR39cn^-;G)vybeG?nu+XW-zErJS9*APz4y}vNiyip85N?ga%jy7iT?S2Z~ z#Of4g4N@)vi{? zleV*b+Ma^aMXv^*{TTvF;I__1)5~QJc3EkKbUJb#(!w2r1dY8Gq8wC;Cqmj(57I`W z*rII*3i-n@Wi-dGdk)2A3<{zPb`HySFAN+lcAqDJ<@M0GPftTY#kaRlx?dUt(6GYI zhJ!{SZGZNKx1tR;(K{TaqX*-Qhw-rxU=9LTlKRrpZP%N}XPctCiAsNSg`tzD)tSOu z-*(N9QF%FsB`@`)%O(`Otf&9FhJ@u_gE2oL5T~@xW&a)!&3PM-u`VJqLwVZUqB#eO zukG@F&C8k*_rxR)h6TecQee6dJa1T{;eFN&u1gm|Da>SNm9TovAxhS2WGDlfz4hS; z*Ux`VXdf!{E`->h#dU^=^ia2-KNcvx?8kz?4HzyP$l}RiXU28^_~wJ~UPunk?yjL%!k7|pDD9icL$PYKQN~it^>Ic@lLaAd~5j+U|ElpBNLyG3?r8%S+BiYc7;R|x5tvJEVso5S+H z9)E0LF~eu$M`~)cmmpj7?Y;1?7x^VQFIO$O2-aiAv)55!#p=&qU}Z9(S6K5MFXnI4 zS$}tb;qdEk`WaRDDPW2K8(Cm{h5(yCwg`vA5`XsZ7K-3X-e0>ZFagdd8nhwn(x#nO zGpUL2akkKZ(_^dpx-kLU*Y|yxtO~Z)GrzdGTyWC4WyIa~@Dk3mZO%jxZEJt!(09aL zAns`$QjX=6rsI>$12K%Xoyns?!`PL!kGnbghjmO)&Cy3c3HGToA50d0X{kWxhT719 z`Ec;&s?5Dcze^wGnCh)(iE=(A;r_1aeIv}zcf3RFa8%Z^e9VfZql_W+lFI* zG^$xgIheP6(NtF3&@zcLp{Hxg7;B;0>wwSxo&Kn8?W!C>2ITAF+p9AbIrivBFFPcI z-tYPI)V#*|vy93QP0iOlb4etTg{?}#p`lm8uKgxDe!`kF)y#L0klOQ9ViHy|6jt(g z!fFs0iox#&x#Bgm1MR&i!*Oi48cWJ|vc#8==RhQ~3pChxs$Dw1JnsHZ{DrrLJwb|r z=lqR*{%7OH8|Wj`>i;vAiW5vgVqA+?1b2-HlKcDIX?bFB$#e&S=BZDFGAqp+&g3tK za;;pS19{}$M7Yw&Cpa0UKld}TBThrc9?mjoeIYeA{yENSPb!M4BmXO7i>Apf%dc`Q zweMou%w<$LFM1AMm`AqR711-f`C*xLmsLZ9vMBHAWI#zFkEDQDY_AamgQOA6mr*d+u7mN{y>AMpP$XQ3CUtS0*US94nQO@O`Ow+puXS1IlPaulqP;mE-jhfG9 zB34iseIU`uSQo)%CB^0D-#nt+Q0K*6o=QIJ`9V@rN>L6Kc+0EE&~;^y zrM+}+1pP_UI)s$h;*ZYDyC~EvQGc#lG8!bsdAFvGMV6m3e3v6;nTxv3HRxCE>*NER zBs$10!X$R`8FnzwEiyj4UEKkmXl~hZqpNcZJ;~*9R*)19+p;;Y%;x9p;nE9ywE}Gts+zsx@#O@ z55Ln?N#7s{viM-30eT!A02D`u9~+?t+R(v{KGJU-2^ZFV>n)p4OMCSjL>BrR>3^gP zo0FZ0k;vZ23XX>djzPxM&fLX<=usc!^UP4)QZB>?|eJcSi<#INx`h)o_W&U<4Z@RZA>N>I6b= zF7~**_&eKl?cALU%qJeb`>E0uS-%}_pF)>I-t-d*V8vK;d`{(QZ!#yd_E$^Ey>X|X zq}xOR8Zdjcdq{PcmWmS#)EPyA&_$Q2O*8^pL7gzq;cG;e|8%NdH!t3VXFu#WD!U!x zeYu4Q%mmhD1$MbRsz2k&F)Tyj?gT65*F=O*apbv?scR<~9TS_j9g?W;pVp@76WVPZ z^+>z>?oO7kVv+^V$6)+X^yiln9b!5*3uQ?_mFaPuZl3QPzHNw1+vD6ZX0&Jcp?`m60y7Ip;0 zK#um0hIV^_T(pbdj;v;`t9DmVm|QzV3HB2eL;RiSocN8}DF3=C?5m;I$(NJv!V93G~^B3Ja;r=N$7B#H1Avl_LbX-vi zC4ey-;M2yDgjkXiNqZC~#*!GzJ&r;`jv6C8fS3hGJ9llnIn$f1U&0md@t~s|<2PKV zOzmvXBfCAWGGLp3bCcfS6gX1gfLddwo1nb)AJ;CKU-R!=;Qj2GWHibvnLY&O*gk4e z)mC+%t}eDwAtCo9kOaYzl@!9LDv$Kg`hwV2DB~H!{ANbSZ=IQ?rB_T4G>cVs81(NR zKeLrdq^)$6>J~@*=+D|3Jy&#oMA$OhN}dTkfIB^Y)| zGVnjmSYYv#{5IsotkR%DVT%R?&O{@Sim(;FltthODig{kjabRuD3_xnkSwE$4=HgZ z1v}xw+*Rhxrln6%56WB1htlETb{N~e`&RpV8>Lf;OUId_CbO`KnGr+iMFZ;-V~-2DR86V1`5?^DQJuW zgqTp*m6C>3OXUbvEsU%qmhc2&_V8y+R*lNB{+Yqdfr}mj-CUBN4eGR^l|-)BUxV$vhNR zW0Vou5R8OjOFwM61u>(IP4mknaS-@~xBdpN+AOio^E zDk=`gGmm#9p;n~aSU@~byAr~FmuVu(mxW0_kR&@60S3S-s3l`gTBxxqU%cSluBx-| zXQWQ#Bw4&XM6FOZepUJ@*bCXX|F_sDy|uMs3Q`+|{8)}Sfp7oXdj2~m5AL;7NsySN z8JY~kVah?mc#hsG@7owr!?YS~Mq6$Rr%&n`(9xS9NzM!whl!=@by@4rm!7soS=i&4 zAVqoj6BIZWREAfdo^wWnt7R#vEiD$>2Q4LHr-BPOtADXSl=1fZV?Mfvg`D6r-#wQ! ze@f`QZURo!?inR@_Ti)*b0d|9+d4k*!;fsSWn4};&=Uc%g_UAJF!gaa(6ej!6(`P7 zI8EM^o@~#U>NjG1QF#+d!E1n^iTATUx4-VsnmwRpw1<=Dz|^=7uj=aVeSMFnx3V&5 zaEVuAsd>e`DjPg0;$$V6k=5+6+0UkU;l%_@Ek!H!+X!zw}S%&K`ghBQrB%q4(z% zQt;t<;wR-c=4-#>Biz4opB*}_D`hm}cQdlE#@7?JJ{mu&@6Sz@ zDSrd6420GsEpltDBKVMcwjAmc@4}pGZi%<=-tJU)olNXg%1BnGV7;R?30{X=+jf|u zyyCK)wtJh_()^p*AtzGnHJaBrGDtaEw_ZJg#}a2so{3k(Yf7WFm-C})0lpX4g}P^{ zmvZC}@JPQ@=0UHnROa54u31s|@pCt7JY#@(D!wwSGJFwu-UK#I9jP@@N*~Z8(~duu zBAQ4mN5w@HJS6+qS=6M_0PBG<9`ovHQma)mlD3DI3$WIX@bgACif8vjJ}J|3Rmrio z>(>&jEFB{=md=^enld6zqBuH_%>xOjN)vLg!?b^86jhL9-Wd$IgkJ{@7kt3H_9&2y zFcp9xv~~4Bq8Z-Q7>X93o66F52EI)AcL?E2LWZ*QJwh!^@vQNVskxcpt`0`yReUbf zU>WbaH#@E-mn7yVTB+cnqD|COqtHLUo6Il5D_jdyEIae?Qw0t#5t!kUYDJWq zsIO~kE(L3aAxO|Gl)LjdLl}f-Cn&(6!tdcnEiT2xcy$Xu zSt~0Gr~TILyh?i4H(l%hy2lzzkn!cI2a13Mz1n%l$pniv+2(G@!0yc-4|TZ9@npas z4CxSsP6aR*Y3Y1sLvEY9W>N>fUQaS~YEZ>UmvI&TCG!&zCFpRQ{%UmZ%LxQ#)wc%m z$Ly;=Orw`MBv@jGnqU-%gORY{^1WTYeSrz(hV|Y3={8^Dtkej&r)>GB9 zJMDD5(^zh3ghER6$tqK=JO?_{?^`58h!CP$!5iLmT3)RsFux?Nf{3+3JpmRx5i++- z#dp#N`dMI3i>=Y8_@j)PO{!J6@RK=NajZKUDii&>?*3G3=8M9Qas&hDcVnX4XJ?6d z%Hlm#C#t8NMcL~*-YF7c_wZk@?C|?dGxC@nmFuJvKX6xquj;N!4f+GEcGb+DMg{q= z2QpOMBT+rE8uSx>veT7EZ_RogFN-N;+N^f7l#$Njw6vvl1ZT5GTO`6z3o_77z$NoRAT8bKd7 z1BGVs%&gb56DyOhFsbI-m8)eqWcJbiiLx@XYLY{{=HzX%e4|-ZmHn%Q=JT6B^5(14 zb1=cx)zgNZfBR#wmNGmpWBu{O>K7I2LeL=$JUl&6s}>cnQhwz%6>j9O^Q~I2915)Y z?AA1L@z7SX^T68QRsCsEx7t+ulGHw4wNdaQ`>Al24NLGcZ75xAxk-&<6L@05Az zd&|Pj_Tz;1e$N5WUVK(n;?33B5Vee)c_LyMHz4<23$Uq z_Jqkc9AY4G#J5R=qwW=mo?>gNc$z7e6TC`suT-%zafyW659FB$r8YaXRnN99EBfv! zokhwp!bp6E<+b_}6ehB13Xu(71UMLX*gD~5vXH}+uP0F@hxqeM#nMp*zy^sLX)qg( z+c>gRRlhfDs_;gUWdYjL%(zul1J%wFTi+6*R5-nuSrVKe9OHk=VUl%F$P?QahHOT4 z+(2`N#H=5Wj*g{Q={@?x7K}b<5Cg-o7hm@ zw2nAj#GAlOjmfMzI=oq3&bVuH8sI7oZt>bK4sMC0Uv@$8>7HZT#w`& zypeo7yk)C#e9Rv?ZmNnCR;D^s-(Cg+l!(7+RzlMO4nPL&$Yd`qr0k=UG%*tbgwpRD zze7-)l#orIcX}@m7Z2s|X0rNn!%~^Y%HI0SXKw6l4Q{fE7iKhBR(db%Z^uU2rSBLf zaHL@cwmcY2#9F6+%&3{>0X7|F?o(`=(7w~Z&T6W8imf8UCXTeBUU4Y&$9qO&Rn%kBiq^scSrY=W6IK4Y!9 z{E%FU!}{1L4DKIZ6>W2NDL+}rxr5Y*uMykd8#DUF7W|!SUZ$D^Ya_F1(A-E}y;`#T zk6pGl{c`B77e=n_VyR}}mm{kohWM2F}Q$cW^^^KIZBIomK!<_($P7L>+h@~u)27Y^1Bqj?i%@_BOlw8u-^-%vL8mF3K7@Drm z!=bjhLeNVGDc-`6z>S^8uja~KDBLR8SoG=R4h;jb7t^O!^L4N^y<$GebDe-poc8NT z#LmAWS|>iVF5!`MpUgMx4N(GGlhcKmE0%e&pJ;K7Ct4E1O&P_gV+uBys1zkr zb4mHaP02#s99@`j`VY}0TWk>_YnR(%&DyzXZ0yj`9i6LfPBZ<#$H0s;5I>g34*pRop8uh(Z)B9gk|E0ia%His?>L z#H^90Xe5}KaPB&tN z+ClH?`*_`-Evqjr`;A~0g=qd8A5nxlY4-AXd@}D?+b0KOOCr|fZO=z$zmm2R-gbQ4 zul(+MTYIWAFU#_b8dtvT0`BV;VID(fK-ZfbAwaPqt0G18!h1ZXOrS|n9;05;Mp>Mj z-YhQ!cn1k4VqRRyD2%U#+nKdkR-M!xACrGCvN7gd-E+lKaJQ7x)u^OAG7cQ-@(2dJ z#JyK;{s$TzZHR{oI=p7Gi5#2r9b7-IQqKxAbgwqah7|q`e={;S0q$5N4R}(@wbq z2K0k2d)3?fPY|7&1I9jk+?0RLvH#%V)$~*Z9g@kH+PRzzjI6+7*FY0AswmBqwjSUw zSe|g;bDjFOb8thG0k=o&2)t}D8t8cxIZqNO~TIaw0b2F zHxC(%Gm40i`C6&!!yG4b3(jCJe^yZI<{0|ivd;Q}*CDPI0o-WLSAwJBazc?K87!5ExL@BVO{{;{BmaNV+)X5>XH>rc8k8tU$U@fC}+HQant!w>?dPtq3{ zbQa5v9M5Tf2T*&JJ0wQQjSu=|)EC06nKtvew!4dw{<*#!u;nxw5f=v>oHJYH+g-DZ zd#|TnBz!NUkfY)hJTD!DxsqNAL-F&@&bo$fw#-2gCG!+Wu@8KZ-Qq9Tlv@k?mnHb_ z*kpM=8-|g;+R1&h_02WU!t_HcEe@n3#XeR8@>p8(0LEa{RBh(~Qk(K;&Iz`q&RwGA zp0ayg&Sx{^zAeaS{a87}pQWM3?{%}HwA0!&mxuFL-YHub zzh0;Df#RiTAbAxX%XT*BsU6rM0BeSp%GA}};OYw*^?}GX+eEk(ij>B(EX?NypM89r zn5reElRaRSqqsi{V@oqB^Yj_Fc{L(2NDi%!Yy0i%0a=d#_H*@+H&qJ22zKK`N0zQF zUI!s4C)xPN8~woZ>czI%|xou`M4HNC|6NKEPh=nH27E)wGQ_2g#5J`OZinG?Y{+!uRWinSy!t0 z@-OTY+VZ_2_a4JWyhN?t->ml>Tm!YCF$iN*G^`ELc>ic*wf8qe=?GeZE|s$Y3j1Kk z*FalP7etc$I?~Sg?Wq3+VKW>J zv_`i$T^DzTtpuY_)PEajLLYyM9VlebIfW2Y|92R9@W4MHLvh04TEEJ5L^8nlW@Br6 z?N~o2F3w7krD7xkG)PV?S_mY$5;TajZf0$m-faJ&WXiyfczp1!AN|*a=}+dmZ1+sN zC2pJN8j|plKv+%zmm(gl#Tbqx6@R{dD>D9r(tXEfoaM5iN2K;V>+eTjGIEtKg_(}{Z z*!Q+AQxQJA441ypCB;o&<8#plq`^&oiv}R#8Yl7bHf&HpC)4=)4+MF!dA9}Ze~vco z5U?d@hjS|`1eU8&ewGq4cw4NW4JeN&Gr}e;B1APvNQ4(7SwH#b^FQdk=5+$^(0H~s zr?0avH;TRP>Ef7YsZgC*A-&|$%+ubvjlBtC)&Im7ub*|-H;(y|3(C}c7C-OcFuJWq z*|;LZ?}+rH3rPgXy2Kt|>kQnAZ3<>iwtS={a&u4swTkaVbr`2FQfyG~?}C@K-(h3o z{v!!7_EpBeEOEA6qr1w;;?)>LQ?|D@lh!*MblRZT?|L+bAjweTc^KJ%-^NxAKW{16 z;b{lrj3BnCAeh_A9?d6nxXgx7LR`rb7W!|or5yH`F9nP(q6T~7RK}g{0DZ710lai8 z3S>VZ;59{(&0fAwLZu2JWsx5Wx+JLl_WY!Vm6H$`Y%&@n#@Jb1qx*fC@|hvjuMw$V z^9`#E&CmS5s1{3ZUT}d7EWqtwUogmmwL&NC7^AP^q`~)aHP9a_4hpR-0>cl-Ip+{j z_%##{lQ+^T2qTKep#@-y;-Ns32IfJDcp(7RoUIzlu?=J(MGlF>48RxMKy z4X_`tDc4V(XYnksVbbd#+8B80L`x)HE1pSMiwS)Fk!OaflwzMA;)Nw4@kDP-B}YIu zVU_~Lr5$6m4B3vZF|{az<4MHm?T-l@f^E<;g&U7?Mv?*I6&n4~L1^>Ba-j$de$SVT zeVx#)vF`i9PES8&tp4$BjuEG!J{ZPwQ3140T?rgA2ut>pePOD2iZ0Tc!*GM55QswO z{tpqxx9u@>?@)hd$)$Q@f@593L~K9`OmpO$!VDM4ED>ox6kAHwMUa*C?lvr7#{CRzBLMSU;E;))x-JXcC9ay zW_K-ZMSm+Kt{_Pinfv+&X^F?!Gc1aZ&3&rxrF0U?j5YUJY#9oZ#Oeyy?iEmX?Ku!H z=e+~60M=z1q**g5(EPMoYKn}==HSHhZdfF^xgo+PDYKEIz!$QjJ8GNABbCT(79uHm zHphUoPWT4H0SR@dzq4luqj)AjcJIamAYwRL()Vp#EB$yc<-P!caCD0?oH%5PUrMguT_?fi{YhaH`T%xiQ8|BWLexNL z(Cbo|>V6T==3juR@Bak*m1SVF@SeWMVB#`}}hkuQG1FgjY%`ATig zi-45+nOI0+oJ_Bt2>0|x!r_Cj8Sg@ez>^cPa0Ed7wPKlAZ#&|&-f?1K49!nk__!bn z&8Op;G$1Q^tn%NWf~L3H!(}5_EcWO<8Ybw9L6 z4xSmmJOxEvZ^&fYwzuV8@AqQ|(C}+&CtPBR)t;g$oFsN*4?#z|)4J;u4uqiP<>^ln zf|ovZ)AbDnspu=FiND)wU8EekWj|q_`t^~88C|UxPm6+~u*&fIK`cr$#(1i0fGG;X z7UlcbLND8-rZtUcJ>i;DD@K1fMk zgw^)2Vu-5ht|DPu5in%Lb#7wAD#%qRd-uhx@_HR-szB|T{Ni*`G^M#^@R$Ct(dM~x z*@q&IY}#Kkprn_FVbB4{PjH$7L~j!2CyM|$4zwT(v5#EiO!@7KOqa|l|c zqCK?gx1EONsD*nLZU#0{B50*{b*6Dy(#AZPxK)iA4IMaI>iFgMM_}tFD5dzi(`ULs zPMw|YFpx7?%v~?MWk(51m`PA3)lZOn7Z?lefrlzN)sb~sTzEi6h-tpIwCdf&^~lH0 z&C)FLs3ebf&I+2Cnm@UHq|agw!?9hNfngd~YH`KA9!hmgqJb>R{sIp%50K3z|dSh!q>6#J8#)mZ?!O7RN|iPQEIMCp9eRX5w^aVyPKL59eW zrIt*8h-r=Onp}|5kkuH{uf*2m#F>2lF5!gzvHAWG-kD!DF*5?3$epsEX@))5<)eBJ z^bqY)gTTIUBh0Wdc?5r^BYcDHTZq{!qxT-FWu??FRR)xd_hU>#{To!v1j{7Lc<$(r+2sJ@8C&_8_F#Om<-&ZR`#T!29rq z6NuMMdEiR1#vwi8r*+O9E>RAH`Fhy7znk8wo`gz*`Z%vUS&HOsTse)x8Afj$18DSi zjSoyTv`-`HiU=9ZPLvfSLj=iVv00!D#DLOibObj6Ljjth!Lt)+boGnwj5vBQSXVO$ zCA50>eUFfFT^urtNOF3x{-bpm2&a`m0(bnK8>Ps;ia|uTzN-!^z{JBLl6NI?-RSyBL9KFaJw>#Rq4wF|pE3Dvog4az zk4N%{Hwv)@!A%|F9m=5}l!G(ce`6v;Oy;yQr*D*cu6B@*VlNm-G$UV(+Z*}VfyI$H z>Rx@dFPY7wvTAPco3phBIC5pK+e+VvP0iMCn-7xZrtNe#*~AcuE-#&V094T*0#grM zY>};p8h%}_wSuooo7#?>NKa+E`}IneVwBtOllIM;J0q>;i2}|vzH2gRK=qjB>6r7? zV9uK4q%sDkc;HU3%sM+O05TsR`Fu5c=xjx_?9^Bp4T&7~_6XG0pQ3m+zj{oP*XSt~ zl+OI-T@u%XA4(ktx=TeIc$bvTr#PW_v`LV((}tG4D+q2Rd83KQh9KBhfrpWm%Thx8 zU3`|4AS!41rn71)SI#Cg{Gr_NrF^$SE!ivB1)3w_@FFP@Y+?Hm$1){jR;+8>clmK61PuVd1tm)2)w@}a?@Kp6TCS@qEh8Zk=GgNj1bXP&@ffv z3hSPrWqDB%!*Umn0_2GN&j~k1fd9wVIYepJL~A;&O51i;+O}=mw(+HH+qP}nwkvJ3 z|9gA&tmkJI>#W%E;@R|4M4JvD50p7vuX^9znGihe9J5A)#o>^`nx+Rp;0_weQ6*Z5WI}ip2>%CFZ)U2ewofMdbQ>M zx^y^Se-ZtovndL@2tqY@3C3qx_%tQ0ao(>f^}-&1fpEp7&ywy-QGglK6s3VtL0MTi zk__sol3avP0mj^|)|F-rcCiTYV3N%pl%!}IzkGGTEdF+U4sFTP(?ko0V|u$EJtfbN z8%YFv8mtD<93^l2S5^jZ{R-0MDnU(C17io@9c zhvCa?3SUxaBddxc&}iXy4W_7$rCqR9;fKcBM%C~lfOpufh=GU@D3$<`;i~zY2rBV4 zLy>04OrtWxY!}Lh6{9cLWN6FKsw^K>Uw6%I^rk7(=OJ8v2IgUl90rPADkFF-)6B+$ z%euBGqYm7;+M!a>=NT3R*wTBxWR8PDYJJ98pa?^rcpg!D+wes@LjU) zLP0NaXyVQ)B4m@iUDtTUL0F$nM4HMy!045FkfLWxd~NE`AC(PGLnXT-4kws;u{+fa zvgt{O$mpsAjH^M2aN6tI(md@cSVnd_64WuQ;*(O^NmwI<%D#L6!)-clG)t=H=<}LP8i)B0 z$U!gH2l~b=iE5Yz9;oG6c-|SO2~`X{MI30C8Q#^X%+qJhK+E-&_ru54guOhb526|l z<-tM<_J6H9XolmFC&0v*yo&?$@0*`MgunAnTCwsQf`0>+3$7q{j>f2bCSR-)Y! zOsNP=GXR<+c*`gJhfK$k`E&Q^AJZ$NpXdd z$C2W?kD{+YcV#c#X_XInI~;gsDlodG!5o0$>{_7uufjeSGysTim2x;t(JbtDIQdUu zN`VzGNmSi&OBFEe;ePaAt<$hbv{zR8HHIo9L~)`X!k;FbAxaIb;v7wB&$wOG^-Y~8 zaGS}EJCe*#|8`YlhgT2mDIJmm`IulWYZSx{-%8+q+2RUg5DZEK63#OPukJVrRCw52 zbg(aSl!C{BQRF{{x4$sHUA$PXc&QVo3|%9;MKF zyU~G-`Aw{EI8!*2e@bsG{tu@^8EAt5!|X>lS_NY&SD_$G_&4;enN#n1>NdJgPB7HF zPeg?J7>!qk%QZ-Qpfe!>LfC~ELtKvylu}q+aFGfSNWh2iFFXjw7MLA!*+V_Zzhs3f z<$$&68qb(N-O@oY_=o`zW!S!U;_r|#O+qi@8cq@i+XYOirqNztSW)84R=MMtRdfUB zPV&;WBM6Yd^s^{IwzRHIpBfZgtJO!w!i)5igT~~9Epm!DflP_vbX3$a;PAD zduu)Alc-FB$46DZT(n*&v$wl1?dL6@j=!blZo2>#JnemEFO(f~M? zQ|tXG`A#gKl|y+YO-O}x)-+I@r-G3x!DBTtn-WCQ4z7f6c5n8lQ$loByqH=xxq@e;dDUZ0$z$J=9c75aIyLc1Z^~Lke2iK(#G%N zv_0(aYe#t}vQ&%4;JUF>L#2rG3`iYvXHs-!LP^msbpzr7n!Qx$749yiZ}qZ=<4_eB zBt1y0n@D3%wH2l}8OGQ5466|4GLi01l@tIbVnPDPhks-8QT;ngqP9AC&Y<72hY_Pp z*#{AJZ^xDxVAN0@p{Muu17gOx`}@#p`up(W_E0lf*u_Ux$44MT`7`FY4p4M;ySi6J z&!@6gxvn;kA9v~%=RQiy-PU1`N|toJMt!)LTG^={N-91XoQm}NKCt#77HX?_blyhk zY`M>W%H6(y)?|`Kp{7{Vk<@BY%;AWctxZvbV}Ch+-hs?mJgQf1BZ zBbctfMS9%)6BYiRrJdii0GKX`99=xUTQx9yu^GL=o}NY22bWN&vQuW8%=emXh2`&H zJ%bhAo}F%okeWCecXv~s#vm~Z8)-&PCtb(iWZ%q$a#`(9OHde+MTBt09~tDX-sh>s zAgKs_?Amg@4Wb6aKnwr5RfQ%!;!+{YEU~(2wK#vpBn$9Th*OrS2H^J*8Wf*1-hfBX zIv6a!G!o!r8ai`Qhw^rpf!9rbbQTyfmq%0*gss^r>iTiaY#1|;+1guv?6BaE7(_zt z2=NCT0bn901pVVzJ{dbJ*8re;lIcLryM@fKfz69Pu_McL@Mlrw55Wgn1g)BN-!A=5 z2#@A(=~~gifoYbwfM2FObRxd(UAB1Uy7SqnD$g0;{*>a}*1}YcJE{ z<$3jf>!CxGl}+WZeN>dHMJvla>VMW_IqfZ&je9?dMAdj2U{6d7PT9|p==;aa#}Oqg z`NRX}TtuJPK%3s(N<1DO+mp^6Z~ZZs8mK&tpzu1hi=PvOb*FHK*~On~l90|Df;Z-^ z3P?99vb9MC(4_pEmjZbV%k4llGn?~aXI`2$0?1dW?3Zw-OZ2RsCk^~AwC(Kqca(#lAzSMdi>>!lh#VIL#+NIfx-TM)wz!9l99KFSn*t& zQjwl0nFDjlra0JcF9(uD#oqvLRtU_MunjElC1M+X0E5+O3+?N<^2+vAGylk{jPN5NWaCo=aH(H|JSyYz8*@R1)Fd2JI}vWW z_S+Lezb2q!-QLu{elu(FuD;Md^qyqo&~p>Ot08+g%DfUzcgiE!_YFzoKrpRfMs)sah3pMl3sDhW5 zHQ%d_C^vI=xUHCLWJfL^avD&GRnb>h+?SgG3Ks?jB_TaI$5hu=94eW2nm5X?j6J2h zqS~6v9#h%i1q#`LzNNq~Jy(|Q|8T=`T^;ctUqMg`5`0GTP1KmQ(Zr@H6fN(JSGXZ5|;7q?~>mwX}qVqlmQR` z{%XJBt|i~PfIeaT*=4eY{0`Y4fm-=*bmdKGpD; zA#6BA6gy#PA(R=<2Mh~`xJ9W?!@X>P$@F|EBrz6E$r*0OwQYyr@DYsuCkiD6E{pV- zak0A{ZNc1SdF@YQ9Q!;DKf2brX~xw19;n6iCHx)I3 zZK4r@vBVV7t~GJb4IExa&zLM4eOY<$l8WeFdxdv;1x&+{O31Ej`f@Tu=mZ?!srJS$ z-C&xw3Bk;){n{j-c)xe(U_ylQlJ8aD*v6V?Wd(uTzS%XnSzH>Z8NUtrD=|J#AN*70b~N4C5$|L~I&QDcA6 z&9r?KR^I*i%$pkAt-q?Qi0k^c{%zLzyp(-Aa^)-I_%xxQ`qFyzw`N^1$0j-3iFlzS$^j+VEOg%Km9%+W7vl)v$F`tqCWD^h5 zB93zun;nKTY{dF&N<`+sNG`}b=Ma4TsE^c^t9kzx)$+7~*t>Zq*+&U46qg5*B>;0G zK;zU}LSJ&cZO-Wyg7nBeXcJHTWM1j5Wy$iE5qE8PXL5p88fJhag-7Kdre5lpOc^f$ z&Z)j!m=2I7h&EggZreOn2eYfz4&>KFPaxAE0X@J+lNU#3Cjh-x=bG<7rr<)fFNSvo zFKcm_OVr(t?eKLTN`Wb2Nl@~&({8NOsfQ%YV2q1Y;{?zVFeb^^(ocL{a2x@m{1?Cj zjLu3Lewm3Q0vu z+yH1y4JiWfb{M`^a^Bp)AZRW3ulve!u(&{0^TG8rhrili~$eK?#Y zI}iAd2o zYKI?>5p4x2a#2ZAS{>(@H3`0T9XF_ky=iHVH!Fj_kVCAAiQ&vQZg8Db>SlxxaO;~iS~YB*>BEY%`} zj+7Q}KZzT|;NO3p1#Qq;XjfUGs63~Tf*^WP?}{z32Mhm|*S*CV5`*6yZu_yCmzt!@VZ-$LB3}<7BlbFQ^26$k zV@Oo>ct@4P05QaB5)qaA&L{TUhO8+ddnqKVu9s+14-16#;ergr+9?I&k^Q+*1_dM=VZKgl^J1 zWN>|nYpqOZ8+3gzG}^~Qv(2sHl|7ah!hV#OR=1%Kc9b(5j6n^gpLaPkSm|(UeS$Mn zC1+N8+&87$Hk*U9gf-&R764pl{uJ*p)efx&Sl0W^qb^6HjRn?9!mNi%*tRR(^sPfpjWXj9B;EHHa@Gj20yl^I&so=)Y`T)`c>6&n zk7V*Qe3f_i#ijL+K0sJeE0xv2dj+tFzs`_ zrwnGW?K!fx+|Bakg~bkBwE42s-w`Yvn=|Mat6j#q41#Gj{Ff0kJTo_=u&mN`0(Qd= zt}n*dG+T3|yP`Iz)fbcf(N~inJ3P=iKUe^G5;Zp9((%Hq1t9MRhRM4BPaFpe11Zpr zWRZhTFw%X}Eky|x4cMB1F`;4@bo5DL?^c-=FCq(1CXgQNH6hXocRRiTJOia36OlbP zH37ktzHO)*!x(e9$08Ed8hraDCy&(LmV9+6 zwQS>V2nHC zXNCN`d(3Sxe54XgydDxG%l>P6_rf2>+|^PW$d=h^xMbm{JSG)SNQYmdWyu)+l+{6{ zYYbW5Q<}#+-D6Y!p3nAg;sRz|n9E29o(9n|R{b#39sr|`x9Xzu4iLj_cE`N`Jo2!w z4-Kcx&Cp$+o%9z$`JXIvbl8h#Bb8W(V>+r`RU~*rg#6&HLEAbbI@Qo7#DPc=68=*W z-7QD};^<}1iol-J@V;ZH(`K@2LRj*j=9Sl3!L%-y>PxL&Nq16GWHZ#_)d^w{VY#`S zA?F~6NkEEl4Db-fLVZ*Hj!z{@p?D0R6wk3J#lI2-q^D9|B~X&I#6(L#<9;sn7nWUgIGK0|oUtBrY>i9d=~FMBXYt*dSsSLCv(X;-VgLY$}$*D@y-S5;9ecBI^%}YTq~tka+30oW*-R?4v$X)zX-^Ak%Iv zLVTCKG}=;jYAfugY|HPi1aNdA?E^97pp&J>K~nrUM2Vp;u2INPE-wpQB1Hlk9#kSt zJ^)FyNs8$ris{lfkxB$%gvRql!G@ab3g%ZARx7IOMeE6__{;N)PYo;s9fCr4-pEwN z>)9x#v^tvzRzy*zy&KKu1myd;5lCz~S862XL>bhY>%~Q#c1lfw#jv+#J9w{a);N`y zr{wl(6WC~0REzSR_^cOOHmqo~U?$xc&4342x_t)4u;+7Iz7(QLJ=;M+D(^)B*p(C8 zhUqwOYo=|}-Iv8=b=M;#CbLG>==AnJZk5R$+en|2+ii<>{EN-LRyQaa6!Kky%r85- z5Zcr9;tc0Dzeb0es+pWV8k#U3+s(ZI|G~Mfm1nRERl{x7N(mQ2w!SAT7cdh_C_n_| zlD;0|ELSJe5h#thMrrFa!Mo4UvUy2gf1<^5mWt<}$`XR02bHA`eN@CUYRio+O)JJD zfquWto#LU0)lGu5osG<9oeqJ_2m0YBL!K}JA){RJ4B%WDPWz=dxZ4agQ#+r2`afaf zys6?ZMxiVlTR-a;QzqhZvR!i~Cjj2);8#&mi<+I=JVyuOZM>S(V7PRK3nanQA)Bc1 z%34D?#}9)@Ue%89uv5`OQ7}*jG6o%2+PC(pHH7AmF`YHZ7TNkO(uhSo@@3w2Fw(^2 zbL@FlKGJ&F5Vm_?v9sc&IF2$M^-2#7H9CbB`3HteOto??(;71(W6uWt1V#I7;?DHzi0IbNd0o7 zjpfd&?-YV*nqg&wO>FPaeW4v~-eq^{0BmeMU9_qrCLY)v@>Gf79l-KW<|jtg(aYj% ztj3ubb@>Dq_PAd)1AVL~o zxtAbMsqWKveN)|QYu;6SqB5Igv8q4p$WHxMIP0gUXCD*Ram@-UZ%Z7Wug$nFv*TOr zsQpjOR|Jp066ou(3xJkLjtE91=Yn_R;~KSaK5N3ekErioZ@d;b`V|EX$>2m0;@VQj z=1K=5mBk3Eh}DPOnlx!q!{(NbsSPDWE}?!A*i|f|H|`3t>!E zAA#qW5!zGwb3u8yi`rLfQp`H)k1~j+15nZLxvxNv7*&(mi*kPK;){(WnWK*e8;7@?wbJx~#(L0DEzT1wsjhndOd8 zCcT#yJP)sI0)VG|EQ|nFL=+1T-L$7l-YW3XoaI?^V!RJC&1wJNi9+LHvqLMVv$HSc zIor2pa($jYtgq+?R1wonbD8Kn#~;oW*Gz%(O#!q%hSzhJ*RqMjhY24XCt<|bl6={i z^g@JdphPB7d^a-C!yT`>-Y$>lF`7?oN-R`*Fez0-esvLjJN@quoZOoIcX?*Ybb|&Q zb}Q~C15mZ5nX92HK#ZPRJ!5U!ip@1;4B?aKpBt7Ygf)r6CaqG(hXC_7Hy8;yD zD@uH&%>VPQ$eVWO!!X73CsKe%+lfTiMNiSQBrPLAHl_tkluG@k3mu#~W@y%l9dIS9 z8(s0;20ojtB`r2(@KeHxD&}XxIh}whz5)x62k*mk7+&@0vS zq`MZFO6qDOx($N1mLd1#Vu#UK!k8`fwM4TZywDcB*NwbRoDIpqteIsu{#uhI@Ej6YCvn20V|&cckZVm)wAAXQ#N0(H~lXakVK>czIq zhw=-Y{RpMe1g;jT^Rr5qbO62g024(%^B^x2|8jFsdJ(!CGym<9v&e#0$9b1n(A<4cjJTm%w!+=is+72ZeV)BhH#j6zwPQle*$6|y zdG@L9r!L(g_;Lv)WLMyP$x^hTux~eQqxDe)9g}EEmr6o|sMDl9cmQ}qYMlL1t$4Va z-L3Ir7v9QzsF%LK}SkaL=}yGW0mP!(@n` z$0-fQ^S3l+2h9*u7Z)Wd#!$Iw1ZpJ)HV?iMKEg<#zsP-w59V?lrxDSWqi#cer!gF+f)44ma3rS>jF@!5-?i8je@V$Mp$;WQv_+Enn6iJ2WAgb-}0@ zJk?<$mn)HI?}( z9l=TM70(_{uTqSA{1P)u@EHggNKgl)dbzA*)$5274+?Js+_d~|a|7oYaSdtVzB^up|RxEkKZQdQyCn2R$J6enrEYjlY9E#cq7t@ZLOlpG<(o36wNTQHSI>K1B z|7*YDLkzk)Y9Kfl`74@EA+s3Fwbthm-_TN%)=I&%fKf0>#~2?XBPDD41TD$m7{k;l z(JLeDT3|51Wubt7hojGpT{Qw>+rJ%+Jf4FEoCV?FVw+ShcNT#sxhN;9o@qj@WXcaV z^kHYQt+74{-n#$KD5g(s)1EK&{_ce@nwBW$KSXf9ZF19fEVCy+DD0G@X6sIwq%UK_ z|9)~jlfS_4Nm3>Re;n^oUN_?q`D_fal3YxvfK!rkO|XG=lR8W&DRC>SIJzI}YiKcI zIA|iy4UUX?#+vcoVnIU#nE5d$-a?46qC^@UCz7B|=>e5+N|H3$??6CONCIMCLXl+;)M<8aVnj!dTStjT)asI#WSj+< z(t&^umjMqr+9ez|DdvJVLBxynDf1dtiUQv`eXEh9njZ{w(=6cJi*)HAhkQBE#bcZZ zl;t#9`jO`pkGP&{xg>cx|8%}Rod1hlZvXXoj6jy?17QhRp<>IZtK;)4O#=Ud?f^pS zkMw4D;T@3TTuE9v9$MiSXSP`9Ba~JL=V1zhAOncs`Z&O>gE^duGHa-p`O#tVvxs~0 z5)M-c0l_R4@O>W%LqsU`!;`~|D9LH0QT+sug9x(w5$=X?S)9}4xyndEAcoo4o^h2L zqPrBUGT?`pCl^<%)*&oqNd7qXCs4)L{Hi;LCefxg!Zgk#}MppxguA= zWtOi+|Fx|WItf7D+ zI4IG}Q$<{;N7x^hNmAjc`KM{}9YqZ3diFMfMX`KwKcQLub-b{(@W?bi-OlS}5XyNo@H5`rRGS~_Yhws{>ZBQ7rDFPFi zW*VixQyzU#)+i1l4$#jqU*n-f9ROfgij{1<{T_+)pz6FWH8o-2-BW&4Gvm|M_de-+ zjt|Xp9Vh(5eJG~qIz9-i%famdCW1;L!gsF(H;^};4YQV15O<(T6;=k4G=>W2>YB(2dSa6QTjFoZjzA4hzd<6`As#{*& ztiAp1whvA-nvdAb=@nsglh__GvVkesJrrg6+YNf+y{=5*%oda?P8aqC8MS-2prJc9f z5dSMK?5FDx8el|?$&fR9w&qBdW^jbH#v()(72DxbrJO9kzka7CS|lWkh^lP{Ng#gJ zud};RIhkY=VZe!RCsIwsyN@y--M}gL^X}6s=tb{~^Wi}Ie;Oe(h?pql10!W+Gv^jj zd59-RyKBh62GSXjdBZZLIsh1Uy{|@$y6r@=8KJn8qY*l(5g`O2P}e;c)Pbu=gHYxl zB3#I^^NIrR_X%M`s$pqL4}s(hoA4i?E~VJKrcC3X&;*d5!vu$;%DKfC^60>Gju~?L z4QqFeRjDE;LV1sgs!gQQCG&B8u*)|C1>6RD&Wz6FRSKY)K`@ht6akzd-nc0>#ms1{ zSaOjw{pjl<`L+8TaFj{VdL?rs3;<6%D51df9n^{!Nx^alz=nB zQ<8(A6%PFkod2$>+YAqUfB}!t94JK#INX^A8>&P7#hs{kz5^};9RzYQlz=82R7mp= z0p>~$KP7v{jN&7{VLt9DyJRDUa6Y>YDog7 zcOG-QZrwg?eA8yDO8b6WX4tzLyb&RnPxcpPB`i!L5de4?9<8-{dKFIYks0=}sZ*&u z+*gXN5{}Y`#>_08o0sI_Nu|y<6QE^Ua~zMGuzC8^bICD~2oOW;d%e*!cPnQ{9t=KA zIn!fF?`SYcb>jnf&u>;TUYi7iO02#OJeBM zZ&P;@6%QA)$s3OyoX^DQ|9D{#FO`-%XUuq+@U&4G{dt*eFQb2Z@oA@@c6l2Shbg9y+j@p%#><*=@9rb95 zd~$E$Tm?{<76GP;9HIT_T1=c;Qs-SVP52sp-#?I2Tq-6LX3O?Tms3+sH5_2@)ct(0 zlZH47h%X2J4ZzRea=1csFqpy6HZQ&gj%*caA`ROlun8Yrmd++c_Ku&pmL*F6oR?Qf zs{o|Mc>d!Lz`7ukTD%sI|E2ObF;Z57+=T4j_jOkUYQ_s5;+hA~hm1%V7=2i?a`U1M zT}?X6=4S3@-pqDq*{@O4%acHs2B;Y5l=XBn83@VRpj2F^lx znH=Z;4sDRzUL=If69V|TFJ2~YF)rb@Q#I@jjRaxE&qoTu?bxU9GvMI<@49sY#wNYF4QCjV{ z;+E3N1W$b?yB#U;Z3aRMhLMuJBBO;_bvw|3iECQnbjAP^q)5j)kZinLc#XM>y6t}BS?Xsv9t?p(@WsX{@ zL0p1=WQHKHk~oegE&$GBHPVz}?n1-SV;z$(e`_hHH4q3xpTA!^ zzfPWQ*gn(-6E2-Nbd$q}!12f?@nO8+2dRjqjBehwsCRh8oAYkj9VE3MbpDVSC}x%F zWh1QGP8wWt2rW);0*jE2_+^rE6n!e5Q8bHx{RDdmXteYOvl?C9BnML>j>l%PzMR-!rI zD}!WC9}oZqNwnNhzrOWyZMSy~>Z0Gh6lf4^tIY()N(*o4 zFclzDbQ?bEb4u=~qAbL#c(;u?HPO*No6KT~DospeI}CTYn1p5RoZeus7lLqCT+i4x z?5JH@&q2$%4b;OB&9MF6vj93}i;zYF4pGoKD3HYy&qL32?XW^H(Q8bh7{n)@Z0%T$ z5aBL7)7B8KFx$P3z-U*c-J2j+Zut#mrhH@3U@q8wn97hH5o`1gjaL=IMNhtxmGdDw zEY*Ec@o}g+P_m3b)r3OpRk$bQK(H9hqArNW+6DH(X6*8K!L6@PF#(}F$%Zkdj5D;N z48*7!;mMxT{j?tH#*wCCLW77aE==Z7#t9xQ@sja>aEnfJXm|@G`j;}#i0O&SfrBoa z8q7F*3YEmOl)>!=_lp%BIIHoV>6wX3vvQo-aYU|I!4hznG6gIyay{3(MUDJ>_GMVC z;bHX%W;g7cI&yzVtO2^QDo&dYaxC5!vckg;UMx~%VQfv&LWvNC-UKdokys;3{MDJ0 z*`z8YqFm;8(mPYpj`W6`KPzR};G;$YlwVePlmi`)2sVnaMk14++r1D-4A0Dg1m|2* z*vgn!6A6aY39M)Xgt7zHM7)U4N<9hEDxY+9#=&-n)2E;4kPACownJdHrU1KA9Spf> zR%uB6g%X_I8c>dXS7B3Zs^hg54`v8cJ0Qa20ur&2Z4h17w|i}0YqoqE^J&xuOQ)d! z+u5;W_hPEUeiLhG3S~1K)E$+DjEu0b^nk86;#8?9=VECebaq>ZYm1kA(D}C}sP3Eo zU!?|KNTdib7w1hfCL@~9Nf^)U0XgQa{X1#7BhATvFMwy9jK32;-U2uMUD*u{9#dje z)k+ujQ-s2)b7zKQ-Nv(xgM-Yrn-1X>$KCmw@}~91yo1_)>Up4Ui886!4;rFmU=DD_ z2`xC7l^u+YWEZ**VyGx(CvvLeX7sV1v-7nxmy!M6=&4hyQu~iBr*Vm~Ppnc4U5};DbyijqR5NcO z)^L4gTHu-l8>mMDjH&flW)%a53N@(NF?~0<1%?DwT4~;L@%eV`930k7(hT$!7i7)z zf(oqBH;&>5vrvUc>)x0t6_J&G-h z0eTQDNSFVGjnuwzc_G&2FRs@82|eqhZFd5K4djQV(;)%KjP`<(q?q?9$x531$9JLE z!_g7&F3}_xYtF;UVvug6m%vhKM(dU)iChD`Hpvpw`Gj34K#H&_- zJ)0kEX-LZWj2el{sv-H%RRO=YSNW(;fKNwn6XnLs7GqrS)=uf@nt)l>?#o4{M3?xh zwf>G;mC?Voz4kVru3f%|>yP=WwmF@;-rY@I^sMLYsq0{g{(9@9f0=3m)fQv%6-vl& z5ds{6Ww&cnjXGWq=QMR1rX{*UA%#-%Mtgc!l4312!otPlK@1r8pew?16xfXDfbOo^ zIst@~u=SUDXp!nAZ#XKi{l3@xD}G@wfz# zJuVa@61?@ea$tL6nNEmIIbE37e;ImsOXj}h)J}y+;VFyJ$+V->Hb?^r{S^*FDI#?% zPw^|KFomGmK^-+@!S<+VX_`c=fI|mjE!!z&OobQ-n4U~^63@tuE26rEe2*0+8lHhW zx_fG5W8AN-vwuuWr!IW+W)*Ppo7FfKTtHM9EL(q{%YpPhGfzFd0!?kud#=y9+L)ww z7`@ib`uGEaP(G@|x$o}79QYu*69whWp*`?Epf+CbZR99J{)%1N07f$~cU5yi z@T2Rn-=JYxKEjOq!3THdexJ*s_UGY%7IJ#fmOilr+GgxD*=$b{fQjVS@E`aOTSwRaup^;P}eXq~S_V zDvT5758msC3tQIYwz^}2wRj--1X~YjybuxhJ?*&x7%J8XPrp6xXbS!< z%WZgt}FIFKTErK-Wvd01u{~{4&m4 z=D~9~D53rgRybg?_+(pjX?QP9;q`sjw+rIA^Y!%oyO!$>1E%g{GAVham)d>Duqag! zzsjj)+r1Z0MS8u6i-&wkga5eiYjZZoXD3oD&c3M+7nh>{SNp$UYn-VZ%lnHH*;u1v zKD8G#_VU=metV=Cz$Xk%wqBoKy{yvm6c$st`v-jvmDez=6mo>2xIsoS+TyS4%j3w# zt@$eZKpBy>rUq1_9bm$_LO7!^TS z2x$V{0jq)3ogQf*6c7{71!M@Dx?I{ZPdB%8C{=cCuBIVGKp^xi0s>CG)ZGlj!Pr+Y z0g8#iAi@KI`OXH~Vvg=Xk??@$oK8?QV#rH7#>ydg7V@%0nR1|wNK~BjuspBFyfr2x zA5v}{o{SPT0WeTDG@5w!+^1M}gQc(z8FqLUR$26ay7E8Dxjqva_MD=x^rqab${~Gg zhT92&rbRA1fJN=|9=pHASyB+25ks~4Pkq5f%7e(#IE;h8#h;DoCm^TcXKdPiZZAwm z$hfOlLfhW^{XLrOg@S-nGXZ-=gtL&3oBd7Doq^lp#P-^&=DaAoONEh>u)iH=@9_ys z9%tqR>Yg?Q0CO(;xdWFljBUbL!9`pG;9wKlZSyB`9*7(QEE2Qn;xUdGh{n*rkLWv} zu-PUO7cQ`Yrlt#?Kyt}8Fl4$%)2xX_;j{T49}qljN5p8pr*ReQu>G&a2=@) zPMk)n>^3+AGgpqN9ZN+!{1l<%?RTOeKIvgXP#2!F`qYG*`9(+~C3z43ptl3CML>b$ zP5xV3wE~4)9o%do)_s#6v<7NMB?-nP<(-JQjx}_|UNI3an0+fbY``uy$TVYv&iEXR z$=+!jCh4K{%tK1swqTOn!aSoy4ihY%6b;S#!$E`|3J;1VBm$i^X&xvam2=;Bp-@%jt*!hShViE-E+PNYFWXdDkjR>d+HI@ZPwM^-5EO^2mkog}{I7AwDB48qo_%2nG-HjCtxYTP<*8RX7685VSdv z@H0OW_=4A;Pyal0CrSpuic)f4-Nttf)Cd6x&|}T$gC91~aKdO9S#+)?(Sf&4t@!m^ zLA9?yxsRjCYI)^AZSEa&OuDcgg)Rf^%cv%x?s)(GYTbbySh!ohLx2euw@1c6!zk4E zIUzzm2?(k{MjN19bIj8+evaCx8yie1y%Wp*C|)#Nn&zs|7C-@7$P0=a@@?DBx&7yO zppa?#J!NK66e)?J^Y=!@m<{+Oy$*8|4wpSIP+wL{dY%ov9~XCv*LJ<~dnOpXFa5lu z%(u7?k<6RmUZ^xpv0-jxk1qP-*hGq>K=re9GSY*S`rEr>9_mn-=NIca*hI%*b;S@R zfK*@Do7jy_=XQ52#lo}^x(wZtEcG1_WOjlN+EB`$tm3cO# z+wDBo`kC^D{LZ<{-_-rrs?MdiSF9up!jr6gc{^b2)225-HoC(UBU^w5Yn{Q89kJgt zxZSy(u}Zr=tvX>Vp*L+($Jtaq$rOTp96)>NLTv5i#m#ob|$tOv?do=6u8?QA@XoPL+D~qVm zCTBKPmuj)}q@vRgdEL(rk1TmVfE8jWep2O**M1;=bZRT59w6^>5&3)`>2^j9K?T`( z0_UT8f&jkSc2c${l!G>PNj&){@8NP~g-3tY)36PY!B39ki<)ZX`erR;g`uo9n#V6&z7_B}c@Bjnp@RsdhcjEx59uH4y>MFx=*p0hy3x2bbxcF6PnZf)(8 zGad{7M1}#)%@;7kY}nj*=)CFZ(!V;r6jhz)nRahlCT{`OnUFAKM6G8j6I2PDfn3<0 zVLIbsyKx@Fnke7rY2`!e(F(xViGf1Xcjm!{V7S_aSe>l@=&iZ)bKzh5es0>%yKVtQ z=bw!ohSdD`83d-cvM=PED#eq|>Sc#No&*P0ktsaA_^)+gO+4#&HBi?Sz0ear&J(3Y+u@xl)tu9SikNh=q_pE=IIWiR$!U#Gp|K3vNka$mgDw%fBy9&Pqg8!-QSxPB1+=Rk3LLh$v52_* zwS{t`VMLDe6%;@#m9qddI?%SEg;*jM`7=Z|s_-UN4}CE;KfNIpbwTv$ZX(1WDQ5X} zc@3MM!jmv+u4ye(4QqJytzo+8&M61Xg1o!Sxw!+<)3N`u!J4Ty;ZaMbTg_CRF9Q=< z;!HsrAsCCIjf<@$-cKxiru|avFX!ykR3kFgM@-h0uFb$ye@_8kYTIa(E8Qc6>16+h zt#fM61lYE9Y}>YN+qP}1<1gyiwr$(CZFFp_lkDDWpS!be>KD{gHJ>rZ81IMtnuezS z)~knt3IY6)%j`!7R3FG<_~(IhkIrTkaEqGY@ntP9aBBx`W)xJ_!=|t^WF0mi2i3$L z_HbsXdB+y)==EP*sn?p1uH?yO6>HYs4nrP@T<2I@sZl!(Pwgg}3ienQCEd&)R$#hc z1*@TZm8}4sQWWfA=S$MvdcwqtXwn_;8^a6yW8T-~y0s{;h8ls~c;}kvFcWWG-SG<$ z8x+_eU(LcXEi8HrTCC&QXFCJrm)g`lsy^??X`S^?;0Ej42X}R#8Rd;w$vet1F8n#D zQzRQzJ7B^!E2Zz2o+2FhUGsiq@Sk9^!O6OZ;bVZkSPrqudZ{z-Qv+#zn@i7y(`k<` zYmF^s9~^MNEwxtQgXi_hO{F>>nePIp{dSrK7ue1P&c6c6)6Y^Btzy*1a$`wt)f2w8e$O)v?ELeixooDW8b8`r)B)@I-;1jA}N{oI$fg@Nz( zkvRajx*3v&eAQ9h7aYKdl4XN1B^=8KFpR?D{CD*Q7Gg)2>FkH`lCX;8IPR>umn-ld z{VL~v{w-{!MzP{1n6fdx5$yVt2i|M5-8C7wefNk7yz>gCEy!HCJD z;{PbX99;htV6>!yO`0UDa5?}#tEY?9Mr!wE6c9Wv>(@;k?%VUn;YAEP>u9)YiE3+VpHyJ1^~+F%4EYFyK#`+>G?! zR|vFY>AIsp4^JZ1SSGDVxi9_^w4;#6*jv6EpJ^mMm}*r8EcYhxBrzHMJnBbyB;cOH zaxK7Dj2jIOfkn8=zq**Pu?qOOK4CAR=zd)M1YVMCTQ8rpX;!j=5SoR=vt*1jv2SQu zCt5985n+D@{Bc7&$JYaaDrL;Y**0}ljenMY%@Z*7!ge>cPgGb4hh2uMsgF1nm*QRxL(nVB1FHhhA|i{iV7ztd#XZ7(~4yj@pHTU-Q zV%%W+acKKW<$(azx!hZ=M{mCCY9dvc~;$Qo`(t6r>FzIS7bB|;l6 zG!JJ8Z{aC;$wiTZ3nif(?Mzr5gVZIg+@GV6n5{}Ci>KrH)u(U=ZUQ1WXJTt&4PQP2 z_ueW09{qcdcl0}Py0c!KaE^MHcoI!x=B7B|Ll|%q_`zLnh(~!g;g840gLIdr&dj6g zCFGeMHwqNS#k#oaXsv6zP;r>1RDHW9uG_}H8hU9T$W7i~52Ya}NCD;^fBjW~zbp07 zs`PL62O1DU+aJ$kfvER|Yvx>c)M-xUHuU)y_|my3j!qgKMzS$qT#^tzMDwvg_$K&c z1PlN*rP1)Pd!^3D0A&(AkFeVzExVl`dy~o#aIq5`ziWjpw@Z0o5`N<3UFU9jnUITB z_qqT0IDCaY;bmeXvjVM(Q-SJOw(IEd{7IPAxI5ay{=K_Cd&7TjVUbTQ-jmYw3-hPz z4IL69DVhi{VNgM)=pi4+^%(GLWNBD9e-VZkWMm%ztwn}MG*P}gZ!Mz3 zh<`SEP}5dpG{CSgq*Fl6bi=>h@JRVXAmqI_*_&;ft=JqH z*Up&fXK;8Hfzxm5v_OCIARAuw*Cgq;!B4g{w&+#qoOe>-;zCJQsf-0q{D>RWogaV) z*aoWNv_#*7<@%dG=bZa8c>i+>wRImq;5{AA(8vo6g@`iD7!zxl6T%Zze&7LNjMGch z38D1R3i-O-4q-hae<#0Z9dDBEn<@J?Hsdwm&(>$?w>!6n^%oI(XRnq3J6Eo^W?AZdDt>=Zo8z{9~A|I9qs~H(%|v9bw;;P zzl58Ki?m<<(QsgDyY6s>TIc|H83= zQPtKMoDS+LBIFEmf;CzKQ#}D{68nfeFUKqe(Y;Oojm}4-NTDm*`S;I{`?x9o7-Za6 z($9o3dujoW9~*b@h1e)zginEp?Of-$Y&lca_ROZ`1R@0ZU?+{6H(4L`cAHwU$)^?< z^FhHycWKy_vVv-9fN?oLG_Y%05>^drA^k`i{9ZEeulNI6%m68wnE`;O^phy`HSykC z%c3&-8S{MC)bE@hPV6)CP6^6Pd3VQf(diu`= z{Jnd$#0TTqev|O*4^7j!0|D4H`PJnS(Qd(H`3Bl3bGqtwkG|O>{T$N8e(l>AjtEGg zT4*O2-_JkZr|WqlopOLP(@XhQJnk3jNA6d7#wdhOr7hyf5t9J-L#0Y0?sxKxuLA3Z z3rSA;q=dWX+sDk)mbBrO8|*xt2&qoh@0 zE4m;6JL+5NmbbN+ncaAxflq&8CN-lWw+75Bd7QzGAPdnXK~(@7Wwe2)FSazok}rvl zXBgQ{Pp^>i(c~c&&!z}J{e7qohz5=$l`<} zSQaZV?zo*e>QDi=ClldY$_%^aF!efOc0YqGEIc^puE5~32@&#v^mh$MAwaK4;aM)A z#!4%yvBBtL4q{&CdQiM3v&Lk?zrc^17YFROf#!RqVSUlSE6MoBKpq%rW=KC8jX?=G zNVNR?`A1OkXQ{kd1n!5+^Obb2MX65<^!#=wkjQ%^o8|`;ImJpeMkBg@={MRVgZPaf zLh2JZKiSJx7XuwdE+WQ3 zu3h@Pfv7R47bs{e1AXA zMXNnuqciCJwoVk1nq#K-85wgF>r~_yROig=7Z;>HM!a)6&Cctr!<5hynMPfi151K0 z392k~UOvn>8&*`&(j@#r{6u<{`;Y9)$^HL_%~`n8^5cPNl0Fg4u82wSUVI)?=h#!;!xaD9`pG$Cv`&K zQ#F2xcDyo9>K}J;$e+Di;(H5RWdep;pL3=xM4pvqu_pxPDyBiaI_@25wqCpKIaVwT zKi|Pm_IjJ?Q+76P5q$Ri2nB6kpLybU)ty6UT=eO$6|PtNiHMW;Hh1cBQ>HF3Mb=Aq zeedSX=Vo*1Wln-Hum=4)vzACM-_Fjd4~7uQ=3q$=Astn#OZn0k#S%5ojR7vI^HL8< zl+2_sl`g<3&}1I8#tbZlBvuL?r92FgG;gK7KW!?#TZ0*%l zS=HW+zY;L{CuILgxK2O&x(Y3aB#?R9z!-@llJd1$^9v78D!jEyDB-c~Bs|c3kSNSX zQRJ(vDYod%C+b19SKkM8sxxADxEl<>&tqqD* z*G9dZ&*$i?4)3qI;}cN5-?vYUUM~eUsp`6WVF2XAix$#3*3{i(-T_+uL>M8b1YXy5<$w=Scc=3cUAA|Vn2iRW-vkYAAm6IJ!;EXU> zg;+l9R}-BIms?WINYU;C!)8?$r+dlY?{u)`A^yzozeDBNS7XNi?0k$WVxF)Y`7deo z-d5}ihNw?lueM_ag#kSJc(-p6yI``7B+U1=oDUmwuIN$-zx)XeoBy>p%XkTtO4rV9 zWk35Zeh7s7oc$f;{heWTlWj=W6y=d<+}QdQ z8|Ar8=l4T{@>U)xfvFB1M~AHKYB~0)#~F5avJA8N;oSA@{see-9+nrABLr{@iREa% zPZiHXRV97sAdby>as|$hS%6l$64btlAhsEah!OHPXeQZ?XJp6FgQoKRx(>ZfulEFR%C)YS9 z1mxHq72JnLcmP04uv8qgT5E2zN3|wToPE~QG-@W@ujpP4RzA+V&6OBjWd8BAtVI7X z>i%>&@StIM%UE?bfrd?kaFrKEHRTX`s2^ty6o8?ug%kG+u4Ek_^uiR0|6N-kFja7a zn5Ks0K^!m1(O?L9jw42^y&O{r?P(O9B8Q5?tK?f$6aWwwNBS}v_OK&1yR((<6j`P{ms3K{RhL0u$L$q){nN#0N&Y zb|RBV4v<^ku$L@S1c~5vdKD8lP~)&kB>;WKrH)}p$t*o383jh@!sc^eECqMCfCNHO zqJCy=Lj$igAv8Kn@l*X=l;8kSzlu}8Q)zTOplIG`qhHb1f0}LBv|B~8Tc`mJ(RkAS zI(R@g^9vH@W8yUI+yfFs=xB!;c@5=tst6vHiHByKKJ5M^Be~W#bl8l$e2q#K*Em6P$j@$Zl+id zhM7vJPez1uGAbLCoxU0)5eU;Tj0ZraDiR}x(pLfES+wP(8CE1}WSl4S13)cFYh{9p zJBJ-ClFJqZ6Y?mSc^3te5<0hr;l=>cV=K{=CR%MvBnenk>cmpX?COEf>Za=k1$TQ2%2O)UI@#^0lGZ^N`iTo|q3! zaJzcl82+=}%aq>Kfd;9FU_LdC6lN(Ci+xyB&y*F* zI0Cy)-xv|Mh(l$&990l=plZ!eQyYP^e-Em4dc0l(*nb^mPR`tC03!F7z7%O$BnV~A zb!rG|Pi>=CG&KWypSWTX-nkDjnlN}OBIGK0phAa5?7=ulpE@cLTFUhnZ6|r;HuD*O zbj|Ur@guHoD=zyHNqIJ3g=#t?F-h5K#4;x;Rm)cp z`@)ew5nLX{&0>h2jr-dKOLc2E{vek3R&QqrV2~u&5_Wb;Loss+q<28jWNe}UvkM5qk>u|< z^n~QWU`Kx2G)h<=Y5t?ThO3}SSR!HX3LcVY@D$1R8~D>qUY=QK7EOb6z5esMDktZQ z(c*AU3S{Fo`*w8h4wiEVo(rXaf%6R)l-;h2q&{J60K(Y4Q!Up3_YAeVr5EJ>vf+tm zib5hpNgkm>e@X96JcE3IU$&vU=&7Fb_#d5fV>iG>ZSbfp$jyUwxDgnMI1m5K?CX8` z=>)8;8V#Rwv|c-DAnY%G*r8GgH>;|WHFC2|tf3E91R*>`N2(~NZ^4UZYm)zm0x>tX zPx>r{#^m}hG!PbM&a{GfVCp32TnP|OmZXkcKH!I>m)ta9`J~vq2*4?bCms+YudBRJ z3Yzha*EcRYNob9PSOPLTW5Eduo^b&rG^m(e1{z-lL;<2fP{>rho7>z!ZW0!_z$m-1 z)(l&NS;wb?&+BOpGq8|UIm)U>Et{t>`TPCN+MaV`7y#r|nMVD=N<0o)RSMZuE2X>Z zb8d$=$pAyh2Hv6s8juFV`~en^!rUT8M0OU+(w_)&Kn*Ngj_P-$opAkHa(@Q0vX8i<#)fh)4K@|kW6 z6~^~Yml`dC=!|Jgo#)Y@9H$x~<-Ih7MLpGlOQ3)Zj3U%O2v}cKxfKrrMVD7YrlK}t zMhz|`!*8UeBH!r4Zq1kHP9%mG01_7FH*MtV;DS(%UZVnO$}XoR^lZ^xJRGBQ^e{iM zcAm~J^bL0oRDysukvj~&)s7#grr6gxc^zi%Q$bQpnjvFXCTk{@BHKtN&H6OA3F*{b zMPx2P4;V?~0fcS!(_&|&KQlKDR^=CsTX`xu%X!?PuysO6m~5Vgd2XQC$@L4o5wCE1 zWjv4gLp^@iGyS;TAjFgEJzKvn%z%r=yI_EO0aqCRq(F~<7cxsSAyYjUBNxLR^?bg$p32L1-t!xe1(aRrC`$a6-}boK8f24^6FPC7 z8&q>nH(33RJHCBX*T}nd2z~2MO7$4bW(V;*&$##FR_7ok&wE@>0pEudtDtV@QOj(n zgFaS#=NI!Cc5}LS;r<@7QQ+D3!3Aj{7*_ual|DI|OXhS4yfW5PepumiCAzkMyV7h6 z867x{D*(Hdtb~z#{^WYfw|Mi$G+PFTdEdK!(7sA*x{`_7iY!o<^}w-qxFD3CKRN7F zEv77Lj-!xQ(xt+uI4)4G6tq;*2U`>GuSJ=a9`j_-P9YLAD(~ZtuSBsK813{L&O$OB zrv`D_8`?u2m`3_Dv?tDB?u0RUG%0;=K-&U4831G$BE2M^QbnoLE1Xx~4NDD(AiH9# zd~r^kT}Qn@xPSh`o=U=-s-NM)#267)^?@6&3+fl$$w!8fInpM_R~F{WCg1zTvYY=f z+PE;8P6Sz^$#PA!2aeXIHgVWA%`%cx)PuCJ#>}NxE`Ah;5ArdJxJY7I5b;GTIUN#9 z8$hWdbDOgZBFmC!Fr!=jFBTf@b4Pe~X5$U9A}fRa9)5%d!VJ1HtQp3oNWq_?aZP59 zp@^`bzFA8%;{FL$Svl+D+jQmu996B*m^3k&Xo@LDlS?}lhO3cyTJ668m^se-!j1KF#)lE8Rm{66)dLfjguWk>BQIqB=Yf&+cAV>%?~{0en8KD zd~msL8Qgj{cY+(BJcXph$Eh?&-?bIoP8mMH z{0pSB;bgcRE`W%SUArq{60jg9oIqxx{r+z|<ZYo`VlMugVczPp5^_~o~$ULKS6UM8%kunJ$oO3XJpaEx6djR@3PB* zICk|%penFlK0K=W-~p;5lP_E&e$>42Abz$7Aq0L8_8U0BNy_w}l!W%5>I+a7Cf1~d zL~6i{&V&Q5BvQ{M?b#L*(pC>6vu}7H)f9CEG+6!dP&iITl9Mx6k$$cV|_V!M{cJZK-CRV>1?VP&ASon;#Z6kBS!Q^oW(W;79u3#2Sl@XHOAu1%Xpj zn#7+`F|v)xA2o+DFiP7Wa$U12J9UgB#U;R=!5-G8K^jv~b0Xv8? zyrGa;XDF+=<;|{ksN9~X_eb&%N}J@Y5-9z7OVfF&ALj` zYV&3KJZhyeOgB#finAO~Bv=a^_A>@79y=6N+1y|Yu}|QOg%RgBybyo0=8-U=35-fO zh%y@ti4a&anPU-xG?IhSyHaooJue`2baGTJh8K^!C`9?1jf^5q3=S9e_o_(L1s1mk!$PVpPJ%2c$%wYgMoPx9A(1U{XMZ7orzIdU21`XO z49rfg+dOinEQ92M9^=})SRnHU(qC&+znvahn4iBcwhU-X+eF#w=iQ2`PB2Y^4SV_ub!kQ@_wz?B#3QroD_F1 zPn>a_^m1&Op2%y^leJJzYD7Tx>BNK`D~Gr^E>>Hzn6!@w@FQIHx}0BRS!A~~-Mr^u zhKxTv%@#yViTHi{G!1*Hx#IN{9X$ajL*29DiSW2Zy3WMuzrNyVoBxKK z^XUfHeK4ME+J*+s3B>@|iBX_cnEeH^f-zl_(9T^CBnRGi#(4zSCj43_vNEgfQu_Q0 zi}p^yRTnnA!QB$dV1JH)C{=-r_ud{vv-{DEeWbBv&?*oUSTG)ls9{YZq_qtalj(o6 zvpW(r=k4jHq|ANeYLQ?BP=)BO2v!paTy33wGnuuh+`F$ck2e5jFrq#%!wBn<6Vt&w z5S8Yp24DHSjj=m^g^P%!-{5k-!+;DvYQcm#7A1Arr3!3H%@)5PH3s{oH_fdfE_{O~ z)R)!fT-BZniJd2tituvO%W`r!?P(k|!No<{wR$i>AGxto@Vx)EIx^v!Wv0A4NW>9S z*W_JI!g`JxjvWI~g}8Wl1HkdZSD_=~h8zfgnd#trS?I6W_c;tZ1^2Axm!^JBnKA>k z9JAHVPj-K_GAg4?E?rQVeRev!OaA-HmfBFjG5Kq( zer5b@J3KU2<8)ZZ^F9+!HE;h>H2&L8^~eZ&hf+2jP9{MF@@`R9@Y3kYu0PV%4v?b2 zN&Q716t@6q0m~P8cJEpGQ)THJS!@n>h|{vJ5qh15cL8tBTIg-EOp{L$O}3)%*HWdl zQzWps&h0J3c)-c~N8#^@(bej7cT^5ZvDtp*mh^qMo<%^(21D%)YuCu6w~LSwTRtro z>uFKpq&6b$0rjX`yqU|Y;{bf)wre(>x$1r|_xL2h@Q7?($8;RS49e-tElg>%b!5vb zYZ!reStXXqcA=?=IS35l86s{Iw|sFEH<1y?V3c{!1CxP%FLsn}sTckYMopGlwvt7s zD=S99;EtUF`8Q98w0F@UQZO7jr8KI1E~UtCkQVY~2hTJgrsF0IMzAS<=bNBa}R+q@c`(`23FAvBlGvNfKYFy2war~G*T5ETN zvis3)uG>=5lh9+zMp{kqv;Wf zAcq87g!b5E?hbs1^>2Z|J(|paCxoRcgOEXARCbC1M4?8CCuD&M>O)9FAT5zuN zti9|NXiopO0J<}{!t%9pKjv~}3#fkeixvZ5I zT^D*k_ARe@pFVkC*a9JAxzbWf=M;A@yH3X* z7U{`An)6xoIL*8WvO>&pu&KT5kr8Ncl{V-Hsk@-Ln5THFc8lRG35AJ#d!-Hpi13Rn z?VIe_F6jWwP^>FFxRghjDl_5G&Lz|TzJQQZmKNA{G$vtD0@LCK0l{_CIhIpx_CxJM z{VIyCmy|2FR9p8t+aKnCTxXaOX`3?qDJF5OsQIR`Fc-~I4>Pm`jSurU|LPI7C|(Tm zNOyf&9;V~{qCo1ttM3M}3Xx=2A-{eM3M5r~_CkO<`$z3|%?a3?e;dGP=%k|>Y<>HE zu1dUZ6+WdRuUanD)2bOU%H%e;d}kUr%mctU@ZJnP()Oq~=P!%F6pMyoxx$z^i&XTa zH8UR`PXUIb-p^*D!H~cDar~IYu*fc zWdYb&Q~JdOJjeL;9uxiAHS@SCh%swTorUUIT3V6)%bM5NT44s?0WJNLX9i5`vc*xw z2Z~JV=;!*hy*gN|OTS~!VRgz6JqGvZ{1IZZjdnp!s@Ui>|H)f6GWFb^v+9``y8}*I z8f1c4hPaCiw(ald2Bu2patbL7elQhfjTE2*<+&7DLPhvrTop9BTPMIVHC9M_i>I&u zbVl@_JO5DP>heO+R`IVM9mlOEJC*>tP@@3HTXK8?$hPuy(Ov8iI4u6r^_Pu;c*@C& z^{rru!gz_qWFjvOas}FH$5tvZYoa;@7tQQEopasTZI%qF+UD8W-BT)* zT1bp^FHTwm2r_C^J*@%-^j>mY>o;=i;9WgcBa;6<4#u30qAEQ-37F4QX$Gcn^Lq0MXpp&|_s0pF9q)J5J zdP&tO0vvP})S5V85&O3sr(>Ux=Aq~#UIu!YQYBBirVG17z(%HP+s-rr3+Q{jT$S2t zr`3QWii9YaLRoFhE>HGrWRZ7L3L)35b?X|#49Few<~Yn!5ptW8R;B^5j1{ogh0eYZ zT5~0wb`A`i%&Yqzcujja*CB5Wk=2atBNxXWZL|GVtG!SfwnsuKC2ZlIReDEu&30QY zv$>$Egy$B;4D{iF9H1?$ft&q6Fute+NjLsjf7np2iMpNb@o0PmmLi_juLjhB^Os5l zK%knaG)}(_2Bko2LFo?$1qwj2+n}Pz=O<||G66%_hqC?F7>QG0i)KEGH{+ZY!ydzd9i?fahn1xy_Bac1tTn^D8 zSmF+C4i{m+vFi+u=>zf?83F0^kc&fU7YT6-H1nl5VW`bf)>;i&;ypRT48E^y) zMYo(K%4f|&019-C6Ae;*LZk9v;#K6>kmB_$gjr4y9z^IAB7H|F<)%Ac{$A;l)^%yD~X^#*fw15IiV$Nv}xlu)RV<Y`?rmL3bLz5kfDw0mxA);x(am$hFQ zUyA#|hv!#(hA4Y5T?Oq%0D(bSLsq3t^kz!IeWkwvOurgOGpV_k$8{`Ox(cvIb&M41 zm3JAgWa$K`8>Y2P8ZgNo zRW8TXlrPw$&HDGRyAIb`NGmn$%LbfzSDBm@n*-jV#5;!|=KP2C*%o`2wl>u2&YZW` z$M1t#gg+czrVY9hnkap5C;pQ;!@7`_)IcFB6gby)o1v{Oh{n13Zk^?q{z1#y?pfW? zIM-irBqzUn3;}w!1%H2mdmlBNwBF8E^^~8XN5?=*s0RJ+y#4N~Z8>0DAJyiJD?oJE^9`8IU2D_p^2=yXhwECAqeuI%pJoIepPW#0s4`?-kr z7HWJases+>3%sXy4gQ`U>VCS&Qt_dW?vx;l>oPSqf|~xZz7^%MmfK#A3DFB6XF|vm zKK^Wqjj_yOv=WZbBVNI3h$W>iizMaUK1)R1H{F`)m6L#OfI4MJrwR$+j!2Zk6#tV9 zOU%j^SOchJVVl%}s4N!hX|mZgm}oehFmUI@6KA@2hDiloheJ(bmsQlW#2!OX zBm^_(J{S~RI03YHoV@>S@Nyj@T~Z57Hb{L1G6%qy&t*lnmPN5pUM@ok%;}DH9urOX#CO3w699(m#F}1g2<`tv&kERagZspjh0Hix)hxT5w$; z2|-P&GAv*+ydthWm3NGBeWN# zpe2cr;@jXJ>uowvBt);z?{3o<1T=!VsV;n`>oYHFPT(Wx=G}FLi1rYPLX@cRO%wnS z*JWMTf5tZ~W~To#zHu?L{4a3HYx172zZYQ^Ap?9}Y_6EwTK3AnLK=c*=^L<{~5iQN6 z%~Yx?_cw*(ClV?Yrunc{N3pWvDY2q>@hbdH4F*k@uw%{W#-1FP-VKuIs=> z9%`ZS+aYf!U(coIsc-yi&Md+vX*A}k z9QziSImA?C$9>q#2<1x)h16s6@(+6zfv>9gssW>N(_ti_cJQ8{!}d#uxll{y&LjE= z*#a}wt4ij)m|}*3g?Ad1Ri_!C!M~oK!78g(uy*VT7tV*#U9-yGj4?n|ok`Lg!hqQy zxNqsBLXOcUEK47IAn;^GiO&i&1bTe}g0)7gYDAE~k$j$uVlhknJI~B)fX%`;Hvth2 z(|&uG!Tx=JvwY0+jmMtHr?jYCC6+s@1eMmTqgc{=ylF^x_PA6^=~DyXARPOF1{mH> zI+v+o`Pbo*9s=BSEiSf z9B9(qfGn-X3A=@8x{?99Q3iP}%*khtyaA$KDkSG?tv_;eMaVHD;kS=$rN0B`0;o{B zw9e`X=qVMz{K5G4lx%3Mv^UNZUV5R>k$p5{%B>{@#Fv@SMEoQRRfeIvE5iIpfWc?s z7x95Qm_}!~tTBGCvmTNs@X1NI{D7P<%DuK{pExsLIP?EIZvf6pGp$1t?Z4z5BqbGK z=e-Ldrow~EiHpQp{KW=a7zb=vIH?WB-#LS(Xf6C&D}1So$t;H~7I>`Ty2^bG6i@0? zW5X@%#!w5HCz*70-z7yT(pRe-uver(2#^5n3S-)ttf(%Q;0G7<1pTenW!QoA4y^lW zE*BVSMBEZB9)Mv=l#8NQHC_(_YeCyTGLJ!K*R+53TIqFCFf#7;`N}dEhfjpL@qM#a zDiV-nEc6em3^lW=5;x7k%O-H(&pq5V7jrPKyR@@q4VXj{fr#e0gs}TMDy{Nbnn|;r z6seh@N3ST4y?nZHXZ<_)K0PsvvArn<0`tgtuqY1RssLS0oVT8h!+HLqDMk0srW|(+ z$}k_2mfIA4d0&@Tr|W_?x-w45H;WXTwxjw>YOciY=LhpO(Rem4PVx++Bx}nOLm{yU zMhBVOC&|d$bn{(4>|(XPcx1TzHM45h2IHPN6|v@&0f_e>(u2OX(u0`Ng9-{L42md+ z>QL)i7JxYC;^nKL#+_aQb6{)&Jp*y}NcdRU^_<0Qg$)RWjzBlbJVl&3mo>Xu@O}(tM6gR6LQs1&Ib~upjow0W?CY; zC7K`SDZNi8ItR;8J3>4fRSn(AQQSd(VjGR!s^;-Y2{@AB&wPxGFwJ4B!eFfCue|v0 zya3Qsao3jok$JKGa|}2ZSH&fIbIdB?+bsm?Ef77liotrqzK%;(SWBKqJ_r-trabKf z1Y|a0Jk94x z)>4T4J_sjnyKyw|jW?m_PuSx$WVo8fO&)j`-SZm z7YxP#91Z#pK}uxb>oFvhiT-Nnmi~R?d@=}U6i7KkUR~KT`raybu~Ro>+fiWiO=Pi- z0_rMTw#JeDRNI0Gdh}E4vHDWO91n(a_ogrNcoAqFZtUKlI+G%z?^+!2dqSR{7C@Jw zehp?@KFAaapGxOxbvokVR;R>#=7~=uF*@RQ&#HFt5dIS<4^k2*S8lx8C&{Qn6c#TI zv$xBz8|TRI@}@%;6ebpmXkoV*-Xa{ELbLg5`oO4kzIy%P_IACjDSd)RCHIs@;olkeX4K7b@Y|MB8> z-;sHw)LAtHjWOTf`}(MoHuZhQ4frUN{=*jzz*(7kmv4V(x^>^??)5_j%fVq)mP_j~ z;k|**cU;$qpV>R-> zqh=_4caf-_X}IuICP08qd{hCe&YkiRL@=0-f*5#n`K>4(n6zxhrZr@$HRlGM>!bzL z?T!*`JGxx3GYA=bB>e6<(zyS(sKPO7_ zg_g_nUMdHp{J|}dW)xJN+9?1EuMA2JJq~J!^(c86;y#Ry9Y9c8GwG#sPubY=7-9&87R zIYkw=0x2@%w9M&J|7AiR0LHsxqV6surAFm@4~}jd_O71I&?m8-eAGT6%7y*!=7e*S znaVji0|`7o0eJeJ;S7D4jl`HV@IQBCVxBvuBk0OWM(V!920qp4tdui!5IZ|IktiFD zXlqY5^-vj_NuB)02@=a0NGv5%D!ozGED6S>op9OaqmHWdpS0hDx+yV=_&T} zW4O+SmS+g@^1)Pyzl7QbvU&ffZZiKj>pUy-|G9yxhLM1ylDdZp0CF%Ao`0%3g-<%c*ZH~g&?D9NE6(6`WPUGarw&7A?IN& zP>34ix^u6gpV@&cv6^($#0_k6ahX)q#7u>#Xj43gDj$cLR^zj>H7`rQ*G14?XU8Z{ zE;PZUxWskqx>$*#q@g+}#*Tnz>}sqNz;m@7H)_TlgX&bG z=27VzZVNqSSAG?b^xE>LcuJ~V(&HZxiWvt4nqmvRi#fO=3a+*CqSMMly~82fpT?&~ zK0P^3K{OL-7-9~|l1@7y%$Wh7a;0=zG9?YUm|C#kz}GhrBs~sUH)WqX8J;=RW!&gCj;SR&Sk_VQm?0 zKjJJeMl~%e2EuPvoDB?u<>J!;9d491uXU9bRz}r) zF(?WNDS($gQlkXc=t9_v!2~lYXOR{Pd^RVFgr^|3r5tJT#67m1`prF}n3Kg5VQa0Ih_polqqpm9%eVQA)0Lv^Dh*Gx*+}gA_u`Grim@wOKv7 z+H(<^`~C4>lM93Wxx!Tt5B>zOah?COItLzHMPz8{lR2ULg@zsiFz?)T@DJx;+h36kXG z2Q+FY=7OV$(%f=b5idN$a*<>UBCe4?YRstnhxTlDP+Xpqz2YngGRPbTka4ESxO`Ki0KxrQ91dc6`F3saaGecA0Uua)3#!Vs7#_leIh~c=558PHLFmxO%@(+@I+6f zp7|@dAx=>xl-wssB?lO9UWV>#4g8uC&X+7eTNhRdn)lJPJvigvG37bV<^<4FJSTNhR(8?C9AInYIiKu4zcb(Rz`MH(qksCaDn{O7r zQo|JlB;8P=jVv?&N~D&ruk42HIad+j!5X&djT*fAQDy-a3|r`^}~siiEUdG+qP}n&Oc5j&cwED+qP}n{&LRwF5j!Z=<4d~?x(Bv zv)10f1-E@exC6Asz-ex3Uaf74m#0}zn*$YKGUPf%Wujf$g94Uj)@c*R7TTsBdT5v& z{TA-pjZF&(Q)R$t)6A`hW00Y@Z8WNLo#8@B3hjfhRp?=DQ28FrOxZ5+J7ZV`TtImg zDV+Xv?X7`PP9SAcd9GGmH^S&RQ2U%FMF-bolu=<#5%wb;Ji9p0LSAyxteo16fd0ME z*YMjzA7_AgsE`#U}D~ zYC^>j99SBMz$fHFo->qxT@{XuQGmc)37!o-zaz={?&yc*QNNvHr1H`MV5+BS(P#q? z9U5qN7E`&ItgUz5{NruL2xJSgz_gtrs+9(!s3#gzZC->7dMZOH=nw!e=Juh;&JR9D zA051?q|&%Y8}TWnz#i9@rXZ!%ct|uvA>(zpha&E(6r!VmCx)zok7T2O({ngJZ~BOm zRpN}}H6PWsvZ!C!Qk4<_u;;Se>!{)#>Wn!t0D+P-`-)^IXkh|#+XmiuIA-(jRA9uz zM~xBi9=L=XsCV;!>CHuq!o>@8S(h@g_hrOuvNXTSKhp%rgh3ns~=iNsj%(=93DK`O2zdAz^8@4 zoS%LgZ|S^ac8D!JeLrcetK7xO!Es+VysU3Pjhbxx_sGTsrl zodK=sq*_U3cQJupHQtfUAJG4}P%`Yeb-G0yx-{RjrWyJGt#v$vq&#suFw(r{y61sFK&ZGoqnCtPweQPcv z%(Xx<%)@(#?ei_o+9Y)fH~VhXk`UtB1%nzY)9ch)GWx@$=-3PuQevtg9m`aK;kHT+ zD2UX|=S|GF_d@xp7Bt6ZxO23Wq#jyvdNn~%l^PCT;f%1dB&nCsPHD}-<29MKOF#^? zsc)D_xPPRN)dH?u9wR&8)Kzg2`MSlTe` z{Ob1HB@u06OScmp;vkOlB%E~&@s!l{cPvfaxk??axl#OuTe(=ad2iCuM|2zrWlOp{R!zdLAy(G@NKeS)3ug~Py<+^CogFLK}9Ql zmfZ%8oG3}mIrb7Ie2R`Jf5GjffCxr4*NN3>HM8z)4XwUDL{uPBREcO{x>-Y~r&H$W zq?^T1NEBGur=4H6M3Ox)6~vYKdegjFpXN|o=$VqR10=)}aD7Fjp3)Qu{m?C1fgLDH z`05p)G@xWvr6~KcOt2$%WL{tZd;$@;jj8;l65rAR1x1~ZSQI7F*gP;^AV`PS0~yo- zPX5G^nYDumYp3ks+fmXvVb3^l`(u$LtO8Wld+3ag7oY_d9z?V%bixkH%NODzoSCMVu35N-Y6O;}KuvXOOPwmQzIzOryrmKgCvc;=UhTwZS8pglf<7%}!egm` z-Ck_Nh=H9U5R4%aVegUVb4z?Pc36mDdXA+tIqF>GiheM#ZeZk=V+UaXqjV!i5i#0< zIrPducj&9RuApm5{=drx%t5RKgzyqL#&IqrU#jZJ@ik=8sa9ZChLBbj%7YR$Lm0uM z12DCsa-ogS{-~H>qGz;#kpSOeAstk}f}4d!WW0I8*itATjD5Mdq4m4iUCDgg4XQ8%Pq80`G!xX1^`W0!iC) z0~CJpF@ZY%l7uPvO?PVYojeh*ydr(iHd?C*MM=6#%q!Tf(CKh9xaqo2qW<1 znhE2ZPMW3*ytTo{9L%j2N>S`H*P?rT@uYaHKIZf?G!O`?V-=Mxi32Tq(z6y{?(dQ< zIED=n1m-yB;09%QSZv2Ap;?T)4ge!KQ5IKBx>RIwLG>cz7;V_9@@?e!Uydj&B7xq=}@c16!nu!*AAaPttdwJkUG)<}7 z21F8Qf8P~srF#}49OB~EJ8l8R3z28?H3^a zD2_(9s7nV)UMXlHRv8h7=SjNtgr`c_?C)U~3fc((-rk2rJ+N>k+MxaNJ6N-J0cr(7 z3L=%a`el8d=c>uOD(${+`^h;s%*d>|Syl|sQL&8Moef5m?#&zJ#WI=l(7i6?XL~5_ zpGRxp-td-uaQ&&SKcRw~GxAsOzb{BoVo73`?xsrp5cBU4<#^|)Q899Q7vE>Bv*eF` zkQ|%<1!M3Wjq)9pV$N!yy3imAWL9w!EtbQvzs;}CjA_Tpo<3c(ujafFY-@beq;|Or ztoPA7pURZz!(}_YP8joWc}Bum`4yH-gzeJd17j%}?|*pqjfxv-XGGeYY{wMaw67;~ z)HVbkLa`&X-K@hwpo{`p1Y{$(@r$Y|&dCP=+?|8bq7P$#2gJL$6~{~)DUf`kuy4>V z!}_w2A&3^)$ll0?@^|2jQTQo%D?&?$r8G-Hi;vptKL_cGcWr+7$F@&r*hf*0^mHv zcrbr?Rz@eee|robKz?I{xh?^wU}vVm@$H46TtRqk;IQ>BDLb%)pDD}Zh}GV))F=Iq z?Q5qPvjjmT5y}YP%9hxd!N_hhpJ450beVR@ZibUU=vkDN2A?#_e1OA4kQK~IOc&&t z#wJ@TbXG0IpYF}U{?jaDbOx2)p(|iRFP56Q>tCCoe8*E+4liHp>^1q493>_M0vgHzqgijD$8wfmQ#-UZF_sBsQaP}#C+Oqmbo)}*np z_)?0*3x{_!IKMP@HPE&G44R2)fi}sGHU<$MB9t5Nv^_MOyr_| zA5cHk#O+pxodPZdO@9jE)}~}SMi><`OTWzZ#J?ES-?FJHVeur42PcP~y}U_%7(urU zD59Z`+5lDpg^dnxW``nuENo-}zlVkTxdi>je#V_JTo=3B67%=F6*uJ)9tnFsvNgP& z5P^ko(0_0T&OU&mUy6%&8ilI%qN9iwx~HDDvQw8^Bj>ZMikeiJO5N zx+<>O;r#_i=>=FdBTeJ%>$I0T3~XXe>xGY)xY;JX?Nfl?OF3XlteyT5NEc-He50HvNEys9V4bDpfG$Ou*K$p#(w zLl{U}UY53%o8>g0$5F9S5<8M1!cL>+Z!F(|#43!Q1<(=%u-DpICpa>7U-_e5sv%;p ze@cAmJ2Kuv#6vYn{f4uaVOYP4;xg`v6i!%rX0*s{27me#qd$TZKj4DoP*`B=me+Di zSXEjG+seAWA&5o@2>ydO0sa$*KmpIj3QaF401I}z3yLyXx|EHz&|Oqo$lM!i$R-smM!pD&*fN*A-J4%D4%p_qtB0_Scf&}oHXd(O4N+b zosbITwJR_xp)OLw7?lj1t09BD3XNsN(_{az+C^ zDFSd4OHVknt(!o@8fHHG8tg&HZ-X#0DUk+$W`0=yg(=@Y)Txi$61AQes&i-q>>_oe}ccb0y_~yORW{X zb!)@bu^E^!YMD53`}rA|XlsMVI!K!9u$E{3Cfsni!_rCrL*!h?{!oTxmM?=A4QbNM zfhGlCWGLHRf_6cAssmGk$Osw&)+?n3gF{kRRNp_?Go0&q1>9G;OxB+|H>Lck#`o29 zp61(-24w!xo4(p^ezpUJ{YRI6opzmWw}ph)cUa)Ds7t@GUX~?rqFi6yl$gN1Iuv9` zJCM~H0khoUI@w&$#=Ok!rq|HTqfKjTs+8^XG3WER2=<)?AJpnON}xrjyCcZ{cY5Wk zpcj3Xki^-S1aJe|^~+OOP=x#G(~i*NYHJ9C@q>E#<9z#@vAakk_jmhg|K;VO0EdwZ zJ6(UOH~1=vnchx&$gc!Ho~3=m)P+!JrNp!HlbsaC(wILq!$`Fue`t~errTJyB37Me zD_>xo8f!wXBG7AiLyC2Hm0xr{y!D;x&O^gO>wGDuP5@or1Q-Fuk4V7njIIT={sGDKit{d0%VD>+;cPBKNvDSH>QzGZK>W z%eOH%GFkG3cgQ1m>p`{~>TR7jdk$U0@M+<7T8}>2PdD+o&(l|m|_QRm*xL30obFFC%JNEb`pU+m?j z=X$u>#IseW1G?U}eD%E5X|BFz|6H%H52|>vb+=~99DgB90Nl2xj=wygYIlEI3N3(H zba7)cgtHzL#<(*pmV`^5k#sl#*%p7r0MhmBt|nYRF20h7veJ$(KPRu%x3|~b>a(|L zc^j_RCzjBYY$?x%VC%k2eyrCz^t{W#=i9eRIWE{Xq2 z!Z~-7=9w(71P+p^9ck(cTR)I|HgOy52x^MQg?7X8K)zt#Zb2J9jpwZav*SZPpeJO1 zd#dx@-~YJQZ&~DN5az)f+?I9vvvS|*GwFqNu1MJo=%gaocLhp zPC{-3{gOK@_=(BbsXpc)G2P4OC!EP;f83TsRW!+n#rhbEG>iF5jQWtaJ{YtCR1~A_ zkcA>>>`VUn^lX3Arrm2vUR0F_AQJN0LKXalhNuPH)c-4r~Qc-;p9v<9D;MTeT7=R2BYWV9W zrK9Ve*kiiH&iM%9-Nrzy`qE^4nW&P_ae;3YHD@Mei(hN1K2#kSun{iOHntnz4{s%LBnViK zn@H$N;v^WcBl4WuKk3T?++QYg%s4-dZ%N)9f_7Ne~s3xfX~$>s+5}@^6pjZ zhr{Zr#Qyyk-kd&w6F-f@2lI@YNM`A%JOF}VE>;hA81Gkav11r?l5va@m*4EQ$C6;~ z_9sqEX!l+~0_Z{iCAfi|&<$zBOKvU}-RFlr6qQY(;5>Sy}g*2)S8P)=i zDzABWgs2D@Qd!~o!x6%PEzX1{e&~l!=KHU_GC)KId27={QK- zeB^(E60L)K03}M9R z;d!}o%ssZF;>4Zn%_=Dw^&JE$PU?*_E+R85HTV%Z!TVzu7zloqSrK)69~}a%uOtl~ z)jU6kcXl^Zu`Pyd^SyDszWH=%qNZa5-sPDKeA1>PWLP-h$%`il1k-dnY|u*L;{Lrq zN(`B)0!FezirIP>tY?v46Kh{Tf3C{jPCCCM+deq#M5ZbUkG=V@2A(+5emhPwX$_1r z4&6XTq(Vf*3j2eU*a0X6D-LQW5KkAi#8(8l^(v+4dPD&m&I^@Nhl>-JQk9CnTL`k3 zU`eHgMamGae|lKFtYvL_=)jB~&Xk7-1fh&E=HiFa{?)+y>T-)Xu+#zFRv{z{gCHimS5`Gtb!MdEJXn*$5yK6O8*k z%=ggM?3k?t1Ai~u8);m*zWQp}s(N)t2%YjVzYAC|Am4vfX|F}K;?L2gG0>M#(qUEj zEQrAz=UY)(T@My%YEN3J%ey8=X6lOpytT0*F+7AEJI9w=8~4>1YW(%PHC{0te;?;U z!x-!0U|khB&%6N^?LUVTc(Dfxd5`)lz7fgavV$Z=su)V8Vdxl08%%V(mD*u1+O zxGBo*U>O0o>IZJ+;ScgcMQedI7`Z@3kXFkPr+${rgbHuBKs}e?auQ+)elk7)0bMc4 zON~uI=(HjIp|_``v&LaG;lSTLz&-ZrS+i1M61s-mGd$QqC%W$Wyb11OdW!8}sKhcs zzTy!{DWciFR?bzZVYmTpa9B%=ZPAOGdhl+VV=)p+xdgmrhEo3eQbF=g8OE@nn@9(k zv#ypkiS&5Za!4F_f`4lHHQ3kz5C%5ls(xuP6cx~!TYB+0yj~QyaXZ(QyQRLG@27aSJ4*Zq&LR=dIM3R20c>rhgP&R} zrd@J4Xuo^*_Mo_^@a6n|m(i`d@4vczJQUQAaoZU_+ePDP_lq{#ZBH}@$6#&_w9tU- z;7(}o{4QOo2sa64rWH5>*j0z?=qCQXP=k=XoIbz!rRv)~_UWS)T$eq*ZtWDGKOUUd zMAF!>2({9Rugr4pCtbL4XvK#D+A~h*)QBxvv8mF7LtSEzTH{A+09?@vU~9kgJ*Q8H zP7eM@E4xvE0{%M|Ark!J1aw7JARX}Va>_HL#jhz zGQe!$S5HnjBLl!iix3A7f@rXQDO=DBDnr6%AB!$Xp(^I}xK~8?dCpvk;|Al#;CcS> z^k&ITlY?LUkP5a0+=+p#O6f_SdrnB5t7Dxa=efhjI0pW)R>h`S!X!}hCNpwlrmr!3;(5{@L zYcP6ZuO(Fa{E9?y%B%5-0%^6C+TKsd#T97GpOzJ&@6e{t&#x+7bll460lrvt4#cG@ zp%wo=c0Uf^><2Sf8(c1$$$(99M``-1!i@GxgwLX1y@t00nS2cv`s-2Ro{}B=A$~vH zxOtj*AoJiCplK)HiDrn%hD329FMw+ZD*8wG{GgWz z@xO9b-C?zHFgaQt6*ttk^=gs=S8O}S^7^F@3N<2;ej7`}f6^z>P=vFWoiW0UN2*(c+0AU2|b_>CC4;GxDXReK^Nx?C_LQ#4Q?=um&0V=9UwC%r>~kT zO~q%>|2i7mLnQaGKLgyvMCA$)3$lw|k2ZKSUdN2};acD@-bVDnS3(r+mx&gAHlCOn zP03}z{tJOffcGfz0c&kyfWdRR;Y?!0^Lpa{Xqb>Tm@usJM&mCH_T^V-&AK>d}U^v}KzEOr@% zWpPsL4-p8LVG%##<3)RaznGfuczF1YLRc&6(pZWyYz2mx-0 zEAFiU2qU!3mp-Ko{BS8=og>pwjKF^o6?pP%j;q|iyw(mF{i6L=nw%MU0WFi43jXT= zx2BHAk_@9Lb+&p~G#s_+=5}ut8idy}fg(y3^p!_V&EHgmFGYLGWLqk11yt63;*7bz zUR;Gcoa%gxnxhr*D zAPgt5&RiZ(z1Rnl8uwe2$?F!Ku$;J7PxQWXuq?I7^CpWmv+KOETMr+UZCA(q#0;_ku&W2Ak0tYq?Y=95;2Sych3qht$$4p&=c6 zc=pXqeInbiD=PWIF{h<9Sxl&07G=|aOT8g4yRNnD%;Pm>)LY@vbF);k-` z#(MQzqkHWL$&<>&OG5*uv574Tcl6xXli z_PR3YW++5&!o}B#=a|g7*E`m7IXBm5w^wSwYb*pV)z=C6gt=nKP>+>WC^e)?r;^kf z2d*viEc5=|);O)$wv7ULlktNkj%G_GvT5wVJ}JB(ylq!6Qs^u^1~POWsi9^g1|6yc zI5gI|tr~xj7L&E7%E=`l)+77ZGaq*&;Ye1D(A7_{BM?i+Ei4-t6!c3ZsXBYk-_Ap~ zw}@E9314~QA%y+>URlL8^oU_j#2C3HyF41A{dg3d4nK_Rd{)1VNy zJ$BHnAn2?Lu>iSPrKY@%to_WH7jS7l<(4=D?O}e(LG|kfe(iDqCTR`bJT3lHjz_Tu z(C-}63W3OqeLTU+40&vB{DUk!;da<_xkdjtBphJ0t4qbgKr%>uP)x%>jwpobF$mpA z1l&f2FG$dZR5wj7?bhuHG-z(Jba!EuW)>X2+_#%!PB=qkxxf>CEurtm@DtdN6~x@D zY{WeUU~A-w`^RPgIZ2pV#0E(v1~jgR$9v2i40j2-9y#B#-_pC4^!(SzsWESfSjo5Um?dj6qvM9V`V*Xg-&k__69);mqcV||{ zqD+}gZ*87H$G6l+C*zb_r2$g`HJ@8bcc7O7uS%9CZVf3wv>$)iX&8J90y4#W)@!3o z2g)+@axF!^nhf0Imi8OzI@un$Rp}bpSCS-Q=9e4vE`Obmom)CafA@TvXb}n2V!?Vvcy65rt-bAf`7GC8j+nj z1506)f0f0@us(g@XAoHVo$DzT@m3F`gyRUXo?Wh5dxE>VxZmg9mZVci1TA@6J5n)t z85fYK77rX*U)`s`cG1D}6S4qGRY&NNS>G1`X}0y{Ly=xo9=$E)?(J#7S9=CMD>|Q! zZ_Mgq5aV6~bp5XWf-l!#x z75Mi|)gV%FW5vOeplkQPXg+YKhKiUcIj&yg}8zbo%>WdK8CY^L%G^qft}a?U&_if&KtHA|LB5?Bwl` z>w=T+U)2*;m~oZDy2rb&3SFIkf42Fd<$ZE>u|GDXUQ^>!F69RRO+!(m?E|p06b6|J zw5WeA6=wOh;)aUsKNISFCJliQimH@iL+9a1y5@!neeALgfMJ9W4(3cEkaAkr#uP~Z zksZ%IRK}r3rjm@lKv*4vQ4zvIkSDO>OnTl!614zxj84^qW9vvIq6XY07s5o+5D#!D z{s_<9wstvESx-k{SV`G=x8z<00n4PzNM;+6`x{Q&^F8cep_#hXwY?xC> zkugdzQxr~7Y($SV2yB9UhDCxjW~O2HQ^sms)*vZYdb(2pSTehn1EnLlZ2wtYOd9)3 zoK>b*6=C0+8dy!e9sTr|+fh9P8vE`q43ZULQ1({l`1AMr7{1w2SbSf;-n6Jp?%>>K zWQ-muFSgg+3q3QvAG&s!d7S)*G66(nT66+GRB8-Iv~jU89H$V*albJ5sLUm3b7fj{ z+AQz~j4>Yo?5-*)-(OX%m0C68H&U-rE}9frpEp0P9~>?$8nx*=4WVD6dnqB9Sw$c? z@)TsCdNf8^v{p6K0AXskbLXZRRFq4=naKI1(Lc7A0lv8fJl1J&&bu6c@a*{$gG=z^g4~F}u1b;!)=~-sP1+M~3Zi(+TJ}LXaz5MN|ILvZpMA zM;Aio4JRK|RCM&0Rf~sqGynIW?Tw?Gaek#H)SMXh8|@+pk(dX{xLvONuZ{f2Cj*Gt z@Ps%3Il_BcQk``{XM4FW&(VDfn($qM#V`nPbu&(t7zW`wN1aFvp|zH-b&^boC_G(! zLudG$qkZI!$1>xYB=vYSW;hIMN|J(itLk&18CW*HBc6XvGL4CHsd^?N37Z6A0rV?; zWEMtx!4SrNiX4AU+rN>mo?^ntd13IU+4Ks)+Gr5a;ekb+=a?CxFr)P(Pu~dc8M>7w zun_UnGhs0yynd+N9B=_u5D}~VrR;+51P^kgbNQ8wEuF+lllm)VU}gX$8bmGmqDLB$ zzh|aDzC1U~Y*?x}{F2Jd9lRGUMt>M%m)2MSTSEYfC1|yjvPW(B#07K`3clCD`HL zgw2nGGJ!dYotfFAR6{{HozcVdv4R zq(5-oLKKzfI`8h2-Jh8>Ho{%Y8p}u6@h;Bc^1r{0OOX43ro_hB+=r*X)KK%8z`1ajsvjv1!Ri3mS|W_m|5 z5Ui`4l}^j3aMwiD>@kw+^1)u?(mKFPq<1@rNgWe5>GQ$qfFH$1PMYU7Vk0M&^Nmp^ z3MkO=|IOGR?J&O8{Qbuaaps_#-4}AHOl*_Fw6U^@YH)KK4YzILK{gKn5$_kZ;gQ5_ zLM=cMzTfSD1#{^au1}@OFs}oZ1$`e07kPWnwDk2Tdo60G@b#~VbUv4ReJV1E_`Tbm z+2JiMQ}I@j_|o`*XjLwC(pkxfYK*_~KS^r3TP; zcpWr)0ADY2yl$fUP8Xa#t?gz8l3zP~P=R(cm1 zOP4~X@_yA$d5YNto+9c*4&^dms)Q?|Gw99v#4$0~!m~2!MOe4OwVwm-;^8PMIV`s@ zg#d~u-p}b?MG9^;@hJN6tSV)X4bMiRn@;rVWU+N`0NRD*&^b>9wac+-uv2bKH_O&_ zB_zGj!$E*(Vae1;7Lrhyec*|)%f1%-CdX0B7u$MW&d_{1{c7FsbHCo9jYx$)?1;mB z$$!LSdj0)6jeTwpW1bHHZ)(rUhBC{ccpAWsa(6}Q=ZCK^TV?5Lzom!H6rS4TU!_g> zO>Jm)zy+347XsFZT%_syOJx2gS3)xCWr2S;N|#1F!VC@y)(Dt<<31><;^CS@sI&Ka z)6GCIVde|%b5e}HLtdBDd-Iv^uCoFi{<-JLHu}p!M~9Z~G~2XKvNeZ71+T^H6gAC4 z@LFc|QT9<581{nBwz3^KQ)J&Q@6E1DSDl~>fKX;k+6dukxsV{WB-2QNG6DH-FiO{H z@zR5eoJU1!u&9mG?Tcw+M>QII<&sxp29KKEMn%!{YKU*BbGJ=n_gO11dgGqmv7R(N zd)ee`am0>Qt~rJ$Mn1G~olCvQ^x*I10I`;2KZs3L1j~<=^qnSOMua0eszs>4x4v3- zK(J_4jc<=&@gr&Imz^eXp()~j0-L^(4$rd?b!UPXoRvgh-ma}+`K zdF9bXE8Im!)#u-aWfyjjER5flmOFd|FP_B@GM-~)I==*9>)(T|%WFCXIWh5Jl=Rl* z8R9%Q*jD?QJ7C55IFB;_4tbYCT!2T}0CJP-tPQSXZ~`MIN4jp5Fn7IQMzvoGomcJc z_~`T$bC2u^ntwL}2uBKK$zcpqY1AtF^%T`C-lD6<^5+(4ZEHV1Sr+i-0=sfjs-{xc zD+7j1j@!kxiRa`m7!M&AKXP{3*WLqu$p z$Y{R3wYRt2vzoMdWYp?iKB@)2;9c4EyftHTXs?%VZMu^5mztMU4?Q#4UGHPW$|}3( z5Gc_%;d)q6#uk0*>Zk&&=-RX=0Y?$1McU+S(D@UYn=&&fe_GEn7#85ZL4mgqEIg`K zb0VCx1jFdK51Ikx0D>L4r$`ltMpDXu$Bv!@$5O zU`)(^IDWPyG=P0pq^@_h8;a$M-#}4jWsc%yOo@{w)X{eJPBY>en2T%wh)0d4yXDw` z!U@0?8-}N@R1Xc6jFE-ey1w3kKb(DTH?@kX2~(CeRhRLpsj0_t_a)Vq%g;71l0EAy zz8yG{)27khU%m}|Uc6tw-j=VoJXbx?Qx4^CUyiPmod9g_YX6*CGvq0Xv0DL7lljVS zWZ{_hQNI(0W+$|g;fSZ;<54_H;sx8CYgct8dA!2-meL>oe6_D%Er&j`ZgN}ie}BF` zT^(7G#o+;DiL1^w(}q3-0cfGlv=1~q+rHBAjjzUq`_ke07WQP}^H$!f*H11IJp9Ds zi)ZMQ0|2ME);}Q@$EN`j4eAZ(TeNgUpl&}4{h!|$qg^Bs@Nm$qxhl}MdR8;c${1!x z-6X$(1)XnK*UP@(4o`+2X`!f&iAjaQ(1@jn*Xq3FKsW$F2-^obS*OBCxY z>1If@89~09){?Q&fb|mNJ(w6S3Z_cU#ZaPuS#5d6fQ~bo#DiWZ7 zXOzm)yoyv3I;eo*84o^YM?EA0IRfS^0?Vk1W}F4?bWYQITG*pqD545e{PJ(N(RUzW zg#pWD=CB%~O@ce70cs=iKv8Kt;}Q*lzrfJ$r*LSCrF6L@Xp#Akjiefi; zK%KAS7hS8wsVlz0ovS)Zk|;S6pIdTMEs3yzrNbfy_26Mj2>`afK0kD=PlL1>H39hT zZO@tcyVD?i?dVgMnWVGqD?2a1lRjcqfC+#!y3DKIlU`srun-rwh>0M25~1NhP>`qIteeWP-)8}$fOoo}34x-k~*xC`=_hi6umnngKT{{bR0#I=LY zA(v-rXspy-ETykNZPj1oB@s1lMUJzCJ8C&ANA zOOHA=to&T6uq_+&<|?_cOpoz62_QV{S6?XZh%PDwN_j8JiCHq%eUp@^ARmx2Z6`=Q zR-)nn3r|9>*KzoF`42rU;Zk$5#SzjzGG0a|XnwH-wol(x1>ZG-lm$TZOxn#KBSL&a zrruqcbl}RyU;?y$di2FTGnoi5VO~C-)GnM&d=B*#s4DkcE8G44^zD2VPNr%!zTGl# z(;L=evG}R=Hgf{2(b;ZqFZX^30lIXOmCv_qERI(QFifX=WDnlAyO*xXA(ecJcF7|? ztwXnS%}ol{D%Z=|tN_OB9|8?&yG||bl&u>F#Hm@k_3l8V&Qv2)J^p^Za{NSdy9$b1 z{ov-N?R!g?&cN0miCM8zg_#uFj1vjAz$}ucwWW7=t9qUREx<-K!s+IrPg}Dnd`941 z?kBz-n2)BH#342dl?q0i{UVZr7ckZ*}yZ*kf$DKS++^3=4a zehaC)MFp8zO0MB0qut9zZ+Jl<=iwV`VgmYZ`aO z$*){;Ro!v{#$8J!_ksxcLLCKBVu-xuE-FX+Fn`7N2Ov+-4L>`}PRe@WZJ8{}WnnYa z8R*F!fAF))t~;^^BC{x9@^X`+Lx)&P&f7x^v7_o_7(QK$?GWj*?a{O?=ta==N%o$l zaIXN@Q}+isfpRJNs8i~L{J?f9^f@cqRCKa`%7A#yJF}XcX?;S+q^z)DwV(j(O?T^h z|12G107T?vsA1IN5rf^YHJmRD#p~3kXGsi3AkOoYfeLedt_BE;2Zz#H zl!XilamI&a_klcH1i_0Fdm;8XgSo%3JX&NnKj$2w$^%0)aG~%s6OH?53 zEC}51lTM+-|7zHbVF!65r{K}G#+W!Te-C>t%Dwa12c0ody4vgg1+)aKX_=V|GZ!to z3n&rWE*JbdSR2TiYmapCYR`>)R)5ZI0aND`SZ?l4z|{K@1&RwOQ5O1oqiMt=A5I=0 z%!#~ajk%@#kX80dfRzt>aT^zRa zU2S4kRCS(^BRfoAsnpH_USQ7_?!ao3VD#(V=O2y6Ke49ymO-?W(Qpwb_L!KM zjM4VjgUR<3lsMU`+BATIzY2o__W8nxWTp%stzFVGdigoPY)j@l1@&U$VSKa9x21%y ziQNh};}4#hBY@Tq!9M9qd~V8;N0OKRCi{e|*Y@2MHX zd|G5;H?1^nbRN^iA37uYOcqAHv>XJq40i~dns(<*?~c9irVBQBwW*#&;~&Q$ig|p! zJ(xMT*v_Eo`Ka=3!p=&WIu(?e$4jgpE$)^NHeJDM&}pZrXb>D*r}BjUjF|rol3$fi zpA1JW3sc3`*Ru!AWi8un<4I#MIiw#RxIE0!3}oD+xrj@k>dYfxN`o^1dyr7Rt&JT~ z9?_OYFG{d-?*L`|+BL!5i}P2HYpA!9pK@H_|5Y-0KshbEhVtd1RlP}@L{~EyyOtx3 z#*E_1U&fgf!(obU?Xm}yydy#!W2zeKWMEm>!{3EtnIHlv{v|-L@AwV{nep~_d>K9P z)Pza~nZ{;0kU$wUqw`TecT)vHmi(hJA4gNq&;_&%Hx=pNDMrn$yNbqaB5c$eGwJkUO`ihup2@P7St;H8iMHd@tPXQ!H zlp&MA=lB%ELTVRnC|sk~PPBn&`TZ@>g0&e2hjE`l0!E?GL|zVF6eq&lZHBmR(qI^T zJBx-8PbZr;v8Jnd;lE71m(${$ z6vPkpM{qz_o4#tX5e_M6n8LU^$I!+F`43M|DG=h)7ux=R%$2q&UeN}dIijrL z>DwVw1y{%v<;-~YG3=V1S;=6_i8-+uo_g&7h4rrPHP|ocF=e$FN;YL>C3U)6zU_KH zvEqQ`X9EPiWptzR!Tz(ox;6$adYV9H0kDpKc~)>-4~8`FZp=xs5 z>~w6~w$rg~?_O)|v+q5ae_>YLRacGikbZ)A@-cdtWS}mwhj^p@SQxcS;0)NG9aSUz zPLvWYXy3L2S|{tMDVoM=OWKV0sa^KZZ@~o>O4;&w2=)kV@LVOBz&FKiEqm<7P%{g% zW@+E;XqqQi&xeh9(4{dQe@EUg2J){B=iV%+n|y(ZTuz{Zy>`82lN@MpeMTTPrE3yQ z2(#!L*tLI8cU&w7g*L7V)FXl_8YQMaSxExH+0p8^4-~*8!Y*!Ye|G#?za<3BfY1If zyU5OwJ`M~V0&1H|k*){=%mgUiyMZ=`8l+hJ`pnfW5)*-15K(HBC`g$UBbjK{75}2i z4<#Kb(Pt-(B@$O-F-3tUw(&u_L+hRpG?L>KS5L|Izr@R#q7o05dRaV%@O2Fde1FCS zarjNJSd*BxV4yf~ar7vmJprpi9;NXkY_rY*%9Ge4E2!AAC;DTsb_s+}+==YF<4^>;>-r0M`+R zOpydq@s7T^MiC#;MhkeXA!14v%Oi~l*TdkgKo)DfL~Mi;NQY~Y%=k$7;dMhznhG|b zzkr&XV{6Ie>2YgYkI6W9!>CspF^E3nP^ijWKsqG0*PwQ*9Tl4jyKx z4K(Vb2TXNoqtIZK!6ky1WVx@4ey|Wp*V)KPD+&b(FZ5Nj0Mfref!RPR`qEj!fYkty zF>ENe>|FNeU++~|l|aHgnbNmF){tsY8j|H8IVO>%0ecogh~+_iTzaPwl^V0AK>N~P zgLA*}ioWV&sWki{(I+;5?dqoa;<2)zNr7Ytr>NPdDvWRD{-VWCY$y{(cf@``$rA?x zEnC7sU#b7RnAfv7h)j%^vZXPq#?%0AUuwgC_8l|{T1MJb_}XW;v$E|Ddi2`378u?0 zN|ClfYy#HPa07pl=(aPiYBx?c)Db}v>Rbl>^AoGm_-!4xGJHPd{m$ zyxAP%Oyp38x}XUE{y?&0!?~MBP4$+!c7b0(`$X zI(}I~I$>UK+^&|@UcW6K-$p2cczie9Dz?-%P!E46oHz*wQ%+WH2=w-Z*V4Fk3g-Jc zx5Z-v4T2%BoQ^_HIbC+(C@-ICLzwB|>in?gxiH@q8~j*sU@{%Zs z&{*-A?7y@8;>jq?O!_7CMaytN5vhuo@iZ2dMauwE>q9I|j|`|;(^#PJ zW&GtgDkq5q30rey4f%mYxLZQueRp(3bAz+>R+R^NM3r$4ptaEnRi(AvZBL*vpVoX*%fmCfuo*r{0smD~qmk`gN|Q zA9y$9vq1&FsBi$fx=4{h+p!Z0I>RAmn&*q-Mxv{HJ*mYt`?`!Dd>J_<(!L~#68lm^ zs@Tcl`|AFQlo73Zpk%RkLs6^rqClP;}P9NMXQsyjdamA6B@KDELAb0VDB|&k93p+3cb(wgy8ru=}s=_1eF=puY z%O)$ygVUx2X5_J!!c%n~9i4_-G_sWpgd^HQdiV9K742r)9=?m?Z*C~A-S+A%=iBz7 zP%p53)EhuERMt!zhkbjtOE>+Ca;gG2$!q!Y!o#abnSa8rr%^|o>P}bO&Sa3QlIGD_ zMshNTwJ+;mq);0(z3WQzc4|*sSoJzr2qM%AsX*Y?xgU@6fOQUfyCZ^USE>Gz{&@FT zS&{JW_?PqS3+bsxQz^`9mJ;4-dzu3q+eAKRT@I*d+mM|wk({EZJlN0^=MaQe zaz>DWf<>znwr3}zOR86A1FdH6lb#u*n=&v(zhe*88ib3E2z?3M!BEuDJ1&+s+C8FU zV7>G&XkY~WgcgnMr^Q62rVRhg-Dn!=#`9qzp9(H~d%qV}7#u`35X$7-Guk2H86c*C?}*y#~B{XLk8n*U-!7%yCjV zId(23%P_F>8JrsE$fJ5(kGsq2jl{4^@fFSHA>}5_w)Wv^0v`5n!Bs`(oIHbY^~OlM3~TE&biM4$6TXSfRv;kE_+&M+^^i zJxAMfrPiUZ4M0Q|7ebKmiB9HMp4EWKIcxP4|6-4arZM*KlW8{LuP#H2-!<)d2#1Fis`~@#f2ClXn{sX+DzHxv_Z^iMf+iA=LI-bE;V zROq5&o`;fMiDyTsQJ2giOh-4?@Z<0E%s)s+Csn>5Bg8cI_;69{`|RWK^gyU19!f+C z7H83#RG2h?W6V&vPV&y?-J0dX_r^o+m5j0JhNBupNw}b&H07z?lNFHJ9YHS!fW;gB zcY|Zr4H*2RkA@sxeuyj@eR>`yxjs5%4{v%j<8_{6hqD9JRi(C2)Vyn9Z* zqz&RI{L5$Dy;F>oR2Zv@kwr$FS$Lw(W|@Z!RZ?PbX1mgncv_fb2k$xM>ROanIo^s= zr3edGBNhUEP%7B@oSaa{_&u7LPRL;-*Iw@*a{pO*i+*E!m7vtIYSsHUW{h3x<@s!* z`qKu(7I{^tTXWU7Hsde#uQ&OI=@#N~>Y%m0L%O`S0o}aB9>pCsNx@~3ElO3L@S2R` zelAitQjng=Ofj)>%W#~KZYeXQlg~t(X3dNWjOmF(AV-n?VHjntE3pFC@IX)GFu~>l zTtMgP`OEt9@dOT9Qw){{6fCkdu=bEO8=&p|5nK}Xb%9_oHU9TR2_iD{m=SL6_=RLwYKV(12?kS`MmecxXBP zoh9>;N%Bc6)uA6nqEY4D*sd!4t6IB%HlXw;d`q5;wj>rBl5jbKjF_9` z1D;Y1i*Jim>(c_lyPkWSn9Rc<3^UZYQ9zwiKpReC zNC6uZig)4J4^8GygJM66t#_S#od!x7A{cCdsuahWmBS1Xdqrp=6s~I8cq?ep|1UIo zn3o54%`Gz#1wTz`Xo{*ob6%3imY$p(;jkfbJ^LrAy@~GpIh>3}3=jB%_kEju2u5>Y z{-~D8zhe?#If5rGD0}{WbI__d06_1|932iBDTIn!vy3(R(fS%b)W&f+vf2mp*t>AK zO*mISNxmY(Z#vmLxKfp?44380;P3;lvY zgjuP(yBVMSG0_{j34)B8VuW?nU9?Ndj#u|lIK3Dz0N z%4rHlvIkKOi4q5T-T3Cuy629ilePN-i}L?n}(2tev&piQ85U`B6-rSD4pl(T?+66^m_zZXZtL z(Z>?^Piu8gyh~0#TLlEfwR=8uHT3H|Y#DU4RhyaICpkWsK@)OKK9h#iG+J)OyJZ7@ z$)6^cFn5ZvV*mCs6+pD({3{`7y^3y>O46d%X->eqZXuo6s(HQUW|8ENz?7qmPU=yD zuB^l5>TaW3`dr? ziygE#0(s-5NVRx01>B@F>8Mg4mosJ11<7`)_^_@&oz~GXC?GgDuwmxkYT9~Ra9yy@ z`>7{~UGvT#cS)1g=nuVU2YiuEtu|J$c8twP_D`pmfxO>2dM|=-GY?mxo6P`HUvPSb zOAY_IhEb=(i9;vf7zdU~kStv5kjkPBvm#Ewwj1)BDerJOHK4=08i?4*Kn^WbiW}-o znT9iXdx9qM8ZhN|9lP>gpvb)i`o)o0mo#NMw4DA(DuyUARxad=xgh~knmi-HZ0vJS zU5+Q&lwBbZHeWVs*3*=~pp8lrt~8pJ$6RWB#g$@hX^h%SiZxjgmtc8yB7w#!$-f$n z*VNp+=&br+T1p{W`b>c;$0OWX6PV@%llFkKzBfJ$4saQ=JTVAz>4MnUbNVI!gnfP6 z5NX0sV51oC=f`xPN8?GA@2u6n%1hY?i~CzMj%AygwCcNU0v;R(Dr{%%eDq22FOP!g z3fp&OguP-K3dL7UOZ{*f(EQ2NqVvtS91WllqfNq7Uh)rgz)oR z9JO_DvJobCK~|a~)_5t)GfXV!V4!CrZ=}=%2sqE9P&mBXzbQ8l+URk|X(75!0$I~1 zQvo}}!K55naZ?22uZ2qf$P#gF;ysw&yqG z&twtl|LQ&0|EnlrW#Q)jPrXk!!vIDCktR>~zyRh1EZGQindmJHPj8FNhyAB`V{mQ` zGs=3AYuE-e8+^~P(H5q=H|nScrMHT|sGy~G8t-=Q-pFp2uDO}(H&^XCd_#anf)i7r z4j=?#+R7vmEj<0yrjAUSG+U%F7;1nqUYu;NY+q08PJ@0V({*m}Z)5bD$kF~tUlXR; zr)vUme{Snfvs?}!W>1}*qR~J`_((bI zl88mDtb`YniE^H@{I_SdisXVj^25Zm>caqcYvtKmw0P!ObSobkR?Qe{484DHIG0;# zNzxoA-`CGUbxfROa_Lw8-sbq8Z1F9XIMW=gn=9eP4^3l5q6(m*j*wx^2N>)Yn{lMi zg*i4hH@iIS#0f{8Y%+;&kE~xhUtY$4ZOk8xw@Tp0i73$Zm41jF+>1KAGYjY zw-h5J)?5ptPy6{42NMZF{r0?K-DyW5GU=awl|(FG5Iiq5crA}U=1WU_l3TiPa8qyfuvPdW}bK4mhmYfgjL;Igdr8?mo|Y<-Gq z;u((DI)gpE?mA8s@yfSVDEGOyaqmzd$+Qdf=(nR@Q>W06>7;f~;YGamyjgeAawA z;Cy3wW-uk@yqUQlY#!5w%)sVZa5CFFX}fTtzlDE?@j8a?v3}#%V3|14@Pox4cn7`% z_X6|HvM>*O_vsMTK?seREVxSTt;J%C0@NC$;ck37`a%)CP=dk~;8hDH>*>uY2p_=H zf$H+N!CC<3bHU@%V%ski|IABrsBC|U>;8t5_Z}e63n~1Lj6`v+Wb zYacb2_*R6 z7DA?G6~swH5r_od--v%-E;FXz7Jric`{=M=(t2wNAttC(z#?Z20m;CU1s{GPZL*cC!%U zOZ6SF+g6jrOjg1HjzEO5gY;7l`sb0C2>q7Wh>R*vIp|sZG3|wE zHt>MHtPY;qpay??jUV#gPZ(7KK<(KnWY`VRA#{0|PfUA6^VDt0?l_+~Z@=6Dq>e<^ zc`<$U?sXJRgcWtXP9`sbMoTAg)3GSk8{+fXS!Y$dim$y#D5Jw&=jDBB zHh)ZSD|U{1hS77H|DJZ?p0?QQwruoB>9auWyk4(HWi}V7L^eL;?N>6R|}!MiExdF@^&a6_^N594h@vt((9K1*+U1QWsE zWnc%F&r$?@7G+$EoxP)+p^TO}fk{~_Y&dc6@W+BPM{-FZ7hPfAQpQza`fR3k&-VNd$@~6RJgmLmx``n? zs~_7uq42gEkuF+xa%$T|MBw~F=5}>m4TPg8wCSE&JO3T zInMk=8KUuH@JHU7eZB~Oo$lOae{3^9Ra!LjaU+!b3C7zd+P>Y{G92OYz_SVv>muT- z)Y~a_Q>H|cbjd27nE@gi+mN6%;a&@Z?~yWrO0$WD5nx|*$4&CliI^=rqv37K1B+F;E7l(J2pPVki_ z!H!As^Fkhnjy4R6`PW>ZY?lb&S=T#g?eGR9rRqkacEV;6EkS%a%zvg%Z<9I1CR)Gu zEt62YB2duFqM&oE0oI}RMn8NQ7HkCjl6Q9QFrtY7Jn>!*UOFP|r9xW7Gg;xf)~5K` zvNX3eQ=0c>wzJEblVSDh{2xI6XJZ|zhECPOkKM`bKazqgf5(;|#n@H%<6Xrx| zP#<8Ku6YnV76rc@V#Pw$9U5s`zU``;B|3YIfdY%Ib}vB*9ayz)Rhl@EewZ|Gaq<*l z3eG?*AldKIWL_y(u>d!XJg#7Wack!?%VvsX!y+YMS6!Wcc%}#tdJ@ay1M>0?;2MBK zv*0$OX^fugw;_Fy{8yPs2=M5?ej(Kc2zaSQrlMpNuS6_qCm`-rHda2KP&vsBLhqAg z?=|!-vUZA5Xtha%F0M1k!uMsKHS8x-e0!x&_hd{OI7lAw@Y@!#Fb*<~HwHoZnTE1U zSjH!z`N!;{j1B_yF$YtPxd1*FVnSf011ne!PN8SD;-<-n&?QD=3etb+nF>uJ(SMm@ zfo)dhwg(XZa2|EKonbIDR~ZTHkg#E>>@gx(3yuSoWac_xcpxX}1paR7Glv73U;r)b&@{#jV9dw<)`{i=Ruq<0-%ODvJd0NOV8L*Lg7u zxw7n>a;6~Nxh*B?e_3E)!#73es8u=$jF)^ zcXyxb+T%*RyRG+i#z~GTMj``dxV1%3rY}{EbZp|>sO@KfQZMrlKji(8Sc^h)d+)D* z@=-e0a(!2;t%|<8@!Zp<3sA17Yt3Fx&qicw(?SYvLRx~0$Ap$oDs(s%)PP}}&ex$Q zY^2#dk~cvAd3L-v!tvV5v|jPH|NY2PwY^#W)lt6XrxW&|yp|yYJ!MPzNr|!MS!B!^ zuTr|Hu-XF&?JG6JFOIBdzRkpI@Vz6Mu0H&>^>ZV{Qo;NE+J5nmm9nZ$XWTBTY;@$3uXbC%CoijiS#n zAuA^^oe6g#4b^Y@@Suk|V{67pBB9Gny)nwTYhmxpdF<}G%z~_)QT!-$?$8y)@g`xN zATg#9M={F)>gZ~22h1$pkM7rRS)}@IfTw9JT?@wcdj9zsz}i_wN1l9MzIx%Z!?C9S zOsUAe_gCA;qF#~S!^BESK02X_t^*XzRQB5W`kjR?A50;U0uc+VJZNw`sac>b5zO82 z8jOmW5DB3`s7W=A;mF%K#|Ymq3(u7x*#t#MJyP_thy zxcm0Xkj2mf0ovBkm^siKPHctc8z(p?IyF-BDmQvT;UPRKv;7tY65uRA1va!HKF72A zWVUW13ca1^O$pvQ?sk0}sjEP?c>FZqgW^>9mviBfqor=p*bqER`wz+MaY=){q5B+6 zO+eyQ&q+oF$qCHeP&?pe=uiMLey-jIx=0i+cNQXJ`|{-YP=9HHKOHR5u&<0#6Li-gk21*JS~oQUfc_Zx&sXChWXMp2mJ5fZEJ@ZakGr$;8_gRz zKkgJ_WsnRdhESxyEQHVuq>1zhDmpag3VS&r9YeH64l5UC$sC+hn{cj92SK8lc7_|R z9Ivr+0QFEk4mG)<)Q#^zK#PeIG_B$p-f%vWtCP13bGaH){A~vdrwkIpzYvb1!KK^zxbMb(A=($gGd#+ogCyk)nun+j$=S78U#4;y@h43``P1 zm^9h=C)a_>J(f5a9ry!kqPfy-%^4Slz(M^g0CD|WATJss8b*L=h7act4A?Q>%6-r} zWpt2>0%F1pdlo6&TWa7E1Nmq1B4k31f(q|Yzev9p zmYMup#zwEWGO!j`?#aWVWjzN0Nm(RFu{17Iub$o;gMeXQ^R7u(sr@=|9!6V*fzuZV z;J03;N^%aVJu!0XXG)u}ZW0(?!m$5Laqq~=EYuMA`_Ixi4y0$P5#=@V%`Uwln8xBv4P49XZvg(Pz9}<6s%{LDtZf4Z%|k}U8s5@XnKt}@br7V zs3DNu82hvc+*r59et9!6Wm){H?2epRlXb~RGX!LZ*fAy)ch{#<(z?f(_Nw86vwj6^Y$ThO9!6r?8g?{8P66 zh31XAclW4NV}ts5)V*a>Ar}H}Hcro5Z?Zp#>Rw7-FYz zA{QXyOfi&{M$T2L*#Qin0ZPWerpDS}jj zouP@-7;Ytw+v#2$p*)QgKw#NwwJ0&8Y$$vn#8eBAW9fccL*rSr04XQAbl*c8l~T03 zy4f&uLBWOdxEp6nB@wmd%w*MqA|O4`l#3fmk?p!}ZZ!BqT91Z8tU z{3O!YE{u^DvF9~Dls`dh7$bPY0pN$x|U_|m^*p+3>RFyfx@ON*& zvcX7Q`%mgS)wnL0vT=$V3DLiZJ%ANqWjkhXB~PipTClzZ0PS@Wjo3xc?UXGcl=oM| z&~tw_J-ck{>r#)x@EhA-=z2P?y)@B(cfbs9^F11A^4bH)_2AxI{h1R=%a@S`#}}#? z3xEAs85&}1l?geRx*WIDz;d%XdbeiKxx@r6JeG2r~#NNG|s^Sd%Jstwdagi-#NH7LO;GLW9=3AIoCWB#=*hc8NY z5Azm^5e3k!n1ruXDA5u{Jh7UaZAQ;e0(&(izhUl)0o$kZnXc6+qH~xvqEO>v6*-7R ze(v8y2Oh_vZfR4W>ark}!y_QCy4FVg)hfo~vc(51t`{w94n;K+Tww^i{@C4B5USSO znUzK2)!j8nuJIi;E~>KiVFs_{Mr>Y{tB7wk`Vl`E5bJIKa3?0O6<_56ptCZJjt}m~ z$L9{e0U|HI2{6N-lTchTPV};CRnvv-`VHbnsOL-hCM z-KQNmbc6Tm-F*+*md|+V$+aH1$3gdNYag!``-o9=O({;qaGNM>P!ZS zYe~)Li=;3|W5}q zNM@l<2Y>CvsZqqYluEH4OZ(V=(Ro-d8HhwI#^FCwb_Y5y5B$~!%BMxq4 zsWBWcVH?M0o2|Upef8ns-3#C_3cyYe-QL(sYDFY6B}fFXR1T0r;11&E7}#Tl)jS5( znn&L5^xoC=p2gb~TGrANjzpbl0C_V{9g__k_zG_&T$^{m;^auEG`c}aT(x_2H~gRJ zC*=Jx3xc021PbYmy^M;@f!VX)i@5n~Z<4T|xBy$s3y0@nYh78v)m8tVVk{t16J;`F zWEAN^A=!AUzb7fYE6F^`W>VQJV1}`MvU?0IC~yRkQf9>tWCBKVzYdI+0Mkvf)Qu)E zcpyaTk*v(RC4{;$wsWE`H=J;=SkTt`o8cws={aUWYWZCR>ffLkTqPKi$<0iM)XhK+ zmDa_W((;y1g#`vbUD2l&$A05S&Sq|ikiuO>xNE^F+jc2@>~YP}UC_gt8)@-t9zdj= zt5PZ0*sS!TQ%L_jH3Xx8Yf^#luw4{8`kd9oOkXSQ)Uff3XG+e(Y*o0`A{yCxXHwsl&-g3Lz{F7NiTo ziG)D=#Z~y1K$N_1o`{9L-gyFdDyAza!Dn#xYz_tMtmIV{ypoC?5&*@#^)}7*jT4M4 zcB^}93AO+)4cHTmJ^eBxoUKXnit~Rh_y6X5#7@ESBX|Yde?)s`!c<$m@S3$s;y{cs;cThik2f!&n?hmL5fj#+e#B_i8f#?TD zCMSOV^=yt(tAw`_p3k(VZm=Qq!ZCbU^$_re+RoD8UfvwlO;7PuF&K}jXdI%$OVlI1 z#lad(|3u?UnQ#>l0W;c z?fQ6aXTYo`a?6#-WVE)PoJ11Qe(!yFU?SPL0~#(n<706DNQ8P0?t zhX0tAd|dS}kl6-@`wlAZ8u5QTfI{D8%fY2p*lXRx8vg&4<O$%VG*ufwFLdwbRk8`Tlphf8ll#x}B~)p0ANc z)C<~+$H}F}^vp7pR2c5%u3uHmZDQsBY%Z?Zs|uE1rBfe+k)RZzD>cPYVzRAQgrhH7UvVE_`BFFq|j zLD*))kI>OtQo_%^p6t6f$z(Re%-VBL^wy)hvpY9}g+GCB$WEZ#>t ?f;U-l&Az z4lqV6;FSs8^a-rv?0V3^nH z+YSE`(Sr?tA(tj_h(G^MeGx*XJ|K-0*ts-&xkbYtiHtUE`S(pz99n9L7|-*}`0M!ZpsPCmT91+iq+r@_1Z<=$y*bCb?STu!lZl#i%YHQ zhH!y^)5cA$OTtfX>imlt>XMYw3llHg?~=W>>ITme{X{{?(QF`$sb+n(ntV61$&72A zY!cZB#+XG`K4%2(yHQ3lUA!ip!c*S-8hYS9JToh-!$o>Jos3HS)ItYC(R#MWZ3L+^ zCVIM08Tlf~qLF-H7VxFfC|%j9UR)l*uWf`VjnzJNUHN`qRe8OTJ^9aRprg(tk)zVp z)ddX>2}`+^Ak{Q?>3E@O^f_4VZPr zPuLyHppZdl>$=fL3KFMx){$i_fGVk>!mihV{5SI()+L=mOPbrq&{seq)$ZJ)eKkrcX>>g8l^kLMTRxF!zwV?D&$g6t8*$yog>u`vhRIn&&BA2TY zK_S*_L}pY=$&o6JUO+WHI{v>yBE!#2i(2RdD%Ft=d!ZD z#oM=p#`QVSS6@Z$QPku4;Y9_PHQ6@5OQ8?%P)1>Rq@UvqUp-K#Gy%IQLRb&oqwX- z$kzE7j=0mP#BYj#@RJ2cKNUL(dv~7V+(nSdoo*4n`2;#EFIv%t0VFxdabd0=y7A6@ zd*lz2Zh8v~9J*O{f|I4zx#E7OD+!E2Bm5Ke<)GuW)jYBE>F$XQ5S7W!x+g0!bQE5f zxt37KOTRSb*hiYL__cZ2+2pD}nRn#WR7KqE{j|oIdzF{km&~h1aZ7Tsx7%w+Lp$p6 zB;S^;(W0e5B*;Um7UR?*7~tome|>G#CepQMk<@YOn64(HT_~x3nf#y>WZ)Fii(uCfi{Sz- zU7E2HkE($Ux&t&(+)Qc}iv3ZNA<@9S1u3i!i%he6LxbZ%LvbWkKK6+l6aKP548iht zMhGnUv3*5*0>lN##Y_xE6^GfFf@Me_&LN|wM$rA3*p7ZgAKpYC&b>t+goNydOCyo* z28Spp)i`;3aU!RsE^TM)VVgAi_@tA5HZLF2Bi658)<0|b&Fni$I&J)e+T*ab7;n&c zPRxfvT62S~r12}mihLh;qO%Y+mTr<(?f0TgB$UKN0l%X2R^h-03}`n}B0n>|yGV2x zA#5)PUm`SPOw^jWO7RD(hKKe`7WccQe4D2r@oOjoef`QZ{hRwBGR zq;4BX=6~?lV|NLW{0UwE%m0zNuS#5Y8kV- z#anef0gbLTfg_n&YE_ptH9Z&a&*s{HG#ZEfnz>sCII91(5GK9P&e0B;VS_JP4gn#5 z#;G4$KEWh5ikpeVXJq?N4;$EreD28k_@<7hC07@{z1nO9+Whr`_p(^K)pIZS`qenC zY#z!j-ytXMKbzX=p_BC6r++iFt~<9+Uys>;0!rdV3^>}EPxG)iCCyru(Z%iP3`i+D z@0vE7Vp8#bSh)n-!g0CIKOgGfwnp8w_rOa6F}mlrrlY^YHzvKgz1$HDc;I%55HFAF z3i9(z`EruRFo%Ef!T&yl-5Zbekt6y$o*Q^Rm(qetuMIu6S%KyojB{IuKwFE9-W3%= z0#IJG&~@~GS~XDdVBf3kFlchMsnt(e zt85BoqHeiGxCrNcs~evKGx1nyD5}-j0SHgK;u6kNU!OTZfUr0^Q`)d~`(RkP^^Dz< z*|o)9xs)rROTU_m%KeV_v{=lY|IU31<0470&#o>t3$GDJ<8MBxa{s(sz84 zNS4pQIq3ojge1P0T+*8Q!+`U&F zTBHYW&!z>`30)`rdX3ef^-AKal%74{^WL!gju7@J5F8*!9d@9) zM#X`M%dAwSc%MGd^7*6weU0{wGXP+Ft1E~9bzj`9|HuBBg@gIOFLyPq{(&DVbssmCU>EcQ&b8V3=ep-Gl8zmf&zDPtez%fS}qH zPQg#{s=)u4Z~ET_l-QKCNTcED~#QEEwonGpaR$9d6RuT9bAeZ;VZOey5_YgaAvDcL(Vvc6d^%>PDj!Ejo1Cx)4k{22HkWC zIGWX!5EmVHyM?z;Pe@oUm|=@nIs~g z78}iO5ttT-((WR9Dz=HB;O=kx%;(22ZExnr5gfg=WeUjOmH>99cv*rPUT3oZkF9eG z4m4`gaLh?2wkEbYv2EM-#Qb90PA0Z(O>EoA#5Q;K-`cC)n^V=-r@GGR?zf+p_Ubda zNY)PT4-h3hqD3qwg3H}DKKv#e@7gYg2$4Wc%WA_Xs4=wUpH9I6=xx8hh4czJ2!Qpw zbIVhPW2p!WYG&;k;BIWa?;ok2F>KzQm`eKlIc3XPwPb2f zkNsqXlM%=}SOV(5N5K=$!3cx-!g*ufTd~g4x8Y}u#FM`g%NjYRY{4;2q|RvT)oaUM zbc@(``E4D*zsTxj#4h~_3vj80q20X8xd?)2$RBM?)eB$|;RHRwntDewDvR@j@GKckpalq^UYqNU^u;-xMpG4-b_UNr zLEtLG8M@#n*1v^N zd<~8`zc6BU9#{-QE1(23Z@imz{tX-@q!t3Ugh=d3z(~Y%V&E0an~W&haBz>W%xk24 zqD4gv&k2)|4dJ#esG?VKqUcsh2Zbxq-TTdj6m zKvk0wdMTZ}=p`}LJ^(iC7yb+NpA<+Q4dRAah8e)%rZ~LBbwJw?VNEwnSYv;xXeh=$ zRZ0ZQoZhY&Zy!*kVx}K&!^$|;zhQ$Gi+RNsPL0Gt7LpdNf$m{aZ>h?#->Nl#A z{z*`baeYV6=gi-v;#P3xMKsBgvFu+}mG6*jD`4>9-^^-%_V68QZf`tDoK}?dkaHc7 zcgF)DU@T#8{SLe|5?+cZaQqc@&m=`5p0J`R!Y+QIG(Z?Ulj?|0!@j?8cH2*!=meq&(M_Y{Y+lz=FxRJ=@O-1~dZLBk!kLfU2Wi0H&8 zviBdDl0U)ucVfC{Y*&*y85`Bw0uQ zU3Y1sguFd}bt}9nQc1_<7_1TA7hHM@=f2AdJiXC?{UnsduQ7X0-MP_;eb6DK*;9a% zm?t(+fRkF1{mATENg?*$eI#=Ah`$1Ew~weaf2n7X56)OJaiz%4o(^;&n^h;ySLR4c z=aA2Rqd%qP?zYHz=HfUUMAF}XRLTLzvc-xiM66IP93Y*1S8Wx+xjm`#_dGqfWpK!P zUUIV8Je)b;S^5ltE$-)PUe;S@&1dXmx)>~~1gSBouEBPfn__st@ShS^{(Jz+x<4d( zQb`U=B&moqAdo|FVy7her}dFT3FXam_W#s-7F{J2eQpH~jae3FJFy&iI^*AGmz*uo z?jgI0EI)*2I(_@$fy24xTY-@hsTE?Wsk+*1-`1a~JE!uJ*p^1l{N84P_|nMG{;@I7 zbJT^zgr-JRbFtFjvsOP&eU=1_)lz2`T5h8-VI*yuzs*H-9YF;KgMQveM-O+}_WmT( zO~$nBmOnou996c~(jTiL{B_1TH>el0{SZUpGt%J5^f6AGdy&a=bxy;ZNlWMURzEd- z_E3L~5=>Kw=2U7_$uYiDU2-!ILqyoX6@7GJ%#Jbkr6bA&Z?B#ea>WDGT1W$@jtt_L zy=Wyv(Si5f61-LvUgNF-*XMqnqeLQHlHE3jSsu#}nj*COWY+y*ig%~raoQ_vU0Y_7 zY(`5ST%(r1Ih%MuCJDUit8A~!Kv0cm%FV)PpJcnid0v|*bl9DIckxk$pr7L1x6L9* zp>ti(x;y1W@C~hwFgXDfEq?vCQ(S5j`FN)9xqHMII|_%i`W>G3vp(iXa*$cB>(e2m z>e}XSiH>Ynmwo4)=r8!hd*_s2~5x|>6 z=1-*`%F2~7Upq1@?De@>Py;-S56;cl?gAuF%&^^}tXVoVr?#em_&FVh#=m#D%ZIj( zAD2g=_WEAffGm8mDf>RK3tz!Vn$2aLZVAOx1AXo8XPR|pHVCY%;@vG-;q9cUniZ!j z@qME{6QtH1*5c9@8R-74hKgg>pgex!-iF{p+h@2=u4(VnswYwKcYF_X%2bo~Mx0GQ zb@!%j#r_m|9i2?3j}5hW6Su3x+k7>_`1(HJsZDUaiZCztbV<$C zX1MYCd|+(_pWwle5Xe7pIQj0bpR_OrCy@U@DS?No!%m{bRjs}}H=+KAK(2Lan_mSf zi!MoZLW*0vSJ%pRAoZ5S!vaa2+g`5b+!lVvQ&1;oI`aK7XOPEy>6wb_XHo$Ha)IHN zs*(*9eonB<-7jo_&yQyyl<;=_;>|@bB$xfgvFB4FN6qobgyv*J9;yCjDLNza7hR9T}rJgR7zH&aEDALNF@UjK^mm7vm z+^-9KR3*hIFG5|c<3yLcH&Dof^e6_q9?=OO7qiakI;yb1>dVdu@JGuLpNFPHX+T^F#H8KGacjAtksLzy?@$MNs z?9V(=cZ$8TDxaFpC94C}hKH$x>*EZv}dB>rjGRjs)ij{adgui08VG6cV|9 zA3`9-{)%)v>Bvp28YIS$p)9D=a-e7F;OW0xCK_=7{$NwBa0t5ecE$ETa9kbt=Y`|N*YZF~o4Z{_4f#oG?MJker0DrgGP_tmYEEZPZTV*baU!Xl z(bVG+He*$}y^?o#yLfbQ6SZ+)#C%iJS338iWyAcrYU{_WZ8!^6ErBLlDdMpr>*=@j zPiv6lU!Giy&l35vwaY^C1%f#Vk!T2W;?CMLK)yIPFJ##HH-YCbRDmp)k!gqPq{@&I z4;)Q=zY?6l5{O&|@kz1kW1w$0G)Yv3(Dz4_VV(*~42LMJ!~hi(nSc>x-@1AMMcJ7W z9x-85XlpF&<08L(wQrXYXux!~WKCLdAkL1MnQKRz=UdEl$~8@%U~lQ>xiFgpOX3a< z0NGv;x;m}?)%J?W64R9>$dU~9ME}WrAtMyU7-_VYMtN{DoutZxw(5+J)kW5z=Rkc! zYVM^e#?3tGxB44h{-G)+tZGU@L>-&J1$)5hWMBTDMacka{ zFDu=xrkAqXE#__fNYBYd-wxPZy$e^-HXCNEn$xR=*){0#bAw%dO6mHST)w>07Aah%wGNDM(Gv{i@H=>~5e0=v2Y z{0;@9q~{v2@-FDgr_#9j$(^p6&l0||HkuAKZLcXtz?ik8o+(f#yCnzgZnlwLOyNTG zhi+E~r(?DZU+{I;o*FCnN-$_U5X?s(G#fG#kaNc&*WYGoUQJ7#k}SRLVTTWBz@&vD zou*CPpX7jEU)7N1ifb(gmO;m+0xpcYFzK_uLY}9b{bHO>7DI&8f=-MO$}788gohJ=ZJML!X0iza@9NF3F4!ZqyJKE%^SI6?mwxk>Cxl zP|7w6vL2d~5X8mX@B3(0_}EtP=ue;R^vFtoiKksO;419$vzI`ZQKJ8A%+HUv2?pM+R60we*84IqKAg?7!vk~kao%P7(C$XwG?TD+2Qz0Kn59$-mp2$A! z!p@K+g+HT^inGX3;V)H&36u{b$=9;Q&}uuz!CL%lg3Zh@%N#fNKF#}W*WpQo~PrvWlLP^c*k>EZ_#oK;duXH9gcz4X{<=% z3#5vMksP$T{Vu9;#$H3ndI7b26aV;&5w@m$4i=5r1vR-gY;)yXCEy=K_HCo(T z;64!qh1bzoKm^@2VPh`aKcy}IT1y`pCmtJrO;F#P|%<1;GzgeE;L|=^5D)2VQ((SuQ6YkkclZ}EfJU;Y?@y)vkn;1}bDx+^k zHs8%FAm{Nu-|$^I008F=AMtPP#XuLyWe-9A){J&O56SOy$$A#NS{C3K-)J1_>aWN< zsBi#Q3GPgXIOoEw@ER0mdf08Fz3#08H|OE-u+4Yt2rX{L(8Sg615wN+;=D8!Y)N>= zCPQF@QxI`a!E>r=)i|#ouHe96YT;*W!Tr5XB4H70S-`_Z2j% zghy^3SAtJa;I6e|PW%}h>|y&%3&id1V!!$*eT2T2hLN4~36u}r87V^-Pp>&*LB$BZ zk(qEg)EEbsC*@3^P(F)0Hp@#1Hf3a^lhBrJP^__9ygH==$%s;{fx5L-ZyQl|&{O;5*{%^(BFeUPJ|+;VKOa^EoX^ zG_$ExGB)FK$XCgs^6JMz$I7nlZlA%|rTkjFr#5FFMheW`NJhYj`8{A5IOzCLjL$2D z_0QGZuvmU`H{n+M<{z(RYWye8mAUFiY13rBUnD3vNjW7Lu>m~_?!5h8K=LHEZKw;k z3P)`o;00vGoT-b~2N7qJF=1^M8swjed~(hVo3{QTj#T`>apk{C@IVR_CV{YCF8CfR zE1`?rMQx2QGcWW@?K?_TQk9w9y>ugHiZwCwPUv-2_2^+kk(g`G!%ArHFY%U|RADZE zdNc+tR19IEo7Q;`(HSP?bj~qnvfvha5_VT#z{RR{Pm?%=am5hY0U6J6=chUuwrQ?1 zX{AIs_fapRFBpnl>EuhRm1{Mmerbxsa0pUo`>x}FNlO#KJF(aELGc)>1{IvzD}>tq zIaamA7{wx#J=CKy-R{Wq4r}1la`wLWMQ~OPdG?hG*OL&6N_dS3Xb$9$^}rd+UDlHf zfY8pbsW^hXR;D+zTFRx{*Hehsm??~1>ew>p1SL_vgOADGhruE*EG9puYT@$j2{g-j z;tLBz zkKkQ|Ao2dQ7mWRa>l^T)o%N4;Pw}u!ATFR+{~Lr5SoD|VzB=JeH4U@zOz2$`DnsN) zJA7^IgSiakVy3pD>*($oYN9H{=EXNB>e2QyvgSPkpr;uMzGW`*rNYzRgDw?`<|uPh zSTVdHa-7P1pC~Qyw!isy;z*Ox%-2uL&{8UAD9tjSC6kQq-n`?=_Z7O!?X_PIKyR{U zY|^>Ue>BF+3H{!({?W~O-h5bpL4xfMt*O|jGc7?})#fZkf$xyt>MjR07omPtWkfW| za&!20TSFGtoao<}4Od8pB`E*(4CK! z?mH|GcbCpn?Mp&ZVREC|hosC1U~Ke?8m!2$g9sA?%5hH^`Ddl5X-@UMXl}Z?C=MUv zm|bqj`iP`O1&xAWO{~Ft=Ov~zZ$oP?ZGk;FR>huOG+KYhTh-Y-YHlMtmV@gxdkf!6623Z?v-V#w+ zesv=$kVES?*1m1sZdG5NEnc*gizp}|D&@WUNySL1DUy?xYu`t?JsR-(!>xDMk$9B# zK!=Ecs7Q~J1pW@7(Ygr&LNJwJCa|viqt-Zw7_1^ z)IET59Xg)uV{{%{%#WQL;l(2NNwaxx8)7OTl$rG&G+(HFSJS?XQ#ylm55V+D7dE8F zuzTwmw~q+@X@@9x4u(Tgp*42=IFR)Fez75AcP!aQHZUJ&MEyY^l$ykuxuttFT`ijh6+nwyaI%=4Ns_)Q;5Za|D zyR!YVnxgNyofpf#I=Phq5dsHZ@%$1jK@=3JK+z+Rn8r*WdqOm{c^4|aVjx;(Q~k9- z(LlN4kth&2x!@RNOzq5_Er^&n6L%-6feWoZJ3NkWU03SID|(H@WC&=i}^VoLU>m5~QmZ=9|P*}BtH?z~)$lm_PtEO_C>goI?#@Rkz48KK9}m+@90 zmQmo4tJx$h{R<(>TCEVrK6L`9t2Kp_qpabQl(dp${M&PwJxzo;xS**=UEp8)0C>n) z$>?{AI76)45cC0(C_{81!O-N77y-t69Rsa?&LRne08kV9Qg}K>ihIT11CXwmAqu*+ z|B?sFtZ!3ghWkxNYPNUGMKOrcGSDy$R((Z@6w;0lh>t@h*B=xjzZZ@sV#T};^VzBr zWW*w8JCLE@kxOaZQ6XaO0q@HORM2W@>@eM#Y4qqK#6vus`p?XKDS~xIA-}Dmg|t;9 z%8O?X!||dD`x)K%x3$H1Ep49N&|Hf!J$ApxaO^Z2IapMk+rbyral_VdU`-L+ES(1M z9*mNN?MO0XBE4U7(IUz4F$U1AZYt^eQF!y*)bEH=HjB zr>687Qob)?_EgjBk(y9s)Anfl_IN)B%t%l~9>!LvXI_K(MMMylY2B}asw<9`WEznN zZgK2xb=7)fq-@riPNXwYFx954tnId=1=nEdF_*x% z@m$|e$T-MPTD3z5-Y=)VcC(72LVsG0 zuyQ+;KL6Bm6YGbkr>DPLUy*asvi~l=m{mcF|m#%*~0i?v#*lhdHizIWsz5+FTRztCRC!I8g4Kx#yR=8(E&2UH+ zn-?>N$}J%G^W|}pdX9eJApBb*1>-~vqD=&Zdh|)%di3HS!LBvNmM9b)TnkIFxpFxz zo+)MF16Nw6KFMlqXMt%; zR7`sD4zVX)Eo>);Ubl_l9-Wr0LrHDoetH2Q{K?SD8$#M)n^T+CLX4Uw5hMp*5&U#?*{%SzPq# zE%5QPX-FnsA}>@CZRMMn;kB9gf2O@fsgBq6RWi@-c|NN+ARTh^b#PB;Z{5ih7`FkP zdg&**RPEN%4Zl34;y;|U%4Xj)<;B&H^rMZWNGN<3%nt8`kk5WjQN0dx&!GhW(|fDv z57D*F8xjuMN>hq{p-AK`?C2A7mi&%BEJe)v+5L09a<8Gq9LVif^JvrS*{WiSE}2#R zcZb!@E-=!}R}$1eWe#hZ8~m$R6wv^urrBB8B0kjUGgynb9;^xkJC~RV1xc?=+^;oP zlaOZDmZP?L-;!Y{LJ)|8DIj!pQTCtJiUd1ZpXC|+whJX6L2{0WC{>t+s__?$8S?(-Os}cU# zw?^)-T6&JA^_9W{3(Lz}@*5-b$dPvemA?pYUK!MQJ{xAgmYa!~5&QG&Bla_gs9z`l zD|h6p=WA{|@eh9kdNKKbzC~axEU7=xz))MbOhBn=!C2V-CvuMiy~71#;rO4(uM$)U zAC&7q0SXfb2kZa7$jn5Xtn5sw$lTy);9Tr1|J|mh23jFi=6!HEV*HZk5Wv7KgOI`I z$Tl~*pk$y1Y*{5rB;4TUh<^NtP5_&mo1@%>k?5XZ|Lc4C_VP9DvEAU*y#3_#=P6>^ z#nzlQN~?W?xtS5S7VMv+=j8AJ4P2b^GTYHUzfH~lGW<~GqQ%AXBX`Z!u{J0 zLQ@#Doe>El>i#_y-qjJBbWRiv#@Ws7Z=I{qHPE~Bn1B}xa*0$3E&v?`GO*d(?59r! z&RUt>3n=aP8G{~k2_ONm&W5fMZq}Sweqeb>NGsU>%#JS2kOQEl>!9spRv@yNwRies zRsar*z(HA1m);QZ#>IjxO*Qub zXjQ*qu*SC5CUL-;ngyxe5jS!2hzbyD#Y~?m_#t2r9R-Qqk&upS@cer=^2TVdY-pgJ zet~h5AiMYMdBOPy3YNBefqk(q&te~4o`2*f;t#GaU8>_WSJMmln2$KA*WS+a|D=Su z>Ph3pf<-z&M@B(80QbuU?5H?s;^3dxComRS>(k=ovBn@Tn1M83qxfUVHDv zG&=l2C0c1#mfvy%fZXfnXOLzjBoqVi=1|W4k7aH+sPzvQu?IrL8<4cy{C@!TosWmt zw^6)5jM`}|3wJ%IJ=)(?lsFU@Sax0$Z?}asHPfJbGc)xedg+J<;dl1;QNdsUcz7`H zuM6>UoLh<*V1=4{y$^&K2#zeD2ZUVT20$PA!TUj~U1=bPHb)W!ia`GuH^yx;Q6XG|>4amBcU>s3skH#Hl^_TTl9+G)2 z#y8NHCPM|a&dx6qVFc_ErM9Lz=ZJvtu_RAlvQ4FoWL_Xf%PsQG{o`QnCGLH3hdqK6?pw}&yk zgmC-}6;_dvt5zT#1RzysdoBpT{iY1wwlfJqo5bEp6q6uAv|`vP_58o$!9k7#;0Vnk z*Ft;pYd^Gav64gJ@dEaix7)o-X?B;IdnCWy&?tLgU?L&`tLGZ>K%HHOr0wnbFr!{z zs%M!7>l_qncT%Qbz^HF}3$i6keRQVX3BVoH(y@ySW4lBIFh%O+x*lB>2ISQ7yy&e> z-%_lIM6KINot@`GQ=G26M+VM~&3c9El@L*6yS-E&{kU7M{@#dtF?P{K4f$$tk{Knt z^UGz%%#5MUC(-pe+)|6Q3LA9od40Oz8f}W0;O;0hSXZol)l; z>?9H7{{K|M?Wa^{-}MW6f<$shn?)4A?seFr<<4?*zmitxvxyy&O4c-~?%9))GA5*h znY@U)Q!{wzl@EErqE*$dRjNs;Qo`GNsghhk2c+D0fAQ3m{2OESHcjaX)9>UQMNU6d zvoZEbhM&g-7eJ?2V|()ZKdT_zAS1?>E2WugQrpe~a&O8{G>~~**=cC^Ae9--ZQ2ix z8j3gRbvUm!K4_1TM4*vZBr1Q^YUcKzpC=PZ^W;5Kw?^PB1y6LL&cB)03?_mUveFi6 zjbY(LO{eHW6Y%Z5+!W&~nGMG6c-PNfmlgGpoP!K?ii`7MAXzVDbZ<7HKj~>+-^15| zmUJQk^;#})OLn8kiTEs+C5egAWs4idlxDX72-TaCCxD@<^I;RIK=z> zav}n!t?EMROxpDKFHIr_fg*FLz!s(x3A9^|)zu)9YR%UC<+H~!@e-6T$iZ-(!lWgO zV=eWZfwCZ9Oz?vJu%7J*PAnuIj6D6p$ks9NrkQ}z!#G~&<%0cT{84DfKP*v9z5#&~ zzruIaV)b&wqFW&;e;>H;orF8P%0}{xK*|NPBn`Td{o~*KOr{TVrv4SHkG$=L;1NYI%Dev>+gCXmu;0}ZO~zP6YR?1kNyRc*)pyf1KVqm`<(grAzMKUAh62x)Yxmw$ z+_lp(p5S~PC9~eMqD~UrP3fPIkH*p+_OA)h`>}phdwo&MppJ#6f4PCDpuxD@7lZoV zJ81`oF=)ymlU&x_HLkGl1(oaxvZn)MzUF1UTcZ5A*||XF9DNL5NU71J(qIqJW+=Ys zEp*(trJkwqJBQiR#_xw1SzhFRSN75^S|v;&=HkbUkJ+9FvQI7=UiU+rYnpmFf{iG} zl-6%hzfb(qI)7&}=}h=7<2byIUWYTJ85kUo>)lrDTTcY8Av>0(t^6q0sUj{*9H?Pr z)->3wJ}TbFTOn#{YbImLf0hW$QHi^_UbUZ$kjY{im|5|=^cIj~r8avfZAvr6Nv@nq z2;GbgA-2;OcvqYH>uDt(@g>S>SyXJ492|}y{zQejZH+35J%?YJy;xpWs;;|J!>7g; znF-q1#$~Ox8Qp}uZ1su4W}md}%qq55FrJ-A-jY*>gOepf)}JwoRPz8>82|po;J7Ib zZA%#y?2*ttEi}`%;_3@_V%AuRHi%F%l8gMl+fgo&MwDHH%wXY26o>Zvsp=?PSDDgp zNV$WNwp1~Zq!ptlOWpnb4GUfn#;$hn<2}+HzlNi5c-S>_%a8esqok;6 zgdD*}_EpdOIDJ&?dc6)9#rLAGRMC7?i&B@!3_Q*yRpFk8;WvlR|MV4Sdq1r!D26z^ zluf@c54sDgQ-mJD}xsB!q`SG{+{9Ut<6(pv*b)X z+zS2-+L>~J9q}Cb(`Iki$k+&R2eUi-1=CxVc=?8 z%2B%6PP+MSB5Vch&HCJLlHi)qv;J80m!P~=` z-_hY6HoIQLFC`gTf4-d;Ft?%T(*wcZ>fn$+wQwi4w>ct!t!CfdW9#S?fU`@{U|enM zr7An6qwe#Q!k3lEmc#FmagD>KIGf$-mFCc=#6M0dktU;>hb@3&fKbj%&mSu0EX2@+ z<@hij+e!*jKSQM_2XEQjsqnMcH)oQK4Vw8;=9IvOeq^*YP~58Qlk0RXLc1L*Gj$N; zEFYS&)bbWyY9aTSuQRJg z^-?rX^>H;?2>>#J!!UJ^PCbXITCwS@!W>Xf_t7A5zZ#Wq)3n~SQD4?~bf#&Qa)Z(| zIiM1Jsxj+cxbAhsZ(Z_^8LrG2_ZGdHOZ@ERPbh)r`1v1L>QX!RGuU)(YqA{CT`uIJ zKepe3xu3mOxK@u`yxW+Q>Im~u+Ps%4R+e!P$PqC~&?Cox_xqQ-Hinq!!C&Kz>xN5< zlG``W4TJv{JM|NKbSmj-(>@E-1xi?E&hi!)N{-1ISC%8dj{=@W60QR zR~Ej)=sV_gQx?hh+!;U}V{kH@rhaf!m6znrIb^@t;p@lY`CGqRk>Ty;-lNO~{;FcD zzYZZxv{R`UxsL5R_iJH{oX=>ZJq+?_YZp8RRpbx`TEuY-o=>I@fqNoUl)@`>JXs20 z-Zr4z@mytz33BT8XZsz!?St_Wti8`{;8tsJO?Q4QDbrHk(fsjFlPV*}2d@+$kTW`R zUHLoa;`Gf53YkHUZ&NRudpA+AD)f3%bHvp>4X(;4Q<|2haa)++dA736oAV;3_-c4a zY_I^{^A-#8j{y%BZsiW=`zMh+gBvDLb-j=x&qgs_S9$Jq(BRQi&x5CBuE#K@^arlp zG~_Sxm2q>G^tUDs&CY<~GL zFBrIE+oAX|QuS90wA)8)A^4UB;x$x-s7KefRxksylWMPRySpnD{0Uq-^aRK;HR> zdF!Eyp0mJqrXDHz)ko9XKXoLh1O_!rQ&v6i9Af;{8rsOzHTpH*gmzvwy_ju1U9PUsloW zE$T6M`PZ>{#5+?MSErT@nAp!;=XAbu+Iuq^s~NC`3$zT+D?hz&6@V>oA-uq%x?*%d zY)rW|0A1d!YbSQjVXc#%p6HZ$>W5})lybAGrRFX&neK^aHF`ftg5Wz(B?wSL!rEev z#vY-v%`)eG*^}Txc2nQD<53NS~9A4{AbLv1nS*XS5(S`!e~OeLbMy7qYM=n^>54#B4p4?elE&wC1PzGIrW zAC!}J=%W`)GF0lsVLMeom-{8WJbMX#kbB0bnO_)ufhn$MyzBUREL*cs^tV52ABP=f z2@uDde*y_}(w7qePC|Gr$?^kF$w{8ghtwI)?``u|`%&^wV{cqV(M%nr^Ao63(W$mR zfiZt_!2Z@Ik2p2ffPC%46fGjYrXR`*9FfxDljcZKZDe`U1dMl8o>V<&Bomu8SYIBoxYjfNt`9+qkr#dumWSDMC z>j^p^sZ@(_{P)vMX&I`aLc(ohT0>Fmz5Rtsou=oKC7ejwT5HZtT1;NzHu15f=R;!~ zb&3i=MFgRjks9j3*|w>eLz0L!jUEFo&Ftzm*+La7&4VXLEA+#u23BmbGAd8M*JP!_ zJDKLm$wWM8bkaC-^Ir=olJq68$N&*119AIU8?2?13CrsI)5a9xN%&~3c_^7f-|;F3 zfGZ1}7p^9liF?CE=jP^Mu(ZODTP;>`d+ZkQPNcS>TOPlgQ0xBLsuASB(%@_M=W~z- z=RL;NV`-RC9o^8MmOsC0jilwU>JgQHkxTeW=HUhsw5%*mo$OTnh+h<;?9WKpCYF(K zn9>zaRoEs%ifPzzG$k`}^zN*d8b;!n;llWL=<5xy*u0fK&P5LW3YdsgEVhWwHaC2L zLF4Vm9N}e6OwxT)>9<22gxdH43ok?e?TGW@WPkO)LaV3K9q@_OF;{8XS+alcc@^LV zwh_k`r3}!;h+${tb7KIKNIYb1_(q?0r$)w2Afa;yE*Rf9@yZ_Oj0Q7D=KT4Os* zFF)Rt6W_)pBO&wElGvAx1bRLfzBuZoNH6LG`RfH}b&MtMp#-&z@cZh(6(@&{|CuhG zx4mQuEMM3QGZH~iTG^YaKl!R?NgvaqQpu0s6@}!Mu_vkn7Xysa?HsqWt3bl%eR?A|_AD6vx|t zkYetM;CmC6XeQZiX&Pw_K{zqCjR%=X1(U;LI^`kpH;C1d%F^GL*i~J}s7#vh7g&=C z5dOIrS@lm4RslNMnM`TGHUsq&lFjCkhLw@OeP#C!Qz0{;(Fk{ci1A|?$`##Pxd!yj zTv`@5zgWkqDDM!KZ``j9F(lBmaGHtn>RRnoHEa9Bz~{-AwlNSa*%;_Oqo`&2GfNX+ zt}4MxIiD(fI-j5fueJ6czoRnKbpQ@7o3X$WVy!a9m7ollGVTeugWE!MN~$2%?6V zJeln;4Ww`R4J!$J109NkLtlR@Z}1J@YpS`#*{-*${9TlfJn=UpAX3Y^?=m}u1o^fz zKB^oZ44DL624hE9$dGYR9`D&wLH5XS#tx!enLcQSKP)ylV-UVO=Yl zLU=_Bjgi*zh_3)DcUeBcUyKVSdJ}MowQt?)WaC@|Hbprn^cE=N-uy^W1fzv%`SN$? zi$>ULwdMz)bCbqG3yW&pG5w2!^9kqKIdsZGC%CxP53w1)kLG8jV#(NlJQb+^DELQS zxsX>qVXFhm_(N7rYuBYaGS|^@3K5*s>|VN7V)m4u+a&D;DM@zJV%9tI(|6g`Uh;Cj zV%B|lY9j4D5#TYwJ6Y~b*uTrOT&9`1dms+WGa3iHjQz!$S2=@2gLGA6t9 z#F1(y462dT8&s|Kk^-)}Z5Q;rdtW|Mg-Q}M7)Ai@7l94sC9zhBv5`~cd1nXpcGB~4 zUlha^TlyoNinFy8Y^`CUSGQu79v1fHE?>f5VxwIC(pzilN!fJvwB*z%Wt{&I2VL9N zL#%3+a(dUOCunX9D4yPHD?^4b%a=o3j;T$A;o6pMOI>zTQwAB-!DcIXS)So+Y9-WO za%sS*{N@K|E&47PgR4rwMasIqlH{;#! zr0|HIDRc%^0z3xysYN#BU`l2#qk_I46?U|*@$HRvsMQ}aM+bRQm{JLMm2YTB(z5%( z7h=(3jXOQJ0l}dvi{mnZv>eRtA65@hLcRd}+bs2p9}uM>l#L=bCOCwj2X@M)F@}j> zFq^weXd8FF51;#*)GM-5ZM$;+Q{wJ**kZ$2PPS&8oIqjVhDE_Bo#||GZ~5fJa4s#I zF5K2ILBzPcac%xi9$Gur1S0z_855m2(Jx&|J(pxY0k%o&XIa2c>J zLJ}3!`>xNJ9StKFLCtx1?7^#T03fdcIwh+o81^Z{vF7$%um-B`TEJp+}Av+W|)r;(Qj-+WV_mAbC1l};*PuT(#vF_)pw2Xdi-c>6* z63VYhA))i-8q`zQnjf^?8ln(#vD{rJiG{Hz%;(tOhz8BgQiJIYH@99F$f^PPVk?1d zezmj6+X2^U*BS6DMejYA{m>F-YnUsF0e4pU zt>y6BXb~^x9O~Wb$DC!_3t^Ub@H%|D{DSG* z4n~6WO-KaV^^7w7QSaZj;I44vi-5pi>W$KeRb}1o%IMkOubX3!n0tV1HaJKtMjgx> zr#OmZ!X9StO55vsszT@M1SUGNo<@<~L7~o$J%@@B+6&O&i)3?PqaS_4;H%EuPP7KG z@Pv$npSz(nIc`0)%_H;v<%w=db3^@G%2HhH=EbC0jfc|rJ>3Fu>dbNrDX_cX7>Zz4 zzmfM16RN1&x^nAJ_6*N78*9yj9_haC2c08`KI>PrBH3#Q zN-m$(`b5tUA1)|~ z>lGPeiA9}9T5FXzaY62zCFb(eh_FpdGsEtQ8P}uQZFyQkZ>NSWXyo}hdFDrG9B(Z3 z2}gc8gllKe*&9IZ$|{v|_}E|xp294v<7CWnBRz2)J0;D!eTPCz7+5;Vf!erT$#+qI81XEyx4lqZ7@EFj7D1)FPluoI&3Cb3#Aa}$2#!G+)4eTpOeQlx79|Yo!n23 zGb@#<;}-1l@t?f|hyv17A(ggqzFK345DW#nvbUEbG%+C{UAy6EclOvPNr{O^avZG; z(2$;5uiYMu{bW!b#+6f(G0raHBxfF)jX{rO^BjmXsr;b~j%ZzW^paf1LgG+1 z6WOb(69gpQFa~K34$BvZU2TxDxZ04$QL74lP2D=|Knlv{P7+>=_9?$ z!$74YM3GlZ;Trhm%4eDkYkwCMA;0Y}GNv3FoI}9Q|5LU@BV$r912|T1ax1eD?qf-z zp2aKX=NBjY@S%DL7bI^dV z!;fP|9jahDgWWW9$Bh9C`~dR6j;_O&@{mLLV|9)aH%<8PTf5r#u57d`+x^_KB( z_|mn)g^fs8T{2m@xr!ht`1C7vIzwmQEL*&26FK-};N|8T)G?)~aH-dM5Y1qZZTNyQ zg#ovPW>g@pWry(1uBSVUd0~z8L}0%Z#!aDu9{p>h8wK*0fbua5TY#pn6tikKvBeG6 z)zcm2uiXSYsMul83N79;V;s8R;VO7kIU`sOzs+&aNk@logLbr0v+~0(V z-cYB7X?eR$ZRyb~2 z>l=dqRxd+*uvuVlD{QzvEvJV?`~VQP=Re46dy*6uFriMEbl7d4;OXHBs_vg?t|u1~dsz#;M?5cFFDZ?DRt{*2EN7mMZ_yZ3#M_`|rx8wW<-TUbS>FH#G^@4pb zv!EcZ&ukDb%Mh=z)S;T?u*aR*4N2#u>hWkOjPzebonv#DUAsWTCTZ-(R%6?ZZQHhS zH@0otw$a#j(%3dmXWnP#{DAB0p4t0ado9NxOoZ}ba_5Jqw`0J5#B#g0LZ(`gI|!U> zRW53!>QKDhnfbwYiw-r+XT3mw*vdwA2ysBUwS7Klm94Q;S0Apzctzvco9$05g(&*{ z==0LxcHiQDH6^G72&~$QaI=x0hd$(X=)vNOaT+qvxZ1k#U9AEOL8e0>fuhd(gxxM& zMf)x$%8bfkA|HT3JIw!aF;;oHZJ%7qbM8a8vL=1|)sjXc^E+P_z2Uh zA=ezaz5Qa4WIm<+J7Frtboy>;WXkq->xCPWb7L~?TMHobclk8$Uuu9+g#mHOxY;+N z3vI}fcGsT`S$p%9ZXgSLt!(qg*9RvjFr07z1EEI{F3ApTyQ^e(=6N+h0KwXnVmK+r zsjTMXi4&5v9WUay&L|?5Cmhs=O~!1HhxcykKM(WwXa>yQ`LdT(&WsgH|J>*-)z8Ol!H^^z@Z@2U*7CogMb++$@9Tq#3v(>!6I-;M9pVwqEyuA$oZJAiDk3; z7Q1QZDMg_EF`30G@W0Tjq0p=9i{X5=;Xahd7<@(UPO;iO&j&NH%y$S z7$nP!X~KU?f(|y&-dwMTT6#J(>fX}G)-|78Ru%$VuwxzcxHKMiXcE$jVEya)cw%d2 z*iVuK-R7phq}&rlH_dRH^JhayB#+*s9Mo^w{#C7;CGNQAa=UMv$gXjFmJ&pKb}zs* zu4wNwvU z^M9E7eM@)tNE^Qz(3?Jv((#cKDo9l~ml~fQaA%WaHOypR`zv7PhE5@N3T`I7i=tyx za2E8=7{+Y3h7)fiod((8HOoCAFo)%`%9%5D%AgTFLzCGs9ElXURmLOl5}7sd1)9&- z2l_vHjD_)kgc!~>s0l0>%YQ6c(i^A)3>eFQEEx_Y7z8313wz5y&WzITe;652i&X%a zy%rejfAiN{VCvt&SpS>XIRT?Z1!MhhKIa`wiVuwSKNjta3!Izu zWE#W&U^GZZ4ptWC1i?@Wpt9Y_*~F%W9Cuw-gd2UmIZN64YNgp`o$Y7k3LAB^Mjf?Y z_S40D$)A_@FXtQruSgd;ZPew?BTarbtQ$lVE~4D5xUkl&DrP)XUJ`bJt^VPWA&Jo` z(XD8yWZBWd3CL?Qvj7qjR0kGD=jPf5bXI4!-EXBCAfWl=!yu-n00=53CMGVHz~I!@ zz-<4-29`isd7-R`u<%<>JRl7sivN}Oa_d~r&;S_=@V2I_udlABu5bZ@?|R%nGg3h^ zCuKm2F_y?dkb?-)6>m{MBw;*Meg}qCg%#wn?u10dH3cyWEcLGpE{z6$*S9=^uKj!h z)wec-ZG0|ap?IYSXox6&6ZMX64X*5sPY!Ob41N-z)Anu+&JOpFo}6Ln??Ny*u{64T zp87T5_4Y5VtU*4@f$2FabpT|>SLRP926shoU~dLT`{%~z<_1@A&*87a%J17ATp2iA zU#RP&ds6VV%pdg-YgircN^?EVdJKA+X2P&q6u%q7I_~&^x=3jd0aywzE8&=$6+QjO zXpAq(fuYg9z_qkH_yb7FOPR|}y0O2aCzA%*jE zZC=6$$qfUja05EC?$pFV4!-cl@4tLVzu4v9xBp>!uw7qv`=4sjpD9xBU!FN%)QznX zO--G7lgl9QUVlKklOywJcdVvCj6uI>u&spueC5RCMFlne`5Haz(X7aL=idARPfcvt zRnrQzp6o6)C^UO-ndz6<=--5R9_U|P!PU1k|5yYXfKe42Kl11$jAnAZjn9fI5H51j z;jerp#ukR=`j3!*O}GkaYrn51fBN>%B>n*Z`pA7m^z^Su|Bsa2m>NNQ)tlO{u?s+% z#0=^^Cj`(B47%6=)peFW&jjHzl`XD$drCsWWUiS0BH!=%o%(r4V|ILQzKbyPS)LJh zq6aK~#cTuF&R?*Rsa17&*EzEau568vzB21yXzvAVt`5%c#4mcVI~Av29A8$KGSV9| zk-MrX7m*(RwDS#2nP+u(!4x7*Z(eD=A;Rb5*ec<$A66Hh?{fogt>tiAXF|sr#cf~B z9yD320Is+aLG+`chSGgux;FaGTde#8I*@N0?`A)P`;F;1iO4jwLd;8te#}X&y%EXVPt8yT8@5geubBWR*YApSiUuq7wi(&K?B!(7Qfs@%&e>giYy*pMY!lF zIK+nyfk(WZ+5O?Z_?sVwZ^fC&6x?1vIJVfazkMz0?su0EyGyS&433xgq5s=UU1Ar|HbIw&PPM35t!nS#fUl$ zPER*)Dv)({Ex)c`4^nFIBwl@r_kgYkiCL~x_u8)pD~Bxm8=7|+rz#FxC;VVOXOxTh z3TGO*R5I6uT?-GAjZj62Ycn5wAImO18ykjDB561_$5=SbI@%mQ z13p|vz`V^j^i7U}HBn6MiO>tKIHaF|Mu?CfgjV|rn=*gIO(+TaAx}Mlgv{4 zju}1_Oc|Ne5#a-A*!<2K>z-?L+ul}EPsH_aM~@&9({KCq^Hx=;3FBbB^j9nflh^!1 zp$o@J-Om$iDEHQLS-?-a5C_{=s}`2pv}9RJ@k1~ppMd6pX~;||1vKgptXvnTrKUim z6|zN!3MPlzJG9SL^DiXtC@+@;4q)1nJQb{{|IEd(&scMt z5&{Y*qIR$(Yv`jHWr#^@T6DlPEAg_!as)IrZ}kfXmyXUbr037H@Fs~;xf@4< zR`D$bLE!t&*$@#UO-t~6h*K>1U&1`y72GO&vx!&{Q?5HtQGt_shizev-z;7URWh&Y zS1KsY2QtXJq5vaXX2Jb49_Pkz#9y~DGAwaQ4l^WXg6NDYZL$nLib?u%X%`DN54&yvd-~Ng*;uKSgZO;p7@}IJW$)aDRt3&sUHm_neISK2Ltd zFOKwyD)Kna=rG3qTNl0>d*L5KrJ2L)n{i8jRx-qdP6F4Y561$fw$ZjdpSjyGhz=&G zD0!2nR3@$QH%PfWy3{9We+v+Qv=)4_5DQV@5B6)4bM9V6klSaX`f7fgW*&rFBJnK< zpyWLy>TYI??5yCI96H`c^s%zM#S0?bO$@GOb=;N97!LVU$h?`ff%pArigrdB>zl1K z|4WHX#S5SpTlH8O#`5pV!d{UsPk%WEBba)aHy;W~aEG!j8>;ZFu_HQa!!oPKLrz=h z7N_tECXlMec|Phlnf~GVnR4|cZw)+s(20z z_0FNJ)9*xonlML#9GP_6?JlhRLVql?IXNmy3Jl=w;(hoVv(#YHaxeCuay7#Qu~*{R zM^RdnX7?>n?tiYD*>;$4!{<9q;bo%FW<4Ti)Cl3^MJDWX?Hjbi6a{4 zR3?CyM}9hrvydb9xB9@g^N^QIb;}9@d=!i!nF$e8iiwUwWm$P^BdPdQZRBTUpE8l} z+0r*jEi*CFHaRf<%l7rIw5_s&$Mzt6Xm19!p(99DTAi!tUsWpfP0tnU5ck^W@e#H# zbttH#f+-N>KIk48yf;tI5lkNHOdcDCC#1kOmVp(HU6FT@@r71mEQ=oNKyic`{)7u! zM73DGY9Dr~T|{j>j~1pVxZQ@Y^6ImoApP3HFiTgZ<60<7Me)(7{ZN(r!&lVn?r()7 z&}f&Qm&?#%y}b)=4ojn!9Th*dEPkg)No}ji2fqWD`B4`h*9aFVRaq3W)@xpzqj>=7 zBwEcPTpfWa>vbi|H{XnpLx>*Icb5ZR*COpgj;%h%@0|Xom-sxVM@lAJSl;94X{}vN zVHz@Ud$4R89fqc^@T2P_{II8R@; zK0?b`;%jPp5Oqinyn%XEg1E^Hv;>g-xFs_y0!LNMkA6iH5TiAjBH-WtAdG(uWm)|7 z?N8qa$X}spX6Cv-Q^y4>VA$WSLUMYJtmFV^?}W(QHA!-0n&3AoCk{hJZ7}xFKV=G8 zr6<1`Y7@DYsslJg+^O{Xcyp+oqG@e~@paDZqhJI~v&039pJXNDP0_ti;->*~o-pU* ztR=E2?cG$T%ishLyOv_Y?z?5n13x?}65C%r-_7>0(wV;3Wm2m@h~IUWBV6fYB5sgG z3oj*w%H|e_Sf&PU(^R|B2=u`g4@5;o4ho3~{CZ9C`Fso&`jD_|+w6ne_isE>ol0XU z>%FbS!%n*?7i`%Y(qIZ`LrVuntOoVqEgN1vG2Y?*drARl27`v}^lu|^U}1xNuZ|q> z8e9m4`;DSakqJ*6dQh8nyP7YG{^lre^DQT0=Qej6qIV{V^&xy*8|p{AdUq0QovU3n zRF!y5S@=(-104`b4qJ1bZfX%oH4`d@EZKxl?Uh*Y(G=mP10tF6$wa^wuKDaoWqEq1 zEjjW5q)mct)D|g8$}8o8W&C-Pj?=jCrAayF9)DpvReFf|Je#c!&Sz_*TG5I5Iq)qW zX;Igc8lRMiC!*aHx=HJr!H0*yP-T0c;!70w(ue{CwvT_t(+TBK_m9{o9`C=uRLDay zv74SEW-8|HYEaOGOR<2}dl8p~MEiI=+%w+T&+?eSg8hp5(eLp$1CxihVk-L&KP_`3 z?dAE$OB?#*AAX0@ifh`RC4X2MeuWUCF$Sk<8mFEX#fT(S`ZSdZS?;(D&^osPIY;Ki zNPngyxx(-QGihC60@rq0u%h0R))GGb=}bXO%v8w*N9^2{=}ZB-78VG$eh$ywA&q8A z+2=I~THMRl>^Kze`Lt1Wut|F)y6WQNTN`Tx(HZpe2ES_pr>)lYQ~$(5S&JJsJ~3o0 z7lHu!4V2!vcSXx}#Im?_^_U*@CBgUBi1rd{?+1aBSDEU}+{hoMt-asC7lm<7T8Ckm z24FM{X1Mlq&<+1>Lq?=n1ILpl)n)N!46uM?9ihfvtQLqP@QVG)o&AxNl{YBK?nxtW zcMLU&Cf=P5>YkV0%+K^}6%*N)ZdXfItIy^Il!%diE&l^r80)eFGarWgoqqNUQaL5& zlZURq51U`!R=-$MqeAEau$|+8ga_`}^uOx?s+sig~%VA4xwLpOWQo<{%OSQJfd@ zH^vR0(?*rsKQ&}-?r5t6hQa9{Zv1w+A6ze(`<|)Sb%HP7e!A(q zEd)FfTx|pPvaaH7MXVS(=Er}M57^6nwyjzW7V;L(j`m>L^Lm1R7KxR*)XAGgh#Xj4 z+P=uy(%;2UF)Atqvk}BXtLY{K*#S#b3dA#S`B-5&!V;{C);mY15p*PBKHyF$_?-X#@u=(Z>KMY{tby~mP%Iob5r8{I*va3`6{D@?Xi`s5}gpSf9 zl?8l0g@0Ex4APgeT6AGgVcv#Y(&BMWtFj4N-e@f(y+spElY_aT1{8T_Gxoldr-Fbg zF?PkB%4E``bSa&~i$M*fhe+2`3i!cqdwP%BU-YpB-c6Gjc=bnd+62BPpmTHbSs&!^ zc(MV+4;TsKSDF;&!p*C>Y!4tqWNc+=+JN0BH>GBC1GGR5+_R$K@k6boUR$>twjS!8JOq$ZM9L1&|BC2` z>l;13Z`Ob3kCZv074&+s4-KmZ3!y=Fu?z@KqE%a3w-$kXIPc3|_;75;HXOAWMG z^phTQ8vbKz`pP9ntk@Cpt6*K;_RK}3tR|nHyihQe?ZaIWWi?UJJy*{bB+O{PRq?cb zpW7N7RH5{Mc%;Ic@<8(ux1FiPRf>+ninUe{7sq zXb-+v3R^uYs-Y8llhk#*tw2gl(H$@8 z14q9V{Wk^WbW};2i{Dh&dlgqBs8OsSoU7{IiqoSL)(8g?L3<}=hYy0PM1xoUqTt)P zI|Y(ESJTVCA^o>UREbR$;*D@AlKtzj5f#`4k(UK}52Ox3w(aRTK!JyNI2DyB-9;E{Ihw$G>tt$euR&2qeMIb?69IP5xAq6_j1Iy*oBC>CHPwi4hHB!1HxcDfj22M)L zq>`dtg_&o~&pC9qGVnyDe<*##=h7AB%@hUYwJos)y&wIG7_1}tX)aJK(XCEbdZRZ$ zny4$5o#|Na2JH_{?tql`lVlBXXG%%O!hS1A_vcen4WF5~$j^AIzh|8BmzYrG=pqC@ zj`WzI^A0w8n+JSx+_UE!gM2^yn!74+_)5dLGlH%vF3%VupZ5C`u;M95I@`f#C3zkZ zrN=@g>p1!-K|jU>Hu08GINX_8MEwACg)#`4PQw+uv(Gms!>V(3uNp(>sOd@K670P2Mg*ZegBlhHG*c?Vz zBR3^9>Fkh#<&=b!ybD))1oWgc{#NCem2D% zRI!OeKcIS3R2;4=_73uH=5<59e4zr?snPk<{*%m#L4mJXjMjTHe6Jl%(3&6KphIak zk6{~DUO_J9N**=C>4h<4Ab|?g4P)Z3K_)ZbPaE?&qfN{+>$1NKt4re z)KG~ydTmYE#kWL6C1-f$b0><}0t#5IoPtpqgjYLm9rXJ6wV0ZG(Oa;$XMA)`)a5>BAaXGOOlh8A zC*(QHCW7c<7GxB9Ej~Ob+lL=IVCpG;TeGl~pT!!CmsQafw1MZ+oJ$jBbO`HLCLGLo zteSZ8&uc%haxZ#w5XG82KhzFAIxZTxRDdgXHtuh@`SMX*T#F-m=+p+r0I%u`2Hzqj zzl)(vj9QZkk(YJ40Gg9}(ZBP}Z^eZB!cg7G5IgG9yaNDpaKPPTQm1G2XRr2z8T6{&4M5o_33h z6$JNsv)iS@ZuHD(*v=g?7f^VBd)GH}dBBqaM5RmMmGYf)<$8JhyUzaEzglp}jUaif z4}at=N6XD>omYK_dx`PFu?1IaNlK_Cz#e|@kTbW{-&XfZnxt=~yFHpYQ1Iapqsrb+ zI=b6;j;@yL;WMLM?~gRX(VkoB&lD?;U@m_)dAe2@iJ{@KV%O`?02G7$mt`#Oh*yFB zG*r2jT~$sGR?8Zyhm!XALAcFMZ_-`Q^nB6uu?e47)A^ zyCPj!z32;X3uU$)@*6DkFOxg%XG3+WarR=w`q&+Y%Irtr2c(`e->vyfPK9D)aA0vA zZr<43Zzdsa0m=1U?eOq{h8DZy0Zk(d^r+1h?no+Dh=9rIX^-I4#iSRJ^tT#FxEeba zHZOX4HI4*c?RJXfpLV~Pv|k5Hge`>|@D!TjaTcVikND+3>N(}X(y3|?q(!dMDx`It zxx5H#a7Y2(g->0AHfa|#b*QjF7mPFa1o?mI=O7mmAaC_;KEJIvic0iEAxmkG>YE{J zcgCg8uXTafIk%d$G}mI7bw4jp(f#cu+iFkS_;rG3x-^g1Lh-zub))x#lwk+%vUH_QL?F%8wdGU41(mr}XzYYHWTg z?}ypRyFMBzRl#a9Y3BhM6t7chjLcjAPPB>|DjQAIOTcep}cw~aaLwZ4>92_f~IPB#$8(q?B+bcui=XW50b@IrOFQEl(}{3s9fD6S10^K=g2H5UB_{Z;7$*3l*$ax#3)JdOTI@C8wDYU z6iluC+uZ4a6|$fpbikzCzF9`VCvFj5M(*9^jfFR=cNwxg((CRn$4Cc_i0sE@1&@2s zX814XWbVMWx1d9NP>LEwX?osHfY|!NUlH`ihvSvxukTG7?s);|;!1K!Ts#6eGv6eK z{3jSu|DQ}AF)G3L+ij?HK?}3CS+U0$1aS0IOhOdeJLVzG<THQ318q{@JrTUNvRG!VE) z`}Ll>Gw?k2>6!HT{E;5!rVNo{5mZ8@)SK!n#pYrR`o>fxT?*Y}M~L|H{ztG1gd3OQ z7Bz4qJycnTcQT4T#Y{vkIq zF>9$Kg+q1M&l6Qy>;BwwiqCBPAdVr4+E{GxW7MSnF2ERE85M7jmkS-l($znI>G+__ zSmO4sjyTK>wM#b8+Ov3@JBSWjEJ{qD#7opkVjX_(=Ds~uAF2N$x%*f} zmM)i-w$%WJKOBqz;871w2g68y7b=UV<$TiC#)BV}EMZ)mSFbMa%-t{>jim@>sF`5H z_6Z{?H~DpjNsJ@Y9{%cqEo{pGHCLm3efCV)Yem7#+VmUQOG~6Zli=U`n)62dChnbA z{Mh=beOt6+H*_$OGWQZ8VWB)-5jPP;w1F8KbEo4e1#xRAkXpvO>%&jlWt6j6o%oFe z6Z%J=rG|f!qC-p#4p{dyX{DoNv=85Z? zt+w&tT{%ujOh+MCm-L3y@mS_SLF)Y;fKfA0@e&W zQO{AmBBIlI?=3#*skm4tQ3=zES=J;-cseflksbWKBtD_UwrZH0+DQZaPDS}Xk_-=% zU15t=@J=dsc}Xc7Ts|Irvet3;ZkTegj&^l$7PSL6U}-MbWg|1oIIQw|-}5^GDg0e^^f(B%K!K$JGbD0E^q^*q)k%Q6$x_bjm|>^c?)X)!WW_bzYcrdZ!6 zAeMN0?xAhIe^aP(P5#|mr269f=)VYU`Kx=6{)G1q4WW_`MKOt(Zi5aOWk%93t8JCz z2#dOBY(M;Zw8U6pn`H`2CY0sqph%fl_S5Sc(E=0~hM^SC+P1}1{)_1&&yV1En@ z*)WfVZoY+kDMEjt(yZSAD!83+NMXCtDfu&%nRoOd!5s(x!wv^*vrEneUPJJ4d+n;& zB;zLN&|T4YtHQ@o6sT3YBQ1VfJ!!9yoaw@h=J)4!9aai2PT7cYHT=qHnfsc zV+?yuZ?W$B^dr@n;Jj4li&w-Duo5PqfEVLhNz+7fE)ZoVNf`#W)w-p6m2LJ_Z05tn zO2Ppj$Ly10mRk@VYKU#hMnj2?kR7BOR1eQ(&lmv@Qub7t8)NH`rbm;8-Ao_oXIJIK zGeS%+Oh`c6!=?AeF`&A=w3;{hUaT$p&-5|haCbE$ZYtQKB|I; z^7#kxExfPle%oYfKV2l{bn^$Q8`3_h*Jla|on^XrYHeCTZB}g%foK3+ozg!_Z;=-5 znH2;lDpQL4olIsbfRQrmHx1$}9&PL%+VMa(@sh!vg!#8eD;C!f!fU#FJjZ=PTpC%@E5yrHmLjLBDs(c_uSpqTFz&mkM+!p{GixCy;4HfS%ODF1|MGJy zk`>XejO)jQ5&;8islLa09436f@+l@+!a!Ydxc^XRnt!E)a+ZsAy6vxfYE$cV7WWb@ zr18IrxJl6AGleoQ8ofEji>+?acF8Xiks8(rNR#2@=9J|tVm%!93zQ$wq-;N)q2YfI zF&BwW6C*Hy$(d8OMcDM8BdJ*xC_MTf1(z1r3Z~#hs~Pr%`&eFyhAuw1?P?3{^JDm0 ztrX(g5s{Am8yRI4CHDtpzyp<`)U0KSIaK@~OzdsQ+ELma{N zRtxkrs1(Y_l`&>88nCd^?VnP5sa>G0CXZ^2W1{%v`9SmvA~G=L_TY3@FIHVqs>0f{ zAtU4s>oBytv1=YB8|AukqiH1h~@Vso>ebkba=NiU!XkJHdWQxT%4lH zt7<-bwlEVr1Lz934rN35%J3r9M3?x*J6$H$uTNES1Ea=$J)g^diFI9+*IBX{*rF!i0OXwUA29^H_RP`xE0C>1s)#B=+_}QO z{nC3=P%KVip2W8GZ9_{zHx=cNteB@ndNq%n2BQ;`br1+G2@0oVQ z03(7p%Mx2#RHJJMf+4RfCMZdN)AoIyW1@Nvuz`ZV{|{O<7ZUn$JTPSq8nHtPn8!v3uNME0s z1ATN7`j=I=B&cE*L3g)3DM>n-OihWHL5#&ulbwoc`AFs*^|K~%QU-&j6w|dmfbj#i zIy$Z^;PgOxC9Gy>FQAex=+PJ{mhkkN8eaP#Lrw*QzCosRO1lO_Y4K++EX>^ucDJC{ zb{@N>+fhDm6R8Ev)68}FU#86fM~^UtJC_ggpjT_!XVPNfWl8Q`qh$R(26QWn1yXWn z)41n=$R^8jmYjdr+`<`pba(Y_!0d}|&r0;VSm#M@nf`l|GkOtT9Wv*Czm=%w%y(LwJ<|0OeVs`g_0*{HcXfnixp zLR+$1$A(dce%!$WYY`9r)`PP8`};>>oOI({DsBoQ(^?^Nvr-BR&J)-ikl$<~{FDc~ z`DX3xq;RyL^BX1+MBzIbOpBEVd=$OW{FbHIO7PG@cNga7nVs$=Prc4u_!;Sy$qQR8 zYp9}Q>$zvfhG$EcHEP}TV&TZ{LD-+RPJ0al^p$$+)W)vgewT2?k?C7K$r_~y`gkcY7r@LxIq{@n#*9d948@e~!)P$(Qi+J7E7F?d9k z!LktY4Y@ZkbtO6ljF#N}9s85Ck`Qj+FQJkaba8Y;vc2JQdqXLt2Aps0AU|aHo@XFE zAsEjU4gJVxPazmla3dkw&tOS%c}_F8)N0*>3N>8h<&)tQq+J@}^OP?=n&BtGvI1Y5 zYCqb4Ol?>MH?x&wgiM)#!plCU*1T`2|7H;BT<&W6n2sYw2x=4$3H}7eMo5$mN9s5D zFd`=`oqH2QJl=&d0+bVrjUS`=k;by~t~bBDBN?7mLtYU;xDd)uXnx)Cp# z3@pse zh4+=1q{Iwb8_?PvVb^FYU-8A0b46(PPQ)NK89%HQM% z_|DId7z<_%{d6Q8l#G&RGtd$kZu}#V-=W^V%2f|CIk9}e>q(8=!Z;prN%F^J19^P- zbYfKWCf{qdq?rfi6a7v@qC0skkB@+L!2ysT~a2Tgpt;&f$*Gi8i6{8@W4X zwP7VE!4qm8H$<7M8iM3t*i?gqHBmPKwx7YA`vEV|gGR-secw7krC?Wehw0f@-^8XC zUC>7D2VjG9rK!oVe<{-~qvK)EKaECb=wdBFTX3(}*{`!<^d6JL^zOWz*I4Qyjw?4(|pp`sr2l6Heiy zRT%#(%tS1{0ndLkj9~vbmJ952yD^oPpQ>l8GJuxrngg%$U(yKPF~&Cgiv{DTEvls)*x8rn{YN5HMF zc6XeN&|V4^hr^%^25;+(!oQHI6SZ@f0!WnLY(9HfEd;^#q1T@u+&Y*xuqNysn6AoZ zQ7QA82+fMV(b@B9PuRnj@7pWV&%d(Ns9_W3>l5uM#OyV3)Y?|fL0{+oeNPh97=A{U>s3Dm~0bt_fI<DzrE~mMto1f zw8Le3LBI;jeF%7P<-TR9pbD~VFhFj!wXohdLLI+z)^`tuxwumq@RSI&f~yP1>Z7ef z3PEPQ8dCA8LNvNWW|CoPmd8!U7@YBsn9$a0%s=}Y>EHh`TaejwxJdpha)Lo-bLrW^ zUL4SuT+nSo9}r7P-HAa}60(k{?SF?aTUZrOItpL*%xf>U8GJAt77@zT2!!osmvZy+ zq=c292;9gdDZ(?lQ-YJzA2RRPT*b|Dag|@_?{GBAZ|8>v$rQAn(7hn?7%whQfNNMJ zBP{egI+J0NVbr+!M)(-1&?6XtanJd==iKhOj*fd%y;jC z(vgE%*KT)V$Ox;GJ=i!S%ZnV7w;hxOnM>h*zi!LtKnM^sHyeAj0_1*TXN`{D!uK&b zUCZaF!&eQo(Xrv`GFk5bt?bk8>3wut+G4kXp*Kp;ehQ z-JDi&+c&_kPp&mc%Vd3o9xY8*DmbA;rEgKhOL_=l^)Vyo0pIj$?FP5b8A!DZJ<7WJ z@qJIo9-PTpkNQ%(&>(7C3)pZ-VM)Y3`NqnLlZVa<&N7^=m2>e>SvH7D%OX1x0+Vc5 zXG?wP?KN`6mjo6}@iUKU1^n9(hZsz-QPdneXt^J&eWj}Cj$gqtDGTz76e@>}jRok) zgA3&*>tRVrfQSQQp+f=pEeCcCsK{+WVHXbKZpc7S$FXd~Gl#1=Bdl$qfkW(lFzm+F zj-Zc|NbPs07fV%Y1>mPBuAgsyvZ!ONxh-2 zkB`q)5qTUxhu3`IQ05i5EABcKc?s-rx_b^F4mn|m?2EY!xZ`md7?~ZH^YunG7*Tb? zvBR%-Iwz=q#>U2!p4=uk51r$OF6DJXMBTuSf?d}Z)%rZSRXbL0Li6mO33}7CheqF- z1~b3509S}7ma6;R7E5?fZBRdoMd#n{{|fEAs3!YoxFqe0ngpK(GuwQ0Z$wNo9x=L| zzcy%CsG&b@N4I}0Ntv%u9|^M+O{vE5h8IU*Eg>^7#uwu zEN}QPu0YX|#G5hu8wu6=TrOtpc`D&ZA}gibXu$Ilr+n@@#qh{0{C0G208^`x! zzI)CzTW@&djKFkg^2XMapz3wv@5x5+y$n5`;;CZHyu3T(kBjtCAEG@3@%L4SV~7`H zz_l<5uA1%WK8gB-G8gjIJ$POX{E6+|5L@)rKXluPzWCXBhE#!mrmF}_ISc-)9!<0R zsFT)^O64%ly8>xz(&EHULP^6W>$%!DugCJIq!!FUor=!A18;m`l%c8UH^GUiu#ue& zLG62E$dGsX9wI&DFDipK0!m%(1@sdt;Fn6sl3=#&pPXem&NNK7DJS{TL3v3aO+b$i zL-INMeA?FJPlEb<)@JS$`1Pl(5d`hY0MA5Ko7v$=wujdpwYeWd)!fO0l%@Np<9Cfm zTLb+cb%>i3C|z-r6pA&}Xlfq=AiX_D=Bb0g(tt~hl8v`Y3A%U5!}GM zqMH2AQ#iKEe%*$3MiIqdwz zJ{GctT=bH}5;|pZ#M&J}dJUQS_W_o7 z6=ylsu_~nEk%PWuzcSlX>%dMbH%+E3pUsXs8j2Rp#?^@*wCe+mmmO;rlCY)esG5{0 zny9VKnXWVrb&>g&9}*6T2LXDOmLGdbeZi+EDJ}>tm4epD9F}Y{6n2xzyQBWEv%(UW z7ZeW7?~ymUFmDdcLVms*z#?6L$u+mv-F-ruCRsZzbu)&0qlq)ECsVU2%aa^6qh`sI z96jUrHR)Gj++V10p}$ObOwzC{W|Ab2v^QRVEfx3DeWd%w`KKmw1vK82L%g}TiA_Uv zWXk_Q&Tvdr6e+iqh$@i#Ivswn^$LqDUHC&{2ldVW(7J2JZIi1J2@piF%ub@Na{z~F zb|f;3ytk9T#2d_fHW{(#?%5=rDRl~b@Tz!jH5fS^RUV0#&hd90GohvLLw6ji^4BISeh&-3u0WqOF30dkfYq0S z+Q=0mazmEC>Xx#zi^ecT^VY!%J30Z&5YBtN{C;S0WAbJz?k3iK%mp0*XE>VHb zl`K0YIB83}AVsrHG-HEAoA7Ahs#QiT7F%zbKJ5$zInd~@9{tuX|9Dt(;z-nNUVT4j^U0IsX{ot{gYHMoA`Gd zzVS3hQrvEq0LsFD50j{Qcdk(wdEadlIhbowk{=R9BL^VqAX|*cem9U2CZbpn{GR+d z)YPZpoO)yhlKEfTuCs2Oc8wL^mwN4#0Aa%ndl*>0LX~1zodoC!E^#0E zHD~G>zhNdrHU|~%M#K_K-*3z#N{dC@io=<|Y$5%|j$g-XB|*+G33vDD<-TQ9!ST9Z#DRHb*=*ad| z4D4yyC(0zYGRWdF0VzG^NuY^X5Zo*ZzrJ@ z|M@X(=Sbht>hGRpbHVMh&Y|laK`oJR3)5Jxuf!uO81TqmF0(&oM(Gass)tv;DX1}dx6@d>S1VWordrEW|GJa~`m14K&S@fV zGu#<;;<#(2RP6@&mW3EhL8 zy@8GjLj!EF1@yqQ_)F7x4CAc7HnJe{yT_6JPPx~-sBLZ-mP2;7wv&o*vh#e^n@dhji*aql|xS!GJ@2WiRn1TJPe{RG5_4!?Gf6BXw-LUE{?CI#)E0-0IR%TH69-t^QW=x4d@;X6 zdn8b?^J96&mmy_e;_|mxmUp1Iwy&5;M8|Q*8aW{D(iSf-TnCT?=Q1_khNB zSe$ug4?+%2EPG2UjBrtk|ICIYd4I>Wv>z}VPLu<)Q6|^20OG^#LPDVynu0b3f>`!k z1eaM^EIoqeOBNh>(#uhq?>;a^ z@&MuN7;v%NtIZBSI_0rw+@QQxK_-mTEsTdE)U?({j(^k#(9VEn@u*(j)+e?rkitb) zA)LRlkZm(cStI1=U0}I?|0GIaHCCe0RMel};+awIrLFdgx&Cef@a1c%8>Ed~euP62 zS4vc^xc)^R-2yvvSC)S8FrYbTlLJh|?@RclQgl5b)x$OVNqG(hW1GY^5Vm?UjSeC+ z`h@}-=|l7~u2xh?X&zVbC=7-#7QzjsF^Y4_5_|ykS(~5r1PuMa<7=n4bL(MIjpGk-e@a3Aguk#wXSHp(~ZaELb_&l*X~}!FXRvnb2AknfX-Bs>IIV3zXQR| zVe_An43mbwag9P`HO34UA=tBJxyei9P;KPe5?)M0ejdUtkma3RZ;K(Hy*Cl#>S7`6 zU$bme{5B-?Le69yB>Ck~Oxvk`=o>8-Ao2O&1+ZmCAA`+6&xQKtvR0Qr&*Cq#Ls=J3 z4ued&DqXY38cN!iv#LDHWC@IfLz@E!_mOg}j}LWB<%IVhpW)9`^HkfTq5Jlx@sTV=_*0}uY!UAcoI4>r^&a|c6UnD-;lq?b6;46+_^K!oa=;Nv&n1rheW zD134PzGe+V_YBXgTpb<8LF=%MKBEfg&lmA&B4OPsmZo7P2a1lBgENqtn7KHhlH0?P zZXKK|VRuSWSof`}k7@r5-u&K5RyZhHqswr2I|uQIz5-?C3lH~Q$lR|QslsS|(g*K! z`N(zv)dchTm$_b-mNgY%w3_LdlPn#Hr&cX$==a_1`vvktMklG)i#A+3?r3HoK%dsn zC3tvrQYZk{fJPTRnY#9(sHjh6ewsApNUXm$#ojL}z&!3<9u8szAn3Pfj>(sX1IJ z*lR)*-mB;>vS%MCPv8udQ^H4CGoI&0xuJ+Kx&)gWI2r@1@(8Swr+`2-dw8F_A{?}e z{rS8_>p<&lp1(9JaPMjN0t{vf4~CD6P)ciNa+iRujX`$I&YSBOZJOvBU{I!cD1_kNUlUf!XX++%F?5ZB zm6E>D(AV$lwg5tn4l?^3(~_}5#m6RU)42XX!J4no)TX+V*dHH{sF=2GD$DD9JtdRf z!@0jxTgwC?Ykjj))>8MS`8Y%jO6##tW#@@X+j_-`9K-6rb}S6LCB7AU}}!%ALBT3}^RGJW5uB9zLEV zN=0=4zZrd7RmP>!RWEQ8d9Fu-$d%vly`^CvuWYe-xDVM!24u@UQG*mm)z{#!)J>Bw zVBJ}Z-9g8`1l5XbGGBZqIP6AFlWB&gb{S)YlNk8|LS9J2NV!c9!s~M4Q*uVcl~Cf? z?9v|qcvc__Doq$sU+1CiR^&DsREIWtZ6W_EW1MN;)W7Shkh59|z70GjO7u(F^j!-E z$1<2$iJGi5+j7M$WOm((vgGHY?kQVKS~#6Bg4K0xTRPb|N8VDi5}?w$emGBbKXPZN z#d7RWv+na|B9#LEYp#a^8jRh^exTvaIRt*96$S9$sRuC@A`z9cJUA|^y`_fPh=itU z|N6kQ1g|VXru+h(;oZ&+ZZ?aDS0mlP92@g=0DLNSY{ueM`R&0M?m0bmAK(FqjZNnM zCFY)u8K>TU{ybz@4w}O$gK@PF9{sV%1rX;tg$|*TGX`;nIsJWa^)F)_}zpT%) zC1*+sPj{$b&?_|7UBrI|aNi^)MYw^TQNM|*K)U&e@$5;pHjiaLXpvuAHj}dyt zkxhq-vwHsn$uu)mc#&W;2IJ2b2=^pZ)6)*Du@f$|%QNZ zVrrrh#%`b3V3TAuEqov4axmU|39<7VNKp5E>`uDTi`GDOX&&RY z_0d0ZgtB3~c)&vACme`NgAa1r^F-D&5VYDVYqq6 z5wqjLWu{4kcY@l*%_l{mD~|wZs*JldFjYmSg@Yfbk0vl?w^=E>sk!)Q$nbQriHz%# zRsn|qs^7##pkQxC`XP*RM=Ze2>OxaYB97EX5@^!bZgr}}Y#pD1OH?+%x+x;OVr8{# zszs4#;T=Wz;rsw{w;8nY6hqP}BQ_dF%_DAf@Ng5MG$8R|CNa1J^C8HFd>n%|A-iWhz{LhTB(SpWTE*~hCuS9!bc&SG4oXVqw2d* zi{R8Gi_v692}DTnNodnVBhaOYg;i*fMRf4&5IP~N={(@sk;b&*S4X$|M{}z1jv?;b zMse-Bo{T4pf%IcBaSuTH=`sdDL{LF7F1<@7v`J`Ym|vgBXgQ?=%CD~`An05q57fLW zLFfh>4j)zQ?Q+aMu{10`d`2w-2GKCA+{6x=EHWOv)@$!_6EXrDf4|1zW86ISHrGX3 zxz10*g$UQOVGG$w9_AI$;h*8I+F1$E#ZAm0POx*I*J23u3Mj<9uS5y>EU>~WM1p{V zy-$jg{NefATlO4$MJUKhgK)_uGL~c=Jd9rZS_G7R2?<7ks+~?aHR1wq4(*Jj2kYLR z!lKUZHq;zCMBu`{OK*v+0?>x5Bex_O33dhzHTSaL_C`B2q}g ziY3~tMTOTkZ6pxj)8qi%vXP>P1Wy19E{nYCRfLDIIu0qkVfi6P-X^0CxWp#7v}4d# z_EpUjrr{4NmZFOJaFRMrkP^75-=4$SJuyG)@D}v=Q zmAP5ZFY|vQ(w=#E{0z7D+tArbgDk=X#k(wLhJOFdSEF@8J#7cJl2+}!=CD=Tbm2zB z;4i8$-h%xmK9^pY;K5VoiFyg~rXH1jus&`I7YYpn>xJYJ2*o~P+L0PV(}_yAKJ0el z6A$#*^22m<)&KgZjzvZPQ7=4gMw<|Fl7Dn*dkEEhS^nJm7k$q?f*#R8eTbA{3bavg z0ky#dVVI-qo!|~E)4B=dMtqML5n*e+N{I%J)lgIno-b}^pCPZ9@&kC6hvvpIK>V2>wQ3GCy4($|8Xy=w_#4_H1a-KCTV}CF(b6fAJ0S?6A(GL9hSJ zj|j1`pBEO&CYNz5=c_c0z*L2sNn2vVf|=cp`TA(!WN)W_)QhKr8sBM=UWJ^WZPP(8 zU8RMAzbM)YW^Kh2GUwDdd^-8=>qW00Ul*Y$oU4%0amNX*^hub& zQOF`A`5t!lPH2SdwYHM*VENqvMY1v*x6yGXZQ`B){;R=ILtJ6-DQWzLd?BxL){-S2?R-Ml`RGerw?EMU0z%j=Tn3 zW$XDXN@1m*nQ_F$I%X$p4MizMl z@`W)6f3vGzvtJ4QJ>)i-AGpb2*4BQki=f%1H`a{X^mO;%#AXp8eKs>j!|k`Nm34n~ z^Wv|*JN4C979QDlYJA6}fvOw=HRyfcbi5NjJ#Fc!p?*~K-`f2fHrp{+$h8UlC^CG0 zqrAVA*`-`cxc7JZFX^mNRW64#a28#y&3R2uQRJsqE1_N|tvK1o z{`cA-o2ws>kMEyuYLm+~!#dBvJe0;`m=UoaKc9}LB%lDT0m=bUVS$lI*Z003m<{=& zU9XFJPLKQ2wJl^w+Lk`(O_;_N;JwO{h`RG!k=N@1eDyQX;T!Kg28uQ7hE4^$<2`>+ ze&SdU276lP#bZ$5OJ4)c46+htoaQG`cXVko9}Az>%aY0{h-Su5f-)e1Mc&|`$LTDf zQ2-R!g0OR)RL7S0_%|TOlHT8MU<8Zq%v#MzxEgWeZl55ehRoR)ZfC1@yW!(n;R;QE zZ6hstfd*#kKwG&>Mc;LXMk-D+^^soMD)oJ)D|jBCfmOR^yPv0RIm7W6A7zf%X}?eF z2I}>usBzR!15_88zErk>JxI-wTHs+k4_Y?Q@1E-fqKJrHjIWL9P#1 zchgchxQJrE1|pBBOF6#TovZkYM&r28{sl=Fl-?{RjA zyYb-j{pjS~rBv5sPRPjzcak_;Cskhz$!+$pm7{~ZtqSe;ZI6u%;O%Zv0o`F#sW34D>UNq#a`w#sU8L3FkE@OEz&LiCNZ zJ$8c|9at?l0{I!jW}?=!s{5 z>$ARzYf6uI?|T;)%Z)^|9eA|ZDd^9Caz32N9%}-$jna+58s(#Ky~HKvzCdzAbL1N# zoEw}39*=Ez!La8u&b%u8n$3&Gi3feJw?}>TR+}ohi-XDsz=W^}f4XzpF1N|T4>-jY zqg2uDD@4Xt@On&!)F@3s+uHk4OgVpFzs>ZY^Nd65b56Ost3k}#Pr*bR?{hBQ>IH8P zSBt{Lh8B^(3zchh-v*liAIqpNf;2c@P+^E6b}e0DV6Hs7TDT1EQ7};AI~r+MXJ-`# zU~Dc+Zp$J+fQ_bS;%Ve9>cs;5?{1%Wf8w6zAMS%Zxe5mgcf8-IA3a;aks>^$!WZ+K zR%Ckh2Zow1BBN3i_Y%(PN%W2T7*I$fg6NWFj_(UkukICJ*eTXzUk2V)zKfOeM4h~D zQQ}xhr!z>)#-G88!^@)bfg}U5^|rj~cG< zvXq#;RG@$$84!;3g_aZJqbL)$={eclr&?%22fw@<&g>N7C6FW_|9lsAQw4^_mIp zD`J~gxx>$WoYGzar_5UHH1xRlUbX_whR6RHb&B{;HSrAJ-qKmljOwj?1A03 zI`~Dl(E_8-cY-%d)b^-V9O8KL0V5ALasnL(1U0QNG3(XR3b8R;h6Rp!{l#?l?Qxbb zX*MGldQC#5MFJf^@h&?vL2{0BRBz;%yB&;pG?g1_g7ORn6xi0nVx5y4!{Q%Xs|E5w z-@L*Wn@81}upQ_U-WaaedmdIVJAg*RCH_H4anFLd%zK-7r+IcS9o>-`Xn&w9O>$qq z4&Tnu3@4ukSe|%&Z>+>B6Hf zb=ks=dxmOQr*4VOOt}dxp#8%SgankOhQ!W?s4G}uJv*y@P2(349V3QcQ3)zcS3I`T1JmFPHrG7|~{Me9u;%!MkplWv{PA~JW z#Jtb1a%q>AuzfQwURTn;616{r&mEo+0cE9BaZAgH;*_%nr#Jubk* z2D98N>fi~vSkION1X!HGWN^gLlI3NAGGlnRb9VErkUN!Y9x3YpYZ2??Kt;gNWB$_ zFSncYtzx70qBt(E8jO%Qx~#wRT+4ZxL2*lGcWEEV!ybVYsJpV%Tt?cjJ{4I8w`T4{ z(hq=P)T`0JNCC%Lzn`bK5(P^{PPx*5Dss0@*h$3Qr*h2yOu1 z{_>v>;N{f;F_Jis22g4lt7FgK?Ra8-TZTvT*lsvov-5kL7G;2hYXAO2R_&A4@<0o>|4s*}~J@ z(v5`1#llL%QiqY1iGziSjg1zbS=7_R#@US~>)S0PbC%96Bq=|^;eprvxS7Ihb0M(1Rom<+y9r6Re1}kD9+Bv#>DmCSxIQVmLO5E^|B=SPcAKq zy^E=bjk~2OiI=6DyREYm2^SMP6FV)ukkEg(%gM!=f(pRn;DKkBvUPL!AmL#7fAr!c ztlV5#u6L01KzU~ZZp48%?QH>qsHWA_ah2oktA(c@S^qMTxui@Xc|&O;TUOPaVuIn zNH}V6fHo0A8q;uC%oZ)EHTVrU+R9)Qq743_D3UT!pnC93IcGEvZJBf}2-9xk6(S#7 zPBxcK%=Y&!csN9uAHk~HGbkA|$4$mDR|ZB_eh4-Jc9Ecq7Qppp5hFN z@8kkJJcYqLV)+~7HoI5qbMSEVh>ee$mWW8X+#wC9ZFoy$Nch_s1PjDDoEvhP*pYHk zh{SL(&^4C|pV#zM!cRcxOTs5yQ)^W~$jANe;bMHf@a@zKyNHz?XkZ{Hy>oZ*wC6gY z&obufkuR)n03~ip-hO2KoO|P548kW)2pkb}01Py)+YiFTV`q6>! zD9;JWwOwyFhsRl?!H%?6C%V3>g4DPcCQ`!*OrCM75YPNeiWKx!rZO|O8+)TTRkWFr zld2BkI4U-#YWePU0~J1o1lwWsUw>t^Aa>wG={F2#T2)as5TGX82b_8>XBIEGCW+3> z@yNvJBBwSm)?eUSg?UnUthaEswdhGr>NdNuQPLYH!`F~mw*KAhRsIw9{MMTlbw%wA z%-nPB9^-Ih8S@qn0KMogIV_OhJMfJa7_*8`3G` z@XvOTW1@9yCzIqjGP75V7&V!}=y%F$?c}lR6KA$})TlH?{Y*Fa)7xTi682I4ea!3Q zAugf9@zb9ueh7_+E{a@Pi=0HtgL80a4y3!E-=~z`0D{Xi;;H8mIcpw|h2iEj#Oo+rm-| z<47)CZdr>B^E=&K?8tGA=1DU#Yp>n(aQUhWXi>u|Kr)NZ z{@_M18RuYC%u2s6VHxQooc>ZYUnG~>hT%awykoNNYVbUTe;+cdX(j^eidbr}CrGrv zH$fy5Q9aQ`DkJKX-FOl3`u!kigM%bP{6<_WnU1XMyP1W`j3>t?Z%r`>Fup6;j>!r*nsl295!`*u=Tff>jRX#(Ti~F(WI+mk}MKAM0wY&J$9N;U5v5eLWXg_cBWG+KtP>4&2kV`b7sW zXdHWUG&CXeKL|m<6UV|ApNy2B1ZG}e*sISme^f*V)k16-9`6ar{M=Nby#ROoHE!dA z>+YJ>UrwRU4paj*TWHW_0$WT0A@~m11ESq%mI65aL?9n7Kko@Fo2SKFe9bxi*lm+I zomkm@l3C&AJ(o|83iyZdZ0FWhTZQ@#0qG1z>g7bz;k7Uzt^bM)W42c%tEY+7_ldP& z(c$1%=AC}Gv@?ok{7=h&MAjFr0`*bp2CsC)|0ZQb>~e;t-3v1(vbHS9j6$Hxf{0gs zGRZuO65k#JpAzV;H?^%HW8CQ&WU|O)xRqs$m4>cxHPswoDe z{QFa2;MidSqV-zpKam(Aw4X=Nc zVE)3{C*)({*6sgP&)G<_o}VCzfnTNB5T04x*3XjU%YOJhQBmmGhVPpL@W6JP8tg0UkCfaV|D4F$oDCPBB&>1n z9wCzdH3dcJYas^ft-ZYWuuU3alSp`E)+{s06;&ga}Pb6 zi?J&agtn!_Of@;!k{DIUi-~f8psj&N@p{Da9*$ubE34~i$<73BDVNm2 z@5QsHg(rV+pWi{F-K62S#@J%m4B=;&(zk@VQ)c7~B9VYGM{0%;{9W9JkgsRNDDctr_K_~Gf-6~H0;VKVdNt_$EaNIGSE(XoQf3pAs<`0biySCmv&)vg|dX2ccpddhffZ3Bodr*!&wi(?ON_4jii}i zdqaB{++y-1kUPJ?AL6-ScSh?|3BaDYoG=bb>45)q?R3$GV-Ak%(%eNq1X8jjsqbUM zF^AE2y>&s0nW8R2ltHV^pfVIGgQq?D7@+xV`GK%6F<34#e0D?Ukv5)dKan zymtF-)pj}U`ifE7A#H}O8f&I*y4Hm8ieXK`I6+lTmL2&81I7y|HB5y@eob7UsjnG1 z{?!&L`iY^KUpTRh_5BbT1e{7)RW%CZkI$nl0|WxTG**HNl>QP_mhg(owE9DS`HnpE zHwj(3yBwtRH|62?gqDO0B)Q1=!y0JPE(-9JYU~FZ433 z1(hF4@%Mf|;lU-}owkEO3?prVW$AXyT|Kpt-6ZA6PJ#&RgLCw3AYB}`9ny{Gx&%OpW*#JH_;vf zQ*jv@pRw;n!seAPx}jlpk}z z>xlw@g zu+F-47dKxs5vjaZ%idB;IV`yzq6P$^+5+vy6SJpG_@GOEl%-~U@!9^6tDLvOKluv5 z%IK1*UWDqNrVLQ}Ua*^d|4y@OVUpE%E+qKtON!D`B=5`qLT0$nmfCH*ynxPfGy zJA%?KqAi=i5t~YW{E>sh7E zS+HYDq&VlIKA{SG?9E*sYY#( z?8m*g;Rp2aP{wkLF=z;i+?EΜP;Ylfg!-0$oBWO0wJ!oPx7E2~8oUNu1Dd;f7-W zppUc#IkHG4D{7%fB#2=i*=Wn0fys)Hzp@6Ihj3Cl;S+Om0C4YGL*!!cbhv$p+N(QY z+tF{~k{Tg=ut|k+_2dxESgA4-W&~v7Fqb^>#sI@${s@&lND7+D<{(jo#!nFL&{0^a za2=%PhEfC%##wBJ7{jHdr?HHsui&f z%dhCn@Wu1fa?yUzE90CzYeZO}#yje~f+;xzb(1OPF5cAF(ejkvfiJmSFq+`7yW$@Z zO`XkbcvNtN_#&8d=})v&9Nm7!37lx`K7KC!<)JWs_~UIBlh+z2WN2;O7r#zvjeJ9Q zXB+)qtm@a{nQ=NST-4o&XHzjPLEm*fWw^Zh+B;x7vpoCY*%}X;fxMDwO)ACmD+A^R zD6aGem2mx-*QVPA5q$xJ*?-~c0S)bsxl4$=qird~;?(SFe*T!o6~^N10S14RUpVrA z=E({W7_6`^ck~uo=s72rTiIU8u_$OBX#4&3^D1-cO1q$LQ-Lr1oHZei+FB@Y^L8(S z(=8v4XNgJ6O|!TcgUmX2#?P48rIuP3C}}m2X(6P#*72q>r}jf7+ z*#3VzM-v-3){J|6Fm!NYR@RK7EKu5%4@e9EOJ=zh5pAcKWkF&&%x=-t-E1-G&)m${ zlYrkj2fiMvbzhk?{{&)NX5e2tS16! zs56>)=_C}PhKJkjR)oLsyX)b1pPyGT#l)vp!)ZfX3THJ>+pGJmz84K?%DHP(>tL<| z&35WrNxZ{Z^XoWeNoYz7eiL~@sD=NxTL2=1QhXVTD1X%9WF8e-`Yr*RWx)y!5D^P^ zc-531#3Aekp}da^HGe4GEDLr}5pN)Uk$XTm`p7F*jnuOp#^<|y<-UE! z3inq^h*v#Mzs%+)1n;wqWwyK?SHJZesEc8=YyWstX~t^BcIQw3(L`v0I3#%GqSeGXWiY%4Eax zTFVH%K-4@KyOc_f%s>vYQc zB4aFG)L(03Jg`l7rc2&gq5ytz_%A;{b`%kE2NbYCOPJFgilWvA?W%FILRlNfSQ43* zG7`%iRcS;pk=tB)(dd{VQz3H9c~n#KXthLfdDBuGaLvM18-=$h218_Na17$k*rKc8LH3K!H;pfgJu=mLQ&KLE|ih@y|+^16OGN zFtxEob51jym06HD0b)A!J5z+8Q*`%Qe@|;sb5>{)r%9jAQJ;X}*B;};X|`OtU0#ag zT*0^>FWKC=6zmWkfEhPIOucm{s(J}OFsTrt2Ffvgvb|G6i5G)wRlLR zVE&~h>;+Dc2nu5V*O{LqwNGR%LaxPgWl-QILE={GR3l@xAGl3kQwxkzV>mR=f^}hD zE#jE0R-nLDzM_c}YS&YIkYOIbLo0_ipoGv)jmsEySwpR`>a{!sSRWP=7I` z+R2R7Wk*}W#S8(~f+~zQ8tX;xCmf_Ob!>5$dtt<(?#@@qn_w+#Y5dnTm{SI(bhCZ@ zP>?MlaIp|W24IVWS#zfSl4XsrcX^u!M{E_j>@q#S{Z_({>9(3-91}9d$Blrw9f~wx zeS%C}r*PiIiy`aJv!bI)YD8=Dml32+HACN%6an*Ahk4CGW6?CXrD+d3?uzZ*tR7vB z_3s={*fPYsrsndL*xO|mw7%YGGV^j{_xO?UU0yNs6L1k760r#f(J&GY#3*Q*l?})- zntCD0{M4XGp~mYv`biYS7aj3EA3}7;p5wz7qdBM2ln^LVQuA>%*D7V2t1rV`?5Owq zkO6C((sTN;8Y4C+g#~ZI**Qg;1V)d3eIIZ6{C#tNFVFoRnd-qrkyQC5FQcp6k2Q zZ;Bvqlh|>OmDtx%lwnOC?g_yK|Ie1HYoK7YF5U zJ)_@A6>98qc;v-ME33`NM;a`VQ93b@^%x~x0T#F>a^aqMPTzub6^54pcU)hSp_TW9 zb0{KEuia?a^jO%Z=5P7ZLA? zfG>H5z5=cl9&FF5m||?&O_60Cy08o77qtiTK_NZXv6CPt>zRa-lW%6`KvhnrlmuKj zrE-#kK5>OqN}qX?*(n&g@{$BhxN5%zU<|dMx*cRY)sqmzW}x{zNH8C>Zlb#@flEn| z>`LVKpocQm`2D7mHy>|kjG9Ffj{pZX`nHo@ae>hVBCP;2bRizfv~g#4dx4kF(&NmU z#D6}5j&|}r>ut0`YX@n*8NA(&yD=;nijo+NadjNoNK!NP7Y8Ke(1z=Tgfsa8D8F=9 z4oK2rh$tMl3Fv=FwWG6JeYaOM6!)mqwLgk>I&9KIWcWtLm@0ZA%~Xo0!Kn~=6T;!0j(H*SURTsiM2 z>W@X(XPwd%GM^O+kjIS1xv4Jz+-<=~1vR@FE#(l1BFiI|jqrF&rH*8?0C48UyJO0< z=Prp0+wllyXo)QQu8Ms_m7(;Wmg}gacF;AD{LrV2snei=KRFQx% z&FwXZ+6Kct%9#dx-hUK1STU)=cqs}4u-t+b1M{(`_Kvc1`&gqBHU9sAa8c8fIra)r zEJ04C-0|ibKdfJIb1TEJO<%ydpU>dkyp-xCl17L@wl1ONK3^wO)5qROIT1FAo7|;h zL}OrAYo5obApZ2=tg1V3_4Tn_0`qU7j7G0!@8QgA`3S1lse8rWy(eYlGNoZD%#I9qhNm(Qon+V{Mn0_vHuLn%q$p4^+aB2dLs{abfjeYBF;*qnR`^o37 z+D)bB$uOG41QQbOeGB$}Up${U?o+dnz$ufy{_)^Zb}Md_%#4}5HhGv%ojZXr<^MWp z<>h1xt{B)PTJ5ijf3o`tncr`hKJ6n!=A#2R#^1TO?9K%AeeI#2L^{hb81rYb#bRp& zF_H#7O#2X_#&cF72O;I41jnK<5XdGG2Mst{h|72Q&dVMP1+?t1zKlO#4%c@#2ryKc z>TD4YvnRTh20X~I=UbVTx*aY^vkEp<+}Uzcq;s=o|2+AsX8PabNC}zpCyU)oax>dfjN6Dc zXjP%N^9|;_t3)u1#C3@LDB@0ywKhOY<}A+fa#>o7ztejw@~T6}e!u*X=reC_f(3HMeKBcwI7m`;G`7OnTcc{kl(^jsf87;>WwK z)*JTBLLQY%hkhSW=`rM>m{az6^bo~n7%$7 zBbZID92ht^xYnEIZKB}BRwqnEp&%Cra~^xJuYuZTNYn&Au$f{lpr6nAA{Gzn5iIWL zGri;ihx1BW>!lnmjHX;Y1_mTHf<|VMgB|bHv^k+x>OX5+b2Y*-%&6bjm$fkF!Rtkv zW?h}Gz$N$8$Xyh}IZrSdA4z1h+-SHfdtoT=$~5p|r3s^^lft9W;Dm4&T}NA-cR?ub zL96@T+9HjIta~31K<)2wIm-FYWd#od{k_kZ zv1(lG0wp9thyVueFy;xWm&+W(fYB7lPgGfCCvO37H@S6UQXo#RUt1xd|80F%vwc^Z z%M4pgwitZ~#>G$~K9MkqL^9G7!e7MwAoHu5i^5nMudA*KZ7BJjZQ51l>%0{jH8e0y zMA38`hw%Gj@i+yHFP4P)28{3ZVKaIJqE)e5JhnUuM-)$OP25mBXu1`Q%2KjkUvw~N z?`DS_CNtE;1|X#lf0xbAg0Nf6f-obpoN1CN4Om4RhEk?b@h(-~iVhJmN5$GrH=|1` z)zJQG?$)EM6wKVDjHN+?3-ID5_?cFBl6HOkY|EY_H(I{WFs%Te*^KrBUMvKn{LMjI z0NETBV-KH?lM`0#Rn%LwRCsDaksxE%N>3xKbLOGa7obf2uxYFQ@pTSAN--fV##&}A z^4wQNN=5_YN)K9`1E7;LfnN0~6xvpTef40)!Gy5rUG*@il7bhgzhFH=9N7Y63`K*;+AMu|#CgY-wG(2+2;Q+}mG5OF3smvF-;VLbAS@!uuq$?dy zukc8uFuH5`YRCYjaesgAQp6qYr4!;#<(KvOi|={=y)|BN*^?HKD@+mQXpC^`Iz zQUGJ2n?#WgyLyt47>Xs2U@=p%2TFqxQ)5Kw;cHdq+W8*zj3jBczfz0EZDMl_`-^q)^HEBx zHg&3amku#Vqf78Qq_a|&$ZL?zpjJ6#{#}w`u16#I_6Nz2>0xdm+!nxMw1ZmlLfEs? z+4x9weYDeLk!TF6lKR~C$bqb~VPSOT?(h2DJ3|Ni-S@eFOZ!o)5iaJC>i>@4SpiTM zLyRU*8)kC(^u3iYj>Zm%bRZ0;s~^Qri2Ah(avlalzTw{9cQ-)7e~ZC4KnnZi21Bn< zrzc&Ysk32U<8pTjOHmd{pF=ck-O%uD3kj=2Gn$609ri-^bXTbn)lx9oBqtK)o&2i< znhR%RqhuQ(DqIX}^`t;+6i#G80|f8}GJd8kV0#ek8IrmO2G6);FI~xSk%yR^|NJt4 z8>3|H-H-V5V4RF7eTUw~opI$wW2Y4nBjN{uKTQtdxf}~^$bHb1_t>}aq_?v?vnqUh zUnL1OEa*9x>ik9`Bo^zh51G+eE&nEU3za3cGvn<#FczxhCaM;LGV$9N19C{iLRzlK zSw_I2Yhgr^Cz2toj_dh_R00WKO7G9^CfRu_Qd1j3D*Nt~v7p$BUdh8QS;UOw1dWwq zLueUy;@cQ`z0gGwpEU7vORyy2ESa_ai$sW4C^zeA%l&>!#su%3LPg!cxXx<5#b={h zK}>8&RDvd--~^XXg;?;G0u5omwpkAIlfr#!QuE?EyYki2o+vwHs;=N~K*1sqz7Swb`+MK+G<^7avC3{t!!+ z26!dcYfC5pC}j6V(HnBs;UTCbPR`+BhBL;|i5(g5=m$w8F8k)JHVDkPfq1ajbu8BB z+N>3ia@Sho8!XR9&9u_p$0e&BW7=o2=?wHRQ4*|H8=d$#|7ri#wRLxm2FQ!L;$CX7mpXUq#%w!j zg}v{&3D^B*2D6T+Oirycd~uK{U`^T+gGWYmf<53Sy80m9w3Vsxlasv!t1?-t8d<7@ zS>UyaO~w%oKwb!0I-Crhm0~;fI4q`=y8d35ROCRRbcirJd}v-iK0mhoOQFLJsL`OJ zX2<$wT6^p~0FDD?T7w@G6Ift)B*zTcr7`z#K?LR2ju|t?>!t^B)UJY}esZ%MV@xFD zl#ApA9>`*iX0VmLZbM7|1hecwcE)bbKE&<-Wm!&*iL;Nr+Lw*2?j-WZ-e!zB%Suy2 zuKuUDzB>(bN|2F55c-~Nbw%pF=EAbvp{IePg!2|p2b2{GONfibj{KmZ7T1Ghkp~M= z?Y^YkA`WRIXL(k?ddM62ErEZ7R(sCO5`i0ye1wJ^IdM7%eA zQ-ihzqh}AaMuX!$wZT{^%{%Ds$PIKj7P_fXckMuS+}emk9{17QnzVw+=9PPSYs0*R zFZy9V0BEwn{G+EnXfslulUeaV0xt-SJ)rS;`O)}@%`l0ZilcFpVfIFu?=B@0%Rz40bIT!6vNl2J;BsSSk0^-6&!c(#K zpqI&M82A* zF<;o9t0sy4EK?|K)_j|(n?wXdHWA*ujEokWY5k)2T#bcJp7(9~xPGtSY(0Nonng3l zN3ECgAwlJ$vS&&~lf^ABGTJi~5`gTOqdc-Rw%MOhlYRn2_d3Ix_seZLu2cSNpoErk z1XORUbU{gd7 z1N#G;Y{Ux!gO;g`e;2@ktrLNQt^aQ|Md9XJ9~Ckmj|t^-_~0b6x4#0!sXn^@4+i6A zC1xi6&wkF}h6Y7|VCClK&S)|RrAg_*M*$Ah7`4Z%I&Es2n>?7}|4aO2@1jf7OYJWA z9g&$qETBT5Wj61P{h3LOHZpI{$Jdd+{k-po@hDhD6H$XLGHT=(<&A?74mOV!o&!9* zJ9chP#OKp5--4A{ORT;R9I9QhHB)J#*Ovu&iJ1evH- zyOn02bYBciXdCabIk#U{`7c1bslm)ywYJtylbE91VtTe{qTwdv87J>JC&y+{zu(hy zY(OnkrJ>*H@>#skU$s6F%7Q{g1yIlNxlWe>F5x>z$aTFg17w?|xr+wf^3`YZXPE}WRqH@O2tupe&0f2XaCL(&d z0kxYKh(?~<+o``$=OFQoFhu}^Vx({~*wdej%JiSQC3+lw80%Z~Kj3t)Ol2W2{X5IS zU^VI1N3DnoCRNlb6&{=Ht)7Ruh^CCdDN+Z@WBs*dYS@a#PqYz9yMIvUB5M<@{)~n( zOkAZ<651*?E=>1hZ;C5w7LXBd1SL!)j}Oz>xu#u^oK0?aSZ5wdJCE)zfRVW$H(Dn=zTQFxjo%f^GlO;0vQ7@(hm0CFFzo?@n~3~=ImRQgbAlOizY z6{S^4fYG+0#b-y4DB7OYO2)Q`_Th{~Jq!a=x$%Y@Mj`%z&2T41UON1qvh#C8tVH`R zbq{u!MpHsGywVyQP+?MWBkl~mQ#6T!TwHKj8Dq{i=|Ml2)Q%t?Tlz$gEiKds9igJN zc0HlEgDGCwR@;#0fo%M7$qf=fTxWJBa$E?VcSx*DaIcT+F#j?R0eRuOP%&blRa!$% zA*p>R;5Z-_6fQle6-QOhPvP8I`kaV}Rzw?$hDdsyK~7Qw94N>u+E9e*i<5{#z?{Pt zLVVUlg~uv|P^W-k3v)Ibi-XUpxghhJX0)>mX3bj1mMdDbAPj zQ#CkRf1yzTI8?d(p>S7KWftyy;`jpd=_V-W|EAQJWs2lC_zz%2@~nUYAq7PYzSRv} zRl3sy9n8=V{3nkp947(ty-F(lCmx%0#|EL^Rb*+jT`lObB#Sta3k^q1v|dMA>%TAv z9qP75##nl#3Za$a?-?$vQ~8R;5r4dV(r`#wH=Mi~uz~h!ZT)p0K4`KXzYJGinULW$ zRmC98f!LwEaHBuIjT8oQub9K4I7sCaOv&5gcm>NV4Qld4OrdO%JU{aMSm0Z^1hv*2 zYeGH(BW~Q@3L9ia!klZ{_Szu^1C{>P-w9OL6Btn2Wfj)frjBNYP7n{YP$=a<*_Qos z5vJw==ybSg%S9;=gMQSWK2ztIixqJ&5{y_4@y#qcdAhq4jZ?Op$->_8XeYp{p;k9; zKT``7oSqlL;o5nt&Aef(zLra_K2ea!^+gwfjcO^$F0r7}r?T;2q2myPakDil$xM2) zEGB_Xw?^~eqo1#2T1DQqv=zC2QnDl=kBdhE%;xAO#lm1_>Z9UpxahMO9D?Eg&uML) zd(-NQ!t8!N{8vZX|J0c)nef}8Vee6z{dRQ23NcuQ<7-6!^633L`_TGxN+Q6l9xo$X zGfW?v(~O2{^^{dXHK5J8ZgnNfmY&KP)_^J?vSTP!f_4B>2^Uk__e&E9l9y*nR5s)Q zWGIpzLXGxCIN&wsldn$o-mG=-d5YMw$rfL>XX7AB0yHxCg#9{eGp*U*(pgye%u{|`1r<%u>>B5 zGZ%DsB}O6_{9$KKE?;VnJb{ODxZec;yKQ&x&iG4$kHfpn49ubGobG9&o9N5W{+}pF zx-UlBmZ&-Y&c2?)bt>AIsr8~ZU+EC9AX&6jNE$(>koyQomd`KO?+5ovb`EqZ|GavI zFgy$ku7J1t31kP&29dli=)K}&!BLiWN6BFByaWSR(~|sG>AcEmaU}Mg9Hc;o9fN9r|Urjs#42Mvx8=ZI9KY2I=GvUNy-b8uYw$NJ%Kq z`@vSkwfLHzS4=Pc*0I)Qyu1#W98$&{*x=*lSD{+WaYJ3R;0e$u$XADjYXV}^D-7GI z(h`}lYn2qvJA74SuI2ic1OEYbb}l^^Sp>>{N-K%1kyxJO%Z{C?t?&x(_%qXPt4oor zn2qXkG78*3jtX*V1mc8{A6Vit>{O?8+hltf_-A_xIAcXZ{Lv$$Dv-a#i?Y&JF;8ip#M=p&fvgX=yStmU1o z;_mkqBG)P!OCmO`QM~`)#$5%QgS;plAiw-=f`M0Tx!n;d!?a9d;}we-AtzW$af|e` zC*V~)QznQ2|C4fOjbH*qBV)Qm8duuw`lRJnwohT85xZ3n#aR(vP+Z26ZI*d7*ZzzV z1h?q9w0?8&i7lTd^r|pz4MPNUotfZ*Y&kA|WDG}8*X*Dr&e$wts>16lL(%a>3 z(~OBF02ReATcJ*M2Xt(xzd#-tY?yWxiOAlkxf*M_H49BWq{>~kMfi7f(O-@wB!1=u z!@qwI!4SF2pV?{n&etqj3kq4`k~)%%*nT5X70iKMD}n%C#XP9^KbBc(JtcADQ2CCf zh*+4EFAvn`OH0Mqyn{Atc-?uX#-9*>DP}5`Hg?m9t*)xc3Wtu=G;ij>t>&@pfrHqQ zO4mlL2E9x2#IyMMJ+B-Da1`_yR2K%i7J$yD$uvwZP?shd8!l>W-!fFG*AR?$Zl z5`!(lXQ}|8pzQWPC?N1&>y#OKH=YNi4kU`umO#`!kYUisL8uCY+O3$zZdf|W=<7M6 zKVE{Mi>LPaLJP-x8hp>OZdaowWHA}}YvNf63o2xeO%bO3DA{jZPP3q-lK z)$^v!PrpaxfqZgN8;A9hnz}-9CG>PMdiu`KJ}nkWm9p+z9;)cD>-Pq2HrDo z^}txW?LfvW#d5Z#Pvx@xL2-+B-Xdh{kS)t8kwrEUtBp5#iLxwb;#aX>zhrl z0xvK5LY*7x$Bcm8SDdb~m)y_+C*Ql}M=#^&-hE2jsBi>Ou&eRx@Oeh_`R{fm6mp7x zKZ4@d*v04lh8)BWp|c9M-g0Y-G}c(-J+@e?bT_JyvSf%fWwCiGd2<19sxNPY{Y!NZ zkw$i)tDv$t7Ur`_Sb1>|+C|`qqD-7(GWCA8%Bp3%q|6}3{Rw7H*YvT&!GQrh;eN12 zh%xtl9DAsBRb=)0rl)hudhxcvt{qI{^`-HTsYb`>joZuxqSI`(}54ReF`uw@8<-*1BOD`WFsOPj;X<}*h1HNuX-H8aRdh0T0U@L!aQ0hCnI zn~?mQRw^{$s;P+_vW?ROx^81M+_cq2a1f)fzWPxd>T7HApg7pCrk65MT8rnBa~*Kc z#>YYMtbH>;5(2yC!vvzNW?5LHCO(fDMk}~Nb_jawl;B#MB5J)@o8qVJ#f-3ac(I294TA* zk!6IE_>L;#_|@swOfDB`<~Is)Fy^M_f}6ui5m~vNN}U!S#6K6$KotxtE@7g@dNAo` z&qiPOQKqfN6H5-{>JG5t)KUl6xk~MQNK073Rl)rvpV^Od5u|OFVT??k)U&XYWX{-y zA&g0;DWz=~mG)+k1nV|RSB~v%Imjy8;mwbHEozl<%sozq{oCZ*!$kPd9HiNT^y}xP zAH7`GOOsOq{<>itSvp$|rfT*Ej#RK8kX7)gHsb{s*f;_#4}+B(eMJ`Ev-3bZSi_A3 zd5?<)=vsb5Er7{eea+TaU-kwO?dvk(5ra!BSSAkj^e}Q$bA&6R5?cL?HU2?RqxrVS zWvl{Da{F~f#-=YLhMh(<{^nXx?^^~#el8;q9{XU7IFA})xAVv1%YPN>VcQp){=U64 zM#=)}WU>Yb5M(hu=UD;-+~Gf*cskf2X-CI%aF@UWx00%YkjwUe)%$qWW}|qx-*Wv2 z4W0IOJ8O+SX1|1Rhadz4$6r%Ow-8YIrauVc!NQ2|f)>!nOkIeeNHwqr9i{il&mo&C zq(fUvv`S6$mysf9d1vA5y!LP5BZ5s>%d;0+U~+-8(ZaWkvdAg}2O4syz(#c>W+QKq zoU)*9$9;3;1SC7!rgU;nj;qwFfG4hWFU792k*y)6(F|j&V&_fWkAxii=5Nnf94G4V zYEaQiE2lKm^r#JfbggB`KG*tl!3(~>a4I8Q-vg4*OO{=J#81ttrg}-CIW3cBze^5p1bMM55(PK@uM6@xuz`qcAR+CD-pb64;LO9cdOI zp<4?kI=kt+->&(zQi^3J?42QUMb>ZPq?{g{@h9b2+Xzcu093d{N~FE0OtE%h63{Ji z3^^t$%ZU^p-e=eB39qNXPNXRLqZxR_;MO8OI&U^WRrSE++G9WF{6#&-G%{CLh)rX+ zg`klIQnH6Jft4_iSI3*MLbg}Dv+Cc-7ZCyCAZb{o65ym@e#3lL=r)~}JRjJZ@`L1t zc#CA$&ceLaWo(fVI4jgiC5Pwt=2Z;akpHXGM-(Bn@Xyw=`tRsyHPJ&rdJ+I*GzNnP zKE5ZpF&`!Whk|cS9C_0dPg|0>?Buiw?uvb3JOFk=+pVvJL}OM3#*C`n4+dS;l4fcF zcT2V?@5dfo7}Glv>MgIh*y6KeNpYRx6mAXxzPW3sM{)Lh>c{ge5cE&-PO%i zzCmUU9ojj4DX0z!#s4R5G#VIMGIh6rP$==4>-hS~Ty|L+$Hl5jr022!g|)6KSP7Nz zkzyR36S)xdXLV%PqDh-vCMBvPE`u`h$HS2O=vNMbDPy_;sAL>A?Piyyx-<&zyn9BD zyE@%o+MieIn#e(g@)0exh)xp~K}Kd+TCnt1S+&z5mFQQoy6J>AQZV3drkM0NRn!7& zfkK>?Gwr(bFpqSwDxuPA-Vf7GQ%6h27h|$59Mkzf(+vyH$gGMV`p&fvdui?Ru6_8n zp})`j>5}f!hItcCbeP`4e3%;iZcCuo=YviE`X0?0w>z;me`f(2?}?%<>9+IzeibnA zzxG*ImHsIpdR*l(=M4~ZH2aF31=?*<2Ly(IEl9vMh#PWPHiw`@dQ1Av9A&ZSJh6Ke z=99#mbA9rnC5gN%RmN(_wePIz_$&N$kBh1J=$$KxioZ6Gq2n+Yh;`!mp_qs#_~+Zk zv-(4W(Lsy^Lo-2p`H#X&YyHoNkFu#siws>(SQa&dd%;)){X8Jdt2VrNGM9dc5pRRN7|yx_Q#sm8I4Eh~S$V`gNfJ#Yy=6RcO|{a9c4kf10g^rzJ$5;OPj zY{q8>B^>gvE5}+nzf*F@T~bA{g4Oku#@s*WD=|p3>)3n;Lr;Mw>Cx6cf71L6c@ga` z#Qw8sUo1|I$oGJq@S=(_`!$HdD5?_1mLh?Ha$`cCwDVcCN}~d<_P{?H+^b(*S^PQx zTWF0zMRjArbHMPLv4Wu_VruHBzB__0esTOMO(MLfKlahDN=M;wbXIY9?>g1e@bZTY z76p|==mK2~UGdHnMP>I;59G8|#%bv%`su*0?RL8*f>40Rvyc;?Hi6Eab#^nE0{Ptm z6sl6Dz|=tFqNU54X=VT~WY*j9H{?_0BbZAz%(`ucSaci4(nw{8Q&`do8?y`6HU~ap zx|FCi#NFSrjrhvbpQrR=9@{4JA_Q`(M$%7&o1rmz57Kk|~4W@aOcE0ACh*t&V0o4iTsy`F9-y!f>HMQ5U&os(--hcAos+a6be zWJA7naCGui1HPq1q_nTIBq*zH-Ui0rmo6bOG=6Lz;#+TFiFR8>0*R1{B)>n_*z#B-IF(yw|d{q~@#LD|VYB&hDkGW1hv< zIeUDrDE8RWK@Ml!rAntqEd)$Keq!F@fD<$YJT+vW{6!hl^-$@rj}dEnQFN&JdMx0Q zt-}NU(Y8R-Z50b#yZ#&e@2LejiL)7=U&;Ui#<{m265hmKyeG#Ky|_~(Y2G;1I!Ut7%Q`k7d_f9Sd6VVd4Br7bO&&o z(r72P%PY)?IKFJ#wjbV4%{JD$t+yRdU!vAPq$1voha}w@ooh}i!t29jS&EO1UXyXV z>n^{}5k7G`((Ru&X{t&jrg%2eS8C z?gc;lXqM!ld5emYW0k+AUtlovUU3|0-wH}eiX4TGl?$(6i7TEaT20F7#?wrk*YgWt zaHJ}#H=QlclNp{n%2HVa-SQfxlC0$w+PLJdl#Z|({?oVldl(^2?r{HRku=GGVlww5-%Z1inRQ4hEA+`j*~n-(M7=*?BbgBXN}twUB&p+9=hYC_ zKS;cnGW+yMop91M+jmd>J0k3z7BrQQ@hix1fT%Kf zHh=2=$+eoJ&ZDU5(B8bm_&~bL^`W%uuT+Hk1NLhC=E&Br0fGz;+C%JO^ueTmc1^p< z^O1Jv3rSwi`YWvrPec;V8nk8zB))NhjBL{e0q`&l?cl@Ck1fBhoXLc4(_9hVw?HOV-buWuj!pwvVYp!&^8-LCN9_NTjFywdDUz#6+XyurgX~U{ zBymWhPRRCog@r7fxt1)c6e(UvH5hNe0UB4#6#qG_R3+c_;{(kIVrfE+4d)%e2+Xj| zQhR)ch!r25GSdF?cQTUA#2tvgkqvoFB3amxKH%o`3|s91{9XRJz;IupRj@qsay)`%*+a7Biss%H-@{$a6D1VfeFKLY0D!M2V}7a86`1gYh0iQ>ItY_fvE&|FNcY#q!xdRv$`j`d7Bsv$ z4Ac%iRxgm6qFTGxW+p()lqh3oP8@lFR4&wDW zneVTH=Qg+Ab;LE{vowOr+rTD{++aOG!m?x|zQ3Z#lt$e|+Io{>RgpBE>Cp?J#(d*i z2j4O|F;4PY#2&T68$AG`-ob*ise`>R>KPQW1VJg85}LYf;?#?ZO=mR&^Y!1ItQ*Kw zL?WDU#`*D`5bi-WGwYfI-{Dg5UNGn&!Ig_41l>?L;F-GWwUPsRUym15Da#+-#kl;s ziKwMR(k>md`=&k-b_F1Pqny6yn*HLipl$6X0SQwmA&GIxFrGk!|2BH8+u<;j5mq4s z*$k^aRCe3EAfvBzrAW$A7cv>*$q8k-j7jlx=mU%R4X;hkigI=+3mdlNrl%_yi9>k-_v!4jmi{7TH*4BdpVe3+6Z%x~X0%1q6{@-+w z5|6P&D>9>o#pzH4roF$oeTBpJgl$yzF$1c!fq%5;VD5nkE{flO%wRjwjJn*yur;gR zno@)b>yYMa$Ca(^{{(mz)YBT63(B4{HkDQD4EkyGaW^*II(8iUui!+5g15h4%0qY1^le=pZ)#lu4|V&R=r4zDkH z1wS7WapncYQ-9(vrR2#~|2#hBPM*RqS9oV(g`t5~HQ;D$z#kl`YI~*eRzwHQ1yd=a zQ7#m+EJ1$zW~8-4uu2Fc!e&ebW_F(46012|K2g^3EfMt`pvsw3ld}s5N|{^3DsW;) zTJt#)@s0;26!=TROS%zmGkP;aJlTxFYMdD;?>Ye=im{|wIaS+cG3TQ?Y{uX(TO7D0n#P)2t;8%^YhFqk2`gq3oiCJSID3)s6ViRr=MuB9UfS%YEqW!Ef} zgv6!j>ReL>Nc=~c+!l|q69Bs%y4qtB(G&aYpM;>TW!J$a|f`sRaNZi`nLhAN9L zs^ei6`A~z%A=c(9GcSTLMg96yPbT3t`_O>CY9V#O6wdtIGR$|N4u4&?fom~k&#k$Z z88Wt2lZ+5Xa3d!S+ek%xtNEk*rJrYJD_i$oqJJ)$Aau|z?RMZzTbj)(59_U2Z`3io zOfojQWHYFV1x+TmdnjGRg)#14&8%_GX6i!6O5eV;KLg$_MI_h3O`yi}%lcP$8z$h3 zlyNhH_+a}D^yCi9XigUaVdClXxB^W)+>b-w$u7!`B%LkKC#=BiYnK0)JMz zkLfUco!;!<8{}J(yEbccB!C=S3p^r{B(Z1Q_KzMl#iMVc0Z#_*$^D+(xY7`=v>v8H{0>40i z+W_q^ie!sAh~<`Kn4lu-EMt_zvH_oAKeMMQM^n|Ikjo1yKDhZK?6?puk`kkG!stEc zufF+?o-cRlig!9R7KWiAYQS64BcK#!&u5);+rGI=HDEUZ`-~1$ISjMO=^N?W^2f@Gru97}rEhaQ(4qF5O`2 z9nNC%z1#e(ZO0oH`CBnTfa%}WZnxc_<=_AkyhGmxTTI?uq%uEC2vB>7DJZEB&XL-` zLHd3#8ZHQ#W`?cy;6-x8Zh>$8;2k`KL=7h$=B0(>?|2y9r;5HLL0mK!D=bU8W`Q@a z)CL~{Vc#?&LA>FI7xWvX-C0ddGxd&jKrTktlXH7;k5EI05k$GuCwog}=(_K1ROYV% zIKF`mst(5`WY1-F6mT%a@NC3nAJexjrd?;_sDm}s-`fuj^-FEa@d|(A<&NC_P9(I@ zd2kqNLM#;=cW_o_(5A7U+d)uq@s-8E-vRVl;^|+ABsP&$&L^69W3x~MO2XB{V@P0Q z?2^Z8%|H7uo8QJyDM)x*nF_0X5pjlx*=u|)<<<)*QJxXD-2f47V&N~_{>t}JE^}ku zbB)yqcf3)1=#iL<9*moJoh9uDGFiLuT+^p)GLX&j`(GqH6UGjrO0^ z^6@nNlT@8ywHYCbOCgGc+@*1xed0Y3Me`JvxtU6N^|S z%z4Wk?JmIuo}P9OJ&+`UB@MHBTkEklkPju~7g3kj6R@8D`{)T#Nbuz`cz)cACEEq< zB!C_!6vGbg7fIApfiG?4dp4igJlv9Hd0jsFJ`(ED=<-AemVAuXFhrU-B<3IL+AkVa zMRD*$sX? z5hxP@HJ7ht0V|V)LKJ@!J5FkP-^S5s;?b2~YB$Xpt zQN624beF^tscoS6g(`}zqlgkJ5Zz(RDB?_a8CDU+$F{`4r)qzqe2(qJ;KV`%nn1$h zRcQCbl%wk~BH9DpVy9u8bSU!ma{76a32Dabn|xh;cUjyO^UGEB!va4rGsI|}FelzA zVh)*PnzHem&v(mWwVKYa!=^>`&21jmu<<^WIN`!7pE1q=iD5jDJV!5Wh|+E(fY8<) z|K<|}`U@`rqWS$*?nI$x~|e`){I>i!ngXu^NS)5W}8!Hyq_>c4LC38&-V zCrpj2%X3MIcH%feM0Yhrg6z|~jxtIZTsJesit6{DF#FYDR9-EltL5+tRd!C8l-oFt zoC2&7d{t%%vof3_<7)gXc3}g=7S3XWd)w){zha^_V0k7IZ9bbVCb08EBij2~s_R9q zmd*jMG^h=w1z%LN<07A3+|TlLlb}Qve=d@t)F2&DWK9&)!98+{tp;8n#NmiiC%k}5 zF5r3*!i<<8+_Z;dsAQQieq-lUWVLC;(V!(6_&jc7olX3cBwV?#U}z3bnHrib4b8=d z7BNZ^2A_t@DG`U$cniU!Ek5BKumA+I4%w@LQ0S!E#b}co9b~jznmt(e>j2u|e>S?; z5Rb%8JYpD+V&yD(q*Lugu~4m^UDDI>G&L#?#2udQqbuT5rOE7jkMGO8MvFNq*MQkjkZBp3@oyN?YN>LBN6Y#8_J^-RwI0L`Rx*d}>JbMOkjl1SIrzvU%p~RNSztBzRGv zskR27XgOS4yOc30f1s46%(MgH#-fSrf?JB=TGzw1%x5hsd=$=s<8XaTHa87>F+xv-{imsy^veb;flNWT%Q)e;xXK;ekzsZanZYX8dJZ z%r5;k&{(g5FFw^~@KuXHNi`k(18+P?@!NeqtFYU8fqIGz0E?#9>tcY>lGY1+hkdWF_dkf_OCpals?jEp;>XFnc>VJOTD zpuG($!4R6$e*(1w#l@dUTqV>psBN)rFiM>UMxmGt$8t#kGDKSO$bu4hsflV632La& zCmf8$*M+22kaAe|456sn2gehy3O(l0<95XjyU5!B8V!b`?V!*Jowdwq(nN@6Z*PLKxXi=zXB;LGlzWXfVsp zd57B_Iyr7^zdj=rczUUO^-5iKa%)P|M7`ngf1NB;oyWI?usYMBHG5W*5s;NO5P;~$FUbi&HYHy!8e z_WPo73FX(|E`FL%WT05CYNFQ{zMFvAjrYF1?asfxyPJKfa_6o`%B#-@1B(B18g^8j ze;o%rl?7Bv8$~-C7#Xsh@6o*YaUdht%_3zgUAu(5`f_zY@9-pPu<){eUVr!+pMB76 z!u%5VgRVCSl-`p&-k%!NHlcDdbtJt~jp_txbV9AF+;w8~b96HZ0E`{5S%1W(yVqsm zZ&y`nhCMK|z9YEe_9&LxZN7c%$}{+T|?O!;;H}!YSnGQM#*YkM@#6lj?|o2(;G#m%|Gg@Kq+5roM6*=Q-4-kT9%Cd>59(2hiPhsre*}s& z9mTz1(HRbl+RpXk!y;q`N5!Cr5A@qrSyMMuY=$&Z-Q)0rEvCwks5rY%q4fG$ntnBf z#}JqzLbte2;MkvnT5^ydB=mk@uIlUGfKL)8z^r%Zyn-ODFkfO3wTl9V;;B4g@aBhY zHfTK^q&&d;IX;mVBY@yz8n?g7e{YKr9q#TLz?s){=f%~c=02cGQcqS#_+sP3) zZp6+Dx~00_I%ZowtEfhViq&0lu?`KECYoY+Rui@5J%H^Fa|CG>`RdC=f2(oIFD#%x z+)OWS;()T6-a;}llm^(=$(`B{q(eoX%U%EVa-aeL>gC5}yTR^Hjz3MeVDkbUR*Le6 z%Ym6rBmOEdSEHisEH&KV)cm3k$r?C{lj>yh0|Nf^>q#KL78FxS%!b2En>~ba9ixBNX3*)s#I8`i4w+rTN0Q|`H6Kf6AjRba`26py-!DLNxq-nPG|Wt#`I+_ z6nccyt9UBD%#S|bRnI?%FnUe2m>2W)G|Xg$6|RdlDp6fMd(OB44$6ZBJDfDW9#S*0 z*gnTA(s;AlJ6{UktuJ^^cfnX#c)Ow8l40W!kroU)BH*iEOS;wjxWTCww0RXSdg zTosaX3$gYzR~4Y1(#BU6&(X$Y@2W4bC>5#2OF&MHR$o}ie;X4^=GMW&N$+^Iz*X1P zV5&3JwArZaQ+Z3fi?_6=^OiOaKx{Ao{XzvBc)!!}3pDllDQwEuir0$3Z zrp!-S*<;?9fBG}{>GUJsm+pHF{Cz2-z&w#WFJ(@S4gS2862)xqd8z+^e_qO*+Wfo} z!m?Fj%kxqp&9(^*+760sd0r}ad0y&{e_pD~j!>wi7ZoqkccneIL&5Bj*R}|}JEYZ? zP7HGym8yq4st_B;G#VHt+=lXlCYtl)ugu^MrIq&Mf4(uvn4t|1STasZ(E&0&*lqoQ zM-|@tGbbmUFiQtxq<3yDq0j25uKs$DH_#hD4qEADnaTOzHPfKgpMp`M%={w=xqU=b znzGJy+8{hQAu6A2hGHLMNQ)ufYa(s#4c)lC$4nJ3=cm5XJjKwMse7Dx3bETfh2C|D z)RZ8GK*rE(|d23B&y1l zh%^ImqD+m|biiPL0Ge_Cgp)CR69F-kv62%6Gc`CflR=&+ldw=P8xBbiE!lA#w18n! z(s1U?xu5S0-(I}>+xJS$llo8~f3K*I`}u+joc(7GzhuRxUY3Wu`EM6L!CY)c2r@!Y zI~SfUs6?9jnZBPBk=2cTmS)@h(3H3S{^zo=H!Qnq>f>(z+J9TF{q-jNc%9EF&OX^m z3ja$!n98lMaesY^B_ayQaO+=mLo_l{y4`9-q?mEUxF1v5F+erqM9dcOf2$5ZT!x2b ze}09Sku!!g52HR}fm4LI1p*m|)Jz-^CfgA(3rVq4At~ZC*ZINke^u;?YPI(t>dJE~ z@Mf1c`L;L|O^Pa55*d*5kg(yJmavY4ul?L5D$wgBza^6FugUIpz1h@rDzclsr^*oQkt$8nHM$XPlafN%?|xQg&82f4t%?C?%7kBoYbDpDIiKyt33!l;x2BV8<}WVmQ@j z_&fC^fAs6lrXdCI^MfD(kM;RcwG5%X z#|cCBj$kAS@#Eg5lm_NAHdT;FN%?TtqOgx=4_o8yO^Vwj!WMsQQp6$#<8+?R6-;Q5 z)L)JqYkF!-PRjNIH53{1xyGl!oiHmnX?Tn{mTF270p5Q>UA&CCXkJ1Whunr+q4lgM z(uagEUqblug@tci{Z7PJF~t!nupQ$n#6V5n6y8(r|7TH?!r?TF{oQt3ScmDIr$-rK zP`MkFmZ0#Qzr(F0s@qg zv9KDzrgoN3j>c)!ltA2;x=R>-C9iE&DzaUE6;`&Whn;C{(NufW)Q9{~*45q(<=s%4 zabgBzN%rA8n=ddMs(*uemrK#ryLQ}7x!JTuok3cUn9+YY=6)*o4yST;Vj1e;wa0e* zMPx_UsCsP5t1{fOcVV6;ul6v-)<1wEYa=#|K?gAeT;^#J0h)eSuGd8~2hoBg%Y3tc zWzto4) zO4Nzf5kG(0?7PCja;5ue0wt&5*TT#fF4}96<(IVu7CIymzS>{67^M3cKKTz01BC0c z-tLZv@W7#+Y*jR+lbrnrRQmgNX6Ye8?IA(9tdG?)(mO&0F-5v_7VC7RFrq8n-lQgx zAk+>je&4ySkH9rBGIBK0Um&=FAichb|?+CP6nQQH)?;QZ78p0s8GtogySw}`xI z<{Uc|-0YxPTC}8}o-zb1`6)wSTAwlm-VeFkbYxY;2l{rH_hJl# zx))86Vy%GN*!4;B*5+XmvOLX(A12sfqJ)3GNh*tFgBme&99&&$GO!)&OoEzh-v$i$ zs%IV$+M0Un@4BErUKwg!*;#GU&wE@aj99Ena0GjBvH8Z&?5z%{a8{SL1<*F5MqC>0 zkJ@P2p~LlqR69N9U|s`9wRBu z+Ow*z3jb@rTLvEkR$nX+{*lluwFAQg%Q?yNFm>KE!+2=FOE$zPHx7@&ZB4NtHt|W= z8|)T4#v)}{+Z>!|GD4maM_t|p0wIjod#i!I-H5^C4CS;bt0E667ig>Rn}aTQ=~&mX ze3R}*NF>ZCNUf7a1cscRJBQ~6;go+XhZ+NhFM7ky!gcehE{GmA`6%&l7oD7!LPZcw zdm>cymY$9ky+OaHA?#kPcn~!{himL?u9=fuQz@>cTcb1vruQ8ih5>`RdXCUBEBbXK z5hU5KienfQp$W?Stm2v_+EoLoFt@t|Ib=Yz70lOF)g-=$H4@U6QUJFP&!4Tz*u=IMKXW!RLf)(CcP-kC3i)sk3*0#=uyJ3tQ)8^c6GJtD(q2g zN=pxFXlY_BO>l0I5d5CR2`M(`L|F@##z8G)vCKD{_T0k+oFg&9rjy-j1T`d6B@3H$ zRM$;vR*;n8-K@Lg{(5agb=}==LM=t&r0Y?WzRgxs;Y>irt4@C_U*`f26LnZj zZ}X`PQAM>0hotN&12I&Wx5Y9)?ybpimwDSuQ(o1LN647TR(-_8{q9gHceg3>mA`YN z;d-cR|Jd3gFin_ve`xZ%xfHgTW0Gdh-TKMGwXQaI{;sSX^0a9`2LcK|Z&%k7s8@w? zSE+iU1F`v~SHt5UBSC*iM4A5bf^gQ+jTj$RIm0rwt^dg98>T)h{RNivM!iXux1d`3 zF(^+po%{6y;#4Iqqn)J|GY$ld)9fZ4PH(OKaoL@#Nz0WBq$&SGOdrN`2qdJ>^OzY7 z*Bhjqw`u%_M`raBVLITKg~NLtmdADl^%w<+M^;}ZJ9=!|eI|b^?_nf778RkfcE5YZ z!HIBpteO)xV8XCP1c?u`9U`i_UOC0-Gb#2!<)S|{FA^Oh7=S5(;i~Jt_gj2%pjAKS znog>1f!(GklC+3y?&zyQrNRrKL4; zPZ$yGn#-yc|3`nq_(Z4Gz{3j$l?OXzE>NQhaoS#b!=9gadUBfEbQ3heG?G3^V8IwV zRw02Q0d3c}nXQ0K&)b$Xp=<0A;04u?@BMMiH~xSpd|G#DogPI_EXE#_j0DK;bv5oC ze9$p~W1E3f9?zs|h76w8plA0hq(r6bn8_ZpIEIM{y(j&;jV)|m2TeOWpT4+PSG z31l?8f+~>>GH2bMsc{b`Yc_jS*PP1LB$p?WHCv&|lWDW=S6fg9ye%wpCw*tnc+3(= zZM%P=$)rJ>1s(Pv0?^4=*6UhT{+po7tiw{_yEgIlpN4t2d#3PLe+n8NT@|b2GJF}j zOe~QEyQzQuHTMrz$L*!>8?n%dVvxZX>*J(9kHG@*Yv zZVu&cQ!ab`*RWmT(S5l5qxQY{lzQFo!PdUY+lG!iN>=0?k*ES6j=NpG4=wul_JaFB zh`Uxp=-htgpmRI+s+rq&Gmg8^#Gl9kC^c|TPS<}C;uM-d)TzFct=&$c3DHd6`wjU$ z$0BMKSg@ZciU=o2XQlE=GrCkQ44R0n~T(`#-k`)5M48n_k5@Iq=3)6^6S4WmBi-5E*JZ$^ND<%|| zD~o??g0Vx!NzTD=)p;1M`g*|mk4d;&+LxVrysOeDrEIjem@p-f0|y}bpWX-5Ym_o; zW?up7C~eP`w6M=;+q|C(+@-5G6G7Ig?t3qverAYnEGDA|RcShy1FP5=+h|{2>7{g` z7j_erb@1+JD%wNYiCJ1VpXQ66lomZOO_6^*w8w`p3dD+RAOYZgboEJ1aQ=)YUnS9F z?HBZUymjPIvfQ*#wmEZiv3qozejQ5K*Di*>B7J;O8F$drB7B^Moa#DFAQWw&ieHxd z_kB;~{xn%!KACSrZV2Mu5VWZgQ}m%7KvJg2!>(jNyj$uh6hz-1T@81u7JhrcALI9)=@ORu&Ew}y;DjeEqlQDb~0y8j^z+M#rHJ7nv z0V z$Yus2lQQ*V7KM|`ZC=3^f#XPGm929>yP^HrwInocCXY(QkW}bq#2mSR6MjKaa^ZMX z1 zaD!+oj?knDw`!t@V(dafqJ(PJ7ZPtY45=%t;;z`BwzR@{6C-iPj3JC#Sl19|fu01k zo_5(Dxtv(afEvxy2p!!(WQb5mjW%{b#&G?VP^Ceuhy{`N*=Ja9HbpGV-W&7@{6S=U zDYRP6VPT?lD2G&-!Fqq+9WVhaTP@9}OeGInKhO^5W56}Wof_zdBb3vq;u8jI&iX^Y zwMy9$Zz80yD0@;#n3h9gK2b##;qYLMt5Af3U!8FA$r1!9K^yoF)WDrW|^0Mw`<_~&gS}Ana z8=?Yr>i|gH%WXB4fNBD#T-b+azH$-Yzl4#yGAy&J{gxPG!-!>x6si{%JQh+O3&jF} zMDeA37|@S7VIk~EC!CI*@L)|~9GY=&sy9LXh;y~%O-z6A9E4`nNbp1NZV2zJ4gFDk zb@UD+!%s(^L7Uz*45KJ(BE?7xEa8$p^&>;R=`;DJPx6h$_y%e6!*D*GO>Ou4eg17`osZUF-b;eOT z5H_+4P~l{?vp4D?t|Ox3SrhZdjz~Ds({Xqn&IFXncF=bRe36x^TZ~3I1d3?v>G&yP z92jmpB}&;o2YOo{p+}-RONrxD!295_`bG(?i-Clm68wxVP@1F!b*_B#9wBL2EoxAC zp)om=Xk#9KS(Tg2{;olkbtcApJ=D0J#}sca7M6!MXJnTPE_vVnBbp+DU? zzjilOi4WX>wiPCiI5Y($58>+E1WumL4Lp#eor|zC$yTMyd<=&2-O{(Ru#*mJzzfD0 ze6F+KeH!52>U|YjMHuH01%BKnWY`&I^nM5$mE*JnYq~6x2xK4un3E}FECKhER%9Ow zQSlS^+K&)EY+(tLfMh8FRg{)4V*shxWngW`p##Mmnc6dnGh!6h$DjrD zVnEy>md^}Wm@=RU6d0n15{wsl%%yO0JJq)m^{80nMQtI0Jr3m*Y%QJ4x`7vvryR-C zK9Oe%zgqEjYb+`xw=)h2wtA(MW|{+zLHG}NQ_FvEE2t6R9@ykrfcv0?^h*Ua)@c+_ zf9J2Yp>d8rj0!VN!HTpCrGU3j=K<+)&I1PO%GH-*`pGOuo|WauXJk2mu zBPU8A{R*%I)~48GzP|eSnNAk)Cu9OdSTnhIL)R1BDAkkIA;HKHy4M13WT= z1|@$--+XWi*aUFsMKf4eqJAzJKQxDUJs89%LI4KSp)~#DLk4M5Kn^Af#8OUJmnZ&& zxB?+Jejwh3Dro8Jo^?!02JnV#ZRk3tVHR|xRWKd}XOyI@Rj&`znMwX}z>#RGo`&Qj z5L&z#iEEPb)~_9gUn8XjuWD6*U+MGAX;wQ3MSeODp>_*jURzikeq#j~?9Kbrh=$n5K8&bQ7nNYdboZa&&%-(h1&F=`o#VC~*A&kvL*-9dPBmVetAd^>;a zN$~|c*Mr0SVU1VaL*7q>x}7p!))HU%{G-C^B3` zxehfgyjZed?TYQHm^XDjSIKboHE4fj{_z(GS+4pKIX?5n3dPdMxfy3H(eZOLPN>!6 zbF*0S$)mTUr)7|raeMT*3^O=({J4w~lb$**>m`-%J$0`5!+8I=47;;Bs=yH8Q*q%; z`R=@0!i*Vpz_tw$9d^H4Vwr^5MhSqJWsG0or;O#Nut*0*0>3iPu6a->f)#&MjY_le z|Hlt_D7f&aCuc7@;8FBhCp=tWb?LwIh=-kZ#3Ro+;z?h0#6u5XI5~RzK@cWGJnmEr zz@EmO57&~Qj^I)(i==?vIB;jo98CdJLJ=;5?LYq*2rqqrOOR$L`v2Erzez0?ask%X zV$+Laqvsqj9GT{L-ABK&?t_0g;j%73=Gg@>-Sd9X191||y?vs-ZW*zsPUdU|EstCc`45)7L8xD; z9=VL$ODe`bIC9leMXy?#r&le-vuy9Raa5ccN9|XOqc6fFGlqG%z}!Av6Dj0ti??S6 z()53a?eY39BvLOg?P}}Fp|7ayYP=!)s><#&2=GPSAv~jkP{uH)WGvLgOv9~z1FI@| z=aYPCM*}i2F_V!sDSyRUOLH4H629|S;Fw%MpdWZtxl-lCak5o$#bx=hiL8Sx(_XC= z(o0c^^Y8cTAvxx7NDfUi$w4z1@I#~D=$;V;a=}qZjuUR#5#cXlDlzWSNi4XF6REhX zOBfU8k|^%gnuahFr`EU||<)00#nE0Z-JhQ@~3@B0?Brm=%HbyO6?+8m4aO`o0Be;P^!&{X@Mb=!hA#pd03@Bg(> zliWddHjyzfRTPlXVuRo$8MXqG@jC=_QezlV4NilB!IDPMg5A`N2y-bVC=4o44mKzl z2zUgNjYzFd1$Yi1B9NV+YgLdnyryD{Kqgnih|>xlV1Ht*V3%RS6oQF^J1z|Er%-H= z;cDdZ;cj$%vuxB&O6z=7x|0SgAGvGRig(44^oME>dc+7-Z;_&4(+9nvdiF z|6w?0j)g`VT{c&Sg)%7wg?F34O!$Fe6L>>r2*dzo!7_8)EC~Yww%LcUIIGQod?|VK z=#d*=xPS2vvzIeBe&W8me|UXAxmn!Krap?7uiZD_jJ|rsIpHtA3L(6(%gindyIi-U z+xCa4U0wwVE#Rh$Y5?yxQG3>{AJ{HWHfNr+#O8LH?AT>V^XmZ-wJ+lLE!lZXXTG`l z>vqd#w?Ep)Nl%O~;M^|vJJ?_IzJv3ZcKJ_Bc7MHPBhY<&`NZX?EgN;;Z7zD&HSfHI ze%y_kwt%;-3=P3I2V_Wn8K|eRpSq6I5$&R#Y5eK89rE38;$H=A?mxF`zi!v9#MgL$ z;oN6<7kiD_e{ZBe=kDdm-48o`@v%?mdDq|nyU#cKv3n8+)aT&w!AR(2inl!%_7-6O>(67Ra9(brx-L0oj5V^1Eqv;ri!7jFFYc5!8FJbE-5 zzx?yV#EqX{znhH4-_53r$#ijV1%)9;;}?_r*~9#1a^Dma&D68Wo7?MeXCK|AQNx*r zl8LTHATh^UXUA)Udpw=aKp?Jj&q1IK;JAE8}bRB@Tf#_-tO1)boHaMCR-8 zFeos^EpDG8tu*R2UPQW35Pdm0l(+YW!-MRPXc7HMP7awka zzkZ+e^=$V3eC$Mq1EWWcNW{|g<;7`qW0uYdb+H#ukR!4yyt#+C~ltifKu9l-@}XMqxr%K{x-lLb7d zL0CdR{?qTnrhHw#Ogh)Ipz&l}lAlf@e{(s(>-U#DPfS(%E zReCX9>C~7m%NVxTk7qpHrDUJgVmbQ+r7I58RPB`^YPekg#*#Uf;H#KzDjITa+KJK_ zA$p;KHiEmMY+hCQxFG7__8g^Km;{fHIUmMRioVutuWr58GDuVdO7Y5rIL7&xYaQAg z=0x2bk*J4dj)FcZqd2mZ-<|~9zQYsYCLOE9pORHQ7X}^eU(A# zj=C(;_H>AECU2)bFXuoXD|dTgQDxb*6(jL9&P}gMTQ2IYz<~9A3F?sA$~d`{J;2cbU5^;O#@UIk>Ir1s#_f zx4C3OAx?PB=C3J9;Fc3nlN5Wm$FkhymU=Shj3TMzWKKBZTspGDYFFOf09#aGC$+t~ z&9>WObK`f>(JmNPA*9~sKb~9Jrhn0h;z02U3!D;(Bgu44^ftAOLJW)6G6a~TWcntx z8Zn3l^}J2%2X+ixojXCZ&C=cMLG}Aj7}I>8VugbG4WG=ZL>xq`miU-76k?5X-lY_% zL2lYrABM+9O76s_*npLgAuQHTw39#vjM<`kNHH!DI6=MJXHP*|yE%qoH-BYVt!YeC zOA(9+rIQwA+7fUJ-c_4onUIh$L^Ykv8NygZ9t~s?97Ef*{lSBNJLEV~XIIDg}0OrBsO5PubFNJf#8G2&c; z>rpFXARAVHUpSy6!+`;h;=$bm3P~kp38!4^ zPF8sW4@fw=*`X(NH__y6auJMZqnBczMhKuJ*3EEj-8p=~A>uk3oJ_eu&aI(ps#gRO zqs~&zSMn$YH>;^?@P8?6q-9)s*N_BZ3a-IKbLa#TvpK;SPk*`FudZj*=8gMow+t>& zUH@L*Zp(e%tlR94VPyzx#Dl!cRSt=V=xKfnuVOjXC?&eIw?Xvx*N(Od&v0Gl*1UT@m0p*J`_kRqIEuTi`(t6Lv0s8gY4GzO zdrm@-aFvo!Bf`xPTp+Ab)44AlghX)0qROE_xlPsRJY{+c+HP#}Wko_@IH?zbw0OQ& z1|ZAW@Nly_4}Xz+(~03*dv>dHJ7a3<&+dJ(UbhO4>&HsJ02iThjeyPg(m_5g9c+b0 z6N+tTFAJ^fWDxY)5P+dIf2!7ofC51{D_03#c$=UBg0bpZrTRX8`yAmR?FcOZ|Ks2q zk3B^I)-UpecOkm9)0{OIKm@J*nIS2w0u-^Qw4wT9%YT|7%d#QM@|Rc^AGPd5Ix}M& z_WEm2Kr&6t8mDIZf2?K~QRa+T)tWe{R1K(V5fj&bfIg@c7Op5ELmtIF=@_ua+yLG* zukQNn6>KsX)cAOt#GM)Qq~X9OEj@z9NxJ6Nw(M(+O*Yf1rkLt$&WPdW46_v; zsoSVm0e?$0)w9!Ot9ou62H20P?56!d(oh-OssjnZo`@GQ5AGzItOW&;V8qu^N+sQp z!yC2_GbT3BF@C6D&ks)-8`zPY2N+$7VW*K%;sp1wEm5kgqLyRtn7=q!TV5wy?GR-| zQ>p#4EdOf@2Plu3tw*b?Lr;cQFn+i9PqV6cxqrE{7}<8)F>eUByRt&pKxSLs_Uc+l zyGQqHU)&b!Z0FZ6-CMp{Zy)()edyHNXRcFUZS5|Lax?dGJ0T({?3(zgmK6{y+A(CT zln8tx5RTyK`^_@XmmXUBx+~OJogSe=jY>~q-(KWNMm4QNJ0wd(Fc8M3d9`K%1pDKx(sYr0M{qRM2d*)T07`Vqeo7PfxIHT19Vq zdd{uPQuT`-w#lzg;NXNlE8-4(`XQW;0k32byqZDqN?rn9hdx2}eL9;HN+FBUDu0`C zCWd#Fr+^3JYlai4{VUZBiM0ULcRzgs1GWC)7;u4b_v3#xC;~XN1R%Pzw_W+s!+#q9s2+)z?#bO|ncfXcBV2^ZapF9Q+HJSAF40@@!w^ zyY8q_%OH`@#ZqgkvU(%*e;<4(SAMWe?z*j)dI=ho$I;-piho9YhZRk*Uq4r;s%hp77*o9Z63-MpVTYy%#W19?j8TlJ zEp_(O<=G!+);D2Y$XXT_3J@1q16~%lXCFV~$rAqf0fH?vdU9Wp-cGF0SWVWG-_HKs zSwVp+h{0g#qbpb~*s{BK31C8K@vyh#P~#bF?K9TsdNMYa3)kU;C#_H;EYWh;wn)?+{7?tGsiV{it^%1hIHd8%8x1Q zxtTirwZ`(Ej58~(<9~7HO8v?#h_;aRL=PZGT&>Xp zzN1GGQ}D=s57Oq_j{UvH866sDuE+Bh)p%a0?+&g#&>j1ZDv6uI&ic9Nwh2{Sdc=~8 zo(~x*jfZ~rag{eM^zTgP?J1nj=z^QVF-V%@^M8=5h5L$LhdiK*BQwN|nK+uo!9Z4s zW!|EDpFoogNgWSCu#^aHhTs18j%j3I$=;tJ0BQhVYnzC@txS#Y{s(%RC{2?wd=!^4 z2LTfUGC4GtKrR6)e_Km)8@Um_>sN43AuA?=_aiPJ?6RGTYd6_sRh5J7!$=&7+~q4I zWl8@&jlnzs5W^W#q&It!C<+5Ky1(xJx`BCjef5`*N(=}`j7vJWz8feyP*@|(#o&4} z_&gLyj>ZIIJpB1#JG~oGFrXHv8xVqw5XFpm zFs2jD5y3;Z zJbVeuyA_4MBGDXiDq!Ve#hZ57I6<0K<4SQRd9i+83XpBf2etp8P)HG7J^8B}qNP7X zBZ^5i;tLkIe;kAZ0gz<%H2397CRhn+QIB}tG6$~)OjD8wWR&(KA+R%shi_7o5ra67 zCDH_TVZC6Y8Iotj@u~rb}C;*ZmWoWma5-|v^s@QEi&NMhk zE&7{UqE=UI)2cyKn~o3y2t84mF$QBF`|g-``GkPqX`@5``<>!vKchuHgOP;w`uU9X zeP-ICaMe@cJ!wH~V3UZEf%Gk&pfUHQZ^I|)e~V9c(AgnvgHMId-Y*nQUx?#Nt2GW) zqHn0$6D$I?&C-6XY6rF02X)963s8G(26*DrI>~c2;)GTM-%e7J5ny`G_qE^&s?qGy zG7SiTNO2Y05)`0df5jjU*nv{03Kqd8nsK08V)kv>tj55Uh4Mw5_M^08*Bg6MHB=(h zf0~41#$vYFrVoFga}XpNX7ZGpYfPbhBmjOJ7fp}(B*xHpaK0zcXikuD2-ZE|HI`VC+B#^p_n zf1Q|t7KX*KDOzS|GlD<@NV2z~7dRb}e*_95>F-=1S*;9+<^o}GMksKRl4WALo|e_` zHosjh%Z5%+Hy7VL&+_?X>pt(QM`jcF|7`c{{#s<~^=x^MQq36b2vWJn4^P=*J%>q% z!9y68L?@8fCTG;OzI}qdcfU8QUACL8mfMuRl+9jEXipVt3Ng%k*VgY+$Og=se_BYI zMm2n@M*K1w>)}6>Q3tp}BTNI@s8i)Kj2pz-ibmB1I*2;8e>|3Em}Vo2hu^H8^He#` zA*Zr)m*cHw5l5Rvwp!Npx`>o5y)MWtghdhsp6&!{I#kzbhAAoa`&jQ%nhylCtwv0T z*>aMakXn+qpQ~^p9x{Y+xGD?_e^LwB(n}oIiMa%#kc0j7^0pmc-h!V~Zo#1NBng(L zTjTv>YB7U|skQYrK|tL)V_SMPJe#Jc;sRE02EK8SNQw{6C=5F)U%hL=4L=$;Z&SJo ze4t4MOQIMIgl-H;Hxr!D6qaQJ^kkCG+*B4QC|3z-L7tC*|N$Df4j}>DcRzq zuvDYNUlUThn9^tmr}j=^IQMmU;o!qeXY>(WF&GLSS_kUt0Hr#laM`s?xA!x9-r^#2 z`LCPjlsd=O#5&!ZPU*C+0S_zHt2*1YYA%pzJCr)6l(aMg!8JJv>(>J5MK*?Y5LR-} zxyix0{6G}ATo5n`6?8W{e+UR~i|J~2hr5LOd|g5vmS#Are|A-~v%3@moPtj1 z5FYHRbJKI19i6tOtxjXFh+7g01Dc>VW*D)`(Xy)KSv<^lv-P~JWgcg{X=;z!0IFo9 zt{4bdn)Bc(LF9u#?q=zA2phMz<78x1a0pu+gWjw_53W3fVy`+Aje-|;PXl3PSpB{o z@NR6M==}O5=O=sJet zzEkfgJ(uDzBy?%GS=##n%goxCLF%n&<&SyUmVDS&9=`gmY+de>IZ7kmxNNT$c7)oX zg^f#>uGzynf8Rj7xCz6`WVXp~ZG$qIoS^mz1rGfilW91mH)u^O^&%l1%}MAwal;I8|qDe<{<^lxcmCd09A_qw>Wh9gc-D zaU8w+aDDaPE9((38)N_tXp{qp>xff1xLsU*{)z_^_~$1m;4PJWEQT%ykdKA3>YERK zx%$`DyW&1;*k}R@fwdXHz7b{e2rX3NQ$IXH2<1ibYnfUx?0ssN;B^K(d@PZY@=$DH zKzN&se>hvDLoEd@jJ4&)fn;IsF*SeqAraWMpL1|}^Oo8``lZzY(aU&)IUWabD+Xlo z2c?r6fg*&PPVDNVPFQen4iI9OGVb8kja>mnlaL{HBUUKv;kOR4o75((%2?UmI|JH8 z541HZ?+drR^AMr~xFyuds6a&a3I!FQTD4ivf0fW?D4Icp2HNaFRb@}AD*xZ8>US+k zeHDDcv?UG^s4)X=`E=FDh^~T3-OcP`=xqz8?fI4kTxs>OpdM*z4$R;oX2)|*0?uUO z2063sSXJM%pd`&Il|@;> z<-+3Ox7_ZyPEJ!^3vESm87TmQX_GsY<1)k;`#^%j_PxbYd~Mo$eEH&yb|+acX0%pm z*od#KhSt~__$d@zu$gAoh&J;00WH#z!kxVY%9Cf}CVGcg+;r+H%6GLf!deHNpGDJuhT870{^n)GJZhU*h3?Ur{&AhO*ui2`LNdtRVg zETdNg zT8w1cPuy>~$Vl5`5vI#inyUWO@bUmqdm?LiD5q`scjwrLm&4-tcK7dU2}gPa!BQA( zf#sZ_L-!=xEoo&arIM<<@jJrZ`Cui(S)H;uC4Rc+&>k-9#z0YuuLqiqLrHhYAKRiLhEhTFP{5-q}m7^KWc#6NW4Z|J!-Dh0!pMRS|;e#(zQAD<{`AZTP z!OtYVL4^A1Dymp}enve~V-uK$D%}D&Lq^J;VtNGK1>oduNgTxko$%}sy4;6J6BWp~ zk+bGOvPSt0pl!h~Bq&E`ia_5YuCBIs%-GI_*58^fKgPRPFUK=;%Qrs={4TrAm$PkM zS8MthUrx0&y3U*5wDSt}ZsqZ#+h#h*Ulu5pN}3P0EQ*@*@G^!&KvhG7;narh4@9M8rTqZESln_-zEG8K zYFqlY^qoVtMvx3QkQqlHXf7 zlJ0O=5pydy>EE`#4(mFL_ucBBP+7x1WKNs+91_L%&QR zc}kXv(#F)AFluTTe0ub4UciIFZ1nFGVeXRC={PWM_~+MPe7txHW-&I_yrtXIu*@YC z?T#+DhQ8^2h79^hNuYcQT#9pn9IAaPYZJyxH3g#D?mGbc7W;$>=0QW8IB#-Q=Ms%T5ne?&7%!xy~Bb!I%8b;!~nefj>@ zF`QtXJXG!@y@)oZ=_n8m@Qd!?GKQAi8_F4PLemQewtV!IMcMfvDDI+Ik6X{`hjt zBGKO1J0@^|iR)9#pv^^39Q)cpt^e|fn97^%M>v>^M$oO!3t{Ah2SPl)G~YBc#w7g# zZkd_9o!)F1 zO%?=QtgINgTL0D#l;tKD9xH$N5KSvC#A`*idV{Qmi1TIU--VkY=a2$SAsV&mc3MP_KC$Q6nRSGwBEPRXnOv-2mDAH z7O5*hSM*lcMU5|$u_e8k&gm*)!m?q)l7AI+pOFR5Ody2U7+^}tv^YF&a}oqYvHWI^ zc|>H}>fZ*s3O>}lQm@&p3)?bLUjAo@-Rj<(+esoAF5`YzmLI26kX3l!+{|kilpgL)jP8W_0(%m%95P2t+^?;&`MsEr zHiJGH^r+2(n{{`W^222KtRAmqg5NkDA6)RoQDTT1JXcz==wL;-hJe{?4FpQEj0+RM z0PRDW2f81jONsX<02rjJLwofNkl^rZD5Jc@V^M-M4uWiY)ovcH8xSzR`42=&`|6(C zYJT2#b-tcp>jp1|kc!|0jy8DI;$7)-ZpZAaek{G+4eBMNp323f4lW37<$A@vwZaJu zoAYFAyLcDKmkpTDuO@4A7w27>GX)ypZQbcMGFl~%HPdG2A^CUpeH@;N7M&y)1wsjY zY>Z)S69F$mjlbO}O(vk^bNOY`ix7kAWs>UG?=&g=dAf0^6UZSCH#WIEV$~2d5~YiS zl-!f>p47s_*jB6CWt}FSr{`;}>M?01Ls-BG(D93z=HIZXICcPMJaLWpzhC^BXV0QUr=_`E(-3dEru5N2lt( ze(Zj$o3annx|`xfwacwFB#Yu^Rz>7TSSP8r}&$uM5J-v6iFW zCR&GA(7;Tj%F`7Zz4n_%ynx6I6J1L75%XCWj z&Spshzjr*?QE9$$->@#9>VAHZp6v0q(BkfB*ui`5e&_R9yuNTI?5fy@%Q$M+T+dps z@)K0D>}l%I`%ROw#E^I|+x51aGm)M3TdR>c{s^<*yFH_U_u}>R43@p{kN;s%PP`k zh62i?ch6!hmaEQ8I0~Oas*VweL7eM(_)b#we*Y+w<5&ExWLr_21xcUJOTXFmpvD?> z>>%CH*E$N{%qd09-~qC-AUYgbliTlg=j_vWX9_p^!_}@kuew$T*KM0#z8SY!Uqz#Q zDpU2A7w?Qh0PU?WA}>^L4JoC~!@#FV=?lg|v5u!=v}6ZEH+d(^6>_E6H=!f<=Nosq zhVhz>H};P0-6HCfF?8Y?gCNJt*;=v-wkoZHHv#(P_jzJ1HlFX|$C(q(Uev~j3Wi2Q z(^~66(wkWhpJqlI;AIY*qxbCdPhs`JOp~Yxd^htKK*zx6S8YWvl3H(ch7yMCxUE45Y`EnE{eLNYlsmz~W zhSPT!dJ$cnBpV3%pw`yWf8~y`<73;Um~+gA0f?TOEjIp5Q$-e8 zuvO(u<3OOqGmig|m2vWY*8c4{FZ9*SDhthJVR1^J`wF7rWnO*MR#5N2eUZktiamaR zm2J}^yr^Q*f%HT_V_N64_{y>=xaZOxG&&MhdBQo^>+riUUnb#cRtuz}l-^75ulY*+ z0wC>1$n^_<2da@ml`F}xW`Z2R1`%xU;bGaVBAObvYejaHEEDoRl9rFwKx$e9`2K!D zeA2=hbo&jS*9C3#n-yzQtoO!>7tI&*y#QBLXc>Q+$d0l86RD~dU<=(qlR)crm zql^<|g9T#dDwbOQvv4Z4S)$EQZRB9^q8pXHlQ-g-?Lv02nlZ?@Vr3c!MgWO| zH_UcS38$?J3QpsohPac zZc~sypO>#JOa@L+%(yBqEqr$k-ma@|Csxtj(qPLA5rx}BNeDN2dPuXSIPh?Et<^y9 zl&!{9|HYNd3hzpciGa%Ul_fJ$F<`Cn0(i)vgZ4#r9QX*GOS;6UZq%s=qg^%xNS)yx zsp>B(>&a^ZRRgFV6|J=*B3kBo1JYOPEr8e#s`urzjJJdt>II=E!4uFW?mw5#2 z@aY+vU?e*YP)k}^9D}0~6cAYaXm|cSy&oxOocoD-HgbTkpttjh_lXFbHGvDBSS`*l ztEDDoI;|E-?>cV|Y7lmqwWD}&oU9WP)R2Nb(qlS8Yw9fGrA!*Xm|*a| zTLv=JqecL53K?;?Ch@&y*pH77fwW%U(?j`h-j)1VoS0~DqdFO{@*lO+p>e0NdBdeu z=;lQ9i&o+;LVRM znA41M4iaiqJtaFInW;jZZN^fL9w}mWaz|pIWOOxX3xTj2VMYE$vOG~{=#9VdsdF~1 z6`gfN*|qXT-(mgI8vqRBkty&sCi#1UBwq zf{mfTCr06d;f&>CW2e+pWaSYDm6xe&Z=$9#4a^sVaw0aTO~xrIMVRUL;5tY{EqdQg zY%x~hY$IlrN&uM`{;Gb|V|D6;`$u~&J*}+n_S1)=5IgJMX{v@+wpmF$W2G@c(n54& zl5({9%!)(Ch!M1o;>3ZZsMqrz)$zXdxbClPP54bl!vb%FUa7yG|8{HjsZk*6r)R86 zqcDe9OJ#)UNZMCs7&x^E>`HFY{IbXQL?ySPLRG+k6#z3lSL@om3OckGBbE#8mJtLT z{Q|!FRG)Fm_pCWWuRRO)#jD~Mb9K`dsQgs}$$H+ZL@!idMHMjPc;8vzVBmur0Vp%= zTWb{r>BtFst^a@zP*MnE1f<9q@bOOq4hW1yo?}$&PfJ70nkpJ0(0^CfY{aVQvm{

=o_-B*C(($?3rI=t8R`Z%^wwqKLfNpaeRuh(HBf zr~I0e>f0QyO+1q$X2QmtX>+DfS(xdCj8)I}E`q56Z}LKx-d!%>&>aD1tqv>dD;edR zL2_CkK?g9-^NVQszya|pF@E<}TAcZ7)JNK+{eYmhOYINA#X2Xf4lmD~^Ui@1;XpIp z%+gVb@-KPBHN|cUak<{T_tM)s)OaPg+bXlGdt)++GBXhRWIY9UXw*^qB+93)#mrZg#;)8AU+{hohZ?lH&) zeujGd=_Im1{Xiz~WLE*%&!wn8XJ8fzk8gWmYUk4MXXRk46oaZO4%10NFoum-!_Otx z%VwR(yVt3~P+~+%l}71lHwSCe2^hgIn2#iU>Z!bt-&i~xCV9ZP$oIh+PTaXmqgt}~A;QEB?>=fXj zgy5VofEiS$3h-?$%LQD87xvM<3B@!xLK z{2ntc-qyoY5}TM`jDswu8jCEg#&!3`_NTj#y&Qn zHaVFed6X%wi-=XHE2b^jel%gmaE$U1(X}r5-U0*Xi8Cp*kmGk`TzzK8-X1CvAut#% zAd-&mNq5mNn7lMph7xmfFqc9yTacF0;P4$i_J7z*YbI$@}7h)LZrQw&M{ zNJQtUw_+w$8RLMa3yvo$HW1)%6d#b1JIBU| zyuimlSdpx6^z~GSkk&mmQOWQuocIzF^j6~J>WjJ;F5eMjOONRv*-+VYetvyJ@))2C zYo(l5WXf@xqM1BhIJ@dGwcaF4O?c-wNo}d=lyFA?XE~Ya3V+7lCHp1n&?fG9-67 z?s$uo7z0`+*0T~(uHRG-)ZSMf1t6;%EK}@SKzWjfBr!(+eFfPTTZx4NAmZpJ`zK2X z9vuT;!(!WufbaIDwMS8ZYO55eqWuGf%i0MxEDQm;%%?|xu4N?48LBgBHDX$s0iy|+ zZ9d!StX(-C`%_Z#i&x;19*g|LE7uyyDkXkzIUSV711x1=ZtlA&RRWF+nJzPw5MsQDeu-LekXmm8M z4%H-VP{)8FBM|Klufu}p{rLXN3!B0?mc!dLn5gy@jfGOl6lAjMCRsRWUjISdJWYPo zzYW_KKlEL4>xCl!rdb|jlk)n(6Bq#c(~8|H@`}7`QV7j0HatrOi0mqfc*^rOp9EiY z>+R%UdC;?Qg45!D570h?pYroEhxbnY)6M0j`WPds2(Y`cy2pqAA%+NzMqSU!)R$

T!Q8|%2=CqJy1kbZ1qp%#^LqbOr-oZEK ztVU3{ccxw54LSMD0baAmht7N+gJR}gM$hsfSJ&SnFpot5j4hYWrXl6bPsgHTS@#iq zgd)YsVGIJH_?mlDyPd&aQI=%YH>|&O?E;w5Fg6$TDXm0Kw9%7GHrz>B@?j)w@;j-` zIx09!W%#%PuVkI@DmK?CLNbt}44k0bKJ1d-284?1=7}(_H@DT?ZfnH)zvA0#gk^%6b@f2=@o18*1Mcx(N;8q-&g$jbZ zeO*GaS(eHKEr&rk+e*s#-O=7`<`G}ke88!qG7GBS8?0B|l~sF#$!mJSs-ulEge6L2WdoyNrY%f>uIUl1(A&ndKG2Oj9nPfF%QePHDZ z5aNsqt^V-6W4b$lHl@)yhbqI*vos+w4S2oNQuj?JMxJWl^0~B8#44a<{i`nh_-(4@ zJT)t8rmBjuCEvfsvNU5%zaXJh*C~NJF9Agbzxb?-X^BslKA!He6F@Lmql&rU>NA3Q;FXn9YlDTtji7A(yD@)PR!qJM>~ zbQ9P8PddWnV*Ed(BSx10T|@#jHSBOW5Pe>16t>lThi>_>5A%T2@#Squas~EzE03Hz ztD;ppsxLqO!H;#Z(Tqid91@`y9mNl%*>!WJay&$&*hQr>ie)N!G52I)@-ZzUQ!g3& zu)M{7#8+Xg9sM0fljS>7u8#jMmLXf?{!+OvjwH``CDSsk_}MKl#5@3Ybi11xJLpj& z)e9(C>3L$D^zEcGm*?db2PxUp`gki2XqP9?r6e-b{*o76;8+XGL5Ix6eR^y-So26` znrwa2AJ7h~yQYJbS$Az@(4Lb`EZddHk)7plTF*6@8&;O-P2_v*$+vgPPr#`bF6#XD z=~rtKfz_CK1=a#9vbX+V=O2lvoZ|RcwSY6adR4t5FOw4s8ArVSDG3vMd27CMd;yc%sv#28*1( z1`g*%6i)JHHOiZ_>x3`ZZa;9eCXnF?-C>}%jgGI%&X+Kxvg;_aKsWV?1j_5LmakcD8(xe8EZwt&ZD+;nx_~AgGlGk zlzYJopH*<+51PQpQ=4dxz;n0n3Ogr01W4aJs4bG*Z6av-wl;@DmT0pp=VwaC+hSY; z65z;k7vY(I%(WK%&M2_>VoDHs*dELbwcfq|mdDt4{2lD>dhzchZ;6n|%q6xOPi~4d}c+Ca2%88{ICdB932n zcfUyf!}|IS7w>FHjesKxS%7<@``NU4k=@tjO*@QShf$BvTX*o2PDAT$uN$0g?ijC) z_=`uUu^-{qS;t)TPx?DvS}uRh=df z+kPvOwXL_=MmIJod5%H>{cL7vO`m4B%8lUF872_tOrs(ETu!lb0AZ2AB{eCPZ~QGb zu5wmTSec1KX5uZU^o+WU-sxWS`7vBk=@q%ti>wZ`k7zq}TkG*rTQm1*K#5&6KVdl{ z6|Dnk)@JVk`|ynlEv__tfjM{A7z`dEq)oVmXAz-V0UX$L2m9V~E>Z7ptQ~S|6~6Gr z!`hGpe(WM1e>_alt$XQXKQ~4t=(JoxCd!c+bee(o{Iv?WwbAiTUOFJwvi6tm=iHJ@ zljYlbZ&k-TNkkd6AH&-#uED#J^dAweJ%9$l=V|kHwSmA7W(1NB@72n4GS|d(-d1Bb z?=%_)_6b_quZ(`ab+>kGSqG?`J(GW_XOzbn-n>ZFKP0+eH;zkZsT`FxsJNd?B#m@< z2HnF)y<{OTgI10V>wYIiKR6#1AsTbK`7l6wrqGRhDL+x=DUmLzjeAm>QA@~mMW6sk zXr>((umWl`KJfFUyXnEn34T?POq5HVkF*XMc2H64zIJYHZZb&8BtiH$+Q6q6X4zLg+wM#yV8f$p|Y$$aOe+t7c)q9;HcnJVjlzd@W7N2xOYSQkIBqzWI*{WH1RDO z;;Xi0?ipJ5bPN(;&Beit*exiZ)`+3m12*E8U@pRZ+!@xmMxv13LT{~!uD=IhWPOd^tz{8WkCc9ojs?PBMt##bR0qvMvzk;e`2!r?3?okR<+ zGKcXJ|3oV!R$Ib*>!^&5g5*ZH<^7Al9?O&L_Dq9tjCqLDeGgJo?Md`U+v-DCUj1E% z$_F6}gMW(QtLs(BBv7oRDzeh@?RC@Qaiyx=Jb&A=mIVpoEZ)IaP||5F#}}6Fs6iU z8BsxwtbaDMaPiR!XI3CsXflHz9y> z2UYrgdknCDZ%->xZ`aTJ41gs^kaWgaLq|{kIqUo~B9+mxD@R8Ta1Sm}sk20H0GCm` zoE>|}n?aMt4Fmt?7pe~(6h?TP-ojWUK)JuH+D9h$gX=3Y=$G2X?OV&n%L;!=#w|2Z zoswDxqZnx-vbNqd=fZ-HyX|Z9ddSSDK?67jP z%w9;rK!u`WhMB&Lj+B!9aR#t3O-8hui-t}KSr8QAN~QezP=O@6@=J`l%q`Mv&a@`A zt_iH^-}wTMUUB>03f$Av>!uo3fMrcX=%aQA_Nifp;`AhveV75~#!;hwr?d@lo$yqB z#uzH5#Y}?BGPAaqO8i?|u$A+rdTejK zOSKe%l5ZD){lLFAO7PXqkeLh7j^(GMvL2o^S}^m!2oZ1F{7fwxx4&(^b@LuH@WO8x z%uN5ySOcSoD7&kD8ot5Rt-~$B2?L!YmZ}Oaj9S$7YT|U?v^`&DFzS zX8bE8ulO~7ahvXhGnQ#-0*HdV^jPCiccsidN!k0*91ht!Ho?I^*|y-y0Oe!R`ZN9B z`QtP!IgaKCpY%>J1L?e5Z)y8Y8rS*!{cNSZER?@hy=upCanv0-Ec*)Q9&oggVor}X z*XpF=5kO`#@YtNO^e;9_TQ1RG!ZA{5l78rb?%VBoC^5L)ku=W;`ll)@ly#wgowZMb z+>a8LNlcevmP1-TH`As|@Al!jP5?y9k>pGEEhmY>C!K+v;13!m-%Ombcl8|=35js4 zkqYTvRxoT5`Orql5?O*tdP<4+M?8-~G2C4ACX;AxdE-l3IbJ8PN)uK!hT_tf9z0Y9 zGq6DzbMvt&t!woV1z>R=S%bdwjS2RVp6X4(l#}exN`VC0jFmEIFgMH}E-fvc)v&;a zm*6hhBQV<`=&@YeTN@9*K`x>x@Z7;XvU>^ve=ns|_?fJwq^ zs+|95+7Vp6b_H*_(m}H&Q(D86pfP)15=p)xh3k0^ zS@RIG2Vpl&^bO)gaP0*}e7yP$(pE#@{N4WI_yg1If2S}lw**q>1s(QM(!^rOIf_vo zc%*zDG7Z{x2lkkIpXwFL)Tf)xx~9^^`&oXZ<;v9ziEmr=JYy1m*@x9DHYrAde-W;R zN>T(MJ~$}Gy#NF=rarec)ag-LZ2{Hy2WYe|)z>5|D~Qu=6BfZ4tXQ6t2yB(rTc3Hl zbA{roh7ckoK~^m?capxuBULHs!36!#0>CYkuJ$}5!2_<+BEMR%iP>>HYJ%9)61>jMf!oR;G`es^YT~cArA4+!tqG2s`2~~2-BGfh1@HtQS&Vk$$+Fs!bw}krI(5);xw+y^9{0Z=Q?*E zA1+iAJug~}nVe^n_Yck<X(AH_2n*gEM()1WurL+XEKx|7wHBG~X_Y z{cI*dd1vF*R{kzbg=jBm-id4}je6(_1flaR4ZClf=!sPm1WA$k% z)Gsp>UNkG>v6rj0#}-}B=u190(Z5?K$~;(cW_)xqn;J0Rw~v2AULJ}%mo#l)a3OJV zCxn?E$1$x=Vv3L+QKjsu7P1u8E93RcYO3ZF!j^4LR@-i+M-bRD@aeRg_f6GGSpoPZ z!3OFIvT3s!uK8xikiTb`82sk(KF3d>K~+|gN=wGxBlH-$A8P7TTlcj9P7vvA>W9YS z#gzBkBY&AuKN5c#q61-t7MLNvURNFrXkdhEpdOk@u!)o|NTf5-g=UyGT0k?`U76|` z7l7gRzg>_96Y0z6lg^0-U^{&py#vUrO+<&US4$<5aEn{iqBdxF#CX>#Ed3Iof)Pv1 zxehbDkuGaW-$^h7v^h_5Dau{T<%l`w!I9*+S4`j%?&${l`Y6~uSa+`I@G_w)77oSN zYqmPr{mijMnF=n9bTLOb=~9s1V#9?pBdofQG^*`xel^nlef=%mg$&i50dX~HmI_Bm zcNmtZ`UJ&Zn?d{KvKmbAOyjIqBU8K-i5bSmZNiJzj+?=ehkANBZfqCb`KVsk-sX(< zy;y}+kwinkO4>V|gkR;=33XE_n2hZIhnB<1$dMvQ4@~tx;m;AIAB5N?5-ntcJOAZ< zUp41ZD|K0*sVaOtJR)$My#y_|Z+@a)j@@%t#5SZvtP=eoa%R@q*~p6C3ml-E_x(XH zOES<%2{TLEMq5KI@P z6>W430erG-T~2Q30TU%lG8Qxkjl5|z)2OaTm6e$<_ohiR&Du6AC;l^hLj-w{M%eQ|N0ncN!xW^&G$Lx-IZvr40NphjwO8e4eG5 zrrAdf}?Q}>j(6QIA*0>~o z0s}ehT5I^jw9$?j`R>&1+B8+>y&%G60?MiOU@s3r2oXQ5`z z$h2o0Fs3gFGlaVsnA7j-dZ4+>l_CQTE_socaTuUb4^z=1?&RGwWH9XvT9sZ zEWZ9qtb%%}!SJ4{tY>6VSe0DWLU z9r?aMA;q$_>6qR&RND5G+BVeM*2lQ9OzyqSMum69PRDrRfJI9JuN~|NKpT5SYFq9G z${33*bR#<2fdZ*XebV54QxcP$4(6QWwt(v>rPY-`u1$_sH?`h?C!Btw9I&oBAb?KG zRj+ApQilBcjZVI7ks;+|Dr8$usExM;jGDokh7@X>LDZe7T%hH@g}DS1B2PLjeo)qY zz#Ff&JQ{ZlF1fxk+R@ztXeCC$lQ~gQPG4o9ED11am}f;69IK>}dAhK4QdMfN9A?$9 zBigFImP%j9_vDCh8iOU5Aopp-g2X*Ys0hE-kjCkUekn}9kEME-eA~je3vSS(pW(|E zl^FgjX}?Pr1|KYm`E`DMImMGrdsnifuwmZ$nfj|!Ii8?|*1G=_FglDsyTMiC#TsA$Qg{mEMU`PNZ~raJzbH zF;ryf$}bin?MzDp>=wErk9e#ljo2-L=wj(_@9vLSBiK3Y=<%1h$P^>78Lbj2;>b>X z3*J&0@;tde7~k0wKwi6-Q1QMRJnPpEZ6hoZAoB>b9{fzGW|S54w}zCFekjk$)55~$ z!Z*w_aVuNPj{%fWWxk>8yGbEhO@gW+w# zP%oNRPx9s=#0EQqGp1udYIP>Ia|-1itKJk5KYiXLqCFD=uq*#3LiX#Qi*u5$-SW4N zvzkq_Z@X=Y2`^{a{1DBepobD(A>;)i*`z)m>k2tC_T02U&S`6cv#h+Cec0BRoZ?cF z#A4bzk5tH9rI2f)+w5<4{VP8|)kL}qe3)FpnvJ38?82R{(6yXHJ1{>CMj?}Gb#+2Tn11?nf={kZ5l z!{SI8eYP}mat_U^t_gUJs9T7#C(uNEq~+pbv%?_`z#nT<<-_ymY^E^u203dW+PV&< z49QXHH3B z7=;>uU+G!ZP#w%;ZV8rOb{819e!}RIKCr47GM9i?jDxZ;Z`q+ttgSx^6Luf(U>EE` zw$5xdsb51XaHmE(es@{uBUK|Zv?%;6B?3sPnb8DRM;6oalW!*!BokXdn+1o zPs9fX;qcR&A)!lV=izGXyHMDDm zC42uMHIVb$Vuh!n6!UYz*s8qb@KUZ}%F;6aN{9CxSzPYO=8LW~4Ls}e77kv^#gt_o z-Hn!UGKD(~wL1Zgdr@ycY|1|ijbBv&VaZb56YP=F6x1N!YLkoFBpHpnV*g(U9B|ol zZ-u@nB5Umh%)5WClrLxrSeMrpqLgPHh(d>{Kw^0jfqbB_;62?F5SUA{jn;pTOGc75 z2svnT_>y#?ZsMP|;_THhLYA6{!)7_*r#QTu#y(y_{a8h9-YXjASR--;u2KMgh?m+< zJlQD;2w#Gf;~_DV1OZVCesRn^F+_egsF%20VGNVUC8xKDHm1AEEibGDp}js)b17S- z-rE^$W-YtXGS(cietr+=0_GA2#@NxN!9l+0QR#yEATvv@dOuK!AZ_rpQJ+W@&R^ug z%ut-!{O&jVUDB$lMKPXPnGQgcX__*7fa^_gdpDgK_9Mz}sp6+W=wqnI45NcFBKbol zET=eTN@GZOA5>-NZ^{d+#XMV3sNSpyj(ZwTHwXEwuAtw-@z|~lYGb@r#>$Vs3ftmhx#i+QoT_7n}({+QUWTa zZg&tz79ne@ne-iEi-`xNW5S$Rw#`SOFN6is_6|V5pB}k;R^YTenQ}#DRq--5rJ|Zz zCc{`uiMwjPLUuo?m+JwPQdN*^+BTsi>h@?WPJ-3Y=<7(Oe1!;-D(G=c1&rm8p8}O+ z8FXnz&iu7%dcSe@K&`vg))n$2ml9VDalvgw1PO{=B0nY$a?^qX_FlbkC=D?UX5spB zg%nBp9_4`p(mjh68u?%$#=LOs=5fC3p<8sAO=I!D#Su4LOvqu7npJkcfwoVCJO5`` zX8ON8^DInU{|(DD3FU*ofD;m02k`;4Qi*hCyj18=#B0UyGfzAi=OG;#J)fl_La6y7 zw};n6imZx13=~nv7P)J)z-f#h1d374F)a1%W%H)lyR63I!;EWBYYCU3w$cNu96B6H zu<8RN@zHVWK!;Dyxsn+)5hYrFo*WhOV8gSMK{kM};ZynA zR!!sT#y8bl_u%@cZ!fc0V3_ag!Yj}0`~Je%6%VSg)_nOh8a&!zAkLVM3B7d zE-X$_K#U_oQ&E7;FuCU0uVE{EZY8JJhgE6Y2#lZM)#|50r<)f*`Bdi=r?Ld4+}&8#y9#vg8b%uLKM zBv93riajRP{v;c2VW~1ZyHV7HD=TRE`d$MD$%&LmCLZUB?or4m!Un9{2$DH$%QEfa zI3R3JvFXPIWUD@Ra`T$K^TOa`mNjmjlwfY?Z$4nN&PO!N%lrLWFF)<;1Ixz41=?RL zwqNNe>8!l3de>Js)XloR8>-nd{caN2BvBmh3M9$nJe~FL-uT*P!307Uj+_ndOKEMF z7*9vb`GlDlq!l+56ad^+&OsD26&?WP!>bK6}|KJ(Vqj*vyaT7BJX>^KGA>;v7|PKF2NP9xv}|r(nNjm5YPqO0fzFe+UG0 zolc)}MUkL^!~jPrA-RGzJB5Y#JlL&|gX`mos4+_c);wHf5>_C8sldLvKN}hEGn^U& zF+HT14sKsV^jqX`)Sf20pImQG$2%PSt762liHqZeY0Db#`%)0ZM_9Da1y7AQ6ApY4ZAi2LO=I`D~rTpE$te(A%R0a0Lj} zscS8+vHAU@{eZuGJFr&|wL46{H($p=_AgD8Ir`G7a;VI)KP{|7FgrXWI@;z3DvAZbJnDWg6-fUmOMVY+n$dUnvK20>FN~h|n0QMKbdK zAV8Q9USheszsQsj9qncoepljgD333q@-$7*3*)!Wa?+-rh3emo@2N!Yfkr|@h{Ni` zI`sOS#>H96z#}e1=J?Q`d#m;8>Ee5BenHlP_X3XEv~8YiP>x0U)1f%Bkd+ss8z!4z zMl`ybh1R`T1#=?b?CXsX8ybS+M`61nnas$6QCSN!BsYXrZSmyO+G^M8wR_qb@XM`n zY^0P|k5V1--vw-jW3ux5)~C@$*-L!mGDg~l79HgTfZiuglLrwKZAQ^fURb@0*B-`f zjsZ*?)`oTKA1XKh528R_zhHM$rCHvJN9e*UKQaLzFtmaph@%l0J^&oR8%77M4DJ=n ztOKadYzt_CQtE8yty0953QR$ub*#6|v-(iw>#$gClTlyM{&%b857GJsLr*JAt^ua$ z#f^4M))sn)!tzkqAvOxwhBoTIw44aq6i^+e<_jTz&$KDMCM8;#AdOjKQbjMD^}Tjs z>TPW>4Ie zlT5&;%8&c<)2{rJ$S)i6(5|E>Fl|9oilk<61L)G^Gd~yaM7l$5*F`!Uvmeeu7U94X zy#%sXXl zQF(Sz@YAjW&IZ2-f`5>}H8hwD2F1U;a#&v~nHyeqB%3l5n8Lx-$cLYyx)V_2(RWwH z2@Envna+YwxovMHoqah#jl9`!?4c@rUfD^17ThfTfZvRn@s|1iCC|M`ZOv$; zDVuT;W298CT$}>Qe#WrNi{oIT>_eg}iUYWOdD^U=!t<1t59C znmDhkk8$rKW|lz0vzb354hXIc!QFSjc-(cHz;IC9pN(_}#bXd3aeE8#>|G#!aAZVd zM@FHhjMm?xsjC;(|fb`M=oq516{mmTYHwfGu;Owic zs2|E|+X8t5%ynlny{KQe+k9VbR?UI9xo=)fa&wszO?-p#y@I)&Bs-SM;*W!Wuy~BO z{2G!w6r687TfcTW&8J{39hvU+hZ*1_&hh!V8cs}Wj?dO`vEG=%XLeP+7VoWpR~$bH z<=>{pyH=7?wVrkg4ImHd5tvyivkPm@1Ac zxcXu6_v?so1axWY;SQ$Wj*Y5i7kU4ZcUImu2OAvMPi}t(r;5CHs`wYavNJfvzv#Fk zHmVw}RC}rls)pP6t0J8P*y}5AVvx>K_@LZjMT1W-{Zy6@4k@=~*gUidSOzeKWmcaf;(;P!tWN}j2cvCR~daZnTkF*PxhL7pgo*b}LzEC!8R`SpBOv*UcD>jFjAMf+4EeEFnl4R*w6B5EBv}w4**&QtIv~ zh8UrZl%aV*x2BQkQ#FEg1S+TK%JUn>G(45II%)^L2CZOHOV%^0Ko~a){16fzRE)*T z3GdsJU_`X&N}q18{{$OgbiUe_=G|wzjYT+BCBorRy1o6pPIu+Q((vnix!SL?;v%?h zsS%+QAV5P9@bzPVA)vYv2XzD=B_{QPuY*Ft5HoHI-m`_TKx>O;1X?5v)A`@B?J}#B zxW98QF-th8nj6?2N1RvQ|1MqTSF3be(#9s;?^a)YDye>6Qd8G#1_{~74wmB10r5QB z6>9p$Hv12M^&qy}Nr5j`Q4hhYWWtwCOjsShmD1C{|3 zH&lvoNErryN5b0qvZ+IxGx1WK@$6w3P4pf_;#xT`kn4VaWqbXoZ;wiXK z`x3sV043gp%@Epvgg7K|kVQ6;_*-F3Ov0KtX08#ei6N}9N);!;8eh^N+IZpCbPZD_77RM4bmwGIyGH5^#i($G9-`! zQL%t-0-yzFFvC#=C>l1Bv;$+lDf+>G&^3eDmPk?x$`^_}fk#aU+fcu#hDdP$+(Z&Y zIS{F>5hWW0w?Hljiej56hBSyFFabNTQ}U>PbwOC%VOyUzff+LB@njpaXoqZL7UknA zP>w-6qfVuOtIYNT_KQU7dFz$_;>z_gO&zPCtr?*)- ztI(h92RgrA70b^>krn0h{e!LA^YR}kj*8XQy86kkvjg|jn{cC$LT%5OoY*N8Q#ytE z`JF-y;B3h;Ewbx!S2hE&o3Wh@_u&11hw|@rR+fh=m!-I#iOq~Or+(7}lmN!Z)db3F z&9$QJGk*^Et?i_E!#4{uFFP$EU1Etqn0{1~(FR?#|42q#czmpdM<%uK6ynnB-~skH zCi5#>I)C1`yN4qBvHUOHRl0BwzuafrVr8ROS!g4OVes9XZ4STMl>=KWmNTq>iqb+9 zyw70eTd1-&m8G?*tgLMx{FFYfYf5B_@_AZX zvF#^G6ggObOBMGFmw09n0#Hf74laEBD0{yqJdK;R}6Y4xB5jRJFs_c+YOU2t- zME5n|r5`|NvJsht>vastfab13)}ktC-){HWE-Uun1?$I!6wX1a27cLPuY2^O?9sc2 z9vj-xLu_bc6J>oi^mgeW2uTt%&)j5LCMBJzF2Z$vdw}xY@{T^a(iXu#gjn2?YkNF6 z5D(<@WBC?J1cccezs5X5~8@KI)*7*Xd7{SCtWF zU;bU^%XGc2B1*bm-Bvn%SncmTmELCgbylpF7h#vApW4N;{+we2mRsI@|{Zbs28}0Avdn$)n(sG>_*>&O)XZN7$Yd}I? zG$G|GTbHC-^_GlPYr-hfJnkHXK?+RSj1r(Q2yp|>p*X^V4S7d183uB#50rTPlwxvx z{}PCG*3Jxn(9Z=Qa6<`p0y^ys8(-})yc)#zDXWnqSNj@R&4K4|G1JqbfTDwH2)(K^ z%yoVVe5XDAA2gK=cnSQmRlsxY)RwO?Lv`;w(Qp>(?9`4L%0#&tH`U9m7c&+)T(1{6 zD8i5$B!^<*XrJoKB4PCod)XlEUH~14jxEf1Y&m^@xVJ8i0leCV-$a?e#x5+jUCQ&A zz^$&wC4LNF-@+6LLtI--D*7aC%ec1BUrN>i)AsV!H}%81 z2RY(@8_b+6HuhJP!z-RY(-(WonSMP8C9lmLRrf@^pU8-is0owsbNHMHK$tdO@!f-< zEy>JCu}HwTux)}04TYY0Wd#^%E{PEh84=8L>gMPMmcO zBv_yZ*I&BeB7XP(fJ-EiFs-WDmQQKhk9T-~r}R7%!gRQ40H~t!J9iD7bmt)>5QQk$ z_L3!Lp|V5`P;<8|5o4v_DN_n@k_?6%y<8OPWBH3g7*ESzbSS~~X9Ehw?aMY3XA0)9 z2@wP+PdzC|36?q>1CH2HHx-Wxwa*WHN5$EbMD}As?E!8W#H=JB*+4=UGc|B3=2Q)yqwLbARrS-7JiUL6MU`LcYwP}uv)?m=PiCc97NDN_4QS`S#}+v_CX zUE9lFZjOAn%bLSEKF@=fn<1(}`K3;O66jSo&T|}(^ZI0Ao=*+)eEgP_?!FL8S$)h| zX|lKn{Z?;Zg4XZvclnpqCT*`hQlm>63SEF|n%?XDb|L2JZneMLxVu#DK=8hPr}H9w z6T=`ydvBqB|C1Mcsz<5+4)y3Atk7ws^=r%~9$5Epz<39Z31qeQv|772SK8kY@6PbXCeEGu%i5dpkJM2(Rn>x}EU6vXnFHB`_XLM*FGBGkYlR=&-lg`=_e}z#XZIEk$J0M6>pluKYjYWKy( z6rGZbC1R>BF|J%(k~oWro!C59r^!fzD+SA>kTDQ;iYxBmhG39vfSsw>pg3YM ze-%^3KpCqVZ-hq~gOtHWj*c?^C`fX}h8Y_OgVBtIMWX|Eq=c&pl#K!#X-66ym55~p zvV{QdvO-vsQi5Zy;9JOXibh-@zd}EF%nBZ(LNgU|MaeFb(Fk53wE-S8K@9>H+4vMw zor!5nn06v$kFnqlwT%nbP}{ErSIHK~f7R|~qp;n;O|7PEnME^9F>>3i5(dy$3Z)nw z3=wb}+6LpM?udcf05(blp@}en^s(5W+5m5fq*=uFNueq%?^2~=%ex%1*(767@u;%P zWcG&=GZlNixXcW8@i<3=M@=qgh9D3kONNrceAa5aV^AUnz(+378Eorv4Pv&%f56}Z zVRMCYQpiqVX$%4YWikbFlF04=Vle$IK|qme3Q#8ns}eveMQl(pz|PQ(lNxP+I~D7Y zfj1ajVR#KNR07P^@MbzvpktPxC{3m$s8|KSBN$#G4#0*-QtU0m+1adlfAyd1)-_M&^Tle`yu5w4>gNBvoc}RvzFXY9Z*N`) zI3&GkerSGdzJE=agg3M1MSHPwuK`al9h9T5!B8qbLkU#GLu}AJac5_)dFh&eEM6^K z^VEIS{`>ZPb-9@P?CERw?YA@hGftkm*AXrg+zZ$I{L8QA)SA^7Ky-e4e|7a{kPh+z zrw`I_CSRdzn93KxY-~XZ*#kunl4VjK3}I4z6&+m@A;rTnN9kB1bV%cLDfp~A>pfe{ zSDnV6A*4a-^&8LB;%2y;Ac)A_1dhXqN1F*Oh|sl}2qt+mfh(}ykpaqFbu%$PHV?NM zm}s8gEG}NQz=ms{KYiw!e^>3NmAmI}4Y%j#zqhmId+^%MSIaJh<8=eoa&dce(b_Fq z-}+bk{__00#V6On(9o}yfve}|H+UCuZS!h};3amoU~jxwu(#{Z){~Jg#WPtf5O0x> z3*ZYEgfn>|`7ESAl9NE*(gT<1saUQkz@a6?>=UOkPI%YnkOq$Of4Lr=`5gOn2&Ox9 zS=1vekJF{(GXms;bay_74DJOX^C)6w@yz3#l0qWj>9ZZG=RqgE~1z|;5VfZQPQ7x@QAXFm(xQd5E-w(en7AtJmo-_snz!jx$DhL_YC6M@P#a2yT zc`O8%(=n1c_+ZuchLIeh39CzwU?lc)L64C`J7b8+-OfbOe_I!X#sp=EH9>191ZF*o z?>G0tbh0#0&d$0WnkN^wdGoUQ>BW!s|EmwH)%7<`^S-@WowwJ^%d5qF>Hot1*Ej8Q z=`R)^8}voBA!>p&Ur%Nj-3e;adww&@r)my2h2JF@Zs-25%Rer!&3F9b=J&>C&C~Yg z@=xgZ&-U%&f48@*5AECb)AeEwH@ZB(di(MGdb$8g-ugq1!84jsPT6Pc6%%S(%Kr}m zEOtsfE?4yE9w;BS>AHZeR<@HzLqA5$zU%YsopA=xE+V;%&=t6Ocji#MCO#@73>Vb> znqfOV^b$ue32epvow~c9MYeN=b*fS6QOQE{Ub4_Qf3=*@bR0x_bOYtY4vkpRD|wU- zG^jXqlrCB$t4Giw;FC_M6{{ga55c2n>IX{7ZBq~RXj9Ez_8YNsWFuB&w?@!<%G@8V zg}0M61V5`UyqyoBDWg&jr@2q(AFr?4{WFGgWWQ5*9F03P$(qC%N9ke|Z|6k+t4?F6 zx!6J6e|GC`VZ+Yz@}uxm!#;k>RQ!}pZCUh`ILb($0g*r=>Hzp#j%c6l%q}CXhQl@F z>cdB0Pgk^20D5g};qFw5(4eZ7wQA+;K@OI;@0RVwrhb$?NFRA@R_acFl|9kJWDFkJ zaKZ#0$7ls|Sy^6uq2UH<5Yc@=J`f!T!9JWxf47`!GP=tix~oh@cU?i0! zcW4HyhaI{s$xDf&bbz(jDMzxy)!^`9LCgE7ffQsH>h^_V8r1EpiSc3|P1aZ5 ze_Z29p^xM%0Sy2mRbl(iCTR39kI83>Uj+J!78A`lT46ybvlCf@+L8dWY2<4N(J2oo zP=$kqa?eV%4>OUEW`$43Ng$PmvQN2W=t;(MgXH(asARIti$^Ag`@)_uAGr=a6{>mD7$o9#yccJP=qjV_vEhV$ev1N0$m?{v{8O30VI z*ElDp$*AAtM=G1^y2eg?(l44Z#kCh8C8XN@+@wntQi6S_9y`Q7r87iA=C)(}WG$HE zR6KeU=nDPB=#L2QUE;sC3WXb!vCR_!Gm~*p6azFjH%MDXnDI#V=LSR>3ucD2lYnGtq5 zCm7=y`{8m<1)jZK-K>`W^6FCY*)HGguP>>XmAiF*Se2XoOI37&nle_*8My4ZVy^4foXw+N?U86n6BQBOlD5~NzZA6v73 zS?(UO*W3={p42#eQYvH^g8|%w?=I(h_NJnfAsy_Aikj?KEO2TW97$$sB9@)Mq}j#WW`PBTkT#9fKBNTV_fk=O z>nThudANkd5Q(&8wcsdE%?FoMlWcB~;3S%~r><^`wXG$m#c*jr9LQ+dOe@lVdoTic z9j@@(t*gEV-^a7o+LTILuQ zp{M8$K^z7=tL}I!=m?#4G}dl^10S_X#YIQ*oqaN+Zw{-&qw@w5 z*3C!;icNHPUKRPZIYo?sBm>e<1G~V2pJ5jUTwlYDMY-D*`)#>d?)@$frT@(B2eAQ9 z%!<2mVK?S}NPB0(yr8CdXE60q_bi-yuBoiCuQt`;Z8~it?O3}*MCguvq4By z8cIJNV7J}sQ_{-=R#RVplGmY9dseq-f~Mh@v7Wu9dhKBa(I5nH05BYuXY>+cl9w2d za?iydvKQN^4tBg(2m zhxgayAfbyH5HyegTI>iqC1BVc_?NTuRy@y^V%|o!1gymaTp{`dv^!*RWU9q+$R7jB zpAsIGA*r1O>zcVNjm2ObVZ{acbzqi8lC;z~(9`1xxTDChuB0Bv8w26dSaCikrA!fJ z^abJXp8|O}foU&)KwFHlr2i@pfFl?ekbB$O&4c~IL-e__RToQ;?+X88zRB;(8=IrM z`xdy`PghB}B(s|Q;6GOztCkxFW9`!x<2dqN`FpWAtn93QI{*7kQLc-_?(y~ITwsrL z;K%CW7B#y-j$_%om1qUnvao|`Sq;26l)JtEX|-9d78SmKv;?`oyv3VYj0o!X#|3xxxmaKP@*B>U@FTDsad`g0k*+hw5O#&W zyX@V?zc0RbCD7OjK~ke3-Q<9Lm5>3m!1?jo%yc*3S72lsQA|$Ao?$?h_98@ufbe;4 zM7>b;B#Mg1iYKfG>BfsE>^P#6qoL!g3DbOXnC4%9C`^+-4!9myqa@yBxUq#nxzb5; z6<?LEFj{9x!mSijvulN zSLNlHy3Y=?`k6mF!zQiM1!?vp$61Z)#eH)!!&3rDVo!gB(+Edh7rUxMABQLH_uza7 z5EcV}PybW8@+k-+?UBKhbE%}KRG?g8jp%S&W2pDV0lS?&}FYu=gL^a?o)k^pIX z?t#Qf{qto38Wgm?UlO?JLhJO=oXLI;n8gC=+G_H9fd7Z~2ORO^+I| zRc%jZgiCXRGlKd|`4o5!+R1T6vfCFC&`vUc16$M)d&7va8WAb7sJ@PIf?*P_8sAIO zSWklt!&3b=UC@Ui>w#*Oz6o|-*`GUy6}Zh-TvhpXGKv_O+n(Z5ft2pLMU#jjASP|b zgRuaa3n$Q;a)^C$BKu^AeO-H(l*cC>r#Nksu%~#Jsk8*5uLRR%j6;kc3r<`j>FUUT z!@&sxjy)=6;Ccm{l$5}%LYY)jLWYx)A$dh^=*NMRYGqXH`i_X+95)=p^vQ-ZAo+Hv zGf2K8_&$~*jD~w0i|WZ}A(PQUVKx#8A{adCj`$gv%EI`0(h2EEj(YMb1L;L4mBp0D z_4ynjOa#7wrNOuY9khKdOe3tsaYrhDCmNHSYD`~P>?R@hzA1GOJ}^rXlTibr1?D}^ zi3*Oi&7HJ_lv11v;g72~l?YQg4dIJZ5x(r?%{UOJa(irLb(ORj37X3zkxsFgQ|5~y zv}@%Efp@xWO%OV!(5v0N<<#j?SpDNPsH^-@~s zDaH7v;`}AvRX3U{Y{DQhp;&u6rMfy;F)`hs94;PEwdkxp(Mv}mc93w<)BEW^dop`yt{obG4f)&}*>?T!L_ z;e)CUfJU}S(5Hk85wNDPK8Oe0t&CvNHv@LnDA8kjDy*kW#ZH>)b7CqV0((<+uxNKz zwg2G*F%bNZkuQ`{_Ul_wMRq&TNlC#SblN|h<{MdQzNxzs+&x!+aE4EBlK$tQ*!;?( zFYV6$ll~W=HkG3dzK|zy(y+k{n$G66Ik1=@DjzN3U8?St(wcG z*`};a;QD^kl(KDqS3N)<)_ITMdRJDp;5H$GFQht?`Tns}Fkjr}^@+~z{;qiCFCU7a z9mOB_gc(iB&=`1a?QjL5s&`I_JIBFoB6?n2-hX}N=-Vsm#Quw3g+mLsb;1mic)Y*? z(80|&Sl}?)0b@czv_wp+62qk;t~@~_-R%j8a7%_wull@y9Ww9J;QoiFcK_HDamf8A za1fw|H=g0{A3*`1u>=bTG_kw5)_frX7%_hQ9u0Om`3UIaNbf0y7*Eu3ARZk{j45YRA|Kbzppt%O^&A;y?EJ7<&{bc2ie?)4q`y^X1Yn*)|N%SxeP6>$g-2>Gc0f z2nUkwd%4@h82+8rROa8(NW?@FLKk_JS?70q`wlrbv-fpZ+xr6~pnCd3V3w1f6%hIz5c;G^GXU zzuW%|vdjIuJZ>Ll^~Uo3-C?!8d-PNftHW*GNV?vZo5KHl6ZplB%N2=F@Y`il32sxU z@qa`>neE)L9dO2Z4G)`5t)Bk_z05YVld;Vc7&I^-Fd%PYY6?6&3NK7$ZfA68ATlyC zGncVt0V;o6kJ~mDexF~VM;oZPyo%%keMr$3+X9Q-O$Ymu&4V)5OtctVQ^}J_{(V1Q zWQ!8*agu2s77GL!nWD*a4!`rA3;o0O)n9(rs$h}`S@Gifw$QxL)DS9FalI+tte7Bt zT{24P>X+THuXp&@bdSTj55D;qNK*ke#Ps<97AkX zNjJq1R#k=5Da_fqIS_e#Co-d?Vx|zp$T;n`@3uj{I*9wO+k6TGw|%{B za$!Z1g^C zfy0(HbH-~b{8e|YO1`cG3-tKo!?sQC-!~T zWRg@u6_8xps2!y&2Qz3_S+Cl!F=>!=;OQ!PY2p!xs!n386t7ArNAJKhgR=4Gt9P2 zB94r7&i9zIgjW0_=M#2fn1D-jsrL!*ed;3bJBsZc?yr731Plgwg2Rayb~v+8>`!qx zJvto1A3F(QRuP4b=_Sr&z7E6H0>giFhGFY5TqUwBBcpV-W?a84Q(UNAnySEE1U@*4 zfEdjV35$pc%Re87_qS{AGLEOZ-$YmGZo}Wd*AEZv?ru1jwnP-aieG(-u)V1xkQ)~{ zKR)!$FwBtsYV+T&-R;ANzHw6;xa={)9DJeaw=>Ct3x}P$uHUq~dfyD+r80jcq1&rq zs}x%l?o$Di6=7_QTa?QzH!#A1_|hV^iWx$N>0AmSCb2n-=@MS6T&4gbHxW%AGK(o9 zs97$SDq)E+5Yz&92LN-G4;xl#udEq4w8Kb9h1+iETRw)hQNJMI!581QhgbThDAs0#oWkdp7rNSwW z3Bcc)-Axnys2+W~-M`No2O)(Xj^a01X7!KUc|dAS4~hPf=~H=V8aVJI8rXDb3X3&qFt~M-K93hfChu?Zf@2$&4A9ZJNGyYkqM5fy2UcEE)oF z9r$^I7z5L@gJ~d%^n?Z4?1v_&9^AxodDJf(@GBQIU2v%gKV+;J%_CfxMUKcaA!uZw zi*vWDpRTWdzj7rPHbj5QqM#}=4CE{nJan_Ydh?bR8@&9rphQB4CqH#tV1}aZd-ug} zSO2>D!I$8PiX7RWLWn~U1WDi8*Bn`$5|!`_Z|wv)UEG;7}^Q z+GHZP>k62ZWUg~!S${;6S*N8^OQ~kwAL{bYWp4J8)UCc5b<2NCsat+Ib!U7Qyoq3o zw5yaB$y)^9O}7OtWL;LmgshtQcwn7PA}|0fZbr+n{4&--_BiX*QY=}E=TR_QYb5&_rBq-grVD{U zWUM(yDZ--c(r14uMPpZ>Q&b$$%ABB-M4O|FFHDoVB{mnnG%_ajaeA70&XS4*mFhWV zgMqax6C4*a-yrbcT5$Gpox;d5W@`920wFeJ-i1#CpSjkNYmO+*Kf{T5&KwT-ykL$sj+J%> z`Ah=>R3d*}GYH`F9P*V5?&pBdjPZ>oF2QPAz9gq3n^aE04DkQdbW)K@iRYLfM*c$H z2gb&Z4$nyei@O*JCslJm9G91&;Rru9?2HK=>}}!pl^1p+7A>08*6>P^bZ9Oz_v>sk zRM2V7lY>2VHnWV)tu@&PQIvDGH{*XXO|(?g$dT_Y*$V5$R)OW`Rg}|4 z_8A?|1OU87*qf+$GV$lL#DB*Ag3D~UVKaluS@g`Gu&acQw|%E}6wmv4q^;wuQ$-1L zU0*5m`J%-5I&U)Z6e99z37fT5EK0ZwtZ_NsKN}1$i1&G4)1rn2&01!kYh)p1H^Tz7hV<9<^hjr(zdzcCnZZ)Ci^dEM;$`eyPwkc)m7H-?EWwr;S{x{IzaVJKvQe+y#H(@8i(!?%bn{?O2TM`^P2>#rvmj2M-Uw zzQF5k{SY2}9~ZOQA$)dioX2nnRp0x5FJ5lDJ}%q%yD~@aH*MpuuH0T-n6_z#_AcJf zNkfd}X-lkwbp5&%+^+h`!Lb!tmr7QvSK(c(NXo9?gz08gKRoo^$95a<#@sBose3%- zHFg6FJ>8(!(@ooS%@Brm-98Nc{@Cv0cW)l!SPK#?g|xo)9|q!mv+>uvrDNKW7y`37 zSN;R^dy3GLvCR{c{O}Y6F*rFhm$795Dt}9J8#fHT>sRbi8dnSnzB`jkW|C=g>D1QE z^+Bd89M2cC`_ULQA5PN`HM*L7dH2bQ>i3jo%BhH_-};ktNTur91!rFQTz^WX z`fjJ2b^Me*Ki%16^-SA^3XljCrfi5?wIkiE> zj4bviDK(mTYb$ERPBp($+a4RooPlTzmKkkQ$qA*)Gv#HjYd#Ws_;eTKBH+w|i6pba zW!nLfpUTx1115!v#ud>g>Jo)z(tlQLBn4@4kA{R?YC;*wF<}T>*YyM=xm@TorN-bw zB9}NL0VNq2+1mz^v_Wl6#IbW1+Pj za>^`HGmT&b$$V};w2K~z@Igk3u|VaLOl(ExMuUge}R-8L1?R$C<=A4LO5;E=2}8T|iC=BWTE3$A19X5DmE|p3%WU zYs`V1fy{E}Z>tgL;p05~d~pzU zrBBoJV?E!jr`MZ!m3{5&bNc!HK3@KnNiM@`&$Rq(9`0;8pQXQkTdgUJO3RrmQ&K`1 zfA64-miOX}ZYO7SeSbJ(A(YuMUS2Q0&+zQo@6-E~jVO-X&x(4C`8r>umohRsRlrNb z@!{rXo<3w8a_qO#PGtLxyV*&&Y{rGni>NmKG)8LoLqXhhJgie>4aFTqRx@rrL1Znb zt%&r*aA&Q8AG6KVX-XtG9k@U`4hB3WY0E;I)7yw#2=##0>^BFcm%uLz9u3~ zC%T;wFI}`&7^JP2(&H019DJM%{uzYhqtzlnrW?v1g=LUIYIc1)xV+-g4FCBB(}}`$ z9~05r5I|&tm_>9k_+#*)-RlH{oF8jeB_W{<<)W=z$(c-w1>n%^3^~Pk(#Z47fOvzQ zA=$4pB!6D1GbCQWH3a?Zci6Kvqz`Bfk)@+IqJC|(a{Qi-tg zR%gg7kWODHba5p7zU~XY5!5ut!y@f4ss{?79}3=F7w4b`;Wl<^2zL7REY#Oe%v`?h93cO>SWVni(7F#BhXT=e5Q?%QhKm*qldW z%b2;XB|NujF8mH1v)B2Gm$S{-6Kf(nl09}{;o zxPKszbaZqHBPEZu3C3P!Sr`dv$fTlIh7bYXiAdTQK zL0=*HlSV()H~Oi=Tn*4ajMbP`F-Vw0Jgv~AFq$a)gItY{e)))NOZbDP!96vtfdmq{ zBwf*KCZw-n@PL_3`JT(}S=uEzrl=;7&VP{Y%8eutuS_Xi<8a!!=B z@+GfIWX*4L_%Pyxt#4@^HH`R=$D?TM&@{ST1S_EojT>*1ckz-wJ@g;H9MdJBj==8m z3!G#iw#WFz^&7wZpPYY|oFUCrP4n9b%?e2t(#&EsbEFjwiGJo!MLBxb zBXUvffH2GTnlgXCU7pYO+w3NP9^>vz5QG)6Qc0y4=Q1-S!No#0E7@hUgzsm;vKM|C z>2L^MLESuqZexT=q(G{E#WuEy<00;UP}36xMv~}r+-cK8}>Gmis?X2S5F(g$-&4s&YQIW#bZW1@D% zhmtPi9zs(HA3*V$LPC4R|9x4kjFXUZvMG(-%hX~kfBgMx;lH@sm4~e{trSo0bGPOu z+qf?mE{gO^-KQzUwt$&mw&e!Eq{vq8%XQZ5vMemb!G{)k;1_qrlnNiU`K=58{HG@< zuh4YHgr2w!)5-fb?hS-Z6vDcRp#MGu8jV7H^h9NzlKUJ2H3PacZhR?(q7XE5?xW!B zC}^w0f774;{o7mr*#Vq|yEgpw>8pGGv^o6YQ-GbnEc}!0t87ZK|KmAakl@L(si--R=)4UmUJu`p+;ygf;5%0W%!Cu0~WfYL)>R*M`>r%%@yTJgL{e8O~?BilXh} z7Pa@6fge}GlMFTtPCiqvTjVZVVS3BF9(SszCb1#&p>fkd3a3m)Tz?#1++P;-T}&J7<% zD9A@L>po0mU-N>*z*QqAjhhVEwjEA4ya@g_;EN19>L{pA*m_bDpv*s9Na}1MvA>9r ze|oG1tHav(E-oKrUkuRdQ9pasOAFa^TWDiOw&0}(gttM?yqvVbptu_92(y{J0)*S65M*f1k9`f2V*FvwEt)0%;R?e~X9sHsy9Js+wf~ zyxo=SXbA~qB~RpS;Bta(<5>f3PX2r{0*F^zC^!TLAOop2Tvc^0+J`oV)dgYPs=SLi zP@qW==RnhughPU&Cc&c5!?lMVNgai!CXXYD36@x}QP<3=#=rv#=`@Gcbkxi#e^NG~ z49vXCgl03wK*MPd=DXB6n8zbGGzlyyAjk$ch#_!1{15c-b1aVP_$G*Fquao%;^V4m z6yZ3`=-teP8Dm!iaxibn5oDlLCC5|a84v?0i?$k!h1cNwqFP4v0kWi4MTb~U0%cs7U|7&?uryD<)#^ z;faTTm(#(E&FB{1+4v+T+ZX$2_y<|DZY2IP!q?QYB2q_*RQ-cRx8-ike?1LWF$o6n z7xXVx#%$^oz3xD3i`|nM7#d{{Vrnet4pzN@gDI&q=N{((b4BCQKSeI+NWrDSj&+PnLWUiV_pzOgR?^n#%m^V9Tt6aPLKr35;>yQ}f2q11Tic(<)w6ah zyu}J{&`m$s}#elV0HPqv7 zqNelG-t!xpN1~Qwbc)E72E?V4PYoE;S2g<9$z8t-70Z#ft89~>$loYvyTIfL61byu zDW1x5&{k8s>Zh{Ir?PMJYM0sZsB>r17{$j8NAux2n-ZYaf4(2~rb24oOjW|Z*x1Uv z?28TH4`XTjk!k8Nq9^Ne=TE4X#)!<@z5isB=L@&W=BnnSa$6Pa;#+3O2hSzoyLX4J zjqB^5`jT)M{w#e5&=g#KhtoX<$6KU8ipWuj(uj!@N=ra0?9r1P+D#AylN^?b?Jd}> zTT4z-Q$$yff5e`?i5j&#|F(}T3()UY7GO$Fe)_a8R*rWAEpC)5bwP*Q?N#Ng)!w}x z{PW)Hn{^Q8WnZkftFK;si-j9_;;F5uJmvHe+yILcMSQT3@d++Fyh7T>_QJbG0oq^? zhCtF^vv!xe%(M8GcSD<|4dTlJfS9cV9W+3~m9^ioe;z}pQBn4h+D*|S*YKY=Qv~(% zewlf?mW>m);&Y@%w1T9@a?h;bK6b$6(cL2ku*KC9c2z(|=1##71FkwvtX_wn9yJ5J*4^G!*TFPDaby+=`Z&8lZs%`reVqXU0aCK3DbV6zC@N#DU?uWKh>vJ>%2vPdHbhKxfg zEx91A_0{9G%E&Q`tn|F!J4e_8%29r0n-)8Pj8Mj=Du+lYSI+Hk$JbXK@R0|EduzgF zbE7TA+7?}vxcd`h5!mA2)IMro6$j8tx9HVo7cppPkf4xCj2~I)naV#N0uYcfN4;(X zK?m4|+%~BQjdMyrxVQZH@BVNQTx^E_PQY<(ihrg_aQhW3rFttlNVT#fS93fYOlx9O zx*-RgDmh9{^hTl@BQL(>v)78}?Oa~I!X+|*76&Yn`sE~hXu+JAN6-iYCE$W{3H1k< zYsxhzQ*XEhL`fU=@*AdQUhThV351<3{SOxK53(DN8nE=k?B3u&>V4O^p^_6tyO!kb z@tmI~E901xMU+Np7Td5jQ!YH@dVSWrAqN2=|JBMjHJn5awR+OF!{Eu^_4bNLPV&>b z4w7ivkeZV+ieYgaWQ|Oh4&MiKo$Qe=H9S?Wap}4nGKU`pJdaeh*a0o2Q zP$v8Ny6){Q(If*%seHy#$a;X=Oq9(J{bvB7$`Dla{`S1XBM<%i1(bv)4p*)AolX-N zugosJh77Ar>#hK`v<_Xj3qrR9;3eUh#gLHZ3%!rGDeTRyJHl2X>Y|D~zH2o1QzqWwly2@A2ryS3mx zGFC=wqdN4ZNHDB?9NSTWe^Kr7OD_Eegl)=^RTalTLlUxwlM!>X+tP2;tQ2_8O0oEJ z;Ze;S`d_Zgc8*hOr4INM$j= zA^2840x%_>l&DT}+MBk?bciA;fkc5!Q56!KG4N}mVeAQP#Dmr@SRT8sgq|YO#JPHL zHokBX((zH$#7C+IlNQDJoS3K!lD8UCw(#Fl*{kZ!6v0WpbHzeD3Iv>1vxOK)4_GoW za;tvv<3$9p1vQf0wuWxE@iskx>L6keVO0dUj+z5V=q&$rVC)X~TjlaJ_vd7cT z%zH(kMj><)pHN}%a3rJ$G{B-vjKNJx-j;Jc+4?e#t|VAj0PBhg z7|9+)H6%(6=)a~{Ki)lWP5K49^LzANs;Sd{{Cc5&NfD7uaw1ZIqqRfdz@&Yjamz?0 zc3nF!RXunap6xK=RX&nZb@nV+D-dyW@rj)g5-=05TKt8(xvd}RMkX_8b>WrbFoI?& zb3c}X0NO_Jz851{y!C? zk5e*#Y4mW5IB=x553=LbV??M#JHV z|0lmpdbHf9TN3C&z3+rSbGblhgh~)To*nlaBDz=0U=rejW@J5eWX;SSyRFuN+e})* zi6ToC!;p5d?O;6)r!yRQ45|l8JI#We_Hy^qXizs5A<`uPEX8fvzf~&xYy2y4>f(8+ zx+Cs-BfmEb*yajr-VSTYdujk_Qd^bv>tm95UhUb-;O6s z?ZBzZ>@K@#q7B0ca0cd06dAd)VLq!@w+(T$_${=ef{(Me28fV8FJ>hqk|m;I$%;)V zciJi;Z;NAqi$(J?qTk;=y!I6c6pkdXnD)1%TqnP`&FBQBz$b?0STI>~Ufki3v+=Tg zXS}&B*+=Ra0e*`6642Bfc4A{Fx9V4H6P)EZ(M)%Y)?417hTrQip3DCBVoolLE;(bu zIy$`goa}<;)-0yh4lMn}sLfS4me6H}+;xfgxw^UlB1vEOf9O=BHu?AnoNKCi{}2SD zsg@54o)*VE;F~dD6)kIYdKLPQ*F9}nIr9r4SE zZL|UGgRPV@(8*uo8>_}L0jDm{zFOU?j`H(I<Teem~JpLZ$Q6avA$JPA1 zss5%Zy#_$4)$FOp%eNf3woAV&#p8p2)dq(w_T+m5=q;*p^%({lp<_n`-D9NvyX}jn zG^JVCN_RKJSexIFY-3=x$YdJ|bX)kGh-3NcVnY48pD!5Y36Rw zH;qjww6ybkc4XLg5N`v{7zmo*(wI-cq3(r#a{~X{upe#(0@^+=1Vr4)hs*4${$4P5 zy#V@u|91|iWMnE-5Ekxap8c4A;85OIohNn&75z~oSn<4?UHI=f3;0ux&u%B7)w zjlo}QfVM#!HRkp%yi^Ohr8;{l3#Pl9zB9eBd=vhC`_tcGonyF^_M%en)BTtf0w~J;W#Rq zMriiZUszNjT1NI$r|MVt?{Pm*cX*Y{9H@>oP3%t5plGlnQTkF)#7Hw0es=dtkJ~a7 z!0wo;wDmwdCsP`NJMv`^XimI?k4V#7t!*su#>IgpMIfJ2v3fX3Hi+lrgN3$8pe(Wi zToEl>31?oYaJzV} z-aBysUk{3G!&OUJ3$|K?AZs$v@Ij7%e=B~*zmZ4v+tU+75Cz)%TXc`@bR7P6)3*NG zlCd&n)!2%nBmBU$|EJ)lrmB~DZAi`}o6foC*6Y_*Xq>5`=YLIz9axUeeEpHk;E!5( z!vP1(-_3r2Hua7vXz+LJ&RzWWu0uv{!rcl(y=5OuAS~>0`f;w4^NgGboy58x@HL|e zRAvGe9C5}5df*UU>Wa$xcJ}v^Y|E$4zTGk^6O!p6f zIslPM5am&5JZo~ibl-)H(5pEfkRV^@-`?TvVn!CS-G>53JsYElgvD%NOHk+k3eVVj zgYtpt^zt>#;iu~uRGs7zy`mv#;AfGjY6}suq$+BFy&8Pe(79bAW{QSv-i-wo$twQa z4U1&68k+r*0~&#a#KJr!UQ;151mskDa*C35UK+`oCp$!aVcaxDMj|a8fCWu`qDcUP zgz7v0Km(e&s#SUd;#7%R8hSGx(FQSAZl)t3 za0Mgkm$_}iS*g0#fvU+$r^ z^D2{v$iEdd8T{Ln|^^W9aOKB|4tf`k5vx$ z<|TwfwrMzjZmh=vutSN-7oM53+=xTm)P2|E-cc3NOsb~pcK@P=(!{)fiY&yzjf_iF zZuAtYxZc5zNQ6a3ch9BLS4`DM1l20`M3xpI#K9uQf*~j$BA+pj)6K#(b-?X7X-7ON z1!_v9OB03#P?XKO?qmYd!?MUt1qAgua^z^I-;sP7vOXl#0 zge;Vt$L-Tbk_&Uqq3dIjn&G=^LQ_%=W2)eWr3H1!PaZjY2RRa7I+)AAkzzG(%?Uyl zm?9Oi=xYj@E#nuNzJv1bQ>m6@IzhHxCk>#Ta*9L)#431(NO^@2c!dBFQV_K#r7sAf zkkoS{?cssr)kZRxD-lhkr@n}t1t^#(X#NgX-ziQEEio@$pHU!XA8|#g$n@UKETGnp z+)S|!bj;EJM3T+Osz!PAgP)&Na-CKnMii))`3<3OKq$=kJ~day8U=M=k9N?SB>tW9 z#aU|(una3!Ev*Lj9l+fubR$akM;+3C42^w9-#u-Nx;?%n8HJiL1u8#e8_Mp`TjlI8 z2ulnEiHNbm62vJde7efp;13}z^x3^?d$sGJ-I!kHi1`S!JI+QBIBK^j`d3DOZIlcA zM~A@(SGil4H><%1!J8_JBIZl^pL<}lcZWc>&Z~I*}WnMX|vEpYPH*tLo+=#)9A_mcNU3^(|cK3ie-K-iJoZ z`_z^~*SJS0J&V-B=pAA~Pd*>55kz%l!5m^({h|8#Ay-?p9P&Rh@t*Xx>xs@f++)Tuh^PVZy7ny`stNv`F1_Lc0!fV7}h#so9$ni;)NJBm(x5C7Slm zsKMmV5kR@^v+$U&c6Kw8>9{n&gaAVHkXA3O&j!nn7Fx$_kFVgouP=qex__J3(S&CW zV;d(FUh*T-MU8vAj%|c^_HV3C$H%q6*s@}3)|q9`uq?A%{!LcNWWSHwiLeyG5h>0x zED*Bcv_E}XILMd8;yFy4$|oXO?Ss`R;H}PDpk2<pdBAy4`K z9*NuH1!5#?7EvtB;9*#1gas)Vsun<@_0Cdp;c?z zE5uFpxqX!&K)pusnd^?V#35}RXN9JZeToB_Z13{jQk6)>R! z_>gAGI@7RfBS+x#Jr7OCoG6e9-Qiyf z4%+Fc)}*&|!y3HKnC$&U*LvHo?9>KOWvwH=bhfTfq&#2RsKgGz*&WP?O?^z_y8%F#=awgzeKAZUj%EZ7 zutCV0sV8!4+L?ZhS&zyL1uql>Q1+L63UJVkOTZ{97Kz=`4w_omwA9{Q(6~zXz?=#! z+Uagw?b648{nyW*T0ng=<-u`!+DSw^-(OBIF}7n%Bhc5Am+QYpnP1b3{8pEsnn z`8rg!acTfw)gr-_B2fA7uX90gtc8{Ib&+Aw;BLEy)MsBINUn3ZL)44oY$sPmfHrOS`J4 z|9%MW$t~+Z5O}y}x~Z>GHp$X{bJ+Dx%lyer`+TIrla&Fv=mh`n1A%1Oq&$#fSwPYY zkIfbc6@j}C+b?^;i)n*hYM62q%eb@)9pYkJ{d=J{0Hser(8v;90~m+fckXtQdm{rQ zt=#aNTcr~+?qnCNMd|rK+7R56GIE-nH~>8jix@se9;5P$L&TYj+dK0|-QXIQQe-{m z#{L_NTc**UThrw05q(YmH^ft>FDb|02zR5$>$FCZbn*?w5h5M2d7qS(-Wet5XMNn5fAo6qFbSUPA?{zA6h|Hesp`SVd!iV4;=6IR6F~v8d!f0cqp!xyh2H zflKv=;6#QssVC#~4r~5SHB*wmYZ9myDh>r`Vwz>D#Si-@1Fo+>t42w7R@72X28hHh zt7e^2A?1H-c+sp|*_V}Cz5bT;NJdJa1I3az6rc}+F?BLA)12v7@WT)s9)98GXdv*H zpCleej-SHvS?u6`iTHD}JCQj&qtwI{;a>JjCrSS7Gpx_MkE8o)^f*yU*?O*o+^6jq zS~M+CyJjR7E&!(Ou_eFM3L9crujsFW8Txi01SpT1TdP&BBt#Q1;T1jTSFzj7uaE2S zZvPun8-le?gdNBx>c4oKJiZzsku}QvYBli4QBpJ1r(ff68QmoR48`h+fVu2sW%Hf1 z4Gv$*hKg$JLDq!$SZH$7=wOoI_7Fg(!=Y5Lr1>QpI{;YNP<2J?vRFfqu9vZO+grMy z?|*3+COe3dxO-t|Y2_x3d%E5puo*dS4&K2f@VHzZi@QRb>$W>zoQXsW z!HEjAp@;))N-2jz6npu^)Q4-}ca2am7uZ7z3ZynchoVIRMQtSO-d&mQ`VE|O;voa$$pFd?*5 z1j3%`lxl%=xN=VJk6aXe+Qq>UWI@ZMQJMbe>b`RH>Lc3x8F8FyZv+*fchu?GJqG9N zXa$szu}iLt5LdoXI|a9rfcx@cJaZ?1p==0}B4K8Ht5ELTr@Iv24;+90oN#0Sqkt#C z60-3wb69%BTl z;Fr)Cw$UwB-Z~T|YWksOBGO|kaPX^3`2ikdStNTDF}amWSq^i5UoPL|E+4?| zY~t*O;R?ZI4R~gx4#3w5hN)9f?+2%$BZBFcbK_v6z)YJciQ;~Q8)v%W9lCj-xUcMW8*1mSy5uXtB^vs8fHT0B%bh;3UJL>R&AIN}>T`B%&7)U;F4c4zB{` zhohJN>1*0nRT+fKXZE~H@K{f=zFwQ zfZibJq6dSH%2_OnFx}3#AIGfMFF9l8;IYZF*d9&2l3vsI zA=98bEEQ(?7+F#JyaUt=6fR+B)|2^muiLyFaqRoHhpTj!OwAjGST{gSLI9>Bxd6FB zkp1@fUfW!7M#Ad9taQY#ULhgvRq7ia%jqn1)j6w@4utNe@#fzvh-SC0-@6s%fRl-X zi%zO4u8&}TIPJlc#{(H%L~gUDl0T7FqiwVBxbASp-V&?Rr(1BnMTgww}{7%~Ft?~!p3FMw`2N{y#)91(5 z^2W-4Ef9kq2RKn`Bs&NYQy2qqkKdd=_U7>yT!TYWjP&23`XVVkb^oHAPk5-)=pJ_( z3_iZ|E1B*r&Jtf*%{#VUM%|QBvVHTGGs`-gG*|~HV7>)?p(IjMtpFLc=a@;Wi413? z2uZ9v5|=a#)9+gM;9{1>DWa1sWJ4b2!h4x|-r+@mdKs5oF9`Q3OjpjALQ5y1j14Lw zW-Vc@#ZgIzyPm~FLc}Gu-Wlh_miImD%-Z9^Z=b|Ir}tx6CQ>Y4=A898*5TGd6{5_S zjS5xBTcd!tA?C@4Z~|LE{pC64nM+_goX!C)(L0xoEIjgHgj{weT`kk z%v{}-W0LjXtk`bq&htkGpP`KNLG7O{ra8d5yX)exNysZNmOY;G5MI@2E;mQ7ka$OC za2xLiEMn6ABoY54r71y3P+addE(||=u>O5Oqffq~YGeg?^a5V`vXekPtiWa;>>pe3 z0lk}iW?vT9EU$NUr~G#g_LuCT4;vU52dhgOA)2jipc#4Tj1z@j!hKmjN^`5CL{q0P z)_>cyifK=q?6&gj)F*lCYA%sJ8_Ct5=dqiXor`U9S<5s%LgmvV)7V{LB+MOh{_ zlPmjAO%+Dc9s$h!sBuQ3kp9pVHFsMFPuYYkQ(51?=Igk{z5_=zAbtMua(7S>66j(f zTWfLuoxPyK6!K}vr-U7Vp(gLtv2v1X1wSQih~H8ZNmzDv)VO`oCe(W<&q~@6?VXYc z=f@ADnAG>(Es~${Af8E#<^Z_uQn4UK_c~`pSd?f^KrwoG zcE;LsI*4+}n;e&t0<0SvfWDM8TM5*ax#m!g8YZ_pr`k5ks~Y7Cj;?znw$*}5Vyxtr z21z)CH6pHxY9}U_m30d+{m+PT_|z`KZC5TyePNSrKTfBGnLieNReJ(0?9$rScphz9 zI28iBg8`$Vp?<}oILF*8Pc~<)5%jB7OI-9>DU#u`69^z9r$%vxd7^QKe_YD9zbn)A zn~T%dD7X+j+h!VXZ0_COPWMLSS7jGZw4w6f8ZUi9rLxqHpAp@ZkFqc2@BqL8?6~q? z_|(w^;+&6$11TrU&(vz^fZ{%M^O-@^M(@w~cI9wN{W`O=)H8o|iL(@Qn z$INhM?9!&_T2L>M5j||)=J|&VCvhpuFeZIxg|;w#3GnyI3(##)Q_(l!@oqA%PRRMQ z*>zeCLNX0;JlGx1`dPI<4Eh>C{wcE6T7@+r*gg3g?zq!io!>*B~yErB%Z7sBnr94!c#iOvf1^ zqIowumpIsWto)#vMl@@eZ}lxFsCvHxX!7S|xoN2NY!F7fcr_k?M*Sk9STu&0*zXQ( z>b$?n%hLd3r2W2;X%Wm-`0C~10I?T%S8xb$J|U#ParmgwtM2lmir*x?bqm2S) zB1tntK_*UQgKIvF0P&aa5GAE1l@4n@lf4m#(1-D8NM(Z-Cu31=Lm^};9d}{^oLj7y zq-e2%BLFAR3}t52%*W>pv!4=oJmZFe$AmQ7+YZi0$sjO#R|t7!s(pcCaFt+4Cb==8 zQMUluS6UTgO3PV178dA#cSoFFO}-{g9E_a;BLsO$a@K=Wcdb*2TjSWEd!a|wHPcV6 zx`UDaz~QBii`pqVg3o zxMzMWn^2cUsw`x&gvBH>FtaXHyWZD3D2L6oJGyL81bPDm(x)a06KOt5YNv<2cFZ$5 z@X>`yypzL_;2@rYLSj;XOJSlwv*yt4rcDIeWLUz%Z~w(e$Yi=#Mlc!$$U>oLzNUZw zS__97YcV8>Vk%j{04bZLSl5nbzeHuoilHxLVkeO{QXj2Q3!p*K#X~ZoVY_z3Hz>7Z zU?v#}_~mF;x`dMSwtyWojHc(P&$Ku1Fb2B4wkRVxXL$E3Zy{S(RH{z!yZU$2VI|;6t!>x}dX0mWHv# z#o>QHfV-5XqXfYiem@6jB6+yE3P294T55W3EM{8E89D|71b!1LUj$@G@T$&gO@H#df)pXNeS*TGO>8q z^K_KU%#;LxcA5cgFZs#+Uo0?S$nU8dL&0R??nSp5GACXv*}LOB`{PBmu<2l6iI%Va z3{MHNm)P)9RPZJi zy5{f#HaatUC!tSi?X%liDyE~T=D6@|5QXF`=0G8U5Z|3_R3MqBIrL&$AzS>8hzVo1 z#NhyYs%_o%JPW#ENV^hEzi>lZr*(|*LV=PCW}R~7 zt8?bc@iK-n{Vcq`Q$>4fLjyEy!`!-G2x$T~ec`}s3!btjPVTYdr6u;Dm(h14-4yFY zfi4k%8jeQMfq@*>udCN=k-w=4iy2!Zs@=h|igbQk-comJUP4Vju<2b1)7^)qV1d~ zdV2g4FK?(*Z}s=_pI+sF-m2))pv;m7V->9f$4QJiYmHM_g$Q{lPYcepRN1=p1o(!c9kWez9)ymp@4wCTz+m8=BVE^ChBB}l z82)7<#AZ$#_NfQB0cs0 zqxE0*_Hb8q&XG>6{;6EWU z<+gL!BLi>p`xZA6V49WI7!Q9vFG;R^613}w)U+P2ZBP4ty2HNjqgr^_Mk~^}jWdKy zrDC{Hzw(x`j>2pQfQCto`=%KGv-MK}d{tmGRGxDe%bmacgL+R`tn`b;&mp+{ zlOTz>vXc}fY9W?vpn6BLPy?@1;cogQp)@(N(|%~7D4s&(pNSfhlp!~poh(dWNZ{fG zE5i4my^D%{4%d#g*9QO?@!L2u3v~Jtl6n`vv^0;ZBlSN16UP=U+Mqh5P!2Gm+7)wz ztxh8V$S)4;@BgAsPOkqa>SX2O{1066pN883cO3br>N{yi&9$&`9gYFf2w_YnfkdPa z>{IWTUz{i=M1h~wc=`EQk(-O{&S?`i7~YJK5NsAjBd?(bcvMsKxScv$n{kMx!XlNY z{-OC`gkgqhilW2LobVI)dmH(b_?mfQs!Z2>j)nRHhoqc=&7LK~| zcC>K>6Pe{wXe>RqyNE!dwfCFT^hJ<==_#=o*B~fYCr7D~PNOBx?qlf5qmXKKZPGsx zF)3uK?_0I@B1+IzEv27#GJEul$v(IfK^9mLE z3ls$Y!PX5tAvuj>78`pAG{*I-WYl#{`zt^bz_5F7JaM-< zktLOStF4!hha&}A(Hsd3L zO1>;pug!SCgGxQ>M;wTcB0TKP(w^U~=Nfw%P9}TmD|q(^v|nfrm{W&AwEe{ax8C;b ze7L<}hY|nuMT>K`jC;UO6`3+sI(Ls!3CS%ycO{h26a>1^k_~sN&K1ivb~?|RLzWU< zTl{{eTOqA7%TI5CtDVX2z(A1w%RM%;$hKf8d+ZMcs=DV z*x^j7GtW3)Q4wt&pclVt`iFK>Zk&~Yi7plHW%V>BK0IhGwqJFmmko)L7rmez6&Z$? zxnN8$WFU?RE$!~_aX*lxiD4c3l9#@0Hr$wa;-U{7CbuVz2}68WKPFFT{o#;%x$UkB$GX&lNU5i2vpaJ02Z+&awC{6v@JtyAx`f} zC2SR>jtY}EV1{Thq?ruzGB_Nx^0lkl-{v^~7f*dw3EbrKJcIQ4hB=^5+Qf7v9LxT=9q&xsui~5iC8m>o=rZd0MaSRg*s1#LlqISCw5m=Sy0k)HkLa7gfZLJAjGiJ4}hfrz*3*z!IA(f zqGlD1nZ!S1^)TdI;=U?yHD;*y39nPg2K<^_{L7(f#(==QZO7ui>nWn;EbU+ABY3Ob zWS_TJ*{q$uI-TPNc0}4@fi_pC_;pSBnW6Y6f!8JmAeZVGOq`9a;>Bu8k?69eP(E@j zFJqLp@X`2YM8Fr|*+M0u7HzUl+^sFkr_TcqcfuN5>1C3@j^*G@;|q!ni@&(ec>Gr9}EVY5%1Ao5g)(SeHdP z59a+1P&h~5xzU-O;{wh$y;!Vm#_9Gbx6;NqVcn8Ne_S=yfnHjVqL>!>xjsr=$pR~R z-LUxqp_8m~YWE8LyII;sIO=zL(A>D$Ma1L2jOQ&=Hm8H0!pYvS+WpAY0SeyA@3We7 z-4p22PGOa7N&W>vP5XngDLd(i%fEI{t{qcxfC)QyN&5pvH?+IP@mS&Sp>}SPuGmT{ zV~blH<~6^v?NYts!xnOcKUkU@#~VZs#P4gWaSP%XY%vPP ze5jfLDC7LZ8CYLpZ?-Lie(V10PT;}hNUI<@uU~ZD zwK}lF97gL`XQo+{`AdsoVKOr39pAC>1 z?Yf)`Ea=9HtunM>L`R&3Bu=ak!7O-dcDejH@UN`o0Ift2BK>TEVOr}R(FOdHMJ2R8 zAm8&Jy~j`;l`@#ob}PVGlRiWb7ba{Mzu5dFr%2jLy1mIjziv7Cx;Z99)CwI9uS}5qhSaYMXgvU1fI;=k zUQfh@D6Hh12;>|K7s-Y_V3q0F)p#ZznSqKqCbmD~ni39o04g0p|DuP9Bp~5@`IspN z&)L5Zp~Ll54%J;YtNx&O)=+`@6%(KAk`C8Zm&1{cS%$7jxW~#Rl zp%5a~vKj6Q$N`w|?_6Qzz1;)y2LQnn@O0@E_cyqk%o*eVhA^)GPw&ja&iy~9;g+^c z@BO<63|Ol=acBxGed6bLt(W2Hn+g8%SX}WPTCbmfV7J2Mw7Z@&Mh*Oqn)mgeI`nuw;#C*6(Yl^^S z+l9*UXW?-XV$hB2$n8f~b{1t0NQML@>;6Qe%AAx=X=gfl;(1)pGbxKex6_|O~~lJdBN*(c_}8^|GqUDmuLuTLfqb@ z0&yA)wB@dbuq~qvhVj{TfF?U+gD;0+{~JL=1B!6VEjOdHj&A><>78p| zc)YVckqU3!0Op>1QW-Se2`U350f?GdmUDo21sa45&IlX@K^Sg25KD|_Xz&ipmz*fp zXh=4F9>8y^hM`SO8sN@tQ4-?=`^Zh2P%_DMW@h{$Nzsjv8{S)&@hzv`%W%$Y3v3y`&%<2L@Scy2gCIcU`dY0o;j5nA^Zhj%AAt*C&QRKq0SeU|(SQDNr)ub>GD>Py z4oLgT8cNvMmB2ts5#GcMJC|l&k4#d0;Eo+8q^4hV!D1n-XbN^sAf8{b@mODhEr8dh zgwCT!E7wNUL!wa~8^bb2%*&(~j6Q(Uv^NaPGHd_H`kG1s5mKrt<+VwM#E~MPO4&zf zmpeyt5tm>eP9Vd?pBnwI~UZ(-Y_}pDe&5M z=GteVyQv-9Z1@?0Nw$S|`Q@yH6Gv^6pVigYD7zxN{ch~J5xppzJOZyT3Ja1$v)nPu zDEJ3^84!>^W<(G=d6V7Am_rva>^FJ>vxVt&r2X`|>b2>I{Ea!aggu4HTt|SB4lt(c z!7}jzt+fGaO?NZvBaqZ`zWdnYirAll*(}n-5XHW+o0B)_t_1p1JL$xPzz~4rQqi2< z-zoSvb>#l4Gbyv-Xd7 zZ8W+I6?m~75)&0TxlP&El%BN=GN9*%7ipt=tnJldA%(7k)(*Dtx@i?}qQvrjum|0| zE@{D5FfolUwpdH|vgz_A$?NdsBL!8|mwhUf50a^q2~P%JZ9-F0Itx77F_Nu#LG=D% zajr{Zm+97ScX8dPV9u#};0i@H)WfpkH5?^(Onb0pOe#B=g<< z<3a`hJJZMCmi!bdeJ*>_LP2OO-9UHHgtJgynNT26MPqR z^le?vd5D-7X2@hP8dB89O2gU?>HU6!sHMn4+x673r8OoTDdXR|BY-gRHW~RM4`5BU zP(gUlbW#sP8I_A6VQ-B>f|jIUab96hXQ6&oB;(&?2CYofK{i2n15&IOa#+mP2Hrr< zux+I)*>n{UliqdYUhrsjNs##+CJG!rnBFvfLr9I4&?FOjIS#<_1@uiTlfMl-yFzBS z{mA;CGmOXkE%3qX@&Twyk{MYq;Bg_#Gb`P1VQ*ZD#RT0Pn_#xW>Zi*{QA=)D~J%>lzh@vj0+V5-Is^ZC}$U`YbJs8!W%Ob`EzJ`>u>ZtG|MA#V8q z#4*v6nx_{oM~@AtLu0}R0--{goUjp>^tc}kXviw1{+oe7xY>SsLTJEj|J8&5N4h!= zxSSaN&vlC1rnWx`=4>(EI_jL_Smu@hK3OIbTUP5(xs*g%cF^2V;PoL;wgOcS(ys*uf_!=U(WrhzZs)nIQX@!o48@JU;+Nx*JVW);vxI0_~+Bj`ghi}?0Z^)^shBIBA7j$w1QnIb%XKEwZ z8a*_|$I%PcM<^K^=pBviod1>CyRi#yqAvl76{{9uF^4E^$5;+))-rTt)kWds)lcs1 z_yA2~h`<(h$}>j%u{>*k~ZYb{?H|k2(CS=O!oDUZZBlp_Jez z5f`t%Rknq=$_sP5x5gF#1n>=p=iO&dy|dH}I9z^*>vQZ+QD+OfMD-G<(EIi6dFtxfJj9#x^|o(~1Rjg7ifL8lE=58B90y7=v&IVcR4yCY)rAXNb0^fy67SSmgP6Lfr=AIL-9vflEWG0K1 z+6>-ll3W1$EbC|;V~S2K36NWuT?{$_tym>Z-6av$k0X?)Aj*W25G zMSvmF@+{8|&otN)O#Z@#>eF(~2oy*1MLPyY9;8S$>VjbI&&dVzJ`!@s-~xZ3;Eld) zqMl{MB3gfEmq4ou6~;k^<@%lF=?

OD6BFkSWdV4+FmmNZ^mcuWn#x$4jMr$mex% zh8G@1S5lSR1hEuiySyx=1W$B}5eAWP^}7HO^$#O5F!FckrzeL?)084wjL8$~)S326 z-Z|;o)$rCGOYFW0-x~C50JwJ`#qhiZt&vnj=1YD|Jh7GG-}s{Jx3wEA8~;A>8F7KR zPCKA8AnUdQBpkAC>s)3?-K#IZ)(U}NNm&@0{iKvQyP2^llVGk2Iyh>3Z#3|ljAsp{ zRvb0;|3}n21qaf7@!GL%+qP|66Wf_goQXP^m=oK!ZA@(2b~3TPIq&~Fb#A(9^~LX| zt9I>P``PR1tf>_=nDF$r1Mtc8%ROqS)2hieg(lNR6SC4?5UiH?3tTKOB(xZ$u^>Oe z?k~rH0q3~T zR-*)eudDz^=1$!f=p)$Q-DE%3@;vCArh1v&%a)v5YEjJr(87?W6trS zLsnnWearfL@d`2}SZqj-S84JSWO4g~p4gD>j3L0OUdi?JL~5d`)#5NkSJ^Vr5}I%5 z(zISVP?EClpvsuRus;6E@AiBfV-RYcXd&ble_&pdZl8xPS$Mc0Uzz-#Cx~2h-^DMK zPGO89(X`zr)<+jGHQdrfNWydK@DT_For$xh8!}0E!GtWl^iwksC#BR%wRc@lBa6ku zPX;*J`aT91%|Q~TUi`6yr7poTpQRHzSJ6mMgm67YQRcZuf+FU4153VCPKR|?m%ih0G}bYv?cg*(Q}SQxUu(Xt(MzNn zWn%81-AJ6&L0;)n*-TkVy}_JeHj~8CXYX%oc=@5E$^3?AJL^90nzqrH(GNJW{9*QSQJKp>V7@t__cf{hZI0{!v{_U-6yVMY*s#8_zh8s;SIT z3w%g@Tq2f)$i$A|+nRbDto7;C?hY`c_G`5H=;_>leU7lj^+*E-nVNLPr)SF^$zykx zZ#yJ3U`L|h&j#N-)CDf_Q)bL|+B4Z^<li}hQfqAiUpRF-toSg$J7h_n zbE+C*4n&yUTTl;Y7T5>PBnY_Q?X1iU=G=VXcE)7xzxd57>S=Fa@X+%>d!~Q~nh{ZE z_RVhzwf9TEw^B4w<4W1jgkxFJ2f}p;A1lwcR zUAeEdTbc2zQD7{(s_%Pl)`xno>;K^Bj`S5&uT*w@K*7a{xcX(~J?rjANC(ykExd6^ zc=aT`6-sG8E=)t;`3(OmIh+`4$hRsRhlw;W(YZJ>GMiZQ=gm~`qsjlpCv;|JH!1r)Di;U%8h z0HYyS_zZoct~qn;9?;Lx=w8qPZm&xcFCkj)t2*?AD>Rm;nXuJPUOr7a+rCtmX2)` z$7J$Hx5}x(9>uh+-Xdu(XDl^ME;_fk(}=YX?Zag~6lE*@hLZ7Eu_U#0Za?CyXX|HU zkEfjso0Ea+lwR0D>k!S%!-DLLJ8S+#<)#%gJz{srbv8gMF-Hb&pvuiOPug9t$2pS) zawXVKby+Gk75BGTf1*1%o}yC>@H8Ws_n!+=ukpP z$^DMj6k8iD{1vCg9SOz+H35Im`sC}Yl# z0#z6(>`|C=yaZujugW08R@+E5->CQMSf34EjAaU>Z|>pbLmXYnJN_7bsgHq?hw_0$ zN@Y~eRHsMC+pvrZ4BQqb&v1GBu8mE6eb=iqsEtp$r3zbo)t?E5S@#}uUv#Eb+A$b~ z^>7m3vN=9NF9jZ-znh0k_m%;b8^^v4yq4um#h8zJ2-XtFP$#Hl3BWhq>7(A+fm_dr zuCOSfs+Zd@cvaO|{(v9`Zgf)I=VR zv{DV&t_&hZ&jK#okDO^o+YjfOc8ESZ7seLt+AQu<9=7gunMkE3me=3yGGJ)V3TnfeygmLivsopgwV zHtYfS4%41Zjqzq*a|j}9%ArF~DKzWJA~QZ4f3zV^7mi<83q2cWV%F((j@u~!GihTs zLf(T0%w#}F2X~DAkxqcM5dE+3RP4!NN0-eMfme`D)uZukLR4m}Pw0gEHE&MELJ32Q z*CytM7~7aI24|8%=sfT+8KY1lVFwOyf42ZJbHb)CwTR@TJ*^6ty(-|Payjk zPh*IwCS!jrUFm}OT~EInKHs6lK`!m;EA8e7jY@T;zKMIeRxaOEkF9Rw+1}ur&o*7- z9cLvEBN$J5Qr#@}X)m7}M&tK2r|1ke7iM%Y^%_O&mgvh@Vx&J&`YEbupy;41|NM~X z;2bP0DYa&x^njwvItfbGoA!@iX<_v+aXeJne`m_PYdPm-*?Co9mj~+)MAd{peMxf_ zkh-l+p1KaZ3cC(%D~4P{6<2E3QEqpoFC;Q~BZWjggbxq+{+_-7`H#6Q9epy6M9?C)~D zHB8z_;Z8hhyY@bGL=zluv99c{xXGZf4$gOkN(oo#z6%DnxnkloUsUfxznLtBR&SiD zPRZdhEg&^c@i0kBb%yssRue#oJBqS4dEx@)LylR8G+uc_m}s3?gF2YHeg`9~n~vxoXO|qCMeNp$)@o3@6gnz@%ocuv3NRaEV;jE>^63-q!hKQco4R!K zA{&d?QEDOT&9}(+D1`U$MM=e&ZEwD#`)g{{*0ZhRRTaFKx#vjBVxcn!Q)_I zI-oWo_#}r8qteqIg3NkS;;p=|;W6iQ>odhC;IaZvfxhvAprUNdVSEDskRxS@3G;SOB4P-Hn zwzGZ=4^+c1kO#Ph&TUN`%@fU)!xD2F{dd$uk%kJ*NeRM&Qi%cIH1k$}Q6E-WuXzTy zk42Y<#$pS77O9bd3gQgKyqe4M-i9z5gz3XvXh=&jLyNi%FN{y4-oa^{sNzEXhIg#tmO zcm(&q15LrA6Q;D<%r_RtDg&!E4YwCF3c1p5!v&eMffOMJ2~39Z&rO2+BU%e%M+Rbn z=nqA%Is$WN84U))cBli9nCAjQ_zi>uF1PoNzwZoAQH-)xPOvnrg2P8L4=X|<-fjW2 z5Hc54+5n)J)Kbo_Tw(@%dz{W|9KSbvR6In+3U5B#8EtKJ8dUYHZhdyP`C_v09@>6v zzD}Pz8o|Qt+BSWf+`mqBMJ)N`sr(b2w&_e_!;Skm^uDeM{@hz)-gs4BPtj}fJNvAC zSsp&`;9keCG5k8|ezgt$iai#eaU87WfX-nVFa=}`jk(K5T)yoM{>c<@t6@VRZzbM% z9q#!=B=eArtYIn5h`ru2nB0>bwrT$bUho&wlB>_VxvT9QK`2O3GAVC}J&etE1=DyD z72Qv}_n^0MJ5Q4@_5-^WezF5BiY?hUL+lIn$tXiU)zi8adok+x8FzXr0oYIR9#!!+s=u}Z06WY?T6=^*L|M%qT$2G(J}Y( zw!Ls$wbBHgw(c*#Y@_Xb#+xz!WyW1_uc8cUX4iXhLyw>McN|ggXJW~T1TIC_9H z=L?k8KYusmy@_%2LE#a>dYd0@s9BFj&j@ogDZgmAIss=TszX|`^D zM%^4ZL6CQ;CiX3we!K~Ma~O0=#0N0KmM!7=b;v5>Aw=BXnzqWCg<^J}+FJes%PJgx zWPhlCKePX{z-k&_u_SF6BK8PX@JU486P7hd49(~ww|Fm8#+OZ;nl5#)CDud5y;Ij( zkUL_N_CrjyJme_p7#+5b^hpr*)n9`mBZUN6x-3;l8Z|t-HT4uWaupP5e+gK)$j8sF zD?TpSrL@l_SoJ@(;WECQfwJETrl!Gegk&h}&IvI#>%A28d@Q>jh5T*&#-`_K9v9^~ z=vf{@vdR{8c*J-U1?8Rm%0hj)LNlqvJ_frLrAoAOklL9kQc)vogUcLpp-y9k^HS-^ zKRd!)YruRvG9v#>MotV{JObFrW$aASI7E>vcHa4;uJ7vJgE@~18Dr<*(&-pn2x?ut z&PyU&5b=bjA!0J22qf?C)UB+xGaO4VmO#$kO9`Q_LPm9-{WiAcTTWOG0Ek$Se|DBi zBloF$uE0L`2i;#)dI~TL{R(EMvB7>g`PM1QNc)T+&6F(2%w!r;Fb#}n3Y_7+UPj#q!2x_$zCUtGo-b4@~~!Nt~y$axXoN=J(pr8%Ou44i+9w z=?z~ARd2v(OLD_9UjfA5!*i^T-rGQDSJ!9L;hTL;R_lZ9JY$m7#E1^|ty;fE*R}sO z2(whzSMLpovhG73!#sNBbkzZGLk1LEgC#d0!oxX|ws=3&N!<}RokD1pULI2LhPXAE z)iQDicZ0#7x;G2Wxy``1Z^F9!GK-c?yU`>trYFEKrA=2W13Y=Z0_ldL1FYWm691H? z+Uh4WDAsqwkKyF^c2l8!(GK5c3ne*HBWP7kpksue*ZVA)AwLZ zv(VFJBM8q7P}N4)GXEP87h3@Sy$nu4BAlqxE{8M%k?Kb|HTMz4!BU2lYhNvva5BG! zx51?ZX=^3Pgc4fDEh~(Go8wq0UFR6Nuu01Cl2@60!Sp1Az0BZ!6I}R_VNU+$bpL5m!j^gs+QWiI zs5|#exr?Z)dN{i{-w4V?Ve9Rb;Krq`Vc1h7K>Dpk$SYP{w}U~_%Y^!oX~iJvBu-ER zu1oYBh{uyb&^T5b4YG%(Rp{_0v3qKS^TFVb~kOT@77QbHtY62&34Yvw8JBdlh^vE4}7wQgQxJ){l+v3nXf)j zwe%Y0`)>q8H^20t7#~d}5VghlLPYkKDJuIFQs}9nbxSmQ2x9 z@yR&%z8HGpmYpiY+V1R!f5eI1HHos zW8?W>71#tSN(jpGUxC8S%l?0NUTh>hoGd9KQDB%5yjLd#a>L2ItKL}u;(#ZPPcTr;? z%j(2$Z&y{!}E?!bwPia!4Ej}ctM-HT{L zHAe==Ar(=9Svxjj;wj3SwJxMUIXOHWa=bp9iWT7}ckV0<#;@82To%s{|rVg8(+8zIY_r4CwiKHdg z+Ag+Z?a^;^ODnF5f>L}X)GIvc7C_#)Vw@KG^!+gz@?37X+wCi4i?+;H!seYCDL* zK=_?#qumKFxA7&1hIz*iaMo8E}~`PO&S)@MsN{ZuJI5UvLaAGHaf&FOxV@?yU zyXwiab+!PBl#elCy~#(xs^hnV4=hEotx|;hQI)Kfrg;UFSr(!tffoD#aM$icg9Fm) zMhGwLMukdQbd0sOav$yF87m&_AQY==up&4rhk;SAloTL%^7SmHszCh5pTL;6tzcs|Q;>F<2So;7q@WKD{Q{qS9%X#HycGe&j z;%o!5dGQ9F>4|O?u0#nVpzZVh-8|RCkSWGbFEl-|sB`cF$ECCECd z0`n}N!DN;;3q9bxnK|UtNbgawXxx*%dHExq+-+X3Yj+)Y3bkz@H{D#=MF+JqpVhEY z)28g&uQ6ORr_24#hFz%KZl&(_Z@kRRnZBGkIgU-kVy<`CyWv%FDMG7vs2P{Om}i$9 zb(ldvawyFhhf3Nra30h#_-KYmDmMSakHKnKsvj{mb4ft5uNu?N-UZ>?CO7B7>^JUB zw4Zj2genc&*sgvJG!hSe9?HJEL$M+*f^{B3aA8wxLBkyxOP1do$NqivR#hxAdP-{4 zNpnc7TlE3RqgOiVHEpr;W--Q=3f(c3vogP5fr(q_3r|q)01w}~VCly?${@DY_Jk|$ zDPn_$T1?~5b2{Low;6!9&Ty+9klPe%3DW_`OfSgN4SVX?RaqIz# zDx1w_BgUH(csYBcTGSS=x6}Jun5JQQe3KI8hx}71*_q?%2kgs%L&7ny-{-ke4<5t{ zJfOxHys2=>^K&O!T{cpiCqPM!Tb<^zTsNWIES=kcsDnz&mar4sHlkGC=*E6>Q$GH@ zT;3#6ZnP{3S2ytx@m!C&?8I|K-)S{9U6S{ED=4tfs}Ko2zXlbGPJ} z(i~6OrF38@6A+kWz~N!Pa;+S?r5U<>I~X#K26Z#SR+?%1QTp@*5t16PBUm7c?iDaUU#nhT~ zhTSOwXo+pgUR^DekiKgCuKk@8cT|v&>PnVPoVFpC`Glyp+MW7aw+grH!jTLQ=i0OS zCiw6Y3`lgWsFS&MB$2j`pi`0m^T9DJSNxAa7S8;BN7;~W<`HEjf|d`mVf&H9ENtZ> zZn$?g_GJw9Q1@c+?yl2}aW`z|0~Nm8Z-y0hz$L)@qW16V)D1}+mJ<2ZA5^)Imk_N5 zWK?WH+_8^yxEC9A5w%mYC=XfUJhTplmsQ9&aVqX`p6+$Uil0;)UH6WPO@KSq#V5L?!nB%3IZO}~F8nQm%h6XDIl9BQH zrT{hK64(OUc+5yscmLR;Ch3-JqnG82pWT3f#v~zRu1t%!kl+)i#xVxYe(rxDz3>*u zYZfmZVzQJZzlzjtE9X36Px={GM8;fPwbuA#svHu5@X(?o%-tmO>GKg_xAUM5U?#6t zk+DcyJe4Tvb|*{adx+_XtQmr=r<8#HcFAgVYKTmfs21N{Ocu8Gf@_o|it6Ug@&?1> zv%!(v$QY=O8eP(2ew`_A^7NWZ9Z$N&oMue1vII5XO;~&DuA*UngCcFKn)=M>)s8zlA5;#w(M9$n~yI>nMKt*!Dl{d`kMS!~5 ztTVa6Z+g;)yk*|27jTI+*cXDWzzJ7B>JO~d?nzzQ54;xjz>Pos zl)B0x_{W@jKE)k7iJ1_ID@VwIpPYtjyKpDCzPelaYg+xq=r~!u$q-%uNWY=FyvjYS zXSYLVOxT)&N(^}qLVNyfTS5$cflGvS+*9*>=eFbF)>SC!t{f~)(&`@+ZzQmpyn10O zxU+w<=&oRWb4rXvkW4)XiQwfgYge};M=J9Th;)l~=|*4MOg8ysoE_uFd6js+(yxXq zy&%Uovu23y=FFk_TO*|f>`5-evxZA-ifb&=@VV8YK2lnJ=ZmW1Am0u zs!JIc>h@+Mp;prOc~X4eaZa%l7=1WoHwbY06wDw4n5i=_uLnKXvPiYO1m`(?@|pBPq{CyTwR zSYRa~lgi4NH_>@Wo-VH)B20@XZc%tbFd_9A$Z$E$tCA&lq`Ob+-`Ncp+XLRo6i;s! z4qnn1Ps&&SD0hmwb!x%*y}YN6Z7LMmUQVu>xPt|Qf}t*S`|8EuF5|V4U4E;dY?UIy z(~=;}o}CT_2#j6zbVBj2DSpi>3=2bl54Y-N54gYV zD><+>~cKq*&(LdRQ_v=}DtlA}jhxdmLr2 z>HS|CFnd+IzV!QFrD8IP3N|DX0&nLSs&gi~ZsM;Dgf=8!Tq;e8$?mxpGPxv(4yg@m zctMlX3dfM7&c%6G6tl=)NUN#tl0?MTQeppI7!+u1agGL}8L1i-Skthnza?;huHq+1mt$ilT#-X&8d$qMkI> z7=F}-c+L2WQ$U7ctKe)1vM%r8@sVkRPq^r0&0&YzDyROuON!x0I(wIkVz3WxB5{Cf zT^Zuc&T4bc#z2wPR4H)%G$Te@7W6n(NVqhHrATmqb`KYQdtXM3G2OIp zdL3BQ!>y-S=_;wdRZeG55$whN}`b`4ZX;cIX@daf@}J^AMq z=#!THaIbfY8u2dtpzZ|Z5*f(}g|2T`B#+^$UDaWKJ4@IJr$hgT!HtV>QsG;WlcF|vmdy;WRX0;56I109S;yj&Q;bN=@L|^rg=?G?E|Rry`jNHZZimABq0q-Wj&GSu5j7F(c{LPm81H)V`W2 z-M5i{@H;!x?F&YRKlW-OT<C?P9h{Y|P58Qk2`bn77>~@{gIpVWu+i4lNzGs2BwSP~axXjn5 zrM!t;jM4+AIj}p!>+_DSzS9`Bmf1`+ z2diGAol16Gl|*cDbJc~#KkEaIt9ISM?r8PG|FSYxp3!xOF4UiM_xVTH})8UM22QdNuX zpIMy7&2Tuieqpxa8-wg06e|>Q*$VREZ>T;4N)Y!NTBw;z2i85hfPrZfJPSo{7kVH! z@pCOln7yL*$WUGHC&X?ZEz>(goNLMFii=yxJ%@^2-vqO?mGd{9?^K_%X;XC%MTNLc z3xaIQe}4U#i_$x?Q)}tRz)gV3m!BBje|ga6EIhbNje;)#d2h+spnd$Fou=d1iJ%yd z%N^U+?VCiQ`$a_s17H>gB|rQwi`g+mc2{X%y%uPeea&N&>~r|(8x5sEG{kF zO($5LaK3=x08UlL7MXUl3^Ve5&ot+b!4_8`1XhI|SxGBu9Jqi2oVE?OkRPP1b!(;) z*%7@zQ0Ddaki#Ys#jcScBjO1$Q>zh^7H>7;F*!B0x?V(?iPWw3h~gid+bgH7?&pn_ z*|MDsGviN?ITPp8VZS4nov_&Xn0pa)0E#~1RtTT_3zSWF(W}sHr~HF2i>-XwIa)l# zs`>I_%i17=fRPmZj?rU7m2{;yVr}STY5#*Ix=p^m?DM{%KouVuOXFEC!MMnYoUrLj z6flWezdFyPBPk?*Cq?IfJEi(8p4DrWYoj#!O~$8c7Qz*z!3e@OL$C0zX;%XgKvb)GgLbg}%z zEI7N>zBY1|J0(A`etAXwXptbV2I(Y{*7@I%=6EjJpPU69HzN3;evLaq;$#YVwDO6g zV)FNtfCIs#Nj6+Jb6)|Op?6_NKx7VVjQ7s6oiD-h+8qLptZ&CPyv_IZiTO84V?594fba)LnH#^c|ggyBv5bVnC0N~001(|b~0$A6Kt zv*l>l{t;nGl<-FG&h%SY)$kAgEt}>Jy0a(b!0JnyRYfR#Tl1U9NNV4!_Ks5!yo=!_-SAtdQ62<>$Te#~ZmvAKQh>-ksDA2Lrf zt_FvkS_QmKstf5a%NAaD!S;ONc%1-h(U`(|_ryuk7#?rzDGPv*piC;r+oyKoud zerD*$!rs*fU-h?NrLE&1N$2KR%_0I1g`ql~Nn7NfC0C1ATW^wQZFA+i=I8jo8(Oxm z`BOH&*J0onzzsVe8W4GK?Xfp>;wwA501^oi1!YG*D#fm5v+_^#4Se1@ZRQ;3!PzGf zpHivXEtfit5h4f)OZ7kguAnqbmfXryx7fi3=t?IEDbZ3iOg!Y0TZag*OHTR%ayHi7 zo`NAF*LUO9o13EVXy$4mM)z`MAvi^9=}Sp-in-d8?91Msn2|wJ#5zWJK2|u|z?={x zkF0(1&D0Iq^UA;OQuSJArf+1{Gpox=h+O@}8_bs!Eq^9|mqhmVDih9eMRe7p=zQx^ zC@IPXj%AZ8db+f-7Tu2lAJdqb>@D?pBI6(lIAnR^b?=&}zKPL8W&%&H?K4T8>HhLA zVdt=)gf)?AJ!)i^8cU`#0|kEgz-j8_DaEzLrH*|%?}9}W`m-mulFEZ~M&D6PV67FU z=vTU7^RWb{LBR~7GMU)6V@^~?N)ICJ1mmWQG4FV{Nz&yxdpgHZ1UPD?{*N>?MQD7w zi&dw`knuOlk&QdngiC>lwYZODovu^zueK+v$k8+I_icdIwL<p7|0=xCp%Q2zaGV+`MGA zFE%VzaZ>Grp!YRg$o~V$G zv|Zgo+6BHU3S-GB&7@AiMstrN!AMPZ%3F5N+ZZps3TRxPoqOiPv}9jDY5A$#kCLm% z1w+2YCJn8ws@^?!5vCd3cF}5f8AYRSYgYUcEoG z_u3VeaoMAB9PT9nUaje+{vIH|wnV{%H^b6D*hROIu#Gg)`CfFR-$6&a4H<6N*L8@IZVsJjTjfg!{pv7I`8@Kt@&~#D#Wt!v z-)KJ6<@6phcelL!;7f0YoSpmk_7Y?EU1;<`zQcxB$g~iPm%qdMHlr8NCSJVnKAD=E z*W|#ur40uGX?!!sElpJMVqqWmf){b?i8hDy9er&v^n1TnFqWT}*&=tPF`AY!9dG;d z;!=Ehc2=6P5&s$FB?m8$w_0nqN5U1md?g$6y}MX~D}K;n$rEM#5IX&eief~wnuIyX zW2118y=ldp+L=bN5~(rFH|OaLlKe3$FD{}@ow%b6Y_q_8lgB0*lbBb%Pu1^-DUo4x zpxuzJ`1WBFmuvz(NsVQ+y`zg`q|rGjy~xXa&0Fq1var?lOTRBI2=uoy)HqZg{xsjh zH~JN}@!R8A7yDm9&%G;l$y~vDLV^*lT~lg=28*+N(=fUE0ePXY_EWSqkvOG%+fNd) zvFwy9fEPUH^Pa_Tu44)!1l4X=-M=5CVV8eHJUtWA)nI+g8-&#Q61KwLRL~6?Ja_p; z^cRVx|Xp?nj` zXaoX%2S~}rXeQPzzIW{3;2Y#LI1rsda|+(;0vwt>7RPDHFdyo)&GBtJ)?YZ42J~@7*u#}EnIrSD@Rs!4!7hRdbwXp;{3eLwl2y(PLUS#8IV5rn}{ZZrmLkuR z42X+k1GLMaJv^ju$6 z7iD)O$P#o~HeCBx1Qeu51Z@phQezu#MYxXX;A|O~*~c>m+aeC2-&`JQjywEr0B1q1 z&8qTBlRWROEuEFGQN^WQMP>ZA%FJte!s8|pnqH+yKKmM^+&!6fqwB#UG4|jM;xr|W zdYZr9jcC54pCOI&R65MRJ5{B6$!VXR`H?7k=Wac3Pm{B@Ykf@j7c@{p+dVO@;TNj} zuIOlnvmNLt!aI(r{*Y#7niH5)016LKbg$+zKEQ4F_Pj*VFXBpJJ@B9zPk5s}=)0F7 zT$=u*1z5@mssf2ctE)L;pcnKtiE`B$n^PnCeMh3I9mQgCcIv9r_elpo#kF_QFt*nH zVMH9`hH}#VX~ro_y)vZY&ltAGX0$*Tm5&iOn zFcHx8m{oQ`17uL6s~6bo=Ra5O zy!vk^yocZm1SEFQz`ninLku!73|VTk>G~_fE0wK{y-gFyM!oM2hG^9Mj`EBbH2Szz zbmGM!U84`eKVqFC%uge+pfzx{%szDGdihkA*beO7~0;h|XLVBbbml7g#St4@|(VNPsXdz$yvYt9&c<9Y|mXS32jCPz&c4=$em~g0Nc)23xEB-Wk>WH>Q2ajYNTj3w~W?gpO^(d=WB>-HUB=EL~r#V@k|o?C?s+XQ0r zYYEo+Q_P329qJsp%vqYo-n{!4yzW|=qsDkJBJ*ZiVmEDI^GEYA54jDAz&q%lzX$u) zbaH&V8~ct+1ZXF)hvYnrkjl)_(ur%GXkbIsO#4m@<6PYz+HYD5@WzeU6-ZhJ*UvPy zO2m5@(rM<=uqD27d#yW`ThgOo8WGIcNUPksaDCh89$`yZQyvLPs4vknR{nczM=&Gc z^waD9QyVJ8$M2>4Jj6h;(lUMs~;DU-!(>Eusw z#^)p4j7KJtypkbv7(XtfQZkzNt0HC~nq5DNe@&~CB|K`oKE+l*Q@M3Djnrmc z1zR!~DS@Bxru~q@EusQ;CqIuxIliL)u@r6y7(>0Z>yG0Qe;VkSF6GBzI}E8TrR?zG zm=!V*Fx<9L+%V)b+oRKX2eN z4~6w-7w!7ZLT5~D4Y|q1yAQu_m-BNGRP%m^W)9V80A_nYpj0$EV=67{OQ*0Ie)@0Q zkxx_4VMpy<)a$q zl~@}G`=_7N(Rs|%N-tbq#PAUl&Oxcc?~Uv1WE9EZWD)sbEd!xv0%^V!8{1))&*3HN zak77Be$Ri}0qQ=gu`Eew2TjgEs%EiQ4C8IWyt>VOAG`50- z3M!Dr(Ba)mUetV2Wsu^E=zgjRZjy{9`RI17l56;JF4@fy$YAReF3j(4&yT`9^H<|@!z+gn+aSz4VdjLQu7 zLdG@Jvbs8{)i?ZZ*iX#UWMg#!eNSrWkBf)nPsicWTUm|3;mP;`x}XpRN=Eh1bU+0{ zPE8F^BhZShOl=L!_D^i!isY3SO35lJ-si*v3LqgwUwJR*r{*S>@FZ_o*<*AkIJexEujkRSKdO)}VxO;od*}Yx`dZ z4@Oms3ZGe5U!V$et2>T*5Dt@{Y6Bv}H;#G138}#y;dOz*4ZnGN7kXbEf$2|5?Iunx zr^e=5&X@7ao;A2!3v=UVW;07U2K3FHb&VKfap>o{poqSb`;B0U=aGsi2DLF zeV=Zq3}`3_<`+44+(d{1zREuVfv=x)FU&0Y%~I}vzM^948v?3sw{6~X0)Yi7rkEH& z_H!BRS?>YJ{Nj9~qPn7zVyPH+m)ht{wn;mka!YL+NFdYw=!(i6Abl)~rjvyHUM6O;3%YDQD-5Ko3iWgnE7NdqetB=CYS7+lI*En=l;~pDFm4kQgtzyLVfEHLExF-wa;d$xud4z|)L zbUp7#rkvJZSz^L_Zk?-S)8;W_G>%QrPac{Ni_~f9jsw(SWDuAvZA1Rx%fFlNh{$O# zJmU`W4@{}W4k*)&g`!ZZ91CmxZlqTsC|Eze5C<7rN7MkS;<;3Ee5*zHC+tAw@CQ5( zuwHh02S`YAO>pJhh&~`*iZ!Ot#(Rc?z~k#zf01giT72Ua?u0`c%~>LN!*5$se6`I< zd_m+#_+~m^93Drn%a-zY>rLoeOOy1uhetY%9mzXb&*y{_kl;GXlqkJl8zS*UsglOQ zVN|J~%`6YpAH7ib=bK!rsG~<~ErdTRQGVA7Rgtq)rdT&btRjjt2WU>?|>`Y`x z)uOqjy{UVu#f^z8)rb5f8j5AIMXFhCkZb}%%k)Cr>z#fgJ-NCE*`qN&7AP_NPDGES<80X2Y`GU38UOpwr)89wYF?{bX_T`J(@>PT50*}e*{cDE|vEs4Z(@~`^o(juznzG7ryC}=Yj&-Usv6@Mhk)4gZ;AW%a zQ{XM`&3{e&JhK^p3YZeN*NGXOaed>=r$HtI6l5&I5|3sU8)qeodA5#~n$j^mMQl)J4WkF+APd4yOtr8%TJFw} zE`!K;=Jf)H^xMXL4`~71>|LfN@pHXbNywHbS$~)d$;e=i!sAcma61o{-zld=Ac4S~ ze$0G0c9wFj)ypqExG=R4(Y#LoDaw~s$ zn(f5#4pjXBNTh>KHIe9DSLV}izD-6w8xukC$U$dCU!pbP=$rLrm=9>8HGqxvFzLJ8 zJbxlK-IK(l6RxT`y5stjZYMm%l~qufCX2a*{z}?|pW=9}xMz-+E@0rE>f4YIm1qzH zW10AbuP++kaZc!Y88p`bJy@6MoffR*n49R@#vt;(?kEMVe?RoSsmXT~jF>|T%+}C_ zFLfFZ7M<2~S7LW4oGT|^LuJ@_WBS=Yb$|M&{{`u4&zf*!y7_#wstMKNuB;%KX)Y?Y zXz(!)(cOe7ML>t^*vOsc`MdYTB{-jc+8%$H@=7=D2uM8UeC|zQN>Ndzbi?$(%5W%t z#HyTypU3j3-m0!-T+xFR0{%qWOjte_SE>WJBzSR@`}l0)bjR-P5CtXFIHO*2et#K} z=pW~IR;XD*&fEr;I2PSvn_QAt@%DErNwdZCPUv~oCid1%U?hY3YjqumUw8fT85tPt zGAx$SAm@swQ0#VID(UpupMmgc>89gzdVP8KmoA^e`t&LYF+Oz9U2KI72h|GXztH_^n&(D{Y+}5$8PfE8KT?cr$)bMeTJ9KH~eJuZqp{ zuUfj^ABkA2UTX4Hwe#z`*d-O-5nv=FkO438Px>lyFM8tb=@e#gGiPwS#edRDd@w-Q zG~P-2O3K)`C?ROPX@pG|tvs4*D;8dlWUhiVP~8<`Bzm?kj0WDiDOB3{K|n6r)WkHs zu5#E2H>;-QN^{1oc=dlpDLA0jdVpd)Y<@VR=oNeW4vyN`EqImVR^`o{y%E(|bADoY zLUnN-kKJM1gOm|R2`v1`!hiZUK)B~onj(?~Pop|^r<>@R5021HhFv&Lg;iQ+y~#rK z(U)lx3oOK}|5IY}5`xS#zt`TqihRy5^ge#I*io6aM)R1+^$GH+O3Ej+QYBG5c>Nn? z$-oqz3D2X6Hrwa%l_UR~BES>m8R~GmR;iD%S*T+8#Q=)MI*~*gg@1tfdGEs%RoH-h z`djhdGWfXFdx2D0fZAZ*UZEoGrf;|%h*6jEByQ;Piy+K6GtL~ zaw~~gpa`a3`f$Xgs2FDa#;H+_u<3vxu!Ji^wv#AXXem$LJ&!YEjx}yW4psATjS)a< zoj^{i`$0mSY88Cxl7GiwG>LHNo%&k}vD`8H+zl(^!EQ}nZ0E~r<&g~`J#Iv=5^LfG zk|GMej5ApENaLZh7WZHm7t|w;UB;D=mcP1(a+sLUEUtGfK;8?g(~Xmq*g}P0SgOrF zZ+k?(?xeTO?qCLdLCSM0`%H+0e|gpph16(Ozgzd>EQ#J+5`RVbE#ga^%B-%A8E*ss z3;6~g9Xjj1X@CqNkWloqFPp=Fx${6$R&!L-C~1%5ZO|PPrj<82h&rJ^ZJTby-Zs~5 zG*4P7Yv{Anjb+~REswa}o6Y;0+}csg>1mHD{I@^~)_bb=?rI_YmAQ(f`ZM_6Z)*nq zO^~G*PODD2kEMXU!RiA z}Ygf*AmiU)~sFCwn#f*lszjGr%u7*)4S``@F*8&2F)PH3g+f*klNp_i&0DnSRMD!4_ zlzg?jSb)+M+-xotDkHB~kprcLg8G?9o#mIT2ZU{bQiEH!?B5dlKLsF&&_sVmhN)iq zSTxDlSDDJE1YS4XL6-UVd5$I+WBHj;C9(mLvBoX|QR)9Nqn5k+`K-5~gPzafnVEqf z@&^`~lYfcREftQHAdXIGah5fT1GXk!6N@XZXQbH9bY?OZUn3ldld~zhY#WoE0eFS- zY#>}=&KrAMO@M;LDfO3)&rw8r?dKo_m$>mrUI~@$r{qX=tj8CIbr2u+qR#0GZ=JDI z-6m&tPY9k~sbB%fw}Jh} zsZY*#J&8Bw-r_1=8i3+Z{LUJr2N^U!GwBM~o7H4L-=bPbT+k99A1K~aZ`VE(v@epq)~S0qcqZ+J95y3$j2kXgsj&GQV_~@RSrfqt zYkT2ofQu;-=Z~bM+!8!el}t3NaKJJb?0;tO>`B-!a{=eEKKOzR{`xLFc-{2nrn<-h zH{(26Z?I5Qs*f>|`L%8B5&CP9D1YVU1|M5{iBCdv(t0(VH{=c3S}Jm$wRD2S;*W?c z*FAl%O$7EP8gtb=LxA~hFg_f*Vp=2RyBKdIYM1CcmNuF-H>pHnfIKUn_4ty(+M{g< z$ITN#NBpfhO@!LB6&o2Ibj3|B9jiZ~T8qi$56u&lA=@lqn>v8v6077)j(<1bJp>Kw zSp=Y08gwZ?^m|5G`HsSerd>Aq;hv6@S~*Vd8Y`G0^g(AXUUC#?zcy==8=B(oNSRvw)BsqdmDr9jFa75Ra z7NQmoqOEy#6c`&Op6I=q58=DSkb3AGE#ZhdViuurfJE*k{$Dp z<`g({mbRJEGKr{(T8;>`Cn2?`X9nxfOLcKFe_K*v8l?`6WHX*Nns6jo*pPM3ykzXM zbxta8?l<$8lK!5VC z&`)>p2Xy{dUp+_eXp8~y$;P6SIu0f# za|mAyBu_sxuTRo)K|U80YS;#OA60yRf8GWRAiPG_Vo{6YY@;~QWVF>^V6_wEaSiyK zzN8MY4G#hn5Pz>p5S3M^mA&T2uB;Rhr*LqAiw$HnW@G4_@(_4WQ?a1j^@nwx%4BBj?!$hvNe-Tda#P#1)FiqZoosV)I_CJ_L}rlJ9@<_5317Rga~2 zUW11{I!a|3c-p}5_CVA{Z&uq0W}{=0rc2a7B#C`=V%Y2koX)QJuIY3;gW9uJtKPYC zMy>~hf`8>fAgmsN+Y(a1UwGn;5_2-%$1S#nc2;6ATrxI9#Tsh(%A$*acol<8@92Hi z(R&Qx+oewjFm=vwmu32I772nQB@aGN#+rG222IBQZw! zAm|nXE-v-xBaGx&-J^w$rtHLQIjVe%d>h+sMZ1u8?-l&v{)DbZKxFb%#`Af(-?Ee> z{4x&1x61AwWX5(9eMHW^qg_YJRTl6~grE+pG1Gt4yf1E~pIZOSS}Z7=rrWJVNvBUC zD1YNOeg@-fc(A=>tc*GZR3BqEc0WbV=y^DLeK~m<+F|$VP+ozJMdQnV@IZ=-6;?0| z{)Lsi0OLd}^CSUM_JjN~qV<$~E39m>^1ceUzXohd_16?1kPtL}`J}q$&3Z~`F52qO z(Q$6*c1OTdT(q<9j^M2orlH!L7G$_qownwbDNC* zMA_ILmC!W{lIv0gk^MB7?M`{8{CqD!MJ-LP^QY*Xi^6O+cZO`-SunSUQTF(#lz~Z; zmHf9gd)U1ZpB?9++PzI>u-(pU&(v2921J~6>zZq!IQnQ23fyU-`O~(vaqJ!{%YV~L zeiSv*OsN;x`;J-`ro)!r`vGU$Bn6r{bbQU1v^TSV{b zY{~&NadvC-1leoX1F!VKK9_OX`hROI7uQ60D6{PTqO$Sk1Qm9?oP*!pKEHXuUTE1k zYA;aRLtM)5sE(}zTqUH9PpS!o+dLkZ1ywAimFzs7j`2u5n~x81D#&+68J<#(Oo>(9-n4mSMY-c)EX9sA zM9aRn)-J(Gqmv9dhT5tA9)CLz^+a6v=9=#a z`KzwQk@`x&>glYS$fzLH<+isd8z4v_tVihNxJ3B7r1aiRPC)c7NCiI>0csHx<=* zQY#1u7cxQ*2tl_UY6pGM__$!UUht@b_$fSnz=s&@L5AI=iMI)K$>YFfry*1(kDjiY z)lQCOBgEJ!r6PEnVumAKkYndzr*wi{M*k7($|^CdYy8(|kZ+M+;Y3g7A+GeVLh37f z2n>GmzSYuJQTD8IGk-Qr4=VR0oKILJ8Y`a5U{9mbW)CqNO(wGvE|d=CY2s?*AHcrV zmrTqG12)B3y2zr7R#TlitDmJ;wc}IN7JI9F1HR+lhP%MkZRJGF$A&Tg?i1=; z=y#T0W%CQpi@Z5hIbQSz4$<@sD$?W60{zrVAV?O{f-4KAsyVa_AdMd_7%GJAMP zWJeaBHJfkU%zw#4!Rw|pxsh!3Fy=%PcG#6pUX5C7O8q6a2`5=p^3?&~IptUd(Tp1R z7KLh^tV*^!Pv`hneU%}0L=kdOPTbn6UjoYO@t59-#-vQAH``U1yW3=sF>WZ7{e?yG zqAFwx2!b&7?tT5k)qJ=mG&D>7bVa11vz;TM6Tu)fR)4aPUeBROPeneFnr(^NqGB^0 z*Y~?*YaqIJD|kPG$yIbWvRqhiTR>(ab*%QQu{$~XKKbQg9G5|B@LqI8C33A^-Bw<< zC<`AZ>@Rqu^AJaaZmvpD8seMAvIQ$j0!j(6k-64gi9ac*zd3SoLE=})k2>@3USqqg z8TIc(gnuhO7rw_XEse8hpATnIi%2Gr3!6uDv`D7fT5Y1l6V_r|l&Sg?tXXrmYVenc z-nt=2p9yqOo%txApjq8~0G>25rY-gM`h;{Px~(O#Jkr1;zc&R`)18KG65JD#!4Ddx z!lB@3ZDH04Jwm|`mTI+a;?#dI#@(eqrS#{AD}QZ=b1Eth&)&VG4r`93mN#}|?l2@* z;}15!yyP07S5@Q{YV5z|)4#n_+w8qUTwzW2aO#{`s=O0QbLtVD4*eXO?To6C*q~Z+ zJy$^X%VI|gXr{P2Fco=Vsx*1k?&<-*ZjfkutogG(a+%^zzYY`$?JU~)F0QilNCWMP zM1Q?^)1tAag)U0rQI%cnDGFpYzMJM9-HCmAM(E$(VXk^^lG7jf zA>$qG}tRO z?)Ae&RVc499^>9vrc}8jR}?-H*s7!3mxEd6%={cnsOW~=H;I9O0%zlXN`8m~!i0q%H{%H8E5s7nj`yYDqI;`Me?Kce1{6W-Ie!(= zJxUSlmWN7t{Ak$`-O0|6j~wiaOs69o=zp=oMf;1~Cg!ePS~o6_ za%*FbP!lwIMp`a4y_$D?%ZwKKC-#mrCzJ|s%UCh`tXtwhIzlDKB!)eA{Xj?j%cF!| zl%aK^-`Oz>jS6B;0Cd=)R+`4YWGh9d54ycONePKRRnp=%UQF#21@`{%oZko1pXV@o790 zJjoGYAiCkFekZRIbT3`pLVrxH{1Se#eHgkU zqkgfOY|Ntp)!3Hjk>XB$GhU52V7`1of~V|UM@a#^vOfnJL8)gs(MR@A4g=+C7eL}I z^D{*k*YLdpuf|xLaBCIIet2W}Z$5FYgaU#jU7WtYiqaOpNsH%z3g%iMKCacvMyNqB zRrbCaCLx2|atF}OY=2KX%?9ByslN3xx44401#)M@f;H5tph_QgwK!XRRNh{)5bD5(Nv0{@}mXCYsyFBPEQNX*1;Ap*M`qnyap z2s7QNguxwqFo16avg2u=IHDC;jhNA>VkMu8cqevf*-x5fJAXy5N!j>FOXF>hYEt$P zRVDsQf;B9oD{FOInQ@9DQzyKm=OCh|cprv0wzsRarytjhJfp(cj4Rwt8T>|cTQVsl z3z(OwrGkZceFM3O=L!ck{A@5Bbayy0s{l?rKc&_sJ_Vx6H7Vy@ObrFd zO1m-I(@3_uEWgZW$$&eEAh*;u8g+?1s%1p84W5-cO-1cIl|Cn!9ic4;^!is2;H`^^ zii+Qmt-U-=)(;y!A2M#qZjR|AQRRJgxJ+9)PZ-Srq61?P5KuAlZsoU&4Nj8-k+;Xl@56u@@FnHHAT6hAML2W+(L_kD_jwFak(0HuEgc2 zu75T4zBNV55Le;T!D-L^vm)z~k?g;a9)yT>)fWxQ!hjb=LDsgsW-&`$2 z973>4dM`{E&FY~R9oG(}d^)U|1hXQ6d*e2uf4xK!oy{>h8Z@ky#SeCs!OP`7cEp^C zty9H1B?aVUu{;r5Y+~^=NNAt$q7jB=R)6HQIlrlV%McRb%(ho0+$m($H)bb+$u`NhRO&AnlBREH`DHQgt;o|4Sk zgYX;%Tu{d+FOTsV6p0-RnW*fL@bPLinenv+J6ONZlfI&I5ZmJvf6xXxZLPZeR)5xi zc;!PKBIsXI#Po21aB;UxZ_8a?!fx~n32NqiE}|{KUP01HnO?V^b&o16W)!9IT9czZ ztCBeX=nyxS$xNiN|6SmVZn5|lff}+D6rW9H*C(m(;o=+dARMQ%A@;=4lsAQ3 z9h2~EXd`+)&D4;hbZo=`<$5Uf7Bf<`Qe|Iz5%mQ5!-0X{EHXqbKUSv7?te1l%_(wA z{!{Y7Xa}dEZ-!KmYs2qq7}taJQ^GEe1ZIerxiCNa;x>XXxSe152s4cdJa5tj0(8)* zhD2v`Gjg2%A+~Ac@`6+ls3}I1<|K8S_=@nhN|SARmp9$$DfzfzLBvjHb8m{>CPI)2 z^NeY=v!gczwnrv1?rGrqn5SQQ}0}^|2M4 zp%GUI{R+=Y*sM`z7G{ikWcA5X=FtACwuM*k=Oo);XU+PbDPmh2x_>w+7?Q!Rl>DIE zpVTw5sobEp#orvNG&IWf?@5j@oMgzc2v3z_SQA>r7lglvKgC2+d43EU6} zC0E6*th38>g26P9t=Y`HV-?de_P~kS*@+>}zl820EQLROZGM}AD3{V1iD&ohy!#85 zIYk69bz0hZW#WW$RDVM#HtNn?Ni!T|Q)rU&%`U(gmX~0A} zlMjA`ifW@O?OjQK(VT?-m&BRX!gLJ3Tby~x%#uo7yxfm2$!setI7?~5=B67lg#1yS zC3NKsbt!i3(SLRkXy<{)`>)%v=HV?>2mN`f1I7jw3~xKK!a=ACy}~3i7?uHmd2ETiP(xk2^jp27b z%5n6xYkjX>e0~0e27MpF0gzveDbcg=0}RF|)D&DrHeq(~t2NO`d%<%f0HTNS#YI?gcE+KNC3xCvYl8?8K5r6xd<${*&9ZYd0buKs-Zyd?xGPZx?=A_V_fKbO`%o=X$ zqqE=4?hm=AkzrnRSV0rs_?&ZdamBf`nuXPq!d_o_VECiu4;w4j;F0U}LwG*^>nSN7 zxXPO{)&7i?%wWPV?-REViCAbTRU4wmihls^fbW~AS@s%GL>EP16p_MR=hjD-)PNMT zF|(RE%fy0P551irp?CS>7v_YfW;x-?n*P?o+V=6(0kz6`9jllL_Y=bAwxtmeRC7fc zSuOAn<`?!~7oV|{2Q=`Px*i&T8n+OHZbN8{%7Gh~0UO&0L}h#!B~<_n1qwI?p7Z34L8N+Fh;Q(?ubI zJLYhM_86k5D0QM2R+!;arsB+fBgE_JP`E~OQz);1!oNrP1kJ@ff7C!9jy$+mPj9Xd zs;;F+C)9v$PE)?yQx&{%^#J;n7C{_sk z^m0dQ@`Y{!&t6A)phuZ9sdOyUrAe$rXokmB5t4kn&=+}CD!O({EN4z=om*HuHt#H1 zfIa>N3Jm4waOE@UwXB>oRD{5!e76`NS#d%2C7RVN4q_xek}vx{n7`N=lYg1^YdlZm zw8tf#)20=SWEg%^8v9EHE`|TeUx&YC(jPpFDyS~nyt%nOSKz~B-C$+?QR7cy7~FtM zi7c_NApDE%3O_GrXF~KC2*?apc~UbR;hL{ffMnOFuP+Sohz5Dn+v!o*Q>scS=&CCO z!^}6vdJO_GR9MZWnU+{g|9^t3V`T53S{Wl|!dsEpAd5?ZJt-JCkQ(et183RhbEU=W zY!k9G=bOb;l?w1e{5+((Z_wTk$c>uaxoA&K2^xYu6x^b?4ELZM9wOvC9luGzK3fZ3 z@#)aDk}lYoC8#)b#Tx3L;uG4l2z)_Cn$?vW&mud8lBk#R+$>T4D$e=-omFijRXcGpd(KlF&uac) z59N=RFNtaN+y0FTv$b6WIX+5al5!nC7gKdJ9haAp-BG|_hTqxWFaN|_Ymr_!1(t;NC z7ki!_0DTTeuYctj0d6YbUe0+Ga^?R`gJ$Rq-vq4ohR~=|@%7z)YGIPmPFQ%^VxNc= zW(HIOSyYY%pMQZ;Y)k^*DS4)=7>P3y8HgmN?1XPV4N~o&BgR<3Y?_0gE1Hvky2^tG z9wR3cdm1SDlR(+9A*$fv(q$M(OQSt`0QmUqoF1ig2-mEfuvWfM>9Dng1hkm8MV@l_dRl&!%LQUvO zq#-4pu5v#ip=L8eQk3`8)}-cJ$P)Wp-T*jD7`@CdQaphYrh|O zDeW9Qvw!x|z6j-+djh;+@F@H2RLfNIx-_Vae!hy_uvt4`X@d+YnJ~Qk(v!S03s0-2 z8%ns3VI$Sc8Khlxc4$Oa$sJePqkk&SSdI?iJ0Bt{7MfVuvAkSz-j^B?`lt&iwu-8e zHB&-hILy$l{n%utTY;cJr+Zt-xDXfhc7+#_RE|I9+GbuP}=G#KUJU zvS1@3ZAn+DL&Z3l?xh0W=!(DvWki-lNjQ^i``93;`l-Bun(w5kQfIj`#x;gp;NWo9 zW`B@)-3i=%pSyYJ4q3+DW}GGNu*{Y6V&O=>`H+y7rFf%Drd;^cU;Ye1LkCa7#q>M2 z!37&HDaN#(VY8ZwM%xqdW0=h#1_JKPjxOoW1aTzH)}yXVw3}1JnR7cLwQOa1jm29P zmCVF2dWu^#Y;nW~8y#z+${DDJS{qjrEq^(9lf$eM5SqnD=Y`9YJWGZm-;F_O{Vegb zj<;}H1coPr?XZ#QD4iUJ#L`j`AG@>$Ioeo1Rap3KHGy047b1~_Z_%3jr|67kcFEml zW#|Hwwcwr=h=*_0a)mJM#G1E|6ar~JdJR?vi5(ni2r~6JH@1pa;Tf|^RT%<8 z)S_wl%0c;kJJtW&8SG_^XE1}lm5}DS3ii6bMqd7_Ny|MogfsvPUc{6p1+nw(ZK8Yt zv!OACFENpb`5mSyX2bx(CCWb%l7Hla^7rvumY>xo2H4FTKC&$ZcEyjC+P)!ItspB_ z6i@VtVyU6CU=Hig?kB^9d?od7~M zee|=MBFM&KS4%ow%zXRVI2}2Ki+{s;to9Y&l)*d| zsIZM1 z#TRHx&0;P|r6SECBu+@(D}q*AjCsoJyBt;{I{^oZ@29_0zxg(N0@t|NTqTGi4X1-7 z6z)2&8o8P3C91={Ehy>)wlpP1D%UIYiS>x5&pp`0uPT*sHh(0dxaxQ-zdH~tdS@qw zSLRN(n7Euc5eS%M&@=3@ydTGqDPk{UVzzXc;~Rfi!k^1rzW_aBP2cRDi4|x5B1>10 z-Nkvhc>8lI%%)f{hGj8#%$CL8=87t)CXF*nhAT2@TrAUd79h?lUW%nw#z4(^FWVyQ z4-2}58$+$GMSsN~)5IqQ|MW+8i7ZOa?@P+C!P=Q#qQD{g;xsLLT|20#={>LLlu1x| zPqu)p1TO}Y+nb!>!ugz05jRrQvFO0#SsTs$P&N9EgG0_B37YSRo7h&>G-aFyDkUeg zPq|v2|Jx?cJECUc^%+cjs3Gu97{5T! z4lBR0I$9Kor|Q{`&vViUuwDY|j*lZx=@<c5%-&pH8V-_75l?alLMsfp; zC|*dbW2vviNLPN5(K0BW#mrQEiV5oOA079=wLX;qt)xPeD$4zHK2S`~%{gi+N^?N;V zoy3DK4!W527RZ^0GDLsU9?nu|M(u-@G|Aa52vQ^0Xx%?2AC#GGBqGv~X_LwLPL$Mn z>Cwoy6&dWw++6x()XiiFMRH~*Fsl$_#P?f+iBZB(vOCjc1`S>ABU*Rt2a(7^wIao|1ycV<)CBW zPq;gxJV_WXKm{r5KH>O=^lDH*$GG*a?Pc_=WB!9g*u6B9i67h z+I=aljVjW|*e|`R?_jVk5Ty`hZQoWbeR=E2X@6m; z!gIt1FPrm0eE-OJf@o1^SXZ9rG`QadiHc;$bYWItHSu1-EH;X~=>p`XLHPT#`_Gzb zEVs|2@5%6wpLn15E8sGMbibV}Ul4+l%fm`eFH=msqn0gzyE{*&6PDzkgf>Ewn#@Yy?31p_Y0yMKSec_+9aEiF&+ z3yh;&v7ve%%p#C_bu_~Cfs@g`+BDsg(~fA$(2SV$9RdfgLzg+<>F*v7(nCH_f0WG5{882P0OX^ndBPY!AL}O`m zpZp=m;enCx$1t$5&42q1smaP9g7l!dvSKx+jvV=l21&0oW+RYafO`06(P-&i=TR}& ziC0IuU-^jXukpTmRANt{Bc9i%1H4G6BD84Ka7-lL8D=`9d9Stj;rn^Q&WhPAfkN;z zY{$li8Z)5gQN#*NQw_q~!q?9sd5)F9&66!3LImRqi}%`2>VF?(fnP_M<5=2cGgY6B zR@tM@a-=Zncfam>4|g&6u=2w3oKEqdUK@gjupM7GH7>GAK6Q!240rpGj*TEn;&3YH zyB&RtwY35bRa6-*$8Q(cUMv&Fw)h2T*|nnU(e{iJCD1#vN_apUA8iJSPY0HwW!~y4 z%xMEJTzCC4^nVOr!p}${E*tdmYd!q#(H_6C*C% zLh2M^#?@y91DXG}Mk(Xm?Brapk0)ApL{ju8X(@!IsCxfa8AO^AXW7wsCGP_s9aA1+F4MTM?Y6 zq)G53j(vassPK2kK-6yS&T3Ylp=o}WdRz_rshD&S_Yc)>E%w7Xmv|>{pz7ixjVI!Q zUP-b>aimzrz1_GmMXZ%YW#m`l!L_T$CCS(&Tn~J#W$;ttavF~Wgu^$st~r?|#ea7Q zWa}SrL60epEvn!Z(u*qx3AwVcYm(1owIj7*A{x7WbzQ5S#umd>kBX0T?_1w5%AP(Y zV~Pv5ik!{<`XLK{!Y^o0$mC9){L2amoK-R3-VroaZpV3OBEkVNEI-oAzLdZHMGN`9 zez6=nfl7M7h7uH&@!-T7J>QAAUw@C?65C}x2IU}incLMR(Q(KuGD)`FxG1vQPcA-T z9a+JM$_Ub+~G^2BF~L+$vJg;)>31@SuWWeH0C-JaRrq>rBl>sb$`B-Ak>F! zgmi8iQTd|~zz$7U3?7AypI#`DIJhZz3DO}*vre|(&QrJrrW`5sQSpIj6BkV{DCbV; z1>7BDAva|mi&g7R|#5lk*4pBkE>|x$X0z9VT~4FlL!$U zeab{Mz$+!x0;qM$KEr+=+&rhwd)hq^YhXFEkxtSjY$DnM5>|!oihnSX!A*yf*CY|L zFd#+=6p?y4h6Ph?f%bLklP|565>E2fVSukMFw;=+0o5ymh_;i`Iom&0o6>@yPZOcY zk23)oh7h?_%%Kyni{s3hLTR=-6-O!1cAj zp6mXJyzRvOV4w|0M$2kAh~WvSvP%+w)gBgDIFYmFw|sk3&4xiS^tNblch2X%UwXN^ zcLBsS&7WlW_^R?ig<;*d0y9pW=I4sF8#Mi%lH8t_U_C<=T;oTf;B+HMMO4y9F#3o; z4bHn$A@J!*H-A{YNx=X!s-{+{)&@ccY~GE8yLrsaU+5m5Rc7pxcRFSu%Uaew1DOt4zZxz$s7!JD3N`5V%a zhlZJJMuQW)aD5nQAcya3vzi()Uuw@`Ecr)P@4HNPEJ*fHpyK1i42czQD&NHgoiNh! z4MlzO?)q;w$2Es9UW9>ArVVJd$ZKgC&ZR!@Dd16aL9+oZ$v6w+2JR0abi|K3pQ zfY=DL`~P$9giZo@Dt;W@8Mw^WJ=!(8Rh zp41CP>V4qbJQKJHlP2(J8a8eREz_OWn{{7j7q*l(nhuTEU8MwcVCKqttACwqH&y4y zn|~Nr!*mAr8oF`zP}?c$D)LTe#frVpIx{$R$ruP}=l~=@*DoahmhIFkyvVf>=aJ2F znQ%1@(|ve$D;Jvsjf26X%RIaZdh;$~IdC|O!~GRBCW=PSsB#cT$MZLGSJaA{ZkFbJy>nWYhg-0mFctAiXI+@SlYjcsK)EDHRng}PUW2*=Mlj_6q<+_T%9o@6 zeR1wpmRrmm$qz6FXSR8o5j;KTr>_P#<)Qd*WaEpd&_dmL4ukGoiYOMh%P zQk~?bpvnsfelmk(aL*1}=o>-DD4(Hr5r?BUj_o0~;xt-K3Fkt1B429&v(2G3EWcVI zm0&W6x=!y}mI&6{+ui!JD88ewT5^&m>8yJzoD1qT!xUV$D6+KhV!Gcwz6WKea*W8~ z>p@Ol#%Fl~_)6wAE=S#)FBd1k`T#)75Lhd|Y!aZWB z*2R!^UXg=b<8LX$Bwmi}QtP4qW`_H(tcP>W^wP0uJCMERs2Hg;OQ-lCL4O-V4DU@D z&6kPVc3Rj#;Y=3&`kdueMfP;sm;sOF!==Y*VnSL9Gm!VqdD~fkjs1Pzry?cZ$a`Wd z_@J(xtrBS5FyV-GA#tIo06_NoU0@!ZxChfZ(klvO`_%V@hO`>yL!6CantO zOTun7_ug4~-X=y%=!|ST+vlf;*KCygP*C%|hC{*)ZARF(pS*S&sfJ#p+XH}RwktZ> z)zN=NgsG-XhW7rrg?&|296+-sE`t+f@F5T^3^4fM3>Ms7g1fs9?t$R$4uRmoH8=!< zy9Rd%5G=X;d-v|{+rD*I^{MVY-RG2ab$xCq48pM*K7I08sX_@3Fosv(X+#e%;S?>= z#(xPuOeZ(=xrld#5G0Zb%Jz?ok3!tVf=SJ`wTD7eAYEBf?_>4_B6K{(O`c-@x{o5P zxGn4UE4Fl6k+u4k{UK*0%k!H}L5pG<`BB=J7d0aDVOkI+7SphTb>eT(GLS3Q=$A34 zfV2$f*xS*W_g-AiapMPvmUXJ<;?tzPO(0fyXc%eN;#$v+i-ZD;Lxb*T8ZT76SpaXp z9H~=9B{pO@D9T^+8v*V7^HZ6UKLz;2?hYZ` zV*r{*4EFxL*i_0>To$}$m zl|iY8)RJ!`MD<#Irrn60xchLc8WXHVnBu!!f;Se}>|HBZ>B zc!d%OB3?|o<&s?#EG9SW>k}^^Y@6#lFi1Rfb9{NjYAl*?HW#~bX-Pt^x&Uu1DQ|LI zxH#u|MB9DCV%$l0QJQn7Xr_VaW^nnbs`!oI8|>QPhVC;ZI@GtddiHaMcm>ZBLqv{| zt&}bucc|&bTSupSXdGbioA#j%`LG*<4?;i zK{U;Us8N(Oy%GP7=Ew8D>m%WQL9P%)g7CdUB{SedxjFs^u@A(}1>r~*&trl&>)Sc4 zwI;ose=JPsz_7nk1hlq_<vQTlSD|vd z_k9Y`APxZrf}mg&QjXjkm^B>Ntd&*roiSpq34u*PFjdaGEfqpvq_oPCkO)>Hx`OcO za6)pLLOp5953*Q_P=)l)>9Jm8HR6a5Iw2MP7#4ss9}|sm94+7g%Hew~by*}FRSn6| zoh%Y!IwGQUsljllaVndbIHnl_X83*xODYLS1`#-<(u!=2k361 zz7xYuCf5V}P=Hw5^oDWy+hWUnDbN&`M*tEz!sIoli>8wb-j+_Z&^Ph3{y61FWHUuz zYH~J=;3`27CW}{>q~!QbGQTAu?GhpF#`J^3n4ONl7)gOZe+=XIb{H5TSgU}zcne=I z_{T)84R%Wq>0FSOf(qOvh*v^>z*-1F4_M<8ValPmjjh=S4ClhGEpd)e1u5^u0iG3H zNEi#4fuUfrbU;R*cmgtDu8@)naetsM^K5{Y-!}f9z-k&CLtPFm!kjy+;56P zsh$f{1Wdyhb}%r$9wx$!B=i#jE2&jmmBGB%8aul&u9PJY1Zp#%={S<7>2$4Y6Igz6 z3>-vIW%hr#euc?FSRxANQ19#=S;PlWT9<<=>OYBFDOjxDAs}!MaY#Gv?Y~7tLE#rs z$DjNl*_q&qJ<@#5ff@AXQfwKx>n)a5=LQ+ZNI?~F26arr0^?BlmMDoHyFZzo8hGQ{ z^dGZSIit1#`Rk~ST!bDl|B06&q1(Jq2w8MCY?3vLfXm?)P=|6C)g*1RM5&|T;0zTY zRDXH0TvE@9a&Wz*iry={b* zfnMth2Sk3~fiMhJd*OZiO8iTo=O!79=JKl!lP+&YGRmhPzFyQ+V+FvDL_nXXl^OHy z51=WorI#n&mr7jEwAQt@(b}^2$hu0h;oh9#a5Cjtx(@xm#JPeLxh(-0<+c&>)YvGZ z07#QXA?q`{Gd>+t5$tOq@+DGO?<#8r6jEvP||2JLSS{qrTY6aDYyWl=m~h zvF)))$dFEueyesHzPX)kph?h9`M1Q1|EnO*c`IOPrtL8lecyEGF|a~gOO&bS59OJ} z%S`>Y?P5~Z#j4BPA~Yd(5T_Pt2&F3|zE!%lti^;P!TIA;TV_C1TRM$-LA>`Ma$X^0 z7eik%zv*nU+A7PQmFQSME$h-^3-9lFuM&{PwP&x_Y3 z?>i(?-76-jmCcX4oywBDd#(JITgQPI!hK)*nsxCvYvbbJPs`QWWoQ1b8IzEF0Pa-1 zRz!KFC^OX!<0%)_IL*n_WtM5C@&q2;8*T3@LF5iqX?~Tdn4gFJ^eeEi@R47Ea4Q=+ zwq=XE#KU6VPlp?4fkwxQ{`G(j1v*ZH*{Cy*dk0}tuW87j`PO6~TQWl)23-T8t)Zll z*(vVwFTq)nV++Da{yNT*auSx{&~+Z3YCd*de{@P#i@gDvuHF%o|C6^ zr)Rih%7GDEL?~ud)YH3XIsaW6`agyY#@_P_7dXbn1`)pw%56*HuQ_khZL$xs+eo z*T;bZ4(-?NhK9-%C>3F}T+>yZhCDnNt~*w@uIu12UBO6##wVLaw(`w!A2)q3e62LS zJ=5ihE`Av(H!h087<{iPmxmTO?jdMbe#>(6T1eA-XKy@ts>eUj^LqRDq?R)9*W1AO zkgie^?!C8@Lc!~YnCcGOMOx7`O$sm0`XN_)?WgBjig}{^tHWP5je2|^9v7{g*b=^F zp$<{*_~Gp$iN0f?Wtanfud-fPuuXTBznUwH6-hooJ4?mi%!TI!bb7zoH+Sng-Kx8c zoE?}1cL3g;jrL2uX_(KF7z+_*l?{)xy6a@xOd)M!_Os|oC{K6F|M&Ow`uB_98kqwG z?XD~G4>;EqY%c2nLFxqT1#>u`;0Fed~r|BExGln{l1- z3?IPAa;K?-B5-KHYh1r^yf#l6B=IpYKlf8;_*X3zJ-9P+0{jNVjp-+gX7ZJ*X@aCl zt?q+TwdKnEUI$sp-Su-Vn0-}0s5*DE>89n$sP+O;c;caL+i$N;nI`b%_E&)ApY6S- zJgDH8^4}>r{&e?H#F)&?tfP>~5Z)}4KtF9G5Q_$c?A7EdHdN3B(d1U#Ib{@S}Ch__F`e+{Qc z+oTVm3#metQ$~#v|198(2i__W{I*eefT!5wUsGD<3Ll%sHPf`s4VosUxpv9`4o07S zAvOo!jD=Iky@Rtp@}2D9Wldj|f%)&vS-n$TO%*7_23L0#KZs)L5bAioRi(6Gtp@ji zb6M*R5Og@vyM6YFW9}!yFR78KjPhHg4;d+nW&KHrr(jJL=CIA88QHOpqiWe>coMFwrYpN` zT=~WvuJgAm*WK#`HUrilHmt@Ig*V}7FAmI?%BBskEA|L%!f*Q`>$Z31om~=Fj7rP2 zpV&p7F?e)xXV-fhTLtKzzIH#52pPaxq z*IPaTt6xmMb+rz!KOz3szJvS4xznn3J2KXBnVwhc1dH-EYfG#*H&_$paKM|<0t3d# z*WO`1y%F~#HYzid>`VP9A6x#_q*Gycu_8w}{DM?A&>B6^@>WTUnsPswH;~{wyc!a|xeoVIU%IFjM#lQe zNI3mBYUOTC?Mwc0zaXsdneXTt${lcbw}1C$N_%qi&g+EP<;9-BYS!KpNMosO)+j3~ zXwMVbxXFumr;Jb6`0~;4%)VRQEo)THtXX@)gqY-7hYGh3kcyP{+wTWhUM-gm3ZVMb zJDc14RO9hkYS>(gPX*pCLGqU!DVp``o|u+9U^I7T%F{OyQvJalU?OOhtnr3Z8V?rA zs+URCaB20Y@(V$ib!jncy*kObc_KttXzcB7!|7>(w#cf+i+!=@gWO)XoV=07?9YV( zQtS7T@?)*PdYM_cHsUhviYS_vdGyO23#8*lW`5%6ou&+O>YBlQj}Flkhb-EbeJ5~O z+QrY@Lr2U}(f#-i=WzOdZJ|L7INQZ>)&Q29@Th@w9zC26%DIw@wY=Ir=P3}OF$=UcjR{+=83G6pZO z_^70*`&$e}*125@S~R>*GTo-0bI7#(RNp{)a#8TS&ov9=prggdx+=i6GHqZHDpUN@ z7fP5DE4xiy-BbUJTjj*SQ2pI3oPWWj8Cf%RMlYdGU_q-H*)$dQMH&<@r4=u6Y8hWZ zE`MB5WC{NzFOC!Dsi0w_gx7(sEXk!)C1d1x0F@7>dEZrkjSFCx_d+n!WF?svf0?i< z(}^YcV)l-TXhuOnOJlex`kjZ0svCs)+ALf+ZB#sUBIp;L*bpJK;N2Nc2w;e@@svHoAe)g!W^ESQsx_kYBJm|kUr6l^># zK%5*9K6Xh9QyUXUW{{n;iMzF%g$c;h!qx3{0uV17CmRS0X&YBJcMumh>c2^6#J$VkJQ3A9f7(6b6$pKq$x+ z<~N|sK7ucbY$qq~iNDdfc3syuj@1A>Wu?>mV21pU!#=%09_Cy65J}AG-C_#QRZ(k{ z9sf~8LtPMOLdAz6C4Pm+oHFY!t`rQQaMwHRlNiRR$v}h>t}wL>#5bo-%m#n8EdPPU z1&j*eh+)Q`<%yt5O7GVh&gmi?YJ`ORL^Pq*fYw>mr3B~t;>x1_J;(*&R8d!As8~XZ zN`MIdh|RnPLVlhux5I($WKg(*(!~X-HakD+(V|nwt~Vd;^3g4Sp!nb2@$9RU?>7=rf(n)vsaY z$N_h+jG>FY(rL#ERS)L47R(nLa;HK|YbFHi&#VZOVo{wNkouRiz^a@n+Y8$e0zM!$ zz|VaYoGBc*#8tkSFuZ%N&vAhlIi(;l|8{(1zu*cLlpToVccl z>E3!B4|gI#$x$>*I=$*MV1GLau#O5z>L81p{g8Cl^=)r=Fn9oNPQFw)tWQT>w;2a= z)jM%Q$G5AJSnr2m3LL(caarHkjI)Uu1yJCtRfqxKRf%OZnIwCCm~@+Q)Scp_UvC=| zN+UOXkQ{CW46yztww%@t{8XqV(O8f|37A)`WvbDz8?UJ5w!@anP2VTO`ru&c+tZQ?OlqzD(oGC^ZbcM1KnA_!LW7g}$Weu8MwOc#j zyXiD)aq~+_*I1*ZbV{l?={Kn4fXw8|0I@n_w&UZ5c}CBsLA;@{caqx(*Zn#Hk?t}ObAX5Pq|*5AtK`}V>QS|^NiF!wU}aTM}Xdkj(@ zOHA+6qs30D3lU5=)5+_PNTr9F*;EGKTfuPK#~GXb=4_5A@h+TZ;$^_PV>-{z2RlM> zGwr4%{xt0y`UB(s7`4FPJg0IWryO@KzwDK z8#;@jkEFRPjtQqGfwP8&jY*Guw&aUNW!q@78$L@1?reeUg?PgZR_}g`w?F(CnLARW zol(4Q%pq}zwfa_n|2=9`uh6Kx>>9fqm2c9LJQdv6h3~!W9X(KDE;$I1$Ifn`fy{%K zBRoWYm6q&V-OOkn3O&^=J?IFgOjs^IoDdxtB1EcVz8oh&kr7Rgd`lT(jv$#%*5YUX^Yz^kI6Z8|v{@n?oG~MJh`D3b(WJ z8AiB?@Gk?VdPao=auKrHzFnUv7`oM1E=hbj#%kk0;CfrctVfUPJ(l)ZrmL0ocelh7 z)&0S#d!`$tL;W$5La(glx4)fhSyDs-PG}XGW;bp0A^9a&)Zd5({e+5FW(9et)$%v$ z$JLI1$A3z8^n@;}FB}r~p~-h7)1H1+*apH^tD9;naXQ9z%^=S=f9d31bYresSrw-= z-Wa5nl}{*poaa@*-%hf*qVKFggqcMh0xbbnKe~>dm+X_Q&@glBu4XuFe1L`Fd>o!<{Vua%@!xX$i|_tWi9evMlcr2v z$ZI6O>!RAeo18``dik}T5*BG zf~x}&hv(_>Vq9MMyJ}G2DLZH`{;q0hV=n0cHn^k?B8$tSb7Yui0l62|eOw*d+zyG0 zGUkbx@HzE)P#q%oj+XONm|_80`gR_E9wNAvYK_|DJ%8-#;2B{{u;&`JV`Lq&`_7$9 z0%k{VTff5u;To?A-(g?s7B#_Lh=>7>W9+3+Ymi{*D*Y)ssf!idg(3{u7+)s-8V@s4 zEQX@bB5c4Ig)hcWj2|%MyxZ#0@4^wB-B&}P6vDj5dJgWlamMr_`zFVaZ0R!gGoXLm z824M4K5Zw`zKfU3c$jt{PtLVx!li&tFL?Kya04Nhf6pQMlA$%+FRDYrn_{icVVlVI z#_)AetjfoxI6rvEtjv+j#xT_u3Fa+2*1t7V-17Xw?8f-_{b2m#AeLTDFCmLaXt+;EHg#d4w#pqcF?JLnik8MlLdH%>& z>KrxcS)an?&4{lsRlZEp4tjs1JX{O&$m|dAJ5r%ejTj*ERA8~soglcQ%h1_Z}>@3uw&&RC;ZtB zhjV3lUO1p2QibnINCoTY7~0D{KKs4aPwu4soc`0HFMG{NKoHuEBi20Q-Y=7ap_7AM zJ`X)>>P1Z52f?{%?^ZSIFd5W*`GD2s0JQ#PJEVlH2>Ud`jP)_zCjU_)yb`Yqziwg-M!a>HI^>+vS*DW3;%eTSx10lpf`$4^lZi6qz&O<68hwbK>vsCzg#g zCoIgde2C1kC9JMX-=!J{X&q=S5<^)wkSUcDzerPwl`z(3)qJtx!u%up@bI@GH@pOP(!%}letEZ_uvs9_s{Eh}}qiHr; zovt^pcl|gwuX}w_t~ammc;2jqlLlYST&()(`thYIRY^V9cp%M>D^mWU%?7dsWd);J z6i{je$@Mblx!RWtWtYyG>;2~%Y#}H=#HY{+?D>El5V2oUcIoGD8mTjmj$T`P^s-Q^ z8OL6ioI50?vXmR9`Ca#(y}+kp=Zk-jT+evn721IR>sSFS(m63v2w&YEer~|KcT&pI Gfd2ygcqJ_W From f2ac90d5b9682f4b78d8ffe99be378ed32df11c5 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:35:44 -0400 Subject: [PATCH 052/110] explicitly define simpified jacobian expressions for efficiency --- gtsam/geometry/Pose3.cpp | 62 +++++++++++++++++++++------------------- 1 file changed, 32 insertions(+), 30 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 5a3b20782..ab3afe37c 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -69,36 +69,33 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_xib) const { // Ad * xi = [ R 0 . [w // [t]R R ] v] - // Declarations and aliases - Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, pRw_H_t, pRw_H_Rw; + // Declarations, aliases, and intermediate Jacobians easy to compute now Vector6 result; auto Rw = result.head<3>(); const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R; + const Matrix3 &Rw_H_w = R_.matrix(); + const Matrix3 &Rv_H_v = R_.matrix(); + const Matrix3 pRw_H_Rw = skewSymmetric(t_); // Calculations - Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr, H_xib ? &Rw_H_w : nullptr); - const Vector3 Rv = - R_.rotate(v, H_this ? &Rv_H_R : nullptr, H_xib ? &Rv_H_v : nullptr); - const Vector3 pRw = - cross(t_, Rw, pRw_H_t, (H_this || H_xib) ? &pRw_H_Rw : nullptr); + Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); + const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 pRw = cross(t_, Rw /*, pRw_H_t, pRw_H_Rw */); result.tail<3>() = pRw + Rv; // Jacobians if (H_this) { + // pRw_H_thisv = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R + // where [ ]x denotes the skew-symmetric operator. + // See docs/math.pdf for more details. + const Matrix3 &pRw_H_thisv = Rw_H_R; *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_t) + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_thisv) .finished(); - /* This is the "full" calculation: - Matrix36 R_H_this, t_H_this; - rotation(R_H_this); // I_3x3, Z_3x3 - translation(t_H_this); // Z_3x3, R - (*H_this) *= (Matrix6() << R_H_this, t_H_this).finished(); - */ - // But we can simplify those calculations since it's mostly I and Z: - H_this->bottomRightCorner<3, 3>() *= R_.matrix(); } if (H_xib) { - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // note: this is Adjoint /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) .finished(); } @@ -113,32 +110,37 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, OptionalJacobian<6, 6> H_x) const { // Ad^T * xi = [ R^T R^T.[-t] . [w // 0 R^T ] v] - // Declarations and aliases - Matrix3 Rw_H_R, Rw_H_w, Rv_H_R, Rv_H_v, tv_H_t, tv_H_v, Rtv_H_R, Rtv_H_tv; + // Declarations, aliases, and intermediate Jacobians easy to compute now Vector6 result; const Vector3 &w = x.head<3>(), &v = x.tail<3>(); auto Rv = result.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; + const Matrix3 Rtranspose = R_.matrix().transpose(); + const Matrix3 &Rw_H_w = Rtranspose; + const Matrix3 &Rv_H_v = Rtranspose; + const Matrix3 &Rtv_H_tv = Rtranspose; + const Matrix3 tv_H_v = skewSymmetric(t_); // Calculations - const Vector3 Rw = - R_.unrotate(w, H_this ? &Rw_H_R : nullptr, H_x ? &Rw_H_w : nullptr); - Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr, H_x ? &Rv_H_v : nullptr); - const Vector3 tv = - cross(t_, v, H_this ? &tv_H_t : nullptr, H_x ? &tv_H_v : nullptr); - const Vector3 Rtv = R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr, - (H_this || H_x) ? &Rtv_H_tv : nullptr); + const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); + Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 tv = cross(t_, v /*, tv_H_t, tv_H_v */); + const Vector3 Rtv = + R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); result.head<3>() = Rw - Rtv; // Jacobians if (H_this) { - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, -Rtv_H_tv * tv_H_t, + // Rtv_H_thisv = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R + // where [ ]x denotes the skew-symmetric operator. + // See docs/math.pdf for more details. + const Matrix3 &Rtv_H_thisv = Rv_H_R; + *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_thisv, /* */ Rv_H_R, /* */ Z_3x3) .finished(); - // See Adjoint(xi) jacobian calculation for why we multiply by R - H_this->topRightCorner<3, 3>() *= R_.matrix(); } if (H_x) { - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // note: this is AdjointT /* */ Z_3x3, Rv_H_v) .finished(); } From 761987a14ced11637d31efc43a3d81250eea0c0a Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 03:37:34 -0400 Subject: [PATCH 053/110] stricter tolerances --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f0f2c0ccd..4ea3519d2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,19 +204,19 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); + EXPECT(assert_equal(expectedH1, actualH1)); EXPECT(assert_equal(expectedH2, actualH2)); } From bc1104c8076beaa29824eeeb6fb40e9d511a47ac Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 11:20:55 -0400 Subject: [PATCH 054/110] Revert "stricter tolerances" This reverts commit 761987a14ced11637d31efc43a3d81250eea0c0a. --- gtsam/geometry/tests/testPose3.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 4ea3519d2..f0f2c0ccd 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -204,19 +204,19 @@ TEST(Pose3, AdjointTranspose) T.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T2.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); T3.AdjointTranspose(xi, actualH1, actualH2); expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi); expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi); - EXPECT(assert_equal(expectedH1, actualH1)); + EXPECT(assert_equal(expectedH1, actualH1, 1e-8)); EXPECT(assert_equal(expectedH2, actualH2)); } From 31e5cbb81aac00bbc0d51cb6df26598c90500187 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sat, 16 Oct 2021 13:49:04 -0400 Subject: [PATCH 055/110] address frank review comments --- gtsam/geometry/Pose3.cpp | 60 +++++++++++++++++++++------------------- 1 file changed, 31 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ab3afe37c..ed605d8b6 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -65,53 +65,54 @@ Matrix6 Pose3::AdjointMap() const { /* ************************************************************************* */ // Calculate AdjointMap applied to xi_b, with Jacobians -Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_this, +Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_xib) const { // Ad * xi = [ R 0 . [w // [t]R R ] v] // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; + Vector6 result; // = AdjointMap() * xi auto Rw = result.head<3>(); - const Vector3 &w = xi_b.head<3>(), &v = xi_b.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R; - const Matrix3 &Rw_H_w = R_.matrix(); - const Matrix3 &Rv_H_v = R_.matrix(); - const Matrix3 pRw_H_Rw = skewSymmetric(t_); + const auto &w = xi_b.head<3>(), &v = xi_b.tail<3>(); + Matrix3 Rw_H_R, Rv_H_R, pRw_H_Rw; + const Matrix3 R = R_.matrix(); + const Matrix3 &Rw_H_w = R; + const Matrix3 &Rv_H_v = R; // Calculations - Rw = R_.rotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); - const Vector3 Rv = R_.rotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 pRw = cross(t_, Rw /*, pRw_H_t, pRw_H_Rw */); + Rw = R_.rotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); + const Vector3 Rv = R_.rotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 pRw = cross(t_, Rw, boost::none /* pRw_H_t */, pRw_H_Rw); result.tail<3>() = pRw + Rv; // Jacobians - if (H_this) { - // pRw_H_thisv = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R + if (H_pose) { + // pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R // where [ ]x denotes the skew-symmetric operator. // See docs/math.pdf for more details. - const Matrix3 &pRw_H_thisv = Rw_H_R; - *H_this = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_thisv) + const Matrix3 &pRw_H_posev = Rw_H_R; + *H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, // + /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev) .finished(); } if (H_xib) { - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, // note: this is Adjoint + // This is just equal to AdjointMap() but we can reuse pRw_H_Rw = [t]x + *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) .finished(); } - // Return + // Return - we computed result manually but it should be = AdjointMap() * xi return result; } /* ************************************************************************* */ /// The dual version of Adjoint -Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, +Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { // Ad^T * xi = [ R^T R^T.[-t] . [w // 0 R^T ] v] // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; + Vector6 result; // = AdjointMap().transpose() * x const Vector3 &w = x.head<3>(), &v = x.tail<3>(); auto Rv = result.tail<3>(); Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; @@ -122,30 +123,31 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_this, const Matrix3 tv_H_v = skewSymmetric(t_); // Calculations - const Vector3 Rw = R_.unrotate(w, H_this ? &Rw_H_R : nullptr /*, Rw_H_w */); - Rv = R_.unrotate(v, H_this ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 tv = cross(t_, v /*, tv_H_t, tv_H_v */); + const Vector3 Rw = R_.unrotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); + Rv = R_.unrotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); + const Vector3 tv = cross(t_, v, boost::none /* tv_H_t */, tv_H_v); const Vector3 Rtv = - R_.unrotate(tv, H_this ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); + R_.unrotate(tv, H_pose ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); result.head<3>() = Rw - Rtv; // Jacobians - if (H_this) { - // Rtv_H_thisv = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R + if (H_pose) { + // Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R // where [ ]x denotes the skew-symmetric operator. // See docs/math.pdf for more details. - const Matrix3 &Rtv_H_thisv = Rv_H_R; - *H_this = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_thisv, + const Matrix3 &Rtv_H_posev = Rv_H_R; + *H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev, /* */ Rv_H_R, /* */ Z_3x3) .finished(); } if (H_x) { - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, // note: this is AdjointT + // This is just equal to AdjointMap().transpose() but we can reuse [t]x + *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, /* */ Z_3x3, Rv_H_v) .finished(); } - // Return + // Return - this should be equivalent to AdjointMap().transpose() * xi return result; } From 7a9d89539c3c8924c9f8b8dca6a0ed63734f4d85 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Thu, 21 Oct 2021 19:23:44 -0400 Subject: [PATCH 056/110] TBB revival --- cmake/HandleTBB.cmake | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cmake/HandleTBB.cmake b/cmake/HandleTBB.cmake index 118dc4dac..52ee75494 100644 --- a/cmake/HandleTBB.cmake +++ b/cmake/HandleTBB.cmake @@ -7,9 +7,9 @@ if (GTSAM_WITH_TBB) if(TBB_FOUND) set(GTSAM_USE_TBB 1) # This will go into config.h - if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) - message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") - endif() +# if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) +# message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") +# endif() if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) set(TBB_GREATER_EQUAL_2020 1) From 79f7861f0c14a0c3a502be0ff3447a1dc737ea28 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Sun, 24 Oct 2021 15:34:49 -0400 Subject: [PATCH 057/110] made changes according to Frank --- python/gtsam/examples/Pose2ISAM2Example.py | 158 ++++++++++-------- python/gtsam/examples/Pose3ISAM2Example.py | 182 ++++++++++++--------- 2 files changed, 189 insertions(+), 151 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index 5336bc34e..fac7ecc1a 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -13,23 +13,28 @@ Modelled after: - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ -from __future__ import print_function import math import numpy as np -from numpy.random import multivariate_normal as mult_gauss import gtsam import matplotlib.pyplot as plt import gtsam.utils.plot as gtsam_plot -def Pose2SLAM_ISAM2_plot(graph, current_estimate): - """Plots incremental state of the robot for 2D Pose SLAM using iSAM2 +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) """ + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. marginals = gtsam.Marginals(graph, current_estimate) + # Plot the newly updated iSAM2 inference. fig = plt.figure(0) axes = fig.gca() plt.cla() @@ -43,19 +48,47 @@ def Pose2SLAM_ISAM2_plot(graph, current_estimate): axes.set_xlim(-1, 5) axes.set_ylim(-1, 3) plt.pause(1) + return marginals +def vector3(x, y, z): + """Create a 3D double numpy array.""" + return np.array([x, y, z], dtype=float) + +def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + key: int, xy_tol=0.6, theta_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xy_tol: Optional argument for the x-y measurement tolerance. + theta_tol: Optional argument for the theta measurement tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + prev_est = current_estimate.atPose2(key+1) + rotated_odom = prev_est.rotation().matrix() @ odom[:2] + curr_xy = np.array([prev_est.x() + rotated_odom[0], + prev_est.y() + rotated_odom[1]]) + curr_theta = prev_est.theta() + odom[2] + for k in range(1, key+1): + pose_xy = np.array([current_estimate.atPose2(k).x(), + current_estimate.atPose2(k).y()]) + pose_theta = current_estimate.atPose2(k).theta() + if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ + (abs(pose_theta - curr_theta) <= theta_tol): + return k def Pose2SLAM_ISAM2_example(): """Perform 2D SLAM given the ground truth changes in pose as well as - simple loop closure detection. - """ + simple loop closure detection.""" plt.ion() - def vector3(x, y, z): - """Create 3d double numpy array.""" - return np.array([x, y, z], dtype=float) - # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. @@ -66,85 +99,68 @@ def Pose2SLAM_ISAM2_example(): graph = gtsam.NonlinearFactorGraph() initial_estimate = gtsam.Values() - # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) - # The ground truth odometry measurements (without noise) of the robot during the trajectory. - odom_arr = [(2, 0, 0), - (2, 0, math.pi/2), - (2, 0, math.pi/2), - (2, 0, math.pi/2), - (2, 0, math.pi/2)] + # Create the ground truth odometry measurements of the robot during the trajectory. + true_odometry = [(2, 0, 0), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2), + (2, 0, math.pi/2)] - # The initial estimates for robot poses 2-5. Pose 1 is initialized by the prior. - # To demonstrate iSAM2 incremental optimization, poor initializations were intentionally inserted. - pose_est = [gtsam.Pose2(2.3, 0.1, -0.2), - gtsam.Pose2(4.1, 0.1, math.pi/2), - gtsam.Pose2(4.0, 2.0, math.pi), - gtsam.Pose2(2.1, 2.1, -math.pi/2), - gtsam.Pose2(1.9, -0.2, 0.2)] + # Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements. + odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance()) + for true_odom in true_odometry] + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - xy_tol=0.6, theta_tol=0.3) -> int: - """Simple brute force approach which iterates through previous states - and checks for loop closure. + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate - Args: - odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. - current_estimate: The current estimates computed by iSAM2. - xy_tol: Optional argument for the x-y measurement tolerance. - theta_tol: Optional argument for the theta measurement tolerance. - Returns: - k: The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. - """ - if current_estimate: - prev_est = current_estimate.atPose2(i+1) - rotated_odom = prev_est.rotation().matrix() @ odom[:2] - curr_xy = np.array([prev_est.x() + rotated_odom[0], - prev_est.y() + rotated_odom[1]]) - curr_theta = prev_est.theta() + odom[2] - for k in range(1, i+1): - pose_xy = np.array([current_estimate.atPose2(k).x(), - current_estimate.atPose2(k).y()]) - pose_theta = current_estimate.atPose2(k).theta() - if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ - (abs(pose_theta - curr_theta) <= theta_tol): - return k + for i in range(len(true_odometry)): - current_estimate = None - for i in range(len(odom_arr)): - # The "ground truth" change between poses - odom_x, odom_y, odom_theta = odom_arr[i] - # Odometry measurement that is received by the robot and corrupted by gaussian noise - odom = mult_gauss(odom_arr[i], ODOMETRY_NOISE.covariance()) - # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state - loop = determine_loop_closure(odom, current_estimate) + # Obtain "ground truth" change between the current pose and the previous pose. + true_odom_x, true_odom_y, true_odom_theta = true_odometry[i] + + # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. + noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(odometry_measurements[i], current_estimate, i) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. + # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: - graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, + gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, gtsam.Pose2(odom_x, odom_y, odom_theta), ODOMETRY_NOISE)) - initial_estimate.insert(i + 2, pose_est[i]) - # Incremental update to iSAM2's internal Baye's tree, which only optimizes upon the added variables - # as well as any affected variables + graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, + gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) + initial_estimate.insert(i + 2, computed_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) - # Another iSAM2 update can be performed for additional optimization - isam.update() current_estimate = isam.calculateEstimate() - print("*"*50) - print(f"Inference after State {i+1}:") - print(current_estimate) - marginals = Pose2SLAM_ISAM2_plot(graph, current_estimate) + + # Report all current state estimates from the iSAM2 optimzation. + marginals = report_on_progress(graph, current_estimate, i) initial_estimate.clear() - # Print the final covariance matrix for each pose after completing inference - for i in range(1, len(odom_arr)+1): + + # Print the final covariance matrix for each pose after completing inference on the trajectory. + for i in range(1, len(true_odometry)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") plt.ioff() diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3fcdcd7ec..3e78339bd 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -12,34 +12,36 @@ Modelled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ -from __future__ import print_function + import gtsam -from gtsam import Pose3, Rot3 -from gtsam.symbol_shorthand import X import gtsam.utils.plot as gtsam_plot import numpy as np -from numpy import cos, sin, pi -from numpy.random import multivariate_normal as mult_gauss import matplotlib.pyplot as plt - -def Pose3SLAM_ISAM2_plot(graph, current_estimate): - """Plots incremental state of the robot for 3D Pose SLAM using iSAM2 +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, + key: int): + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. Based on version by: - Ellon Paiva (Python), - Duy Nguyen Ta and Frank Dellaert (MATLAB) """ + # Print the current estimates computed using iSAM2. + print("*"*50 + f"\nInference after State {key+1}:\n") + print(current_estimate) + + # Compute the marginals for all states in the graph. marginals = gtsam.Marginals(graph, current_estimate) + # Plot the newly updated iSAM2 inference. fig = plt.figure(0) axes = fig.gca(projection='3d') plt.cla() - i = 0 - while current_estimate.exists(X(i)): - gtsam_plot.plot_pose3(0, current_estimate.atPose3(X(i)), 10, - marginals.marginalCovariance(X(i))) + i = 1 + while current_estimate.exists(i): + gtsam_plot.plot_pose3(0, current_estimate.atPose3(i), 10, + marginals.marginalCovariance(i)) i += 1 axes.set_xlim3d(-30, 45) @@ -49,7 +51,6 @@ def Pose3SLAM_ISAM2_plot(graph, current_estimate): return marginals - def createPoses(): """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], @@ -60,9 +61,9 @@ def createPoses(): [1, 0, 0, 15], [0, 0, 1, 20], [0, 0, 0, 1]]) - P2 = np.array([[cos(pi/4), 0, sin(pi/4), 30], + P2 = np.array([[np.cos(np.pi/4), 0, np.sin(np.pi/4), 30], [0, 1, 0, 30], - [-sin(pi/4), 0, cos(pi/4), 30], + [-np.sin(np.pi/4), 0, np.cos(np.pi/4), 30], [0, 0, 0, 1]]) P3 = np.array([[0, 1, 0, 30], [0, 0, -1, 0], @@ -74,99 +75,120 @@ def createPoses(): [0, 0, 0, 1]]) P5 = P0[:] - return [Pose3(P0), Pose3(P1), Pose3(P2), Pose3(P3), Pose3(P4), Pose3(P5)] + return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2), + gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)] + +def vector6(x, y, z, a, b, c): + """Create a 6D double numpy array.""" + return np.array([x, y, z, a, b, c], dtype=float) + +def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=0.3) -> int: + """Simple brute force approach which iterates through previous states + and checks for loop closure. + + Args: + odom: Vector representing noisy odometry transformation measurement in the body frame, + where the vector represents [roll, pitch, yaw, x, y, z]. + current_estimate: The current estimates computed by iSAM2. + key: Key corresponding to the current state estimate of the robot. + xyz_tol: Optional argument for the translational tolerance. + rot_tol: Optional argument for the rotational tolerance. + Returns: + k: The key of the state which is helping add the loop closure constraint. + If loop closure is not found, then None is returned. + """ + if current_estimate: + rot = gtsam.Rot3.RzRyRx(odom[:3]) + odom_tf = gtsam.Pose3(rot, odom[3:6].reshape(-1,1)) + prev_est = current_estimate.atPose3(key+1) + curr_est = prev_est.transformPoseFrom(odom_tf) + for k in range(1, key+1): + pose = current_estimate.atPose3(k) + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): + return k def Pose3_ISAM2_example(): """Perform 3D SLAM given ground truth poses as well as simple loop closure detection.""" plt.ion() - def vector6(x, y, z, a, b, c): - """Create 6d double numpy array.""" - return np.array([x, y, z, a, b, c], dtype=float) - # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) - # Create the ground truth poses of the robot trajectory. - poses = createPoses() + # Create a Nonlinear factor graph as well as the data structure to hold state estimates. + graph = gtsam.NonlinearFactorGraph() + initial_estimate = gtsam.Values() - # iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many + # Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many # update calls are required to perform the relinearization. parameters = gtsam.ISAM2Params() parameters.setRelinearizeThreshold(0.1) parameters.setRelinearizeSkip(1) isam = gtsam.ISAM2(parameters) - # Create a Nonlinear factor graph as well as the data structure to hold state estimates. - graph = gtsam.NonlinearFactorGraph() - initial_estimate = gtsam.Values() + # Create the ground truth poses of the robot trajectory. + true_poses = createPoses() - # Add prior factor to the first pose with intentionally poor initial estimate - graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], PRIOR_NOISE)) - initial_estimate.insert(X(0), poses[0].compose(gtsam.Pose3( + # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations + # between each robot pose in the trajectory. + odometry_tf = [true_poses[i-1].transformPoseTo(true_poses[i]) for i in range(1, len(true_poses))] + odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))] + odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))] + + # Corrupt the xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. + noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), ODOMETRY_NOISE.covariance()) + for i in range(len(odometry_tf))] + + # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate + # iSAM2 incremental optimization. + graph.push_back(gtsam.PriorFactorPose3(1, true_poses[0], PRIOR_NOISE)) + initial_estimate.insert(1, true_poses[0].compose(gtsam.Pose3( gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - xyz_tol=0.6, rot_tol=0.3) -> int: - """Simple brute force approach which iterates through previous states - and checks for loop closure. + # Initialize the current estimate which is used during the incremental inference loop. + current_estimate = initial_estimate + for i in range(len(odometry_tf)): - Args: - odom: Vector representing noisy odometry transformation measurement in the body frame, - where the vector represents [roll, pitch, yaw, x, y, z]. - current_estimate: The current estimates computed by iSAM2. - xyz_tol: Optional argument for the translational tolerance. - rot_tol: Optional argument for the rotational tolerance. - Returns: - k: The key of the state which is helping add the loop closure constraint. - If loop closure is not found, then None is returned. - """ - if current_estimate: - rot = Rot3.RzRyRx(odom[:3]) - odom_tf = Pose3(rot, odom[3:6].reshape(-1,1)) - prev_est = current_estimate.atPose3(X(i-1)) - curr_est = prev_est.transformPoseFrom(odom_tf) - for k in range(i): - pose = current_estimate.atPose3(X(k)) - if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ - (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): - return k + # Obtain "ground truth" transformation between the current pose and the previous pose. + true_odometry = odometry_tf[i] - current_estimate = None - for i in range(1, len(poses)): - # The "ground truth" change between poses - odom_tf = poses[i-1].transformPoseTo(poses[i]) - odom_xyz = odom_tf.x(), odom_tf.y(), odom_tf.z() - odom_rpy = odom_tf.rotation().rpy() - # Odometry measurement that is received by the robot and corrupted by gaussian noise - measurement = mult_gauss(np.hstack((odom_rpy,odom_xyz)), ODOMETRY_NOISE.covariance()) - loop = determine_loop_closure(measurement, current_estimate) - # If loop closure is detected, then adds a constraint between existing states in the factor graph - if loop is not None: - graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(loop), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) + # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. + noisy_odometry = noisy_measurements[i] + + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. + loop = determine_loop_closure(noisy_odometry, current_estimate, i) + + # Add a binary factor in between two existing states if loop closure is detected. + # Otherwise, add a binary factor between a newly observed state and the previous state. + # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. + # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. + if loop: + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, true_odometry, ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose3(X(i-1), X(i), gtsam.Pose3(odom_tf), ODOMETRY_NOISE)) - # Intentionally insert poor initializations - initial_estimate.insert(X(i), poses[i].compose(gtsam.Pose3( - gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20)))) - # Performs iSAM2 incremental update based on newly added factors + graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE)) + + # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + computed_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, computed_estimate) + + # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) - # Additional iSAM2 optimization - isam.update() current_estimate = isam.calculateEstimate() - print("*"*50) - print(f"Inference after State {i}:\n") - print(current_estimate) - marginals = Pose3SLAM_ISAM2_plot(graph, current_estimate) + + # Report all current state estimates from the iSAM2 optimization. + marginals = report_on_progress(graph, current_estimate, i) initial_estimate.clear() - # Print the final covariance matrix for each pose after completing inference - i = 0 - while current_estimate.exists(X(i)): - print(f"X{i} covariance:\n{marginals.marginalCovariance(X(i))}\n") + + # Print the final covariance matrix for each pose after completing inference. + i = 1 + while current_estimate.exists(i): + print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") i += 1 plt.ioff() From f0fc0d445718df993d904e4f1eaba5c8d567e91c Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 26 Oct 2021 03:18:04 -0400 Subject: [PATCH 058/110] much cleaner Adjoint jacobian --- doc/math.lyx | 253 ++++++++++++++++++++------------------- doc/math.pdf | Bin 264527 -> 272118 bytes gtsam/geometry/Pose3.cpp | 83 +++---------- 3 files changed, 147 insertions(+), 189 deletions(-) diff --git a/doc/math.lyx b/doc/math.lyx index f14ebc66f..6d7a5e318 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5090,29 +5090,22 @@ Derivative of Adjoint \begin_layout Standard Consider -\begin_inset Formula $f:SE(3)\rightarrow\mathbb{R}^{6}$ +\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$ \end_inset is defined as -\begin_inset Formula $f(T)=Ad_{T}y$ -\end_inset - - for some constant -\begin_inset Formula $y=\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}$ +\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$ \end_inset . - Defining -\begin_inset Formula $\xi=\begin{bmatrix}\omega\\ -v -\end{bmatrix}$ -\end_inset + The derivative is notated (see +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" - for the derivative notation (w.r.t. - pose -\begin_inset Formula $T$ \end_inset ): @@ -5121,68 +5114,17 @@ v \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix} +Df_{(T,y)}(\xi,\delta y)=D_{1}f_{(T,y)}(\xi)+D_{2}f_{(T,y)}(\delta y) \] \end_inset -Then we can compute -\begin_inset Formula $f'(T)$ +First, computing +\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$ \end_inset - by considering the rotation and translation separately. - To reduce confusion with -\begin_inset Formula $\omega_{y},v_{y}$ -\end_inset - -, we will use -\begin_inset Formula $R,t$ -\end_inset - - to denote the rotation and translation of -\begin_inset Formula $T\exp\xi$ -\end_inset - -. -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial\omega}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\frac{\partial}{\partial\omega}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times}\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} -\end{bmatrix} -\] - -\end_inset - - -\end_layout - -\begin_layout Standard -\begin_inset Formula -\[ -\frac{\partial}{\partial t}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega\\ -v -\end{bmatrix}=\frac{\partial}{\partial t}\begin{bmatrix}R\omega_{y}\\{} -[t]_{\times}R\omega_{y}+Rv_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}] -\end{bmatrix} -\] - -\end_inset - -Applying chain rule for the translation, -\begin_inset Formula $\frac{\partial}{\partial v}=\frac{\partial}{\partial t}\frac{\partial t}{\partial v}$ + is easy, as its matrix is simply +\begin_inset Formula $Ad_{T}$ \end_inset : @@ -5191,76 +5133,139 @@ Applying chain rule for the translation, \begin_layout Standard \begin_inset Formula \[ -\frac{\partial}{\partial v}\begin{bmatrix}R & 0\\{} -[t]_{\times}R & R -\end{bmatrix}\begin{bmatrix}\omega_{y}\\ -v_{y} -\end{bmatrix}=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times} -\end{bmatrix}R=\begin{bmatrix}0\\ --[R\omega_{y}]_{\times}R -\end{bmatrix}=\begin{bmatrix}0\\ --R[\omega_{y}]_{\times} -\end{bmatrix} +f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b}) \] \end_inset -The simplification -\family roman -\series medium -\shape up -\size normal -\emph off -\bar no -\strikeout off -\xout off -\uuline off -\uwave off -\noun off -\color none -\begin_inset Formula $[R\omega_{y}]_{\times}R=R[\omega_{y}]_{\times}$ -\end_inset - - can be derived by considering the result for when -\begin_inset Formula $\omega_{y}$ -\end_inset - - is each of the 3 standard basis vectors -\begin_inset Formula $\hat{e}_{i}$ -\end_inset - -: -\begin_inset Formula $-[R\hat{e}_{i}]_{\times}R$ -\end_inset - -. -\end_layout - -\begin_layout Standard -Now putting together the rotation and translation: \end_layout \begin_layout Standard \begin_inset Formula \[ -f'(T)=\frac{\partial Ad_{T}y}{\partial\xi}=\begin{bmatrix}\frac{\partial f}{\omega} & \frac{\partial f}{v}\end{bmatrix}=\begin{bmatrix}-R[\omega_{y}]_{\times} & 0\\ --[t]_{\times}R[w_{y}]_{\times}-R[v_{y}]_{\times} & -R[\omega_{y}]_{\times} -\end{bmatrix} +D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \] \end_inset +To compute +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset -\end_layout - -\begin_layout Standard -We can apply a similar procedure to compute the derivative of -\begin_inset Formula $Ad_{T}^{T}y$ +, we'll first define +\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset . + From Section +\begin_inset CommandInset ref +LatexCommand ref +reference "sec:Derivatives-of-Actions" +plural "false" +caps "false" +noprefix "false" + +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\ +D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1} +\end{align*} + +\end_inset + +Now we can use the definition of the Adjoint representation +\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$ +\end_inset + + (aka conjugation by +\begin_inset Formula $g$ +\end_inset + +) then apply product rule and simplify: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}g^{-1}(T,0)+g(T,0)\hat{\xi}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ + & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ + & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ + & =-Ad_{T}(ad_{\xi_{b}}\hat{\xi})\\ +D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) +\end{align*} + +\end_inset + +An alternative, perhaps more intuitive way of deriving this would be to + use the fact that the derivative at the origin +\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ +\end_inset + + by definition of the adjoint +\begin_inset Formula $ad_{\xi}$ +\end_inset + +. + Then applying the property +\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$ +\end_inset + +, +\end_layout + +\begin_layout Standard +\begin_inset Formula +\[ +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}_{b}}(\xi)\right) +\] + +\end_inset + +It's difficult to apply a similar procedure to compute the derivative of + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + (note the +\begin_inset Formula $^{*}$ +\end_inset + + denoting that we are now in the dual space) because +\begin_inset Formula $Ad_{T}^{T}$ +\end_inset + + cannot be naturally defined as a conjugation so we resort to crunching + through the algebra. + The details are omitted but the result is a form vaguely resembling (but + not quite) the +\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ +\end_inset + +: +\end_layout + +\begin_layout Standard +\begin_inset Formula +\begin{align*} +\begin{bmatrix}\omega_{T}\\ +v_{T} +\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\ +D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\ +\hat{v}_{T} & 0 +\end{bmatrix} +\end{align*} + +\end_inset + + \end_layout \begin_layout Subsection diff --git a/doc/math.pdf b/doc/math.pdf index 06dec078d5fb590fe6dfe74dcdc13aace1a22328..71f9dadc65bea48ee1dbd925f2cee058bb2bd832 100644 GIT binary patch delta 26953 zcmV)OK(@cnk`VUI5U|n(0XLUHE&(Z%`~|*$Q}HnjFePC%xgxL&QgFN4hA4*0Qo>XU zNg@3lf4d`~HZ0=a{p7H{dfFQH#g{jHihG2}>pUy*l zs=t7}Ogou@pt2}voL?YCO8?qF?i5~Yug_!MG=8)VcXp=T>8{oLC@>uT=v23MmAZX@ z2(Iv#*0h_ZZgmyR)aa@Yo9%zOR9*C(+5TJaQjdn)2Y(%o;l{yqC%=i3rkfzLK!tOV zKpYdlm8T%dRtl9uI3pF{e!k+UWWh{7W6Xk_N@tWvFtyqwl3(YnXNO#X5uFHZlc+VA zfDmF$GO9W$Kp7Geq5y(VvZbEs#1s^NV5>yzb8h1WDq*&;{4uu?E1{yq&$z(3X*eua zMxm$tG>o%k8j@35AX=7_5YDbeFbt#6m)ta<%_#7Bw0i19yQRc6G|a*iA#812k`N0& zWt~?j#35=Z%*Q7K5k%q!Apm%RFidQdP}9z+T zsTx1;scP$kY4-k~HQwW~-W~nlow>v(+h8q)=eq!OpW^QCnRHPQn8nJ1_7x=|5)`qy z8l?b>xdqm+`*x)M&b50ejFO3o( z?eb!6Iy*0Q?y=MBR(H;T@Mkn&J`f;+FFhcHkZl$7=ASbEo(9Zs@b-Q~--qEdpkl3vx8op)=XSO(mLe>?w~x zwqhq4C72_68Pfwh6M`&%D#y!uIclwQlfZgd-Kst^_01crk1Kre6fTTKN}EgR4Ge+9 zYy|K`G2iu$;Qc54gh#yf;MP9ecb!IKUT-8sams*@y{5C{viTyo?fj0Fh(D=;EJj?&nErM%5C<@<0mQ8ib@S|J@6WpW3>}p1BsmR^hd{MGqNh?`tHTD7>0#T$YnT38GMcA?Vx--&Z?GQd`Ac(^w5% z{gh=RCA>ST@t16cWXw(uvB|C&-I5lvN~HSB?iicOYIube+Y^GB-tQFkRi0PbPIH`w z#A)!dY2w$j2N+zA2rd_+rI>_R7NA{ITf9$Ksg`@cI*mQFU~2vQe2cJ$o&b{zN!H)!CRR~vgU4Wtlj+<@xkIRbs%h@X`q|&u>*?UCG3RK+NJgln zNCNK85XD#TaA;_8wu=X22NMplbtJYct-7}$wc)&8s8Y9$0cR`Vm4 zr)*zg%hqpvB8$@#S?b#@g%rJp&p-=sxtgSZZugh(%o?svbF*@mZ)8*aM&+cdMCvJz z%~;a^qf=mx5r^T6Q(%@W_5dPd6K4_LcaV|M#_uG1(}vMD;8L%LVf;EWe$Uvy`blg4 z0VGu@X1Ke4(VYp1_+0B|KU-q_NR%E>gKg-T9<3{7{tdvu%9op5+LsoouN@%5926CQ zw&za2!Q_IibjRC?A>wx$@$x&7 z4;vt+u;hcM4wm}p;b``ApxBzrcxXCrO(=}z)tV3CFy{W_NYF+QTXLfjujqszR}m?J z_69O%%sw(R#4MX(0I`B6=Y}8tbpPQ${b;X&v&#-m)RPb>6tgVj(G3AMlMyHr12Hf# zm$795DSuo^cjLwpzR#~vP6nR`o!-0QygAqz&wI`xhgi2H2S|h@I25UH@r?extGb(5 znhg>ZNzIJkTtomWqL=z=ujWtJZ~pqhnky!WkeXjz-(4AgWvC@os;lei)xR?)2%qGP zQkuQ5mi2_u?05WAuG{Nsl5<6~#hw5Bar)b$o`0_=|GoYPyj@)}Mhd0e+v!!#1<{4{ zFaP^uzN)9y(vudvODYYqrhsoa%DdFt4AIJLDSk-3M3I7tEyb^s+-5%qGTSAC^>ei) z`wvw#HJT(^H1iRu*T87TbY8;(k?0a>!Ub!IC=X-N-h?QzmT#&4)lA2dLTiZ>7G@)1 zPJaUC7qYV;&oF+y_fdpvejDMJ|7+t4$0pAcB=BXGDLvuz`ONB)U25S+- zF#CcP$5K+j>^f9rk_&3H>xXKK1kfT}f#!fes72g0Q(5@GM~ukki*>o);srUqsse=| z61Ooa@Hx`}LJ;PhTbOWI`g_n|pqN@jb+%K2t~0nU-E34BRxKqAYZ9pH?vT_hMt@cJ z4V*~ZXrt%EWQ2put^(;{N;oZ(o-Z8~gHLHOkc=OagZ3TM)Q%e>TBY}#mRSHmZ&f+x zY)}ArBY5uz6p=%}wK`a+q$(lC04`gqyX1Cj&d$l!RyInj!ZPxYtJ~ayq z%3G*cXSZSO{OqPf>~1Zb2L1tG9D}5{tIWhEy^jf8R6}$K7pZj}R@1TN72|vjzT<18 z-DcisY-er(5>>f+o@kW?gp}cNy$%~UnY zLr8}zZTL9@1{>Qq{HOVl_Mi{RUc-rm4Uj!2^83_a5Hb|1RWCW?A~`5-yLq|iy4P^C z7czWjy?usvBC`IH8BwYLNmF`D$%IV$xFF>mUEGH91VKZ7@NfJ`NooNXojBrBA;%cS zH`brI)Ycp}{{77>h%QF&oqssz-m$dNhyyDFu_@E&T@WSNS-b-|#yb#Wy#sr6a7KrM zvmy6@eJ20FUN9GN5e*V~q}*u2I8Krh#o3nB(rnT7NULNHuK?bfL;ff`cTD)rAHAGv z*-flA01y7>$Lc%GEYD{C7L$h%zWAGME-@LrfNpOC+xwv`t^Hi?-+zH1+z|nr`s1{I z#M`8xy31&?v2l?_GnWdpZJ(Zy?h545aCMs9iKC0oqA>XfZ2YiXJbK>!THUVe#oXV3 z2)#|)8cayTD7nx?S#^x$^E5ZnL+DW5CnU*d??0R5_osq>Exn`xIg||=X%>=b1J@UK z=u}6rOJ^yeR5|;hm4Cb(AjA$JfK=F~T0;|Kg2vX3ePRiA=AI%4DYRb?valw^FN`cP zjI7>f_qCm-;+){>&G9+`1WTA!F9bsD2w8!pR+tnAhbX-6p9?I4e1xqL5-gwpye;Sc zVzY8jRJMKy{|W}M4w@96xY8?pHYD2`th-rOPs?gm&DW*-Mt@=pxP?f@9G$PZPa6OZ zwgwAN?8T9c?He!K8n1T)7O?dY@6**V4uW)Qcr^r?ecD6F6ES0mvA%{YTWKu+R|c3Y z=D%(3C$5yXMZ--pCs!CEINQ5R$pSVEttFwX0M>&`Duc!Zs0A}#K4BKnT|O-r)6H%0 z)0UfA=nN?5(|=CJu)cdvrtkn@QmS!Ae`E4(v@PN-h&#z7 z%s1~)ev3z6(cYvnhxq98j-|6742)%t8_HhU`+JFKrP645?^S1Pd)rCHH9(+al4s-V zEU%r5+kex%IdWx}sc=Ogw#0xz>Z`-iUe4=fK+Knv0Xg2yJ|P7nE@GsT%FM<%NKOsR z8hD;cxIoEBzCSKT^LEBu-G~Hmae791WL2K;tpe}Q?7>EmdtQ17z=YI<%X*Il4N>QlDl=_0@C0(8qd%IW$ z6Rxl+ntzq(jF{}RCQ9;t#d%xI(4UP=cYm@8cG=Ug~N8@^kIX+uBl2yGOL5^!8iUWNb zIX5)4*oro>@}ny{n5>+wtL40Rpa*+7B+Z_xeVwefSrlh>hK{C?ac!v%D*BN2{$(Ish(<&%g(|t)?3nwZQ-Uw^(yv!9R4713caP;|7KE0290yFmpWTasYA#?fnAB zwJ&y4_$oLF2Dr`kc7Q8z6#->0R|lfkBZFL&-Fko0P64u9Bex}V+tPKDo*pI)U{Kj{ z0OpJC2JcQnLQJ3sW)V9zQh)bZ!b~g6V?Wo9ed2cr?|4NkK`l!w?Ximcm^G@cmT=3NI3Eq;gx% zVH37f3o9$vn`Jq35&^f}-)+GSSosx}{&u(d5SF}X_RP(WgO@8XN`L8Y?Xp@emKZRD zXrM%a1Cre?H}l&EAB5oJhvi~(|FFHO{6#stuWptl1Tcz9|Dz)Kcv`K?dbaW}a$Lfv zi^qEHenJFqZYG>&n|1Gf*c!WX3IQM1t1yQDxSKGD;UE2t7wG#<<40`U)#J^~ zXD8fw6-Ao?@qyoGxPSQnre1Fm0kpT(Ot*TJ2l0k^`_N?8}tnu+A9e=vpz@r#_NNK-`Ysx7d zAaVqvEe_%k``FKG;=0A1Bas+|{S<#@47U|ae)jbOhOd2B)n56Q1-37+L}STWu1WcluY}x_k zBKM#21!kF`tTFhALD?Bb<&02dO-n`xG+zPwm}6y1$C@u%hXy|2Z-OaKc;KxM+TQ$p z{pJsjj%^~7vCR`0H8CJCAa7!73OqatFHB`_XLM*FGBPqVm$795Dt}v#+cp+{pI@Oz z8>m=bMY2F2Qnbakz@lwB*q3Y`6k9XVVr;pRok{+EKVD?(LVKLfG!MlBK}M!%@?5_2 zopVTkcYE{ok49%q5+Mto-QH!IXPO#9rOIyC*_#CuL@aYgDP8=u?e^ss|ElKDEpx8u z;y+8gSyT_rs@{KEzJI;_8Oy1RF=CbS%h4?7oG4a=)qY$uwP;#=xv-12zOUDK)N~=f z4@3L5+;(?OyD9f|v+cr)mJe5Ck`W~}R`x629_JWhi$c07hOnZ@oK9iR*3E$+L^74- zcx&Q|cPWt>B?U8?AV$V%cWtu?@|8i{cg^}!7`SW8O_dVU3V*`6>ZMF>WC`!z@0-#3 z+j7b2;zNDER10s3CpKeL5v5qa+BUr#v$mh{ScQ$=hmD+9s_23D@uBwK&V%WMR)kuX zco>bBb zeWjLG4!#a2P|h7tDK_)-zK_$)NE$q5c|mj$J@#$VWRes@WsqFks2!y&1v6+@Sg&J0 z^IY_)gmNzYWGei|LK~vFb0C+xmqyW)G)`=D4Gp9OQGZVhqKUA@M3CMrVz5n+0|7|Y z(R}o1=+nFcPe-2~aM*8gkj?LK^HD>tP= z$euFHF&3(JGm$KWaM-D7+I77x_f_}RScXyR_J1lcYR2HAh@S$WEC^$L;-Xw8$$=4W zKs*+;RV)z_ROf04F-XoyQ0E9+*BdaU+ObU&77RI!NEI)92-Z3fG*CzqM9LfQ6pZR3dVam+Pk__J%e z`b0#&_iz<_z1cl{8qAn5+PZ3Mx8?`;ABZeG$IcLE}pmnZ#_S-)_Z(`A@~@MGDE(KN<|nH7p6k%xLFy1Ib7`Qi5Fj~mx_K|Itg3V*7i z%s}BnK}D<0&6~G0TjS-=86^^)ee_c|8D=QzzW0#*e)HSScdic~tH@CTD&#q2LeP{R zXqYoDk44TnJkg|4rx>SfM1atC&VO{O?4*H3LOAw|uhxn5jZEuv#(+I;Q({@~qQR{5 zQmMIA)9?@7`R9^1dr9_IUyQxwwSVj_pTXXVs0DK(Wd^0Mh-j#x%O8{UR&o%scND?SukGv8{+! z&g7=I3`;L#4a)|z&Mn18Yw+u1_oa)WOfRbo4+E<75 z#MTIMq(W@Sd<>sQK66J%?vO-j{uxokllE}X$LfwZP91jw`%D7_RDYsmGsxlc684qL z@0Xy@jPWNdA#qnfMi{W^IpD(JBBNdccaqglqL<{IpS zM9R4jpjnwc-b(9ZGh@kJ_L!HK1@o70ynUSLJ>D=S_$ zt9XZ3IqnAr(zst5_}hd2Do4euoY&R9Emwmdg&Zvqg-DpJL$krrd=Gf1Zn3br(Ln z!<<97kSZVixi4OBnl>(5`HM5h@2k4<_gHSPCQMsbU40)f>!cw@^6|-71L@jjF1TH^ z1IMuiS>{R>i&x=Y>`U^dU5DvrQSNqa^ReEowLP1S{= zZL<$Ue;n$4{O;-y$7;}EE~NEW0AV0rJ{y16n>()Unc)=5e*!2^j(L-@%@Y?jH6Sn` zZ(?c+JUj|7Ol59obZ8(kIWUuv94dcWOLN;s48HqU@Fk^Q{flW|2 z<@4=;%1;$!?O`BL>10${`oMn~Q8|IGq9atS#seu5idADyNnR++=(;W^D#;g%K67F$ zJxCQRj#)sV0E@kKfUFK`&mOMA~B1i(r2 z!iA8Gs!KnYdhjAxtK%{h))SVL@ZJ`_#p=^)62Dn>%x*7wscCMH6-a-#;8ijT!su+} z8K-+B)t4%{=L7|zHXuJQu~|JKDiu%Y0ybkz!vIb=+IMB;9fzD~$pDT-l0|i{4>=qu zW86TpdO;%$;So@TLV=_hGgR@y28)p*=_tuGItYh#+%&bG0Vh>EwA2pU)qX-@FZjWl zJE;^=hs|?(9|nru2t|J+Lpx|4lMW-hw?w8=wJr6-DR$vh)p0sf=y`C(xNg->bm`Yc zSHudVbpA*&nn!8!yL0h(x9BLIN?EO$<gZ%I?&yDaa!22XJC=)bJtM5Sy)^%*xYk97ln$M6d|!Eh?~C1l20xo{sl<7Ff2-m@m4(h2L9zT+c4kbCC&+x4O;F{#D#SCd3X zHi^41{j&))vdMOqg8Q-*Y@c#BONj;-Qn{j{YJ<>Fy!Jm7$xZL0q6KSE?%=dWNEaAR z>tu%-iKl-!a)(&fR19kPv>%LnA&^upa#(c1gKeVw+9XwV#U`DZ-5#9TwH978bkggt z%uWGY)0ud~HZ!}kDZh$z(wlts>HKeP_H2vj*KCV~=V*(-bGAhSp&hqB zlaOZ8-*}~-`p{a1khNkqE}#10(Z})8KS%NSBr*Y_ys;3{R1jIBXcyN7(p;a%vExF^vBVMcJC7`k$);*H9|@qHVbTY(k`J(Cas_iuOg3-7s3Tpkd zW9FE76x75Vx`QBUf(Owb??`8QTM*QF#ZV7|mED)3BCFiO2#l~UmW=TT&z;>GcE&a- zYUo@hBPXDTOKXZ|RKS?JOE(b{Y-lsTJxvlo7=rI}n4}cd!z7`XNz`i*j#kT<52Alc zQV~F(HKPmhN#7A(;-D!|kwWnoyy+Q6HCZmsg(-|;|1jFS?^DLBlMd2g5lFk%hf-^7 zMPvvcHVQ0P{-gm=_YHvhFmD5L5$g=4R*Dn$5N9hTX$mOX{UC4S17I=t+7bbwsqj{# zHuXnHF;Oh#t9v69h8hSD802i;f!TjOU^`<+7u7`48MggqBPqr!EeF{+obkSC7HsU= z8|M91`tr-Xr-uj@#mRfwz#mL$y1$MrubI!+E3slE5v+-cMrnJft@guou zoYlMfYBo@wc4=QNdQrN1zPP=auiE37$_$H}QczLgN!c$jqpG1sAI5uCIhcBC);Bt z0U!;Nv62+GJ}v>y3X^eA6Sp5Z0YwUvv62+GctZh;2>~{@1Wf@Y2>~^e5hxV5K2`x- z4FNQh5hxRy2shy>E)v;T7t$5s!7Q?W7)|U=cB3m2z~_HXmMM)Hq2FA zAy4Vn2F~OSFzeQD353?1I~t|>4_tjEjeNvHPFSvDk(sLDbBr+Cf*B(Wc^OQzByRu& zqF@j8WpQQVmotX7g2o&)Fn2Z=6EbCM&KV4g+d;fh+d?Y=MC`ABz_0;M+ETCt9tz=% z9CXm3G2j3(17ycsjRQ32Bfy>&{|dwvh-IwJs`0|LZx#O(6GOX##o<-+c@l+^TvRB% zB&Zha4uvtC!M%vDJ6R~|5Kje-=8YOkmbP#Vm5fO-kE^i!>iT-USu9`Oz1?*E@2{@kFP2}g zZ+>0fyh-p*x>$a<{O|IcH-t{QSS){Bz1xI0q7n@r2Bkz7S}a9_bXHRYXhHW?c=jwT zUxnqj>(}eBd>;O@`rqB<=4yQ%bCfT_mtQXMcfKSn6k$hy*ixe-oVCUbe>JjL+n69a zqafB|ahyB~QZ2$Arpb}#lLH41P=i!aA2k9Tf@~5AF z32+V|G6MJE`tHMri$OYen2*xHD`N$edvq_>*PBkj7akjPzW?F{e47!(?mO^HMi6`G zRCL;7$7Ak)XEFo_M9KRO0VIgRkFVet#Xi3Rq0wUxIF|I-gX^-Tt^)y=FK^cGUag?! zuzdOaMOeOGecFVJzU5wC{=Ql)zri|I*PC09mHxTg=XQN}^KNz9WtMK}ht;oFmtU_x zh0dY~LvUh-iC$jb;9Fz`zNhQJaEnB?Czg`{P2{A=l$X@W&)FP6pu4Sq6c>* z<0u|fYmK$YaWKiOe%|0m**;4mfHl(7F+sljZN1)LwpHZ$VWQ{p|CA!fEI5lb@}4!aoNNsp zx5k!cddk>d*u<)@A3*!H-4>uvR!!SiO*vULbs|vf1fce5uqlnT=21F9iZoP>qjX4r z@q)2YI$~5K<7)u?qwc2M0SD<>z6X$gxEl0{VLuv1%c%gdec!Q@eaCbfKzeiz5RQI~ zF-9k-Vl%4hr$?!tLVJ4ed1=oG{XN(DQL60c44arUitg4gR%fkSQj6IS8&R*Q@5TiN z$$lJZkrEx>IKhMVisUjerIiUOZHLBxw^Gt<){>%EI1GB?EFjpA$EBPKg7Q$rKFB^3 z7*JJ5m(w?^ex|5apm0AbBJ57u)4*`%#tmhOb%(PSO??B0n$}Oqu>q>}xFqcWDktWz zG9iEM&?rl!3B0NGD>aMn64E4 zo<~yj%T6+l-L0G=)a$q;XC01zqPZ;zrRX=ObK|_#Z&D}hU_ta489-kZf-9B zZh8QGeqlQWe0@4#is6!6N9jc1s|U`Vcj=h(!E@|gI%+khGR}{XqFd3Yr-}Z4`$#hA zL2@90?(Xt?TZ64zxKqJOx(8O`Nnp)Xb`q`Mv`<1pkIutTi_K)54*A3n!k(U< zLSPq^5CVT`FV8Cg@Pwfq07g(06zKtl0DhJX&;z?bf0rgi%n2}dfT8|$fDm?QPcRY+ zxHdS$Y@u+}HN_ncfg%Cdw*x?$IskoFDEv>b&Yu7Q!0+Y&M1(~C4)>e=GY|~^iww54 zMYyIQ&%**XaSyujGo z75ab6`OAD=gTJpU!WCe5T?W)2W(U3gBlbmsJ)i(I(jDsW`=5?~pNK_701%ih8ejvp zhrx;eiGFQ{+WmRHo<9=i1+V~K%TELV{Q3L)&FWfV5Cq)W`ycqP5eqA8s4AKn@&7LQ zZ>N$H!VBOl4G!fPcr*LtG0N3gG!i=axVTpzZZTW98L7EDJOA?Y{0aDf z{9qTDv-h6}Rd;77^1lJ5&_DN87Yc#7yZpCT6Aivz0Y$jI^WW`*q10erP>2BxZR>yV zr%Ha=q1WR7H&+#Ii-7#x5>W|B02qk`dlLh%r64LH0q_;MUMC3D>z4`v!a{Ha`kDf8 zbw~RH><~!epCb_$2MAvqVb|Z7pAO=G1-EraBCl8E*Zi(;{_Ff}AE8h$s4elrEW-9~ zh+}O?`)Rczlc(Uu=)Eb<4b!K*g1&zX$PV{&a)K1zs;npPktd2N1FtCGuBq}I-(O_? z?z`JkPY~XkWY~7$cWIqqG_lb}JU>p?H}-s2vF`P4B4$D3`-TWQ|IpMZocWH6v zy17e}8|2fRdiJV&)x9oV8V;M?Fx;$?)F!zsofM2QiM9+Xp61N6c{anqj*ov9yiLeQ z{n?9R`uJpuI%n*PRV#s?*nclZtkBndMKtzo#%HY2Js^u-#o1~Y55^{wM~+zM;i z@M@d)c^OAF7~Ku@O4z}Qm_gaDERE@pBR-w*Gi+Igk=fgZ{!OG`VEh4X7Q-ss35HzU z#INxZUx6+O{X)7rD)ontZWXRNVtuNy7#!IBAT8=R1Tj}nUbDMTd{-EUmT@_N;TA{F z70`OQTalHr>1(^xAb5X~mG&MJd0>j<#>>nYq)^!hS1RQNrznn>QF1{`0!rn;IL1{> zp*{EBn~(hl1~3^%i=yH{>^zDVVZ$Lq;jsC5{vaP5jg3gNJ5)EO__BU1f-pv86uPpV zl)QM%&JNs^fPKV<al>n$%9P*OmRzv}Ni*5L+G5y+-|5mUDB*Ptw`QGM~owRnn?3bV)(5 znn;VD!c)_I1}c9K3fBa+MP^(nR>b>Cwz4=nqVMob;^m3qp|^L8)5f-;;y0wf-zZ8T z$I%gAZ4yr%UPBCKv*EK+@o%v}=t@$0T50Yipc}Ki;old;LyFZD($?&x3HQ)9@ISoV z>gF*9QA{NJ*s|uOpMmN*hV73Uiwm~3uPhQWbyv9b?{0r!*7&&t?zhh#(p^c!^2Mo$ z;t3;1iL`zAzsy*(-V6)=I^}xHfvDk8!DEMoksqJ1OwA?%Pk9@=G743kfW}_9sk8Kq0JZFNhS-RF`PwI(TH?ciHL_)TJ z)~17P@46)V1wv3DetIqz-Dr!)OKHXh{{xHav*a$y`C|&pc2mMv%&RIGEc0|}D&b{iV7@TDX7ulw4}WLx)sti)t-fl^P;TJdwtUrK(K#igdK4r{gWYjr|0&8boaQjy?@To*;g+r9ohQ zn1Fw_>-`h_5Ks3odwTb_JM}VaxIEfWvh_#;d|5m%HyH2q)Jj0!jM^aGBc&#Sgj0hf zFQ_0{EH2!)>LN-_dDC|8@hwwOh(z&^uUKJ*J*nRlM>KCst*F;UWzCH67J6gjCMvC` zHK`Mb>UqXl5C?LIPi5)APID|7gD#0!dsu(YRth3=VoMkNQRkX{{JG1aVHuCV(}X4` zT%ihITt-l6g@fmu1!M|5+lZuPt1@ZpbFO?1C>}5=H9s$D=$cKsDS%8m=ems@a3S=_ z#yUm5=dtMS@FJtI&9GedbYMF0QRZDk`Mw-d)lS><-sF4>nr^JVni(h}{ z%Lca04PQN5)hHNpIyMNGII7FN%jdeQKA(B|p)Kw(51yW0l_w@3pw6{)@OAR08dgah zbMNAE3EqB}H<7iz8I8DMWi8`b8`}FV*IK@yJnGOQ=*VJRam9zjHPhrEu4uE|Ot(dl zAD7|s5D;Q`@TE$iwJ=fKeKuMp z`gnk6d!_EuY(8Ju9Volsf2>)}5~ZHls3Bu0mZh zx&>K|^+h9df=J)g+R;^7U7OJV&~P@2UrJ9lLHGhf=oQmIW?wA6^wwV5(58PBI+(%A zT77Qt$M+lOYdkm}CFW_(YJCwGT{#RxT>PfSE}wkVRHpMhVz&eC3cip?(0&J(@>?V( ztn?quSB)!<=JjWd7|@@xd#j7f{Mm;rixGEsolG&yxOND=6&cjZy>7iV1%p)H?!|gb^mvB;Fh7l`#*p?2<1`ksQ zX77_%X;4=e5;(U010PV9yJu~K9g1@HH_*8Fp5grc6KQGz>vBkgz3U5OMVZDLX{xT= zwu30&_qurJ*4`fg_d=c0fHv{`;~9v8-X-VDVaEdqA#0#s*~$?&aDIP}W8i_Tll)_* zt&JE3?PLCXVl>d{LI72%EnX-wB5ZKx*juriF8-dpy--W#*YEP^sA4|B7qLbroeiVn z-J2Q?m|JIcPT6c&M71OOp*K93X*C4qSH@3S`)~?enfe1`$jusRitS8T=?NIqdUZ_Gln`xth$zx z>LCW#bHNX`H{1spSBn!<%`##BG%D$TzDw9cNgeLZt95Le3`l>|OgU#Y8V?9bNt83O zgjHR!@{4}~Z^?EZTzv%>#VV}|iRA)A&Oh$DJZkiN5LK6}OnQ8>0Ph{Q*e7`^0$;|G zZ?8N;Du?QIHgBlC0WTG=*WRy1DzR45l5gsw97&?CEGz!=bRxV%dr!<6oYn)_gz!T2vx-ufUAEOUcTq3enV_RSoaIP%Ok{5ke@#Bj2&gn^>-V}R zXZJbZ$@BEX^Ca0?iUD@QS2K4b)T+-v5KU(6L^cvr3}*^QV2C$g8^3ks!?)|gsoio7 z_tP_a$Pj<^j!;X6UV#)tWc67TWAwUKjRDtqc)Vis%T@&k|C1v&)(R)qeHzl3u*SNr zc%8)2d)}w?0)mdBIMWUtcm%F94HFni6WIbOwXmJ9+=}Ut-IU{|>9N(6a~PQ6pe@&Y zE*+~y?}L9DaMb2q+6P(-&+K=l4+{6&G|*oo>nwl6DLz?oYGmX_?DXvX;ogxI%x&!} zoB1lIIgu)GNBM5D{ghK^FB^bmaUoO6DFMb}v>UN;z2+vues zuc%L@D2+B5+4*xvBha&Tm%^362C@RBbvMDS(d2eA{2y6%rgV=r?tWh+n&mUsna)95 zKGJ`5vNv9#H`NVq@*o^!))g+;izW|>;5wSlA*Htw=-iL9_M5axV z7GxX4mu0I3+dGPfn1fKBTC#i7m(0{nV|8M=3EQxADGpoA>5TI`3+Gg5pxk)uUc~12 zMSDNzh9|tbAtY)u2KVQxnS&OjD;pwSFxP)CXc@LN2J{L?rD-t|u0be1lPgpQnXZ&^ zCRM$O<5Mz`m{#%jqJ3Xp={(wcJbmg#b?nYZ_qLx08kUA4qmbTLmY}LqvzH|qaINX3 z`=Ou_TvnnJf4n6op838`|L~!5_uG8*4SJP`v5{KtS^6L&_0`S#8;R7mOC3!!V$@PlEts3M<`Q;^6P(B<>u{k9Jbe%+{nq|R^LFA+Gt|uD4(yii`vn$ zjJ+~!oW4lK1?5ouFr7|oQ|;!Y|;W9p3zyI z2ChIJ)kRuxZcBX5#fjb#;?{XuG+gcDhq#6J@#Op*7^)5y$mKFM*mzY0I$-+hos1hs z7)gKZwj0SNWX@HVjaN*9vN3=3a(x@B_|XJ~!%Jc0gtJGTio{6bOg@*Iu73LIGR z5j2AcJUbQYJgF46invtOswAE0JaG#pP)}VvQ7!k~yh%nxdQLT~db>8LBygd#@ z(ShZhp7oUkM`G2Rt!BL^f>X(#tZ3%HRCboKyCmeJIHE=qcXXm$QIq5B9O(YCr)OR9 zF$NzT_=~jaI~$W;?p$;>MkV1+GdU7%SX4w=TTDjtNi7Wel5G1yqmPMON8>U>UvGBZ z)QU+6ftRN~;^a(`zZ-win2`9Yg2lR06w7cY#%C(p-6OOFuAua~kB=Aog{V%^3;c=i z<~?GaFm{?)Hv$W5Fy~CE$k&G4XV0t_@^(_#fVk4J`~}(BmQv3R=}SKI-6oVFitoW* zTb1x>X%HeYJ*Ls~LSC3^%1F#qbzS12&#v6%uxL`4g?v?19YB*b?(Xg${Nh~Pg1fsr zT->=3oZv3O-Q9z`1h?SsE`cE7=iB}F?Bkw!>8hDCkJD$mtBbDd-I31_d>!yX?SueU zYNkEd41gaCx|r5{+tKB>$8z$uE-nqnF1ZqY_QThlEbI_VC}SOkUjFl^pnNy(KC*lg0ts2w9i~Lnm=> ztmZ^}y1vD!F2Jrv1p z2&M{9IQ&*7IUI|)CX?i!#dFJ<^U#kv;7ir7uY?^)@pa5HT)$8%oHLQ!X? z^?`qC4T0_@8^Vau8EMea*4uT#8pKg!tuV+GFh1PcbC_ad)g=&=&I=Q=wX$+ZJ*P(lJZexfKA}1+$kqR?RCyBPz>uvceT;^ zn!)f^zfaOuNMHJhIPd6}Be@;Nu3#++_F(xzhd<|`voQlp1Z^QQlqSe$kSB#}^43;z zPF}wbTm>NdX3)~KFBI;{!T)FKbkFAdmo+%@;!u3~DFq(STJi$u3Og^yG}#BVy=e}* z@zL*ZggoYjv-wO`JMeks2Z8%-H-Ct5{NzOS*^S&JiWgD~$f+XM?2DdRF5 zYdnwrJ?YnBU&qhWIHzE88}^rQ>$v%>@CSN-gO^PAkbape2_-NJFWdp74y==G&AmdftT&0FRXss=qtGnVN4+oF2t4 zfR7X1tM|Ds38wk?BBCtm&!RD@*BZyKwB2UAP$y+_VNe?vN(#Bj*Sb=+oyKx8qMc0F zS`m57)t=n$Nh*6c`IGgZq5cJNcYl>5G0KQkk`sKptFsU|dedW{U{BWI#g^7VZdKxf9z1W!K7iRcRcTIyU{Kyv&zgo9Slm2(64f=xtwxm*cVt?b*_ z8?g;{@1Q<Gs;>PZ zz=&d-0elu4kA94ZgKQ#E=&mbbLhM(&C7ywn;62^l>TZlY)B-tEMm%)1O@8V1$71v7 zu3sLG$blgOX%L8+QTuee3F(3C#D`g<+RseQ^aUDz*m^=2sJksRWITWV1%fRv+L>Gr z4rZ>UQf98NQzY%Cc`X9fZ@~r6X|$EGT$}clhX!`UY+7P{FY-1GAe{B;@cW0FKH(S@ z!4=`7VV0cdI82N=P3F!=X?Yp>xhy}5#B&k_pmFC}a#%zoGMBnF-<{YU;Bp6v$xh3B z*32)8(u`-D9po^|EdNnd1r}?6DmPJP#(jEzOU))=h*CVifIQXtQH`-*vO)c;dftGg z|TD?ufWY>J| zDTAKpQ2!&t*}(Rl!4ueGSwA9kS^xL&@Nr|l_{o=-Z)=zKwJ-wt73_$=+_Sb&B`ihf zh;i>57kNpTRI<{*ol0c6qT`2kaVG?Y)J>2hU_uA1SSV>;dd-MI;2Ac=u;XKq=zYO+ zC|!gVuCxNzi6twjRXwZSFC!lh+deT^KfPN58n12W7)l!{9mD*SWxs=B#!i2#g2r!igN2qAhT;LF zWBzAjK>`{`4#mUW_KA+aei|437mHR6T`ma4!JnLHOP9WN42=fKmtOy$*&#uF^$qFe z>o+8!D{WSlZ8lAPeOR<=Qe)_$h8$N%R0`4T4om4FZ}7X9l5w$9EEjwAlW7~4OpQ~; z%=@xTP;n_mm_;)?d!x9HLDT>eoR_Xj3BeeJi(d^vvC&O% zLp(QWs!D(OP9iynD1!VZU@-IP-~eU(JF%RwK)_BEr=vSOQ&WG(!Hi^wj zu|a<@WnyV*hquB0ocUnfd~?&OUj7Rulg&x z4TTC5mL6)_5m7_45!T{#*t~0&Jm@rwFjY{xi}v&b5VnCCn15^F5rhX>L+ENU=%h1P zo+{qm=fsCaus~I}O zl&Jz^PQ+#vdAuQTuil=Fz*q~|0Yhl<%hgCCy+f!GZcayWSFNQ!o? z`iN~v3Y_*}mqljLV@CLh;t2--1$C9|GQ7<38LFM(iCro+>|&DGZ8)w+^Ff?*9jba) zn^Tjh{BY{Vb9L2s@`Q-qIUOuo_fPt(b_NX|&p2Be9DwLKt9VxRi@xd*yc$eIWP_BE z;>bvk!-k#{6+h~A-OUb z43ZI^T3G1Y!fdy17PiG-Dl{REB;`aZrOr5kJ@=YIH!Art-nBO3rF%fP_D4_h}f*Q#f3=>m9 zpk9|a?a!4$HJqcN^Q}HKU)PYIlj(~-(ey6S3%s^;b%Z7;PG(_OaI7Z*&qBWyWyj;W z*j9czkG1KPbCx$*sSm`@D<&$^AD0gHxm6jCT*{RAd+ZWNAXE2DrVyUq|J_H^{L97S zS8oad8mRi{KT!zHE}ihod(&R;CtOK1D8~6Z3dHd}^To-fxm<~V>1!1*nTsi_aYo(I zDIsp@=eAog3Bs_@=`w^GX4fHNb8X`B4AhYyz_1%~Dkoe)0SZ zJ`tGSXBt;0-xzRwDNCSz^~5XG4F_y-CpSlgv44M(CA@#Ip|@nOU#RlOeBq&*2ko9#(N}ZG~j9{gV$4P zzsKTdB_+4MItm`lLY~idj5rY;KcmI%TbpEaGIm7!k zKcgL41?y6Uu#d-*pbuNVT}fAhlZH<`>AzWAbtY`+;SC7fdogp{^qaMqMDsa#@tpUs zsjKuhPMsXD%#xv88Ad zJ>6&cZq4H5d;L>WH?gyd6X>E7&88l)Sw_`aZB@kT*OBpy?hwns-bJ&vEcwxqHD^<* zv&qZ)xJ)D%>|SO*fQS`F{bw7qHdflQP1^kCate3y?5S(K`M!9FeB$uIDTlGI4N(Aa}?9gtyHJa_%AXx z6|;T$-?gTzvSp!f5Bw_)n5+H{Z@TMo_phihiGnm)wI8daRds65b%k=OU9oR+6mIsO z7tr%^Nk9DWtQKM5a#XtN3ZBOqJjAmyA<}aYV@O@ZYp|egGKqFdVRk1yes9!P0$mU~ zzU#~wgGtB(1p@vYU4RsrG2|G^cYbu?rI)ESXfQD*i}UIojs%=lcGezivMG2@d=u&R z{kyjmPcb!D9^1$Hy4uZwb`;^sRPnnqo#7&Go*`GxZEd;8AU_9dyK8@iTfTT?HLC&PBr?YI6eW1bjH3Z3?*1JB&wDf(nZQF+Okk}FMbk^Q^7xcDq+ z&UjG|^qmIend9y2_5BsXgh%Vi^syllM3l4xjt2|El6c6Zk^gv&lLQcYNGtLCO{T(Hb zp8@ng&6Fnl+@ssn^tK_jUzH#W=7%UiC!gNokqn*gT78>t1g$|>+nkL*RQB|+rsos3 z+jLYHglpE{_e^s)suu!%@U zTHcy17Or9w8`Ko<)e>-92k#KXPa^Kxfs7wUJq*TaBI2UmjN56~6Hh7((-iH(&HZ6> ziJ=+abI$KN!#_TPXj%RmH#~Y0h4AT&nxb7;0RxPFTE?k;me=H!YjO1Y`~RD@-H$Fu9G8adCuiNYFh} zW?GaPY(qcZdk1#3S03|HfQ2+jY>8`vD@qfTuGq)=Sm_-uL$|cZ8CIoJS zly-h%lMZ*q+}j;*Pv`tF{|f<{CV6M^HY)Hw@4z^ekc&tIajA%OswfUL*8{&V4onOM zAXg<%d2QmQF492Tjc!vbQIUdWef3lR9t8EC5JUZ)EQ+BS^Mn?q0+VYCyX%%;i=Sa~ ztAky`hN53Q1XyAiW{~A4X5weDU~-fmLwn`(?9V2xP9#z`4Y{16&?$e$(VZi0I|MpT zbB4VpLp@yG6&uwMqsS@T)zO{J!-mjd4hM?GrU_>uff0zRs3RQ!>mwXu7}>4 zuh&_3ykEdxwX#7mxN%+5ecF?L>!W_TjJ5SkF74vqt3_VFJ+gofG2`+SNZ?v?Cwf*i z=Hb5$b1!=buDpP>MXeAOJM6TRCzFbg!y4xz)bJbs6m}Zy@Pn&^SL3ml170)r$WS0F zm4}^Y+15w#Qrn2M!)FIeW-ov0N-3ay@an$405bImegrEXyyNagM|S4vcTtX-s+02) zBI|Jn^WDLo`?cDEhAmCXIZOCJx*SMZ09U%gR>v&2MM z?44!+>@nvN_={OT*P|Ep`)22aF{tn@M*y-H7zX#0%KRpWup!lV+KYx~0IPSkBA)wo z5a!470!Cc{%Jxlk@}Wvvf!x92<_t=h0`W(;m_C z*i7q_07qIxN#08PkAj{r5-HlU|0@qW1C;q1ay+;W+!6J#X$Tn3gM?*c72n9m?&{@r z|Dv4KlEBR+2>Ts=c<@^oIjf9|Uf)yI8T!*`ZpFRdiT8lCO{UoBJ7lNV6!L6gNa^6L zbJgp}`|T+NmutKv`-y0-nYYEByj>fQorI$ir$DY(>mk!)YMle>#n#=P*3{8A{LPsB#gG z92T??1!?63rmk73CGIvd18gl8tS|F>IFc);87i%D5)tddhct5ti+{th008BbfU#*5 zHHYEUOw$Cm-K#N?&bei;NZ zEVx=atyWkl;M~+aHA+-^%nn%i(wIb&7$>Qwo#+RvTdHebe&Mmyt{vkx#swG7n!j{DDKS)wg55jOydP1@Gg&7Fdai~WCeszbsVw9CT2OY@rolT|a4p#=0iXo?fO!7L3M zFM-Cd5B)5`ep)h2K9)_5qQnu_;2Bh$;%Wh{5L`$Or!H!}!;W8u*pI`11T_s{0bklU4hs6_>yqJ)RD>5@4J2OKNCpuW&0#F@j*M+Clyy~!#NS^IsR&H}TQwB2P8&7uC7)i{ zRKSgiKRyMd@&SM5_DoHUBwEiP@s3#YjySZ>l@RlTpuZD$?1tkzVYRHqjyd(AZVqL3 z=0({Zf-^g_D!xBcAhQVzy?FygrjT2V(VutJZt%-w9&U!X3-Atu>%x;tZrCx5*apt^ zo`Hb&xe$%V!j?2*fPBrPsC~Ch%|<7&CX$F*5#Da<7D$~buYAb~PZgKE6A`c^`n3ar z@s}OHBq4d_gtcA4U1u$j>+@2}8h=l6d6BPRWmYrfHevM?v&rC9Gfe%Hf^0rFQBmfX z=tn7ixg3vpDoZP|;vwem#b;mBp)GOfpYcMlEfTdBiMP|vbwGxVuCNt9bFdkcyL*NG zv&dGrx4VIcB0d1}>3gzCf+3Ub_lI5gtZSwFhmh5M@nsgKr}hIWNpNnNvPG#PVuBPH zc#D<)$D0WTaf_;9zifX4X2!}{{~P2ApAa4cArvIz-XkV|w=Xac-MBl@OCUgUzCpqE z;@A7_{cce(ADyAAdw{?w%)IJJ-pYYyPlDn)Ta*bW{T?k1rV6dpEQ&`EL5aqloj+>yiVxm+7AEK7Z9Cr0xU6t_RCSx`$>pA!35!`F)(|Vf%?Rrzw=TJZ ziQEU!uft}d@GdE;%vBHmzFx$B6dfmN^|Khou0jec?!p1&*|h6H%_Mix`hBxpx4C8( zDr+8G=^ST2+HFYGpN902!bMkt(k5(}60qt~zdhm?esPj5Y&J9}=4l?2(ic_JZ^Kk2 zHr2z}M_?nuNKQ3G%BW5O`-g#mI!Nmt18Hw3BOTPU+!U-LnaW2fYF4O*Yx;_M1%C}o z?4C*vnbetlsG03%qb+Ssd3Q+@9WH>Tzc=^0Ki^kx6g|L>B&Sb>hzKF(1+n~9>dyY0{w`69ZJoLvy&2#dAp_2-VaJ6EjBa6f4 z5QZB9xx`(5tjRIgUsyTPm6neUpfZ(02cQ>W7GA&Ggz~_w6k+-nF~e(dW3~R=kW=uG zSB+n*o2S@tD^3dN11%&@xKrC|qTVE2LzFqZPK;K?tjaOe+aEg|Vqc4~M@j``k?$Ai*tyGp|y!f%M;LZ`ekk_>d~VD`AU7VlQ~G72X&kn-L=6 z4*q&X#%68^=?0Q6l-vsmtlJaICWo$GFJ&~dURFs;i8`5q-?|%C0 zio?(cx`;)m`8@Nz4|R9+kNX@0$5s*=Ri!qF6Ok=<`#^)4=5LBGOy+9VcJn2_K`ol2 zBgd1ckcueMbs`Ah?!48d?}PsBLYuS4^TY;y2_HZ1*Z2MXeo|Zf4J6XydzWGiFzU`U z%GpxZ1`8?tCMerf_L`CKTpg?+xe~e8;A{Estjw1_2L(d~<~KxQQ@8cCq~KxaM`F{a z(C454P;h*bYR=B?pCr$xO^X^5o2-+S^C#l{|45QTA1EO%$t}&xCB-8l#v#G|xg^B+ zrN!ALB*dk~#QCHpgem^-BZjE|o8`GU`IE(R8Njq@{0^j=IN{fsPZusE*Ijv}qw+t8 zTpZ{d_E-05TK64V_b;%mZ_^l}G1O=TXc!pOW>rK9Y2;|4kx3C%i7>FlW!yC)Sz%cU z#W4}Kp1S_}&+tCQT3_)WZy{2Kek5TenNTjKf53<&gwog#5q<>;wZjcSG3-f$9z${S zvA|vg+A_MW8*!9&>3oJwu;5SABZ3>;DyiUkvD0IbR*`cF-E*s2Okh_TNVcCP5^grO}T?ntpc2W4ksAWzK2XxsZx?4R04aZ6p|;=i*^SwHIu95Q|H7QVsI@8^He^vU?;^_f^lWkK z4U5)Z+pw`-i9TmJ#St0%x>r#MRn^A_!nm*-RIk$gru1|?qCl+=# zE!OJQ#A`04j!ChY7^uK6dwx#}JM9KbDhj|}XmqJxDn-3`Aj^Nx64&QHyj*NrXi*v# zcnvx-7&-;%O=@DsGdpxIhe4G?oN->7GUm76*&+9K zSNXIA6^^fq#)dr^or(+PySmEJI>WK#tN6B1-za&ef4HtJ1-!7^M@gfcApROwnbK%;ti9@>qH$iPt6;vPTP%P#L*_qh1y?I z5V_6%oMvm{TW%OR`IiltHK#Gl#U8P{!bVM{6E+bWFk_Zb#k>vqY+yaP4L(I|;*=y7 zZWZ%S+a-%j_}F}K6t@W`b!J>n{_sGaGk0B16%9qwnoi@*HMSu|LE^!%QPYHwj2$(9 zG_;imwX20d(nxt1NOXjwX+Enq>Au{E)-sWroeXdeF0EoZv&|L7=R?_NrqFj8ao8(4 za-ou*$Ht#J%AI6^y2Bf{`-#Gu#qv>7Rm@xqrBwP=yl5Jng4^_{%q=yVJWY*7Av+6xMesGk|+ z&2aKrxyDLqKGMzaWUY|*e7MDL0xHgb9?&;C`Y-M&V~Ktl#n>-e{5c22X8qa{UMU|= zn!tw+1FP8sGn&TyE(dT@PmnKc~=Pp+*!;NO+X3{MPobk{t_=u9;)LQ*3TLi83 zTwbgij~l4OHQ$jh{iDTfw5>hHbt+2?*#_1zNN$R63(&LMh;;%*!jsuk@re0L5s=1e z&fgT+x0~Q{VL>8F2=6U@ZT&K|%*%c3J|dfR>h{7QLjX@=P8gv*Vw*||@4suQuI2EN z|KsP@=8edTj0yO^28Z2UElnN&6aB}Z)Xm-0)!p0G(h7-_mz@g$K%%9UQkF*ge>%X* A?*IS* delta 19439 zcmZs?V{l+k^eq}&lVoDsw(U%8V`AGUHYc`i+nLy$*tVUy^ZVa>>(#6GzI3ndy{pfM zKGnU}-n*ZsP-cFi)XhVJawRgiQzw2v_5f(NbI4RBWK|Rz5N^V7b{gv_zZJD)(qxFK zi*onv-60l3#Wy+ue1jr9X-N2iC@gjQ1ZAV8_d&tA49J*!_#&xOgZGi_0qe_0&#KJfxR%}N=_cOIW-mg>&_crOv*jA)RsgyG z``3Ji+rnaXO<+2X-lI|DCu3?wR1WJpaISYY=kUH$JIK7jm3DQ&sWk6`CWcYwfP@-u zve$ZtPPQtOuMA^e1hz4wftO5|c4{826|Zs{jS5?$o+);Bp7nSU56@J97GR6o426aw zQY9^+MhQtJDk6d|bR*re$eNuN2!Ponhx<<3b_tPBVx$NjaTt@UCcR)|a&aPt{T`=! zNXiX$Opz0Hl7SbSa6%neZH5UoLVx>}3DtxRDG;ZwkZsYL)D{}5rADNfXDZg*#-C3P5?FNTWB7 zjGFr?XvWU?&4es+FHDQ){-TfJ`iqghQhc z-H^Gp565kL8r|LQCkPg~KfofnWB7L-ncW|OcJwWuHPxA8l?uIZP9K;qH-0Qu+AGx4 zuaY-EE}rIh^V&R{#zy&yhMI&16Jsigl1e)!zYzNN#=@M6A#?X`VrP%)sHQ(9Gb^4F zHLCZLDD;)6bJcV+p-IJAsv(ptbv#3LM1nVY$OosR4e)AB>8T7W4p%VP1@2 ztcvU^uBmTZ6YmJp#{RA*qMysvL(dkX?{XHLVM2v?ADy#sYzdDGg-%nYufG*dZ+j zB!|ju!(AxPi0AQ`rd>z$oi`5MRjvAD1s z69_dp%h3b-emi{zF?ZYLd|rHNY2@YyHDr^O_TQqEwMagwj#oaeZq4c+_rG*{Bc4jF z-^k=#fw~7w;*!23RqJC37iK7WyupQNsKiV-u}}%412hoX-n;Ks(9%|wZPqxI`IWg1 zHbueilI&lODa0;~8YDq4uOb;jNzObhSX1wG^UH0zeJ3-Mft z`W*5fUQZxF6W{s|jhoODQ6|8etgWJ+PidB%t~E;Q!!YEkf!yocZ~iOQX@>1-Y3uVV zxneXE0B^XVVI_JjNUCKGGsMcQ;)$Np7@aizKAOh6$W2{Z#jEv4hMd$#%_S?e#L^oP+UXTGN)nHp_+T))@1n zD%rl!dBUXMxcS)DD zHmh$j3Z{-;B(lDp#qk9zH`o@S3>t~R;^o&}w$|X1rJefpU5riSc3GB7vK0SeJ+|x{ z-~uAP=uZ!lZZv?axRh)sPn1UWuz}xYCvY` z+b-A3^Fkxc-`Qen-Sd9bQ;&sxs4j;NHc?+Y-4g9G8TdEF+z0pu=hMfF_p@2OHzV*N z^c0_)#eQW%e!M9^By=5TY-IW4YS@taF710_yx|Q14|Wa4Sk{hh?|U}!4GjtT?3U0` z^s1y;XxdL!1S0jDgN>WP+$+Hk=q;35|Hg47`9)C9L`OxmM%w*3L{PT>AUHE8YhnUB z4KU$E+WLL-oHi|6)sfUgkRtgvS4-tyMbWQ%8m;K=yrJcA>IhY-ahy+Iq*-JRA(uvK zWrEei7_kH*oM+do_P&sw9=`Vl_hq>>G7&3*_O5g+0p>+inxz9Lj#rOqh+Ry`!lWEP zrRE4TI_K{NAji4Bawy*ao!?+fwRd~w3+T*Z--VvdE9FDWHOx zrxs6qv*=H3My(M+>9qM_T+*F5cjZ@BO=G0WONU%jZlrAU`jWQs@0w)>=LOAyrpP9B zhw_=fzcOREjawJGbDK?CI&QzLUW*KexV{N;+#*cf4!bQn>qn@csKZaF6*oNI^;GHW z4F+&746p1{YD%)%l6y~2OuJSdXamL)Cff%PXK76GRTwe9+N#V8>!pm9xeBbj1%EaL zK`X0MOAen$rs!K3Ck=4@XaYu%Ke$=5Nx&-U-I~*20!DYd2EH>7H?x#w4FnNj>a4-4KVpsyD~ z)Q?SF^KYENi{W?P2A%ZGiW{;>KgVZ_I&AF3HE2iWb=jOKcivw$$E3X?p_#}8+hx-hg&2l zey9>bM`y$)3BjhvamSjLiXrldVjT~PL5|5^g11&@v}VkKe!!awLI8I)DTM**lI^tW zQ7EYWCixh$5Cc9!^!|wW2pF_x?{vie$sXmz5av}ukZ98|L7K5x6|p+CEJMWUy)NCG z=CCoYfoJ09QzpMSUxozd7YR6KAbIZc0?-Cb$U}~oQ`)=eK^X?p?n@iqeEAXWEeJCfXrgaAX{4Rb8$45iW{N-a6FhN*pxndUzB_D62ou5Yue)`m+3wSad= z+EQ+9Nz|j>aiZ%hlb$^1;ifalag+#eq?)$krBz==7Qa5U{2N{&xP-(Qly#e@ZmZB= zw(X6hn+YM6mhX9STsOKU(Bg3q)Cs%1gbCF32lmPO3MasV$ySuYepa1AS z4MXfM$#Mh?w6+zmMiPs7ox4Fij@U*=-zG&qR04@1v8g+9-pL{Q#&d=FOonzM7CRE2 zBP~VAr(ONI*c>94(1{?RE|u=5YPn|iH!@CX;v%?Lr05*1%%Wke{WJxkx{hC?TYaU( z(F6#u{|{u!pVHmE4+(=%}?F_K}J{XA$9O)v?E z;^mJ;ze#?yXqU<>c{>K_KP{TC)Ir&SFc{GFluMo&BmrL80fdTt@N?nm7D&r#vv)|| z^jL-657?_)`CNobbq9{6y@uavb?S;<`>D|uZt28LrGQ_CSve%*`CL1OLs{8WNNZ#I z)JPEI(PMIWWT(aO1>WPz%!&+{1N}jIHbiyxERmlF#!R9q6KWxjzb0*K3H>Iq$8fW=`&8V^#`*~; zj6Fn~DFLmE7kvId`uq`)S99ID_8LClbWkQK?mf?Fr(I{%L;FhYYVKS~%Xay;ULF<@Prt`$r zjxF|yp&ev=r~%(%-=dhr`F~O85Ou9)8b@9=c>~q_I@OE^Zd>0Z6nW2k2Y&3&PMMnE zujPyvBJcQ==J5vH-zFp}e85o?;)=f>Pz_UV`i)a=5gp3*&0Z_I;YXWsCkRr?@7!R? zkhx9k8kB~gPIO1~&=}x36RE<;N8HBJDxq-To?H_WT;&Wd7j83k^kh$;cNZ@0>vgaH z`@YTMs>K^kBj^ks8Alxm?V`eYFmCgHDHhoc?lx4MX5tk7qU z`ndz4m#U<;X)GImHoqI)+{Pkq|Ma9-00X23C2aX+uv@+tp^M$`b|FBx4vICV(`8vS zfd2r0AB_}$d(XD=^Q?F+>7?=tsET$uS9pCY`5A?>+ne3xBPU<=R+aqH{Dix!N9r?b zJz)>Cz)X2+askV0bfWL=^dorN|9xsL5&!p#d)1W&pzHYh+vEXqy~63bh30SaZwo-C z0bg7opg(>)oy9g4JiJly`26*AaC1ZOxPzx;3LEd%`r_rYTZ5iDU;6#6KcOJVk)pr0 z=`Xl$Om8;-N1-Y@FUq(x((`FF0wVL4XN?u*8pq|U1V8Y^=yF1FODM5P_7x+)?_KHi z>CsxQuR)Hl&-eP3sau+X6;kU1JOd%b8?7=nk|AHld>vW(+v@bx$}cpetBKYfL+b@9 zs=Yphmo^<0Is`Q(=S;Z;%{(=n6W_N`Fb-lGKae_yGFtR}52-4WFD>`{=cHQPU)Aks zTC#;pC&PK=N3$`C2zH4R=NVCbOI^>}f-05z;yaX~7o|3*53um)#+3eQz5y>D6(4)M zj*XU_TpSY$fMS|b4mOqw8`A6 z6@D~HS0ra}n+wU~l5WLk6|_qT?!{~WI{c->F>*=-KCyoSMS|d;xnG%zPeUq(F*2t{ z)pNtEndGJ$vo=+7-3J4>kQqMbucCE5HVbjikLzXMx~_&{6n!`dlqfEn9?d}&4R;7S zF?HS7;o9UrYWw0`Z^#>7$Yfk?KsopCAKr*o8o-S@ER^|0I&LsHsMkE;{xI(K0Qk^) zO*K_m4JXn8H>$l=>7O5dew@|itAkdadea0NQxPhgNSnHFTo(Y2N)Iy5s6w>a`%84; zCT~*e_sgPyUi2QVMC4gKbevHL#pZo*a^=G{$1oS4^_H8V5aR3?`sb85L&t(1=l9k# zzg-t42Euc%lWokGgRU+e{Ta>~-&7lJr78i-*J)b1#gMh^+N0c~90=S+y=_%{NS5e< zTY;Nh*PaFuS7JbZT+RgfX{DGby)4^Ai8=`_A_TqXv~>AFO~JD&Jw(FR`S!)ExvLg~ zt9seHIg4Mzexs`7c{S88%%#`1x%aGH0JC||{@6f{k*i|rwKQtSI^P1z3#$-LtiiQW zeC9XGN}yz0sz3CmIHXlfBura!h!wZU@8tu7J!QS7i2I{&OdS>YDH-Vh)0WmP{JFf z(`i%>8YpX6zQxvz7tSxz+tq)3vM&`2USEpZmj|LFIX8!N{-B=T?>JbCTsa#(@-R%I zVY=yI_FTwD=MeZep07Yf`sBpI!mkwYAftbcHIUOaP|*GL(cRwi$Z65#m)B@?{iqfG zLUQ9W@X?OTqrYCcwe3kUTy9-fKlIAxa=VX{tf=mtN2bQyMC{{0A7AoqXrKwSX6VqJ zIs&3jOLQqY;R+|SH|1y1+1k&tm=+QLf`e`!SbEm1=0&;Wh=eon9kc?K0MU-ZQ?#06 zGr3zub@q8<1Xir3^`|%yJ&>R>8QiRq_dFMRdJhbLTy zOF--}CjI@vp0Z%}e)AVL-DMZ8yi<*{Vy~yR<_h{#1PcL zrM9f|J_l;gyT%RGN)-x7jCqBVR0T`&)KA)2`$p$ksVwZJwO^!TCNsSXoFI`zkjhOX z(^u+;#ww<0Vw^o+Z=fG8zPFn?rL@FpE86NSgtWA@6Zrcw8Y`7&n-{5Gja7dgxl=P{ zFy3GO8u`BXynel{TyJ@;dSa#>D&D>TN7t#&obMXHoZGV$sY-GG$>*s;Rd zq~WH?1ki#+QItPd$6G^GZ1@5A`<}^+0#M7*^7oSfgnnQd%`>@Q7KJiVW`( zwWKZ@P$cGqkGU~V=^*aFdCQ;*nvz);;XA$4%)U0RSXZi;;xzxl+igr>2L?fGrNRP1 zOQJ<&$1G4|R1qX5gMUK0DF_Mz<9-^CzEoD97y2sQ(Sr97TNAf9bzO%nv|xdzDW54l zRdS1)%#R$>?Ok+GDRp2j98n1C6i^YCLQ7pYj5)^-B zdD4od-I|Vce9(ikLe2`Xpqj&$O);9qt;~NXD8)J;H=+%iC~3#pN57G?k(mYZCA?&ZyDZ8wtotR z9-7-)LP@7?;AcBXbd)2Xh3F7w}O;LOd&~>3^6{ ztl_RrMlq2vLGP6hnV~vU4_xFZn>odIdaVS?)8`ywU+0Eq4t#^tX7zyUNif7%=3l7X zn4CdU`M*KgEIJ6yz%0u)qpjnQ9raf4wUK)$YkZ;JYaX7>L zl$D;uZ%^YrL5`~98$b=*{%or%tP?*)<0MV?NNja^^tsfG{G(V=c#437CEV-R5| zNuHrCqTl#UN?#C3$hnruuI((p8j0vSyt`>BlNG+z^o| zXpMS~zb=1aW+Yu|PqjJ0I7BDP%Lgwkl_3lmx~UNYZc*gybkF3yLUH1xCls2!#VH4F zoJ>DK)=!VV_-3b)fuF1^$5Xn+bIH$PUcxn%{%aMxe?R@YU&T^snoV!FjNA=Ibl5Hb zF~7~cpju4M+uO@M2K``t2AS&TTTXVTD`a?<(>;m@pWEF_x75&TL1p{YQQ!9A+xgZO zB^x#1dL@^GIafKBo4ix6o_^ZSog4bpywhfPC|Ym2nWd3%zfm=Dvb9qU-M#Vm=BC|y zTaVt*7Mt{(o~$sqqLYTqzTB3aE`fzsxQ&*lgEG?7Rb)>sdz!Z}`dc(G z*)^UNZ<=zfOor=|5=7I9TdwVvqWLx=3&W3Xjl9vYs%d0f=%69Nc7_dzc@{zXDV4z5 zN3)gz-G~L}V2D}po}ZIc8^Nf%g|v6bv>~c=OoN;Y*|@2Z{6F%mN;&Au+SKV!Jt~C1 z%(zOH;ChnbI3N@ay{+mEq!#5WhEM^d%^R%HV3q#%}Xv+u3@_O{eENLm` zhg6TD0y|`pixUS?t^}j`2n2o|29Dc3x7%EY3*xWH5n8I1xMM~aIcC=swvlbZp5edp zUkbl)-_}+dJ}OGY*B2{a?oEJpbg+qZYw(4?7h@PfP}*A?wTjOV-t`L2&?9)WkqlEl zW%l#!n1z{z%#wpY&9#0^6vszzf1fiavo7^hBW#|qvZfgzKBsFYSc8+ULfu&uzTYRG z#zcZ@+Kl4@d!wY{*SEo%Jg|5Ve=W(s^W6uZ{i$-b*AE4yO}<;-_RyLxx#M?Y&m=eI#<@(8cA_9kH){0kA37Ez-w4)n*;Nku=LJU&>Ec+Z*g z$p)aE@gx@aIX9qdI}C@|(p0FRSRGh%f{vZx{d#mx*m&-#!%Y|*K6(nDi+m2Y#+X7~I(`Xh5)Cc7=n#fqwd)QEPG zny!iuZ58sdPNao~&6!WD&35YxmbE`g_J8xM{};VtXCY!D`j2Sw@xd|5{j@W8u^?h* zWlD_4qXD$!?boFcy5BV>=Ook@h`Dpa^x<|yEH;Kl+-hF@3Y5kIH zS!nx>aWWPu?#vYz7ne2G`Fb$*euAFxW4b;APzq3EQo=o7{E*3(7o@jOS;4G42h6wR z|E7Jvn0%Pntnh0q6YQa9xVEc3JMLUsD)96MdVvma{ViQSx@)`h+8m6>f75VIAWg;+B@(d9%A?pSzi& z4FO%6CyB(zap+QhKOaw4ZeGqaI7UI5Lfi1Oa+Yo-Ro00z>qpDGm4i(;$U02=X&O3Y zr}pUrQMOTwh+xH4#muS5?-k+dxP}G}^FYpu{WgId7K>x%;eqSJJl#;%J%+24^mn}l zWNbM|CQwf@>bJG=BkCji^4KM54!#|bEU4X|_W2_m85an&HEpp%+HGMIZ#QXk@(#(>hxvFi|Q z=_-7hM=Wehy~`OX9xBp@#v??Kt4%*da+6AEBN6tE)oWag5J%(#kr zEEU^|in0d%Ex%5Knv_;x<=F_?UM+j$ADdKI&eF#$@d6+g=ffZ*3^|dVbx+GhFym z92NYP^9`HiK!fuW5~&GYlVDtkS>M36?OUq-LK!%$QKi2w3|rv{G4=6s0vOJgX1}e! z045Q3QEU5?{omCaLSPzl=Kt?B<@hg=)+&Gmih=;j@}F>RHO2uYg#qK>`X56E2`CXF zD98U8rWrw};Xs-G2iNg{ioyM#c^GlfC|FR||CulS0g4LyZ=wGdozei+Lj-0150?u3 z1O-by$AX~!r-%FP@Hi2=uQg8o=r@s&|3XE9us|)Mp9K+46kYFwrrd0iP_{p-iaNM{ z=W_d%tv566!Oz`9Wq6^;iWfmbNJt(7Zz+|}1U>$J1@8~U3M$;U8g?nmfFj89HY=p@ zFWo?z8ZFV}XluA6W$h%nfQ}p%FH=!YZfKe@SNOMn03I?$_J(VK}a{uP({7EK5;`Rv;IxhS>AVDnYq4k zSEXPk%OImPSdBl*WRP|OKtenkg~5;r#e--J5gX=BxbIfAFcTI9`+*$8&X1I)9aSQ> zUhw{GKoz}~)(+Eyg;t+FQZm%jdEng4k1|Ae3=&}tJ+!?tQ9&|u1dbm~)Zh3vpuIiT zdwKKxmi9)B`Kjk4mUE}Y*ukRu!VbQ;o)@;36Kk5_cKIxj|6q(Xd`FrE6XoNIn+`=z zfGLo6byHc-pVEi#HjkMuUfGxKfi(6zn^+|s0LRCu&TJ??(~OH<%Xt|Mmw_`ZVwl8w z)n}dIOLNl<*Dvx=1Eylc7<}|ZqnNf{z!s$ViS$-)M*{&WfYB#neUGG52hnQe)DIKz zJ-DAQ8q5X991}gT-LN0!I)sQSh4&DmKlz&>S}x}sONeC1Z2mW6$dzS$?_Umi(J zfQ4&n-|q5gGX3Jl+TK?8@4MHq*2=n%qYs~MExKI2o(-2v!s%%P#uVgb%-$LXeKJ$Z zY`R_@zh0l0z*#BEsKdBQjm#S`|Hw$9a_xszPz|NA(oAEDpe@ebt?oJ>jFjz~+}goA zr5CHmmBZ5`8(#wQquRmGcleLZP1I?0fZY>D)@9PUT*;r4Jf=AYTiJ|FOs6SD5LcNU z>|@~~{`G<=dIF*jJvflR)tPKI8m8ta8(W9%Sm6y=dh8W|Fp=x`1sMub*qr15W5}L#<)YCqWS;T8lfX2etgKRk-PQ1!n{Dau*vG zE?t8_P!%snB&R#yPQRfi3$*y}X9={#pHI7P`3FptVxDWvs5>}o3iv*7(dE`|*$+qy zIw>ds2J|V&%yCPvNBC*=xQqMucD|msR*yB6beq+bl?F3E2&=Zk7z)lTH?fGlyu1S3 z`-@$QR|0nNC0t3|b-MoO>AeDP8MU<fU$0M7H1iCD z2N4K~luVPcNH&oW8ZoE!>oH5eg}c|7TBA{Ma4jq)<|}?^^G&OW9=OplCpSpSh1iSB z>EzIOYNLC5fh%pyjYSmNBM(26Sv-W?Wj&g92IEDlFtT;$ZFc)7br4eW^Zejo{;^!U zL=d#A%>V6V{ac!LMQbH+Ur&I(b)Q#c!cWC^)h}Ix?IJW|iH6A_*(vd?rxNQHBL}Xb zVDAs%=2Dj!**spR4|QnGu$==9)6` zPq@V`qI6V72dNOC8a0^GTQ?00a}xWbX?u|j@8wG!2%yYo0fF)HR@6g$yT9d z1zv|)kSFauT5Y1fzlvpH&+A3i0p*Zapp$n}XX{?B(4-yc(oa9lrS7nnZFKUIN%(Zu zE}#3zRFKp-GKevjA*J+PG&_6{`F74TP5m~)JC7RjX-cSSFihXRU_>}%D@!H*7gZ`} zamRp!t2966unZ}Sr-x^~YOk@?9LVia_iWei-Ku7eDVLb^ruIcaHo~rAJYzpomchv zt=K7Wuj3R8GLzR+Bfk9Jg_uu-o$~w%cy`vlDqJM#G$Wq+*C+zi$}Z5g|EBQ4!t(Q% z=3`_YIr1-}2^QlmsDPR*WW(&&@iLPzVSoMojQh$V8qh8Hn>+fq_wW36;%~vm?PU5LRPU{f>S{=ruoFbL$-?@(ahQ+dn5kXng&z@Y3?^@+j0xBeRg zI;n>=VCb#W5a3#HsbAdxATKgFJz6S7`9B9=5^!08)OHAP%+_r~a6ZCR3S@AMR%U&n`<|V1)#Sx|>+UUb-%}PbisGX&qH)?9 zM69^57^xEB>>xIsR~ zE(7{-m0(1LAfT@Xy211_224SXs{#`YL-v#hVyKc9Z#2t9EN!3t3lx_@+Q~xPBBj+W z%7qcB;cSI?VZCr=Zjz~^$@(M{1}s$d>j#`@;9!RXsdhyB8AM>WP5QuVvM}X>BE+Q3 z5jUhscc=TZOAON_;K0wEgY2t!At{^kl7PTf1TsbcAA|S-yh!%xU?}B9R&fDJNi;j- z{Z=xF;?pn7fpSF!(O{}_pn}76TLBDm{vCL?W@TKheb^;wA~0E{w?Fo?^Fai}Q?(vS zL>RSW+;5`Muvm+B&_AcxxAOfAS*5<`!F$9z{Sfkg;KW2hjwTMN+cAwqSB~OWLouv{elZaGG^o|^+wTg zpcKNah!>$s3*!qB;}O%K3Wujj5D6+#B?)QaTF19TQr3LHwjz#b#3>JN_6}!PzdhITK0y;0?XQqlj>kt}Av;-PNt8pQ;lXeCUf`_7!gg-jq0A~VekEjjp z+?>Fq!s^slAJ~WIz`8;E6G;(Q1Gb9vC((ev{Z3zfC+okMz8okQaR4)1zoP%2#l=7b zGI3~;Sc}E5;L5t?7(5)RbigSMF}#oe0C(D9hFhf!e-~QQCV@LBD`3yvaL@*e&=8Ay z2-3{DtbW)i_(AcfuzVJ*xMm%=7}hVCmta;`^sg%HX>DHFK_zJSAGtv@_)e%zAvDn} zpQ~0yPUcf{yk7{^M=mbM!RB7en(K++dFVUQ4s$7iIbT`I)OIL`%|Ih@+4@^LOQ}U0 zRyY*Sj3WIF$Ti`K00hF&Onj+l`kjKGuOzR)ud)PSx`H6tzGw?7mmK8z$@rq>Jxf7i;+0v z&5|BBtFcD#K!OxH310GjHI&-$yl&=EkR(XWyd?L8{r(4qk5$l20T_>WqMr=8-*iHf zRjrTZL(~>DtCAuOry9nW*o}Qhcdg-n!&6vZWtrFIXqU|@CIf8(eXgkJhIGF*y^NGX zNh~bH>*$0l>mZHbgkvJtiza{a#f^H_sA1P67>exd9Wso}s z<1TAbCU|lNxjHZ7!qxzsW#4#Snl9uSJlmbC^^%FBzFzC7Dm3JBif9cJeX!u7AF9y@g13pEN?M&04`qvg^cmOnix* z3YGR^_y$ER?jNwxOrzf@qr-}6hYLJhZhr-0tUau?8&o}Wrdb@P%O=1@!k@4yYv$ zg823VX6Q*bG0N8iLmV$OehBs!-)@t|E3k0t@2615Zt~&0>Gp+$(Bs>Uw%Yb9ukT^d z39JOT?PJj02;twJwYUryPzF)VC32Z;CbsdD;RBC}d=WTMGg8#|x||WXyw1Wyz?n>I zR19>-uSC1&^~?A{B+amF>kavR{+^&i`__f4BjT>M%pWg9ztuq;ytCK@?7OyQ z|8O1+et+pHgylxxE*1!T{zJRJzL=X7`>L{wfwRrOl>35RIw5CFFeR6CqwDTQh?R7L zv5lVK*;rZb87dm_A$#zF!^4JOA!C*7 z;@12*KZNEeu`H4uB^8du;6w{p9@eO*5Gmd55NWp;iSb?AK` znBn5f5=8I4W>vaiJr{V|Wh|b@pDtU=kvJF*f*y)oocR z?fmHE##?@S=&7S1IIw0{`+-gcQQEgtqn)#2dn@Tw@|^JuUN-lH}i(%jN3PB}67@TCFoDXzr<8 zv!8OsubpsGqCeMSE5Ox7f;ug`ybj3kyiU55OdV9>ut@}E(o$cUQeziJdTulm=(JOh zl78&|sPwZqUwwT1_;OMnnXBm6d;v0%YxkiBL^`}YTAt$oK5AX$ZGzlveWA9Tu2uA! zEaA4d8Ew1AEy>CT5=4!k9;kKb1|^`K3gQS_QyifeOFlf+CX$Z#Yt0At1OtLMF12-Fa*)+Rhh3W=y+&lb4rccVcJ_a~Zzl1^jg70nz49T{H^gjT=eX<69&9_s@)g zx1*nrV}NV{;&5M}d#=fzGri$hW*WHl?@4~P7@p9`I4>zPO9ka9Kbp!rIIDM40ovM$ zjxI;XKS@ARe9d7Mkj37JGvI`DEKc_&8$PdgzmqAnnsV97?IPxIcV^WSjHKyDOE?ZJ zO?bvG{&>9o*gQR*t0kar!KTJYKz+GS|FkE0tnks$Pci_hl?}sk6BV0GZPDqQBwY?* zUuMU1d2G7%hdvp%=T_uZtDiB5-R^q3+3Tt{TT#sTSMRY6!~}(S)0|MZIgR9g!phI< z{}Nt1N1$&6twyK&6{f~-S$Q{zF5~U#wUTr^Mc=nHX_uk3;K!)(>`$=#G3n5*lKuX0 zJ|jq|XBv8)t5Bi!-dl(JIfvrFPlf3Q5rhz6)zB6M;>fk3jz#Aj1_dFyu9|p$bX1}X z!s0ONG$*tQ)EXU$CXzO&^wph)h9$^v#vD}>=@@j;ArpuA(Zr4K-{l^j-^stSk}XQV_IxPjh!k>#9lZS^ z$261tWz67iH=YUg^%p*J@xu@MZ1Hc*a`_x&CZj$8qySqhw9(!1(ZCX`#+3H9r>M*u z$y4{oiWhMrZ}P_iW2qz~vSd~uj2(?NN(mlE;qo4%DamaVvuAvUt>0>C!#%pQR%sAAb8vTGdwgleb+fW%-f4 zx2yL(qyf~ybtkEQC^t@4qz_Y09BX-BeHT4}vDjui!5k`H_6Ek@ zE?%!sCo)wIN;tJTapSerJK?r6KHa3T7AYUVPXleBbI9i}ujW^theTh`mn&64b=Slo%M7wz zun)iJyxr}qYz3fcq#Krqlh(w@A4yel{Hkz_hn5Et&EUp{q+Q%^2jgT!Z>@oLOHG_S z%W%HIms|d;SxRe^GB#0cS)YN2D;d6)ZM=#`sEDOR_2$b%O=Ax7Zhn7S?(A8{IifP>j&c& zG*dsQT!&#p6Z1}YvDERfaMl9o_ZN8k#YMgFV=->7U>{~$y|%Q6CV#t!EH%t{`?vX4 zdd8SpR6ud;#ksSoLvJJuFao0S&ExG8=vRwB{7TtPXOY{n>_!l_XwwEBk!XU$&a_y9 z4LbTNm`862jg2_*Od-94w*~kVB>F^7`edO$gA9TnHD&I~5IUqoi+}@W?GJHUh;X>R zh8;55z}x;46s{NhtmSmSHhaoCGyM=M1`+BJBDcR1BQaf4%Ss$|n^c-MLyYhkeld|p zF2SV}Z;a2#Me{q)i;nqWi!6n)_u!8^tp`H%URFFmK70HuHUYoA27;--ivo2bpbjLj>IrP$x#95714?xQ;$(;VWMgj%x^c@k2BNf6ZVODh zyh>*`sPUUuqN26Ly-QKLlX;xs@Zpi?3+4Z)>ysU_S77ERU^bhiXS-QrTE>)H9OuZZ?1C{aMuqAnnC7!vHLFZn@!^Sv$zmmW zgJy7_Ox+5v(y{;*F|#flR*grycr)DQ$&I|>4I*z&srqD%qfn)8(`zW{VNXvh?jxwV zPGK8Y@R@3sbO6`%2r8L<#+Mm=wNn^j10@7vCN7=Ui_?Mi$E6y&N#8wz2SZExL_-&P zd;6_P1gA#0?PuKGmY~m0E)`igo~+;Udsun(B8ggBBeOpLO?1rt-Qs@6L!U|`B2Pv; z@q5W~xsJk9G(0h&5hYDL>Fs#VnAztw!VnCTV*t~2zJrLj;I?K zie9@y6)gc2bLno1)=U^A7AfIe=Q+>WJZ3!>YlZVnICpxD4c~gfHQ`rNH%8=zT?Qu!_Xoq~3R_$i6B?1|oWR;h7ER(L8E zm8SYMC?r@|=`WPSgiO!>H=o79#GLAf3E@x6%)!LU%ErV(&%{Q_#6(FA$0%=aDrV&J zlZZ-;hlPoSnVISTx};KLLBxV_uqUA4;;?bSF-llEIlBb0v6JC7%5 z;(k6-qKH(?u=^o{2BbxI19BpvL`~n}gsX!&l!((qa}hw&8ur7Vk*PpHVNTV z83jY5H>g7_!Y#v6m--_UB=h!#5dlTQD*ofe?BQJ0MUs^}=vK>bU|AS4(itowme)6+ z!C*mNcniueJMrl0PExiRQc%*eeb8}D-v*!~4(N$bLfG%Xx-iyc$&L_t4o*RX6X;Ar z=3v0rSlxcT`1gkoSa>L_3kem=>{9JC_5Ts-6NEVeV*)#YbwVl<*;B|15Ci)C9Wy9! zxQ!0Qy!ZsT=Fx)H)ED>!Je+SIPJb`uz8|__$j)Rn~ps?Ohb+?S%ND1 z9yeZJ4Td(JHy2v@kR~ZjSUQ$tv|2IDLCJMBs^;oHAVK$`txVMmKU;8YW!b?w)~c;0 zvDqtBSrHejgqIc+5o;HNgn$b6_;I@u(Ud1*M8AJmMoRKpZD$y}q6Q;UT-h!xTgmch zCC{CfuiR%Je>0TM+huYCLJJOrPECJ`Ss6tQ9!k8m&!O8~O8&HCobdQ0msErn(yto& z>QgMUAXmzkr3Ut<25pHEt$GI*a$19UxC)Y=jZZ6`3im-T@13b(=K!Vs|0>)}qoH8* zFm7#L+aQ``EThnCi(xSKU4xKiNVXw_?AwrKhOs1BlYQ(`)HhC-rt>^9_%8Am#(6 zN8Bu3M=yEJY5h%BJ(5F7^rsS_2kWZSs-yvzXR&^hqlO z>i;a0v~@iO8r+j$GhLm1$&>%S*6s`hrUv7_w4a}lp-f~QypP!4KGdMUu1aHClsFg z5HYMXlb(t5B#ayYmZpw zYBVJ6SqbV*V75p;1gI`#SX6BYdNtNC{ z*WX9O=ClYkOW?spy-s1pyLsig3Pg;*$T+6_*(JS3yB)4QzI$s;*YI9#b?H~OTPaF` zNo%r5w2NhkYD1xB@}NjZUxed)cATv=xX18@6Z7w#LRWdguIO`PA2*ez@Mt(1!y!Z1!M8s7Eyx53< z1)%OiJN>PH`nTA`QK;WS>*>(0QQFF2sKo1#)m+WgAa#N__}+dC!EYnGz>|BQH#9DK z6iRzkd!f^_f#pkNq0=j)UdErnLB(}{#P;YnC-I2lYw{v-XOaw z&>1+I!d_8UpifJP6rI-e#DD7{Ol^mxga*m>66OUt>viX^BqWOlfqIR41bubSzmiyr z)wqkMobzXn?TR8lxZeMW%DQpi;xlB};5jYq1lGvCH&_x%oC>(s+NFE-k<#Uf9H~Cc zG)^vl5}{NQ(*%u1nO9P#u||E^L*-AeA|9?wKNmgm{Vj2uLDX%{D0DlNm;6md7zUQU zc^6q#L@A#4x#*G1S)U|1`Sl87-$-(oj$vji+@5p3&7K)4uZSqVh|sh}c-p+#+Fp;{ zO|ku$Si2?UUbWH4E@E$u<^TN??-q>d@{!)}%=^gs$7Z>@!RT#-kdKIJVC+z!G^Sl4ct*olWJ`j$J7&UipWru*fzK~PNaw|JS zT%iy(2=o-%hlWO-LY32no;|m+MTlSMX@dSInL=E3;i~2)8mbz`rlxRpV<=oh!&uWy z(@fo5P0d^b3WdV;A^-oxo%g?DhN=Af$wC2Ram^hmLLEa(7V@M~bsf0Vx`c0TMBVgf z3~U+lbc`<-*;@i?Gwp6I26(%XRj#-(C=Fn}0Jo@`nY0Pod3O>N8opNhi6T;`VgBX& z*gA8R8IO+rp48tXmAdN*G6pH8u5Vu56?Yl(zBFo>4ju@;te7miDAJaETIc(>`!O>@ zv&mxJl2rMr^A*ceaPiu#2u*`5y1a|fpY+k?r~|+86rEi9d7sv5zHaQ4?HzzPNk;6o z{uYNjsT_XX8ZwS^rT#TW^cc?|J4YYLdKrkk30{&A++o3y0Hlmmoxbb)P>?zaklK$r z-zI^s3gdXNax)y9O74?zQ{IulEPJS=gWt3Y?Xc6~5rto5Dp-!f0pWL20dJ&t1a^SS z!ti@KDXucQ96-8XST|QLwvHr3N>NFUj&xo*GVTJ&Ayd;1UtLUW?= zyTb77OQ8qc%NN2x-S=E&bph*Pt4goUZ?Jw1?+$b3QpRSHkc(%RWmGV^4xd3>$~Xnm z5sA~-OOPpmPP=~ovB!eFl1&q1eV7zXiZedm-6Hu_ggh}w6q-vRBOU{smR^J%_S~Is zCWS1<8OsFn&Ecjzkp*+%1e})fxuG*598J{vmAhE*AYR7NOaJXQg(2uL?GgOTS-<+L zkaa~BW#U}rGH_`qA2Vg=fzx?VE>jOq1lyRIB;jp-i8a+~86k-A?58;xKC+Wda#@5) zjqLb}u|t~LD*TurH%|0}JU^y}ST^3_p$(B-h6kZL;n~}PK_3@pM#+lPNe#>9I^@b# zxX7i&cY$oYIH_jghV@D(PMsc7spJ$OdY{y7Fc0DmY3q9{O#Jf6E{NYa*o?E_ns~3qmN1Mg=);0_+ zbq1_t%=MnDX6+7MHeCDtlb9yc#KE_V6~LNk{EwK;g;HLdL2Whag$KQXUI%>Q*A}$Z z8G#DNm@h|B7m&^()wA5lfyT7#&^lJ(Q?H}%r8hL* z;`BdpXR}&LNuv_AZA^AWQ3>Z1urtYcpfCQVA(X{C%Sry00@_hc8nC4wftV0 zIEE@J-jc@8%8$RY*jUYDCNpXM{d~zzOa1^AM%~lH+k_erZ@vd z3yR#z&l|3Hw|DU1cOp|{Ytnt6O%g9BKT75;#CpVZSP{Io41s%;DV8prIdSf4bh0cl zSF=;TTrUpU>_@EXogoZ|wqg7i86aTBn$-U^f zckM;1W4v>-gaWcMaOO_KNV`*_)8RMU*2Cnpcc(3tKMxMdUOyAGneVRqg8-7F&I7d# zFQW`{gO>8eb$%Rzj#!*?bYDE43wUKf1enGY49T8Fv)9E4nOO wzCF|Ql?73UYN6uWfRXPD{G(m6|M#jOYNvQamP4FZLlv%}$}J;fW^2y H_pose, OptionalJacobian<6, 6> H_xib) const { - // Ad * xi = [ R 0 . [w - // [t]R R ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap() * xi - auto Rw = result.head<3>(); - const auto &w = xi_b.head<3>(), &v = xi_b.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, pRw_H_Rw; - const Matrix3 R = R_.matrix(); - const Matrix3 &Rw_H_w = R; - const Matrix3 &Rv_H_v = R; - - // Calculations - Rw = R_.rotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - const Vector3 Rv = R_.rotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 pRw = cross(t_, Rw, boost::none /* pRw_H_t */, pRw_H_Rw); - result.tail<3>() = pRw + Rv; + const Matrix6 Ad = AdjointMap(); // Jacobians - if (H_pose) { - // pRw_H_posev = pRw_H_t * R = [Rw]x * R = R * [w]x = Rw_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &pRw_H_posev = Rw_H_R; - *H_pose = (Matrix6() << Rw_H_R, /* */ Z_3x3, // - /* */ pRw_H_Rw * Rw_H_R + Rv_H_R, pRw_H_posev) - .finished(); - } - if (H_xib) { - // This is just equal to AdjointMap() but we can reuse pRw_H_Rw = [t]x - *H_xib = (Matrix6() << Rw_H_w, /* */ Z_3x3, - /* */ pRw_H_Rw * Rw_H_w, Rv_H_v) - .finished(); - } + // D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b + // D2 Ad_T(xi_b) = Ad_T + // See docs/math.pdf for more details. + // In D1 calculation, we could be more efficient by writing it out, but do not + // for readability + if (H_pose) *H_pose = -Ad * adjointMap(xi_b); + if (H_xib) *H_xib = Ad; - // Return - we computed result manually but it should be = AdjointMap() * xi - return result; + return Ad * xi_b; } /* ************************************************************************* */ /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { - // Ad^T * xi = [ R^T R^T.[-t] . [w - // 0 R^T ] v] - // Declarations, aliases, and intermediate Jacobians easy to compute now - Vector6 result; // = AdjointMap().transpose() * x - const Vector3 &w = x.head<3>(), &v = x.tail<3>(); - auto Rv = result.tail<3>(); - Matrix3 Rw_H_R, Rv_H_R, Rtv_H_R; - const Matrix3 Rtranspose = R_.matrix().transpose(); - const Matrix3 &Rw_H_w = Rtranspose; - const Matrix3 &Rv_H_v = Rtranspose; - const Matrix3 &Rtv_H_tv = Rtranspose; - const Matrix3 tv_H_v = skewSymmetric(t_); - - // Calculations - const Vector3 Rw = R_.unrotate(w, H_pose ? &Rw_H_R : nullptr /*, Rw_H_w */); - Rv = R_.unrotate(v, H_pose ? &Rv_H_R : nullptr /*, Rv_H_v */); - const Vector3 tv = cross(t_, v, boost::none /* tv_H_t */, tv_H_v); - const Vector3 Rtv = - R_.unrotate(tv, H_pose ? &Rtv_H_R : nullptr /*, Rtv_H_tv */); - result.head<3>() = Rw - Rtv; + const Matrix6 &AdT = AdjointMap().transpose(); + const Vector6 &AdTx = AdT * x; // Jacobians + // See docs/math.pdf for more details. if (H_pose) { - // Rtv_H_posev = -Rtv_H_tv * tv_H_t * R = -R' * -[v]x * R = -[R'v]x = Rv_H_R - // where [ ]x denotes the skew-symmetric operator. - // See docs/math.pdf for more details. - const Matrix3 &Rtv_H_posev = Rv_H_R; - *H_pose = (Matrix6() << Rw_H_R - Rtv_H_R, Rtv_H_posev, - /* */ Rv_H_R, /* */ Z_3x3) + const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), + &v_T_hat = skewSymmetric(AdTx.tail<3>()); + *H_pose = (Matrix6() << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3) .finished(); } if (H_x) { - // This is just equal to AdjointMap().transpose() but we can reuse [t]x - *H_x = (Matrix6() << Rw_H_w, -Rtv_H_tv * tv_H_v, - /* */ Z_3x3, Rv_H_v) - .finished(); + *H_x = AdT; } - // Return - this should be equivalent to AdjointMap().transpose() * xi - return result; + return AdTx; } /* ************************************************************************* */ From 115852cef72bcf380167c9ee47720e5944c1cfd7 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 26 Oct 2021 03:24:00 -0400 Subject: [PATCH 059/110] delete "FIXME" because `compose` unit tests check `AdjointMap` --- gtsam/geometry/Pose3.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index cff5fd231..b6107120e 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -145,7 +145,7 @@ public: * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) */ - Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect + Matrix6 AdjointMap() const; /** * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a From e4a2af5f3fd8ff206b8c8963b67b61adc04e08fb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 09:58:07 -0400 Subject: [PATCH 060/110] address review comments --- python/gtsam/examples/ImuFactorExample.py | 55 +++++++++++-------- .../gtsam/examples/PreintegrationExample.py | 30 ++++++---- 2 files changed, 51 insertions(+), 34 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index d6e6793f2..dda0638a1 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -31,9 +31,28 @@ BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) +def parse_args(): + """Parse command line arguments.""" + parser = argparse.ArgumentParser("ImuFactorExample.py") + parser.add_argument("--twist_scenario", + default="sick_twist", + choices=("zero_twist", "forward_twist", "loop_twist", + "sick_twist")) + parser.add_argument("--time", + "-T", + default=12, + type=int, + help="Total time in seconds") + parser.add_argument("--compute_covariances", + default=False, + action='store_true') + parser.add_argument("--verbose", default=False, action='store_true') + args = parser.parse_args() + + class ImuFactorExample(PreintegrationExample): """Class to run example of the Imu Factor.""" - def __init__(self, twist_scenario="sick_twist"): + def __init__(self, twist_scenario: str = "sick_twist"): self.velocity = np.array([2, 0, 0]) self.priorNoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1) self.velNoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) @@ -65,7 +84,7 @@ class ImuFactorExample(PreintegrationExample): super(ImuFactorExample, self).__init__(twist_scenarios[twist_scenario], bias, params, dt) - def addPrior(self, i, graph): + def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph): """Add a prior on the navigation state at time `i`.""" state = self.scenario.navState(i) graph.push_back( @@ -73,7 +92,8 @@ class ImuFactorExample(PreintegrationExample): graph.push_back( gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise)) - def optimize(self, graph, initial): + def optimize(self, graph: gtsam.NonlinearFactorGraph, + initial: gtsam.Values): """Optimize using Levenberg-Marquardt optimization.""" params = gtsam.LevenbergMarquardtParams() params.setVerbosityLM("SUMMARY") @@ -82,10 +102,10 @@ class ImuFactorExample(PreintegrationExample): return result def plot(self, - values, - title="Estimated Trajectory", - fignum=POSES_FIG + 1, - show=False): + values: gtsam.Values, + title: str = "Estimated Trajectory", + fignum: int = POSES_FIG + 1, + show: bool = False): """Plot poses in values.""" i = 0 while values.exists(X(i)): @@ -103,7 +123,10 @@ class ImuFactorExample(PreintegrationExample): if show: plt.show() - def run(self, T=12, compute_covariances=False, verbose=True): + def run(self, + T: int = 12, + compute_covariances: bool = False, + verbose: bool = True): """Main runner.""" graph = gtsam.NonlinearFactorGraph() @@ -193,21 +216,7 @@ class ImuFactorExample(PreintegrationExample): if __name__ == '__main__': - parser = argparse.ArgumentParser("ImuFactorExample.py") - parser.add_argument("--twist_scenario", - default="sick_twist", - choices=("zero_twist", "forward_twist", "loop_twist", - "sick_twist")) - parser.add_argument("--time", - "-T", - default=12, - type=int, - help="Total time in seconds") - parser.add_argument("--compute_covariances", - default=False, - action='store_true') - parser.add_argument("--verbose", default=False, action='store_true') - args = parser.parse_args() + args = parse_args() ImuFactorExample(args.twist_scenario).run(args.time, args.compute_covariances, diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index f38ff7bc9..95a15c077 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -12,7 +12,7 @@ Authors: Frank Dellaert, Varun Agrawal. # pylint: disable=invalid-name,unused-import,wrong-import-order -import math +from typing import Sequence import gtsam import matplotlib.pyplot as plt @@ -24,13 +24,13 @@ IMU_FIG = 1 POSES_FIG = 2 -class PreintegrationExample(object): +class PreintegrationExample: """Base class for all preintegration examples.""" @staticmethod - def defaultParams(g): + def defaultParams(g: float): """Create default parameters with Z *up* and realistic noise parameters""" params = gtsam.PreintegrationParams.MakeSharedU(g) - kGyroSigma = math.radians(0.5) / 60 # 0.5 degree ARW + kGyroSigma = np.radians(0.5) / 60 # 0.5 degree ARW kAccelSigma = 0.1 / 60 # 10 cm VRW params.setGyroscopeCovariance(kGyroSigma**2 * np.identity(3, float)) params.setAccelerometerCovariance(kAccelSigma**2 * @@ -38,7 +38,11 @@ class PreintegrationExample(object): params.setIntegrationCovariance(0.0000001**2 * np.identity(3, float)) return params - def __init__(self, twist=None, bias=None, params=None, dt=1e-2): + def __init__(self, + twist: np.ndarray = None, + bias: gtsam.imuBias.ConstantBias = None, + params: gtsam.PreintegrationParams = None, + dt: float = 1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" # setup interactive plotting @@ -50,7 +54,7 @@ class PreintegrationExample(object): else: # default = loop with forward velocity 2m/s, while pitching up # with angular velocity 30 degree/sec (negative in FLU) - W = np.array([0, -math.radians(30), 0]) + W = np.array([0, -np.radians(30), 0]) V = np.array([2, 0, 0]) self.scenario = gtsam.ConstantTwistScenario(W, V) @@ -80,7 +84,8 @@ class PreintegrationExample(object): fig, self.axes = plt.subplots(4, 3) fig.set_tight_layout(True) - def plotImu(self, t, measuredOmega, measuredAcc): + def plotImu(self, t: float, measuredOmega: Sequence, + measuredAcc: Sequence): """Plot IMU measurements.""" plt.figure(IMU_FIG) @@ -114,12 +119,15 @@ class PreintegrationExample(object): ax.scatter(t, measuredAcc[i], color=color, marker='.') ax.set_xlabel('specific force ' + label) - def plotGroundTruthPose(self, t, scale=0.3, time_interval=0.01): + def plotGroundTruthPose(self, + t: float, + scale: float = 0.3, + time_interval: float = 0.01): """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) - t = actualPose.translation() - self.maxDim = max([max(np.abs(t)), self.maxDim]) + translation = actualPose.translation() + self.maxDim = max([max(np.abs(translation)), self.maxDim]) ax = plt.gca() ax.set_xlim3d(-self.maxDim, self.maxDim) ax.set_ylim3d(-self.maxDim, self.maxDim) @@ -127,7 +135,7 @@ class PreintegrationExample(object): plt.pause(time_interval) - def run(self, T=12): + def run(self, T: int = 12): """Simulate the loop.""" for i, t in enumerate(np.arange(0, T, self.dt)): measuredOmega = self.runner.measuredAngularVelocity(t) From 755484c5798e0d41bd225a2978e57b4dec171cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:01:50 -0400 Subject: [PATCH 061/110] fix small bug --- python/gtsam/examples/ImuFactorExample.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index dda0638a1..5b4b06b55 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -31,7 +31,7 @@ BIAS_KEY = B(0) np.set_printoptions(precision=3, suppress=True) -def parse_args(): +def parse_args() -> argparse.Namespace: """Parse command line arguments.""" parser = argparse.ArgumentParser("ImuFactorExample.py") parser.add_argument("--twist_scenario", @@ -48,6 +48,7 @@ def parse_args(): action='store_true') parser.add_argument("--verbose", default=False, action='store_true') args = parser.parse_args() + return args class ImuFactorExample(PreintegrationExample): From 15e57c7ec8c5d8ada86346bd46e9a8ec82ff76b7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Oct 2021 10:03:31 -0400 Subject: [PATCH 062/110] specify optional args as Optional type --- python/gtsam/examples/PreintegrationExample.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index 95a15c077..d3c0f79b8 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -12,7 +12,7 @@ Authors: Frank Dellaert, Varun Agrawal. # pylint: disable=invalid-name,unused-import,wrong-import-order -from typing import Sequence +from typing import Optional, Sequence import gtsam import matplotlib.pyplot as plt @@ -39,9 +39,9 @@ class PreintegrationExample: return params def __init__(self, - twist: np.ndarray = None, - bias: gtsam.imuBias.ConstantBias = None, - params: gtsam.PreintegrationParams = None, + twist: Optional[np.ndarray] = None, + bias: Optional[gtsam.imuBias.ConstantBias] = None, + params: Optional[gtsam.PreintegrationParams] = None, dt: float = 1e-2): """Initialize with given twist, a pair(angularVelocityVector, velocityVector).""" From e31beee22b2a38676e792ba86503b763562d8f35 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:33:11 -0400 Subject: [PATCH 063/110] removed ground truth; angles set in deg and converted to rad --- python/gtsam/examples/Pose2ISAM2Example.py | 53 +++++++++++----------- 1 file changed, 26 insertions(+), 27 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index fac7ecc1a..be4118b1f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -8,7 +8,7 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in the 2D plane. Author: Jerred Chen, Yusuf Ali -Modelled after: +Modeled after: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ @@ -21,12 +21,8 @@ import gtsam.utils.plot as gtsam_plot def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): - """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" - Based on version by: - - Ellon Paiva (Python), - - Duy Nguyen Ta and Frank Dellaert (MATLAB) - """ # Print the current estimates computed using iSAM2. print("*"*50 + f"\nInference after State {key+1}:\n") print(current_estimate) @@ -49,14 +45,8 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_ylim(-1, 3) plt.pause(1) - return marginals - -def vector3(x, y, z): - """Create a 3D double numpy array.""" - return np.array([x, y, z], dtype=float) - def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - key: int, xy_tol=0.6, theta_tol=0.3) -> int: + key: int, xy_tol=0.6, theta_tol=17) -> int: """Simple brute force approach which iterates through previous states and checks for loop closure. @@ -64,8 +54,8 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, odom: Vector representing noisy odometry (x, y, theta) measurement in the body frame. current_estimate: The current estimates computed by iSAM2. key: Key corresponding to the current state estimate of the robot. - xy_tol: Optional argument for the x-y measurement tolerance. - theta_tol: Optional argument for the theta measurement tolerance. + xy_tol: Optional argument for the x-y measurement tolerance, in meters. + theta_tol: Optional argument for the theta measurement tolerance, in degrees. Returns: k: The key of the state which is helping add the loop closure constraint. If loop closure is not found, then None is returned. @@ -81,7 +71,7 @@ def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, current_estimate.atPose2(k).y()]) pose_theta = current_estimate.atPose2(k).theta() if (abs(pose_xy - curr_xy) <= xy_tol).all() and \ - (abs(pose_theta - curr_theta) <= theta_tol): + (abs(pose_theta - curr_theta) <= theta_tol*np.pi/180): return k def Pose2SLAM_ISAM2_example(): @@ -89,11 +79,23 @@ def Pose2SLAM_ISAM2_example(): simple loop closure detection.""" plt.ion() + # Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xy_sigma = 0.3 + + # Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees. + prior_theta_sigma = 5 + + # Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xy_sigma = 0.2 + + # Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees. + odometry_theta_sigma = 5 + # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.3, 0.3, 0.1)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector3(0.2, 0.2, 0.1)) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -127,25 +129,20 @@ def Pose2SLAM_ISAM2_example(): for i in range(len(true_odometry)): - # Obtain "ground truth" change between the current pose and the previous pose. - true_odom_x, true_odom_y, true_odom_theta = true_odometry[i] - # Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise. noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i] # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. - loop = determine_loop_closure(odometry_measurements[i], current_estimate, i) + loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25) # Add a binary factor in between two existing states if loop closure is detected. # Otherwise, add a binary factor between a newly observed state and the previous state. - # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. - # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop, - gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) else: graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2, - gtsam.Pose2(true_odom_x, true_odom_y, true_odom_theta), ODOMETRY_NOISE)) + gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) @@ -156,10 +153,12 @@ def Pose2SLAM_ISAM2_example(): current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimzation. - marginals = report_on_progress(graph, current_estimate, i) + report_on_progress(graph, current_estimate, i) initial_estimate.clear() # Print the final covariance matrix for each pose after completing inference on the trajectory. + marginals = gtsam.Marginals(graph, current_estimate) + i = 1 for i in range(1, len(true_odometry)+1): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") From c51a1a23098c87a13849a4e77cb36ed165983282 Mon Sep 17 00:00:00 2001 From: jerredchen Date: Wed, 27 Oct 2021 22:35:03 -0400 Subject: [PATCH 064/110] removed ground truth; set ang in deg and convert to rad for Pose3iSAM2 --- python/gtsam/examples/Pose3ISAM2Example.py | 79 ++++++++++++---------- 1 file changed, 43 insertions(+), 36 deletions(-) diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 3e78339bd..82458822a 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,7 +8,7 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modelled after version by: +Modeled after version by: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ @@ -20,12 +20,8 @@ import matplotlib.pyplot as plt def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): - """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2. + """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" - Based on version by: - - Ellon Paiva (Python), - - Duy Nguyen Ta and Frank Dellaert (MATLAB) - """ # Print the current estimates computed using iSAM2. print("*"*50 + f"\nInference after State {key+1}:\n") print(current_estimate) @@ -49,8 +45,6 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) - return marginals - def createPoses(): """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], @@ -78,34 +72,27 @@ def createPoses(): return [gtsam.Pose3(P0), gtsam.Pose3(P1), gtsam.Pose3(P2), gtsam.Pose3(P3), gtsam.Pose3(P4), gtsam.Pose3(P5)] -def vector6(x, y, z, a, b, c): - """Create a 6D double numpy array.""" - return np.array([x, y, z, a, b, c], dtype=float) - -def determine_loop_closure(odom: np.ndarray, current_estimate: gtsam.Values, - key: int, xyz_tol=0.6, rot_tol=0.3) -> int: +def determine_loop_closure(odom_tf: gtsam.Pose3, current_estimate: gtsam.Values, + key: int, xyz_tol=0.6, rot_tol=17) -> int: """Simple brute force approach which iterates through previous states and checks for loop closure. Args: - odom: Vector representing noisy odometry transformation measurement in the body frame, - where the vector represents [roll, pitch, yaw, x, y, z]. + odom_tf: The noisy odometry transformation measurement in the body frame. current_estimate: The current estimates computed by iSAM2. key: Key corresponding to the current state estimate of the robot. - xyz_tol: Optional argument for the translational tolerance. - rot_tol: Optional argument for the rotational tolerance. + xyz_tol: Optional argument for the translational tolerance, in meters. + rot_tol: Optional argument for the rotational tolerance, in degrees. Returns: k: The key of the state which is helping add the loop closure constraint. If loop closure is not found, then None is returned. """ if current_estimate: - rot = gtsam.Rot3.RzRyRx(odom[:3]) - odom_tf = gtsam.Pose3(rot, odom[3:6].reshape(-1,1)) prev_est = current_estimate.atPose3(key+1) - curr_est = prev_est.transformPoseFrom(odom_tf) + curr_est = prev_est.compose(odom_tf) for k in range(1, key+1): pose = current_estimate.atPose3(k) - if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol).all() and \ + if (abs(pose.matrix()[:3,:3] - curr_est.matrix()[:3,:3]) <= rot_tol*np.pi/180).all() and \ (abs(pose.matrix()[:3,3] - curr_est.matrix()[:3,3]) <= xyz_tol).all(): return k @@ -114,11 +101,33 @@ def Pose3_ISAM2_example(): loop closure detection.""" plt.ion() + # Declare the 3D translational standard deviations of the prior factor's Gaussian model, in meters. + prior_xyz_sigma = 0.3 + + # Declare the 3D rotational standard deviations of the prior factor's Gaussian model, in degrees. + prior_rpy_sigma = 5 + + # Declare the 3D translational standard deviations of the odometry factor's Gaussian model, in meters. + odometry_xyz_sigma = 0.2 + + # Declare the 3D rotational standard deviations of the odometry factor's Gaussian model, in degrees. + odometry_rpy_sigma = 5 + # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.3, 0.3, 0.3)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(vector6(0.1, 0.1, 0.1, 0.2, 0.2, 0.2)) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, + prior_xyz_sigma, + prior_xyz_sigma, + prior_xyz_sigma])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_rpy_sigma*np.pi/180, + odometry_xyz_sigma, + odometry_xyz_sigma, + odometry_xyz_sigma])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -154,38 +163,36 @@ def Pose3_ISAM2_example(): current_estimate = initial_estimate for i in range(len(odometry_tf)): - # Obtain "ground truth" transformation between the current pose and the previous pose. - true_odometry = odometry_tf[i] - # Obtain the noisy translation and rotation that is received by the robot and corrupted by gaussian noise. noisy_odometry = noisy_measurements[i] + # Compute the noisy odometry transformation according to the xyz translation and roll-pitch-yaw rotation. + noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) + # Determine if there is loop closure based on the odometry measurement and the previous estimate of the state. - loop = determine_loop_closure(noisy_odometry, current_estimate, i) + loop = determine_loop_closure(noisy_tf, current_estimate, i, xyz_tol=18, rot_tol=30) # Add a binary factor in between two existing states if loop closure is detected. # Otherwise, add a binary factor between a newly observed state and the previous state. - # Note that the true odometry measurement is used in the factor instead of the noisy odometry measurement. - # This is only to maintain the example consistent for each run. In practice, the noisy odometry measurement is used. if loop: - graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, true_odometry, ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose3(i + 1, loop, noisy_tf, ODOMETRY_NOISE)) else: - graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, true_odometry, ODOMETRY_NOISE)) + graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. - noisy_tf = gtsam.Pose3(gtsam.Rot3.RzRyRx(noisy_odometry[:3]), noisy_odometry[3:6].reshape(-1,1)) - computed_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) - initial_estimate.insert(i + 2, computed_estimate) + noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) + initial_estimate.insert(i + 2, noisy_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. isam.update(graph, initial_estimate) current_estimate = isam.calculateEstimate() # Report all current state estimates from the iSAM2 optimization. - marginals = report_on_progress(graph, current_estimate, i) + report_on_progress(graph, current_estimate, i) initial_estimate.clear() # Print the final covariance matrix for each pose after completing inference. + marginals = gtsam.Marginals(graph, current_estimate) i = 1 while current_estimate.exists(i): print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n") From d98e6e500a20d72d9920a2b016bfea108fc1d3ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 09:51:31 -0400 Subject: [PATCH 065/110] address review comments --- python/gtsam/examples/ImuFactorExample.py | 17 +++++++++++++---- .../gtsam/examples/PreintegrationExample.py | 19 ++++++++++++++++--- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 5b4b06b55..13ed24c6c 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -27,6 +27,7 @@ from mpl_toolkits.mplot3d import Axes3D from PreintegrationExample import POSES_FIG, PreintegrationExample BIAS_KEY = B(0) +GRAVITY = 9.81 np.set_printoptions(precision=3, suppress=True) @@ -42,7 +43,7 @@ def parse_args() -> argparse.Namespace: "-T", default=12, type=int, - help="Total time in seconds") + help="Total navigation time in seconds") parser.add_argument("--compute_covariances", default=False, action='store_true') @@ -70,8 +71,7 @@ class ImuFactorExample(PreintegrationExample): gyroBias = np.array([0.1, 0.3, -0.1]) bias = gtsam.imuBias.ConstantBias(accBias, gyroBias) - g = 9.81 - params = gtsam.PreintegrationParams.MakeSharedU(g) + params = gtsam.PreintegrationParams.MakeSharedU(GRAVITY) # Some arbitrary noise sigmas gyro_sigma = 1e-3 @@ -107,7 +107,16 @@ class ImuFactorExample(PreintegrationExample): title: str = "Estimated Trajectory", fignum: int = POSES_FIG + 1, show: bool = False): - """Plot poses in values.""" + """ + Plot poses in values. + + Args: + values: The values object with the poses to plot. + title: The title of the plot. + fignum: The matplotlib figure number. + POSES_FIG is a value from the PreintegrationExample which we simply increment to generate a new figure. + show: Flag indicating whether to display the figure. + """ i = 0 while values.exists(X(i)): pose_i = values.atPose3(X(i)) diff --git a/python/gtsam/examples/PreintegrationExample.py b/python/gtsam/examples/PreintegrationExample.py index d3c0f79b8..611c536c7 100644 --- a/python/gtsam/examples/PreintegrationExample.py +++ b/python/gtsam/examples/PreintegrationExample.py @@ -22,6 +22,7 @@ from mpl_toolkits.mplot3d import Axes3D IMU_FIG = 1 POSES_FIG = 2 +GRAVITY = 10 class PreintegrationExample: @@ -68,7 +69,7 @@ class PreintegrationExample: self.params = params else: # Default params with simple gravity constant - self.params = self.defaultParams(g=10) + self.params = self.defaultParams(g=GRAVITY) if bias is not None: self.actualBias = bias @@ -86,7 +87,13 @@ class PreintegrationExample: def plotImu(self, t: float, measuredOmega: Sequence, measuredAcc: Sequence): - """Plot IMU measurements.""" + """ + Plot IMU measurements. + Args: + t: The time at which the measurement was recoreded. + measuredOmega: Measured angular velocity. + measuredAcc: Measured linear acceleration. + """ plt.figure(IMU_FIG) # plot angular velocity @@ -123,7 +130,13 @@ class PreintegrationExample: t: float, scale: float = 0.3, time_interval: float = 0.01): - """Plot ground truth pose, as well as prediction from integrated IMU measurements.""" + """ + Plot ground truth pose, as well as prediction from integrated IMU measurements. + Args: + t: Time at which the pose was obtained. + scale: The scaling factor for the pose axes. + time_interval: The time to wait before showing the plot. + """ actualPose = self.scenario.pose(t) plot_pose3(POSES_FIG, actualPose, scale) translation = actualPose.translation() From 99ce18c85735e7722b744446d13ecf65d99f88ae Mon Sep 17 00:00:00 2001 From: jerredchen Date: Thu, 28 Oct 2021 12:29:00 -0400 Subject: [PATCH 066/110] formatting by Google style --- python/gtsam/examples/Pose2ISAM2Example.py | 20 +++++++++++++------ python/gtsam/examples/Pose3ISAM2Example.py | 23 ++++++++++++---------- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/python/gtsam/examples/Pose2ISAM2Example.py b/python/gtsam/examples/Pose2ISAM2Example.py index be4118b1f..c70dbfa6f 100644 --- a/python/gtsam/examples/Pose2ISAM2Example.py +++ b/python/gtsam/examples/Pose2ISAM2Example.py @@ -14,12 +14,14 @@ Modeled after: """ import math -import numpy as np -import gtsam + import matplotlib.pyplot as plt +import numpy as np + +import gtsam import gtsam.utils.plot as gtsam_plot -def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, +def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): """Print and plot incremental progress of the robot for 2D Pose SLAM using iSAM2.""" @@ -94,8 +96,12 @@ def Pose2SLAM_ISAM2_example(): # Although this example only uses linear measurements and Gaussian noise models, it is important # to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, prior_xy_sigma, prior_theta_sigma*np.pi/180])) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, odometry_xy_sigma, odometry_theta_sigma*np.pi/180])) + PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma, + prior_xy_sigma, + prior_theta_sigma*np.pi/180])) + ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma, + odometry_xy_sigma, + odometry_theta_sigma*np.pi/180])) # Create a Nonlinear factor graph as well as the data structure to hold state estimates. graph = gtsam.NonlinearFactorGraph() @@ -145,7 +151,9 @@ def Pose2SLAM_ISAM2_example(): gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE)) # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. - computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta)) + computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x, + noisy_odom_y, + noisy_odom_theta)) initial_estimate.insert(i + 2, computed_estimate) # Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables. diff --git a/python/gtsam/examples/Pose3ISAM2Example.py b/python/gtsam/examples/Pose3ISAM2Example.py index 82458822a..59b9fb2ac 100644 --- a/python/gtsam/examples/Pose3ISAM2Example.py +++ b/python/gtsam/examples/Pose3ISAM2Example.py @@ -8,15 +8,18 @@ See LICENSE for the license information Pose SLAM example using iSAM2 in 3D space. Author: Jerred Chen -Modeled after version by: +Modeled after: - VisualISAM2Example by: Duy-Nguyen Ta (C++), Frank Dellaert (Python) - Pose2SLAMExample by: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) """ +from typing import List + +import matplotlib.pyplot as plt +import numpy as np + import gtsam import gtsam.utils.plot as gtsam_plot -import numpy as np -import matplotlib.pyplot as plt def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsam.Values, key: int): @@ -45,7 +48,7 @@ def report_on_progress(graph: gtsam.NonlinearFactorGraph, current_estimate: gtsa axes.set_zlim3d(-30, 45) plt.pause(1) -def createPoses(): +def create_poses() -> List[gtsam.Pose3]: """Creates ground truth poses of the robot.""" P0 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], @@ -118,7 +121,7 @@ def Pose3_ISAM2_example(): # simply showcases how iSAM2 may be applied to a Pose2 SLAM problem. PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_rpy_sigma*np.pi/180, prior_rpy_sigma*np.pi/180, - prior_rpy_sigma*np.pi/180, + prior_rpy_sigma*np.pi/180, prior_xyz_sigma, prior_xyz_sigma, prior_xyz_sigma])) @@ -141,7 +144,7 @@ def Pose3_ISAM2_example(): isam = gtsam.ISAM2(parameters) # Create the ground truth poses of the robot trajectory. - true_poses = createPoses() + true_poses = create_poses() # Create the ground truth odometry transformations, xyz translations, and roll-pitch-yaw rotations # between each robot pose in the trajectory. @@ -149,9 +152,9 @@ def Pose3_ISAM2_example(): odometry_xyz = [(odometry_tf[i].x(), odometry_tf[i].y(), odometry_tf[i].z()) for i in range(len(odometry_tf))] odometry_rpy = [odometry_tf[i].rotation().rpy() for i in range(len(odometry_tf))] - # Corrupt the xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. - noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), ODOMETRY_NOISE.covariance()) - for i in range(len(odometry_tf))] + # Corrupt xyz translations and roll-pitch-yaw rotations with gaussian noise to create noisy odometry measurements. + noisy_measurements = [np.random.multivariate_normal(np.hstack((odometry_rpy[i],odometry_xyz[i])), \ + ODOMETRY_NOISE.covariance()) for i in range(len(odometry_tf))] # Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate # iSAM2 incremental optimization. @@ -179,7 +182,7 @@ def Pose3_ISAM2_example(): else: graph.push_back(gtsam.BetweenFactorPose3(i + 1, i + 2, noisy_tf, ODOMETRY_NOISE)) - # Compute and insert the initialization estimate for the current pose using the noisy odometry measurement. + # Compute and insert the initialization estimate for the current pose using a noisy odometry measurement. noisy_estimate = current_estimate.atPose3(i + 1).compose(noisy_tf) initial_estimate.insert(i + 2, noisy_estimate) From b15297ae4096c9f8f0e745a8dd3b4a4505ae63db Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Oct 2021 15:19:36 -0400 Subject: [PATCH 067/110] address review comments --- python/gtsam/examples/ImuFactorExample.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/python/gtsam/examples/ImuFactorExample.py b/python/gtsam/examples/ImuFactorExample.py index 13ed24c6c..c86a4e216 100644 --- a/python/gtsam/examples/ImuFactorExample.py +++ b/python/gtsam/examples/ImuFactorExample.py @@ -137,7 +137,14 @@ class ImuFactorExample(PreintegrationExample): T: int = 12, compute_covariances: bool = False, verbose: bool = True): - """Main runner.""" + """ + Main runner. + + Args: + T: Total trajectory time. + compute_covariances: Flag indicating whether to compute marginal covariances. + verbose: Flag indicating if printing should be verbose. + """ graph = gtsam.NonlinearFactorGraph() # initialize data structure for pre-integrated IMU measurements From 06bb9cedd156cb8aae602ecc40c001dbb2fcf1cc Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Sun, 31 Oct 2021 20:53:15 -0400 Subject: [PATCH 068/110] Address review comments: negative sign and AdjointTranspose section --- doc/math.lyx | 242 +++++++++++++++++++++++++++++++++++++++++++++++---- doc/math.pdf | Bin 272118 -> 273096 bytes 2 files changed, 223 insertions(+), 19 deletions(-) diff --git a/doc/math.lyx b/doc/math.lyx index 6d7a5e318..4ee89a9cc 100644 --- a/doc/math.lyx +++ b/doc/math.lyx @@ -5086,6 +5086,13 @@ reference "ex:projection" \begin_layout Subsection Derivative of Adjoint +\begin_inset CommandInset label +LatexCommand label +name "subsec:pose3_adjoint_deriv" + +\end_inset + + \end_layout \begin_layout Standard @@ -5098,7 +5105,7 @@ Consider \end_inset . - The derivative is notated (see + The derivative is notated (see Section \begin_inset CommandInset ref LatexCommand ref reference "sec:Derivatives-of-Actions" @@ -5114,7 +5121,7 @@ noprefix "false" \begin_layout Standard \begin_inset Formula \[ -Df_{(T,y)}(\xi,\delta y)=D_{1}f_{(T,y)}(\xi)+D_{2}f_{(T,y)}(\delta y) +Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b}) \] \end_inset @@ -5149,11 +5156,12 @@ D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T} \end_inset -To compute -\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +We will derive +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$ \end_inset -, we'll first define + using two approaches. + In the first, we'll define \begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$ \end_inset @@ -5194,18 +5202,30 @@ Now we can use the definition of the Adjoint representation \begin_layout Standard \begin_inset Formula \begin{align*} -D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}g^{-1}\right)(\xi)\\ - & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}g^{-1}(T,0)+g(T,0)\hat{\xi}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ +D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\ + & =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\ & =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\ & =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\ - & =-Ad_{T}(ad_{\xi_{b}}\hat{\xi})\\ + & =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\ + & =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\ D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}}) \end{align*} \end_inset -An alternative, perhaps more intuitive way of deriving this would be to - use the fact that the derivative at the origin +Where +\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$ +\end_inset + + is the adjoint map of the lie algebra. +\end_layout + +\begin_layout Standard +The second, perhaps more intuitive way of deriving +\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$ +\end_inset + +, would be to use the fact that the derivative at the origin \begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$ \end_inset @@ -5224,28 +5244,212 @@ An alternative, perhaps more intuitive way of deriving this would be to \begin_layout Standard \begin_inset Formula \[ -D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}_{b}}(\xi)\right) +D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right) \] \end_inset -It's difficult to apply a similar procedure to compute the derivative of - -\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ + +\end_layout + +\begin_layout Subsection +Derivative of AdjointTranspose +\end_layout + +\begin_layout Standard +The transpose of the Adjoint, +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$ \end_inset - (note the +, is useful as a way to change the reference frame of vectors in the dual + space +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +(note the \begin_inset Formula $^{*}$ \end_inset - denoting that we are now in the dual space) because + denoting that we are now in the dual space) +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none +. + To be more concrete, where +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +as +\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$ +\end_inset + + converts the +\emph on +twist +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame, +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + converts the +\family default +\series default +\shape default +\size default +\emph on +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +wrench +\emph default + +\family roman +\series medium +\shape up +\size normal +\emph off +\bar no +\strikeout off +\xout off +\uuline off +\uwave off +\noun off +\color none + +\begin_inset Formula $\xi_{b}^{*}$ +\end_inset + + from the +\begin_inset Formula $T$ +\end_inset + + frame +\family default +\series default +\shape default +\size default +\emph default +\bar default +\strikeout default +\xout default +\uuline default +\uwave default +\noun default +\color inherit +. + It's difficult to apply a similar derivation as in Section +\begin_inset CommandInset ref +LatexCommand ref +reference "subsec:pose3_adjoint_deriv" +plural "false" +caps "false" +noprefix "false" + +\end_inset + + for the derivative of +\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$ +\end_inset + + because \begin_inset Formula $Ad_{T}^{T}$ \end_inset - cannot be naturally defined as a conjugation so we resort to crunching + cannot be naturally defined as a conjugation, so we resort to crunching through the algebra. - The details are omitted but the result is a form vaguely resembling (but - not quite) the + The details are omitted but the result is a form that vaguely resembles + (but does not exactly match) \begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$ \end_inset diff --git a/doc/math.pdf b/doc/math.pdf index 71f9dadc65bea48ee1dbd925f2cee058bb2bd832..40980354ead4a2339ffb1ef0636825a77fad1cac 100644 GIT binary patch delta 26881 zcmZsiV|1oX)2=7BZ95a=jxn)q+qSMGlVoDsn%K5&XJSo~iEYe&p7-7BTYIhj{pz#3 zYIXlTy6UXQY1FZ0)Oc|)_M`+tT3|~{UXef=wfjx;p0!6|>Y{ zF*1w2)NWj~Cj^8h_V;>_kC$oQwd0ow0u&&a-m9TbQ=4#4bD+|k1AUY<`B7L@6fY5? zG;k7}bf6Io3_Js+4LM&#dXI{fTmpr50dfyaSsUG{THR>?qN3luSeyU@dMv!94h3VK zOuS9}jaP#(2Mz07{aUjhFbK_J_#Wb{fCis{0lT$iEsh;SE<_r!t0oLufG^@XTQCkz z2N9c6E|Rgrxx**m?Zcg~_oaa(hcJ$Y_MGcX^u!^+nWgB^Bm6S@nt}*0ej%e~MdpDE zX}Bczho*4uNNA}Wind@K6A)_0Y@$hlbNdLBj_DLf3n#fpqf%D^mV9CD!jG_u>w$9U zWeq_jFq#VBpzz_4uiwAI=s*QA!F)nP_GClPLT)OstFb`4_e0UT!b0%I=c74epn^ta z0^x8d6^(eTR^S;mV^iQK)kFCyxD<`Tg>y6$myvv$_t~+ivw7=|k;=&Dj=$Pszk4Hm zpwIY>+CYEL#VV%+Dj^fSo)qj*h95uAt*Bc(hqET4#7}H z7o-GrK>Sn{m8+Wv|b+_x^e=ff*X3d`( zG$|kNQqm0ZCoB%L_x@}W%HCFNcEPyYkuREw@ZDlk>mO!3GR9PW+pS7aQ5{lH#rk^4 zGrnQZ-?&@>#1*OikX7H`Tj||z4l9S2F&y)`qx$BU;SQopi&mEP&n<21q#5W*ciO0NeX*`@fFcldI^a;I&1A{?yM%t{=hkymr_I#j^eswq zEOE-yP&ys?kNJUB*h^rb%pY39^=(%Bn?8!@6C$jT8%`&2NKef5Q}{Lcq7p(@rJk1W zmhl5AfRJT>9s2W?0R<>kQ+&(Pv3N-(u#v!eE03ZcVZvE#LYUpJF8#=vIOJV6rvC{o z{4!lQ8nHfC(){UG0Iyr}J(VRYAv;;)#vd}{kTv?N3(dm3#w_huykmg?Hj}?|y%B4Z zArJoDDFW)Di5#+g^7gWxe)|dbeDn)%@o|H0z#021D_#O)WOIEr_J@LwzqL?LB@)5u zYgdhA_m;cWG29sf>S}A{Nq_smW9I<`?EHj_^&@?f#uJ|mj%%ZF!2cK3|pR6 zQm2}yR!L_>%`MrjVAbC*W;@?Y;LRP(T-{vEjqU$cjwUwnY@D1)^@$h|Y@DDZ)^@t2 zcc>nK{dOLOwuG{ZS`*Sk1l~b&19ezQTP9tGjIKEEz|j+ODNKB`6Cg4yCX|Lk7>L5v z_?n<%y!=+gwdLvng=(hJ&BEEfff{}Dy69b|_<&QFW=+G6s)h{}bB9nYbz=B7nloT~ z@!(yRH4@jdH#5`3x7F-J0u@$7l5@IJqtXhX^kaQ4WO^(vRo4V(;OjpaH-0duRYc{o zZvbcdr}Iv4yS0O?o1pZ|-`vUzZWv+CTb9^!(t`nT zyOePM8T&3_swwPLp(8F6Dz)TiJRA@=(vRT@+WX|ZFy~acZ!R(jVv{cD18dE&VaAxR ziCNH1cu<0I8j3kqt;ua+VcK5gA=NRm=q7umRd?{AATgxSmuEB5V5v<~$S=&OVG;J| zGH;nFKK$SfzaK|%6PvW~U0QfTmuLahd+PMBCXrF|=0X;n%xI>RkxTL7a`ahlk1ky9 ze9vUXUBJRo6OFvLbBfk7Aokf2;+0&lQ z2&#Tw`{4v$s&0M=Br(B*H)?6m^%PQy1(S1W7GWB(w)WwB?95<#I+%aKr3wVtBzKMe zE}(D*A~KA<7OmY2*s+(t`V5|w>9yNB5xk-X(0u;UEcR> zBl)jn!y6@3i1#tNio}-ix>4)4m5X7-3LK)1X`E9QLAr`DoB9}8ipB%thhEyulGW}G z=vpY1y33L!J>I4d&Y>u|1EKX&)~zD@CiOBAV4DsqwQjh5dttby7mz`mf!%|Ta}#`ywIqo-P& z>b5PcpLhpzLSlg^5}Uy4hY}c|@3NXxFTCFwS>u6@$AGqxn_MKSExKoqNg1yh8rVyR z=Bj@Ab&{)&KP`b*x4A774fW|+`OUVH=U5*_^z@B|)jTo!Nq{fh$fy!C_Dh;|4J+j8oYEhC z<#9&ouLl^KZ=%x^p(@3($Z{IP$c1eDm0)X#-p2kK*Up(O zhk(u#7EtG$CejvqBk&a<(&IcLjKLj;O~I7z+rle+J|#j%`a0urV{~pbV60s=u@`Mu zI2Tp6`zT!Rr^?5Q5=DqiWt?!OPg+is)NNjGcLvU=ydaJkmek{6485AR5p*{B78liM zp!w8CG#j;QV!0(rOih*OO5%5GhI(4}d8m>%3&a{3qm`4zV!}f&zieidzG9Alk)jGW zawi|qwDW3kg+iV!+v3O=%k|g~32g^l$#I+Ebw$MMNn3NEU+WOaAgf4!_)?ME$yNG) zVg^b(Hxu3A5@>sGpmeMQDZ14m`J*-gL$}En40da;F^Xo+KIF3goj>CXSFiD`z?d|X zfTfGi+Z^q|WorkGncEn<$eprmw-hOX!+Jc~SHKN^iglmG&l!HoWtdpl%x<4?;0>9L zZT&gN%yFG%9a``7fhF2da^wo{O;gBvS}3p>ei+enmAQi*3@y)g>)cW{nj~TY!ln+1 zXtBus=Tt8kuGWi7lI+_8`9m{=So-%uVBm4{ggafYF6GjT&w0~7kTa0R_~YkF_`O#&qbs^C@m!p4b5&^>L0Ax&ES3DH?<7&9j z`YxS2GQyEffDmpS)zbaNYHnNjD0)rOA}rmU9g$Sy`uFDbVBV$BJ4_%_ zt^fam5!_r!&Po`Kx5z(`z{{$##HITqrS^uMBIhzr|K>tY0 zMlDfXWH9bVXC+zWq!?=kWNvt7Idca~H!D(B9uVt)zJ9~nJFXbqr~yxg1(IF~%{tap z(=1b8f?C*F%%zCe!}viF1$6x-vEK%P9mm<5y}K8`k@Uy@g`9Wm=E{!({T$D2&_NC^ zw%a(WeCL($Aw2=%FZ{@~QXDZ*CdWux&hf~Pw?1kn*F(2nHvjHyxzBrJ(;=KaiAIg#^- z1`NSJ|3KhT*Z%b#Oc%P2K!Pcy7+9~AIz~nb=_}&h+~@dckLv7CT%!3Rc^!f z_YSiS14GVbzRCj6EafEp>zdR8a<6uAXzR&cUB1T#YY@_W>{-G@uAoEwl?B${mEnrN z%_rp~nnQ6-p?gbvFSh6EHh=Xxp9=tuS-zT{tv`)FxpG$Jeg9TYa&7c<;=mA)|4FLO zoeRgj(xJZgbNBUbPuci3<=5D%S3BM)PV)0xWG&@3w|-H#@5LAAO^(=DkW+o3{_r1mwGr`;`$M_F1apW+^5$?r}wc+OyneYmrK=WX- za(?k7hsPZkmGGeaxe-iiibeCT9O`o1j18)Mk=}gKoYPeI-fidYEw}R1ZqBGYQwW!M zCwI2;DNQp8<@?^^$+-)EWok*|-t3@Z*sMZIHlL~m^U+bVNmFJMb^{YxoZPm>Z^bc7 zMmG4q+;sPg=)XNgUzc1Rk70}fNS?fy_!OCjVFpQ)?-qW)QAgV1FaVGOxQ2zi;uTNC z(8|@!IN1{=&ZHC=op7JtVSH*cN4Q;4oPYK;*wYX~(|0o_Ey^acx?!daM(pu-?sALe z`hd3&6w2gGQXTnPn%31-%N!60IdMf0G0~Y|H`X6y#MtnD0a{Y!vLOOgBd3>OVrMk4 zBE=>J2E1xpE}J@QXxDDgD&&XJ$86Zo#zZrF)c%KOV z{`ozO|EkNqOdV?bcXKu{?5wTS3jeAXV3ps0Ku-Zno2v}MPR7?+KWj(&8u85>l?!Gk zs`BL*G0^eK|4Mq}$NS*~@(4HXSHdSlhlyd!wukqU#EhKUnTgHxkL-N$fmq!;v&E&m zHje0SgZ*qyzZ`^WRgxq@KCH!(?gr%*O&5?z(&-$#SYm3qzgqxOR?kkJQqQMbwqyl( zjQ8-v%l)EPI)6GvMV+hT3HZ$^FhbwcRF z#fmipuf-~2KSGf5cdMVgT|+tu{NLRK1kE1%x5;RVGZ6@@6_`mpcY{|4g_$}-gP?2R zAyR+hP>(5qk39l=!J`c|B%|Vuy8(~5M+KU_{vc6uH1Lbxdq_gt`|`oWlX74`6O;bFDcQf0oZz6~^iG0BtRrhZdi1%>4P_dAL)Bng z3twEM@KM#Hv>`IzKYZRQwm<&u(Rc@4GdQ!#GQhI8gb;;X56)VQG73h+>G;EY`YITE zlI+|#US1a1sJ^zvz3HF*GVv9I@`V~hZ>zkisAh3J!LTZT{%jCx2w{IN$~1(lwP3_> zG{J&W+Jk;AYr^LDdQL%cd6k7lU1ol*m6svs>yk=Q9A9}t#tL~v0D+pp{o*dzol72g zm59;+Ypl!4Ri6{LKp^gV>1EgZ@`WAOtI9yooeQo(j#&R*aAd=uRxbe>bYBNq zQ@$DRwdqHKwpdVWScH-CMH!A{e}_Dp9o8`#N2A7<;QU{Ti;~&%AGAvtIy1{{02ufC z8Or~U4eabmb^nmUf;bpO@bUDLOLLqyAGWn8EQ==QHp%u1B)!n%X7gq9n) z?v#eI4vPRVTVgdEr8TsiR8S6;-V(rjbwucI_N73`C5sjyn*vk*>uqplMTgc&ZP|5NC#GeEXM-r1HVwUtH6G`){G(-ECD-1 zv-OxjYcXNGz=lbxR7($(<`lOh*pm;2Ll0hzX0^NE04fzspaS3xWCP`f*`$opz$LJz za_c^=%+l>5El8o>IvC@+5;RBU06hlapI=P2`CfcdIcV{|ly~+v4 z3#Ux|qOBr#Lg1v;`Bnv%49_J(3ubMGWTcBB5BrvkagSvup0N`Vw>f7e_cCq|24%s!(fq4c$O(pu( z$6~b%wj20M9VUL5xQZ7nXY}uv&R@0WKY|PHjYCXa%Fp~h9*SS~96TN>^xIrl->=_8 zZ4FvUa4t=P6-#!`gdBDK+TNRUICex=Jv!1MR)Cz`q$6{_ysaJm=DV5FPR+m8JNkwJ z>~DW&APgvV8z*qmk8Y9wa>i**2xKW(;+}RL;4Th>EMD|ZTM9&Jgscs=diC8t07FF} zi(_Z%a=Y1cOU-TGZ`E(d&({GX1VQx@USs*(Ui)W7tpo~XE@DH$FAS5}jzi!u6PPxb z1@!JV{pUZgK3$vzvV0zHWOa*@Xb1EJ@=wL06!Vc&e=qN5d}pRZu@)^|4RN{0s(HzW zOZmMAn^!#U>gmv5_-!Rl=)+Dek8_aJWAQC3E2`V259-GtTh(udF6Q!AaAfmMwf&1K?^^=%MG9_bpxt0EQ{{qyF%5NzSQX`1>&f@W*gwZvJYO#-kZH4 zB7vue_shW&e9OEpS+&MJb{F8e<9%#EC?jwQ-`yrZ&(98r$skVkz2>D16C^W>1JErL za=o=wC+c9WKHbtzy_IKsA7QlT%;EUn&$LKnj3$~%P>~;G|X`W`?;L<(Vj^Pel%mtKc zu}D9uvu5i)d7Ggzi-lB+D1e1z2^3>P=O_A0Aa5p+s!4(MMRtx~bsAbULY3GmP7DY; zHS_ldawtiQ$@HjbK0i-=qtsI<4R^T)8~*d19KRmDOm>78{cS$KbgU+0UhLS|qA)yp zL z_HgdpG->be`?h_w=?pTJ&~lcRB7fkvLkaw#`2rPG_N{wT=?RkN+_G0oUKC zbyRDh992i!OZ7p6$Z|Wy0WfLZ|3;=nJ#!&4wUpf&myP^3a9&L*`E zoR-Thk*Wt)@|#P%$U*|lVPQ(6cL*VE-YwcoAyK^ZLJq9<&N+nlA@FK&^>sOR>$yWe zHYMRuD`lF$;J50?U#p+|o!lJdx zaS!tKqQ^JL4tF#h&ly;`l4hFWi3Rv-Y85MwZP&n+I2YS z7q7b@uZ`}|6O&bJnhrGQpSyLwA}FpI|Jg{ zUlDksCrak%+;d-1v}eC`yt#DZ`KnqNLf6xkO8e0}E5(YCzmyHGb@S|fvVYG#yH6S} zJq8zT2zB2tsyEQkGQ@Ypv6-aF!F&tv^_VT$tr%ESRw#R#i`!4xx9ks)_!*H&h~*sY z?7<h#A1bFH`ac6U3dAz{v<4jG*;g)#fWgPe$Ez zkw{+{bDveaAPa)3ZpmiA(8(fN#2TdN89v_brvm%F;Kq}}spv@JpowuFFB|sy-do>Z zjxy`Sz)D8T@@8&EV-nF-p&glUWW;{vpoaYcQQ;o^cJ0ZGfm8Uj{cDzW%0p7Bwrl2m zAS9&lS8M7h>jnU*|Ax#DhohQ>I29cz%itx3z4iaCPp`FH^gEPBLXvJ5$;NmNE$WJh zDM>)Jlf7yEMT%}+)q?pKzm)f-oOF>4?>sNkrDmtTul@v-DCZcVWk(nkz0l86OllIt z84(G+XhN^2iz))1hVa^~7b77dbNR%S5lF>q!JiO4{SysHGf9Rv+Yv9t2#4rLTU=Iz z)uNWO?^yH6W}`~9c4+0DEq!EAY;aH3#b+fWV06r7NiCT!gLV^@kcWpySD_Vi_wE9i zX`Ni7T?$KGQcuMo7rq=M4+Mvoc0_(nr+{FGQbWe3V6pP~ zcH@>QE;GU@pR{h$_s0F|$GM1E2X1_Fjy*)RXyqi0RxlycT(!eJ1G+Q59vQ%}4qb>y zrht>7(}6OQZGI^`bLEMQ_g~S^K^;$v5ZQD(i%S8J-dI#q736&gaq*L|m-OI~Z5H;w zq?H)ISbKJ4qmTL)X4WC#m@ul+7YgGo@4|)1#O(78=CD0rrqLk_Z&0ZE$$qh;N)~sv zxT%g!V_YZG9wODjOP(c?nHf)6fhv*OETbKk1Y4F*hDdDSo@!z(;$uB{kxW+z2>+)y z%f$`6JxkiW7wNxa_&-a&-OipL>O3@d_y>8Zd~@c z+h%w=8y>uMX3yFgX{$7br$)xO<4#9p{4L2V(}W_3N$7pDsB73SN@R zL7V}vub_o^bqEv@+$MID{ZUD|?!NU!4BplGbEUgQuWM%ctHv`N`-Y?`iEo4G=BPkO z01GRvXF2o@%Cq^$_g*Dj79)CdQly5-@KQBtu`+$i{4^YzyA7?*-!w^-FT9<-t%@8{ zj%~>jX2vY3WiI?U(PDAdu*Unb_dDJp!{(2kFgY^M-%hdfixo>MO?K)1;MrBODytlxm`XZMFY6mcF+ObK*`glJ05m31M*e~)n< zM(E+H0tf8!wxoG_hYi>^Sr+t7BzddoQXpKQr~ zlUAutsAWnb_9(7~fI}azsZAl6lk+36q*hcUeg-O2f4zwDgBS@v6!ZDT+MYJ3QK%7| zHtK~WuGLhNcXNM_K68W}p0(pHy%5URc9G|on`-MxVDmuq_c+hL=OjS?T9%-J<9pij zf&V;w!gQ%fth2`*q5LaeX-fCJ=8r#Kx2d(nB~`3-o=&JwunuM>fzhx^-9OO^K}-=L zLC)^h+-u)}{xEfhmg+Gh5dKYW7N=7rhw(ge1}ZvcHo$m9q{(1>aW-WcjFO z9WmLl-J17o-ip1$mRTp}4(9G14FYGh>YY8E(3XzcmkzxJ@sD0={a;jArdvZJ)vW1K z->XuLQB|Eg_)xi*jooQr9^$pFTzzzWQMRvz`*i&MT=RH}2K_o_OvyyowMkqQ(G}%HtZ2%CUwcPAbEOBsNqMYUlbH(j_ zw&9U6t4MhSm(C7cICX1iC6+oSS}+}-$LTIL!gghc-jY3%fME|DVKP&Z&Pn6K0zUB~ zoOGDgU^6*h%dkUr8a5R>TNf)B}4z`mDg3vVKcb5_=B+ z7eSoLO(1BT#PT64@^Q81hQasI&`(}R^8+Bl0n}(U%POvVw!Xcs5Ge6EL}pdgP6thL z!wGIH3h30po7rQ3`p!S(w|qsyZ?o<{w3)ngZ=qxA)%m-uAD})~-Ak(wr#kZca(+`13~-Q%r9Yg7QsY z82EzJ7TDnORGZrx4%cbRC{aaSX-tXmx9A%NCAjb$+e&kc*KH=cPoyh$RHkr|GktNx zx{m?lkcYwZ$cBl(0cEj>m@W~@Q6%=Rb`5s^9QXVTD)RbFU2abdS?KERjq0dSoYP}X zA)i25RY|nwR@FX2xeu>pUWbp7+Mn$1IN6g>^;>g7^V{uLf6h$a<3Au_$1=%X28!~*Q9@6|H3;Gi zPWHVJx#$HS+vVf8g>6j;aVa8HF4WxW zkFse?KIB^YXlRTlOMvoj`(bLgHMbp@7a^|%un-Ao2y8 zGvKG%{h&lJ;`{*9%3z<6`0*gM%?PvnSRsajCatu|i`M{&hONjNTkQul;pzG3^`F|s z^(%!_$2?s-KLi5p*V$>Nuf!eG7asK!8g|}!>e}^iikFpcLg(I{RGVar_V9u~5rx5y zme@8p%tPBl6DPZ_^Q!GU>jjGAfX|8m8Saa3q096-$?{y$0xhWQiB6e%#zi4cB_mwx z7O1&6C^^qfH=47?15NNa;_dtJVKylp#k|QDX3fwnU=j!Zu^r#KVVJq8u%7z}I9{namL>0xzDbv)Tey(7>NzRW3*XVb|ffb%DeO(N1|mNtYO66bW6U4 zujAKbFitgMeY#|7kuiqDF^SE*YVT_fiH$TVty5zUO&5S*6aLQS#&XN^y4a(O9u zxr6spd&6sa={^8e1v?_g$)~S4<8V&ipPbxe0m-j%ygSEn)>zx_fHt(j=l&n0hSasX;!@CD*#C4xXqw8xpJFCm{_^2=$Tg5jL|6p>y}a z*kqjyf7QKruXD!OnB!%GyieJI)47)qR-75x0?*WegnxzWsvqd(4TWO7KTA{2wS4(wNoUraAx-ZsH-4h0=EH9R*r zt5FwGVn|4S!cr9 zC)ZOQ@@3z3H#(!H+E-WI7C{A<{et#fd})(nTangc#kr)5$n+j;uqnYO{f9*ar^l=96za7Fqft|s zqMT){NM@vF z^JuD7JeHFrMY%&tS29|L7?m&C$gy(aNMFe2WFJTpxwsS2hi6q`qF!DZ@#^Z}auEk5 z7H0yb)$-E1!u2hrxg__krjx5@6ehNfG-hDkb=eV%Xv&zu$Xva?FT0**dA#clhB{S! z1AFuUtlq}w9^aIf-dVS&oYrXMes=S9%hD5{_s1y z8uIl!9pe7b3tq4=aOeLBL;IPdPRm}$Qauu?hW0Ae*Wt1~T-6o7%a*pgUh{pwlDgFB z^r4%fb>|&orzY}2CD3-8;+*M{g7Kug(LMBD2mt)?~hXR+AhQ%qY*y^y(x!PDf{J0-8;{&7|%8Nd{tNZdD+LFeP zfLDr3hfFeB(KYo(32UHCMtaB+UHdnKeJc#O6y${C(Xx5HY$b$0f@V@2_yP5Tzg5oW z$5fHaC?@$%0>`795z~H&3Qia?)4U$1?|RakD5;%hkTat1bQ3!Xug4W}Bpz6KE}u;9 z?}_P>>qnArZimXtMqMB_zpxU)DDWQp19D`jtUCxBu_MeiFWLuaNJrmj~)z^PL@ z$~o+@9bQi)~#ze#Vk@&jV1BxmUbw6Kbtt~vv zg78o3sw?VT+Fj@It)LxeB%u=Cv_`oZ$o&W=!d3Naqgy6DAOiMo?XtdZjSXGRcLj^Y zC(y~TkL0j3puHt(iH06eFL-GbfROaas0tknQ;EH}&9joJ+4x4n+dPp>7((8%?3gU( zADKH7Xc6|kES*t0{SSe09z{#2E(;e=sq5F-UuO9d7q%q<{LuDN%yd9sV&F9MYg*5%ptrrZb(A=l+kO#9>foN zB!0VrNfbF;bf|mb{%EU{1gMI_Q=)H8z%8laHm7$S96vn|PMNHJ3=Nay71RtupA1_7 zcPH%nb~lI22r`uSehxn_amMU7*9i6bnFP1r-;QXWD6-dh8l5h{VH-W1!aYK5N6fo& zzQ53673i}a^`68@gMZAwm{?LVf)8NzLWj4ySt8CPKt@aCtjG}A0-R3)hIzf5o>APt z9h?(7AE+*KbspF}qcoj7hb;TE0$E&Yo8{vBGcH5?fKyLBbS^WW-v_zwnSC`5VCrcL z#mkXvhl9p>NJ6`XcO*_PNw(V_oJTVP%s3eV^BHsYL(N#M6Rmm6>KB&o2AEl6`_8yi zX6|M}2bf4YCQN6`z}KMcJUP?*lSihp$!6y#) zSdz>CmpH zD5UouA?x0yMt2m#?4{MFqiC7`V%EG9$_+Rp0~=d{e(8>-1M-492F$B(c>@-?qgmF| zkPbo+lWiz`pHL$qKZ85Hs!cW0MWvuu)MRlWd9DgDZz!y2! zKc3c+95;_m^G>T#)Rp0Yj$)LXj4U#e&>d+fhz{IO`XN~&*rZCTrDjs>N;8(hJxyoS zfzlsUqL_CInBeMG`5IE*JB>R`GoOMOUKUFpWQ;5`>c&+@I=}XBU(Y{a3>dE`g%Dpv#wb36cygVPT8V%uS%lkvVvE%CE4rV|SWY|#)%U`nZ$Kw%F-V!D!g+t(f z4T3(3?zA~*ZC>7|*wr;hezdYntyn*h2W*Qz8p_h#W`AbPH;_<;6&P7z+(<>E(ti;N zqQ&54HT8jCYKw=33(#^;STi7VfHjiiW6c)@?_vJaL$7Rv07`)lHJ%y6=bP@1G%r3}CSm^&`!WERskoR2|d|MC+-#HG=Uq$6WOqy}q}u@U#S5HQzX zv)JAZCjt$0(SD9R(03uK0P|FE^GdGQr(0iGz&qDLTO_|zF z|NKz<@yDqn))O!*iYp)x#lx9Y$I1^sOC-q*w5!9T+GONPgMmYMt1vV#W8zC&r!(7nAmPV zS6Lm^g66-ml>Z6$p>FMVv=zWZbFA*t@PcXJ=vnz-=~GGl#ysg{-`lMpCM%ox`f1T9 zfnzvbJFvQXmvwE?j8du530%Lpxca+x<|WA-YigqGO`c7+tb4pz!*?KFr7>*QcC1n} z>N0v(&Yty^C7UXUj15*S{DoF*&6%Nm9_M1qPOm$WV_<)w^3zAJ8?%%$7c+x!c+k zr9-KuEDzrqwDE2(8#v+bGULI6Hak=k3na6&a-*%ak*%yFnds2p`tjsxsF7w^L2Izp zoLmTh#lw$bdS43JBm{5uMl1Z6LUspnU_{BRFWiB54I~{Wc2&_$znGQ^KEtT=rFDQP zIB&OoiK|GdHoSF77Px8@gHUaWaibi*$@ff9RT;A&(l33Z>_3RxROz9>r};A7V0Mmi~GEr0l6 ztv2Ai^IMknUS&%&qB)AIZ=acdc>CB*v8i2ST;GAB*Z^q^21w_22IrjWLL0)KC8~bU zJWRBbM&XLtAZ)B&c~o+4z9;8g43~_T@{k)+gX%W>4%NUib(0NSIa=XR_leYYGS7=> z`$1Tg>#6*Lp$Z{EZx?lveN}KE1#m)_#3s`sTnQyIFogY)IxwPtGon{XCkH&d?Uc2+ z_i5Xfq@2d*f!)M!yX0kNO|hH(k7fFJ)Y)eh0#Wo-8GZJ$YSMoZ>trT7Je;^yV&N_W zAX$mOLwR9h=c2)1h#9*jFfP6L;fV^b7g{C3Zz!d63-}`&S}xu3|3U2Nz$BWZGZsVn zQ)L=hOpDUNRXjf`a+Y|#BJct@gj4N&J-O1{ryeuK0+@NaxZH>gAAY;^AUoC^|IIML zEN4oK&Nf)3svYsXyIycQx_&wM<|Qx(=(Lw~3jAJWPBm+Ka6uCcn9vkma5>8CQezth z9#|wN4I8jsoWF+n=v6bP1R$z-USAqgndqy7@P2E`+m_$VS)dsukai-kGma)dVE(~-`gL+JYm0D?Kf3^|C4$*EqW){%bj@?=r$Oy>oK zr-e$UeI69~Rk0%4LRb^_PYlPbtf{`BYKzU+SnSZ2F3A`vHO5zn6i(%1h2y)P)yE8B zblu+>gOkcxW%XQpD^y(GBTEVp4nKLrph{m=_$2H?*$$C56fRbhA*0q$N7QCs9o)M+ zfK10);zEAW3=RjVVWh{32=#z0uKI})g%tOmsnR}%ig^1p(~QeH01KW9s7{{%_PhCq{TorfO?sDI3)13-2bJ=8a4L)7&ZrleuVw(6VA09E&V zouD4mJ(RCcZj7E|!OZLO7&KAClIzxOSdH_DjnLA_f z9R1Q>1XgH?(r}3_Dlyz%Qj!vvq~DIcOhfSnCFofGT+kLBtEofRMhi4WAsOxo^=_ z2C-|)?T*eZJ`5Mvo;JyiCrNy|g5cEV#)6Qdx(7K5_9wA($-ZqIhTCVi0M8L@$j;oQ za?THqHyS&E*)@rXVs5+6BT}l7qEraSaS0Y{ZSfX`3%8?}T(Y&EBP_A*{~F`HGdSIJw` zHXAvkV&AGZ)4?l)Z^u`!0HIQN;+If3N zkj0c}U?qbgL=*0O0&Q$)of5XGPlEswMme8ESx&1CB1om}u;Y)}Y0F|Zz(UjaKH|grcY6c}(sTe!zUEvz^Q@9!)E8Zr!WSuCXSwMCL0-*$m=JQmTj% zo4ip?x*UFb7MOc=i0F8pS|4WoeDXKr@LlfV`Kkz!K1ld!oc~kES4G9yG~F_|L(l*L zhCpz44?0+IAKcwt2O9`3L4vzWaF^i0-3Cp7;1D3V9p3MI|9_o}v(~xlwd<*_r>}Zd z*REY<2w@>A5L2MO#(w)(CnU}k(`FU>a*ZWK)v}QrEa;NC(sx370V<%ssv7PB8b*^k zUTS%p88U=wnkt#RB8tVZ3(UM(b;kR1p|aM0JE`v%!{agEcjns0uHKr4nK>-XIF{pO zI7pDy4YF2m=z46=og4g+VqkkPpLo^@mO$X4=jpd@Ff*jx2;$uV3#+VsJh;lN^>4Ya zM_a<^bT!$0I`@GR^Sv0bq;5+5Sy=xKs9Bom)PX#A`s#e#far+0=jZwT-9AiP&2(Bc z>Pz{^zF*zwA8X-w%E8;3BGmou>~!(#VFiXLYjmcaUvhMkG>2t(t zM||`y-_e7S*3Ej7d)H&&UHoo~cK4wBiA3P{QyqR%XD6)!y(|{#P@n3O9~$3pJgkyi zOzt4-_v?bpMiD*L&o|x;Z3dgtv5#Kbi_A&pCHqSDas?=1%sh+*sfmNIA;d%yq&lK# zzqK#Gh4%%++yd^R0;orp-ihd+5=97HX>LddH9?AI4@sTa+Bpk?WiOXTFQA~HFKIeI zF}&h)MGbOy5R?D|928;0I&ai{{nG^g+O$B{Z{~x)sk?##+7aRe4)R-ak2YQRH#sMI zr0w@NJ5vS~LQWPum$G}@vgfAcT+MG=h~jcER2!s*lE8LV&|DP!slz?2z@qC?8cZF( z?f_82*-L|=Q!R|~_uY<4h#r;p`_dtroOxR*tWa)E(~+w(V5SiyQ=M>4O=F(gUI5}- zJuo3jQBy2c>8Tb*5}LhTuoGj7--`@=&+zMGXOetuY`mSx)$Z?>sa&nZ@HZe@(A#ue z6J+5820RU(p)>=>`Piw8h48L?8AeC!)|q2Iuc3fa=wuoFX8zSk`wz$NUUVE?MJe%`unK2d2q2L)5aUs z<#`04gGvIoFB}B8Hw~&*hS{hHTI5nlm2h< zITW>)^xe$l@5{SUrr^@EZny&4)I>l3u?nN0vO*S}_U9HJn!Z@%Sb*H?iswg;@e-BQpl zyL4(KLw%!36tl2Zlm>xAlyAaYo_I=WcFd4GCDTMakxibyP;9|4P7d)@G&I-m+W|Cu znN2x|nu>HHF4(y9{oeYQZNTLbW})=o6=?%Z(Tg63fzQlZdCni{s2eRIClD}Aomt5%i>+^m8i6!A?VE5D0@4|zJtNU%>@U0h&;b#S8%1c=P${Y1Vx^fXA zKCMrVz+Q;+ZUov7-lmuuCE2S_+>@oKp?7T2wr-FzO+jnN$L+s96o*EGY!^R7Z(W%Z z^MC{{Gaqx^)CiQ&d;Bpf5+3x^e)kcI61E+{!D6%ySPjLLLuNzd9hn1 zgL59?KX3cV!e3N^Bg#?QFo*X3>6&XK4`l=&&3$?I1{^vTuVPRqS*wH=1k*g0pPbhIRY4&;kmj0L4+-$nScv20B_4H&?E=&|6?R5 z&723&!t=2I2ej1P1I#htdH%DTpoIw;o{J2g=Rdm%THLANUC;o$|2V#?SQz1vaQ+(t zKTW6{9=io72#@UaDzEtO_g7P(9z5^Afu;}jeHI2u4yQnmYOfA(R$ z96XJ4>KQYC9?b{u-lz&Zs5!K@zP3tbjUcN-0!$`)ln_i)^FN;wj z@A_-_CV9Ys?!#%Bgxw43FCFPn0~7vX_dyKmZ1FF~`g5!ciVz|bK-Sl}PEdU7AY7&q)z$n}S{u-91 zAgq>8XK2$XSz$3mNW_>08pyclj+F(j-i(YuT@IBuvtK0`+e=9j=15sWOC=ND~qKTK}9pJRX2#L@U`9i6Ha$r5c|uVx(o}-Xj1-%53O=a zEJ@1nUi--D7ScrQ+)5C5GYc_gUm^KTcqD+NOSzor*A#_*DvT^R7Ku=O5Eumwm1+5N zA(SKvU@4AW8q}AwMcBkV#4)Z?6bjVeQXUebj*u~qnVWwv@NOqm^&AoAwu~Ea%b^{9 zEUBeJB4Kneq70)ql%hn_nEg1PFOD5K?>cYdA@=WvmjNpe-7erZgtv@j zp4_?BQMrMPqY!GlPW*c`8FL;&D@uW%fSv(=K6$V@hpM_F&CkY4hj9WwFKztqnmp=hBG%) z*39=4@1Wxu*EafG5phvbCyV;2YUy(3vw9+adl z+wW5$`UaU5*u1p9U9Ms${hY@_{wC!)@B_Epa0SA$0 zk(~&Ln=)EtpeT`^rI;TWR$QNlKeaZ6YDH~557S+VGXJypSJeJAQ5{7ip5zzF9;~T4$U6H5<4;7t?d@xB(3w`oBqlEJIR7mX`4^y0A=I? z%qvKz^D`_)wM^*1XEok|bbF69O2<2?c-g)tPeGGwmmiiOw?}>b?b&8NJEWN>ZH-AuS0(DP97_8#nB^N^t$;zrm>RJ_;sAj{o{){E_b}4+6 z@d|I`&1a)hfnbRrRe=Kaf_lxE&8Xqmd5tlx>}l8E%>{kKkE0v&U>0K`HKnEY9XpRF zoJXwcvJ-+x+dtRCXv2|#jql53aPc*ru*pyP912>`t#kU#-E{<|g1{;zd%aljb5syQ zAmd6Ym2uTDMUIBgCYDvaY!Z7P{D+Lj^DSycL7PK4eBIXVGzJic4JMO;YP$?7@R{JQ zqb{jHMb$)N;lc2jl#o1k3qOT5b0JqHjNj*LIn!k^9mVxn?;UuQTRMVp5 zz6D|8q{qJ>QE?}1QK)cvQky=CO|457lDo# zWr{M(vewHV>`5LyfsTBaTV&0miGdB|RM->V?Yyf=5$~|b3ufNA zZ=x+aCc;>Bj`@yK(%9^6Is<8p1e@D<^-4<4hR<6krS+xOX783-^_q>#9VdvY+MIr^ z#p>0{hME^uZqDk$ta+2TQz1ofj#7(GsO+ph{z#%RX~gWJm^59dwX|PAite$M?}>K6 z=Cl}utvhu{#t9kYd(})hX21kH6CJ%^C;TC4o^xo~CCj#vma8^nZhAv3&Tc18rE_oV z>(+&x_6FYC<`nY|yMdW9OUp$BDCqTUh z=x$*ciL@EBKdMYERv-eiAk(v5RPyST?auiOwT{IM8>yPEcXAfWMsb*Jc6jLsi^rPj z!gW$y@AV|QB6a@0HKJPWv3B?_uK579Lqp^?#;Q{%sVk z{2)Q}&^Hr-q}D42^50;hWMeyAK{ssG7A0R7{WiSBR|!vUDuHv&BY8Gx%S~b=oMWJD z>+9WKQF^AbdXjRsjj8K`+e*T+550q=>_yK=ir?fv$kBI<n`PK;#p-XYSaLMeOo^J@Ms%yT(Re)9gec2D=67bVvADtgWrY)lJe}uS zoxy!Sh=^VJLHHm_GJ_H|RE7=sejf0{<|Wu}Q1g4&UAX`A!MZt5ceX#1 z)zy_X6ykj$syXR&v~ss@uJ*0qTcQ@wS$BhVXs$i7&s88Nj;>JJ5x(OKS`KBP_w~=2 zpWkG@@nw8Z!Y6&_L__y*~g4Dn@Q)&G&C(*5->ARDKtAQm{C; z7ZLw4OV~Hy-unw!MSgZ~{`m+#MzdV&{epbCc|K&db=2XyeC*yI7<&>FXADo~U*Aq@yt)EiK=Ei)+esN*i{oU0tkfA5_rGohKU3oU= z#6jiucE1sVnkkoBfw=svRq*L%T7OX2)g=A-q_IG+= z&IL|bLOXCIXjW*q28S}DJz%*Oni8p`;lgI(LPHdJptoy@&H{NyBRo{lW z^$M1Cv2op6_2G!EMt8PpHenXHUu(b^;U>dW*UQyjE)Hah zbMQu)u5jfLQrq-AgR4=J#<~WFOafmxa)R}4VB1;by;Zs&_ZDX5xwWf6cRQm~FebL) zMqFL^!rMEKEb4mo0%4zv^S|p6GEPHtmmRw!{`Z!rh9^BWY{O*J)5%um{#McT3pc;AKIQwnVTCdI4F`=R-hY`w3*ZGAIG9w^CTA#&@%YpDNUwJAxgvJOJz-c*yI&*S|j6( zUR{LW4%-YJ1gib475F(4vfWI#xJlfd*OO_eSSvlXe(TyzYLL)cExBaGMq7B0V6^Gq z(G{$hM)gDUN@32N##FE@9k8PD3HC)cG{?R&HxVhCIt7ENegL?e81=h1u^FNY|&IiPC+ zv{+Q>=B4k>g0C)}Jvn;bP1WR!XuY#Xw<5l(za7B$>LexBB+RZW(}x`5OU}ZjlqqXa z1@!#P{_aO7An#0awy@FDCFC=V4%o_WJ8#+df!2Vhq?2)i<5+$Ui3OZV@}SN2Y3ddc zDSbHA6fh_d>3;8d(@c*slD`ES6nlOIQp+{3!^!Ss_6{%R{0Y~# z4@!?1C5%k@bM=)-D-~h|HQOCV$qUQlVo>_K@ba#F>L@>+zC+09j&)tiTZDwg?nU~r zsbxa9gnmB)XJ>tP#=A~mRYffc!V~2%`DV5!mQ9;a;e{)vcc3BG?`78{x8u-Fo7Y6S zNomaBc(Uup_1o0i-}EsJue1tBPpb_`-QwP9GXfjlsi)WJksst21*B=n=uiWW(SZ~o zB~=kg|B{)!UR~FGcN|I3J<~G-%<7P$d7_p&?D07=Yox_pkckvyo=cy5ZyBj-%^SJEe%aB59JUp`%_j$q%0T0e6eD(TikeD^I9^me=xBUwTvP zWFL&igyk+i;iQ|flg;SLcw6fua>vHlRv@E}8*lb~!0&4W-CJ*W12)x*sWWSgx`^k# z9x_fUhh(_X}>#&Ba!x_rNBCW2OS-juZ;Vn`K7QkrANE?f1Ets(B2W@R*xxN1pttpaNoI zRcu$&7l^v-CI+*#kXy`z64fu1id{2qGjos!EOI#dUil?@e*SLTerD1!glE!mh_7Md z`?1c$-HNUEHQ9YrVS>MP^ReZnLlPeY|E&B{n^tX*W{%9oPHc{r905(sn;WMhTWcAKEp$%VeRYS}PO@Bc%@ zJA#)>&&I+UXLL}oF@0G+y0~!}7Z$_U*xJzF`0E(p-^R6W-!`Fj-ehCOl+)%wZRxy< zYP8*4F;#!4Q=etgPgC3IDQ0{GThVgLX2B0WSlUHs)R0lIFHXrY3DwUgiCG7Y?Z>3P zgXCEV7QSsuN&R76M+GK_St7tpnr9FiJR+JGSJxyh8%c*TPG_P>w1TXohD6+REvyLE zxuV9Cmn07lCoxAIGpjkd_NlmDSq*u|;*@j~71o7x@j4T*Ni!*D=0Tp>V$XS+cKOHM<+4GUX!9$ zGycO_)nE%a+REg8_+z-2<(54eODYl;B|7OM3Df~KgWvB5W)+vDQ=}H?L(&gS;Z{(U zPER=||AtJBUb`QY*O&JqD13SPE%rBB4rwOi;FuSj=$fOc|BLbam+<4|;7ki7Mhs%& z;^1WG=I7>QVdth}XQyL8XH#@Ce{FBD0MbhdaV2;Tpw{Ii2uk zY|_$`sJBO1^R@eP{gf7E`C!j=p;FtTMFhVo;XUFTaYQD9w{ME3Vd!UJplLgkEPPf} zyH01VAk02FfGE1y+AY&`pB|GvuJ}(%CU~WKH7Psi8WlX%!gD9Md`PGYx(j9=w=oSv zvUoiDNld&p`UySm`yk5!dSuMsGw*F5gAf36YThh_uE2J5{Ghxr-e6o7Zevbd_P85a zTuOFgQh1YO)!SaG;?IF0#6#c+$`JN<7zS@~tnrK4RcBG42gZg$m{nR~Kpy-p^!K!w z*pc}fi)8m(VX^kxND%t*v}{t0TL4Ky-It==QXG4fTL5J;oduSyZGXxaq6KX3uorp+ zI#tIAUBO7`A7KnSMCRl#s;$=NA3s3TkOP1SY`umef&Ly(dq4fT&~}&yDYciBk!Xi< z12=O^R=J|Ga+Kz*7#Ti3dh@U6N{=h|oSuu12%oH+1FS@5wIRwm8mFqw$D*x60WKrY zlIV+*VLl}rU(*YFvbwX%P0}1ST(Wr3+?z^w=A_*$2|C0H5@sc zhGH`{J;*8q^*Uw{_!1>HGwt* z)6k5Rh%3>hCdeOl+>Ob;RjpgjFYC+V6`qT*!`SquMbEsZ>?U55_1JPKWgSPupCQNA z@|2cPPu_+y5cc|?Ph7ZaQxdi{B^srE*5N(AeCRi|SL+o}2i%E5uIJ>jBN z1=wB$S8u|UNyz<+B{piAu2IuQW7E9!4NX!7-{E-GXNWmWHVw;yAR?f$7n@U|5(YiX z&!KmiVALoYb)78IKYw<8v1uoUKyDsyuV*SRpWQc1NOVEx>&j>G!o4tZ!!R5-yg?*s;qHnYX(1wwj~xqVKZq_mznY`9J?diGIJ)=aP~!1>r)9_{y@*W801y!&V?Em)Qy6G>r+ z!|wHYk!7+->!E`@4Mj`8{(IK{?9X2ygq`6DL#BmD52EuT4(FJfOoLfJ8-fI?G%x`ITmZfE6y%hi_Ov%n8mh^-}B z*COBF>*#{F#-8WaCz4Zjj@jiH?yzZ2vJb@6M9GOFI?ZfzqOX!XNT_EJ+)r5?9Gj7B zAXMRTDAj$-CK{{<;27Hv#ANal`J4v3^2H1FSD!o<+6T+ZniUw58q*KfpCuW^Y;qA3 z?v<(&2eum4Lrh4w0vaR5Ef|FyBxFy^?85#=)q#TU2bVqCUOdYFv=!`#xkqLBY)PYdk!{w;XIekPe+#ADckQ3RpfBqHP#>hL4vQ&%{OYLL2%|3ey-kYp8y5vkU9LAFH^S99fc{_aNsIJY79jdZ^Q=S1|jiTv= z%@(c#=)|*y@5HpEEaf^omK3ecWI=Yvl_5!C#&Q$vZj-YDk~>772Lss}9}ZX`C(4_b zN(UY&YC_4OiIw!5k4GztzymQ4|NpgSH>-p?{SvMCh8 zT~$uIkq0m|DCvXs48Z=k2E~UEuclo_bw;03Ok3QJ{T$K`49d^uR>WvbrxY9dwh9yZ zjk;zEOqDH0YL1w)g}RBBVc_p&yQq|uDUpS5PCqof``KR&&qJCoP^%M;ks7U=8EI33 z<3f-CTgH;Th}81oA*D*908tYR56_?uqFFL=Shjs_O8=VwzH&O>I_~t1-!tfy{|T_KHNVB% z1`#A&7*m}u*w)6}I`Zg1r2N}p`V!MlSlkvuglo~{}|&UpR^}ToU~jt$T_e^zP|WG$JJt-Ol=XJ z{(~*Tiy!@t0P}7dv5h|=5P!?4c-E-vs(9|!H&b$ljB-a0bH|Jqvcu*KBu10C9kOtD z`Naz0+(TU2ZMpcI_Dk`@70_JIi9?1JFGMJ5e)2h zbdB90(Dif?g#((^d>7tKAL2@A}w>`{L5<%>MBdboW4eH+{@ z`xJzQwL4t0NioSM8leXo<*QsfKfUPs6LBzjzV80C^rR8v0$X=~NN(tN*hq7J0lbsR zX8M0623%?8Ktw7C2e$z_o4Sp!1(28HmHE;J>Tv+MfE=$zH76(cS0e|I9jJ!RCg*7B z^oltDMN&XLVM!@IZhl^VPDv?tJ|139ULI*_Np3EFDQONqZt2%W6!`yl!G8VG{|ry% z=KiNyo*wccb<9aqlVIpZ@TR#~q}b`c)8SxyRziURDxuK!cbaJQs1XVeg-4Q?m=BH| z7(nBSbul%4M0hK>Z2OLXHq5#pGPGDdTRsLK1vDJC{}k{k(Cgco@Y(a?b{K9SL=IK1 z7B{2vV=u~=1oLcbWW2^F>jzYfyw9Gf)uFY>^92yaye9m`U=eF|YlpC_JirM7n8^g4 zbS8BgjCvD7+i1~fDG^*4ND*5Y>G(ZcL}t#ym)~jk$a+m+Ci*h|kv4;31*v_;bK1MI z2<30Vy3E|sbubEMjLNFh4@t}Q@p=-hyAOBkk-PiOqk4fZWuF+^`|O(XmikY z!k@SNah{|nfS=6#!CK-S%HDY6`9D?LCHZlb@(n;8GOch*g$Cjs3OWF*=|3!8lC7wm zjXaGGeGSHFSevt}VPnQf4j3myKLwmA9LOLJ(awbWV*M?DX@5#Pqn%K}u=>Wf&|%b# zM_~eG-QlmvzG9z8ZE!Qn3F%KwXRK;M#eAjixW({h@iLTksUzvR>c2@X(|UE#cYtT0>{p@hRRHkd^5l}`{B6Z-5KsSX`5}80{@N|;w_zddiA3Pc;VsfX(-HjtWlge(BW4(|D>C{}Z z_0slEFqHf~Vp9w!cap|D-W`bB_M4{U%b=345W`_6QR?IY!RVlz6J2h$EySI3ab#J) z)Lw7dyMWNa7sh9RFvSymViceTfncWQtv^Su@tkL+6fz!y5Y}*d&F2%A{SX?Lj8sxHvr$1@d9(Gs-w{H|c zrwk7WcoaN_-Nc^G-s_-L5_ONd=VWYYiam_-0Th{f5O|sW=e}J!!nJDzlC2@w3fmG0 z?$$slH)jt;mwEgyi@_L%ARe6Y=vKnY@aOxQxQfssu_w5W!ge@b2q-J^Eg>26W4>49 zA<9W!sEBdqdnK8esDQk=zMe>>)1L&_qOWzwk-o7O4`aA3i+3EA0nAvE)P_SG4U3{S zZwX!44_TM-ODFD$JqX6$%e}2*=Dhq>rF$nrKDfz0MYaz&$ENk2Q-?kCPJi#G(?C?K zsn(qiY4@OJkPin+8e~H2WnL9ob6~7YH7rzB(tP6h5Jgb>QWgRAvS4VEXxi8Z)iKjy ztn>4_>Ku|?Vb$PJSv~_wWq9|wuYyJsfLW*Rl&v~SOuY>G&&iiKctxcJTM=tGf287A zV>WB}i996ha9iuiCKnD`kv8StL!qJ9? z?y*U=5F?Z36rs%{6bx4(dxr5*^6Gyh5{6TcEUTip0!oNbeKYzRh49eU5RtPo1t9#Y zO%n@3Q~vCTYc6krZbrR@iK_?m1WC?`Yk&tPXIxSzUkcxn~v#Gnr!P3NnW?#zUbplh}iTo^qlY z4nT}k)Z??B!qTqBvS6fM;_{NR&u$?XKf(uU>O%xV^LkHbz&>?}ub>`1LOuz$UDH89 z1G{5(K*5E1j9&99^H?%S@g;{8>`&8#o^H+myiwLK1pJzoyM@(!6}^K55IEAG{tcKxE}fG+UYCAq1N48se&9z1(w zgL9*nKuicLzZUtPb%P@_%yr3hZ5qREh)9pQ8T%$?l? zHY_Epow43-)09pVeYWXU`-f>C7^9Z>xouP`?2J#XFzN2I z@7xLNY(3r&ft1sOCG_{NXN33qRjP1il&0NpsRgam+}X)GBjrgxox?)}g4PDO=SE%5 zdGGDl=|0MeNGKTt`1Wv~xd+3lqqZ>IE?F1~j4+Dt7@KX^39 z2GSBnFfEG47KSpQZ2gETeSk)tJm)p3cK||eAdq`*9o)g7{jl~OQ1;c#^D)^q+UuV` z25|*3f$Thca3$|>48Rg)h466gQr}51wj>OwifHx`eyl}T*x7@k)4`;PX2{B1Ey8sdS+-$V5d z;=0T6op1i;kC=DRp{HwWNXOdxuNyCbaIOrTse2WtOru@U< z!KYdY7>2=HcZ=)T-cr`ZuS@UE4xxQjr_`^$x*wRH)g24edUF+>ED0>{<9}cK+p<3v z8l4&Xv!4Wlt3#g__E4-L^1T;{(>;@ZHRM@-Ih{L+Y&07EPVfgLkNPZL(of-L?-aAt z4!7z=vt3I-k(NpzU|+(i8rIC-)Wy}=%*gJ4A_rq@SmvZYCk!xVHje*&WRt!jdVwnU zOz@24Mr`dQ^>E6Lof~14VH3&&s<2YTU--aI(tLK>gPpI-SCV5%YE5MQp?d4cWZ0@5 z!oTOaE;Io|O&6gRP`0#1 zvLxf*4YEXo(`t)w-2jvvXxpT6w)6mGV>#wZZYn_>RMbL{Ace+~-18xHbf96C=#`88 zAq2HiOZ_KPD+*0P;m~YbU%M42t-5$Y*(YmElPPYZiZ+pUY7+8Htr#PG(OlXVYyLFl zomaE>g=lBV5F6{%02;Baoo*sBoPb%52IUa3HRaSO1sICx0VFYqXCR}jZIa@~YW30~ zN<4Wh$4Gqf@IrjkG*z*9ATXb!-xfthH-`R$smd!7n^dVdjMi9ry|^i@sbaGqiTL;{ ziM@=DC^4WWRIBlfF}iNLhyT05_d z1Qj)x7TMmcn7B*m;*%6n^+Bc*> zRcgz$tK%`CdhU-`1ZvUeKyo0hf1%JLW6G3g*TG4JO3?mLU!nr${tXbjlu?PkT88YJ zU8@pRzW$8K|N09qo(?<7jhqSqv-@<))}GiQx*ROo=5pWrg*0k?SL9Om@??M3bfO9p ztx5-Ko&o2~x?I4rJ|IV)>3BsIC)i?4fkU27>W#Aq#)B3_3#1;M^yadzaY^d)*4(b- zXMUQ-tBRxXy+fyu5tGi6IY)pG9JB?$6Je#*opVLH8t*mw{p?1n7cTOAQ zKfy==6Ue&xth#G)g2ITKLgLkkvd=AFVlX~3u7(KWX&A(A$>+EA(J7!lT3Mmp^yhvS zJN%m%)R*y~2Y3?>`+Aw!c@qPHx~t~;H)%{Wa_zYoA7cU))}e?fu!|b}Q!X;S%Gp7y zCGfaCW@){0csVAZZ8UDfK<8SM!^3Q6n~~q@dJV zL@;(?&A+5;n(6q%QP#gISxQy*pHo%bGQqG3-dR6VIWSYMJ7d_!qGCg+mBsCB$2&Yh@--P3 zLTtF;U%tXJW&N7j=dfpPq>93g5FoM87X-ePvw1@ZK&y#JYvImE%tUIW}|fR`{WIF z)`YcZD8?pQP{z(J`jc9&#=5*qcG!^BU8B{(Y7-Q;ZY(rCR#8cMh$d#fo{CPFf<{HE zPcQc$zGg$~uMgk2IFK&;VerY}p{>-OZ!%@A9iR|6m`;f!c$=)tAL6#KFyY%MX>%!S zoV>OTtf25=)oFvnRa4;u54$u?rT0UY5#jxMFqbfSaw>S1P?9K%4ISbI_ z#*Xc`jm1HM+{fm63YX#K;=qxF`YbzF;~HN)KAT*SBsi&+-Lv27N;bixhS5pqTaxV} z<-kg-o3Z!x-L=c}y-KE2%OOee@?uHQn;GO`jax@cOQ)7-C=)0Sc{L<>{2P<>H| zR;Z}stc^%7%_B!jm}uWyP$|BQE4pYi9W2AhM0#V!?0(X)2=Vmkt+NrNBwEl9nHCXB>i1usNxo|ybVnYb7EgBRf6YcBAG7#CQ zs_XR8wXum!|GDMwsx51Ba6a`%($q3r1Ea|m2VautMKrC8O}bF@?yy-wILG$PVxav? z+U*o>t-MQ%BT2oah2!=Rot}Z!cUQkVJu4cezb~VD$O$pi3yqo%8o~T!u-fB zna^U_Jo}zt*8X5`@wST%qb=8{2e#>5ez)Tp?k z1>u8+cI03j^a|MB z<&b$klxC}A%{cPXz*(HB1bJEk2+d+;p=6E?1 zX`k`r`|v;DN7ivJLa93oVbMUL3VeoSwwU5)W*MT*G6HD`qBspB{C54>7x=QQ3;h3r z2%-q|rYFP*1W-=)qab2w-V)Qwk7L#*gg+21T) zEjd!98JyuQu}D!x#rF7ADaT81FJGyNmI=wCV(J^glE|O+YaH%W&ZgPKSO^kZiB#k9 z9wRJ=HwY?ye0y|?`Z0SF{CF?{AI7MRqNd9E0FC19iR+s0L0Zu5{76%F)bhzz=NDnyi}L~-3aeQ1~k&4 z2C!nZ|K5ropj>ZS&>{v!%-g#e3ye9FR>gi1&8B>sp1tbqwI zQG4h^2|=wrVIm$w>kv1q+B+yS*1HM{`J(VD=N3LX1gU4v2zid5Xe< zP*cLBG8sko5#zAS!d&rt8irAr178>aO)tUcW%_8W-qwzsK1QDpy&Jyuc)dD0-qMvt z7;7)*mo`&X+WYf& znxnJ92N`PVcyE42($X{v2}pqD*;1>oU*YT#mEjPZI+@DLbEVWG=_GSt!ou3IarrwU zsno@G9IQ-xmh*7~KF?rkHu(<}667GqZcohg-SXL?C!;TO&eUkq8wM;&-B|zK)2p>N z9}l5{+FvQQC+E*^*U>d#lyu+^7>;t*Aofm zDIYBIrSek8v^gI$fet!T03Wm6Wz0{{FS@Cwe~(3sK_ru~=;tzxp~*#-RfXd`#Y{Jc zA+e)UeKRqQuEUX@Ex=|0iqR!0`cxS;C!NYl4#!fq_Iiv&e)-o3?gHpb%RnGMw6reB)9U*AyD+^VMI=1UGqmy?rCHJspx)P4N$6GnK7$j|!$4S=UFd3@nn zSnLoO+h;#RC-#ao(S|Kj_=I&9tLzcKUAm)qH1$;~G7^ zJPCBMpNf%QMPIkH%X2ANBDUJZ&Tg+qp6ARKE>nP&nLUj#(~?ow0vslEE>cpcO6l(v zXRA@k*&I&DG;sXo)0n@7!hpUfmS4<^Bgdcn7f;=_`pom2Vsl%3^S+G&XSPnT0$K9 z;R^RJZ17qJ0e(~+oJt}zudnAY$$!fYd4^tuW2nc|I2Y#s-X@P<)0d%=MC8tF=pCCo zE~*R!u(ssEw4PbkWb$D9KxS-`Sk5*OXR6^CrL^8_!7rtg4VnB%c0W|)-w1*c z3MV6fLB$BS?s8;^bpkC_erFHoQsVY8z3|WQ69iTr9sKx}vmef0<{{1Ol7p<5xPx3! zyoi7)#%gQ&ydooBq<3Rv=Z0Ken<1D%z}KmR9y$<_Dn*ovL1U0=x{iCA7QCl zE&!w9#;|fl7-ADPw!(h({(00+C~ni3++kHOTG`2z&K$8;hrEP%&kRLkBXt^$IeK9x zRk1ch{mPQ}ng^c8YNn~c-i1YA#yTZm{?t-VX(ADYJ$*fQd>%j9vcIbjBwRXk>Lo`E zLJ*Ko{DSpH9H1hRHoke&rrze2XwJLkaFo({(1jy4RLUyV&qi9Yn=riO6keFx0Em(e z`)87I7JVq4Q8Y_@eusDpYX0pBVKctENf_D=8FRHHn`cg3qr3fPFZ_n^)q-71Do8{F zW##yf4A7faoSb1n+5QhD==l!9rQMr=p*68Y1i}J1lJ5RX7*8~X1LgdmWV9yX+-_(9 z>;HymWem_s#Q$N2dfhlonh*g1y@{or6Ar-izncPvBw)Z;nK_y~R!ZSPSy&U0D3Lh+ zAEsbtVr2V2D)O+lodX^R>UXU{f7~?|y@ljVJXDe329aGLCUhdcqemc0+e{17g=8&x z>v?alg?eJu@}|9+9%6*xjd!YVnD5CS*`te~XeQ36s8oC#@hrWl(+-s3d`l= z^f~o3j9V2);bAJSaWho34edJ%cij2kPHcw%&e#UKG# zIFhJZ;dA3|s4P4afF@9wV^cyQPQ2WhSWw-mi*gWN@qAEDCX*oCe5H}Bs0LX6T8tWQ z#Dw2Upxil@&a&X?bE!DaD9~IPpg{s=I!NQ1SjMhvPE7Ei?S1Lm|J9Z@EXEUcSrzia zvZ&0O0LcDN&?Gt)PT!`nVGgB%uvBqMftj0NP>_+C zt|4@yi>$Q~cXci@`HssIo?bQ)g?kh4Qx%>?u@Y81tVKH?mK^RG zXzu0HrL#9z$@BYM@OxSY|H(xRY4;u{0_xD~?+J58&aQnI_hZcylezkn-GTK$dW#B+ z@jib!5PRM1j9@c;(yV@7?*1_KmTDCscb*SkUmpu`nW%Bn4`%p6tdm(7>~)4gCI|4X z92#dVg~O_(TvVOyr88B;vC@vBHiWU#rUlJ*umWvMF$Cf2q9^aNh>m&V)Yn{LC?yjyM-^UR*NfcMYgyG6$ zm5(pnOYeL0;T?XH_7Dp#=3R9+kQZ&W#Pc^(-YK2FBOtH|0-#Aee%jn2n2zIaP)M<9 zK?Hp#3O}37i9oz>S0z%)IC0_aoWt;9MX1zz)L1^t%d0OxKA7<V?J$O+N$b>BATS6cHG9e=0mW>204Nj=&QxllKa-Rwx2SELp&xbPI zojN-(;$u+~l{Tq>R>q_$cvyhtA{AY6ek+$PlX<*4D9kGGa<@z3sLKNOyzlrM_-fML ze9yDqZf*{%`Ec~M=g6IYBTWH4c4tn%y`LNQk*!6Rz^!_CaTue3>_;VWo-E4cQPLM= zT`nN@W#4gG1`anW-7r8twjY}&_J{=q6oVSkyopMn4`g?Xp`|7uGXxR!j$+oF9T&o70KYVJUuvBoh7 zqL@e80Y8S3?sWy!ZC)KJ5+2wu?jWS~zBgiV{#Vo&cDOs!{Sj!&qD>q3`ypu{bS1~J zz}s59zzko?+Zl>EiVmk?2y5R$5|LSw+f|WXo2!cSXzxKvVGQe;e?kFY8 z$FE+M$L(;^o=aUk)r8IV6ox#PQ>9e9J zuFfYG_SQz8RIM80q)0D*@9z98P{o0mg1sqqDh?4xaYvo{P6oQ%ERsJ#fo7RvsLoB1 zz-(+8z!D!}3L=w0T4~8MG8Iu{1SDyHy_;WsG_yw+G|19!i^s? z^YZDICNTqMExH}OY5mbrQ-2m69o|dk)wwbP9U;~V)KAUAGB|U@af@H@6aIA{2es}` zoBeI3PO-oPu~XWblbR=atF8X8_OzITc$w~Rr|DgOi{4lOoF~0IrgQqvDQ_7f^rSmg zkUH)3$?<(uPC79fVWsQ=X6GXJC61wofqn1jPAV5LS;%(MOiEuGKiPy6h40+)*-+l| zbt>P2`^)tHyj`%ce)7qgWc3dA z!r4T0c!+?_%1*>c^gp8&Uf%y((_mp@{J%7fYh5Xi!x5C8**Zme$ULZ=ozDF(t~H!l zpBdtC%~mof$q+J5^a{uOnD6eYjx5bg1XOgX>6l$zQ4s3Fw0G;e6NOcW@Av*pn%|M} znFe~hdN}@O<>acBRhQ?>y)3_>^NwPaGrQ|jpF-xZJU&%wH!rOdUBsG(bACRZ-Zu+@ zxpfpj-QQ4MdU{Oauq9iY=V~($^P&G3em;zQD$AA^k}$hus1X`U0~|OfI+r%#=O=8b z{3W|Z6M`b?hU=2H%mtvBH_3yaT>g@OAgQskWs+&LKB82t_F0a&>JNe@oS|9`K-ff5 zBALwZgito5I#hfw%Z?@r+eD&cnv)s<L;zA>RK|3#S# z@UxjTe-s#&p!UHp8s%R6X48{`ovh_nMa1B;;Py$&y>!|c9C&UDzK46T*OsmLdBpVZBT|7KY}num6iLh{_7wU;jEi3!>yyi=SXw|PXYwW z#kSZ~v|f89V#ZFQ-P(5!Ah^0KK|_g9TbZctxE4W#kx#O9(j7J5edY}F)3rt2T2z8+ zS=#8gZB?<<>B!-2P|GiF;+JTF;%YAl*^--%V1GuEyJd0E$^D8c$yFVJ^dV%Z94N(+ znEeSKiVPZ;tve!}dc6F0D^H*NY;bTBI^`M`twvX$SEV9X_H@~h-v9&r;NsS*siEK9 zaWJ~7BCgO_6>bFTiw(yZhrS3shc?CA<~x?Q7WM#CwRZ1Z%M^%^@EH|A8A~FFfs%oo{!?; zW62Pi8NE`Cf$NZji_*T+tNpZN)v*bS+1H}mFU*6a@jDSmsS-p=#am`PME1Rn=onvb zE9?PWNW}ln;Zst|8i19Q9xVsrF#S14J6t^0c5=&Hv1247Uy?NS~fN~C5W-4!8Y0D<;LH@9?d(tWo}0n zMb8v6zCqc3*6Z?m?|f-**Io{9?FK-1%WdMyN)A%C&xvZe#T-4da?FdIDQ;7cja%Fb zWF5Vf1^Lr;7X!GG5pb*-UHC*ph(rOZYo*#EXeL1; zD}VfqVcbu0Nua2j4Uih^e-AOl&@#nT@0h>*%_%*|-n*E-=LUewq^mQ43tvHN1(v9% zqeF#LcHQK`(FXa3)S#f!=KXZhoqiiPIV-5P_(T7|Le?W@qUW|P(D<^GZQ@pUYN;zs zTB}X3mV5xkjp5otB}edc>e?0XKoDCX;S{ch6JoCM7&dY1JisvPpO9}Cml+X*T&+FX zz%%_eM8?pVOd=Y3!f14?Xn!;y(`iq7?QSS)lUu|F<(Mfnut_*KJ7vZ}+2L!*3#vg@ zf@P?KN5IP=SoHagFz596>97{3?d}#+v$W3ODNlim@+0*aWC@zKB}%%XX#k7&N&$V3ZfrIk0f6HC05qi#Dq;B$li9}7PxUl4?TP$KmL z#z6Ul0jt~7Q8oKH2n=mGE) zj?9X!OGR3#d@T7r=$KqD7tN6XA&CO>!{y>&ZC;CFZ+Ix8;ZCp4uBkfi^2mqs;Ub&PtvnWa zYkn||i#9v1yLj_fkvIkJ-Aj0M+^z4vcBEP%AsEee_%!;LYveG{vNu)qb^~E1nT;quE0^2U?T~(VOTz7Y)R4+Gs-dv#8?( zgW(n>(DsV9~eIt))$2OZ<4{dUUnl_(R{@Qk@zU&GCFr(;wJDeGi(L7h5hxV-%>ZH7&Pz8AIZ zaaLO-;NF>0&}qO(NCZUhvN8ygC;>KB5?*Sx|C{ZD>ZmEj!}{Cz^VzEDV%RU>ngGVT z{c#8x`Ury7g~jN?d+H403~~JhI{KHvUFDUU69%$v`q@2z#t{Qx#-IUJxJnwVo4oA} zT#yGe>XNpSyKXMHCFTD54R5Hp?gVlYeFJ$bFDxvZ4`q(R!v<7-0j8U3_iqluSiV9* z6;;V@?Vpf}5O_1YsnZV*cU`Y7A&Uxu61Cw|DLO5j5X0Qor3>-;h(g zET5;QV}sLZ7=hB~_ScQtbg9^k!It@=fnbT-^c;_yP=q0ShvCP@E449>a(WznPF*Z#ub4)(vZ&9mHY{nl_honZllD^-kPKW$lz z5&MK+f3VIz?^|dleFrMw3(nHAaVvTq(eMF{#cu}Ip`S9Ax*#z{Xz$t!?$9{_&+~^ zhSm9LEbG6eLbNc{i<*pU9ZqZFy#^#%8D|j^S~mC3V1j}P63&TusX2Nipw5ontm0qY z-2$0tW7=`2Qma{P!DS0GZ8reNBrkan5at>A%+;?ELhCr<$1#egax&sjtOkJhbQ3e_ z(M$&Ia7g|=haf6>&%GL?jh#>5uM?!$SUZxWyE{rSW@eWEqhDTY>%?ucWBTVA6iDEe zYwC+N3KA+e54ZFc+HtR?+5SS;VVO6!ZBNXR`t}phH@y`0$(~_F8U_^-FSUHo?Rm^` zkpz19JU+#VTgajk!{u)8m}OdI;tH2c*srcLJViy3BtqwSC*`qg#6FRy~(oiF=Sl zK3cZiJ>)v6Ufi0l2clAuG)xmw$l27NX?A{{VjD*bcTk1Z7mj7i8_ldt2NH=gryP6Q z97KCiCE$=luw)R4k63xwHNiZj2yJ;xzpG%k*-Xl%paK_R2ih-6WbI91Zg+39=HEA+ z$IlN${CZX493m;$O;k}zG;=>ylG* z4{yGab3AGIJJ?AIvF#PXcrYDcelb|EmJl-jP7mmgfDFl1U4Zo~ zNmWPeL8MTeAyA}Ze|+qTTbz=_D%T*WD~9D4~U5vDDD4GZA>y z{Z|p`Cl-7|ZKA+4gZ|j19Pz(fDQh+OrtREJQ-=Q_Gmx~7jNG{L& z{;hGg*f6G2Ai9B1xU;2alq_1Dxb_ZO_m&?@NT477rII=qWPss2fC!J~XjS`% zf4@jU55te5!LQHnuvKKhFrETD+jBpFg=P3VHDJ-NkNB&h3E3P%X)RqJP4>hKo8BDz z1PutU6Sr?nHp$B(ZDBL=G|4=iGVw}aFj zsG`-)z%DSYQ?I-GmOh0JWw`D@gOJo#!wlT{nE7;6PseQ=e7$bv$L8Xi-`A3cqeOTt zmwAc_v(lX~u{xqb*6UX$n++NgFRa*+D^ zFB_kYZx%cJuZA_pgFaePJxojPP1w#DPSI%r zM7t)emI-x;!q6Hw;lj>g2poSJaf$+jnJB11dl2CV^`619yV|fJ^+#Xr%Wcw$kbZ%L z#V6t>t-q8PxUr3Q^|OMjK&pjmLj-V%J4(|Ia{_{y3-l~TX?-Gy>wz6zRtZI5XtCdyf@OT!795vVPp9%+#IpsyqOZ=Dr@r)?fI0UWpljt-aiEwm=W}%3O60#kvX?F+0eto<##*<9{nd}k z7452vcSzfBr->XsQqSrJG2Ou9zgKj5{X3E65H2K#h3^w3VhC9Kt=8;BVWxA^Yw;Mt zlt^7oai16HmLHj=wrNweH%NHsqT_ew%5Qfd3ttT*tQ}=vBs3>eJhvFvA)w+7H4*AF zN2RUsG7KgNlaNg_bi?8{_pI>0ZJt~Oh*LnjhFpKgN5QMN2~rOfek=g7q{K`#`c%;I z-zKt&Qxe$Ai1ISb@zd6HUlGUCosu6Pw_Fm`$;MO_r10>o^;-AV(%)P_e`;`S@=mB& z(cFhNdW11*6=>d<`e&ODCkTm;6k}zaAUE~?TEAm{Nm;r~@S*=8*`3UvnPL$cA}H`8 zZ9tA|l*-w;#XTn}8MxQ5bXoNov ztw1{$3)_zQ!skiwyGTgh#EV*e<0uU!Jv+{gUc_?96L^4r$btdERSQj7#`YgjpHn7p z)ADCy?kVJTgI1e;rPL+FDr^%%zIU~isg)`3(O(9~fC+Da#&65;vf_YYy;RZkN-7fh z$w|kD2uZf9AQ*xhyOZaX)nC$6v(`h!v1mm_nN?R`++MWQXZ80|fIRLZt&!3b+^b;G ziFTxXtqi^$nNlD!M**{3v+Cec0>5b^#7Z<2WU7I>5@K94!B)E*o2uHbOfus|OA zPZBs$o2)rL6H|FM*<5qD9*1kk3r}m2C-)qwLymcljchv4Pl~XCy=;ubGbGw9F^OJA z#Q1uY7@kKcTkYtFMeshvJwMN0v!$s}bLx6oY?ERO-XzdZ<#&6wE)riIquEH9_Df#! z^Rk;&o?k=rEi8|P8MC!!ESup3oixnc%>B*17`1+XWJyETVj8u+q1ZhQjXryo zXSfE}4HV=;n*r|YX7D$)#Bf#i>T(aAMlfNh?h{K=^O*42eB=$1g}m!q8t>fSK`_0F zPXY(Ejx%5ii7x#9Z9!G*%Z<{AiB`<@rU*vZfthNPSWI}H@!nJERwxuOH(JXLi@$R) zTcjc!%l+5Q#ZQs8WMwH;SL^<87i%)@6@+}E zk-%ajT1tk-5$hGi7v$B*_uowtN2D?dHrV*9&H<6r0$hb0bM9}8;L*1$Yi}O>z=U8a z_xZuc`!tU{P<3Eg&YV64^T%?YvT#c!y`B^h#g$V1Tz2Qlv^!J4^yMY(F?#aKJy*c3 zLHCr9_5n&nuDEdkN@I+dAog?5q-Smw(;8sjBt+VdU~SZ}Md)lC13jZQRGwq(f84^? zsP73k4xL>;Voo!fT}{MV!JWp2N6q5d7I}#pKM*LV$a(yD9WbGQVR~&uq0k(bh^oILBGr=T9kispAQrsb2wpo!HIEC7k_b_=LRw4Noy zr?K4Nz`>!Zej*ctqZ4_){@B@2T4>0jTv_|dKa+|Ve@UF13kHY{;w(hAh!w#I;9^;hxkS(WCx1WMT6tEKsFf?fK z5mRF*oPn~N$PR+Yb8dK;9TS9STaJUoiYAJ=)gp1>Q<-cDHhK$@!tbLfh6o21W3#B< z4?spIkepMFvvNa+yKS-l%>_`X@iZ?XZZ7)^8!^20XYy2_D}_(HcoRZKZ9@aa$V7N7 z36c;=J6lCdmH*N`4N|%9t1dfnE1!8e4!7Aa%e$VhifIhN7P-xBxRvYa6m&UM<%Fh) z7f4>^)(?EwD?`+-o!O&&h`!84+$ao#UWJBYg$T2#C?;0XvHA{784G^TvCUF%g$?32 z2`}Vl*KzI(S&T0$9p|%hfj^=P{eCo^M-^qg1H4sXl)*l`P%AAYPoAGTZ>qJe(zV!+BDp zo%v?4urzkdT%SLG#_dL=o5>K*RQaZSbM(x_akP_Q;_-&Z``?{Yw~ut4+tcY*9>>*$ zMjSN%X(4CT36|rP`-ANYKQ_qJ+=hSWWNiP+UjK`!&*HjpSvw^U&{@pz9AszdY@+Ioa$U&s zeLDa2_Gi=5<5ORdlr3IXpS!*?ZcjIVnOt^N_w{jU(&nG4qYx8Mkjogs%1qJZC+Dm7Q=mza*msx0^t}Dvxs@AY>DudL(In&%p$U081DhK%W zmdjRXbb%Te9W7!4uFOMrZ-wU%mDj9KLGx`?u-0GACMW4$=u1QR9l=D_K~VFAw~`j# zI><|2!-_~jDcfmiT2gwT3q?T+@V$~lxl>A4r-k<`@Y8t)Aniu@8m1j&Z6@W1Kp>p1_v5t=RJQY zRoi!Vcx;0b!o8>5WoA?hPIZ)}>E5uTw0Q#Mp0wR6d%5%7nq4v1-O7N@TdBjlr>#4q zZsRb00zNuRSy5?EYMp_xsQzP`(mjUK5V(sHx(yTs@N~{T*K|n)DtN__+=g!sQlQtZ zAxNCI!wds?smVzdDw(dMG->R30?>5dJM=d?!oOKEES_M?P3nDqgVixpY?}N&{FpZV zR#*^_N&GD!xO&rpbIKh}98>*wFfbFL>eX2y+fT_)1Vd*2gw_J#2UOk&`Okkt&7{XW z6aZK2FBAX?JSaQc|2VH&)d&D&&`JGHXst2i03rlX4z47p5A@anX23KofaQNviueE` zumH~gv0}A;NB|=L^I~!Ruf?l1Qwe|q1IozOTB8kcL;$e1{;~w@0sveI$P`GNT>tlg z^gjS#{yzZlS4+nMuNBpQwQle59B#PXQ4&-rutjJCtXXjGsK9Oyib73?v?9f1QtbV9 zmcxc;Sz7yAdjm8z?PvxuoMl&6l3GhrC<9DtSOqjKrVNsNped}HPSKuNgI6x7p?9_) zUV*B>yq^U^nu<=9Qt>~#X#(6@1o*;HGc3QcC1(*(_?tnopY5_ncUEWzq1PiH9~Si_7PlY)^~4^bN;K75Mo4|uk8 z^}VYMj59|_q&}Iz9BQXNL7R#aM<7uQ)`mUcvknn|6h}<-OQ|VPaxDnc5LXOh!rhyI z$*ut!k);udePW1f2aUj2N{w@wtdC=z7FE1M`-5*H3lX@f{MD*Q_;8$f(?RiDBFf!XbtC!jWwva{vJ^G(>tbb22+fV+s2k zG=k5{=)sPK#%yLYg99JEDb%I3DNF+py{ZHD)dS==lBTm`0f2RuB!g?;@!Jji`HPfd)xKvwyt)UA=RMH!C!B?OX^O(L({+Y z^Y$T&Bjmr37O3038NyMBitr1vL=;P#RxH?6T?O1y=ta%*nU>Zd^tknM^|Idg*LTzV z?)LGCwI6{?GQ&XHf5-6XA)`e%k^rtL7m;TF9f1e3HEM3A6LA+2tr$CH^TI0A4G@%$ zC!8jM{lehWHVvf;@C2`w#95j3yePM_)k!?v|I-L{PB{2bmIs7!L?mJcec6A%84QmW z;&xAuTj--Pfk^goWms9wlJCR%h3C@qa{%F+g)(%*b0;H?>~p5-x?wVEas#wr28R+I z|3CzgMGVB%?O_e`_ktG8n;>zf{#y&aY%Hk-1EeuHtla9?m_UQX+65_Xw&xgbE~gr>BOacw{M=h`*}RtyKS1K+Mk6l*J-2~)Cq*n6hezQ8iz&d>+kI8 zm_qcFUG_OfYw>Ni)8<~?3p0FP!#B^B@^5a`uFTd-LDBVs@vzL2KaYh|4G-%^g{SvF$t0t+oY8C#xonNuw*S9EzA7jVrrQ>GcMI;p z2hZRE0>RxT3GOhsGZ5U}ZE)8hgS!L>9w10?hrxq|aPpo1JLf*!TXm~ndUf|+`>}WJ zT5IFJY8y$n@*Q9Pusl>%PbdA32v}2F(yBJ=!hKPP-*n^%J2&1}*GL@oIn5aZ(4A^K)2^e*!lJ<4S zBZv=- z&PPzuz6R3RCK0{b5bY276}>CKUz&!=;TSOjY15i>SWGU`7sc`2=AvIt7%k1k`h2A^ zBsnHYOrfExm@zL>9avVO2L(%UGM=if%=dx!M{q5pJR-i3sRkfNWG||ZH6m8s!kEtx zINA2=1sEV_Gwkn6FuOXsn0MTBHA>4}k;Ws6R|Qk2!J#^q_`7NOC|BIWI{8-3wCh2T z*;d;M&6^!-ZX~WF442u=-!(rcP^*i-xLIZz{%HxAo@(`j!6kb?*E|tcr>Ddjs`J*d z8&;0_+u2@NA&ZIJWi3AIg^~RqLka`X;)0GLj3Me}+ zX4a8B=PPR6OsEOOt=Hx`>V7K|{3uJG{efBeA#&tM zV1WL`Uegg=?N$`A-`v)I**P{3T;P?Nz8aIfho!yy!5gk!a@=-EIjnuA5`M}L!Reuc zJ2~;ZIQb&@2b#V?^$!sL6up&fqImRnvl@LW&+91jy5H>-ByJ~ECNPzdSY$j~ULN%) z@adhvp^PhiFdTm6XB<*%4Absc?m*YuR{Hc!$CIdbqgh>6HeqT|Bj%e$F5bfs{_f}@ z7n9k%A>s~9{009wRG$35HBA|<&xm+P0FeyM8YJR2R#!w$E+l|hh9(g*Nt;zZ;x#oQ z|9^l}2jX85HsBvX+BUp|_`-rDDD+LR=a(43Kkgxdb1Bx9t%j`aTyzeh*b|0@_5Yx|32i2o;hIScZ8 z$^T?0X+yU_mZ1Avt^6mzC5Z-TBpkC=S9V!k}~=*i)KE)CRqcMr~xcAFGG#0i(t-*5+v4+PYc_6+I;u;tgWAt zXJlQGh*f_vm!oQr4-o5tSy@!P`%4iZKogSBc6h{w7{i3hq{#-k2wL1hD5u&;9}&@& zm6mbT(@KL3D`WxHTTvX%CDNe3Vk#_k=GV=r!lfi(g?*{1ReYbfUTag}GAbnw8I;r+ zm}Uj(W1;9KpDC4M*<%L1*4AJoNwlW1LgVL0vO{3NE)$vz?ay+p(xm?qmW5^rp(H3; zcn1Q_*jvBQ=VTKEg?C4;M z8L>!3G2(kN@2$`8f)FN6INQ=tMF7UWZL82WbA(+21k>o6afT$%no-q~m1BGmmQlBcp0ERn7XaiJT zfnb1Y8Qou7DacI$8!ogI15zT2N zHpZbE>{7l_TKDOUp$rcAU1-)+0ay%NM$<0u!T8E`E+SZXAGoknjRBEFB-H-nWb7^b zL8S8Y&@&{C(h|x16|8CzaHWX@g^t}h@mAK&^ zAx?ybx^L!dR}43Jo&xkgzXXAv(s>#aM+n)HHOQ`#WOD8~s!1&Fo`j_Lamd63fl! zaBTxQPtgsa2{5#5K^EaUeP}Fz}hxhy(zSgI(>N zbnIJL^@H>V5-v(uMyP3fL@o1LSc}tM^Ojl4pwkS-WMSDB-d!#rY>hAwdTHVjM21*P z>H2BV$zZS|O}4wwNtB3U$364{>6b=Zis@HLX=I)1lLA-f zr?BEUP4SjhAGHllq0=_%qSOq2?1(5;0>$8;pw7}Q_QzRKkk&Do%$Zu_79ow@n&WD; zD8h;0o~Cz=ITMXWu2VOetE;h-Cqne*(O~hafAWLYF$DQ*=JEHz0fe68%6m<}Xn6Y@ z_+SzyFQTHdsD4L}+$T6ASjTE{eTNnk{gwYl-5|}Ks6iYdcD@Cl@Xo7??O+l&nl%@x z2fP;&8o0?|kfP+|>cW8~U$D45KQ^AT#GLY>evSCUeeFPQYeR;NP9 zRl-n1GU|@cfN;q+yU{{O5k`d1nkmsZvx*Rxm+w)!v~@X6gA59VZqO+ zXMp5Exo%v2+5Ma*X#wK;k&iyWWv0Z(62W`Hxs!b6oK6oTMfA3gpBra1l|SJ$`_ueN z$En>8Spr9uwxX3Xb4iz{ESFES!5BiS+93v9`Kxi@gZCnw3FvExpkcGQQ2*c@3fdpd z^3`&F31ag;5rp2yI`G5K>?FSN=13AaW{@Gmovp*@9(Gz_SBWyVrLXxZ-U%pnZ+3U9 zGn2nCv^hP*WX~vMhHR6DcjFy7 z&3%_Yoy+Gb1Wai++V~j4w>ua1FCet`mi{>_DRT^doDPMT>*i-s8=9+Q%=`UgZ?V*c zte=8qqJGIrau-w6y)_doP5L_cwduvBByQ;EC}Ee;7j)P{w(O6^Y}>a^{xnKH>QeDI z?e;`j$cv|W0=8yRFs9dqQ*W8QW8X03{I*z8=vLV#5HXT7T9Rc2f9WkZYuSdd+%#WF ztwip}PjCLQJgpt}X%^PUg^6-R@O=5RUntZ`_+Vx%q8Xr2ZpK0nqQy{yh`R zg_(G}A^E_$sJPJ@c`qEGc?yX6xEqFoZ2QZy9%%AW?;5QOT<{bnpC z@O*Y3J?H#u>#Mv!rcHdU%2s4p8J2fWs>^o5!&vXKY&8bm=>KR^A`6082(?%7vS|;2 zMe50~t9`q3)q2t(I(MoZld~x)?Gj`Ni74!JDXMSVdBQmjf(XA^_ zxpm~pTbJ+n;$?kME(Hm8FE<~+Bno5twLw@Hr(oHpV198nNjhWv;B;dHJqwYYL zSH*MuvsAb5!}j+C`pMafxITfW|~Ya$I@>*%>7t#ugRKS3IQL*Wz9ZXhGZ+nav3 z*IV_SacMyflC^(&`*b%d<)_-{qS{!&5iEt>{Gy~P!TT}eV9$DHDQuo1-E$#xdR_zy zsBhFWlw+BxtjqrXVN1qiZ^=aS&0d{)f%*qY$lYm8)?__&DmNgfn^uoZI8uYY5x4(i zAA=yKpOUX>J-{)G(6Mx!vE5jA-b$+{M<$PRV`Fg-ViDRcD!%g-h+H*ia3F~ z!7&~1`(czWVH(hXH(i$EbB%9P+uMc(gO_5zfrh9cryO08(F`4JS$&>r04#nlzfJv zY`NVK%U_Rb`tBv|gZ9POg8tVK4ymvqsg+-$*LS6FW;VXnmCqC%IQh5Oux5-ooy>fP zJny*+P+y&_I!_pb-@7}1jI>UfAl>xME_exSv*A3HUYmklOC-v3s*gZ zp{F`yk}o%p_rqI3)v@SUx-;keXWBl~ARRc*xWzLqT^xz~mqk;Qfb2nkl_^XgPPY+S zLH_VHIo4Z-EQ@lJ4dmNv@4$9g)qx0vUP6b)2J0Z>d&hkmE8H7Io{MhH1yc<$i=>`W zPGpNr4BP;#ZFUjA8t#m}wmaCE%F8u>KmbqCJac=0DD*yQC)kryjz|X!YDjfxs17tY z0Am&g#)ks1t5YVu*2&Tqn33&9H<;8IUqO_7jZ+_XgL)6CkscH54F@d$|_g&Y^G`)p*Os@x^A7GkWIc!?^exo2=G*syhI(8 z9SY3%Fgo`2I_^&J3)rqvH-VHit-f`i@?`t@(y&<0)A~A}W#Iw7AmX=88_+KEu_6@_ zxYFE#pB+uO_h-%A%ick-ARv7~FGRzRIQ{VMQ{~HEt#dJM_=SHeA2V_IE`0atpwFqaP<7 zhdjs@#WC&cdeEKm^9G)NXN^+dJssBF@x$oCsi(@cdeztS@h!IQQgZBY75@ysY9;V% z_|WbwH(nlhr5k{HAOP8aZPqV%>&5iE-ZB0WT=Y9n9I+P|hIW_6^;;QZO}_7_7mv&Y z)d;>MoBwVQC3kV2AT+%5)~lczduv)-y@=s=0=N*xyC+P2G3-M3)eF|ZMgWCmTsWSn zN1vquETZMMnZ+j&?Nu=YT`S8UdN$EWtZ2*rcfx$^Naicp2@ptqd(_Q32ryiLh|0?& zy9Ooh>=ki;t)ASHC?xm>H3ofn@N*a)kD`lF-(B@F{@rMP<+b0T_ke;;mdxlgVu#lx z_DoSo+2D+G_0!1n4@h2CM*k_mGK{A`Y1de3E_D}r zY*6mqp?j%g4TJ0=@T_D7k8{H=&h4~K+%fd6NwTR+!+!ZEF;v=ElN7bdS}PRF!DYl9 ziCam@m0WPII6GJ~78u!HGZs0uW`#IZbCs*D4!&); z$1zT?{g~q%m8jBEV&4Lln9wMMn?kIN3$+0n=HXxuE69u$HBYU8iJbq@6poR@@bKTQ z!fc+VAmzwjVGC7|US43@ij`Kb1WlfTVSsy;6n@3sl1C1L1 zs9*q$P2p%c45ww8Ci4Esl1Vl-p*6zGl5w{pF#gu(R;glzmX#%AIqdfFM#8zvxXz*M>d?e?Njc}(;79=1S4sOUA|5u*N=3(O}Ltp?6NL+$DBxm6}b2# zqOx{|(GqqSz`1!$GQG^lgD)|uRPJrpS{ZL;waKadY9ih+_*;eePbQ@|_OrE%MTVIT z484VUl!uE%vQ5xJIT<08<=8T*beIBJRgvIE1p@dP}#A1THo&r_IrMbSn&i#(l zOAmGy1O^qtzaQ18%ohr*>mGebQoGFwn19`rOp_G zEu7OuKhn=bR4zq#i@~XbYVCEuShmhO==m9IbZDAfN4B~YUzAo3>>P*Vv3uy_@0UESR21qJ#3w~xgyD)et3E6#8=o<@iuprHPO zDTHLP?MgnHQ9gn!GHEx>_bO0JJP zD-JPDH|FoJvw7yS5GbKSvM4UsfHKwZVAho%uMB7eg0{H%wwQm=6Y}UrvX_GI2Te(W z*SP-rYUS|6jFIo<_>M}4>Be%HaMbw28a;zbQe7>ORf3D?(6ptkH~GlRG5blx_K~Il z+(4r1TwBY-h)+~wBk#*xG|?b z-1VWXj)EvV$X-@Qc4f>xJvOhT#BXn))FgJxNBsLutqbyU#hZ&Ep+d5~;QH|7(hEL< z54-~>M)yDfY&Jyawx}hY8t}gMR@%PXrgp7^S{F;ote9*o?I&29v!LR;6PYF{T?Z!M zyY#zu432MhVs9zws>ZGD3a>iqfP#NXEo=Qf>E260-l_ALVYf+Yr9~S0_ zx=BlOJx1Tk8!P8|BrsZ9$&?In#grVs%RsgyVY?>_A+|`;TcF-ZKQRDzk0}9oa>0&O6wy{98zVxw=J} zDrO=?9(YLv{pHPxg87rNai@G|4Q1NO+4wW!lBfh3J0%h#$M!9uShp{*0RLlmpqF^S z+qp&++tY8)m)Bdx!J@1nSN8z%QIz@Xr=!7Fhyqq7MBkX3R7V9iGh1$oz~P3~nI1?p zVXjIUM;oJq{=mXc0zukvQk6fk$Dic+nn?hVkd>ytQyg(kTMm~)VMgvQmJJz2!=S!b zH=++NAS20Tza`cgK6@(%=50Cp^BieTl8kG-bd+kmGP5Y*HyCQn?tEfVqi|95k4v9f zYswKAU{@d#BiUV7r z?KQKRPr0yaH0JV6>O_+f9_O*u(P6hnxZwdm(R$QKciCXv620Tw)e9)-kScb3*fLJD z*4qo3N`p18Tc=}WYB#N;zhxI2z0!0S%2IY0kz)|l<*+lyTB$EXjx!S<8SN3J9})fOoCZ)C4^ctsdh-*r3AXLeW`b6v!kUI(8t9bonkh?o5LPfDRm~Wy zpDtbU2a|+$k)MXmq|secHMy#9{C&Ns{pi~d(;H?8N?avWmfR%+=yF(AgPLisqK*4z z1TXW=EHqX;1T*-Lb73|#+IK_7SmDx3LFwZ*oQXsYxSwyyi(We^6*Ys*sfC-z9`e`eS{k^Sz_33r6MQ^4yf&ncsLCqHQy~ zR@@AcMiCw6S@9UUuLRLz^0A_<+iU4q1sgo~XV1~5oR+LioSXgxq&WeIFmiHXF{xgx zLS#wUEXHtSprEWv?us&3!>N@cYgxtE04`@4asYlYVbS@sO{g%+QZb=#F&Da?5K(K_ znzD+=d(DKE`Z@YFx02+5KJa|fxI2@rF78F*IYPO^)A(q0?6NX@gZ+WS9`U&hU!;6M zHr?hzd@L-itYmQTn{5N6XYl)1hg$LPaMt?~D=d9dKv7UC`YMYF6fK8k4SE_WkRPLl z3kFi>t-nKEc?NVTknV)*P&*LHW$)q&yb!PAp72aJBY`eyoWA_NQmJiSE}~*&n~!Jc zUkFihyL`u;*_2^@+;^qV;9G`1OJNICGLK}amEHtW z>k(4n4*o_|AI;n_GECmOFbK^j@`NW!E(JX;rhPf7)<(z>E$3xU?1_{XXJpXDT~lxa zdiS%PSMG)0utqF6%@vq$zi7MTzg*{;IJVL-YpS&&97_Fkw+}R#ZvL$L$Z4)+Z8ul? z1KgrJI&v^^gs5tQQ!j-9=`L9Q9y90o7OMJ<_UfuEc>teEa2&B>zeU|44 z_|Tp8A@8TUK14$02Sxe1y4SRv=kj1B4P5G4N3`Yt^+$_lykhcoe-E{so!$Q){^R}E!s1nOvU2{5ME*r@>5YMMvTubHL0r8wk-UNL1aS0Nf5dW)l=c|j?R)tm$Pn?pfLhy} zobZ;O%kQ=l%I3|w%^QkZD6SXwXbS5Y6!{LJT>lV-WsNnyO{GIx#0k56NeaOi{6}MjG-U+$Q?&&9#$#+F)2l5hfBI1K@K26jd%ydqC;3TCcdQn70)wHxy{S;6lip*o%9F5043Tkg^N+d4Sdq3&7wTl+4% zb4MEWBY7iv3?-f2XkKWFuSCDWJzy+rK!$H{T-dd!-I%SX^8jf08{yn(omKfTh+ECP zlPbs4v&F49ELwkM&Bl5u`h@$4L~88adhs3*B_6?JnqAwMGLHY)+&y=eLHgv80NM}n z?~`3We^k7wKWXduj&R7f&|4zL6ac@0o&>t*pXjo2yZ6?2svxdRt0QGk9SJO**grQn zq$X%nOFEmD==JK7HJ35Prdo^-RFaq9KPQJBbwl2&ild(DbZVceMLl|8zmLh5H5S`D zTlg~HqBbo46ntVS%w^(N^)s3> z=N*D5mM^!z$((aU@UMeG>CP*{K9>wT0C`zIENr@Gahf%#)#R)ZApNGWbqtb1)6dv?#0_53#0ZZ@>@0-`s+fKc@Ne6TkxvddL3I6pm&*CPwp{^l`gy8uP( zucMr5J-hMq^owwD`f-W$d;&_v!R;_Y@o9t zr;kgTd`m=H&DCZ5-Qo;A4ho49`b5Z-6`uzk9w=}Ys?V!trcYimXu3Ej2GJKL?GAtV zGA^NL$0QbwY~{h^Y9XFHQqc*P9^wBomtB{9UGag%GKq07VN{qSCZF8TpWsHiBKvCBg~OB0{Zd+8!c~W)R`yP|cnXqA`sG$#NPaYB ziiwEI11qV&!Y%w}Xw}N=gN*=YoVdf7&jO)bVDZ%xhQnV6hjFrFUxcbS@@LYR4RnQ- zM;vBpKQqLO;gsVFoux7;*2Rw$y^v=p+QMgX4d-7s`0MTcr`HT|RNp?t+AmoAIswFG zfBPx9R56@9P8MgxeY6T$sQ8vVD?-&`Ebx?|@G9M3s6w-xG@4h4)37jb+QYE$B}!pk zZ}~x~7+LSWqC_)+G*C@&t{tk7+hR7_28(r_%$C8nL3IpLo)rBF=-Fx_Iz*xo&g!YW zCH$reNaryZ`x4l Date: Tue, 2 Nov 2021 08:44:41 -0400 Subject: [PATCH 069/110] enable expression double multiply --- gtsam/nonlinear/Expression-inl.h | 12 ++++++++++++ gtsam/nonlinear/tests/testExpression.cpp | 13 +++++++++++++ 2 files changed, 25 insertions(+) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index cf2462dfc..b2ef6f055 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -246,6 +246,18 @@ struct apply_compose { return x.compose(y, H1, H2); } }; + +template <> +struct apply_compose { + double operator()(const double& x, const double& y, + OptionalJacobian<1, 1> H1 = boost::none, + OptionalJacobian<1, 1> H2 = boost::none) const { + if (H1) H1->setConstant(y); + if (H2) H2->setConstant(x); + return x * y; + } +}; + } // Global methods: diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80262ae3f..92f4998a2 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -293,6 +293,19 @@ TEST(Expression, compose3) { EXPECT(expected == R3.keys()); } +/* ************************************************************************* */ +// Test compose with double type (should be multiplication). +TEST(Expression, compose4) { + // Create expression + gtsam::Key key = 1; + Double_ R1(key), R2(key); + Double_ R3 = R1 * R2; + + // Check keys + set expected = list_of(1); + EXPECT(expected == R3.keys()); +} + /* ************************************************************************* */ // Test with ternary function. Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, From 508db60f74a8c3c24da9aa2e0720f0c603a7a520 Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Tue, 2 Nov 2021 12:04:04 -0400 Subject: [PATCH 070/110] add jacobian of second argument to adjoint and adjointTranpsose --- gtsam/geometry/Pose3.cpp | 12 ++++++++---- gtsam/geometry/Pose3.h | 10 ++++++---- gtsam/geometry/tests/testPose3.cpp | 27 +++++++++++++++++---------- 3 files changed, 31 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..59594680a 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -116,7 +116,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) { /* ************************************************************************* */ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -127,12 +127,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Hxi->col(i) = Gi * y; } } - return adjointMap(xi) * y; + const Matrix6& ad_xi = adjointMap(xi); + if (H_y) *H_y = ad_xi; + return ad_xi * y; } /* ************************************************************************* */ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi) { + OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) { if (Hxi) { Hxi->setZero(); for (int i = 0; i < 6; ++i) { @@ -143,7 +145,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Hxi->col(i) = GTi * y; } } - return adjointMap(xi).transpose() * y; + const Matrix6& adT_xi = adjointMap(xi).transpose(); + if (H_y) *H_y = adT_xi; + return adT_xi * y; } /* ************************************************************************* */ diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index b6107120e..884d0760c 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -177,13 +177,14 @@ public: * and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * */ - static Matrix6 adjointMap(const Vector6 &xi); + static Matrix6 adjointMap(const Vector6& xi); /** * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives */ - static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, - OptionalJacobian<6, 6> Hxi = boost::none); + static Vector6 adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); // temporary fix for wrappers until case issue is resolved static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} @@ -193,7 +194,8 @@ public: * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. */ static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, - OptionalJacobian<6, 6> Hxi = boost::none); + OptionalJacobian<6, 6> Hxi = boost::none, + OptionalJacobian<6, 6> H_y = boost::none); /// Derivative of Expmap static Matrix6 ExpmapDerivative(const Vector6& xi); diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index f0f2c0ccd..e1d3d5ea2 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -912,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) { } TEST( Pose3, adjoint) { - Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); + Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished(); + Vector expected = testDerivAdjoint(screwPose3::xi, v); - Matrix actualH; - Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( - testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); + Matrix numericalH1 = numericalDerivative21( + testDerivAdjoint, screwPose3::xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( + testDerivAdjoint, screwPose3::xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ @@ -934,14 +938,17 @@ TEST( Pose3, adjointTranspose) { Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector expected = testDerivAdjointTranspose(xi, v); - Matrix actualH; - Vector actual = Pose3::adjointTranspose(xi, v, actualH); + Matrix actualH1, actualH2; + Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2); - Matrix numericalH = numericalDerivative21( + Matrix numericalH1 = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); + Matrix numericalH2 = numericalDerivative22( testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); - EXPECT(assert_equal(numericalH,actualH,1e-5)); + EXPECT(assert_equal(numericalH1,actualH1,1e-5)); + EXPECT(assert_equal(numericalH2,actualH2,1e-5)); } /* ************************************************************************* */ From a61cbdc4d12e8fad03e98ed703ca40ebb72ec61e Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Wed, 3 Nov 2021 17:14:37 +0100 Subject: [PATCH 071/110] Fix warnings raised by GCC -Wpedactic --- gtsam/base/Manifold.h | 2 +- gtsam/base/Matrix.h | 22 +++++++++---------- gtsam/base/Testable.h | 2 +- gtsam/base/Vector.h | 4 ++-- gtsam/geometry/concepts.h | 4 ++-- gtsam/inference/FactorGraph.h | 2 +- gtsam/navigation/MagPoseFactor.h | 4 ++-- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 6 ++--- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam_unstable/slam/BetweenFactorEM.h | 3 ++- gtsam_unstable/slam/PoseBetweenFactor.h | 4 ++-- gtsam_unstable/slam/PosePriorFactor.h | 4 ++-- .../slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- 14 files changed, 33 insertions(+), 32 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index dbe497005..a14404268 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -178,4 +178,4 @@ struct FixedDimension { // * the gtsam namespace to be more easily enforced as testable // */ #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold _gtsam_IsManifold_##T; +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 013947bbd..26f03f3e3 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -46,18 +46,18 @@ typedef Eigen::Matrix M // Create handy typedefs and constants for square-size matrices // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 #define GTSAM_MAKE_MATRIX_DEFS(N) \ -typedef Eigen::Matrix Matrix##N; \ -typedef Eigen::Matrix Matrix1##N; \ -typedef Eigen::Matrix Matrix2##N; \ -typedef Eigen::Matrix Matrix3##N; \ -typedef Eigen::Matrix Matrix4##N; \ -typedef Eigen::Matrix Matrix5##N; \ -typedef Eigen::Matrix Matrix6##N; \ -typedef Eigen::Matrix Matrix7##N; \ -typedef Eigen::Matrix Matrix8##N; \ -typedef Eigen::Matrix Matrix9##N; \ +using Matrix##N = Eigen::Matrix; \ +using Matrix1##N = Eigen::Matrix; \ +using Matrix2##N = Eigen::Matrix; \ +using Matrix3##N = Eigen::Matrix; \ +using Matrix4##N = Eigen::Matrix; \ +using Matrix5##N = Eigen::Matrix; \ +using Matrix6##N = Eigen::Matrix; \ +using Matrix7##N = Eigen::Matrix; \ +using Matrix8##N = Eigen::Matrix; \ +using Matrix9##N = Eigen::Matrix; \ static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero() GTSAM_MAKE_MATRIX_DEFS(1); GTSAM_MAKE_MATRIX_DEFS(2); diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 6062c7ae1..74e237699 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -173,4 +173,4 @@ namespace gtsam { * @deprecated please use BOOST_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable _gtsam_Testable_##T; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index ed90a7126..8a7eb1d55 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -48,8 +48,8 @@ static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zer // Create handy typedefs and constants for vectors with N>3 // VectorN and Z_Nx1, for N=1..9 #define GTSAM_MAKE_VECTOR_DEFS(N) \ - typedef Eigen::Matrix Vector##N; \ - static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); + using Vector##N = Eigen::Matrix; \ + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero() GTSAM_MAKE_VECTOR_DEFS(4); GTSAM_MAKE_VECTOR_DEFS(5); diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index 207b48f56..a313d4448 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -71,6 +71,6 @@ private: */ /** Pose Concept macros */ -#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; -#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept _gtsam_PoseConcept##T; +#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept +#define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index e337e3249..0a0c89f50 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -121,7 +121,7 @@ class FactorGraph { protected: /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR); /** Collection of factors */ FastVector factors_; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index da2a54ce9..feb6e0e19 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -45,8 +45,8 @@ class MagPoseFactor: public NoiseModelFactor1 { using shared_ptr = boost::shared_ptr>; /// Concept check by type. - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: ~MagPoseFactor() override {} diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index dbaf31898..615b5418e 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -101,4 +101,4 @@ private: } }; -}; +} diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 47083d5d7..1d7be99fd 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -218,9 +218,9 @@ protected: X value_; /// fixed value for variable - GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_MANIFOLD_TYPE(X); - GTSAM_CONCEPT_TESTABLE_TYPE(X) + GTSAM_CONCEPT_TESTABLE_TYPE(X); public: @@ -296,7 +296,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { using Base = NoiseModelFactor2; using This = NonlinearEquality2; - GTSAM_CONCEPT_MANIFOLD_TYPE(T) + GTSAM_CONCEPT_MANIFOLD_TYPE(T); /// Default constructor to allow for serialization NonlinearEquality2() {} diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index c745f7bd9..71c204ad0 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -39,7 +39,7 @@ namespace gtsam { VALUE prior_; /** The measurement */ /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index 98ec59fe9..f78f9b334 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -56,7 +56,8 @@ private: bool flag_bump_up_near_zero_probs_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T)GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_LIE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index a6c583418..624fd4a54 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -40,8 +40,8 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: // shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index f657fc443..bea849c00 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -37,8 +37,8 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE) + GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE); public: /// shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 956c75999..3f2e02a78 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -55,8 +55,8 @@ namespace gtsam { SharedGaussian model_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T) - GTSAM_CONCEPT_TESTABLE_TYPE(T) + GTSAM_CONCEPT_LIE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T); public: From 89ce766269b0fece98cdb47c3c47a4096d437164 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Thu, 4 Nov 2021 07:11:28 +0100 Subject: [PATCH 072/110] more consistent notation of macros --- gtsam/base/Lie.h | 2 +- gtsam/base/Manifold.h | 2 +- gtsam/base/Testable.h | 2 +- gtsam/base/chartTesting.h | 2 +- gtsam/geometry/PinholePose.h | 2 +- gtsam/inference/ClusterTree.h | 2 +- gtsam/inference/EliminationTree.h | 2 +- gtsam/inference/FactorGraph.h | 2 +- gtsam/navigation/MagPoseFactor.h | 2 +- gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h | 2 +- gtsam/nonlinear/NonlinearEquality.h | 7 +++---- gtsam/nonlinear/PriorFactor.h | 2 +- gtsam/slam/GeneralSFMFactor.h | 6 +++--- gtsam_unstable/slam/BetweenFactorEM.h | 4 ++-- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h | 4 ++-- 17 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index ac7c2a9a5..cb8e7d017 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -370,4 +370,4 @@ public: * the gtsam namespace to be more easily enforced as testable */ #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup; -#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup _gtsam_IsLieGroup_##T; +#define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup; diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index a14404268..962dc8269 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -178,4 +178,4 @@ struct FixedDimension { // * the gtsam namespace to be more easily enforced as testable // */ #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold; -#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold +#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold; diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index 74e237699..d50d62c1f 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -173,4 +173,4 @@ namespace gtsam { * @deprecated please use BOOST_CONCEPT_ASSERT and */ #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable; diff --git a/gtsam/base/chartTesting.h b/gtsam/base/chartTesting.h index f63054a5b..8f5213f91 100644 --- a/gtsam/base/chartTesting.h +++ b/gtsam/base/chartTesting.h @@ -32,7 +32,7 @@ void testDefaultChart(TestResult& result_, const std::string& name_, const T& value) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef typename gtsam::DefaultChart Chart; typedef typename Chart::vector Vector; diff --git a/gtsam/geometry/PinholePose.h b/gtsam/geometry/PinholePose.h index cc6435a58..7a0b08227 100644 --- a/gtsam/geometry/PinholePose.h +++ b/gtsam/geometry/PinholePose.h @@ -35,7 +35,7 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase { private: - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) // Get dimensions of calibration type at compile time static const int DimK = FixedDimension::value; diff --git a/gtsam/inference/ClusterTree.h b/gtsam/inference/ClusterTree.h index e225bac5f..7dd414193 100644 --- a/gtsam/inference/ClusterTree.h +++ b/gtsam/inference/ClusterTree.h @@ -110,7 +110,7 @@ class ClusterTree { typedef sharedCluster sharedNode; /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) protected: FastVector roots_; diff --git a/gtsam/inference/EliminationTree.h b/gtsam/inference/EliminationTree.h index e4a64c589..70e10b3bd 100644 --- a/gtsam/inference/EliminationTree.h +++ b/gtsam/inference/EliminationTree.h @@ -81,7 +81,7 @@ namespace gtsam { protected: /** concept check */ - GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); + GTSAM_CONCEPT_TESTABLE_TYPE(FactorType) FastVector roots_; FastVector remainingFactors_; diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index 0a0c89f50..e337e3249 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -121,7 +121,7 @@ class FactorGraph { protected: /** concept check, makes sure FACTOR defines print and equals */ - GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR); + GTSAM_CONCEPT_TESTABLE_TYPE(FACTOR) /** Collection of factors */ FastVector factors_; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index feb6e0e19..c0a6a7ece 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -45,7 +45,7 @@ class MagPoseFactor: public NoiseModelFactor1 { using shared_ptr = boost::shared_ptr>; /// Concept check by type. - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index fd9e49a62..a7a0d724b 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -149,7 +149,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams ¶ms, const bool singleIteration, const bool gradientDescent = false) { - // GTSAM_CONCEPT_MANIFOLD_TYPE(V); + // GTSAM_CONCEPT_MANIFOLD_TYPE(V) size_t iteration = 0; diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 1d7be99fd..43d30254e 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -218,9 +218,8 @@ protected: X value_; /// fixed value for variable - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - - GTSAM_CONCEPT_TESTABLE_TYPE(X); + GTSAM_CONCEPT_MANIFOLD_TYPE(X) + GTSAM_CONCEPT_TESTABLE_TYPE(X) public: @@ -296,7 +295,7 @@ class NonlinearEquality2 : public NoiseModelFactor2 { using Base = NoiseModelFactor2; using This = NonlinearEquality2; - GTSAM_CONCEPT_MANIFOLD_TYPE(T); + GTSAM_CONCEPT_MANIFOLD_TYPE(T) /// Default constructor to allow for serialization NonlinearEquality2() {} diff --git a/gtsam/nonlinear/PriorFactor.h b/gtsam/nonlinear/PriorFactor.h index 71c204ad0..c745f7bd9 100644 --- a/gtsam/nonlinear/PriorFactor.h +++ b/gtsam/nonlinear/PriorFactor.h @@ -39,7 +39,7 @@ namespace gtsam { VALUE prior_; /** The measurement */ /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 2e4543177..bfc3a0f78 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -59,8 +59,8 @@ namespace gtsam { template class GeneralSFMFactor: public NoiseModelFactor2 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); - GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); + GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) + GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; @@ -202,7 +202,7 @@ struct traits > : Testable< template class GeneralSFMFactor2: public NoiseModelFactor3 { - GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); + GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) static const int DimK = FixedDimension::value; protected: diff --git a/gtsam_unstable/slam/BetweenFactorEM.h b/gtsam_unstable/slam/BetweenFactorEM.h index f78f9b334..572935da3 100644 --- a/gtsam_unstable/slam/BetweenFactorEM.h +++ b/gtsam_unstable/slam/BetweenFactorEM.h @@ -56,8 +56,8 @@ private: bool flag_bump_up_near_zero_probs_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T); - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 624fd4a54..444da275d 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -40,7 +40,7 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index bea849c00..665bb4680 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -37,7 +37,7 @@ namespace gtsam { boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame /** concept check by type */ - GTSAM_CONCEPT_TESTABLE_TYPE(POSE); + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE); public: diff --git a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h index 3f2e02a78..956c75999 100644 --- a/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h +++ b/gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h @@ -55,8 +55,8 @@ namespace gtsam { SharedGaussian model_; /** concept check by type */ - GTSAM_CONCEPT_LIE_TYPE(T); - GTSAM_CONCEPT_TESTABLE_TYPE(T); + GTSAM_CONCEPT_LIE_TYPE(T) + GTSAM_CONCEPT_TESTABLE_TYPE(T) public: From 2307fc7fa24f3bb946fe16908084f6f4db4e63d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 4 Nov 2021 17:50:12 -0400 Subject: [PATCH 073/110] add printErrors method to GaussianFactorGraph --- gtsam/linear/GaussianFactorGraph.cpp | 29 ++++++++++++++++++++++++++++ gtsam/linear/GaussianFactorGraph.h | 8 ++++++++ gtsam/linear/linear.i | 1 + 3 files changed, 38 insertions(+) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 13eaee7e3..24c4b9a0d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -510,4 +510,33 @@ namespace gtsam { return optimize(function); } + /* ************************************************************************* */ + void GaussianFactorGraph::printErrors( + const VectorValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& + printCondition) const { + cout << str << "size: " << size() << endl << endl; + for (size_t i = 0; i < (*this).size(); i++) { + const sharedFactor& factor = (*this)[i]; + const double errorValue = + (factor != nullptr ? (*this)[i]->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + stringstream ss; + ss << "Factor " << i << ": "; + if (factor == nullptr) { + cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + cout << "error = " << errorValue << "\n"; + } + cout << endl; // only one "endl" at end might be faster, \n for each + // factor + } + } + } // namespace gtsam diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index e3304d5e8..d41374854 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -375,6 +375,14 @@ namespace gtsam { /** In-place version e <- A*x that takes an iterator. */ void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; + void printErrors( + const VectorValues& x, + const std::string& str = "GaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; /// @} private: diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d93b14d3e..c74161f26 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -401,6 +401,7 @@ class GaussianFactorGraph { // error and probability double error(const gtsam::VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const; + void printErrors(const gtsam::VectorValues& c, string str = "GaussianFactorGraph: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph negate() const; From 4bd80357f5a1d5e451dc54982bf8efb517ea7136 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:46:19 -0400 Subject: [PATCH 074/110] Use Eigen expressions more effectively and kill & in code. --- gtsam/geometry/Pose3.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..8e43105cd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -85,20 +85,20 @@ Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose, /// The dual version of Adjoint Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, OptionalJacobian<6, 6> H_x) const { - const Matrix6 &AdT = AdjointMap().transpose(); - const Vector6 &AdTx = AdT * x; + const Matrix6 Ad = AdjointMap(); + const Vector6 AdTx = Ad.transpose() * x; // Jacobians // See docs/math.pdf for more details. if (H_pose) { - const auto &w_T_hat = skewSymmetric(AdTx.head<3>()), - &v_T_hat = skewSymmetric(AdTx.tail<3>()); + const auto w_T_hat = skewSymmetric(AdTx.head<3>()), + v_T_hat = skewSymmetric(AdTx.tail<3>()); *H_pose = (Matrix6() << w_T_hat, v_T_hat, // /* */ v_T_hat, Z_3x3) .finished(); } if (H_x) { - *H_x = AdT; + *H_x = Ad.transpose(); } return AdTx; From 238563f0e5c164014e1624982ba797eb41b14d7c Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 6 Nov 2021 13:51:15 -0400 Subject: [PATCH 075/110] Cleaner Jacobian. --- gtsam/geometry/Pose3.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 8e43105cd..d0e60f3fd 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -93,9 +93,8 @@ Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose, if (H_pose) { const auto w_T_hat = skewSymmetric(AdTx.head<3>()), v_T_hat = skewSymmetric(AdTx.tail<3>()); - *H_pose = (Matrix6() << w_T_hat, v_T_hat, // - /* */ v_T_hat, Z_3x3) - .finished(); + *H_pose << w_T_hat, v_T_hat, // + /* */ v_T_hat, Z_3x3; } if (H_x) { *H_x = Ad.transpose(); From c4cd2b5080d2a20bb2c77037821def2a4d21b157 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:05:58 -0400 Subject: [PATCH 076/110] fixed formatting (plus small fix: std::vector -> fastVector) --- .../tests/testSmartProjectionRigFactor.cpp | 525 ++++++++++-------- ...martProjectionPoseFactorRollingShutter.cpp | 121 ++-- 2 files changed, 350 insertions(+), 296 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index a2a30e89a..8e6735dbd 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -19,15 +19,17 @@ * @date August 2021 */ -#include "smartFactorScenarios.h" -#include -#include +#include #include #include -#include +#include +#include + #include #include +#include "smartFactorScenarios.h" + using namespace boost::assign; using namespace std::placeholders; @@ -37,15 +39,15 @@ 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); static Symbol x2('X', 2); static Symbol x3('X', 3); -Key cameraId1 = 0; // first camera +Key cameraId1 = 0; // first camera Key cameraId2 = 1; Key cameraId3 = 2; @@ -56,15 +58,15 @@ LevenbergMarquardtParams lmParams; // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor) { +TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor2) { +TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaPose; Cameras cameraRig; SmartProjectionParams params; @@ -73,19 +75,19 @@ TEST( SmartProjectionRigFactor, Constructor2) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor3) { +TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor4) { +TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaPose; Cameras cameraRig; - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartProjectionParams params; params.setRankTolerance(rankTol); SmartRigFactor factor1(model, cameraRig, params); @@ -93,7 +95,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Constructor5) { +TEST(SmartProjectionRigFactor, Constructor5) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -102,10 +104,10 @@ TEST( SmartProjectionRigFactor, Constructor5) { } /* ************************************************************************* */ -TEST( SmartProjectionRigFactor, Equals ) { +TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); @@ -115,23 +117,23 @@ TEST( SmartProjectionRigFactor, Equals ) { CHECK(assert_equal(*factor1, *factor2)); - SmartRigFactor::shared_ptr factor3(new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); - factor3->add(measurement1, x1); // now use default + SmartRigFactor::shared_ptr factor3( + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noiseless ) { - +TEST(SmartProjectionRigFactor, 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); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK) ); - factor.add(level_uv, x1); // both taken from the same camera + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); Values values; // it's a pose factor, hence these are poses @@ -181,12 +183,11 @@ TEST( SmartProjectionRigFactor, noiseless ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, noisy ) { - +TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -195,25 +196,25 @@ TEST( SmartProjectionRigFactor, noisy ) { 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)); + 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)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); measurements.push_back(level_uv_right); - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; factor2->add(measurements, views, cameraIds); double actualError2 = factor2->error(values); @@ -225,10 +226,10 @@ TEST(SmartProjectionRigFactor, 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)); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor, sharedK) ); + Pose3 body_T_sensor = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -247,8 +248,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 0, 0 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 0, 0}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -256,7 +257,9 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); - smartFactor1.add(measurements_cam1, views); // use default CameraIds since we have a single camera + smartFactor1.add( + measurements_cam1, + views); // use default CameraIds since we have a single camera SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views); @@ -283,8 +286,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { 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)); + 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); @@ -304,16 +307,16 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); + Pose3 body_T_sensor1 = + Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = + Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_T_sensor1, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor2, sharedK) ); - cameraRig.push_back( Camera(body_T_sensor3, sharedK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); @@ -334,8 +337,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // Create smart factors - KeyVector views { x1, x2, x3 }; - FastVector cameraIds { 0, 1, 2 }; + KeyVector views{x1, x2, x3}; + FastVector cameraIds{0, 1, 2}; SmartProjectionParams params; params.setRankTolerance(1.0); @@ -370,8 +373,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { 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)); + 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); @@ -387,29 +390,29 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); // 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}; - FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig + KeyVector views{x1, x2, x3}; + FastVector cameraIds{ + 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -427,21 +430,21 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { 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/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 + // 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))); + 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); @@ -450,15 +453,14 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Factors ) { - +TEST(SmartProjectionRigFactor, 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); + 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); @@ -470,12 +472,13 @@ TEST( SmartProjectionRigFactor, Factors ) { measurements_cam1.push_back(cam2.project(landmark1)); // Create smart factors - KeyVector views { x1, x2 }; - FastVector cameraIds { 0, 0 }; + KeyVector views{x1, x2}; + FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor - > (model, Camera(Pose3::identity(), sharedK)); - smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( + model, Camera(Pose3::identity(), sharedK)); + smartFactor1->add(measurements_cam1, + views); // we have a single camera so use default cameraIds SmartRigFactor::Cameras cameras; cameras.push_back(cam1); @@ -501,7 +504,8 @@ TEST( SmartProjectionRigFactor, Factors ) { 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; @@ -528,8 +532,8 @@ TEST( SmartProjectionRigFactor, Factors ) { values.insert(x1, cam1.pose()); values.insert(x2, cam2.pose()); - boost::shared_ptr < RegularHessianFactor<6> > actual = smartFactor1 - ->createHessianFactor(values, 0.0); + boost::shared_ptr> actual = + smartFactor1->createHessianFactor(values, 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); @@ -538,11 +542,10 @@ TEST( SmartProjectionRigFactor, Factors ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { - +TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { using namespace vanillaPose; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -551,15 +554,15 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; + std::vector> sharedKs; sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); sharedKs.push_back(sharedK); // create smart factor - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -578,22 +581,21 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { 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/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 + // 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))); + 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); @@ -602,13 +604,12 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, landmarkDistance ) { - +TEST(SmartProjectionRigFactor, landmarkDistance) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 2; - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -624,17 +625,20 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -646,7 +650,8 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { 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/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; @@ -662,14 +667,14 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { - +TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { using namespace vanillaPose; double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error + double dynamicOutlierRejectionThreshold = + 1; // max 1 pixel of average reprojection error - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; // add fourth landmark Point3 landmark4(5, -0.5, 1); @@ -682,7 +687,8 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { 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 + measurements_cam4.at(0) = + measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartProjectionParams params; params.setLinearizationMode(gtsam::HESSIAN); @@ -690,20 +696,24 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4( + new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -729,15 +739,14 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, CheckHessian) { - - KeyVector views { x1, x2, x3 }; +TEST(SmartProjectionRigFactor, 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 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); @@ -753,17 +762,20 @@ TEST( SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -771,53 +783,53 @@ TEST( SmartProjectionRigFactor, CheckHessian) { 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/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 + // 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))); + 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(); + Matrix CumulativeInformation = + factor1->information() + factor2->information() + factor3->information(); - boost::shared_ptr GaussianGraph = graph.linearize( - values); + 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(); + Matrix AugInformationMatrix = factor1->augmentedInformation() + + factor2->augmentedInformation() + + factor3->augmentedInformation(); // Check Information vector - Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + 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( SmartProjectionRigFactor, Hessian ) { - +TEST(SmartProjectionRigFactor, Hessian) { using namespace vanillaPose2; - KeyVector views { x1, x2 }; + KeyVector views{x1, x2}; // Project three landmarks into 2 cameras Point2 cam1_uv1 = cam1.project(landmark1); @@ -826,15 +838,15 @@ TEST( SmartProjectionRigFactor, Hessian ) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); - FastVector cameraIds { 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), - Point3(0.5, 0.1, 0.3)); + 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()); @@ -843,15 +855,16 @@ TEST( SmartProjectionRigFactor, Hessian ) { // 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] + // 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( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { +TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -860,8 +873,7 @@ TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, Cal3Bundler ) { - +TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; // three landmarks ~5 meters in front of camera @@ -874,11 +886,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - KeyVector views { x1, x2, x3 }; + KeyVector views{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); - FastVector cameraIds { 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); @@ -898,21 +910,21 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { 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/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 + // 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))); + 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); @@ -924,9 +936,11 @@ TEST( SmartProjectionRigFactor, Cal3Bundler ) { typedef GenericProjectionFactor TestProjectionFactor; static Symbol l0('L', 0); /* *************************************************************************/ -TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_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(SmartProjectionRigFactor, + hessianComparedToProjFactors_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 vanillaPose; Point2Vector measurements_lmk1; @@ -936,14 +950,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // 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 keys { x1, x2, x3, x1}; + KeyVector keys{x1, x2, x3, x1}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0, 0 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); @@ -953,10 +968,15 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam 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; @@ -965,8 +985,8 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam // ==== 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 @@ -984,30 +1004,31 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, sharedK); Matrix HPoseActual, HEActual; - // note: b is minus the reprojection error, cf the smart factor jacobian computation - b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + // note: b is minus the reprojection error, cf the smart factor jacobian + // computation + b.segment<2>(0) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(0, 0) = HPoseActual; E.block<2, 3>(0, 0) = HEActual; TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, - HEActual); + b.segment<2>(2) = + -factor12.evaluateError(pose2, *point, HPoseActual, HEActual); F.block<2, 6>(2, 6) = HPoseActual; E.block<2, 3>(2, 0) = HEActual; TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, - HEActual); + b.segment<2>(4) = + -factor13.evaluateError(pose3, *point, HPoseActual, HEActual); F.block<2, 6>(4, 12) = HPoseActual; E.block<2, 3>(4, 0) = HEActual; TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, - HEActual); + b.segment<2>(6) = + -factor11.evaluateError(pose1, *point, HPoseActual, HEActual); F.block<2, 6>(6, 0) = HPoseActual; E.block<2, 3>(6, 0) = HEActual; @@ -1017,20 +1038,22 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam 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)); @@ -1049,8 +1072,7 @@ TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSam } /* *************************************************************************/ -TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { - +TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { using namespace vanillaPose; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; @@ -1060,21 +1082,24 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); // create inputs - std::vector keys { x1, x2, x3 }; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - FastVector cameraIds { 0, 0, 0 }; - FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + KeyVector keys{x1, x2, x3}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + FastVector cameraIds{0, 0, 0}; + FastVector cameraIdsRedundant{0, 0, 0, 0}; - // 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 keys_redundant = keys; - keys_redundant.push_back(keys.at(0)); // we readd the first key + measurements_lmk1_redundant.push_back( + measurements_lmk1.at(0)); // we readd the first measurement + KeyVector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); - smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, + cameraIdsRedundant); SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_lmk2, keys, cameraIds); @@ -1097,20 +1122,22 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { 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); @@ -1120,13 +1147,14 @@ TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { #ifndef DISABLE_TIMING #include -// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +// this factor is slightly slower (but comparable) to original +// SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) -//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 +// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 +// times, 0.065103 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ -TEST( SmartProjectionRigFactor, timing ) { - +TEST(SmartProjectionRigFactor, timing) { using namespace vanillaPose; // Default cameras for simple derivatives @@ -1138,8 +1166,8 @@ TEST( SmartProjectionRigFactor, timing ) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); @@ -1152,8 +1180,9 @@ TEST( SmartProjectionRigFactor, timing ) { size_t nrTests = 10000; - for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1165,7 +1194,7 @@ TEST( SmartProjectionRigFactor, timing ) { gttoc_(SmartRigFactor_LINEARIZE); } - for(size_t i = 0; iadd(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1181,16 +1210,21 @@ TEST( SmartProjectionRigFactor, timing ) { } #endif -///* ************************************************************************* */ -//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"); +///* ************************************************************************* +///*/ +// 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(SmartProjectionRigFactor, serialize) { +// TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaPose; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; @@ -1235,4 +1269,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 6a3e31dd7..09a16e6fb 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -84,7 +84,8 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ @@ -98,7 +99,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); factor1->add(measurement1, x1, x2, interp_factor); } @@ -122,10 +124,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - std::vector cameraIds{0, 0, 0}; + FastVector cameraIds{0, 0, 0}; Cameras cameraRig; - cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); @@ -153,7 +155,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor2)); EXPECT(factor1->equals(*factor3)); } - { // create equal factors and show equal returns true (use default cameraId) + { // create equal factors and show equal returns true (use default cameraId) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurements, key_pairs, interp_factors); @@ -164,7 +166,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false (use default cameraIds) SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x2, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -173,10 +176,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { { // create slightly different factors (different extrinsics) and show equal // returns false Cameras cameraRig2; - cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor2, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -186,7 +190,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // equal returns false SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); - factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement2, x2, x3, interp_factor1, + cameraId1); // different! factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); @@ -377,13 +382,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -450,13 +458,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1,1,1}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1,1,1}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1,1,1}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -470,7 +478,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -503,20 +511,21 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // create arbitrary body_T_sensor (transforms from sensor to body) - Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); - Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), - Point3(0, 0, 1)); - Pose3 body_T_sensor3 = Pose3::identity(); + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-0.03, 0., 0.01), Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-0.1, 0., 0.05), Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3(Rot3::Ypr(-0.3, 0., -0.05), Point3(0, 1, 1)); - Camera camera1(interp_pose1*body_T_sensor1, sharedK); - Camera camera2(interp_pose2*body_T_sensor2, sharedK); - Camera camera3(interp_pose3*body_T_sensor3, sharedK); + Camera camera1(interp_pose1 * body_T_sensor1, sharedK); + Camera camera2(interp_pose2 * body_T_sensor2, sharedK); + Camera camera3(interp_pose3 * body_T_sensor3, sharedK); // Project three landmarks into three cameras - projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); - projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); - projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); + projectToMultipleCameras(camera1, camera2, camera3, landmark1, + measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, + measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, landmark3, + measurements_lmk3); // create inputs std::vector> key_pairs; @@ -535,13 +544,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor3, sharedK)); SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -555,7 +564,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { Values groundTruth; groundTruth.insert(x1, level_pose); groundTruth.insert(x2, pose_right); - groundTruth.insert(x3, pose_above); // pose above is the pose of the camera + groundTruth.insert(x3, pose_above); // pose above is the pose of the camera DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); // Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), @@ -608,7 +617,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -702,13 +712,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -766,13 +776,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(),sharedK), params); + SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -841,16 +851,20 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK), params)); + SmartFactorRS::shared_ptr smartFactor4( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -901,7 +915,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1038,7 +1053,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1193,14 +1209,17 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, Camera(Pose3::identity(),sharedK))); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1244,8 +1263,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, min: 0 max: 0) -//| -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, +// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 +// children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, timing) { using namespace vanillaPose; @@ -1271,7 +1291,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model, Camera(body_P_sensorId,sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS( + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 From dfd4a774549923eacd2a380ea0e7fd3b876a65b3 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:11:46 -0400 Subject: [PATCH 077/110] formatting + const& --- gtsam/slam/SmartProjectionRigFactor.h | 240 +++++++++--------- .../SmartProjectionPoseFactorRollingShutter.h | 82 +++--- 2 files changed, 171 insertions(+), 151 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 3cae1ea64..e8b59cfe4 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -14,8 +14,10 @@ * @brief Smart factor on poses, assuming camera calibration is fixed. * Same as SmartProjectionPoseFactor, except: * - it is templated on CAMERA (i.e., it allows cameras beyond pinhole) - * - it admits a different calibration for each measurement (i.e., it can model a multi-camera rig system) - * - it allows multiple observations from the same pose/key (again, to model a multi-camera system) + * - it admits a different calibration for each measurement (i.e., it + * can model a multi-camera rig system) + * - it allows multiple observations from the same pose/key (again, to + * model a multi-camera system) * @author Luca Carlone * @author Frank Dellaert */ @@ -30,40 +32,42 @@ 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. + * 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 assumes that camera calibration is fixed (but each measurement * can be taken by a different camera in the rig, hence can have a different - * extrinsic and intrinsic calibration). The factor only constrains poses (variable dimension - * is 6 for each pose). This factor requires that values contains the involved poses (Pose3). - * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! - * If the calibration should be optimized, as well, use SmartProjectionFactor instead! + * extrinsic and intrinsic calibration). The factor only constrains poses + * (variable dimension is 6 for each pose). This factor requires that values + * contains the involved poses (Pose3). If all measurements share the same + * calibration (i.e., are from the same camera), use SmartProjectionPoseFactor + * instead! If the calibration should be optimized, as well, use + * SmartProjectionFactor instead! * @addtogroup SLAM */ -template +template class SmartProjectionRigFactor : public SmartProjectionFactor { - private: typedef SmartProjectionFactor Base; typedef SmartProjectionRigFactor This; typedef typename CAMERA::CalibrationType CALIBRATION; static const int DimPose = 6; ///< Pose3 dimension - static const int ZDim = 2; ///< Measurement dimension + static const int ZDim = 2; ///< Measurement dimension protected: - /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) + /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each + /// camera) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), + /// identifying which camera took the measurement FastVector cameraIds_; public: @@ -74,21 +78,20 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typedef boost::shared_ptr shared_ptr; /// Default constructor, only for serialization - SmartProjectionRigFactor() { - } + SmartProjectionRigFactor() {} /** * Constructor - * @param sharedNoiseModel isotropic noise model for the 2D feature measurements - * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in the camera rig + * @param sharedNoiseModel isotropic noise model for the 2D feature + * measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) in + * the camera rig * @param params parameters for the smart projection factors */ - SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Cameras& cameraRig, - const SmartProjectionParams& params = - SmartProjectionParams()) - : Base(sharedNoiseModel, params), - cameraRig_(cameraRig) { + SmartProjectionRigFactor( + const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -96,14 +99,14 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /** * Constructor - * @param sharedNoiseModel isotropic noise model for the 2D feature measurements + * @param sharedNoiseModel isotropic noise model for the 2D feature + * measurements * @param camera single camera (fixed poses wrt body and intrinsics) * @param params parameters for the smart projection factors */ - SmartProjectionRigFactor(const SharedNoiseModel& sharedNoiseModel, - const Camera& camera, - const SmartProjectionParams& params = - SmartProjectionParams()) + SmartProjectionRigFactor( + const SharedNoiseModel& sharedNoiseModel, const Camera& camera, + const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; @@ -112,24 +115,28 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionRigFactor() override { - } + ~SmartProjectionRigFactor() override {} /** * add a new measurement, corresponding to an observation from pose "poseKey" * and taken from the camera in the rig identified by "cameraId" * @param measured 2-dimensional location of the projection of a * single landmark in a single view (the measurement) - * @param poseKey key corresponding to the body pose of the camera taking the measurement - * @param cameraId ID of the camera in the rig taking the measurement (default 0) + * @param poseKey key corresponding to the body pose of the camera taking the + * measurement + * @param cameraId ID of the camera in the rig taking the measurement (default + * 0) */ - void add(const Point2& measured, const Key& poseKey, const size_t cameraId = 0) { + void add(const Point2& measured, const Key& poseKey, + const size_t& cameraId = 0) { // store measurement and key this->measured_.push_back(measured); this->nonUniqueKeys_.push_back(poseKey); - // 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(), poseKey) == 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(), poseKey) == + this->keys_.end()) this->keys_.push_back(poseKey); // add only unique keys // store id of the camera taking the measurement @@ -137,68 +144,70 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** - * Variant of the previous "add" function in which we include multiple 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 poseKeys keys corresponding to the body poses of the cameras taking the measurements - * @param cameraIds IDs of the cameras in the rig taking each measurement (same order as the measurements) + * @param poseKeys keys corresponding to the body poses of the cameras taking + * the measurements + * @param cameraIds IDs of the cameras in the rig taking each measurement + * (same order as the measurements) */ void add(const Point2Vector& measurements, const KeyVector& poseKeys, const FastVector& cameraIds = FastVector()) { - if (poseKeys.size() != measurements.size() - || (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { - throw std::runtime_error("SmartProjectionRigFactor: " - "trying to add inconsistent inputs"); + if (poseKeys.size() != measurements.size() || + (poseKeys.size() != cameraIds.size() && cameraIds.size() != 0)) { + throw std::runtime_error( + "SmartProjectionRigFactor: " + "trying to add inconsistent inputs"); } if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " - "camera rig includes multiple camera but add did not input cameraIds"); + "camera rig includes multiple camera but add did not input " + "cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], poseKeys[i], cameraIds.size() == 0 ? 0 : cameraIds[i]); + add(measurements[i], poseKeys[i], + cameraIds.size() == 0 ? 0 : cameraIds[i]); } } - /// return (for each observation) the (possibly non unique) keys involved in the measurements - const KeyVector nonUniqueKeys() const { - return nonUniqueKeys_; - } + /// return (for each observation) the (possibly non unique) keys involved in + /// the measurements + const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - inline Cameras cameraRig() const { - return cameraRig_; - } + inline Cameras cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { - return cameraIds_; - } + inline FastVector cameraIds() const { return cameraIds_; } /** * 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 << "SmartProjectionRigFactor: \n "; for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); + cameraRig_[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const This *e = dynamic_cast(&p); - return e && Base::equals(p, tol) - && nonUniqueKeys_ == e->nonUniqueKeys() - && cameraRig_.equals(e->cameraRig()) - && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); + const This* e = dynamic_cast(&p); + return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && + cameraRig_.equals(e->cameraRig()) && + std::equal(cameraIds_.begin(), cameraIds_.end(), + e->cameraIds().begin()); } /** @@ -211,12 +220,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body - * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i - cameras.emplace_back( - world_P_sensor_i, - make_shared( - cameraRig_[cameraIds_[i]].calibration())); + const Pose3 world_P_sensor_i = + values.at(nonUniqueKeys_[i]) // = world_P_body + * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + cameras.emplace_back(world_P_sensor_i, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -236,9 +245,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * 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 body poses we interpolate from), the point Jacobian E, - * and the error vector b. Note that the jacobians are computed for a given point. + * @return Return arguments are the camera jacobians Fs (including the + * jacobian with 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(typename Base::FBlocks& Fs, Matrix& E, Vector& b, @@ -248,7 +258,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); + const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); @@ -259,35 +269,36 @@ class SmartProjectionRigFactor : 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 { - - // we may have multiple observation sharing the same keys (e.g., 2 cameras measuring from the same body pose), - // hence the number of unique keys may be smaller than nrMeasurements - size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys + const Values& values, const double& lambda = 0.0, + bool diagonalDamping = false) const { + // we may have multiple observation sharing the same keys (e.g., 2 cameras + // measuring from the same body pose), hence the number of unique keys may + // be smaller than nrMeasurements + size_t nrUniqueKeys = + this->keys_ + .size(); // note: by construction, keys_ only contains unique keys Cameras cameras = this->cameras(values); // Create structures for Hessian Factors std::vector js; - std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - std::vector < Vector > gs(nrUniqueKeys); + std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector gs(nrUniqueKeys); if (this->measured_.size() != cameras.size()) // 1 observation per camera - throw std::runtime_error("SmartProjectionRigFactor: " - "measured_.size() inconsistent with input"); + throw std::runtime_error( + "SmartProjectionRigFactor: " + "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point this->triangulateSafe(cameras); 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( "SmartProjectionRigFactor: " @@ -303,30 +314,34 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { // Whiten using noise model this->noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++){ + for (size_t i = 0; i < Fs.size(); i++) { Fs[i] = this->noiseModel_->Whiten(Fs[i]); } const Matrix3 P = Base::Cameras::PointCov(E, lambda, diagonalDamping); - // 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_ + // 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::template SchurComplementAndRearrangeBlocks<3, 6, 6>( Fs, E, P, b, nonUniqueKeys_, this->keys_); - return boost::make_shared < RegularHessianFactor - > (this->keys_, augmentedHessianUniqueKeys); + return boost::make_shared >( + 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 + * 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 + 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 this->createHessianFactor(values, lambda); @@ -337,30 +352,27 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /// 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 - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(nonUniqueKeys_); - ar & BOOST_SERIALIZATION_NVP(cameraRig_); - ar & BOOST_SERIALIZATION_NVP(cameraIds_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + ar& BOOST_SERIALIZATION_NVP(cameraRig_); + ar& BOOST_SERIALIZATION_NVP(cameraIds_); } - }; // end of class declaration /// traits -template -struct traits > : public Testable< - SmartProjectionRigFactor > { -}; +template +struct traits > + : public Testable > {}; -} // \ namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 84e3437a7..b8e048a34 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -11,7 +11,8 @@ /** * @file SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time + * @brief Smart projection factor on poses modeling rolling shutter effect with + * given readout time * @author Luca Carlone */ @@ -42,7 +43,6 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - private: typedef SmartProjectionFactor Base; typedef SmartProjectionPoseFactorRollingShutter This; @@ -57,10 +57,12 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) + /// one or more cameras taking observations (fixed poses wrt body + fixed + /// intrinsics) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement + /// vector of camera Ids (one for each observation, in the same order), + /// identifying which camera took the measurement FastVector cameraIds_; public: @@ -72,8 +74,9 @@ class SmartProjectionPoseFactorRollingShutter /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation - ///< pose is interpolated + static const int 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 @@ -84,14 +87,14 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise - * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) + * taking the measurements * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params), - cameraRig_(cameraRig) { + : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // use only configuration that works with this factor Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; Base::params_.linearizationMode = gtsam::HESSIAN; @@ -130,7 +133,7 @@ class SmartProjectionPoseFactorRollingShutter */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, - const size_t cameraId = 0) { + const size_t& cameraId = 0) { // store measurements in base class this->measured_.push_back(measured); @@ -164,29 +167,33 @@ class SmartProjectionPoseFactorRollingShutter * for the i0-th measurement can be interpolated * @param alphas vector of interpolation params (in [0,1]), one for each * measurement (in the same order) - * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) + * @param cameraIds IDs of the cameras taking each measurement (same order as + * the measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { - - if (world_P_body_key_pairs.size() != measurements.size() - || world_P_body_key_pairs.size() != alphas.size() - || (world_P_body_key_pairs.size() != cameraIds.size() - && cameraIds.size() != 0)) { // cameraIds.size()=0 is default - throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " - "trying to add inconsistent inputs"); + if (world_P_body_key_pairs.size() != measurements.size() || + world_P_body_key_pairs.size() != alphas.size() || + (world_P_body_key_pairs.size() != cameraIds.size() && + cameraIds.size() != 0)) { // cameraIds.size()=0 is default + throw std::runtime_error( + "SmartProjectionPoseFactorRollingShutter: " + "trying to add inconsistent inputs"); } if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " - "camera rig includes multiple camera but add did not input cameraIds"); + "camera rig includes multiple camera but add did not input " + "cameraIds"); } 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, alphas[i], - cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified + cameraIds.size() == 0 ? 0 + : cameraIds[i]); // use 0 as default if + // cameraIds was not specified } } @@ -200,14 +207,10 @@ class SmartProjectionPoseFactorRollingShutter const std::vector alphas() const { return alphas_; } /// return the calibration object - inline Cameras cameraRig() const { - return cameraRig_; - } + inline Cameras cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { - return cameraIds_; - } + inline FastVector cameraIds() const { return cameraIds_; } /** * print @@ -226,7 +229,7 @@ class SmartProjectionPoseFactorRollingShutter << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); + cameraRig_[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -234,7 +237,8 @@ class SmartProjectionPoseFactorRollingShutter /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>(&p); + dynamic_cast*>( + &p); double keyPairsEqual = true; if (this->world_P_body_key_pairs_.size() == @@ -253,9 +257,10 @@ class SmartProjectionPoseFactorRollingShutter keyPairsEqual = false; } - return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual - && cameraRig_.equals(e->cameraRig()) - && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); + return e && Base::equals(p, tol) && alphas_ == e->alphas() && + keyPairsEqual && cameraRig_.equals(e->cameraRig()) && + std::equal(cameraIds_.begin(), cameraIds_.end(), + e->cameraIds().begin()); } /** @@ -292,7 +297,8 @@ class SmartProjectionPoseFactorRollingShutter dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, cameraRig_[cameraIds_[i]].calibration()); + PinholeCamera camera( + w_P_cam, cameraRig_[cameraIds_[i]].calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = @@ -317,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter /// linearize and return a Hessianfactor that is an approximation of error(p) boost::shared_ptr> createHessianFactor( - const Values& values, const double lambda = 0.0, + const Values& values, const double& lambda = 0.0, bool diagonalDamping = false) const { // we may have multiple observation sharing the same keys (due to the // rolling shutter interpolation), hence the number of unique keys may be @@ -405,7 +411,8 @@ class SmartProjectionPoseFactorRollingShutter */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; - for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement + for (size_t i = 0; i < this->measured_.size(); + i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = @@ -415,8 +422,9 @@ class SmartProjectionPoseFactorRollingShutter interpolate(w_P_body1, w_P_body2, interpolationFactor); const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, make_shared( - cameraRig_[cameraIds_[i]].calibration())); + cameras.emplace_back(w_P_cam, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -429,7 +437,7 @@ class SmartProjectionPoseFactorRollingShutter * @return a Gaussian factor */ boost::shared_ptr linearizeDamped( - const Values& values, const double lambda = 0.0) const { + 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) { From 1e384686a16d43fce3684223a1e02126cd053471 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 18:34:34 -0400 Subject: [PATCH 078/110] more const& --- gtsam/slam/SmartProjectionRigFactor.h | 6 +++--- .../slam/SmartProjectionPoseFactorRollingShutter.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index e8b59cfe4..d7e802658 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -175,13 +175,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// return (for each observation) the (possibly non unique) keys involved in /// the measurements - const KeyVector nonUniqueKeys() const { return nonUniqueKeys_; } + const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - inline Cameras cameraRig() const { return cameraRig_; } + const Cameras& cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { return cameraIds_; } + const FastVector& cameraIds() const { return cameraIds_; } /** * print diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b8e048a34..d16cfa2da 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -199,18 +199,18 @@ class SmartProjectionPoseFactorRollingShutter /// return (for each observation) the keys of the pair of poses from which we /// interpolate - const std::vector> world_P_body_key_pairs() const { + 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 calibration object - inline Cameras cameraRig() const { return cameraRig_; } + const Cameras& cameraRig() const { return cameraRig_; } /// return the calibration object - inline FastVector cameraIds() const { return cameraIds_; } + const FastVector& cameraIds() const { return cameraIds_; } /** * print From 710a64fed42d15df30876341d4ebb7b08e38030f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:22:28 -0400 Subject: [PATCH 079/110] now throwing exception is params are incorrect --- gtsam/slam/SmartProjectionRigFactor.h | 24 ++- .../tests/testSmartProjectionRigFactor.cpp | 170 +++++++++++------- .../SmartProjectionPoseFactorRollingShutter.h | 24 ++- ...martProjectionPoseFactorRollingShutter.cpp | 99 ++++++---- 4 files changed, 206 insertions(+), 111 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index d7e802658..4bfd56695 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -92,9 +92,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -108,9 +114,15 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 8e6735dbd..ec3d5ddf0 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -57,68 +57,87 @@ LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +/* ************************************************************************* */ +// default Cal3_S2 poses with rolling shutter effect +namespace vanillaRig { +using namespace vanillaPose; +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors +} // namespace vanillaRig + /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, cameraRig, params); + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, cameraRig, params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor5) { - using namespace vanillaPose; - SmartProjectionParams params; - params.setRankTolerance(rankTol); - SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params); + using namespace vanillaRig; + SmartProjectionParams params2( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + params2.setRankTolerance(rankTol); + SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2); factor1.add(measurement1, x1, cameraId1); } /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor1( + new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); SmartRigFactor::shared_ptr factor3( - new SmartRigFactor(model, Camera(Pose3::identity(), sharedK))); + new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params)); factor3->add(measurement1, x1); // now use default CHECK(assert_equal(*factor1, *factor3)); @@ -126,13 +145,13 @@ TEST(SmartProjectionRigFactor, Equals) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noiseless) { - using namespace vanillaPose; + using namespace vanillaRig; // Project two landmarks into two cameras Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK)); + SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params); factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); @@ -184,7 +203,7 @@ TEST(SmartProjectionRigFactor, noiseless) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, noisy) { - using namespace vanillaPose; + using namespace vanillaRig; Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); @@ -200,14 +219,16 @@ TEST(SmartProjectionRigFactor, noisy) { 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)); - SmartRigFactor::shared_ptr factor(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor( + new SmartRigFactor(model, cameraRig, params)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr factor2( + new SmartRigFactor(model, cameraRig, params)); Point2Vector measurements; measurements.push_back(level_uv); @@ -223,7 +244,7 @@ TEST(SmartProjectionRigFactor, noisy) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor = @@ -253,7 +274,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -304,7 +325,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { - using namespace vanillaPose; + using namespace vanillaRig; // create arbitrary body_T_sensor (transforms from sensor to body) Pose3 body_T_sensor1 = @@ -342,7 +363,7 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setDegeneracyMode(IGNORE_DEGENERACY); + params.setDegeneracyMode(ZERO_ON_DEGENERACY); params.setEnableEPI(false); SmartRigFactor smartFactor1(model, cameraRig, params); @@ -406,13 +427,20 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { FastVector cameraIds{ 0, 0, 0}; // 3 measurements from the same camera in the rig - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -454,7 +482,7 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Factors) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives Rot3 R; @@ -476,7 +504,7 @@ TEST(SmartProjectionRigFactor, Factors) { FastVector cameraIds{0, 0}; SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( - model, Camera(Pose3::identity(), sharedK)); + model, Camera(Pose3::identity(), sharedK), params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds @@ -543,7 +571,7 @@ TEST(SmartProjectionRigFactor, Factors) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { - using namespace vanillaPose; + using namespace vanillaRig; KeyVector views{x1, x2, x3}; @@ -563,13 +591,16 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -605,7 +636,7 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, landmarkDistance) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 2; @@ -620,8 +651,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { SmartProjectionParams params; params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::JACOBIAN_SVD); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -668,7 +699,7 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { - using namespace vanillaPose; + using namespace vanillaRig; double excludeLandmarksFutherThanDist = 1e10; double dynamicOutlierRejectionThreshold = @@ -742,7 +773,7 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { TEST(SmartProjectionRigFactor, CheckHessian) { KeyVector views{x1, x2, x3}; - using namespace vanillaPose; + using namespace vanillaRig; // Two slightly different cameras Pose3 pose2 = @@ -842,7 +873,12 @@ TEST(SmartProjectionRigFactor, Hessian) { cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); FastVector cameraIds{0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors + + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = @@ -875,6 +911,9 @@ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { /* *************************************************************************/ TEST(SmartProjectionRigFactor, Cal3Bundler) { using namespace bundlerPose; + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); @@ -892,13 +931,16 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -942,7 +984,7 @@ TEST(SmartProjectionRigFactor, // measurements of the same landmark at a single pose, a setup that occurs in // multi-camera systems - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1; // Project three landmarks into three cameras @@ -960,7 +1002,8 @@ TEST(SmartProjectionRigFactor, cameraRig.push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1073,7 +1116,7 @@ TEST(SmartProjectionRigFactor, /* *************************************************************************/ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { - using namespace vanillaPose; + using namespace vanillaRig; Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; // Project three landmarks into three cameras @@ -1097,14 +1140,17 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { KeyVector keys_redundant = keys; keys_redundant.push_back(keys.at(0)); // we readd the first key - SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1( + new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); - SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2( + new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, keys, cameraIds); - SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3( + new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, keys, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1150,12 +1196,13 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // this factor is slightly slower (but comparable) to original // SmartProjectionPoseFactor //-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) -//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 -// children, min: 0 max: 0) | -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 -// times, 0.065103 wall, 0.06 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.061226 wall, 0.06 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU +//(10000 times, 0.073037 wall, 0.06 children, min: 0 max: 0) /* *************************************************************************/ TEST(SmartProjectionRigFactor, timing) { - using namespace vanillaPose; + using namespace vanillaRig; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -1182,7 +1229,7 @@ TEST(SmartProjectionRigFactor, timing) { for (size_t i = 0; i < nrTests; i++) { SmartRigFactor::shared_ptr smartFactorP( - new SmartRigFactor(model, cameraRig)); + new SmartRigFactor(model, cameraRig, params)); smartFactorP->add(measurements_lmk1[0], x1, cameraId1); smartFactorP->add(measurements_lmk1[1], x1, cameraId1); @@ -1195,7 +1242,8 @@ TEST(SmartProjectionRigFactor, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); @@ -1225,7 +1273,7 @@ TEST(SmartProjectionRigFactor, timing) { // BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); // // TEST(SmartProjectionRigFactor, serialize) { -// using namespace vanillaPose; +// using namespace vanillaRig; // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); @@ -1242,7 +1290,7 @@ TEST(SmartProjectionRigFactor, timing) { // //// SERIALIZATION TEST FAILS: to be fixed ////TEST(SmartProjectionRigFactor, serialize2) { -//// using namespace vanillaPose; +//// using namespace vanillaRig; //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d16cfa2da..4a9481d6b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -95,9 +95,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); } /** @@ -110,9 +116,15 @@ class SmartProjectionPoseFactorRollingShutter const SharedNoiseModel& sharedNoiseModel, const Camera& camera, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params) { - // use only configuration that works with this factor - Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; - Base::params_.linearizationMode = gtsam::HESSIAN; + // throw exception if configuration is not supported by this factor + if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "degeneracyMode must be set to ZERO_ON_DEGENERACY"); + if (Base::params_.linearizationMode != gtsam::HESSIAN) + throw std::runtime_error( + "SmartProjectionRigFactor: " + "linearizationMode must be set to HESSIAN"); cameraRig_.push_back(camera); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 09a16e6fb..1d32eccca 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -75,6 +75,9 @@ 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); +SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors } // namespace vanillaPoseRS LevenbergMarquardtParams lmParams; @@ -85,13 +88,12 @@ typedef SmartProjectionPoseFactorRollingShutter> TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - SmartProjectionParams params; params.setRankTolerance(rankTol); SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } @@ -100,13 +102,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { - using namespace vanillaPose; + using namespace vanillaPoseRS; // create fake measurements Point2Vector measurements; @@ -130,15 +132,18 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations - SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor2( + new SmartFactorRS(model, cameraRig, params)); factor2->add(measurements, key_pairs, interp_factors, cameraIds); // create by adding a batch of measurements with a single calibrations - SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor3( + new SmartFactorRS(model, cameraRig, params)); factor3->add(measurements, key_pairs, interp_factors, cameraIds); { // create equal factors and show equal returns true - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); @@ -147,7 +152,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1); factor1->add(measurement2, x2, x3, interp_factor2); factor1->add(measurement3, x3, x4, interp_factor3); @@ -156,7 +162,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { EXPECT(factor1->equals(*factor3)); } { // create equal factors and show equal returns true (use default cameraId) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurements, key_pairs, interp_factors); EXPECT(factor1->equals(*factor2)); @@ -164,7 +171,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different keys) and show equal // returns false (use default cameraIds) - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! @@ -177,7 +185,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { // returns false Cameras cameraRig2; cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig2, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! @@ -188,7 +197,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different interp factors) and show // equal returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr factor1( + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! @@ -216,7 +226,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -291,7 +301,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK)); + SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -383,15 +393,15 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -457,13 +467,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig.push_back(Camera(Pose3::identity(), sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {1, 1, 1}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {1, 1, 1}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -543,13 +556,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { cameraRig.push_back(Camera(body_T_sensor2, sharedK)); cameraRig.push_back(Camera(body_T_sensor3, sharedK)); - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor1( + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor2( + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0, 1, 2}); - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + SmartFactorRS::shared_ptr smartFactor3( + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0, 1, 2}); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -597,7 +613,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // 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; + using namespace vanillaPoseRS; // Default cameras for simple derivatives static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); @@ -618,13 +634,13 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam2.project(landmark1)); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor); - SmartFactor::Cameras cameras; + SmartFactorRS::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -772,7 +788,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, SmartProjectionParams params; params.setRankTolerance(1.0); params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + // params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); // this would give an + // exception as expected + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); @@ -916,7 +934,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1054,7 +1072,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor1); SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1210,16 +1228,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.at(0)); // we readd the first interp factor SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1263,15 +1281,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, #ifndef DISABLE_TIMING #include //-Total: 0 CPU (0 times, 0 wall, 0.21 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.15 CPU (10000 times, 0.125521 wall, 0.15 children, -// min: 0 max: 0) | -RS LINEARIZE: 0.06 CPU (10000 times, 0.06311 wall, 0.06 -// children, min: 0 max: 0) +//| -SF RS LINEARIZE: 0.09 CPU +// (10000 times, 0.124106 wall, 0.09 children, min: 0 max: 0) +//| -RS LINEARIZE: 0.09 CPU +// (10000 times, 0.068719 wall, 0.09 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)); + SmartProjectionParams params( + gtsam::HESSIAN, + gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors Rot3 R = Rot3::identity(); Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); @@ -1291,8 +1313,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple))); + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( + model, Camera(body_P_sensorId, sharedKSimple), params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -1307,7 +1329,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { } for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); + SmartFactor::shared_ptr smartFactor( + new SmartFactor(model, sharedKSimple, params)); smartFactor->add(measurements_lmk1[0], x1); smartFactor->add(measurements_lmk1[1], x2); From 7fa3b5cc96a1e7d91a6bd3e5338dcbb3acf8c8c6 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:43:45 -0400 Subject: [PATCH 080/110] added variable in loop --- gtsam/slam/SmartProjectionRigFactor.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 4bfd56695..f4a72694c 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -232,12 +232,13 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { + const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body - * cameraRig_[cameraIds_[i]].pose(); // = body_P_cam_i + * camera_i.pose(); // = body_P_cam_i cameras.emplace_back(world_P_sensor_i, make_shared( - cameraRig_[cameraIds_[i]].calibration())); + camera_i.calibration())); } return cameras; } From 29f3af560d882d81d0349cfcbae74a44de687a33 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 19:58:33 -0400 Subject: [PATCH 081/110] point2 -> measurement --- gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a9481d6b..75bd95226 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -143,7 +143,7 @@ class SmartProjectionPoseFactorRollingShutter * interpolated pose is the same as world_P_body_key1 * @param cameraId ID of the camera taking the measurement (default 0) */ - void add(const Point2& measured, const Key& world_P_body_key1, + void add(const MEASUREMENT& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, const size_t& cameraId = 0) { // store measurements in base class @@ -182,7 +182,7 @@ class SmartProjectionPoseFactorRollingShutter * @param cameraIds IDs of the cameras taking each measurement (same order as * the measurements) */ - void add(const Point2Vector& measurements, + void add(const MEASUREMENTS& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { From dfd86e8c57f50236eb7e5a9bff8f0c03befd180f Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 20:00:23 -0400 Subject: [PATCH 082/110] this will need to be applied in #861 --- gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 75bd95226..4a9481d6b 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -143,7 +143,7 @@ class SmartProjectionPoseFactorRollingShutter * interpolated pose is the same as world_P_body_key1 * @param cameraId ID of the camera taking the measurement (default 0) */ - void add(const MEASUREMENT& measured, const Key& world_P_body_key1, + void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, const size_t& cameraId = 0) { // store measurements in base class @@ -182,7 +182,7 @@ class SmartProjectionPoseFactorRollingShutter * @param cameraIds IDs of the cameras taking each measurement (same order as * the measurements) */ - void add(const MEASUREMENTS& measurements, + void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, const FastVector& cameraIds = FastVector()) { From e0af235e532823fbefb87cb9c71dabcad6ef6637 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 6 Nov 2021 20:06:41 -0400 Subject: [PATCH 083/110] disabled timing for test --- gtsam/slam/tests/testSmartProjectionRigFactor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index ec3d5ddf0..1ea982391 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -29,6 +29,7 @@ #include #include "smartFactorScenarios.h" +#define DISABLE_TIMING using namespace boost::assign; using namespace std::placeholders; From 3a4cedac1f32775dcb033398bf726e2ee699c068 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 12:26:50 -0500 Subject: [PATCH 084/110] fixed readme --- gtsam/slam/ReadMe.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/slam/ReadMe.md b/gtsam/slam/ReadMe.md index d216b9181..ae5edfdac 100644 --- a/gtsam/slam/ReadMe.md +++ b/gtsam/slam/ReadMe.md @@ -42,11 +42,9 @@ If the calibration should be optimized, as well, use `SmartProjectionFactor` ins Same as `SmartProjectionPoseFactor`, except: - it is templated on `CAMERA`, i.e., it allows cameras beyond pinhole; -- it admits a different calibration for each measurement, i.e., it can model a multi-camera system; +- it allows measurements from multiple cameras, each camera with fixed but potentially different intrinsics and extrinsics; - it allows multiple observations from the same pose/key, again, to model a multi-camera system. -TODO: DimPose and ZDim are hardcoded. Copy/paste from `SmartProjectionPoseFactor`. Unclear what the use case is. - ## Linearized Smart Factors The factors below are less likely to be relevant to the user, but result from using the non-linear smart factors above. From 2e8d373ff520126494361b2664f1e15a1ca88fe8 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 12:32:43 -0500 Subject: [PATCH 085/110] serialization is still off --- gtsam/slam/tests/testSmartProjectionRigFactor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 1ea982391..c652f9b41 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1259,8 +1259,7 @@ TEST(SmartProjectionRigFactor, timing) { } #endif -///* ************************************************************************* -///*/ +///* **************************************************************************/ // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, // "gtsam_noiseModel_Constrained"); // BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, @@ -1276,7 +1275,9 @@ TEST(SmartProjectionRigFactor, timing) { // TEST(SmartProjectionRigFactor, serialize) { // using namespace vanillaRig; // using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params; +// SmartProjectionParams params( +// gtsam::HESSIAN, +// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors // params.setRankTolerance(rankTol); // // Cameras cameraRig; // single camera in the rig From b1baf6c8b3f347006f897df2014e70a50ab3e164 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 14:50:56 -0500 Subject: [PATCH 086/110] final cosmetics --- gtsam/slam/SmartProjectionRigFactor.h | 14 +-- .../tests/testSmartProjectionRigFactor.cpp | 8 +- .../SmartProjectionPoseFactorRollingShutter.h | 85 ++++++++++--------- 3 files changed, 56 insertions(+), 51 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index f4a72694c..26e7b6e97 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -71,6 +71,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { FastVector cameraIds_; public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef CAMERA Camera; typedef CameraSet Cameras; @@ -127,7 +129,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } /** Virtual destructor */ - ~SmartProjectionRigFactor() override {} + ~SmartProjectionRigFactor() override = default; /** * add a new measurement, corresponding to an observation from pose "poseKey" @@ -176,8 +178,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "camera rig includes multiple camera " + "but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], poseKeys[i], @@ -376,9 +378,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); - ar& BOOST_SERIALIZATION_NVP(cameraRig_); - ar& BOOST_SERIALIZATION_NVP(cameraIds_); + //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + // ar& BOOST_SERIALIZATION_NVP(cameraRig_); + //ar& BOOST_SERIALIZATION_NVP(cameraIds_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index c652f9b41..d6a9b12fe 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -1229,16 +1229,16 @@ TEST(SmartProjectionRigFactor, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - SmartRigFactor::shared_ptr smartFactorP( + SmartRigFactor::shared_ptr smartRigFactor( new SmartRigFactor(model, cameraRig, params)); - smartFactorP->add(measurements_lmk1[0], x1, cameraId1); - smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[0], x1, cameraId1); + smartRigFactor->add(measurements_lmk1[1], x1, cameraId1); Values values; values.insert(x1, pose1); values.insert(x2, pose2); gttic_(SmartRigFactor_LINEARIZE); - smartFactorP->linearize(values); + smartRigFactor->linearize(values); gttoc_(SmartRigFactor_LINEARIZE); } diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 4a9481d6b..e69767ad7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -84,6 +84,9 @@ class SmartProjectionPoseFactorRollingShutter typedef std::vector> FBlocks; // vector of F blocks + /// Default constructor, only for serialization + SmartProjectionPoseFactorRollingShutter() {} + /** * Constructor * @param Isotropic measurement noise @@ -197,8 +200,8 @@ class SmartProjectionPoseFactorRollingShutter if (cameraIds.size() == 0 && cameraRig_.size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " - "camera rig includes multiple camera but add did not input " - "cameraIds"); + "camera rig includes multiple camera " + "but add did not input cameraIds"); } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, @@ -275,6 +278,43 @@ class SmartProjectionPoseFactorRollingShutter e->cameraIds().begin()); } + /** + * 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 { + typename Base::Cameras cameras; + for (size_t i = 0; i < this->measured_.size(); + 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 = alphas_[i]; + const Pose3& w_P_body = + interpolate(w_P_body1, w_P_body2, interpolationFactor); + const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); + const Pose3& w_P_cam = w_P_body.compose(body_P_cam); + cameras.emplace_back(w_P_cam, + make_shared( + cameraRig_[cameraIds_[i]].calibration())); + } + return cameras; + } + + /** + * 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; + } + } + /** * Compute jacobian F, E and error vector at a given linearization point * @param values Values structure which must contain camera poses @@ -404,43 +444,6 @@ class SmartProjectionPoseFactorRollingShutter 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 { - typename Base::Cameras cameras; - for (size_t i = 0; i < this->measured_.size(); - 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 = alphas_[i]; - const Pose3& w_P_body = - interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); - const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, - make_shared( - cameraRig_[cameraIds_[i]].calibration())); - } - return cameras; - } - /** * Linearize to Gaussian Factor (possibly adding a damping factor Lambda for * LM) @@ -457,8 +460,8 @@ class SmartProjectionPoseFactorRollingShutter return this->createHessianFactor(values, lambda); default: throw std::runtime_error( - "SmartProjectionPoseFactorRollingShutter: unknown linearization " - "mode"); + "SmartProjectionPoseFactorRollingShutter: " + "unknown linearization mode"); } } From c105aa4e1e5648fa995d88febd3512ef7985fdcc Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 14:56:06 -0500 Subject: [PATCH 087/110] added intermediate camera variable for clarity --- .../slam/SmartProjectionPoseFactorRollingShutter.h | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index e69767ad7..b770ee7cf 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -295,11 +295,12 @@ class SmartProjectionPoseFactorRollingShutter double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); + const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const Pose3& body_P_cam = camera_i.pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, make_shared( - cameraRig_[cameraIds_[i]].calibration())); + camera_i.calibration())); } return cameras; } @@ -347,10 +348,10 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); + const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + auto body_P_cam = camera_i.pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera( - w_P_cam, cameraRig_[cameraIds_[i]].calibration()); + PinholeCamera camera(w_P_cam, camera_i.calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = From 2c2e43ee5b95547bbe32c59d3aaf70f85e083104 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:01:28 -0500 Subject: [PATCH 088/110] got rid of second constructor --- gtsam/slam/SmartProjectionRigFactor.h | 23 ----- .../tests/testSmartProjectionRigFactor.cpp | 27 +++--- .../SmartProjectionPoseFactorRollingShutter.h | 22 ----- ...martProjectionPoseFactorRollingShutter.cpp | 91 +++++++++++++------ 4 files changed, 76 insertions(+), 87 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 26e7b6e97..d2a3140b4 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -105,29 +105,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { "linearizationMode must be set to HESSIAN"); } - /** - * Constructor - * @param sharedNoiseModel isotropic noise model for the 2D feature - * measurements - * @param camera single camera (fixed poses wrt body and intrinsics) - * @param params parameters for the smart projection factors - */ - SmartProjectionRigFactor( - const SharedNoiseModel& sharedNoiseModel, const Camera& camera, - const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { - // throw exception if configuration is not supported by this factor - if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) - throw std::runtime_error( - "SmartProjectionRigFactor: " - "degeneracyMode must be set to ZERO_ON_DEGENERACY"); - if (Base::params_.linearizationMode != gtsam::HESSIAN) - throw std::runtime_error( - "SmartProjectionRigFactor: " - "linearizationMode must be set to HESSIAN"); - cameraRig_.push_back(camera); - } - /** Virtual destructor */ ~SmartProjectionRigFactor() override = default; diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index d6a9b12fe..f518f3dce 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -29,7 +29,7 @@ #include #include "smartFactorScenarios.h" -#define DISABLE_TIMING +//#define DISABLE_TIMING using namespace boost::assign; using namespace std::placeholders; @@ -110,17 +110,6 @@ TEST(SmartProjectionRigFactor, Constructor4) { factor1.add(measurement1, x1, cameraId1); } -/* ************************************************************************* */ -TEST(SmartProjectionRigFactor, Constructor5) { - using namespace vanillaRig; - SmartProjectionParams params2( - gtsam::HESSIAN, - gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors - params2.setRankTolerance(rankTol); - SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2); - factor1.add(measurement1, x1, cameraId1); -} - /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaRig; @@ -138,8 +127,8 @@ TEST(SmartProjectionRigFactor, Equals) { CHECK(assert_equal(*factor1, *factor2)); SmartRigFactor::shared_ptr factor3( - new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params)); - factor3->add(measurement1, x1); // now use default + new SmartRigFactor(model, cameraRig, params)); + factor3->add(measurement1, x1); // now use default camera ID CHECK(assert_equal(*factor1, *factor3)); } @@ -152,7 +141,10 @@ TEST(SmartProjectionRigFactor, noiseless) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params); + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + + SmartRigFactor factor(model, cameraRig, params); factor.add(level_uv, x1); // both taken from the same camera factor.add(level_uv_right, x2); @@ -504,8 +496,11 @@ TEST(SmartProjectionRigFactor, Factors) { KeyVector views{x1, x2}; FastVector cameraIds{0, 0}; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( - model, Camera(Pose3::identity(), sharedK), params); + model, cameraRig, params); smartFactor1->add(measurements_cam1, views); // we have a single camera so use default cameraIds diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index b770ee7cf..fcec7de28 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -109,28 +109,6 @@ class SmartProjectionPoseFactorRollingShutter "linearizationMode must be set to HESSIAN"); } - /** - * Constructor - * @param Isotropic measurement noise - * @param camera single camera (fixed poses wrt body and intrinsics) - * @param params internal parameters of the smart factors - */ - SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, const Camera& camera, - const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) { - // throw exception if configuration is not supported by this factor - if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY) - throw std::runtime_error( - "SmartProjectionRigFactor: " - "degeneracyMode must be set to ZERO_ON_DEGENERACY"); - if (Base::params_.linearizationMode != gtsam::HESSIAN) - throw std::runtime_error( - "SmartProjectionRigFactor: " - "linearizationMode must be set to HESSIAN"); - cameraRig_.push_back(camera); - } - /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 1d32eccca..615f452d8 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -29,7 +29,7 @@ #include #include "gtsam/slam/tests/smartFactorScenarios.h" -#define DISABLE_TIMING +//#define DISABLE_TIMING using namespace gtsam; using namespace boost::assign; @@ -87,22 +87,28 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); params.setRankTolerance(rankTol); - SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); + SmartFactorRS factor1(model, cameraRig, params); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor); } @@ -226,7 +232,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params); + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensorId, sharedK)); + + SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -301,7 +310,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params); + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensorNonId, sharedK)); + + SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); factor.add(level_uv_right, x2, x3, interp_factor2); @@ -392,16 +404,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -633,8 +648,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params)); + new SmartFactorRS(model, cameraRig, params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 @@ -728,13 +746,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + + SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); + SmartFactorRS smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); + SmartFactorRS smartFactor3(model, cameraRig, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -794,13 +815,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + + SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); - SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params); + SmartFactorRS smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_lmk2, key_pairs, interp_factors); - SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params); + SmartFactorRS smartFactor3(model, cameraRig, params); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -869,20 +893,23 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor4( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -933,8 +960,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1071,8 +1101,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), @@ -1227,17 +1260,20 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor + Cameras cameraRig; + cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + SmartFactorRS::shared_ptr smartFactor1( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, interp_factors_redundant); SmartFactorRS::shared_ptr smartFactor2( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors); SmartFactorRS::shared_ptr smartFactor3( - new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); + new SmartFactorRS(model, cameraRig, params)); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1313,8 +1349,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { + Cameras cameraRig; + cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( - model, Camera(body_P_sensorId, sharedKSimple), params)); + model, cameraRig, params)); double interp_factor = 0; // equivalent to measurement taken at pose 1 smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); interp_factor = 1; // equivalent to measurement taken at pose 2 From ac5875671f06d8ca56f532f284b36180e7b60e13 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:12:19 -0500 Subject: [PATCH 089/110] further cleanup before moving to sharedPtrs --- gtsam/slam/SmartProjectionRigFactor.h | 3 +- .../tests/testSmartProjectionRigFactor.cpp | 59 ------------------- 2 files changed, 1 insertion(+), 61 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index d2a3140b4..adce44c15 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -62,8 +62,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each - /// camera) + /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation, in the same order), diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index f518f3dce..519850d98 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -578,11 +578,6 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - std::vector> sharedKs; - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - sharedKs.push_back(sharedK); - // create smart factor Cameras cameraRig; // single camera in the rig cameraRig.push_back(Camera(Pose3::identity(), sharedK)); @@ -1254,60 +1249,6 @@ TEST(SmartProjectionRigFactor, timing) { } #endif -///* **************************************************************************/ -// 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(SmartProjectionRigFactor, serialize) { -// using namespace vanillaRig; -// using namespace gtsam::serializationTestHelpers; -// SmartProjectionParams params( -// gtsam::HESSIAN, -// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors -// params.setRankTolerance(rankTol); -// -// Cameras cameraRig; // single camera in the rig -// cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); -// -// SmartRigFactor factor(model, cameraRig, params); -// -// EXPECT(equalsObj(factor)); -// EXPECT(equalsXML(factor)); -// EXPECT(equalsBinary(factor)); -//} -// -//// SERIALIZATION TEST FAILS: to be fixed -////TEST(SmartProjectionRigFactor, serialize2) { -//// using namespace vanillaRig; -//// using namespace gtsam::serializationTestHelpers; -//// SmartProjectionParams params; -//// params.setRankTolerance(rankTol); -//// SmartRigFactor factor(model, params); -//// -//// // insert some measurements -//// KeyVector key_view; -//// Point2Vector meas_view; -//// std::vector> sharedKs; -//// -//// key_view.push_back(Symbol('x', 1)); -//// meas_view.push_back(Point2(10, 10)); -//// sharedKs.push_back(sharedK); -//// factor.add(meas_view, key_view, sharedKs); -//// -//// EXPECT(equalsObj(factor)); -//// EXPECT(equalsXML(factor)); -//// EXPECT(equalsBinary(factor)); -////} - /* ************************************************************************* */ int main() { TestResult tr; From 4ba93738edde14256c37729504fa37fa0b5a84a8 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:25:35 -0500 Subject: [PATCH 090/110] moved rig to use shared ptrs --- gtsam/slam/SmartProjectionRigFactor.h | 21 ++--- .../tests/testSmartProjectionRigFactor.cpp | 86 +++++++++---------- 2 files changed, 54 insertions(+), 53 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index adce44c15..87125020a 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { KeyVector nonUniqueKeys_; /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) - typename Base::Cameras cameraRig_; + boost::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -90,7 +90,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { * @param params parameters for the smart projection factors */ SmartProjectionRigFactor( - const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -151,7 +152,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { "SmartProjectionRigFactor: " "trying to add inconsistent inputs"); } - if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + if (cameraIds.size() == 0 && cameraRig_->size() > 1) { throw std::runtime_error( "SmartProjectionRigFactor: " "camera rig includes multiple camera " @@ -168,7 +169,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } /// return the calibration object - const Cameras& cameraRig() const { return cameraRig_; } + const boost::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -186,7 +187,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { std::cout << "-- Measurement nr " << i << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[cameraIds_[i]].print("camera in rig:\n"); + (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -195,7 +196,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && - cameraRig_.equals(e->cameraRig()) && + (*cameraRig_).equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } @@ -210,7 +211,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { typename Base::Cameras cameras; cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) // = world_P_body * camera_i.pose(); // = body_P_cam_i @@ -249,7 +250,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose(); + const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); @@ -354,9 +355,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - //ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); + // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); // ar& BOOST_SERIALIZATION_NVP(cameraRig_); - //ar& BOOST_SERIALIZATION_NVP(cameraIds_); + // ar& BOOST_SERIALIZATION_NVP(cameraIds_); } }; // end of class declaration diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 519850d98..b111ee572 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -70,8 +70,8 @@ SmartProjectionParams params( /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); } @@ -79,7 +79,7 @@ TEST(SmartProjectionRigFactor, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor2) { using namespace vanillaRig; - Cameras cameraRig; + boost::shared_ptr cameraRig(new Cameras()); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -90,8 +90,8 @@ TEST(SmartProjectionRigFactor, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor3) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); factor1->add(measurement1, x1, cameraId1); @@ -100,8 +100,8 @@ TEST(SmartProjectionRigFactor, Constructor3) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Constructor4) { using namespace vanillaRig; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartProjectionParams params2( gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors @@ -113,8 +113,8 @@ TEST(SmartProjectionRigFactor, Constructor4) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, Equals) { using namespace vanillaRig; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr factor1( new SmartRigFactor(model, cameraRig, params)); @@ -141,8 +141,8 @@ TEST(SmartProjectionRigFactor, noiseless) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor factor(model, cameraRig, params); factor.add(level_uv, x1); // both taken from the same camera @@ -198,8 +198,8 @@ TEST(SmartProjectionRigFactor, noiseless) { TEST(SmartProjectionRigFactor, noisy) { using namespace vanillaRig; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); // Project two landmarks into two cameras Point2 pixelError(0.2, 0.2); @@ -242,8 +242,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { // 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)); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_T_sensor, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_T_sensor, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body = body_T_sensor.inverse(); @@ -327,10 +327,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) { Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3 body_T_sensor3 = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_T_sensor1, sharedK)); - cameraRig.push_back(Camera(body_T_sensor2, sharedK)); - cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_T_sensor1, sharedK)); + cameraRig->push_back(Camera(body_T_sensor2, sharedK)); + cameraRig->push_back(Camera(body_T_sensor3, sharedK)); // These are the poses we want to estimate, from camera measurements const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); @@ -408,8 +408,8 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); // Project three landmarks into three cameras projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); @@ -496,8 +496,8 @@ TEST(SmartProjectionRigFactor, Factors) { KeyVector views{x1, x2}; FastVector cameraIds{0, 0}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared( model, cameraRig, params); @@ -579,8 +579,8 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // create smart factor - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( new SmartRigFactor(model, cameraRig, params)); @@ -647,8 +647,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -718,8 +718,8 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -784,8 +784,8 @@ TEST(SmartProjectionRigFactor, CheckHessian) { params.setRankTolerance(10); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -860,8 +860,8 @@ TEST(SmartProjectionRigFactor, Hessian) { measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam2_uv1); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK2)); FastVector cameraIds{0, 0}; SmartProjectionParams params( @@ -890,8 +890,8 @@ TEST(SmartProjectionRigFactor, Hessian) { /* ************************************************************************* */ TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); SmartProjectionParams params; params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); @@ -918,8 +918,8 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) { KeyVector views{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK)); FastVector cameraIds{0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -989,8 +989,8 @@ TEST(SmartProjectionRigFactor, // create inputs KeyVector keys{x1, x2, x3, x1}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0, 0}; SmartRigFactor::shared_ptr smartFactor1( @@ -1117,8 +1117,8 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) { // create inputs KeyVector keys{x1, x2, x3}; - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); FastVector cameraIds{0, 0, 0}; FastVector cameraIdsRedundant{0, 0, 0, 0}; @@ -1204,8 +1204,8 @@ TEST(SmartProjectionRigFactor, timing) { Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; // single camera in the rig - cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + boost::shared_ptr cameraRig(new Cameras()); // single camera in the rig + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); // one landmarks 1m in front of camera Point3 landmark1(0, 0, 10); From 620f9cb99fb5b5dd5bda1296eaf93f933b11e73b Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 7 Nov 2021 18:32:43 -0500 Subject: [PATCH 091/110] now using shared ptrs --- gtsam/slam/SmartProjectionRigFactor.h | 2 +- .../tests/testSmartProjectionRigFactor.cpp | 2 +- .../SmartProjectionPoseFactorRollingShutter.h | 17 ++-- ...martProjectionPoseFactorRollingShutter.cpp | 80 +++++++++---------- 4 files changed, 51 insertions(+), 50 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 87125020a..8d6918b3e 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -196,7 +196,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && - (*cameraRig_).equals(*(e->cameraRig())) && + cameraRig_->equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index b111ee572..b8150a1aa 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -29,7 +29,7 @@ #include #include "smartFactorScenarios.h" -//#define DISABLE_TIMING +#define DISABLE_TIMING using namespace boost::assign; using namespace std::placeholders; diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index fcec7de28..23203be67 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -59,7 +59,7 @@ class SmartProjectionPoseFactorRollingShutter /// one or more cameras taking observations (fixed poses wrt body + fixed /// intrinsics) - typename Base::Cameras cameraRig_; + boost::shared_ptr cameraRig_; /// vector of camera Ids (one for each observation, in the same order), /// identifying which camera took the measurement @@ -95,7 +95,8 @@ class SmartProjectionPoseFactorRollingShutter * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, + const SharedNoiseModel& sharedNoiseModel, + const boost::shared_ptr& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) : Base(sharedNoiseModel, params), cameraRig_(cameraRig) { // throw exception if configuration is not supported by this factor @@ -175,7 +176,7 @@ class SmartProjectionPoseFactorRollingShutter "SmartProjectionPoseFactorRollingShutter: " "trying to add inconsistent inputs"); } - if (cameraIds.size() == 0 && cameraRig_.size() > 1) { + if (cameraIds.size() == 0 && cameraRig_->size() > 1) { throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " "camera rig includes multiple camera " @@ -200,7 +201,7 @@ class SmartProjectionPoseFactorRollingShutter const std::vector& alphas() const { return alphas_; } /// return the calibration object - const Cameras& cameraRig() const { return cameraRig_; } + const boost::shared_ptr& cameraRig() const { return cameraRig_; } /// return the calibration object const FastVector& cameraIds() const { return cameraIds_; } @@ -222,7 +223,7 @@ class SmartProjectionPoseFactorRollingShutter << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl; - cameraRig_[cameraIds_[i]].print("camera in rig:\n"); + (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -251,7 +252,7 @@ class SmartProjectionPoseFactorRollingShutter } return e && Base::equals(p, tol) && alphas_ == e->alphas() && - keyPairsEqual && cameraRig_.equals(e->cameraRig()) && + keyPairsEqual && cameraRig_->equals(*(e->cameraRig())) && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } @@ -273,7 +274,7 @@ class SmartProjectionPoseFactorRollingShutter double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; const Pose3& body_P_cam = camera_i.pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); cameras.emplace_back(w_P_cam, @@ -326,7 +327,7 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; + const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]]; auto body_P_cam = camera_i.pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); PinholeCamera camera(w_P_cam, camera_i.calibration()); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 615f452d8..c17ad7e1c 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -29,7 +29,7 @@ #include #include "gtsam/slam/tests/smartFactorScenarios.h" -//#define DISABLE_TIMING +#define DISABLE_TIMING using namespace gtsam; using namespace boost::assign; @@ -87,8 +87,8 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); } @@ -96,8 +96,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); params.setRankTolerance(rankTol); SmartFactorRS factor1(model, cameraRig, params); } @@ -105,8 +105,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { using namespace vanillaPoseRS; - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig, params)); factor1->add(measurement1, x1, x2, interp_factor); @@ -134,8 +134,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { FastVector cameraIds{0, 0, 0}; - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensor, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensor, sharedK)); // create by adding a batch of measurements with a bunch of calibrations SmartFactorRS::shared_ptr factor2( @@ -189,8 +189,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { } { // create slightly different factors (different extrinsics) and show equal // returns false - Cameras cameraRig2; - cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); + boost::shared_ptr cameraRig2(new Cameras()); + cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); SmartFactorRS::shared_ptr factor1( new SmartFactorRS(model, cameraRig2, params)); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); @@ -232,8 +232,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorId = Pose3::identity(); - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorId, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedK)); SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); @@ -310,8 +310,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { Point2 level_uv_right = cam2.project(landmark1); Pose3 body_P_sensorNonId = body_P_sensor; - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorNonId, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorNonId, sharedK)); SmartFactorRS factor(model, cameraRig, params); factor.add(level_uv, x1, x2, interp_factor1); @@ -404,8 +404,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -478,9 +478,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensor, sharedK)); - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensor, sharedK)); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -566,10 +566,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(body_T_sensor1, sharedK)); - cameraRig.push_back(Camera(body_T_sensor2, sharedK)); - cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_T_sensor1, sharedK)); + cameraRig->push_back(Camera(body_T_sensor2, sharedK)); + cameraRig->push_back(Camera(body_T_sensor3, sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -648,8 +648,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1)); - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -746,8 +746,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(true); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -815,8 +815,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setEnableEPI(false); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); @@ -893,8 +893,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setEnableEPI(false); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -960,8 +960,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1101,8 +1101,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor1); - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1260,8 +1260,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, interp_factors_redundant.push_back( interp_factors.at(0)); // we readd the first interp factor - Cameras cameraRig; - cameraRig.push_back(Camera(Pose3::identity(), sharedK)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(Pose3::identity(), sharedK)); SmartFactorRS::shared_ptr smartFactor1( new SmartFactorRS(model, cameraRig, params)); @@ -1349,8 +1349,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) { size_t nrTests = 10000; for (size_t i = 0; i < nrTests; i++) { - Cameras cameraRig; - cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); + boost::shared_ptr cameraRig(new Cameras()); + cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple)); SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( model, cameraRig, params)); From 94aa96e00a9bb21e7cccb75219c614d1a91653ff Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 8 Nov 2021 22:41:59 +0100 Subject: [PATCH 092/110] prefer semicolon in definitions --- gtsam/base/Matrix.h | 20 ++++++++++---------- gtsam/base/Vector.h | 20 ++++++++++---------- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/Pose3.cpp | 2 +- gtsam/geometry/concepts.h | 2 +- gtsam/navigation/MagPoseFactor.h | 2 +- gtsam_unstable/slam/PoseBetweenFactor.h | 2 +- gtsam_unstable/slam/PosePriorFactor.h | 2 +- 8 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 26f03f3e3..61c61a5af 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -57,17 +57,17 @@ using Matrix7##N = Eigen::Matrix; \ using Matrix8##N = Eigen::Matrix; \ using Matrix9##N = Eigen::Matrix; \ static const Eigen::MatrixBase::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ -static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero() +static const Eigen::MatrixBase::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); -GTSAM_MAKE_MATRIX_DEFS(1); -GTSAM_MAKE_MATRIX_DEFS(2); -GTSAM_MAKE_MATRIX_DEFS(3); -GTSAM_MAKE_MATRIX_DEFS(4); -GTSAM_MAKE_MATRIX_DEFS(5); -GTSAM_MAKE_MATRIX_DEFS(6); -GTSAM_MAKE_MATRIX_DEFS(7); -GTSAM_MAKE_MATRIX_DEFS(8); -GTSAM_MAKE_MATRIX_DEFS(9); +GTSAM_MAKE_MATRIX_DEFS(1) +GTSAM_MAKE_MATRIX_DEFS(2) +GTSAM_MAKE_MATRIX_DEFS(3) +GTSAM_MAKE_MATRIX_DEFS(4) +GTSAM_MAKE_MATRIX_DEFS(5) +GTSAM_MAKE_MATRIX_DEFS(6) +GTSAM_MAKE_MATRIX_DEFS(7) +GTSAM_MAKE_MATRIX_DEFS(8) +GTSAM_MAKE_MATRIX_DEFS(9) // Matrix expressions for accessing parts of matrices typedef Eigen::Block SubMatrix; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 8a7eb1d55..35c68c4b4 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -49,17 +49,17 @@ static const Eigen::MatrixBase::ConstantReturnType Z_3x1 = Vector3::Zer // VectorN and Z_Nx1, for N=1..9 #define GTSAM_MAKE_VECTOR_DEFS(N) \ using Vector##N = Eigen::Matrix; \ - static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero() + static const Eigen::MatrixBase::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); -GTSAM_MAKE_VECTOR_DEFS(4); -GTSAM_MAKE_VECTOR_DEFS(5); -GTSAM_MAKE_VECTOR_DEFS(6); -GTSAM_MAKE_VECTOR_DEFS(7); -GTSAM_MAKE_VECTOR_DEFS(8); -GTSAM_MAKE_VECTOR_DEFS(9); -GTSAM_MAKE_VECTOR_DEFS(10); -GTSAM_MAKE_VECTOR_DEFS(11); -GTSAM_MAKE_VECTOR_DEFS(12); +GTSAM_MAKE_VECTOR_DEFS(4) +GTSAM_MAKE_VECTOR_DEFS(5) +GTSAM_MAKE_VECTOR_DEFS(6) +GTSAM_MAKE_VECTOR_DEFS(7) +GTSAM_MAKE_VECTOR_DEFS(8) +GTSAM_MAKE_VECTOR_DEFS(9) +GTSAM_MAKE_VECTOR_DEFS(10) +GTSAM_MAKE_VECTOR_DEFS(11) +GTSAM_MAKE_VECTOR_DEFS(12) typedef Eigen::VectorBlock SubVector; typedef Eigen::VectorBlock ConstSubVector; diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 8dafffee8..a0b3d502e 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -28,7 +28,7 @@ using namespace std; namespace gtsam { /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose2); +GTSAM_CONCEPT_POSE_INST(Pose2) static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 4bfb574b1..f8c61fd59 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -30,7 +30,7 @@ using std::vector; using Point3Pairs = vector; /** instantiate concept checks */ -GTSAM_CONCEPT_POSE_INST(Pose3); +GTSAM_CONCEPT_POSE_INST(Pose3) /* ************************************************************************* */ Pose3::Pose3(const Pose2& pose2) : diff --git a/gtsam/geometry/concepts.h b/gtsam/geometry/concepts.h index a313d4448..bafb62418 100644 --- a/gtsam/geometry/concepts.h +++ b/gtsam/geometry/concepts.h @@ -71,6 +71,6 @@ private: */ /** Pose Concept macros */ -#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept +#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept; #define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept; diff --git a/gtsam/navigation/MagPoseFactor.h b/gtsam/navigation/MagPoseFactor.h index c0a6a7ece..da2a54ce9 100644 --- a/gtsam/navigation/MagPoseFactor.h +++ b/gtsam/navigation/MagPoseFactor.h @@ -46,7 +46,7 @@ class MagPoseFactor: public NoiseModelFactor1 { /// Concept check by type. GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: ~MagPoseFactor() override {} diff --git a/gtsam_unstable/slam/PoseBetweenFactor.h b/gtsam_unstable/slam/PoseBetweenFactor.h index 444da275d..a6c583418 100644 --- a/gtsam_unstable/slam/PoseBetweenFactor.h +++ b/gtsam_unstable/slam/PoseBetweenFactor.h @@ -41,7 +41,7 @@ namespace gtsam { /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: // shorthand for a smart pointer to a factor diff --git a/gtsam_unstable/slam/PosePriorFactor.h b/gtsam_unstable/slam/PosePriorFactor.h index 665bb4680..f657fc443 100644 --- a/gtsam_unstable/slam/PosePriorFactor.h +++ b/gtsam_unstable/slam/PosePriorFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** concept check by type */ GTSAM_CONCEPT_TESTABLE_TYPE(POSE) - GTSAM_CONCEPT_POSE_TYPE(POSE); + GTSAM_CONCEPT_POSE_TYPE(POSE) public: /// shorthand for a smart pointer to a factor From 582f6914cd8491a83bb40fcc4091a4dd83d69c20 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco Claraco Date: Mon, 8 Nov 2021 23:07:05 +0100 Subject: [PATCH 093/110] more extra semicolon warnings fixed --- .../linear/tests/testSerializationLinear.cpp | 20 +-- gtsam/navigation/CombinedImuFactor.cpp | 2 +- gtsam/navigation/CombinedImuFactor.h | 2 +- gtsam/navigation/tests/testAttitudeFactor.cpp | 2 +- .../tests/testImuFactorSerialization.cpp | 14 +- .../tests/testSerializationNonlinear.cpp | 48 +++--- gtsam/slam/tests/testSmartFactorBase.cpp | 14 +- .../slam/tests/testSmartProjectionFactor.cpp | 14 +- .../tests/testSmartProjectionPoseFactor.cpp | 14 +- gtsam_unstable/linear/tests/testQPSolver.cpp | 2 +- tests/testExpressionFactor.cpp | 2 +- tests/testSerializationSLAM.cpp | 142 +++++++++--------- tests/testSubgraphPreconditioner.cpp | 2 +- 13 files changed, 139 insertions(+), 139 deletions(-) diff --git a/gtsam/linear/tests/testSerializationLinear.cpp b/gtsam/linear/tests/testSerializationLinear.cpp index c5b3dab37..881b2830e 100644 --- a/gtsam/linear/tests/testSerializationLinear.cpp +++ b/gtsam/linear/tests/testSerializationLinear.cpp @@ -39,14 +39,14 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -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::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"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // example noise models @@ -129,9 +129,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) { /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ TEST (Serialization, linear_factors) { diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index ca1c5b93a..7d086eb57 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -257,5 +257,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) { /// namespace gtsam /// Boost serialization export definition for derived class -BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index ed94750b7..89e8b574f 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -353,4 +353,4 @@ struct traits : public Testable {}; } // namespace gtsam /// Add Boost serialization export key (declaration) for derived class -BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams); +BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams) diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index d49907cbf..9304e8412 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -66,7 +66,7 @@ TEST( Rot3AttitudeFactor, Constructor ) { /* ************************************************************************* */ // Export Noisemodels // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html -BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); +BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic) /* ************************************************************************* */ TEST(Rot3AttitudeFactor, Serialization) { diff --git a/gtsam/navigation/tests/testImuFactorSerialization.cpp b/gtsam/navigation/tests/testImuFactorSerialization.cpp index ed72e18e9..e72b1fb5b 100644 --- a/gtsam/navigation/tests/testImuFactorSerialization.cpp +++ b/gtsam/navigation/tests/testImuFactorSerialization.cpp @@ -31,16 +31,16 @@ using namespace gtsam; using namespace gtsam::serializationTestHelpers; BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, - "gtsam_noiseModel_Constrained"); + "gtsam_noiseModel_Constrained") BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, - "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"); + "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"); + "gtsam_noiseModel_Isotropic") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") template P getPreintegratedMeasurements() { diff --git a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp index f4bb5f4f6..4a73cbb0b 100644 --- a/gtsam/nonlinear/tests/testSerializationNonlinear.cpp +++ b/gtsam/nonlinear/tests/testSerializationNonlinear.cpp @@ -35,37 +35,37 @@ using namespace gtsam::serializationTestHelpers; /* ************************************************************************* */ // Create GUIDs for Noisemodels -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); -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::noiseModel::Robust, "gtsam_noiseModel_Robust"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained") +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::noiseModel::Robust, "gtsam_noiseModel_Robust") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* ************************************************************************* */ // Create GUIDs for factors -BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); +BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor, "gtsam::PriorFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional") /* ************************************************************************* */ // Export all classes derived from Value -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCamera); +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCamera) namespace detail { template struct pack { diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index 951cbf8f4..ba46dce8d 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -127,13 +127,13 @@ TEST(SmartFactorBase, Stereo) { } /* ************************************************************************* */ -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"); +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(SmartFactorBase, serialize) { using namespace gtsam::serializationTestHelpers; diff --git a/gtsam/slam/tests/testSmartProjectionFactor.cpp b/gtsam/slam/tests/testSmartProjectionFactor.cpp index 1fd06cc9f..133f81511 100644 --- a/gtsam/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionFactor.cpp @@ -811,13 +811,13 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) { } /* ************************************************************************* */ -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"); +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(SmartProjectionFactor, serialize) { using namespace vanilla; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 997c33846..f3706fa02 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -1333,13 +1333,13 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { } /* ************************************************************************* */ -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"); +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; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 67a0c971e..12bd93416 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -226,7 +226,7 @@ pair testParser(QPSParser parser) { expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0 expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0 return {expected, exampleqp}; -}; +} TEST(QPSolver, ParserSyntaticTest) { auto result = testParser(QPSParser("QPExample.QPS")); diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 66dbed1eb..75425a0cd 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -606,7 +606,7 @@ Vector3 f(const Point2& a, const Vector3& b, OptionalJacobian<3, 2> H1, if (H1) *H1 << b.y(), b.z(), b.x(), 0, 0, 0; if (H2) *H2 = A; return A * b; -}; +} } TEST(ExpressionFactor, MultiplyWithInverseFunction) { diff --git a/tests/testSerializationSLAM.cpp b/tests/testSerializationSLAM.cpp index 2e99aff71..496122419 100644 --- a/tests/testSerializationSLAM.cpp +++ b/tests/testSerializationSLAM.cpp @@ -114,94 +114,94 @@ using symbol_shorthand::L; /* Create GUIDs for Noisemodels */ /* ************************************************************************* */ -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::noiseModel::Robust, "gtsam_noiseModel_Robust"); +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::noiseModel::Robust, "gtsam_noiseModel_Robust") -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); -BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber") +BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey") -BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); -BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); +BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel") +BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal") /* Create GUIDs for geometry */ /* ************************************************************************* */ -GTSAM_VALUE_EXPORT(gtsam::Point2); -GTSAM_VALUE_EXPORT(gtsam::StereoPoint2); -GTSAM_VALUE_EXPORT(gtsam::Point3); -GTSAM_VALUE_EXPORT(gtsam::Rot2); -GTSAM_VALUE_EXPORT(gtsam::Rot3); -GTSAM_VALUE_EXPORT(gtsam::Pose2); -GTSAM_VALUE_EXPORT(gtsam::Pose3); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); -GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); -GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); -GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); -GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2); -GTSAM_VALUE_EXPORT(gtsam::StereoCamera); +GTSAM_VALUE_EXPORT(gtsam::Point2) +GTSAM_VALUE_EXPORT(gtsam::StereoPoint2) +GTSAM_VALUE_EXPORT(gtsam::Point3) +GTSAM_VALUE_EXPORT(gtsam::Rot2) +GTSAM_VALUE_EXPORT(gtsam::Rot3) +GTSAM_VALUE_EXPORT(gtsam::Pose2) +GTSAM_VALUE_EXPORT(gtsam::Pose3) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2) +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2) +GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo) +GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera) +GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2) +GTSAM_VALUE_EXPORT(gtsam::StereoCamera) /* Create GUIDs for factors */ /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); -BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor") +BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor") -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2"); -BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera"); +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint2, "gtsam::PriorFactorPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoPoint2, "gtsam::PriorFactorStereoPoint2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPoint3, "gtsam::PriorFactorPoint3") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot2, "gtsam::PriorFactorRot2") +BOOST_CLASS_EXPORT_GUID(PriorFactorRot3, "gtsam::PriorFactorRot3") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose2, "gtsam::PriorFactorPose2") +BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2") +BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera") -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2"); -BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3"); +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint2, "gtsam::BetweenFactorPoint2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPoint3, "gtsam::BetweenFactorPoint3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot2, "gtsam::BetweenFactorRot2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorRot3, "gtsam::BetweenFactorRot3") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose2, "gtsam::BetweenFactorPose2") +BOOST_CLASS_EXPORT_GUID(BetweenFactorPose3, "gtsam::BetweenFactorPose3") -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera"); +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint2, "gtsam::NonlinearEqualityPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoPoint2, "gtsam::NonlinearEqualityStereoPoint2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPoint3, "gtsam::NonlinearEqualityPoint3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot2, "gtsam::NonlinearEqualityRot2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityRot3, "gtsam::NonlinearEqualityRot3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose2, "gtsam::NonlinearEqualityPose2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera") -BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D"); -BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point"); -BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera"); -BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2"); +BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D") +BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2") +BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point") +BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera") +BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2") -BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D"); +BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D") -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectionFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2") +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2") -BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2") -BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D"); +BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D") /* ************************************************************************* */ diff --git a/tests/testSubgraphPreconditioner.cpp b/tests/testSubgraphPreconditioner.cpp index fb9f7a5a2..84ccc131a 100644 --- a/tests/testSubgraphPreconditioner.cpp +++ b/tests/testSubgraphPreconditioner.cpp @@ -200,7 +200,7 @@ TEST(SubgraphPreconditioner, system) { } /* ************************************************************************* */ -BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor"); +BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "JacobianFactor") // Read from XML file static GaussianFactorGraph read(const string& name) { From 7d468e98a04a581caeafb674e0ac4af024552f59 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 14:50:48 -0500 Subject: [PATCH 094/110] fix warning --- gtsam/nonlinear/GncOptimizer.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 3ddaf4820..3025d2468 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer { * provides an extra interface for the user to initialize the weightst * */ void setWeights(const Vector w) { - if(w.size() != nfg_.size()){ - throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" + if (size_t(w.size()) != nfg_.size()) { + throw std::runtime_error( + "GncOptimizer::setWeights: the number of specified weights" " does not match the size of the factor graph."); } weights_ = w; From bfb21c2faad20e2731cd4d98c56c9b78d4e60ea1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 16:50:14 -0500 Subject: [PATCH 095/110] reduce call stack --- gtsam/inference/EliminateableFactorGraph-inst.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 81f4047a1..4157336d1 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -36,17 +36,17 @@ namespace gtsam { // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // before creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateSequential(function, computedVariableIndex, orderingType); + return eliminateSequential(orderingType, function, computedVariableIndex); } else { // Compute an ordering and call this function again. We are guaranteed to have a // VariableIndex already here because we computed one if needed in the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateSequential(computedOrdering, function, variableIndex, orderingType); + return eliminateSequential(computedOrdering, function, variableIndex); } } } From 916504129990b40683427b81453cacbc787a82da Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 17:56:03 -0500 Subject: [PATCH 096/110] use safer eigen indexing syntax --- gtsam/nonlinear/factorTesting.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/factorTesting.h b/gtsam/nonlinear/factorTesting.h index 74ef87737..3a9b6fb11 100644 --- a/gtsam/nonlinear/factorTesting.h +++ b/gtsam/nonlinear/factorTesting.h @@ -35,8 +35,7 @@ namespace gtsam { * zero errors anyway. However, it means that below will only be exact for the correct measurement. */ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, - const Values& values, double delta = 1e-5) { - + const Values& values, double delta = 1e-5) { // We will fill a vector of key/Jacobians pairs (a map would sort) std::vector > jacobians; @@ -46,24 +45,24 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, // Loop over all variables const double one_over_2delta = 1.0 / (2.0 * delta); - for(Key key: factor) { + for (Key key : factor) { // Compute central differences using the values struct. VectorValues dX = values.zeroVectors(); const size_t cols = dX.dim(key); Matrix J = Matrix::Zero(rows, cols); for (size_t col = 0; col < cols; ++col) { Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = delta; + dx(col) = delta; dX[key] = dx; Values eval_values = values.retract(dX); const Eigen::VectorXd left = factor.whitenedError(eval_values); - dx[col] = -delta; + dx(col) = -delta; dX[key] = dx; eval_values = values.retract(dX); const Eigen::VectorXd right = factor.whitenedError(eval_values); J.col(col) = (left - right) * one_over_2delta; } - jacobians.push_back(std::make_pair(key,J)); + jacobians.push_back(std::make_pair(key, J)); } // Next step...return JacobianFactor From a634a91c1a7233d1afaa016d3e909ae7077d4b73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:14:01 -0500 Subject: [PATCH 097/110] wrap Colamd function --- gtsam/nonlinear/nonlinear.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index d068bd7ee..ecf63094d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -115,6 +115,10 @@ class Ordering { Ordering(); Ordering(const gtsam::Ordering& other); + template + static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph); + // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; From 1bcb44784a1dc7f5239001b1cec6a6383a058ed9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:19:47 -0500 Subject: [PATCH 098/110] format and refactor the SFM BAL example --- python/gtsam/examples/SFMExample_bal.py | 107 +++++++++++++----------- 1 file changed, 59 insertions(+), 48 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index dfe8b523c..65b9e1334 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -7,49 +7,58 @@ See LICENSE for the license information Solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file - Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert) + Author: Frank Dellaert (Python: Akshay Krishnan, John Lambert, Varun Agrawal) """ import argparse import logging import sys -import matplotlib.pyplot as plt -import numpy as np - import gtsam -from gtsam import ( - GeneralSFMFactorCal3Bundler, - PinholeCameraCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, - readBal, - symbol_shorthand -) +from gtsam import (GeneralSFMFactorCal3Bundler, + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, + readBal) +from gtsam.symbol_shorthand import C, P +from gtsam.utils.plot import plot_3d_points, plot_trajectory -C = symbol_shorthand.C -P = symbol_shorthand.P +logging.basicConfig(stream=sys.stdout, level=logging.INFO) -logging.basicConfig(stream=sys.stdout, level=logging.DEBUG) +def plot(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the trajectory.""" + plot_vals = gtsam.Values() + for cam_idx in range(scene_data.number_cameras()): + plot_vals.insert(C(cam_idx), + result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) + for t_idx in range(scene_data.number_tracks()): + plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) -def run(args): + plot_3d_points(0, plot_vals, linespec="g.") + plot_trajectory(0, plot_vals, show=True) + + +def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - input_file = gtsam.findExampleDataFile(args.input_file) + if args.input_file: + input_file = args.input_file + else: + input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") # Load the SfM data from file scene_data = readBal(input_file) - logging.info(f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n") + logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), + scene_data.number_cameras()) # Create a factor graph graph = gtsam.NonlinearFactorGraph() # We share *one* noiseModel between all projection factors - noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v + noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph j = 0 for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + track = scene_data.track(t_idx) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement @@ -60,20 +69,18 @@ def run(args): # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( - gtsam.PriorFactorPinholeCameraCal3Bundler( - C(0), scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1) - ) - ) + PriorFactorPinholeCameraCal3Bundler( + C(0), scene_data.camera(0), + gtsam.noiseModel.Isotropic.Sigma(9, 0.1))) # Also add a prior on the position of the first landmark to fix the scale graph.push_back( - gtsam.PriorFactorPoint3( - P(0), scene_data.track(0).point3(), gtsam.noiseModel.Isotropic.Sigma(3, 0.1) - ) - ) + PriorFactorPoint3(P(0), + scene_data.track(0).point3(), + gtsam.noiseModel.Isotropic.Sigma(3, 0.1))) # Create initial estimate initial = gtsam.Values() - + i = 0 # add each PinholeCameraCal3Bundler for cam_idx in range(scene_data.number_cameras()): @@ -81,12 +88,10 @@ def run(args): initial.insert(C(i), camera) i += 1 - j = 0 # add each SfmTrack for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(j), track.point3()) - j += 1 + track = scene_data.track(t_idx) + initial.insert(P(t_idx), track.point3()) # Optimize the graph and print results try: @@ -94,25 +99,31 @@ def run(args): params.setVerbosityLM("ERROR") lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = lm.optimize() - except Exception as e: + except RuntimeError: logging.exception("LM Optimization failed") return + # Error drops from ~2764.22 to ~0.046 - logging.info(f"final error: {graph.error(result)}") + logging.info("initial error: %f", graph.error(initial)) + logging.info("final error: %f", graph.error(result)) + + plot(scene_data, result) + + +def main(): + """Main runner.""" + parser = argparse.ArgumentParser() + parser.add_argument('-i', + '--input_file', + type=str, + default="", + help="""Read SFM data from the specified BAL file. + The data format is described here: https://grail.cs.washington.edu/projects/bal/. + BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, + then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector + and (x,y,z) 3d point initializations.""") + run(parser.parse_args()) if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - '-i', - '--input_file', - type=str, - default="dubrovnik-3-7-pre", - help='Read SFM data from the specified BAL file' - 'The data format is described here: https://grail.cs.washington.edu/projects/bal/.' - 'BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, ' - 'then (wx,wy,wz,tx,ty,tz,f,k1,k1) as Bundler camera calibrations w/ Rodrigues vector' - 'and (x,y,z) 3d point initializations.' - ) - run(parser.parse_args()) - + main() From 5051f19f307783420365e1e263ac664710d94a33 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 9 Nov 2021 18:25:42 -0500 Subject: [PATCH 099/110] properly deprecate eliminate functions --- .../inference/EliminateableFactorGraph-inst.h | 42 ++++++++++--------- gtsam/inference/EliminateableFactorGraph.h | 2 + gtsam/linear/GaussianFactorGraph.cpp | 5 ++- gtsam/nonlinear/Marginals.cpp | 11 +++-- gtsam/nonlinear/NonlinearOptimizer.cpp | 10 +++-- 5 files changed, 40 insertions(+), 30 deletions(-) diff --git a/gtsam/inference/EliminateableFactorGraph-inst.h b/gtsam/inference/EliminateableFactorGraph-inst.h index 4157336d1..35e7505c9 100644 --- a/gtsam/inference/EliminateableFactorGraph-inst.h +++ b/gtsam/inference/EliminateableFactorGraph-inst.h @@ -78,29 +78,31 @@ namespace gtsam { } /* ************************************************************************* */ - template - boost::shared_ptr::BayesTreeType> - EliminateableFactorGraph::eliminateMultifrontal( - OptionalOrderingType orderingType, const Eliminate& function, - OptionalVariableIndex variableIndex) const - { - if(!variableIndex) { - // If no VariableIndex provided, compute one and call this function again IMPORTANT: we check - // for no variable index first so that it's always computed if we need to call COLAMD because - // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex - // before creating ordering. + template + boost::shared_ptr< + typename EliminateableFactorGraph::BayesTreeType> + EliminateableFactorGraph::eliminateMultifrontal( + OptionalOrderingType orderingType, const Eliminate& function, + OptionalVariableIndex variableIndex) const { + if (!variableIndex) { + // If no VariableIndex provided, compute one and call this function again + // IMPORTANT: we check for no variable index first so that it's always + // computed if we need to call COLAMD because no Ordering is provided. + // When removing optional from VariableIndex, create VariableIndex before + // creating ordering. VariableIndex computedVariableIndex(asDerived()); - return eliminateMultifrontal(function, computedVariableIndex, orderingType); - } - else { - // Compute an ordering and call this function again. We are guaranteed to have a - // VariableIndex already here because we computed one if needed in the previous 'if' block. + return eliminateMultifrontal(orderingType, function, + computedVariableIndex); + } else { + // Compute an ordering and call this function again. We are guaranteed to + // have a VariableIndex already here because we computed one if needed in + // the previous 'if' block. if (orderingType == Ordering::METIS) { Ordering computedOrdering = Ordering::Metis(asDerived()); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } else { Ordering computedOrdering = Ordering::Colamd(*variableIndex); - return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); + return eliminateMultifrontal(computedOrdering, function, variableIndex); } } } @@ -273,7 +275,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateSequential(function); + return factorGraph->eliminateSequential(Ordering::COLAMD, function); } } } @@ -340,7 +342,7 @@ namespace gtsam { else { // No ordering was provided for the unmarginalized variables, so order them with COLAMD. - return factorGraph->eliminateMultifrontal(function); + return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function); } } } diff --git a/gtsam/inference/EliminateableFactorGraph.h b/gtsam/inference/EliminateableFactorGraph.h index edc4883e7..3c51d8f84 100644 --- a/gtsam/inference/EliminateableFactorGraph.h +++ b/gtsam/inference/EliminateableFactorGraph.h @@ -288,6 +288,7 @@ namespace gtsam { FactorGraphType& asDerived() { return static_cast(*this); } public: + #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated ordering and orderingType shouldn't both be specified */ boost::shared_ptr eliminateSequential( const Ordering& ordering, @@ -339,6 +340,7 @@ namespace gtsam { OptionalVariableIndex variableIndex = boost::none) const { return marginalMultifrontalBayesTree(variables, function, variableIndex); } + #endif }; } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 24c4b9a0d..dc2eb8dd6 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -290,10 +290,11 @@ namespace gtsam { return blocks; } - /* ************************************************************************* */ + /* ************************************************************************ */ VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { gttic(GaussianFactorGraph_optimize); - return BaseEliminateable::eliminateMultifrontal(function)->optimize(); + return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function) + ->optimize(); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index c29a79623..2a3dddc37 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -80,11 +80,14 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ void Marginals::computeBayesTree() { + // The default ordering to use. + const Ordering ordering = Ordering::COLAMND; // Compute BayesTree - if(factorization_ == CHOLESKY) - bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); - else if(factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); + if (factorization_ == CHOLESKY) + bayesTree_ = + *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); + else if (factorization_ == QR) + bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index 0d7e9e17f..3ce6db4af 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg, } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) if (params.ordering) - delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), - boost::none, params.orderingType)->optimize(); + delta = gfg.eliminateSequential(*params.ordering, + params.getEliminationFunction()) + ->optimize(); else - delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, - params.orderingType)->optimize(); + delta = gfg.eliminateSequential(params.orderingType, + params.getEliminationFunction()) + ->optimize(); } else if (params.isIterative()) { // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) From 5c9c60a0be9e0ac556e0b6801412814ecb7434bd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 13:57:31 -0500 Subject: [PATCH 100/110] address reviewer comments --- python/gtsam/examples/SFMExample_bal.py | 45 ++++++++++++------------- 1 file changed, 22 insertions(+), 23 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 65b9e1334..77c186bd3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -16,36 +16,37 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, - PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3, - readBal) + PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) from gtsam.symbol_shorthand import C, P -from gtsam.utils.plot import plot_3d_points, plot_trajectory +from gtsam.utils import plot +from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) +DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot(scene_data: gtsam.SfmData, result: gtsam.Values): - """Plot the trajectory.""" + +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): + """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): plot_vals.insert(C(cam_idx), result.atPinholeCameraCal3Bundler(C(cam_idx)).pose()) - for t_idx in range(scene_data.number_tracks()): - plot_vals.insert(P(t_idx), result.atPoint3(P(t_idx))) + for j in range(scene_data.number_tracks()): + plot_vals.insert(P(j), result.atPoint3(P(j))) - plot_3d_points(0, plot_vals, linespec="g.") - plot_trajectory(0, plot_vals, show=True) + plot.plot_3d_points(0, plot_vals, linespec="g.") + plot.plot_trajectory(0, plot_vals, title="SFM results") + + plt.show() def run(args: argparse.Namespace): """ Run LM optimization with BAL input data and report resulting error """ - if args.input_file: - input_file = args.input_file - else: - input_file = gtsam.findExampleDataFile("dubrovnik-3-7-pre") + input_file = args.input_file # Load the SfM data from file - scene_data = readBal(input_file) + scene_data = gtsam.readBal(input_file) logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(), scene_data.number_cameras()) @@ -56,16 +57,14 @@ def run(args: argparse.Namespace): noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v # Add measurements to the factor graph - j = 0 - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) # SfmTrack + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) # SfmTrack # retrieve the SfmMeasurement objects for m_idx in range(track.number_measurements()): # i represents the camera index, and uv is the 2d measurement i, uv = track.measurement(m_idx) # note use of shorthand symbols C and P graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j))) - j += 1 # Add a prior on pose x1. This indirectly specifies where the origin is. graph.push_back( @@ -89,9 +88,9 @@ def run(args: argparse.Namespace): i += 1 # add each SfmTrack - for t_idx in range(scene_data.number_tracks()): - track = scene_data.track(t_idx) - initial.insert(P(t_idx), track.point3()) + for j in range(scene_data.number_tracks()): + track = scene_data.track(j) + initial.insert(P(j), track.point3()) # Optimize the graph and print results try: @@ -107,7 +106,7 @@ def run(args: argparse.Namespace): logging.info("initial error: %f", graph.error(initial)) logging.info("final error: %f", graph.error(result)) - plot(scene_data, result) + plot_scene(scene_data, result) def main(): @@ -116,7 +115,7 @@ def main(): parser.add_argument('-i', '--input_file', type=str, - default="", + default=gtsam.findExampleDataFile(DEFAULT_BAL_DATASET), help="""Read SFM data from the specified BAL file. The data format is described here: https://grail.cs.washington.edu/projects/bal/. BAL files contain (nrPoses, nrPoints, nrObservations), followed by (i,j,u,v) tuples, From 0ccb18b055074777593f6169e2c5110037cd2cab Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 14:27:28 -0500 Subject: [PATCH 101/110] add return type definitions Because my time is more valuable than a reviewer's pedanticness --- python/gtsam/examples/SFMExample_bal.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/gtsam/examples/SFMExample_bal.py b/python/gtsam/examples/SFMExample_bal.py index 77c186bd3..f3e48c3c3 100644 --- a/python/gtsam/examples/SFMExample_bal.py +++ b/python/gtsam/examples/SFMExample_bal.py @@ -17,8 +17,8 @@ import sys import gtsam from gtsam import (GeneralSFMFactorCal3Bundler, PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3) -from gtsam.symbol_shorthand import C, P -from gtsam.utils import plot +from gtsam.symbol_shorthand import C, P # type: ignore +from gtsam.utils import plot # type: ignore from matplotlib import pyplot as plt logging.basicConfig(stream=sys.stdout, level=logging.INFO) @@ -26,7 +26,7 @@ logging.basicConfig(stream=sys.stdout, level=logging.INFO) DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre" -def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): +def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None: """Plot the SFM results.""" plot_vals = gtsam.Values() for cam_idx in range(scene_data.number_cameras()): @@ -41,7 +41,7 @@ def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values): plt.show() -def run(args: argparse.Namespace): +def run(args: argparse.Namespace) -> None: """ Run LM optimization with BAL input data and report resulting error """ input_file = args.input_file @@ -109,7 +109,7 @@ def run(args: argparse.Namespace): plot_scene(scene_data, result) -def main(): +def main() -> None: """Main runner.""" parser = argparse.ArgumentParser() parser.add_argument('-i', From 57d7103a4717ea854d6c10cab74fca6a920582e2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 18:22:54 -0500 Subject: [PATCH 102/110] fix OrderingType declaration --- gtsam/nonlinear/Marginals.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/Marginals.cpp b/gtsam/nonlinear/Marginals.cpp index 2a3dddc37..41212ed76 100644 --- a/gtsam/nonlinear/Marginals.cpp +++ b/gtsam/nonlinear/Marginals.cpp @@ -81,13 +81,14 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut /* ************************************************************************* */ void Marginals::computeBayesTree() { // The default ordering to use. - const Ordering ordering = Ordering::COLAMND; + const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD; // Compute BayesTree if (factorization_ == CHOLESKY) - bayesTree_ = - *graph_.eliminateMultifrontal(ordering, EliminatePreferCholesky); + bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType, + EliminatePreferCholesky); else if (factorization_ == QR) - bayesTree_ = *graph_.eliminateMultifrontal(ordering, EliminateQR); + bayesTree_ = + *graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR); } /* ************************************************************************* */ From 0cbec6736aed04361df4495ef670062b4e1eadef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 Nov 2021 18:23:38 -0500 Subject: [PATCH 103/110] update cmake to copy python tests whenever they are updated --- python/CMakeLists.txt | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index b703f5900..e2444a51a 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -89,12 +89,19 @@ set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +# Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) -# Symlink all tests .py files to build folder. +# Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam" "${GTSAM_MODULE_PATH}") +# Hack to get python test files copied every time they are modified +file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py") +foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY) +endforeach() + # Common directory for data/datasets stored with the package. # This will store the data in the Python site package directly. file(COPY "${GTSAM_SOURCE_DIR}/examples/Data" DESTINATION "${GTSAM_MODULE_PATH}") @@ -147,10 +154,16 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set(GTSAM_UNSTABLE_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable) - # Symlink all tests .py files to build folder. + # Copy all python files to build folder. copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable" "${GTSAM_UNSTABLE_MODULE_PATH}") + # Hack to get python test files copied every time they are modified + file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py") + foreach(test_file ${GTSAM_PYTHON_TEST_FILES}) + configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY) + endforeach() + # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) From d86fc9870688e3652416a927495691742018f7de Mon Sep 17 00:00:00 2001 From: Gerry Chen Date: Wed, 10 Nov 2021 19:01:31 -0500 Subject: [PATCH 104/110] update python wrapper --- gtsam/geometry/geometry.i | 3 +++ python/gtsam/tests/test_Pose3.py | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 9baa49e8e..a40951d3e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -473,6 +473,9 @@ class Pose3 { Vector logmap(const gtsam::Pose3& pose); Matrix AdjointMap() const; Vector Adjoint(Vector xi) const; + Vector AdjointTranspose(Vector xi) const; + static Matrix adjointMap(Vector xi); + static Vector adjoint(Vector xi, Vector y); static Matrix adjointMap_(Vector xi); static Vector adjoint_(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y); diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index e07b904a9..411828890 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -59,8 +59,16 @@ class TestPose3(GtsamTestCase): self.assertEqual(math.sqrt(2.0), x1.range(pose=xl2)) def test_adjoint(self): - """Test adjoint method.""" + """Test adjoint methods.""" + T = Pose3() xi = np.array([1, 2, 3, 4, 5, 6]) + # test calling functions + T.AdjointMap() + T.Adjoint(xi) + T.AdjointTranspose(xi) + Pose3.adjointMap(xi) + Pose3.adjoint(xi, xi) + # test correctness of adjoint(x, y) expected = np.dot(Pose3.adjointMap_(xi), xi) actual = Pose3.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) From b82acc133b46cdd2b4a57ca327757079a37c7124 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Nov 2021 14:24:15 -0500 Subject: [PATCH 105/110] properly deprecate additional methods to fully finish deprecation --- gtsam/linear/GaussianFactorGraph.cpp | 7 ------- gtsam/linear/GaussianFactorGraph.h | 11 ++++++++--- gtsam/nonlinear/Marginals.h | 2 ++ gtsam/nonlinear/NonlinearFactorGraph.h | 2 ++ 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index dc2eb8dd6..36db9f5d7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -504,13 +504,6 @@ namespace gtsam { return e; } - /* ************************************************************************* */ - /** \deprecated */ - VectorValues GaussianFactorGraph::optimize(boost::none_t, - const Eliminate& function) const { - return optimize(function); - } - /* ************************************************************************* */ void GaussianFactorGraph::printErrors( const VectorValues& values, const std::string& str, diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index d41374854..2dc5747cd 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -395,9 +395,14 @@ namespace gtsam { public: - /** \deprecated */ - VectorValues optimize(boost::none_t, - const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 + /** \deprecated */ + VectorValues optimize(boost::none_t, + const Eliminate& function = + EliminationTraitsType::DefaultEliminate) const { + return optimize(function); + } +#endif }; diff --git a/gtsam/nonlinear/Marginals.h b/gtsam/nonlinear/Marginals.h index 9935bafdd..e73038db0 100644 --- a/gtsam/nonlinear/Marginals.h +++ b/gtsam/nonlinear/Marginals.h @@ -131,6 +131,7 @@ protected: void computeBayesTree(const Ordering& ordering); public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated argument order changed due to removing boost::optional */ Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} @@ -142,6 +143,7 @@ public: /** \deprecated argument order changed due to removing boost::optional */ Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} +#endif }; diff --git a/gtsam/nonlinear/NonlinearFactorGraph.h b/gtsam/nonlinear/NonlinearFactorGraph.h index 4d321f8ab..5b9ed4bb1 100644 --- a/gtsam/nonlinear/NonlinearFactorGraph.h +++ b/gtsam/nonlinear/NonlinearFactorGraph.h @@ -265,6 +265,7 @@ namespace gtsam { public: +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** \deprecated */ boost::shared_ptr linearizeToHessianFactor( const Values& values, boost::none_t, const Dampen& dampen = nullptr) const @@ -274,6 +275,7 @@ namespace gtsam { Values updateCholesky(const Values& values, boost::none_t, const Dampen& dampen = nullptr) const {return updateCholesky(values, dampen);} +#endif }; From cf0c8d2fee04c14a6fba59a814988f3fd4ff082f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 Nov 2021 15:45:27 -0500 Subject: [PATCH 106/110] fix VectorValues include --- gtsam/linear/GaussianFactorGraph.cpp | 1 - gtsam/linear/GaussianFactorGraph.h | 7 ++++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 36db9f5d7..664aeff6d 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -19,7 +19,6 @@ */ #include -#include #include #include #include diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 2dc5747cd..5ccb0ce91 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -21,12 +21,13 @@ #pragma once -#include #include +#include +#include // Included here instead of fw-declared so we can use Errors::iterator #include -#include #include -#include // Included here instead of fw-declared so we can use Errors::iterator +#include +#include namespace gtsam { From fe6eb433e0472090198f311755ce4b0d25d59d3c Mon Sep 17 00:00:00 2001 From: duembgen Date: Mon, 15 Nov 2021 11:10:29 +0100 Subject: [PATCH 107/110] Add namespace to ambiguous placeholders --- gtsam/navigation/tests/testGPSFactor.cpp | 4 ++-- gtsam/navigation/tests/testMagFactor.cpp | 16 ++++++++-------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index b784c0c94..c94e1d3d5 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; @@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) { // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); + std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; diff --git a/gtsam/navigation/tests/testMagFactor.cpp b/gtsam/navigation/tests/testMagFactor.cpp index 5107b3b6b..85447facd 100644 --- a/gtsam/navigation/tests/testMagFactor.cpp +++ b/gtsam/navigation/tests/testMagFactor.cpp @@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) { Point3 expected(22735.5, 314.502, 44202.5); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); + (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); } // ************************************************************************* @@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) { MagFactor f(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); + (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); // MagFactor1 MagFactor1 f1(1, measured, s, dir, bias, model); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); + (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); // MagFactor2 MagFactor2 f2(1, 2, measured, nRb, model); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// + (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// H1, 1e-7)); EXPECT( assert_equal(numericalDerivative11 // - (std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// + (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// H2, 1e-7)); // MagFactor2 MagFactor3 f3(1, 2, 3, measured, nRb, model); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal((Matrix)numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// + (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),// H1, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// + (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),// H2, 1e-7)); EXPECT(assert_equal(numericalDerivative11 // - (std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// + (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),// H3, 1e-7)); } From d27d6b60a78278b02a153eb7073c2a201c5e8d7e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 10:54:00 -0500 Subject: [PATCH 108/110] Formatting with Google style --- gtsam_unstable/discrete/AllDiff.cpp | 172 +++--- gtsam_unstable/discrete/AllDiff.h | 120 ++--- gtsam_unstable/discrete/BinaryAllDiff.h | 157 +++--- gtsam_unstable/discrete/CSP.cpp | 161 +++--- gtsam_unstable/discrete/CSP.h | 145 +++-- gtsam_unstable/discrete/Constraint.h | 117 ++-- gtsam_unstable/discrete/Domain.cpp | 161 +++--- gtsam_unstable/discrete/Domain.h | 184 +++---- gtsam_unstable/discrete/Scheduler.cpp | 510 +++++++++--------- gtsam_unstable/discrete/Scheduler.h | 253 ++++----- gtsam_unstable/discrete/SingleValue.cpp | 129 +++-- gtsam_unstable/discrete/SingleValue.h | 127 +++-- gtsam_unstable/discrete/tests/testCSP.cpp | 150 +++--- .../discrete/tests/testLoopyBelief.cpp | 123 +++-- .../discrete/tests/testScheduler.cpp | 51 +- gtsam_unstable/discrete/tests/testSudoku.cpp | 127 ++--- 16 files changed, 1301 insertions(+), 1386 deletions(-) diff --git a/gtsam_unstable/discrete/AllDiff.cpp b/gtsam_unstable/discrete/AllDiff.cpp index 9e124954f..d6e1c6453 100644 --- a/gtsam_unstable/discrete/AllDiff.cpp +++ b/gtsam_unstable/discrete/AllDiff.cpp @@ -5,107 +5,105 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - /* ************************************************************************* */ - AllDiff::AllDiff(const DiscreteKeys& dkeys) : - Constraint(dkeys.indices()) { - for(const DiscreteKey& dkey: dkeys) - cardinalities_.insert(dkey); - } +/* ************************************************************************* */ +AllDiff::AllDiff(const DiscreteKeys& dkeys) : Constraint(dkeys.indices()) { + for (const DiscreteKey& dkey : dkeys) cardinalities_.insert(dkey); +} - /* ************************************************************************* */ - void AllDiff::print(const std::string& s, - const KeyFormatter& formatter) const { - std::cout << s << "AllDiff on "; - for (Key dkey: keys_) - std::cout << formatter(dkey) << " "; - std::cout << std::endl; - } +/* ************************************************************************* */ +void AllDiff::print(const std::string& s, const KeyFormatter& formatter) const { + std::cout << s << "AllDiff on "; + for (Key dkey : keys_) std::cout << formatter(dkey) << " "; + std::cout << std::endl; +} - /* ************************************************************************* */ - double AllDiff::operator()(const Values& values) const { - std::set < size_t > taken; // record values taken by keys - for(Key dkey: keys_) { - size_t value = values.at(dkey); // get the value for that key - if (taken.count(value)) return 0.0;// check if value alreday taken - taken.insert(value);// if not, record it as taken and keep checking +/* ************************************************************************* */ +double AllDiff::operator()(const Values& values) const { + std::set taken; // record values taken by keys + for (Key dkey : keys_) { + size_t value = values.at(dkey); // get the value for that key + if (taken.count(value)) return 0.0; // check if value alreday taken + taken.insert(value); // if not, record it as taken and keep checking + } + return 1.0; +} + +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { + // We will do this by converting the allDif into many BinaryAllDiff + // constraints + DecisionTreeFactor converted; + size_t nrKeys = keys_.size(); + for (size_t i1 = 0; i1 < nrKeys; i1++) + for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { + BinaryAllDiff binary12(discreteKey(i1), discreteKey(i2)); + converted = converted * binary12.toDecisionTreeFactor(); } - return 1.0; - } + return converted; +} - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::toDecisionTreeFactor() const { - // We will do this by converting the allDif into many BinaryAllDiff constraints - DecisionTreeFactor converted; - size_t nrKeys = keys_.size(); - for (size_t i1 = 0; i1 < nrKeys; i1++) - for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) { - BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2)); - converted = converted * binary12.toDecisionTreeFactor(); - } - return converted; - } +/* ************************************************************************* */ +DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} - /* ************************************************************************* */ - DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } +/* ************************************************************************* */ +bool AllDiff::ensureArcConsistency(size_t j, + std::vector& domains) const { + // Though strictly not part of allDiff, we check for + // a value in domains[j] that does not occur in any other connected domain. + // If found, we make this a singleton... + // TODO: make a new constraint where this really is true + Domain& Dj = domains[j]; + if (Dj.checkAllDiff(keys_, domains)) return true; - /* ************************************************************************* */ - bool AllDiff::ensureArcConsistency(size_t j, std::vector& domains) const { - // Though strictly not part of allDiff, we check for - // a value in domains[j] that does not occur in any other connected domain. - // If found, we make this a singleton... - // TODO: make a new constraint where this really is true - Domain& Dj = domains[j]; - if (Dj.checkAllDiff(keys_, domains)) return true; - - // Check all other domains for singletons and erase corresponding values - // This is the same as arc-consistency on the equivalent binary constraints - bool changed = false; - for(Key k: keys_) - if (k != j) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) { // check if singleton - size_t value = Dk.firstValue(); - if (Dj.contains(value)) { - Dj.erase(value); // erase value if true - changed = true; - } + // Check all other domains for singletons and erase corresponding values + // This is the same as arc-consistency on the equivalent binary constraints + bool changed = false; + for (Key k : keys_) + if (k != j) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) { // check if singleton + size_t value = Dk.firstValue(); + if (Dj.contains(value)) { + Dj.erase(value); // erase value if true + changed = true; } } - return changed; - } + } + return changed; +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { - DiscreteKeys newKeys; - // loop over keys and add them only if they do not appear in values - for(Key k: keys_) - if (values.find(k) == values.end()) { - newKeys.push_back(DiscreteKey(k,cardinalities_.at(k))); - } - return boost::make_shared(newKeys); - } +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const { + DiscreteKeys newKeys; + // loop over keys and add them only if they do not appear in values + for (Key k : keys_) + if (values.find(k) == values.end()) { + newKeys.push_back(DiscreteKey(k, cardinalities_.at(k))); + } + return boost::make_shared(newKeys); +} - /* ************************************************************************* */ - Constraint::shared_ptr AllDiff::partiallyApply( - const std::vector& domains) const { - DiscreteFactor::Values known; - for(Key k: keys_) { - const Domain& Dk = domains[k]; - if (Dk.isSingleton()) - known[k] = Dk.firstValue(); - } - return partiallyApply(known); +/* ************************************************************************* */ +Constraint::shared_ptr AllDiff::partiallyApply( + const std::vector& domains) const { + DiscreteFactor::Values known; + for (Key k : keys_) { + const Domain& Dk = domains[k]; + if (Dk.isSingleton()) known[k] = Dk.firstValue(); } + return partiallyApply(known); +} - /* ************************************************************************* */ -} // namespace gtsam +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 80e700b29..b0fd1d631 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -7,71 +7,71 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * General AllDiff constraint - * Returns 1 if values for all keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Key and an Key. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. +/** + * General AllDiff constraint + * Returns 1 if values for all keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Key and an Key. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ +class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { + std::map cardinalities_; + + DiscreteKey discreteKey(size_t i) const { + Key j = keys_[i]; + return DiscreteKey(j, cardinalities_.at(j)); + } + + public: + /// Constructor + AllDiff(const DiscreteKeys& dkeys); + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const AllDiff& f(static_cast(other)); + return cardinalities_.size() == f.cardinalities_.size() && + std::equal(cardinalities_.begin(), cardinalities_.end(), + f.cardinalities_.begin()); + } + } + + /// Calculate value = expensive ! + double operator()(const Values& values) const override; + + /// Convert into a decisiontree, can be *very* expensive ! + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * Arc-consistency involves creating binaryAllDiff constraints + * In which case the combinatorial hyper-arc explosion disappears. + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT AllDiff: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - std::map cardinalities_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override; - DiscreteKey discreteKey(size_t i) const { - Key j = keys_[i]; - return DiscreteKey(j,cardinalities_.at(j)); - } + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override; +}; - public: - - /// Constructor - AllDiff(const DiscreteKeys& dkeys); - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const AllDiff& f(static_cast(other)); - return cardinalities_.size() == f.cardinalities_.size() - && std::equal(cardinalities_.begin(), cardinalities_.end(), - f.cardinalities_.begin()); - } - } - - /// Calculate value = expensive ! - double operator()(const Values& values) const override; - - /// Convert into a decisiontree, can be *very* expensive ! - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * Arc-consistency involves creating binaryAllDiff constraints - * In which case the combinatorial hyper-arc explosion disappears. - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply(const std::vector&) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index bbb60e2f1..d8e1a590a 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -7,94 +7,93 @@ #pragma once -#include -#include #include +#include +#include namespace gtsam { - /** - * Binary AllDiff constraint - * Returns 1 if values for two keys are different, 0 otherwise - * DiscreteFactors are all awkward in that they have to store two types of keys: - * for each variable we have a Index and an Index. In this factor, we - * keep the Indices locally, and the Indices are stored in IndexFactor. - */ - class BinaryAllDiff: public Constraint { +/** + * Binary AllDiff constraint + * Returns 1 if values for two keys are different, 0 otherwise + * DiscreteFactors are all awkward in that they have to store two types of keys: + * for each variable we have a Index and an Index. In this factor, we + * keep the Indices locally, and the Indices are stored in IndexFactor. + */ +class BinaryAllDiff : public Constraint { + size_t cardinality0_, cardinality1_; /// cardinality - size_t cardinality0_, cardinality1_; /// cardinality + public: + /// Constructor + BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) + : Constraint(key1.first, key2.first), + cardinality0_(key1.second), + cardinality1_(key2.second) {} - public: + // print + void print( + const std::string& s = "", + const KeyFormatter& formatter = DefaultKeyFormatter) const override { + std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " + << formatter(keys_[1]) << std::endl; + } - /// Constructor - BinaryAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) : - Constraint(key1.first, key2.first), - cardinality0_(key1.second), cardinality1_(key2.second) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override { - std::cout << s << "BinaryAllDiff on " << formatter(keys_[0]) << " and " - << formatter(keys_[1]) << std::endl; - } - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const BinaryAllDiff& f(static_cast(other)); - return (cardinality0_==f.cardinality0_) && (cardinality1_==f.cardinality1_); - } - } - - /// Calculate value - double operator()(const Values& values) const override { - return (double) (values.at(keys_[0]) != values.at(keys_[1])); - } - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override { - DiscreteKeys keys; - keys.push_back(DiscreteKey(keys_[0],cardinality0_)); - keys.push_back(DiscreteKey(keys_[1],cardinality1_)); - std::vector table; - for (size_t i1 = 0; i1 < cardinality0_; i1++) - for (size_t i2 = 0; i2 < cardinality1_; i2++) - table.push_back(i1 != i2); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - /// - bool ensureArcConsistency(size_t j, std::vector& domains) const override { -// throw std::runtime_error( -// "BinaryAllDiff::ensureArcConsistency not implemented"); + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) return false; + else { + const BinaryAllDiff& f(static_cast(other)); + return (cardinality0_ == f.cardinality0_) && + (cardinality1_ == f.cardinality1_); } + } - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } + /// Calculate value + double operator()(const Values& values) const override { + return (double)(values.at(keys_[0]) != values.at(keys_[1])); + } - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector&) const override { - throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); - } - }; + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override { + DiscreteKeys keys; + keys.push_back(DiscreteKey(keys_[0], cardinality0_)); + keys.push_back(DiscreteKey(keys_[1], cardinality1_)); + std::vector table; + for (size_t i1 = 0; i1 < cardinality0_; i1++) + for (size_t i2 = 0; i2 < cardinality1_; i2++) table.push_back(i1 != i2); + DecisionTreeFactor converted(keys, table); + return converted; + } -} // namespace gtsam + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; + } + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains + */ + bool ensureArcConsistency(size_t j, + std::vector& domains) const override { + // throw std::runtime_error( + // "BinaryAllDiff::ensureArcConsistency not implemented"); + return false; + } + + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } + + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector&) const override { + throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); + } +}; + +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.cpp b/gtsam_unstable/discrete/CSP.cpp index 525abd098..b1d70dc6e 100644 --- a/gtsam_unstable/discrete/CSP.cpp +++ b/gtsam_unstable/discrete/CSP.cpp @@ -5,99 +5,104 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include using namespace std; namespace gtsam { - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::sharedValues CSP::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(); + sharedValues mpe = chordal->optimize(); + return mpe; +} - /// Find the best total assignment - can be expensive - CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { - DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); - sharedValues mpe = chordal->optimize(); - return mpe; - } +/// Find the best total assignment - can be expensive +CSP::sharedValues CSP::optimalAssignment(const Ordering& ordering) const { + DiscreteBayesNet::shared_ptr chordal = this->eliminateSequential(ordering); + sharedValues mpe = chordal->optimize(); + return mpe; +} - void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, bool print) const { - // Create VariableIndex - VariableIndex index(*this); - // index.print(); +void CSP::runArcConsistency(size_t cardinality, size_t nrIterations, + bool print) const { + // Create VariableIndex + VariableIndex index(*this); + // index.print(); - size_t n = index.size(); + size_t n = index.size(); - // Initialize domains - std::vector < Domain > domains; - for (size_t j = 0; j < n; j++) - domains.push_back(Domain(DiscreteKey(j,cardinality))); + // Initialize domains + std::vector domains; + for (size_t j = 0; j < n; j++) + domains.push_back(Domain(DiscreteKey(j, cardinality))); - // Create array of flags indicating a domain changed or not - std::vector changed(n); + // Create array of flags indicating a domain changed or not + std::vector changed(n); - // iterate nrIterations over entire grid - for (size_t it = 0; it < nrIterations; it++) { - bool anyChange = false; - // iterate over all cells - for (size_t v = 0; v < n; v++) { - // keep track of which domains changed - changed[v] = false; - // loop over all factors/constraints for variable v - const FactorIndices& factors = index[v]; - for(size_t f: factors) { - // if not already a singleton - if (!domains[v].isSingleton()) { - // get the constraint and call its ensureArcConsistency method - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast((*this)[f]); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v]; - } - } // f - if (changed[v]) anyChange = true; - } // v - if (!anyChange) break; - // TODO: Sudoku specific hack - if (print) { - if (cardinality == 9 && n == 81) { - for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { - for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { - if (changed[v]) cout << "*"; - domains[v].print(); - cout << "\t"; - } // i - cout << endl; - } // j - } else { - for (size_t v = 0; v < n; v++) { + // iterate nrIterations over entire grid + for (size_t it = 0; it < nrIterations; it++) { + bool anyChange = false; + // iterate over all cells + for (size_t v = 0; v < n; v++) { + // keep track of which domains changed + changed[v] = false; + // loop over all factors/constraints for variable v + const FactorIndices& factors = index[v]; + for (size_t f : factors) { + // if not already a singleton + if (!domains[v].isSingleton()) { + // get the constraint and call its ensureArcConsistency method + Constraint::shared_ptr constraint = + boost::dynamic_pointer_cast((*this)[f]); + if (!constraint) + throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + changed[v] = + constraint->ensureArcConsistency(v, domains) || changed[v]; + } + } // f + if (changed[v]) anyChange = true; + } // v + if (!anyChange) break; + // TODO: Sudoku specific hack + if (print) { + if (cardinality == 9 && n == 81) { + for (size_t i = 0, v = 0; i < (size_t)std::sqrt((double)n); i++) { + for (size_t j = 0; j < (size_t)std::sqrt((double)n); j++, v++) { if (changed[v]) cout << "*"; domains[v].print(); cout << "\t"; - } // v - } - cout << endl; - } // print - } // it + } // i + cout << endl; + } // j + } else { + for (size_t v = 0; v < n; v++) { + if (changed[v]) cout << "*"; + domains[v].print(); + cout << "\t"; + } // v + } + cout << endl; + } // print + } // it #ifndef INPROGRESS - // Now create new problem with all singleton variables removed - // We do this by adding simplifying all factors using parial application - // TODO: create a new ordering as we go, to ensure a connected graph - // KeyOrdering ordering; - // vector dkeys; - for(const DiscreteFactor::shared_ptr& f: factors_) { - Constraint::shared_ptr constraint = boost::dynamic_pointer_cast(f); - if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); - Constraint::shared_ptr reduced = constraint->partiallyApply(domains); - if (print) reduced->print(); - } -#endif + // Now create new problem with all singleton variables removed + // We do this by adding simplifying all factors using parial application + // TODO: create a new ordering as we go, to ensure a connected graph + // KeyOrdering ordering; + // vector dkeys; + for (const DiscreteFactor::shared_ptr& f : factors_) { + Constraint::shared_ptr constraint = + boost::dynamic_pointer_cast(f); + if (!constraint) + throw runtime_error("CSP:runArcConsistency: non-constraint factor"); + Constraint::shared_ptr reduced = constraint->partiallyApply(domains); + if (print) reduced->print(); } -} // gtsam - +#endif +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/CSP.h b/gtsam_unstable/discrete/CSP.h index 9e843f667..544cdf0c9 100644 --- a/gtsam_unstable/discrete/CSP.h +++ b/gtsam_unstable/discrete/CSP.h @@ -7,84 +7,81 @@ #pragma once +#include #include #include -#include namespace gtsam { - /** - * Constraint Satisfaction Problem class - * A specialization of a DiscreteFactorGraph. - * It knows about CSP-specific constraints and algorithms +/** + * Constraint Satisfaction Problem class + * A specialization of a DiscreteFactorGraph. + * It knows about CSP-specific constraints and algorithms + */ +class GTSAM_UNSTABLE_EXPORT CSP : public DiscreteFactorGraph { + public: + /** A map from keys to values */ + typedef KeyVector Indices; + typedef Assignment Values; + typedef boost::shared_ptr sharedValues; + + public: + // /// Constructor + // CSP() { + // } + + /// Add a unary constraint, allowing only a single value + void addSingleValue(const DiscreteKey& dkey, size_t value) { + boost::shared_ptr factor(new SingleValue(dkey, value)); + push_back(factor); + } + + /// Add a binary AllDiff constraint + void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { + boost::shared_ptr factor(new BinaryAllDiff(key1, key2)); + push_back(factor); + } + + /// Add a general AllDiff constraint + void addAllDiff(const DiscreteKeys& dkeys) { + boost::shared_ptr factor(new AllDiff(dkeys)); + push_back(factor); + } + + // /** return product of all factors as a single factor */ + // DecisionTreeFactor product() const { + // DecisionTreeFactor result; + // for(const sharedFactor& factor: *this) + // if (factor) result = (*factor) * result; + // return result; + // } + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment() const; + + /// Find the best total assignment - can be expensive + sharedValues optimalAssignment(const Ordering& ordering) const; + + // /* + // * Perform loopy belief propagation + // * True belief propagation would check for each value in domain + // * whether any satisfying separator assignment can be found. + // * This corresponds to hyper-arc consistency in CSP speak. + // * This can be done by creating a mini-factor graph and search. + // * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels + // deep. + // * It will be very expensive to exclude values that way. + // */ + // void applyBeliefPropagation(size_t nrIterations = 10) const; + + /* + * Apply arc-consistency ~ Approximate loopy belief propagation + * We need to give the domains to a constraint, and it returns + * a domain whose values don't conflict in the arc-consistency way. + * TODO: should get cardinality from Indices */ - class GTSAM_UNSTABLE_EXPORT CSP: public DiscreteFactorGraph { - public: - - /** A map from keys to values */ - typedef KeyVector Indices; - typedef Assignment Values; - typedef boost::shared_ptr sharedValues; - - public: - -// /// Constructor -// CSP() { -// } - - /// Add a unary constraint, allowing only a single value - void addSingleValue(const DiscreteKey& dkey, size_t value) { - boost::shared_ptr factor(new SingleValue(dkey, value)); - push_back(factor); - } - - /// Add a binary AllDiff constraint - void addAllDiff(const DiscreteKey& key1, const DiscreteKey& key2) { - boost::shared_ptr factor( - new BinaryAllDiff(key1, key2)); - push_back(factor); - } - - /// Add a general AllDiff constraint - void addAllDiff(const DiscreteKeys& dkeys) { - boost::shared_ptr factor(new AllDiff(dkeys)); - push_back(factor); - } - -// /** return product of all factors as a single factor */ -// DecisionTreeFactor product() const { -// DecisionTreeFactor result; -// for(const sharedFactor& factor: *this) -// if (factor) result = (*factor) * result; -// return result; -// } - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment() const; - - /// Find the best total assignment - can be expensive - sharedValues optimalAssignment(const Ordering& ordering) const; - -// /* -// * Perform loopy belief propagation -// * True belief propagation would check for each value in domain -// * whether any satisfying separator assignment can be found. -// * This corresponds to hyper-arc consistency in CSP speak. -// * This can be done by creating a mini-factor graph and search. -// * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep. -// * It will be very expensive to exclude values that way. -// */ -// void applyBeliefPropagation(size_t nrIterations = 10) const; - - /* - * Apply arc-consistency ~ Approximate loopy belief propagation - * We need to give the domains to a constraint, and it returns - * a domain whose values don't conflict in the arc-consistency way. - * TODO: should get cardinality from Indices - */ - void runArcConsistency(size_t cardinality, size_t nrIterations = 10, - bool print = false) const; - }; // CSP - -} // gtsam + void runArcConsistency(size_t cardinality, size_t nrIterations = 10, + bool print = false) const; +}; // CSP +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Constraint.h b/gtsam_unstable/discrete/Constraint.h index c3a26de68..b8baccff9 100644 --- a/gtsam_unstable/discrete/Constraint.h +++ b/gtsam_unstable/discrete/Constraint.h @@ -17,77 +17,68 @@ #pragma once -#include #include +#include + #include namespace gtsam { - class Domain; +class Domain; - /** - * Base class for discrete probabilistic factors - * The most general one is the derived DecisionTreeFactor +/** + * Base class for discrete probabilistic factors + * The most general one is the derived DecisionTreeFactor + */ +class Constraint : public DiscreteFactor { + public: + typedef boost::shared_ptr shared_ptr; + + protected: + /// Construct n-way factor + Constraint(const KeyVector& js) : DiscreteFactor(js) {} + + /// Construct unary factor + Constraint(Key j) : DiscreteFactor(boost::assign::cref_list_of<1>(j)) {} + + /// Construct binary factor + Constraint(Key j1, Key j2) + : DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) {} + + /// construct from container + template + Constraint(KeyIterator beginKey, KeyIterator endKey) + : DiscreteFactor(beginKey, endKey) {} + + public: + /// @name Standard Constructors + /// @{ + + /// Default constructor for I/O + Constraint(); + + /// Virtual destructor + ~Constraint() override {} + + /// @} + /// @name Standard Interface + /// @{ + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class Constraint : public DiscreteFactor { + virtual bool ensureArcConsistency(size_t j, + std::vector& domains) const = 0; - public: + /// Partially apply known values + virtual shared_ptr partiallyApply(const Values&) const = 0; - typedef boost::shared_ptr shared_ptr; - - protected: - - /// Construct n-way factor - Constraint(const KeyVector& js) : - DiscreteFactor(js) { - } - - /// Construct unary factor - Constraint(Key j) : - DiscreteFactor(boost::assign::cref_list_of<1>(j)) { - } - - /// Construct binary factor - Constraint(Key j1, Key j2) : - DiscreteFactor(boost::assign::cref_list_of<2>(j1)(j2)) { - } - - /// construct from container - template - Constraint(KeyIterator beginKey, KeyIterator endKey) : - DiscreteFactor(beginKey, endKey) { - } - - public: - - /// @name Standard Constructors - /// @{ - - /// Default constructor for I/O - Constraint(); - - /// Virtual destructor - ~Constraint() override {} - - /// @} - /// @name Standard Interface - /// @{ - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - virtual bool ensureArcConsistency(size_t j, std::vector& domains) const = 0; - - /// Partially apply known values - virtual shared_ptr partiallyApply(const Values&) const = 0; - - - /// Partially apply known values, domain version - virtual shared_ptr partiallyApply(const std::vector&) const = 0; - /// @} - }; + /// Partially apply known values, domain version + virtual shared_ptr partiallyApply(const std::vector&) const = 0; + /// @} +}; // DiscreteFactor -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.cpp b/gtsam_unstable/discrete/Domain.cpp index 740ef067c..a81b1d1ad 100644 --- a/gtsam_unstable/discrete/Domain.cpp +++ b/gtsam_unstable/discrete/Domain.cpp @@ -5,92 +5,89 @@ * @author Frank Dellaert */ -#include -#include #include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void Domain::print(const string& s, - const KeyFormatter& formatter) const { -// cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << -// formatter(keys_[0]) << ") with values"; -// for (size_t v: values_) cout << " " << v; -// cout << endl; - for (size_t v: values_) cout << v; - } - - /* ************************************************************************* */ - double Domain::operator()(const Values& values) const { - return contains(values.at(keys_[0])); - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; ++i1) - table.push_back(contains(i1)); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool Domain::ensureArcConsistency(size_t j, vector& domains) const { - if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); - Domain& D = domains[j]; - for(size_t value: values_) - if (!D.contains(value)) throw runtime_error("Unsatisfiable"); - D = *this; - return true; - } - - /* ************************************************************************* */ - bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { - Key j = keys_[0]; - // for all values in this domain - for(size_t value: values_) { - // for all connected domains - for(Key k: keys) - // if any domain contains the value we cannot make this domain singleton - if (k!=j && domains[k].contains(value)) - goto found; - values_.clear(); - values_.insert(value); - return true; // we changed it - found:; - } - return false; // we did not change it - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && !contains(it->second)) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (*this); - } - - /* ************************************************************************* */ - Constraint::shared_ptr Domain::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !contains(*Dk.begin())) throw runtime_error( - "Domain::partiallyApply: unsatisfiable"); - return boost::make_shared < Domain > (Dk); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void Domain::print(const string& s, const KeyFormatter& formatter) const { + // cout << s << ": Domain on " << formatter(keys_[0]) << " (j=" << + // formatter(keys_[0]) << ") with values"; + // for (size_t v: values_) cout << " " << v; + // cout << endl; + for (size_t v : values_) cout << v; +} + +/* ************************************************************************* */ +double Domain::operator()(const Values& values) const { + return contains(values.at(keys_[0])); +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; ++i1) table.push_back(contains(i1)); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor Domain::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool Domain::ensureArcConsistency(size_t j, vector& domains) const { + if (j != keys_[0]) throw invalid_argument("Domain check on wrong domain"); + Domain& D = domains[j]; + for (size_t value : values_) + if (!D.contains(value)) throw runtime_error("Unsatisfiable"); + D = *this; + return true; +} + +/* ************************************************************************* */ +bool Domain::checkAllDiff(const KeyVector keys, vector& domains) { + Key j = keys_[0]; + // for all values in this domain + for (size_t value : values_) { + // for all connected domains + for (Key k : keys) + // if any domain contains the value we cannot make this domain singleton + if (k != j && domains[k].contains(value)) goto found; + values_.clear(); + values_.insert(value); + return true; // we changed it + found:; + } + return false; // we did not change it +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && !contains(it->second)) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(*this); +} + +/* ************************************************************************* */ +Constraint::shared_ptr Domain::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !contains(*Dk.begin())) + throw runtime_error("Domain::partiallyApply: unsatisfiable"); + return boost::make_shared(Dk); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 5acc5a08f..15828b653 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -7,111 +7,97 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * Domain restriction constraint +/** + * Domain restriction constraint + */ +class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { + size_t cardinality_; /// Cardinality + std::set values_; /// allowed values + + public: + typedef boost::shared_ptr shared_ptr; + + // Constructor on Discrete Key initializes an "all-allowed" domain + Domain(const DiscreteKey& dkey) + : Constraint(dkey.first), cardinality_(dkey.second) { + for (size_t v = 0; v < cardinality_; v++) values_.insert(v); + } + + // Constructor on Discrete Key with single allowed value + // Consider SingleValue constraint + Domain(const DiscreteKey& dkey, size_t v) + : Constraint(dkey.first), cardinality_(dkey.second) { + values_.insert(v); + } + + /// Constructor + Domain(const Domain& other) + : Constraint(other.keys_[0]), values_(other.values_) {} + + /// insert a value, non const :-( + void insert(size_t value) { values_.insert(value); } + + /// erase a value, non const :-( + void erase(size_t value) { values_.erase(value); } + + size_t nrValues() const { return values_.size(); } + + bool isSingleton() const { return nrValues() == 1; } + + size_t firstValue() const { return *values_.begin(); } + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const Domain& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (values_ == f.values_); + } + } + + bool contains(size_t value) const { return values_.count(value) > 0; } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT Domain: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - size_t cardinality_; /// Cardinality - std::set values_; /// allowed values + /** + * Check for a value in domain that does not occur in any other connected + * domain. If found, we make this a singleton... Called in + * AllDiff::ensureArcConsistency + * @param keys connected domains through alldiff + */ + bool checkAllDiff(const KeyVector keys, std::vector& domains); - public: + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - typedef boost::shared_ptr shared_ptr; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; +}; - // Constructor on Discrete Key initializes an "all-allowed" domain - Domain(const DiscreteKey& dkey) : - Constraint(dkey.first), cardinality_(dkey.second) { - for (size_t v = 0; v < cardinality_; v++) - values_.insert(v); - } - - // Constructor on Discrete Key with single allowed value - // Consider SingleValue constraint - Domain(const DiscreteKey& dkey, size_t v) : - Constraint(dkey.first), cardinality_(dkey.second) { - values_.insert(v); - } - - /// Constructor - Domain(const Domain& other) : - Constraint(other.keys_[0]), values_(other.values_) { - } - - /// insert a value, non const :-( - void insert(size_t value) { - values_.insert(value); - } - - /// erase a value, non const :-( - void erase(size_t value) { - values_.erase(value); - } - - size_t nrValues() const { - return values_.size(); - } - - bool isSingleton() const { - return nrValues() == 1; - } - - size_t firstValue() const { - return *values_.begin(); - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const Domain& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (values_==f.values_); - } - } - - bool contains(size_t value) const { - return values_.count(value)>0; - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /** - * Check for a value in domain that does not occur in any other connected domain. - * If found, we make this a singleton... Called in AllDiff::ensureArcConsistency - * @param keys connected domains through alldiff - */ - bool checkAllDiff(const KeyVector keys, std::vector& domains); - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.cpp b/gtsam_unstable/discrete/Scheduler.cpp index 3273778c4..415f92e62 100644 --- a/gtsam_unstable/discrete/Scheduler.cpp +++ b/gtsam_unstable/discrete/Scheduler.cpp @@ -5,298 +5,286 @@ * @author Frank Dellaert */ -#include -#include #include #include +#include +#include #include - +#include #include #include -#include namespace gtsam { - using namespace std; +using namespace std; - Scheduler::Scheduler(size_t maxNrStudents, const string& filename): - maxNrStudents_(maxNrStudents) - { - typedef boost::tokenizer > Tokenizer; +Scheduler::Scheduler(size_t maxNrStudents, const string& filename) + : maxNrStudents_(maxNrStudents) { + typedef boost::tokenizer > Tokenizer; - // open file - ifstream is(filename.c_str()); - if (!is) { - cerr << "Scheduler: could not open file " << filename << endl; - throw runtime_error("Scheduler: could not open file " + filename); - } - - string line; // buffer - - // process first line with faculty - if (getline(is, line, '\r')) { - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - for (++it; it != tok.end(); ++it) - addFaculty(*it); - } - - // for all remaining lines - size_t count = 0; - while (getline(is, line, '\r')) { - if (count++ > 100) throw runtime_error("reached 100 lines, exiting"); - Tokenizer tok(line); - Tokenizer::iterator it = tok.begin(); - addSlot(*it++); // add slot - // add availability - for (; it != tok.end(); ++it) - available_ += (it->empty()) ? "0 " : "1 "; - available_ += '\n'; - } - } // constructor - - /** addStudent has to be called after adding slots and faculty */ - void Scheduler::addStudent(const string& studentName, - const string& area1, const string& area2, - const string& area3, const string& advisor) { - assert(nrStudents() area) const { - return area ? students_[s].keys_[*area] : students_[s].key_; + // open file + ifstream is(filename.c_str()); + if (!is) { + cerr << "Scheduler: could not open file " << filename << endl; + throw runtime_error("Scheduler: could not open file " + filename); } - const string& Scheduler::studentName(size_t i) const { - assert(i 100) throw runtime_error("reached 100 lines, exiting"); + Tokenizer tok(line); + Tokenizer::iterator it = tok.begin(); + addSlot(*it++); // add slot + // add availability + for (; it != tok.end(); ++it) available_ += (it->empty()) ? "0 " : "1 "; + available_ += '\n'; + } +} // constructor + +/** addStudent has to be called after adding slots and faculty */ +void Scheduler::addStudent(const string& studentName, const string& area1, + const string& area2, const string& area3, + const string& advisor) { + assert(nrStudents() < maxNrStudents_); + assert(facultyInArea_.count(area1)); + assert(facultyInArea_.count(area2)); + assert(facultyInArea_.count(area3)); + size_t advisorIndex = facultyIndex_[advisor]; + Student student(nrFaculty(), advisorIndex); + student.name_ = studentName; + // We fix the ordering by assigning a higher index to the student + // and numbering the areas lower + Key j = 3 * maxNrStudents_ + nrStudents(); + student.key_ = DiscreteKey(j, nrTimeSlots()); + Key base = 3 * nrStudents(); + student.keys_[0] = DiscreteKey(base + 0, nrFaculty()); + student.keys_[1] = DiscreteKey(base + 1, nrFaculty()); + student.keys_[2] = DiscreteKey(base + 2, nrFaculty()); + student.areaName_[0] = area1; + student.areaName_[1] = area2; + student.areaName_[2] = area3; + students_.push_back(student); +} + +/** get key for student and area, 0 is time slot itself */ +const DiscreteKey& Scheduler::key(size_t s, + boost::optional area) const { + return area ? students_[s].keys_[*area] : students_[s].key_; +} + +const string& Scheduler::studentName(size_t i) const { + assert(i < nrStudents()); + return students_[i].name_; +} + +const DiscreteKey& Scheduler::studentKey(size_t i) const { + assert(i < nrStudents()); + return students_[i].key_; +} + +const string& Scheduler::studentArea(size_t i, size_t area) const { + assert(i < nrStudents()); + return students_[i].areaName_[area]; +} + +/** Add student-specific constraints to the graph */ +void Scheduler::addStudentSpecificConstraints(size_t i, + boost::optional slot) { + bool debug = ISDEBUG("Scheduler::buildGraph"); + + assert(i < nrStudents()); + const Student& s = students_[i]; + + if (!slot && !slotsAvailable_.empty()) { + if (debug) cout << "Adding availability of slots" << endl; + assert(slotsAvailable_.size() == s.key_.second); + CSP::add(s.key_, slotsAvailable_); } - const string& Scheduler::studentArea(size_t i, size_t area) const { - assert(i slot) { - bool debug = ISDEBUG("Scheduler::buildGraph"); + if (debug) cout << "Area constraints " << areaName << endl; + assert(facultyInArea_[areaName].size() == areaKey.second); + CSP::add(areaKey, facultyInArea_[areaName]); - assert(iat(j); - cout << studentName(s) << " slot: " << slotName_[slot] << endl; - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t faculty = assignment->at(base+area); - cout << setw(12) << studentArea(s,area) << ": " << facultyName_[faculty] - << endl; - } - cout << endl; - } } +} // buildGraph - /** Special print for single-student case */ - void Scheduler::printSpecial(sharedValues assignment) const { - Values::const_iterator it = assignment->begin(); - for (size_t area = 0; area < 3; area++, it++) { - size_t f = it->second; - cout << setw(12) << studentArea(0,area) << ": " << facultyName_[f] << endl; +/** print */ +void Scheduler::print(const string& s, const KeyFormatter& formatter) const { + cout << s << " Faculty:" << endl; + for (const string& name : facultyName_) cout << name << '\n'; + cout << endl; + + cout << s << " Slots:\n"; + size_t i = 0; + for (const string& name : slotName_) cout << i++ << " " << name << endl; + cout << endl; + + cout << "Availability:\n" << available_ << '\n'; + + cout << s << " Area constraints:\n"; + for (const FacultyInArea::value_type& it : facultyInArea_) { + cout << setw(12) << it.first << ": "; + for (double v : it.second) cout << v << " "; + cout << '\n'; + } + cout << endl; + + cout << s << " Students:\n"; + for (const Student& student : students_) student.print(); + cout << endl; + + CSP::print(s + " Factor graph"); + cout << endl; +} // print + +/** Print readable form of assignment */ +void Scheduler::printAssignment(sharedValues assignment) const { + // Not intended to be general! Assumes very particular ordering ! + cout << endl; + for (size_t s = 0; s < nrStudents(); s++) { + Key j = 3 * maxNrStudents_ + s; + size_t slot = assignment->at(j); + cout << studentName(s) << " slot: " << slotName_[slot] << endl; + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t faculty = assignment->at(base + area); + cout << setw(12) << studentArea(s, area) << ": " << facultyName_[faculty] + << endl; } cout << endl; } +} - /** Accumulate faculty stats */ - void Scheduler::accumulateStats(sharedValues assignment, vector< - size_t>& stats) const { - for (size_t s = 0; s < nrStudents(); s++) { - Key base = 3*s; - for (size_t area = 0; area < 3; area++) { - size_t f = assignment->at(base+area); - assert(fbegin(); + for (size_t area = 0; area < 3; area++, it++) { + size_t f = it->second; + cout << setw(12) << studentArea(0, area) << ": " << facultyName_[f] << endl; + } + cout << endl; +} + +/** Accumulate faculty stats */ +void Scheduler::accumulateStats(sharedValues assignment, + vector& stats) const { + for (size_t s = 0; s < nrStudents(); s++) { + Key base = 3 * s; + for (size_t area = 0; area < 3; area++) { + size_t f = assignment->at(base + area); + assert(f < stats.size()); + stats[f]++; + } // area + } // s +} + +/** Eliminate, return a Bayes net */ +DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { + gttic(my_eliminate); + // TODO: fix this!! + size_t maxKey = keys().size(); + Ordering defaultKeyOrdering; + for (size_t i = 0; i < maxKey; ++i) defaultKeyOrdering += Key(i); + DiscreteBayesNet::shared_ptr chordal = + this->eliminateSequential(defaultKeyOrdering); + gttoc(my_eliminate); + return chordal; +} + +/** Find the best total assignment - can be expensive */ +Scheduler::sharedValues Scheduler::optimalAssignment() const { + DiscreteBayesNet::shared_ptr chordal = eliminate(); + + if (ISDEBUG("Scheduler::optimalAssignment")) { + DiscreteBayesNet::const_iterator it = chordal->end() - 1; + const Student& student = students_.front(); + cout << endl; + (*it)->print(student.name_); } - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr Scheduler::eliminate() const { - gttic(my_eliminate); - // TODO: fix this!! - size_t maxKey = keys().size(); - Ordering defaultKeyOrdering; - for (size_t i = 0; ieliminateSequential(defaultKeyOrdering); - gttoc(my_eliminate); - return chordal; - } + gttic(my_optimize); + sharedValues mpe = chordal->optimize(); + gttoc(my_optimize); + return mpe; +} - /** Find the best total assignment - can be expensive */ - Scheduler::sharedValues Scheduler::optimalAssignment() const { - DiscreteBayesNet::shared_ptr chordal = eliminate(); - - if (ISDEBUG("Scheduler::optimalAssignment")) { - DiscreteBayesNet::const_iterator it = chordal->end()-1; - const Student & student = students_.front(); - cout << endl; - (*it)->print(student.name_); - } - - gttic(my_optimize); - sharedValues mpe = chordal->optimize(); - gttoc(my_optimize); - return mpe; - } - - /** find the assignment of students to slots with most possible committees */ - Scheduler::sharedValues Scheduler::bestSchedule() const { - sharedValues best; - throw runtime_error("bestSchedule not implemented"); - return best; - } - - /** find the corresponding most desirable committee assignment */ - Scheduler::sharedValues Scheduler::bestAssignment( - sharedValues bestSchedule) const { - sharedValues best; - throw runtime_error("bestAssignment not implemented"); - return best; - } - -} // gtsam +/** find the assignment of students to slots with most possible committees */ +Scheduler::sharedValues Scheduler::bestSchedule() const { + sharedValues best; + throw runtime_error("bestSchedule not implemented"); + return best; +} +/** find the corresponding most desirable committee assignment */ +Scheduler::sharedValues Scheduler::bestAssignment( + sharedValues bestSchedule) const { + sharedValues best; + throw runtime_error("bestAssignment not implemented"); + return best; +} +} // namespace gtsam diff --git a/gtsam_unstable/discrete/Scheduler.h b/gtsam_unstable/discrete/Scheduler.h index 6faf9956f..faf131f5c 100644 --- a/gtsam_unstable/discrete/Scheduler.h +++ b/gtsam_unstable/discrete/Scheduler.h @@ -11,165 +11,150 @@ namespace gtsam { +/** + * Scheduler class + * Creates one variable for each student, and three variables for each + * of the student's areas, for a total of 4*nrStudents variables. + * The "student" variable will determine when the student takes the qual. + * The "area" variables determine which faculty are on his/her committee. + */ +class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + private: + /** Internal data structure for students */ + struct Student { + std::string name_; + DiscreteKey key_; // key for student + std::vector keys_; // key for areas + std::vector areaName_; + std::vector advisor_; + Student(size_t nrFaculty, size_t advisorIndex) + : keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { + advisor_[advisorIndex] = 0.0; + } + void print() const { + using std::cout; + cout << name_ << ": "; + for (size_t area = 0; area < 3; area++) cout << areaName_[area] << " "; + cout << std::endl; + } + }; + + /** Maximum number of students */ + size_t maxNrStudents_; + + /** discrete keys, indexed by student and area index */ + std::vector students_; + + /** faculty identifiers */ + std::map facultyIndex_; + std::vector facultyName_, slotName_, areaName_; + + /** area constraints */ + typedef std::map > FacultyInArea; + FacultyInArea facultyInArea_; + + /** nrTimeSlots * nrFaculty availability constraints */ + std::string available_; + + /** which slots are good */ + std::vector slotsAvailable_; + + public: /** - * Scheduler class - * Creates one variable for each student, and three variables for each - * of the student's areas, for a total of 4*nrStudents variables. - * The "student" variable will determine when the student takes the qual. - * The "area" variables determine which faculty are on his/her committee. + * Constructor + * We need to know the number of students in advance for ordering keys. + * then add faculty, slots, areas, availability, students, in that order */ - class GTSAM_UNSTABLE_EXPORT Scheduler : public CSP { + Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} - private: + /// Destructor + virtual ~Scheduler() {} - /** Internal data structure for students */ - struct Student { - std::string name_; - DiscreteKey key_; // key for student - std::vector keys_; // key for areas - std::vector areaName_; - std::vector advisor_; - Student(size_t nrFaculty, size_t advisorIndex) : - keys_(3), areaName_(3), advisor_(nrFaculty, 1.0) { - advisor_[advisorIndex] = 0.0; - } - void print() const { - using std::cout; - cout << name_ << ": "; - for (size_t area = 0; area < 3; area++) - cout << areaName_[area] << " "; - cout << std::endl; - } - }; + void addFaculty(const std::string& facultyName) { + facultyIndex_[facultyName] = nrFaculty(); + facultyName_.push_back(facultyName); + } - /** Maximum number of students */ - size_t maxNrStudents_; + size_t nrFaculty() const { return facultyName_.size(); } - /** discrete keys, indexed by student and area index */ - std::vector students_; + /** boolean std::string of nrTimeSlots * nrFaculty */ + void setAvailability(const std::string& available) { available_ = available; } - /** faculty identifiers */ - std::map facultyIndex_; - std::vector facultyName_, slotName_, areaName_; + void addSlot(const std::string& slotName) { slotName_.push_back(slotName); } - /** area constraints */ - typedef std::map > FacultyInArea; - FacultyInArea facultyInArea_; + size_t nrTimeSlots() const { return slotName_.size(); } - /** nrTimeSlots * nrFaculty availability constraints */ - std::string available_; + const std::string& slotName(size_t s) const { return slotName_[s]; } - /** which slots are good */ - std::vector slotsAvailable_; + /** slots available, boolean */ + void setSlotsAvailable(const std::vector& slotsAvailable) { + slotsAvailable_ = slotsAvailable; + } - public: + void addArea(const std::string& facultyName, const std::string& areaName) { + areaName_.push_back(areaName); + std::vector& table = + facultyInArea_[areaName]; // will create if needed + if (table.empty()) table.resize(nrFaculty(), 0); + table[facultyIndex_[facultyName]] = 1; + } - /** - * Constructor - * We need to know the number of students in advance for ordering keys. - * then add faculty, slots, areas, availability, students, in that order - */ - Scheduler(size_t maxNrStudents) : maxNrStudents_(maxNrStudents) {} + /** + * Constructor that reads in faculty, slots, availibility. + * Still need to add areas and students after this + */ + Scheduler(size_t maxNrStudents, const std::string& filename); - /// Destructor - virtual ~Scheduler() {} + /** get key for student and area, 0 is time slot itself */ + const DiscreteKey& key(size_t s, + boost::optional area = boost::none) const; - void addFaculty(const std::string& facultyName) { - facultyIndex_[facultyName] = nrFaculty(); - facultyName_.push_back(facultyName); - } + /** addStudent has to be called after adding slots and faculty */ + void addStudent(const std::string& studentName, const std::string& area1, + const std::string& area2, const std::string& area3, + const std::string& advisor); - size_t nrFaculty() const { - return facultyName_.size(); - } + /// current number of students + size_t nrStudents() const { return students_.size(); } - /** boolean std::string of nrTimeSlots * nrFaculty */ - void setAvailability(const std::string& available) { - available_ = available; - } + const std::string& studentName(size_t i) const; + const DiscreteKey& studentKey(size_t i) const; + const std::string& studentArea(size_t i, size_t area) const; - void addSlot(const std::string& slotName) { - slotName_.push_back(slotName); - } + /** Add student-specific constraints to the graph */ + void addStudentSpecificConstraints( + size_t i, boost::optional slot = boost::none); - size_t nrTimeSlots() const { - return slotName_.size(); - } + /** Main routine that builds factor graph */ + void buildGraph(size_t mutexBound = 7); - const std::string& slotName(size_t s) const { - return slotName_[s]; - } + /** print */ + void print( + const std::string& s = "Scheduler", + const KeyFormatter& formatter = DefaultKeyFormatter) const override; - /** slots available, boolean */ - void setSlotsAvailable(const std::vector& slotsAvailable) { - slotsAvailable_ = slotsAvailable; - } + /** Print readable form of assignment */ + void printAssignment(sharedValues assignment) const; - void addArea(const std::string& facultyName, const std::string& areaName) { - areaName_.push_back(areaName); - std::vector& table = facultyInArea_[areaName]; // will create if needed - if (table.empty()) table.resize(nrFaculty(), 0); - table[facultyIndex_[facultyName]] = 1; - } + /** Special print for single-student case */ + void printSpecial(sharedValues assignment) const; - /** - * Constructor that reads in faculty, slots, availibility. - * Still need to add areas and students after this - */ - Scheduler(size_t maxNrStudents, const std::string& filename); + /** Accumulate faculty stats */ + void accumulateStats(sharedValues assignment, + std::vector& stats) const; - /** get key for student and area, 0 is time slot itself */ - const DiscreteKey& key(size_t s, boost::optional area = boost::none) const; + /** Eliminate, return a Bayes net */ + DiscreteBayesNet::shared_ptr eliminate() const; - /** addStudent has to be called after adding slots and faculty */ - void addStudent(const std::string& studentName, const std::string& area1, - const std::string& area2, const std::string& area3, - const std::string& advisor); + /** Find the best total assignment - can be expensive */ + sharedValues optimalAssignment() const; - /// current number of students - size_t nrStudents() const { - return students_.size(); - } + /** find the assignment of students to slots with most possible committees */ + sharedValues bestSchedule() const; - const std::string& studentName(size_t i) const; - const DiscreteKey& studentKey(size_t i) const; - const std::string& studentArea(size_t i, size_t area) const; - - /** Add student-specific constraints to the graph */ - void addStudentSpecificConstraints(size_t i, boost::optional slot = boost::none); - - /** Main routine that builds factor graph */ - void buildGraph(size_t mutexBound = 7); - - /** print */ - void print( - const std::string& s = "Scheduler", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /** Print readable form of assignment */ - void printAssignment(sharedValues assignment) const; - - /** Special print for single-student case */ - void printSpecial(sharedValues assignment) const; - - /** Accumulate faculty stats */ - void accumulateStats(sharedValues assignment, - std::vector& stats) const; - - /** Eliminate, return a Bayes net */ - DiscreteBayesNet::shared_ptr eliminate() const; - - /** Find the best total assignment - can be expensive */ - sharedValues optimalAssignment() const; - - /** find the assignment of students to slots with most possible committees */ - sharedValues bestSchedule() const; - - /** find the corresponding most desirable committee assignment */ - sharedValues bestAssignment(sharedValues bestSchedule) const; - - }; // Scheduler - -} // gtsam + /** find the corresponding most desirable committee assignment */ + sharedValues bestAssignment(sharedValues bestSchedule) const; +}; // Scheduler +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.cpp b/gtsam_unstable/discrete/SingleValue.cpp index 6324f14cd..105887dc9 100644 --- a/gtsam_unstable/discrete/SingleValue.cpp +++ b/gtsam_unstable/discrete/SingleValue.cpp @@ -5,75 +5,74 @@ * @author Frank Dellaert */ -#include -#include -#include #include +#include +#include +#include + #include namespace gtsam { - using namespace std; - - /* ************************************************************************* */ - void SingleValue::print(const string& s, - const KeyFormatter& formatter) const { - cout << s << "SingleValue on " << "j=" << formatter(keys_[0]) - << " with value " << value_ << endl; - } - - /* ************************************************************************* */ - double SingleValue::operator()(const Values& values) const { - return (double) (values.at(keys_[0]) == value_); - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { - DiscreteKeys keys; - keys += DiscreteKey(keys_[0],cardinality_); - vector table; - for (size_t i1 = 0; i1 < cardinality_; i1++) - table.push_back(i1 == value_); - DecisionTreeFactor converted(keys, table); - return converted; - } - - /* ************************************************************************* */ - DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { - // TODO: can we do this more efficiently? - return toDecisionTreeFactor() * f; - } - - /* ************************************************************************* */ - bool SingleValue::ensureArcConsistency(size_t j, - vector& domains) const { - if (j != keys_[0]) throw invalid_argument( - "SingleValue check on wrong domain"); - Domain& D = domains[j]; - if (D.isSingleton()) { - if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); - return false; - } - D = Domain(discreteKey(),value_); - return true; - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { - Values::const_iterator it = values.find(keys_[0]); - if (it != values.end() && it->second != value_) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (keys_[0], cardinality_, value_); - } - - /* ************************************************************************* */ - Constraint::shared_ptr SingleValue::partiallyApply( - const vector& domains) const { - const Domain& Dk = domains[keys_[0]]; - if (Dk.isSingleton() && !Dk.contains(value_)) throw runtime_error( - "SingleValue::partiallyApply: unsatisfiable"); - return boost::make_shared < SingleValue > (discreteKey(), value_); - } +using namespace std; /* ************************************************************************* */ -} // namespace gtsam +void SingleValue::print(const string& s, const KeyFormatter& formatter) const { + cout << s << "SingleValue on " + << "j=" << formatter(keys_[0]) << " with value " << value_ << endl; +} + +/* ************************************************************************* */ +double SingleValue::operator()(const Values& values) const { + return (double)(values.at(keys_[0]) == value_); +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::toDecisionTreeFactor() const { + DiscreteKeys keys; + keys += DiscreteKey(keys_[0], cardinality_); + vector table; + for (size_t i1 = 0; i1 < cardinality_; i1++) table.push_back(i1 == value_); + DecisionTreeFactor converted(keys, table); + return converted; +} + +/* ************************************************************************* */ +DecisionTreeFactor SingleValue::operator*(const DecisionTreeFactor& f) const { + // TODO: can we do this more efficiently? + return toDecisionTreeFactor() * f; +} + +/* ************************************************************************* */ +bool SingleValue::ensureArcConsistency(size_t j, + vector& domains) const { + if (j != keys_[0]) + throw invalid_argument("SingleValue check on wrong domain"); + Domain& D = domains[j]; + if (D.isSingleton()) { + if (D.firstValue() != value_) throw runtime_error("Unsatisfiable"); + return false; + } + D = Domain(discreteKey(), value_); + return true; +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply(const Values& values) const { + Values::const_iterator it = values.find(keys_[0]); + if (it != values.end() && it->second != value_) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(keys_[0], cardinality_, value_); +} + +/* ************************************************************************* */ +Constraint::shared_ptr SingleValue::partiallyApply( + const vector& domains) const { + const Domain& Dk = domains[keys_[0]]; + if (Dk.isSingleton() && !Dk.contains(value_)) + throw runtime_error("SingleValue::partiallyApply: unsatisfiable"); + return boost::make_shared(discreteKey(), value_); +} + +/* ************************************************************************* */ +} // namespace gtsam diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index c4d2addec..a2aec338c 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -7,76 +7,73 @@ #pragma once -#include #include +#include namespace gtsam { - /** - * SingleValue constraint +/** + * SingleValue constraint + */ +class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { + /// Number of values + size_t cardinality_; + + /// allowed value + size_t value_; + + DiscreteKey discreteKey() const { + return DiscreteKey(keys_[0], cardinality_); + } + + public: + typedef boost::shared_ptr shared_ptr; + + /// Constructor + SingleValue(Key key, size_t n, size_t value) + : Constraint(key), cardinality_(n), value_(value) {} + + /// Constructor + SingleValue(const DiscreteKey& dkey, size_t value) + : Constraint(dkey.first), cardinality_(dkey.second), value_(value) {} + + // print + void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const DiscreteFactor& other, double tol) const override { + if (!dynamic_cast(&other)) + return false; + else { + const SingleValue& f(static_cast(other)); + return (cardinality_ == f.cardinality_) && (value_ == f.value_); + } + } + + /// Calculate value + double operator()(const Values& values) const override; + + /// Convert into a decisiontree + DecisionTreeFactor toDecisionTreeFactor() const override; + + /// Multiply into a decisiontree + DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + + /* + * Ensure Arc-consistency + * @param j domain to be checked + * @param domains all other domains */ - class GTSAM_UNSTABLE_EXPORT SingleValue: public Constraint { + bool ensureArcConsistency(size_t j, + std::vector& domains) const override; - /// Number of values - size_t cardinality_; + /// Partially apply known values + Constraint::shared_ptr partiallyApply(const Values& values) const override; - /// allowed value - size_t value_; + /// Partially apply known values, domain version + Constraint::shared_ptr partiallyApply( + const std::vector& domains) const override; +}; - DiscreteKey discreteKey() const { - return DiscreteKey(keys_[0],cardinality_); - } - - public: - - typedef boost::shared_ptr shared_ptr; - - /// Constructor - SingleValue(Key key, size_t n, size_t value) : - Constraint(key), cardinality_(n), value_(value) { - } - - /// Constructor - SingleValue(const DiscreteKey& dkey, size_t value) : - Constraint(dkey.first), cardinality_(dkey.second), value_(value) { - } - - // print - void print(const std::string& s = "", - const KeyFormatter& formatter = DefaultKeyFormatter) const override; - - /// equals - bool equals(const DiscreteFactor& other, double tol) const override { - if(!dynamic_cast(&other)) - return false; - else { - const SingleValue& f(static_cast(other)); - return (cardinality_==f.cardinality_) && (value_==f.value_); - } - } - - /// Calculate value - double operator()(const Values& values) const override; - - /// Convert into a decisiontree - DecisionTreeFactor toDecisionTreeFactor() const override; - - /// Multiply into a decisiontree - DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; - - /* - * Ensure Arc-consistency - * @param j domain to be checked - * @param domains all other domains - */ - bool ensureArcConsistency(size_t j, std::vector& domains) const override; - - /// Partially apply known values - Constraint::shared_ptr partiallyApply(const Values& values) const override; - - /// Partially apply known values, domain version - Constraint::shared_ptr partiallyApply( - const std::vector& domains) const override; - }; - -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/discrete/tests/testCSP.cpp b/gtsam_unstable/discrete/tests/testCSP.cpp index 3dd493b1b..1552fcbf1 100644 --- a/gtsam_unstable/discrete/tests/testCSP.cpp +++ b/gtsam_unstable/discrete/tests/testCSP.cpp @@ -7,49 +7,50 @@ #include #include + #include using boost::assign::insert; #include -#include + #include +#include using namespace std; using namespace gtsam; /* ************************************************************************* */ -TEST_UNSAFE( BinaryAllDif, allInOne) -{ +TEST_UNSAFE(BinaryAllDif, allInOne) { // Create keys and ordering size_t nrColors = 2; -// DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", nrColors); + // DiscreteKey ID("Idaho", nrColors), UT("Utah", nrColors), AZ("Arizona", + // nrColors); DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); // Check construction and conversion BinaryAllDiff c1(ID, UT); DecisionTreeFactor f1(ID & UT, "0 1 1 0"); - EXPECT(assert_equal(f1,c1.toDecisionTreeFactor())); + EXPECT(assert_equal(f1, c1.toDecisionTreeFactor())); // Check construction and conversion BinaryAllDiff c2(UT, AZ); DecisionTreeFactor f2(UT & AZ, "0 1 1 0"); - EXPECT(assert_equal(f2,c2.toDecisionTreeFactor())); + EXPECT(assert_equal(f2, c2.toDecisionTreeFactor())); - DecisionTreeFactor f3 = f1*f2; - EXPECT(assert_equal(f3,c1*f2)); - EXPECT(assert_equal(f3,c2*f1)); + DecisionTreeFactor f3 = f1 * f2; + EXPECT(assert_equal(f3, c1 * f2)); + EXPECT(assert_equal(f3, c2 * f1)); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, allInOne) -{ +TEST_UNSAFE(CSP, allInOne) { // Create keys and ordering size_t nrColors = 2; DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(ID,UT); - csp.addAllDiff(UT,AZ); + csp.addAllDiff(ID, UT); + csp.addAllDiff(UT, AZ); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -69,67 +70,67 @@ TEST_UNSAFE( CSP, allInOne) DecisionTreeFactor product = csp.product(); // product.dot("product"); DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); - EXPECT(assert_equal(expectedProduct,product)); + EXPECT(assert_equal(expectedProduct, product)); // Solve CSP::sharedValues mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); } /* ************************************************************************* */ -TEST_UNSAFE( CSP, WesternUS) -{ +TEST_UNSAFE(CSP, WesternUS) { // Create keys size_t nrColors = 4; DiscreteKey - // Create ordering according to example in ND-CSP.lyx - WA(0, nrColors), OR(3, nrColors), CA(1, nrColors),NV(2, nrColors), - ID(8, nrColors), UT(9, nrColors), AZ(10, nrColors), - MT(4, nrColors), WY(5, nrColors), CO(7, nrColors), NM(6, nrColors); + // Create ordering according to example in ND-CSP.lyx + WA(0, nrColors), + OR(3, nrColors), CA(1, nrColors), NV(2, nrColors), ID(8, nrColors), + UT(9, nrColors), AZ(10, nrColors), MT(4, nrColors), WY(5, nrColors), + CO(7, nrColors), NM(6, nrColors); // Create the CSP CSP csp; - csp.addAllDiff(WA,ID); - csp.addAllDiff(WA,OR); - csp.addAllDiff(OR,ID); - csp.addAllDiff(OR,CA); - csp.addAllDiff(OR,NV); - csp.addAllDiff(CA,NV); - csp.addAllDiff(CA,AZ); - csp.addAllDiff(ID,MT); - csp.addAllDiff(ID,WY); - csp.addAllDiff(ID,UT); - csp.addAllDiff(ID,NV); - csp.addAllDiff(NV,UT); - csp.addAllDiff(NV,AZ); - csp.addAllDiff(UT,WY); - csp.addAllDiff(UT,CO); - csp.addAllDiff(UT,NM); - csp.addAllDiff(UT,AZ); - csp.addAllDiff(AZ,CO); - csp.addAllDiff(AZ,NM); - csp.addAllDiff(MT,WY); - csp.addAllDiff(WY,CO); - csp.addAllDiff(CO,NM); + csp.addAllDiff(WA, ID); + csp.addAllDiff(WA, OR); + csp.addAllDiff(OR, ID); + csp.addAllDiff(OR, CA); + csp.addAllDiff(OR, NV); + csp.addAllDiff(CA, NV); + csp.addAllDiff(CA, AZ); + csp.addAllDiff(ID, MT); + csp.addAllDiff(ID, WY); + csp.addAllDiff(ID, UT); + csp.addAllDiff(ID, NV); + csp.addAllDiff(NV, UT); + csp.addAllDiff(NV, AZ); + csp.addAllDiff(UT, WY); + csp.addAllDiff(UT, CO); + csp.addAllDiff(UT, NM); + csp.addAllDiff(UT, AZ); + csp.addAllDiff(AZ, CO); + csp.addAllDiff(AZ, NM); + csp.addAllDiff(MT, WY); + csp.addAllDiff(WY, CO); + csp.addAllDiff(CO, NM); // Solve Ordering ordering; - ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7),Key(8),Key(9),Key(10); + ordering += Key(0), Key(1), Key(2), Key(3), Key(4), Key(5), Key(6), Key(7), + Key(8), Key(9), Key(10); CSP::sharedValues mpe = csp.optimalAssignment(ordering); // GTSAM_PRINT(*mpe); CSP::Values expected; - insert(expected) - (WA.first,1)(CA.first,1)(NV.first,3)(OR.first,0) - (MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) - (ID.first,2)(UT.first,1)(AZ.first,0); + insert(expected)(WA.first, 1)(CA.first, 1)(NV.first, 3)(OR.first, 0)( + MT.first, 1)(WY.first, 0)(NM.first, 3)(CO.first, 2)(ID.first, 2)( + UT.first, 1)(AZ.first, 0); // TODO: Fix me! mpe result seems to be right. (See the printing) // It has the same prob as the expected solution. // Is mpe another solution, or the expected solution is unique??? - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // Write out the dual graph for hmetis @@ -142,8 +143,7 @@ TEST_UNSAFE( CSP, WesternUS) } /* ************************************************************************* */ -TEST_UNSAFE( CSP, AllDiff) -{ +TEST_UNSAFE(CSP, AllDiff) { // Create keys and ordering size_t nrColors = 3; DiscreteKey ID(0, nrColors), UT(2, nrColors), AZ(1, nrColors); @@ -151,24 +151,25 @@ TEST_UNSAFE( CSP, AllDiff) // Create the CSP CSP csp; vector dkeys; - dkeys += ID,UT,AZ; + dkeys += ID, UT, AZ; csp.addAllDiff(dkeys); - csp.addSingleValue(AZ,2); -// GTSAM_PRINT(csp); + csp.addSingleValue(AZ, 2); + // GTSAM_PRINT(csp); // Check construction and conversion - SingleValue s(AZ,2); - DecisionTreeFactor f1(AZ,"0 0 1"); - EXPECT(assert_equal(f1,s.toDecisionTreeFactor())); + SingleValue s(AZ, 2); + DecisionTreeFactor f1(AZ, "0 0 1"); + EXPECT(assert_equal(f1, s.toDecisionTreeFactor())); // Check construction and conversion AllDiff alldiff(dkeys); DecisionTreeFactor actual = alldiff.toDecisionTreeFactor(); -// GTSAM_PRINT(actual); -// actual.dot("actual"); - DecisionTreeFactor f2(ID & AZ & UT, + // GTSAM_PRINT(actual); + // actual.dot("actual"); + DecisionTreeFactor f2( + ID & AZ & UT, "0 0 0 0 0 1 0 1 0 0 0 1 0 0 0 1 0 0 0 1 0 1 0 0 0 0 0"); - EXPECT(assert_equal(f2,actual)); + EXPECT(assert_equal(f2, actual)); // Check an invalid combination, with ID==UT==AZ all same color DiscreteFactor::Values invalid; @@ -188,36 +189,36 @@ TEST_UNSAFE( CSP, AllDiff) CSP::sharedValues mpe = csp.optimalAssignment(); CSP::Values expected; insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); - EXPECT(assert_equal(expected,*mpe)); + EXPECT(assert_equal(expected, *mpe)); EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // Arc-consistency vector domains; domains += Domain(ID), Domain(AZ), Domain(UT); - SingleValue singleValue(AZ,2); - EXPECT(singleValue.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(0,domains)); - EXPECT(!alldiff.ensureArcConsistency(1,domains)); - EXPECT(alldiff.ensureArcConsistency(2,domains)); - LONGS_EQUAL(2,domains[0].nrValues()); - LONGS_EQUAL(1,domains[1].nrValues()); - LONGS_EQUAL(2,domains[2].nrValues()); + SingleValue singleValue(AZ, 2); + EXPECT(singleValue.ensureArcConsistency(1, domains)); + EXPECT(alldiff.ensureArcConsistency(0, domains)); + EXPECT(!alldiff.ensureArcConsistency(1, domains)); + EXPECT(alldiff.ensureArcConsistency(2, domains)); + LONGS_EQUAL(2, domains[0].nrValues()); + LONGS_EQUAL(1, domains[1].nrValues()); + LONGS_EQUAL(2, domains[2].nrValues()); // Parial application, version 1 DiscreteFactor::Values known; known[AZ.first] = 2; DiscreteFactor::shared_ptr reduced1 = alldiff.partiallyApply(known); DecisionTreeFactor f3(ID & UT, "0 1 1 1 0 1 1 1 0"); - EXPECT(assert_equal(f3,reduced1->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced1->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced2 = singleValue.partiallyApply(known); DecisionTreeFactor f4(AZ, "0 0 1"); - EXPECT(assert_equal(f4,reduced2->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced2->toDecisionTreeFactor())); // Parial application, version 2 DiscreteFactor::shared_ptr reduced3 = alldiff.partiallyApply(domains); - EXPECT(assert_equal(f3,reduced3->toDecisionTreeFactor())); + EXPECT(assert_equal(f3, reduced3->toDecisionTreeFactor())); DiscreteFactor::shared_ptr reduced4 = singleValue.partiallyApply(domains); - EXPECT(assert_equal(f4,reduced4->toDecisionTreeFactor())); + EXPECT(assert_equal(f4, reduced4->toDecisionTreeFactor())); // full arc-consistency test csp.runArcConsistency(nrColors); @@ -229,4 +230,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp index 9929938d5..c48d7639d 100644 --- a/gtsam_unstable/discrete/tests/testLoopyBelief.cpp +++ b/gtsam_unstable/discrete/tests/testLoopyBelief.cpp @@ -5,14 +5,15 @@ * @date Oct 11, 2013 */ -#include +#include #include #include -#include -#include +#include + #include -#include +#include #include +#include using namespace std; using namespace boost; @@ -23,11 +24,12 @@ using namespace gtsam; * Loopy belief solver for graphs with only binary and unary factors */ class LoopyBelief { - /** Star graph struct for each node, containing * - the star graph itself - * - the product of original unary factors so we don't have to recompute it later, and - * - the factor indices of the corrected belief factors of the neighboring nodes + * - the product of original unary factors so we don't have to recompute it + * later, and + * - the factor indices of the corrected belief factors of the neighboring + * nodes */ typedef std::map CorrectedBeliefIndices; struct StarGraph { @@ -36,41 +38,41 @@ class LoopyBelief { DecisionTreeFactor::shared_ptr unary; VariableIndex varIndex_; StarGraph(const DiscreteFactorGraph::shared_ptr& _star, - const CorrectedBeliefIndices& _beliefIndices, - const DecisionTreeFactor::shared_ptr& _unary) : - star(_star), correctedBeliefIndices(_beliefIndices), unary(_unary), varIndex_( - *_star) { - } + const CorrectedBeliefIndices& _beliefIndices, + const DecisionTreeFactor::shared_ptr& _unary) + : star(_star), + correctedBeliefIndices(_beliefIndices), + unary(_unary), + varIndex_(*_star) {} void print(const std::string& s = "") const { cout << s << ":" << endl; star->print("Star graph: "); - for(Key key: correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key key : correctedBeliefIndices | boost::adaptors::map_keys) { cout << "Belief factor index for " << key << ": " - << correctedBeliefIndices.at(key) << endl; + << correctedBeliefIndices.at(key) << endl; } - if (unary) - unary->print("Unary: "); + if (unary) unary->print("Unary: "); } }; typedef std::map StarGraphs; - StarGraphs starGraphs_; ///< star graph at each variable + StarGraphs starGraphs_; ///< star graph at each variable -public: + public: /** Constructor - * Need all discrete keys to access node's cardinality for creating belief factors + * Need all discrete keys to access node's cardinality for creating belief + * factors * TODO: so troublesome!! */ LoopyBelief(const DiscreteFactorGraph& graph, - const std::map& allDiscreteKeys) : - starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) { - } + const std::map& allDiscreteKeys) + : starGraphs_(buildStarGraphs(graph, allDiscreteKeys)) {} /// print void print(const std::string& s = "") const { cout << s << ":" << endl; - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { starGraphs_.at(key).print((boost::format("Node %d:") % key).str()); } } @@ -79,12 +81,13 @@ public: DiscreteFactorGraph::shared_ptr iterate( const std::map& allDiscreteKeys) { static const bool debug = false; - static DiscreteConditional::shared_ptr dummyCond; // unused by-product of elimination + static DiscreteConditional::shared_ptr + dummyCond; // unused by-product of elimination DiscreteFactorGraph::shared_ptr beliefs(new DiscreteFactorGraph()); std::map > allMessages; // Eliminate each star graph - for(Key key: starGraphs_ | boost::adaptors::map_keys) { -// cout << "***** Node " << key << "*****" << endl; + for (Key key : starGraphs_ | boost::adaptors::map_keys) { + // cout << "***** Node " << key << "*****" << endl; // initialize belief to the unary factor from the original graph DecisionTreeFactor::shared_ptr beliefAtKey; @@ -92,15 +95,16 @@ public: std::map messages; // eliminate each neighbor in this star graph one by one - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { DiscreteFactorGraph subGraph; - for(size_t factor: starGraphs_.at(key).varIndex_[neighbor]) { + for (size_t factor : starGraphs_.at(key).varIndex_[neighbor]) { subGraph.push_back(starGraphs_.at(key).star->at(factor)); } if (debug) subGraph.print("------- Subgraph:"); DiscreteFactor::shared_ptr message; - boost::tie(dummyCond, message) = EliminateDiscrete(subGraph, - Ordering(list_of(neighbor))); + boost::tie(dummyCond, message) = + EliminateDiscrete(subGraph, Ordering(list_of(neighbor))); // store the new factor into messages messages.insert(make_pair(neighbor, message)); if (debug) message->print("------- Message: "); @@ -108,14 +112,12 @@ public: // Belief is the product of all messages and the unary factor // Incorporate new the factor to belief if (!beliefAtKey) - beliefAtKey = boost::dynamic_pointer_cast( - message); - else beliefAtKey = - boost::make_shared( - (*beliefAtKey) - * (*boost::dynamic_pointer_cast( - message))); + boost::dynamic_pointer_cast(message); + else + beliefAtKey = boost::make_shared( + (*beliefAtKey) * + (*boost::dynamic_pointer_cast(message))); } if (starGraphs_.at(key).unary) beliefAtKey = boost::make_shared( @@ -133,7 +135,8 @@ public: sumFactorTable = (boost::format("%s %f") % sumFactorTable % sum).str(); DecisionTreeFactor sumFactor(allDiscreteKeys.at(key), sumFactorTable); if (debug) sumFactor.print("denomFactor: "); - beliefAtKey = boost::make_shared((*beliefAtKey) / sumFactor); + beliefAtKey = + boost::make_shared((*beliefAtKey) / sumFactor); if (debug) beliefAtKey->print("New belief at key normalized: "); beliefs->push_back(beliefAtKey); allMessages[key] = messages; @@ -141,17 +144,20 @@ public: // Update corrected beliefs VariableIndex beliefFactors(*beliefs); - for(Key key: starGraphs_ | boost::adaptors::map_keys) { + for (Key key : starGraphs_ | boost::adaptors::map_keys) { std::map messages = allMessages[key]; - for(Key neighbor: starGraphs_.at(key).correctedBeliefIndices | boost::adaptors::map_keys) { - DecisionTreeFactor correctedBelief = (*boost::dynamic_pointer_cast< - DecisionTreeFactor>(beliefs->at(beliefFactors[key].front()))) - / (*boost::dynamic_pointer_cast( + for (Key neighbor : starGraphs_.at(key).correctedBeliefIndices | + boost::adaptors::map_keys) { + DecisionTreeFactor correctedBelief = + (*boost::dynamic_pointer_cast( + beliefs->at(beliefFactors[key].front()))) / + (*boost::dynamic_pointer_cast( messages.at(neighbor))); if (debug) correctedBelief.print("correctedBelief: "); - size_t beliefIndex = starGraphs_.at(neighbor).correctedBeliefIndices.at( - key); - starGraphs_.at(neighbor).star->replace(beliefIndex, + size_t beliefIndex = + starGraphs_.at(neighbor).correctedBeliefIndices.at(key); + starGraphs_.at(neighbor).star->replace( + beliefIndex, boost::make_shared(correctedBelief)); } } @@ -161,21 +167,22 @@ public: return beliefs; } -private: + private: /** * Build star graphs for each node. */ - StarGraphs buildStarGraphs(const DiscreteFactorGraph& graph, + StarGraphs buildStarGraphs( + const DiscreteFactorGraph& graph, const std::map& allDiscreteKeys) const { StarGraphs starGraphs; - VariableIndex varIndex(graph); ///< access to all factors of each node - for(Key key: varIndex | boost::adaptors::map_keys) { + VariableIndex varIndex(graph); ///< access to all factors of each node + for (Key key : varIndex | boost::adaptors::map_keys) { // initialize to multiply with other unary factors later DecisionTreeFactor::shared_ptr prodOfUnaries; // collect all factors involving this key in the original graph DiscreteFactorGraph::shared_ptr star(new DiscreteFactorGraph()); - for(size_t factorIndex: varIndex[key]) { + for (size_t factorIndex : varIndex[key]) { star->push_back(graph.at(factorIndex)); // accumulate unary factors @@ -185,9 +192,9 @@ private: graph.at(factorIndex)); else prodOfUnaries = boost::make_shared( - *prodOfUnaries - * (*boost::dynamic_pointer_cast( - graph.at(factorIndex)))); + *prodOfUnaries * + (*boost::dynamic_pointer_cast( + graph.at(factorIndex)))); } } @@ -196,7 +203,7 @@ private: KeySet neighbors = star->keys(); neighbors.erase(key); CorrectedBeliefIndices correctedBeliefIndices; - for(Key neighbor: neighbors) { + for (Key neighbor : neighbors) { // TODO: default table for keys with more than 2 values? string initialBelief; for (size_t v = 0; v < allDiscreteKeys.at(neighbor).second - 1; ++v) { @@ -207,9 +214,8 @@ private: DecisionTreeFactor(allDiscreteKeys.at(neighbor), initialBelief)); correctedBeliefIndices.insert(make_pair(neighbor, star->size() - 1)); } - starGraphs.insert( - make_pair(key, - StarGraph(star, correctedBeliefIndices, prodOfUnaries))); + starGraphs.insert(make_pair( + key, StarGraph(star, correctedBeliefIndices, prodOfUnaries))); } return starGraphs; } @@ -249,7 +255,6 @@ TEST_UNSAFE(LoopyBelief, construction) { DiscreteFactorGraph::shared_ptr beliefs = solver.iterate(allKeys); beliefs->print(); } - } /* ************************************************************************* */ diff --git a/gtsam_unstable/discrete/tests/testScheduler.cpp b/gtsam_unstable/discrete/tests/testScheduler.cpp index 3f6c6a1e0..4eb86fe1f 100644 --- a/gtsam_unstable/discrete/tests/testScheduler.cpp +++ b/gtsam_unstable/discrete/tests/testScheduler.cpp @@ -5,14 +5,13 @@ */ //#define ENABLE_TIMING -#include +#include #include #include +#include -#include - -#include #include +#include #include using namespace boost::assign; @@ -22,7 +21,6 @@ using namespace gtsam; /* ************************************************************************* */ // Create the expected graph of constraints DiscreteFactorGraph createExpected() { - // Start building size_t nrFaculty = 4, nrTimeSlots = 3; @@ -47,27 +45,27 @@ DiscreteFactorGraph createExpected() { string available = "1 1 1 0 1 1 1 1 0 1 1 1"; // Akansel - expected.add(A1, faculty_in_A); // Area 1 - expected.add(A1, "1 1 1 0"); // Advisor + expected.add(A1, faculty_in_A); // Area 1 + expected.add(A1, "1 1 1 0"); // Advisor expected.add(A & A1, available); - expected.add(A2, faculty_in_M); // Area 2 - expected.add(A2, "1 1 1 0"); // Advisor + expected.add(A2, faculty_in_M); // Area 2 + expected.add(A2, "1 1 1 0"); // Advisor expected.add(A & A2, available); - expected.add(A3, faculty_in_P); // Area 3 - expected.add(A3, "1 1 1 0"); // Advisor + expected.add(A3, faculty_in_P); // Area 3 + expected.add(A3, "1 1 1 0"); // Advisor expected.add(A & A3, available); // Mutual exclusion for faculty expected.addAllDiff(A1 & A2 & A3); // Jake - expected.add(J1, faculty_in_H); // Area 1 - expected.add(J1, "1 0 1 1"); // Advisor + expected.add(J1, faculty_in_H); // Area 1 + expected.add(J1, "1 0 1 1"); // Advisor expected.add(J & J1, available); - expected.add(J2, faculty_in_C); // Area 2 - expected.add(J2, "1 0 1 1"); // Advisor + expected.add(J2, faculty_in_C); // Area 2 + expected.add(J2, "1 0 1 1"); // Advisor expected.add(J & J2, available); - expected.add(J3, faculty_in_A); // Area 3 - expected.add(J3, "1 0 1 1"); // Advisor + expected.add(J3, faculty_in_A); // Area 3 + expected.add(J3, "1 0 1 1"); // Advisor expected.add(J & J3, available); // Mutual exclusion for faculty expected.addAllDiff(J1 & J2 & J3); @@ -79,8 +77,7 @@ DiscreteFactorGraph createExpected() { } /* ************************************************************************* */ -TEST( schedulingExample, test) -{ +TEST(schedulingExample, test) { Scheduler s(2); // add faculty @@ -121,7 +118,7 @@ TEST( schedulingExample, test) // Do brute force product and output that to file DecisionTreeFactor product = s.product(); - //product.dot("scheduling", false); + // product.dot("scheduling", false); // Do exact inference gttic(small); @@ -129,25 +126,24 @@ TEST( schedulingExample, test) gttoc(small); // print MPE, commented out as unit tests don't print -// s.printAssignment(MPE); + // s.printAssignment(MPE); // Commented out as does not work yet // s.runArcConsistency(8,10,true); // find the assignment of students to slots with most possible committees // Commented out as not implemented yet -// sharedValues bestSchedule = s.bestSchedule(); -// GTSAM_PRINT(*bestSchedule); + // sharedValues bestSchedule = s.bestSchedule(); + // GTSAM_PRINT(*bestSchedule); // find the corresponding most desirable committee assignment // Commented out as not implemented yet -// sharedValues bestAssignment = s.bestAssignment(bestSchedule); -// GTSAM_PRINT(*bestAssignment); + // sharedValues bestAssignment = s.bestAssignment(bestSchedule); + // GTSAM_PRINT(*bestAssignment); } /* ************************************************************************* */ -TEST( schedulingExample, smallFromFile) -{ +TEST(schedulingExample, smallFromFile) { string path(TOPSRCDIR "/gtsam_unstable/discrete/examples/"); Scheduler s(2, path + "small.csv"); @@ -179,4 +175,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index e2115e8bc..78f159251 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -5,21 +5,22 @@ * @author Frank Dellaert */ -#include #include +#include + #include using boost::assign::insert; +#include + #include #include -#include using namespace std; using namespace gtsam; #define PRINT false -class Sudoku: public CSP { - +class Sudoku : public CSP { /// sudoku size size_t n_; @@ -27,25 +28,21 @@ class Sudoku: public CSP { typedef std::pair IJ; std::map dkeys_; -public: - + public: /// return DiscreteKey for cell(i,j) const DiscreteKey& dkey(size_t i, size_t j) const { return dkeys_.at(IJ(i, j)); } /// return Key for cell(i,j) - Key key(size_t i, size_t j) const { - return dkey(i, j).first; - } + Key key(size_t i, size_t j) const { return dkey(i, j).first; } /// Constructor - Sudoku(size_t n, ...) : - n_(n) { + Sudoku(size_t n, ...) : n_(n) { // Create variables, ordering, and unary constraints va_list ap; va_start(ap, n); - Key k=0; + Key k = 0; for (size_t i = 0; i < n; ++i) { for (size_t j = 0; j < n; ++j, ++k) { // create the key @@ -56,23 +53,21 @@ public: // cout << value << " "; if (value != 0) addSingleValue(dkeys_[ij], value - 1); } - //cout << endl; + // cout << endl; } va_end(ap); // add row constraints for (size_t i = 0; i < n; i++) { DiscreteKeys dkeys; - for (size_t j = 0; j < n; j++) - dkeys += dkey(i, j); + for (size_t j = 0; j < n; j++) dkeys += dkey(i, j); addAllDiff(dkeys); } // add col constraints for (size_t j = 0; j < n; j++) { DiscreteKeys dkeys; - for (size_t i = 0; i < n; i++) - dkeys += dkey(i, j); + for (size_t i = 0; i < n; i++) dkeys += dkey(i, j); addAllDiff(dkeys); } @@ -84,8 +79,7 @@ public: // Box I,J DiscreteKeys dkeys; for (size_t i = i0; i < i0 + N; i++) - for (size_t j = j0; j < j0 + N; j++) - dkeys += dkey(i, j); + for (size_t j = j0; j < j0 + N; j++) dkeys += dkey(i, j); addAllDiff(dkeys); j0 += N; } @@ -109,74 +103,59 @@ public: DiscreteFactor::sharedValues MPE = optimalAssignment(); printAssignment(MPE); } - }; /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, small) -{ - Sudoku csp(4, - 1,0, 0,4, - 0,0, 0,0, +TEST_UNSAFE(Sudoku, small) { + Sudoku csp(4, 1, 0, 0, 4, 0, 0, 0, 0, - 4,0, 2,0, - 0,1, 0,0); + 4, 0, 2, 0, 0, 1, 0, 0); // Do BP - csp.runArcConsistency(4,10,PRINT); + csp.runArcConsistency(4, 10, PRINT); // optimize and check CSP::sharedValues solution = csp.optimalAssignment(); CSP::Values expected; - insert(expected) - (csp.key(0,0), 0)(csp.key(0,1), 1)(csp.key(0,2), 2)(csp.key(0,3), 3) - (csp.key(1,0), 2)(csp.key(1,1), 3)(csp.key(1,2), 0)(csp.key(1,3), 1) - (csp.key(2,0), 3)(csp.key(2,1), 2)(csp.key(2,2), 1)(csp.key(2,3), 0) - (csp.key(3,0), 1)(csp.key(3,1), 0)(csp.key(3,2), 3)(csp.key(3,3), 2); - EXPECT(assert_equal(expected,*solution)); - //csp.printAssignment(solution); + insert(expected)(csp.key(0, 0), 0)(csp.key(0, 1), 1)(csp.key(0, 2), 2)( + csp.key(0, 3), 3)(csp.key(1, 0), 2)(csp.key(1, 1), 3)(csp.key(1, 2), 0)( + csp.key(1, 3), 1)(csp.key(2, 0), 3)(csp.key(2, 1), 2)(csp.key(2, 2), 1)( + csp.key(2, 3), 0)(csp.key(3, 0), 1)(csp.key(3, 1), 0)(csp.key(3, 2), 3)( + csp.key(3, 3), 2); + EXPECT(assert_equal(expected, *solution)); + // csp.printAssignment(solution); } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, easy) -{ - Sudoku sudoku(9, - 0,0,5, 0,9,0, 0,0,1, - 0,0,0, 0,0,2, 0,7,3, - 7,6,0, 0,0,8, 2,0,0, +TEST_UNSAFE(Sudoku, easy) { + Sudoku sudoku(9, 0, 0, 5, 0, 9, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 7, 3, 7, 6, + 0, 0, 0, 8, 2, 0, 0, - 0,1,2, 0,0,9, 0,0,4, - 0,0,0, 2,0,3, 0,0,0, - 3,0,0, 1,0,0, 9,6,0, + 0, 1, 2, 0, 0, 9, 0, 0, 4, 0, 0, 0, 2, 0, 3, 0, 0, 0, 3, 0, 0, + 1, 0, 0, 9, 6, 0, - 0,0,1, 9,0,0, 0,5,8, - 9,7,0, 5,0,0, 0,0,0, - 5,0,0, 0,3,0, 7,0,0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, 9, 7, 0, 5, 0, 0, 0, 0, 0, 5, 0, 0, + 0, 3, 0, 7, 0, 0); // Do BP - sudoku.runArcConsistency(4,10,PRINT); + sudoku.runArcConsistency(4, 10, PRINT); // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, extreme) -{ - Sudoku sudoku(9, - 0,0,9, 7,4,8, 0,0,0, - 7,0,0, 0,0,0, 0,0,0, - 0,2,0, 1,0,9, 0,0,0, +TEST_UNSAFE(Sudoku, extreme) { + Sudoku sudoku(9, 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, + 0, 1, 0, 9, 0, 0, 0, - 0,0,7, 0,0,0, 2,4,0, - 0,6,4, 0,1,0, 5,9,0, - 0,9,8, 0,0,0, 3,0,0, + 0, 0, 7, 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, 1, 0, 5, 9, 0, 0, 9, 8, + 0, 0, 0, 3, 0, 0, - 0,0,0, 8,0,3, 0,2,0, - 0,0,0, 0,0,0, 0,0,6, - 0,0,0, 2,7,5, 9,0,0); + 0, 0, 0, 8, 0, 3, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, + 2, 7, 5, 9, 0, 0); // Do BP - sudoku.runArcConsistency(9,10,PRINT); + sudoku.runArcConsistency(9, 10, PRINT); #ifdef METIS VariableIndexOrdered index(sudoku); @@ -185,29 +164,24 @@ TEST_UNSAFE( Sudoku, extreme) index.outputMetisFormat(os); #endif - //sudoku.printSolution(); // don't do it + // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ -TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012) -{ - Sudoku sudoku(9, - 9,5,0, 0,0,6, 0,0,0, - 0,8,4, 0,7,0, 0,0,0, - 6,2,0, 5,0,0, 4,0,0, +TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { + Sudoku sudoku(9, 9, 5, 0, 0, 0, 6, 0, 0, 0, 0, 8, 4, 0, 7, 0, 0, 0, 0, 6, 2, + 0, 5, 0, 0, 4, 0, 0, - 0,0,0, 2,9,0, 6,0,0, - 0,9,0, 0,0,0, 0,2,0, - 0,0,2, 0,6,3, 0,0,0, + 0, 0, 0, 2, 9, 0, 6, 0, 0, 0, 9, 0, 0, 0, 0, 0, 2, 0, 0, 0, 2, + 0, 6, 3, 0, 0, 0, - 0,0,9, 0,0,7, 0,6,8, - 0,0,0, 0,3,0, 2,9,0, - 0,0,0, 1,0,0, 0,3,7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, 0, 0, 0, 0, 3, 0, 2, 9, 0, 0, 0, 0, + 1, 0, 0, 0, 3, 7); // Do BP - sudoku.runArcConsistency(9,10,PRINT); + sudoku.runArcConsistency(9, 10, PRINT); - //sudoku.printSolution(); // don't do it + // sudoku.printSolution(); // don't do it } /* ************************************************************************* */ @@ -216,4 +190,3 @@ int main() { return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ - From aebcf07ab5a1ace4741edbed3e721f867062fa09 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 18 Nov 2021 11:31:11 -0500 Subject: [PATCH 109/110] Formatted sudokus better --- gtsam_unstable/discrete/tests/testSudoku.cpp | 57 ++++++++++++-------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/discrete/tests/testSudoku.cpp b/gtsam_unstable/discrete/tests/testSudoku.cpp index 78f159251..4843ae269 100644 --- a/gtsam_unstable/discrete/tests/testSudoku.cpp +++ b/gtsam_unstable/discrete/tests/testSudoku.cpp @@ -107,9 +107,11 @@ class Sudoku : public CSP { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, small) { - Sudoku csp(4, 1, 0, 0, 4, 0, 0, 0, 0, - - 4, 0, 2, 0, 0, 1, 0, 0); + Sudoku csp(4, // + 1, 0, 0, 4, // + 0, 0, 0, 0, // + 4, 0, 2, 0, // + 0, 1, 0, 0); // Do BP csp.runArcConsistency(4, 10, PRINT); @@ -128,14 +130,18 @@ TEST_UNSAFE(Sudoku, small) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, easy) { - Sudoku sudoku(9, 0, 0, 5, 0, 9, 0, 0, 0, 1, 0, 0, 0, 0, 0, 2, 0, 7, 3, 7, 6, - 0, 0, 0, 8, 2, 0, 0, + Sudoku sudoku(9, // + 0, 0, 5, 0, 9, 0, 0, 0, 1, // + 0, 0, 0, 0, 0, 2, 0, 7, 3, // + 7, 6, 0, 0, 0, 8, 2, 0, 0, // - 0, 1, 2, 0, 0, 9, 0, 0, 4, 0, 0, 0, 2, 0, 3, 0, 0, 0, 3, 0, 0, - 1, 0, 0, 9, 6, 0, + 0, 1, 2, 0, 0, 9, 0, 0, 4, // + 0, 0, 0, 2, 0, 3, 0, 0, 0, // + 3, 0, 0, 1, 0, 0, 9, 6, 0, // - 0, 0, 1, 9, 0, 0, 0, 5, 8, 9, 7, 0, 5, 0, 0, 0, 0, 0, 5, 0, 0, - 0, 3, 0, 7, 0, 0); + 0, 0, 1, 9, 0, 0, 0, 5, 8, // + 9, 7, 0, 5, 0, 0, 0, 0, 0, // + 5, 0, 0, 0, 3, 0, 7, 0, 0); // Do BP sudoku.runArcConsistency(4, 10, PRINT); @@ -145,14 +151,15 @@ TEST_UNSAFE(Sudoku, easy) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, extreme) { - Sudoku sudoku(9, 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, - 0, 1, 0, 9, 0, 0, 0, - - 0, 0, 7, 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, 1, 0, 5, 9, 0, 0, 9, 8, - 0, 0, 0, 3, 0, 0, - - 0, 0, 0, 8, 0, 3, 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0, 0, 0, - 2, 7, 5, 9, 0, 0); + Sudoku sudoku(9, // + 0, 0, 9, 7, 4, 8, 0, 0, 0, 7, // + 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, // + 0, 1, 0, 9, 0, 0, 0, 0, 0, 7, // + 0, 0, 0, 2, 4, 0, 0, 6, 4, 0, // + 1, 0, 5, 9, 0, 0, 9, 8, 0, 0, // + 0, 3, 0, 0, 0, 0, 0, 8, 0, 3, // + 0, 2, 0, 0, 0, 0, 0, 0, 0, 0, // + 0, 6, 0, 0, 0, 2, 7, 5, 9, 0, 0); // Do BP sudoku.runArcConsistency(9, 10, PRINT); @@ -169,14 +176,18 @@ TEST_UNSAFE(Sudoku, extreme) { /* ************************************************************************* */ TEST_UNSAFE(Sudoku, AJC_3star_Feb8_2012) { - Sudoku sudoku(9, 9, 5, 0, 0, 0, 6, 0, 0, 0, 0, 8, 4, 0, 7, 0, 0, 0, 0, 6, 2, - 0, 5, 0, 0, 4, 0, 0, + Sudoku sudoku(9, // + 9, 5, 0, 0, 0, 6, 0, 0, 0, // + 0, 8, 4, 0, 7, 0, 0, 0, 0, // + 6, 2, 0, 5, 0, 0, 4, 0, 0, // - 0, 0, 0, 2, 9, 0, 6, 0, 0, 0, 9, 0, 0, 0, 0, 0, 2, 0, 0, 0, 2, - 0, 6, 3, 0, 0, 0, + 0, 0, 0, 2, 9, 0, 6, 0, 0, // + 0, 9, 0, 0, 0, 0, 0, 2, 0, // + 0, 0, 2, 0, 6, 3, 0, 0, 0, // - 0, 0, 9, 0, 0, 7, 0, 6, 8, 0, 0, 0, 0, 3, 0, 2, 9, 0, 0, 0, 0, - 1, 0, 0, 0, 3, 7); + 0, 0, 9, 0, 0, 7, 0, 6, 8, // + 0, 0, 0, 0, 3, 0, 2, 9, 0, // + 0, 0, 0, 1, 0, 0, 0, 3, 7); // Do BP sudoku.runArcConsistency(9, 10, PRINT); From 4a05da53af352427cd950a12dcbd0bcae82a2da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 18 Nov 2021 23:29:10 -0500 Subject: [PATCH 110/110] wrap KeyVector methods --- gtsam/linear/linear.i | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index c74161f26..cf65b1a38 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -302,6 +302,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; void printKeys(string s) const; + gtsam::KeyVector& keys() const; bool equals(const gtsam::GaussianFactor& lf, double tol) const; size_t size() const; Vector unweighted_error(const gtsam::VectorValues& c) const; @@ -514,9 +515,9 @@ virtual class GaussianBayesNet { size_t size() const; // FactorGraph derived interface - // size_t size() const; gtsam::GaussianConditional* at(size_t idx) const; gtsam::KeySet keys() const; + gtsam::KeyVector keyVector() const; bool exists(size_t idx) const; void saveGraph(const string& s) const;