From c1b00e28b1a6da91006e0831c27085302aa91e5b Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 14 Aug 2016 18:54:00 -0400 Subject: [PATCH] created SmartFactorParams class collecting common parameters in smartProjectionFactor and smartStereoProjectFactor classes. This largely reduced copy-paste. SmartStereoProjectionParams is now a typedef --- gtsam/slam/SmartFactorBase.h | 10 -- gtsam/slam/SmartFactorParams.h | 134 ++++++++++++++++++ gtsam/slam/SmartProjectionFactor.h | 94 +----------- .../slam/SmartStereoProjectionFactor.h | 86 +---------- 4 files changed, 140 insertions(+), 184 deletions(-) create mode 100644 gtsam/slam/SmartFactorParams.h diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 57a53daa3..1453d80a7 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -35,16 +35,6 @@ namespace gtsam { -/// Linearization mode: what factor to linearize to -enum LinearizationMode { - HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD -}; - -/// How to manage degeneracy -enum DegeneracyMode { - IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY -}; - /** * @brief Base class for smart factors * This base class has no internal point, but it has a measurement, noise model diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h new file mode 100644 index 000000000..0f970ac86 --- /dev/null +++ b/gtsam/slam/SmartFactorParams.h @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * 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 SmartFactorParams + * @brief Collect common parameters for SmartProjection and SmartStereoProjection factors + * @author Luca Carlone + * @author Zsolt Kira + * @author Frank Dellaert + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * SmartFactorParams: parameters and (linearization/degeneracy) modes for SmartProjection and SmartStereoProjection factors + */ +/// Linearization mode: what factor to linearize to +enum LinearizationMode { + HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD +}; + +/// How to manage degeneracy +enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY +}; + +/* + * Parameters for the smart (stereo) projection factors + */ +struct GTSAM_EXPORT SmartProjectionParams { + + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor + + /// @name Parameters governing the triangulation + /// @{ + TriangulationParameters triangulation; + double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + /// @} + + /// @name Parameters governing how triangulation result is treated + /// @{ + bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) + bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) + /// @} + + // Constructor + SmartProjectionParams(LinearizationMode linMode = HESSIAN, + DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, + bool verboseCheirality = false, double retriangulationTh = 1e-5) : + linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( + retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( + verboseCheirality) { + } + + virtual ~SmartProjectionParams() { + } + + void print(const std::string& str) const { + std::cout << "linearizationMode: " << linearizationMode << "\n"; + std::cout << " degeneracyMode: " << degeneracyMode << "\n"; + std::cout << triangulation << std::endl; + } + + // get class variables + LinearizationMode getLinearizationMode() const { + return linearizationMode; + } + DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + TriangulationParameters getTriangulationParameters() const { + return triangulation; + } + bool getVerboseCheirality() const { + return verboseCheirality; + } + bool getThrowCheirality() const { + return throwCheirality; + } + double getRetriangulationThreshold() const { + return retriangulationThreshold; + } + // set class variables + void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + void setRetriangulationThreshold(double retriangulationTh) { + retriangulationThreshold = retriangulationTh; + } + void setRankTolerance(double rankTol) { + triangulation.rankTolerance = rankTol; + } + void setEnableEPI(bool enableEPI) { + triangulation.enableEPI = enableEPI; + } + void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { + triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { + triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } + +private: + + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(linearizationMode); + ar & BOOST_SERIALIZATION_NVP(degeneracyMode); + ar & BOOST_SERIALIZATION_NVP(triangulation); + ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); + ar & BOOST_SERIALIZATION_NVP(throwCheirality); + ar & BOOST_SERIALIZATION_NVP(verboseCheirality); + } +}; + +} // \ namespace gtsam diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index b015f1d7d..840ecbc4d 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -20,6 +20,7 @@ #pragma once #include +#include #include #include @@ -31,99 +32,6 @@ namespace gtsam { -/* - * Parameters for the smart projection factors - */ -struct GTSAM_EXPORT SmartProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - // Constructor - SmartProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false, double retriangulationTh = 1e-5) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - double getRetriangulationThreshold() const { - return retriangulationThreshold; - } - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setRetriangulationThreshold(double retriangulationTh) { - retriangulationThreshold = retriangulationTh; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - -private: - - /// Serialization function - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(linearizationMode); - ar & BOOST_SERIALIZATION_NVP(degeneracyMode); - ar & BOOST_SERIALIZATION_NVP(triangulation); - ar & BOOST_SERIALIZATION_NVP(retriangulationThreshold); - ar & BOOST_SERIALIZATION_NVP(throwCheirality); - ar & BOOST_SERIALIZATION_NVP(verboseCheirality); - } -}; - /** * SmartProjectionFactor: triangulates point and keeps an estimate of it around. * This factor operates with monocular cameras, where a camera is expected to diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 9b9d3c935..29ae6233b 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -21,6 +21,7 @@ #pragma once #include +#include #include #include @@ -35,87 +36,10 @@ namespace gtsam { - /* - * Parameters for the smart stereo projection factors - */ - struct GTSAM_EXPORT SmartStereoProjectionParams { - - LinearizationMode linearizationMode; ///< How to linearize the factor - DegeneracyMode degeneracyMode; ///< How to linearize the factor - - /// @name Parameters governing the triangulation - /// @{ - TriangulationParameters triangulation; - double retriangulationThreshold; ///< threshold to decide whether to re-triangulate - /// @} - - /// @name Parameters governing how triangulation result is treated - /// @{ - bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false) - bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false) - /// @} - - - /// Constructor - SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : - linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( - verboseCheirality) { - } - - virtual ~SmartStereoProjectionParams() { - } - - void print(const std::string& str) const { - std::cout << "linearizationMode: " << linearizationMode << "\n"; - std::cout << " degeneracyMode: " << degeneracyMode << "\n"; - std::cout << triangulation << std::endl; - } - - // get class variables - LinearizationMode getLinearizationMode() const { - return linearizationMode; - } - DegeneracyMode getDegeneracyMode() const { - return degeneracyMode; - } - TriangulationParameters getTriangulationParameters() const { - return triangulation; - } - bool getVerboseCheirality() const { - return verboseCheirality; - } - bool getThrowCheirality() const { - return throwCheirality; - } - double getRetriangulationThreshold() const { - return retriangulationThreshold; - } - // set class variables - void setLinearizationMode(LinearizationMode linMode) { - linearizationMode = linMode; - } - void setDegeneracyMode(DegeneracyMode degMode) { - degeneracyMode = degMode; - } - void setRetriangulationThreshold(double retriangulationTh) { - retriangulationThreshold = retriangulationTh; - } - void setRankTolerance(double rankTol) { - triangulation.rankTolerance = rankTol; - } - void setEnableEPI(bool enableEPI) { - triangulation.enableEPI = enableEPI; - } - void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) { - triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold; - } - void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) { - triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; - } - }; +/* + * Parameters for the smart stereo projection factors (identical to the SmartProjectionParams) + */ +typedef SmartProjectionParams SmartStereoProjectionParams; /** * SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.