From 19b7312edbc0a934b5d7b3f61b4edcc6dd2b3e19 Mon Sep 17 00:00:00 2001 From: Toni Date: Sun, 17 Jan 2021 11:08:53 -0500 Subject: [PATCH 1/5] Split .h/.cpp, use const& (WIP) --- examples/CMakeLists.txt | 2 +- gtsam/slam/SmartFactorBase.h | 6 +- .../slam/SmartStereoProjectionPoseFactor.cpp | 97 ++++++++++++ .../slam/SmartStereoProjectionPoseFactor.h | 147 +++++++----------- 4 files changed, 159 insertions(+), 93 deletions(-) create mode 100644 gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 476f4ae215..7fc33f9210 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,4 +2,4 @@ set (excluded_examples elaboratePoint2KalmanFilter.cpp ) -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}") diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index d9f2b9c3da..85f9e7c3ff 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -13,6 +13,7 @@ * @file SmartFactorBase.h * @brief Base class to create smart factors on poses or cameras * @author Luca Carlone + * @author Antoni Rosinol * @author Zsolt Kira * @author Frank Dellaert * @author Chris Beall @@ -131,9 +132,10 @@ class SmartFactorBase: public NonlinearFactor { /** * Add a bunch of measurements, together with the camera keys */ - void add(ZVector& measurements, KeyVector& cameraKeys) { + void add(const ZVector& measurements, const KeyVector& cameraKeys) { + assert(measurements.size() == cameraKeys.size()); for (size_t i = 0; i < measurements.size(); i++) { - this->add(measurements.at(i), cameraKeys.at(i)); + this->add(measurements[i], cameraKeys[i]); } } diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp new file mode 100644 index 0000000000..c28cb25a19 --- /dev/null +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.cpp @@ -0,0 +1,97 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartStereoProjectionPoseFactor.cpp + * @brief Smart stereo factor on poses, assuming camera calibration is fixed + * @author Luca Carlone + * @author Antoni Rosinol + * @author Chris Beall + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#include + +namespace gtsam { + +SmartStereoProjectionPoseFactor::SmartStereoProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, + const SmartStereoProjectionParams& params, + const boost::optional& body_P_sensor) + : Base(sharedNoiseModel, params, body_P_sensor) {} + +void SmartStereoProjectionPoseFactor::add( + const StereoPoint2& measured, const Key& poseKey, + const boost::shared_ptr& K) { + Base::add(measured, poseKey); + K_all_.push_back(K); +} + +void SmartStereoProjectionPoseFactor::add( + const std::vector& measurements, const KeyVector& poseKeys, + const std::vector>& Ks) { + assert(measurements.size() == poseKeys.size()); + assert(poseKeys.size() == Ks.size()); + Base::add(measurements, poseKeys); + K_all_.insert(K_all_.end(), Ks.begin(), Ks.end()); +} + +void SmartStereoProjectionPoseFactor::add( + const std::vector& measurements, const KeyVector& poseKeys, + const boost::shared_ptr& K) { + assert(poseKeys.size() == measurements.size()); + for (size_t i = 0; i < measurements.size(); i++) { + Base::add(measurements[i], poseKeys[i]); + K_all_.push_back(K); + } +} + +void SmartStereoProjectionPoseFactor::print( + const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; + for (const boost::shared_ptr& K : K_all_) { + K->print("calibration = "); + } + Base::print("", keyFormatter); +} + +bool SmartStereoProjectionPoseFactor::equals(const NonlinearFactor& p, + double tol) const { + const SmartStereoProjectionPoseFactor* e = + dynamic_cast(&p); + + return e && Base::equals(p, tol); +} + +double SmartStereoProjectionPoseFactor::error(const Values& values) const { + if (this->active(values)) { + return this->totalReprojectionError(cameras(values)); + } else { // else of active flag + return 0.0; + } +} + +SmartStereoProjectionPoseFactor::Base::Cameras +SmartStereoProjectionPoseFactor::cameras(const Values& values) const { + assert(keys_.size() == K_all_.size()); + Base::Cameras cameras; + for (size_t i = 0; i < keys_.size(); i++) { + Pose3 pose = values.at(keys_[i]); + if (Base::body_P_sensor_) { + pose = pose.compose(*(Base::body_P_sensor_)); + } + cameras.push_back(StereoCamera(pose, K_all_[i])); + } + return cameras; +} + +} // \ namespace gtsam diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 124e45005a..31ee121ff3 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -1,6 +1,7 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * GTSAM Copyright 2010, Georgia Tech Research Corpo + * ation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -13,6 +14,7 @@ * @file SmartStereoProjectionPoseFactor.h * @brief Smart stereo factor on poses, assuming camera calibration is fixed * @author Luca Carlone + * @author Antoni Rosinol * @author Chris Beall * @author Zsolt Kira * @author Frank Dellaert @@ -28,8 +30,10 @@ 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, + * 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. * */ @@ -41,14 +45,12 @@ namespace gtsam { * This factor requires that values contains the involved poses (Pose3). * @addtogroup SLAM */ -class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { - -protected: - - std::vector > K_all_; ///< shared pointer to calibration object (one for each camera) - -public: +class SmartStereoProjectionPoseFactor : public SmartStereoProjectionFactor { + protected: + /// shared pointer to calibration object (one for each camera) + std::vector> K_all_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// shorthand for base class type @@ -65,129 +67,94 @@ class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor { * @param Isotropic measurement noise * @param params internal parameters of the smart factors */ - SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel, + SmartStereoProjectionPoseFactor( + const SharedNoiseModel& sharedNoiseModel, const SmartStereoProjectionParams& params = SmartStereoProjectionParams(), - const boost::optional body_P_sensor = boost::none) : - Base(sharedNoiseModel, params, body_P_sensor) { - } + const boost::optional& body_P_sensor = boost::none); /** Virtual destructor */ - virtual ~SmartStereoProjectionPoseFactor() {} + virtual ~SmartStereoProjectionPoseFactor() = default; /** * add a new measurement and pose key - * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKey is key corresponding to the camera observing the same landmark + * @param measured is the 2m dimensional location of the projection of a + * single landmark in the m view (the measurement) + * @param poseKey is key corresponding to the camera observing the same + * landmark * @param K is the (fixed) camera calibration */ - void add(const StereoPoint2 measured, const Key poseKey, - const boost::shared_ptr K) { - Base::add(measured, poseKey); - K_all_.push_back(K); - } + void add(const StereoPoint2& measured, const Key& poseKey, + const boost::shared_ptr& K); /** * Variant of the previous one in which we include a set of measurements - * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing + * the same landmark * @param Ks vector of calibration objects */ - void add(std::vector measurements, KeyVector poseKeys, - std::vector > Ks) { - Base::add(measurements, poseKeys); - for (size_t i = 0; i < measurements.size(); i++) { - K_all_.push_back(Ks.at(i)); - } - } + void add(const std::vector& measurements, + const KeyVector& poseKeys, + const std::vector>& Ks); /** - * Variant of the previous one in which we include a set of measurements with the same noise and calibration - * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement) - * @param poseKeys vector of keys corresponding to the camera observing the same landmark + * Variant of the previous one in which we include a set of measurements with + * the same noise and calibration + * @param measurements vector of the 2m dimensional location of the projection + * of a single landmark in the m view (the measurement) + * @param poseKeys vector of keys corresponding to the camera observing the + * same landmark * @param K the (known) camera calibration (same for all measurements) */ - void add(std::vector measurements, KeyVector poseKeys, - const boost::shared_ptr K) { - for (size_t i = 0; i < measurements.size(); i++) { - Base::add(measurements.at(i), poseKeys.at(i)); - K_all_.push_back(K); - } - } + void add(const std::vector& measurements, + const KeyVector& poseKeys, + const boost::shared_ptr& K); /** * print * @param s optional string naming the factor * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "", const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const override { - std::cout << s << "SmartStereoProjectionPoseFactor, z = \n "; - for(const boost::shared_ptr& K: K_all_) - K->print("calibration = "); - Base::print("", keyFormatter); - } + */ void print( + const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; /// equals - bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { - const SmartStereoProjectionPoseFactor *e = - dynamic_cast(&p); - - return e && Base::equals(p, tol); - } + virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const; /** * 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; - } - } + double error(const Values& values) const override; /** return the calibration object */ - inline const std::vector > calibration() const { + inline std::vector> calibration() const { return K_all_; } /** * Collect all cameras involved in this factor - * @param values Values structure which must contain camera poses corresponding + * @param values Values structure which must contain camera poses + * corresponding * to keys involved in this factor * @return vector of Values */ - Base::Cameras cameras(const Values& values) const override { - Base::Cameras cameras; - size_t i=0; - for(const Key& k: this->keys_) { - Pose3 pose = values.at(k); - - if (Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - - StereoCamera camera(pose, K_all_[i++]); - cameras.push_back(camera); - } - return cameras; - } - -private: + Base::Cameras cameras(const Values& values) const override; + private: /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar & BOOST_SERIALIZATION_NVP(K_all_); + 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 +}; // end of class declaration /// traits -template<> -struct traits : public Testable< - SmartStereoProjectionPoseFactor> { -}; +template <> +struct traits + : public Testable {}; -} // \ namespace gtsam +} // \ namespace gtsam From a567a570ede8a8e1ba641eba4b5245af5a2d045e Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 18 Jan 2021 14:41:28 -0500 Subject: [PATCH 2/5] Move example to gtsam_unstable --- .../examples}/ISAM2_SmartFactorStereo_IMU.cpp | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename {examples => gtsam_unstable/examples}/ISAM2_SmartFactorStereo_IMU.cpp (100%) diff --git a/examples/ISAM2_SmartFactorStereo_IMU.cpp b/gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp similarity index 100% rename from examples/ISAM2_SmartFactorStereo_IMU.cpp rename to gtsam_unstable/examples/ISAM2_SmartFactorStereo_IMU.cpp From 5ad65ed46c8883d77eab27487d81afc6ed6e4842 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 18 Jan 2021 14:41:59 -0500 Subject: [PATCH 3/5] Fix formatting --- gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 31ee121ff3..f014a69170 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -1,7 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corpo - * ation, + * GTSAM Copyright 2010, Georgia Tech Research Corpoation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) @@ -30,10 +29,9 @@ 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, + * 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. * */ From 3d7e182822d96f000479aabd317250deb85e25c8 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 18 Jan 2021 14:47:39 -0500 Subject: [PATCH 4/5] Remove gtsam_unstable lib from examples --- examples/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 7fc33f9210..476f4ae215 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -2,4 +2,4 @@ set (excluded_examples elaboratePoint2KalmanFilter.cpp ) -gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;gtsam_unstable;${Boost_PROGRAM_OPTIONS_LIBRARY}") +gtsamAddExamplesGlob("*.cpp" "${excluded_examples}" "gtsam;${Boost_PROGRAM_OPTIONS_LIBRARY}") From 96dc9bfa5a4deaaae79feaa7f9c869358e4a8386 Mon Sep 17 00:00:00 2001 From: Toni Date: Mon, 18 Jan 2021 14:48:19 -0500 Subject: [PATCH 5/5] Fix formatting --- gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index f014a69170..93a83ab301 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corpoation, + * 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)