diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index fa45951be..76c1ecde9 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -46,7 +46,7 @@ Point3 triangulateDLT(const vector& projection_matrices, double error; Vector v; boost::tie(rank, error, v) = DLT(A, rank_tol); - // std::cout << "s " << s << std:endl; + // std::cout << "s " << s.transpose() << std:endl; if(rank < 3) throw(TriangulationUnderconstrainedException()); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 7232670c4..30b9ccead 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -34,10 +34,10 @@ namespace gtsam { static double defaultLinThreshold = -1; // 1e-7; // 0.01 // default threshold for retriangulation static double defaultTriangThreshold = 1e-7; + // default threshold for rank deficient triangulation + static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in presence of larger noise // if set to true will use the rotation-only version for degenerate cases static bool manageDegeneracy = true; - // if set to true will use the rotation-only version for degenerate cases - static double rankTolerance = 50; /** * Structure for storing some state memory, used to speed up optimization @@ -100,6 +100,9 @@ namespace gtsam { boost::shared_ptr K_; ///< shared pointer to calibration object double retriangulationThreshold; ///< threshold to decide whether to re-triangulate + + double rankTolerance; ///< threshold to decide whether triangulation is degenerate + double linearizationThreshold; ///< threshold to decide whether to re-linearize boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame @@ -124,7 +127,9 @@ namespace gtsam { typedef boost::shared_ptr SmartFactorStatePtr; /// Default constructor - SmartProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {} + SmartProjectionFactor() : retriangulationThreshold(defaultTriangThreshold), + rankTolerance(defaultRankTolerance), linearizationThreshold(defaultLinThreshold), + throwCheirality_(false), verboseCheirality_(false) {} /** * Constructor @@ -141,8 +146,8 @@ namespace gtsam { boost::optional body_P_sensor = boost::none, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(defaultLinThreshold), - body_P_sensor_(body_P_sensor), + retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), + linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(false), verboseCheirality_(false) { keys_.assign(poseKeys.begin(), poseKeys.end()); } @@ -159,12 +164,13 @@ namespace gtsam { const std::vector measured, // pixel measurements const SharedNoiseModel& model, // noise model (same for all measurements) const boost::shared_ptr& K, // calibration matrix (same for all measurements) - const double linThreshold, + const double rankTol, + const double linThreshold = defaultLinThreshold, boost::optional body_P_sensor = boost::none, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(linThreshold), - body_P_sensor_(body_P_sensor), + retriangulationThreshold(defaultTriangThreshold), rankTolerance(rankTol), + linearizationThreshold(linThreshold), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(false), verboseCheirality_(false) { keys_.assign(poseKeys.begin(), poseKeys.end()); } @@ -188,8 +194,8 @@ namespace gtsam { boost::optional body_P_sensor = boost::none, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : measured_(measured), noise_(model), K_(K), - retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(defaultLinThreshold), - body_P_sensor_(body_P_sensor), + retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), + linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), state_(state), throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) { } @@ -201,8 +207,8 @@ namespace gtsam { SmartProjectionFactor(const SharedNoiseModel& model, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - noise_(model), K_(K), retriangulationThreshold(defaultTriangThreshold), linearizationThreshold(defaultLinThreshold), - body_P_sensor_(body_P_sensor), state_(state) { + noise_(model), K_(K), retriangulationThreshold(defaultTriangThreshold), rankTolerance(defaultRankTolerance), + linearizationThreshold(defaultLinThreshold), body_P_sensor_(body_P_sensor), state_(state) { } /** Virtual destructor */ diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index a9407e854..71b9c0954 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -631,8 +631,10 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K)); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K)); + double rankTol = 50; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K, rankTol)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K, rankTol)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); @@ -724,9 +726,11 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K)); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K)); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K)); + double rankTol = 10; + + SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K, rankTol)); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K, rankTol)); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K, rankTol)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);