From 38fd695228c600b35126fd42db548681c83b65a9 Mon Sep 17 00:00:00 2001 From: = Date: Fri, 5 Aug 2016 22:34:38 -0400 Subject: [PATCH 1/6] Fixed compile error on BOOST 1_60_0 --- gtsam_unstable/linear/QPSParser.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp index 748c4db38..a6fc072c7 100644 --- a/gtsam_unstable/linear/QPSParser.cpp +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -85,8 +85,8 @@ struct QPSParser::MPSGrammar: base_grammar { >> +blank >> double_)[colDouble] >> *blank]; quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >> *blank][addQuadTerm]; - bound = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> +blank - >> double_ >> *blank][addBound]; + bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank + >> double_)[addBound] >> *blank]; bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> *blank][addBoundFr]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; From c1b00e28b1a6da91006e0831c27085302aa91e5b Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 14 Aug 2016 18:54:00 -0400 Subject: [PATCH 2/6] 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. From 7e44a1776e15ebf4bdf598952779d64283aeaf61 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 14 Aug 2016 19:20:38 -0400 Subject: [PATCH 3/6] added default string on print --- gtsam/slam/SmartFactorParams.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/SmartFactorParams.h b/gtsam/slam/SmartFactorParams.h index 0f970ac86..761703f8b 100644 --- a/gtsam/slam/SmartFactorParams.h +++ b/gtsam/slam/SmartFactorParams.h @@ -68,7 +68,7 @@ struct GTSAM_EXPORT SmartProjectionParams { virtual ~SmartProjectionParams() { } - void print(const std::string& str) const { + void print(const std::string& str = "") const { std::cout << "linearizationMode: " << linearizationMode << "\n"; std::cout << " degeneracyMode: " << degeneracyMode << "\n"; std::cout << triangulation << std::endl; From 7e3aa7aa9ab02ae173f90c64c06bd690e968083d Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 14 Aug 2016 20:03:45 -0400 Subject: [PATCH 4/6] wrapped throw StereoCheiralityException inside #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION --- gtsam/geometry/StereoCamera.cpp | 5 ++++- gtsam/geometry/tests/testStereoCamera.cpp | 14 ++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 5c6646454..c34d4d8f3 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -39,7 +39,10 @@ namespace gtsam { const Point3 q = leftCamPose_.transform_to(point); - if ( q.z() <= 0 ) throw StereoCheiralityException(); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + if (q.z() <= 0) + throw StereoCheiralityException(); +#endif // get calibration const Cal3_S2Stereo& K = *K_; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 497599a6a..f76d841bf 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -104,6 +104,20 @@ TEST( StereoCamera, Dproject) CHECK(assert_equal(expected2,actual2,1e-7)); } +/* ************************************************************************* */ +TEST( StereoCamera, projectCheirality) +{ + // create a Stereo camera + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam(Pose3(), K); + + // point behind the camera + Point3 p(0, 0, -5); +#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION + CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#endif +} + /* ************************************************************************* */ TEST( StereoCamera, backproject_case1) { From 4c07399e3feb0bed47b48d0cd908ecd51e3be82c Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 14 Aug 2016 20:29:36 -0400 Subject: [PATCH 5/6] fixed test according to cheirality flag --- gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index f0a864fdd..fa1f83a75 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -1022,6 +1022,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac /* *************************************************************************/ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) { + // this test considers a condition in which the cheirality constraint is triggered using namespace vanillaPose; vector views; @@ -1096,8 +1097,14 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) // 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 } /* *************************************************************************/ From 79844753b8939e14416047e0adc59d2101487466 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 16 Aug 2016 18:53:19 -0400 Subject: [PATCH 6/6] added another tiny test --- gtsam/geometry/tests/testStereoCamera.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index f76d841bf..bc432cad3 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -115,6 +115,9 @@ TEST( StereoCamera, projectCheirality) Point3 p(0, 0, -5); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION CHECK_EXCEPTION(stereoCam.project2(p), StereoCheiralityException); +#else // otherwise project should not throw the exception + StereoPoint2 expected = StereoPoint2(320, 470, 240); + CHECK(assert_equal(expected,stereoCam.project2(p),1e-7)); #endif }