From 695d080e4dd5f12701423855bacd68a8de34588b Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 12 Mar 2015 07:56:59 -0700 Subject: [PATCH] DegeneracyMode --- gtsam/slam/SmartProjectionFactor.h | 33 +++++---- .../tests/testSmartProjectionCameraFactor.cpp | 42 +++++------ .../tests/testSmartProjectionPoseFactor.cpp | 69 +++++++++++-------- 3 files changed, 84 insertions(+), 60 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 067d30329..7c137ad5c 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -44,13 +44,19 @@ public: HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD }; + /// How to manage degeneracy + enum DegeneracyMode { + IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY + }; + private: typedef SmartFactorBase Base; typedef SmartProjectionFactor This; protected: - LinearizationMode linearizeTo_; ///< How to linearize the factor + LinearizationMode linearizationMode_; ///< How to linearize the factor + DegeneracyMode degeneracyMode_; ///< How to linearize the factor /// @name Caching triangulation /// @{ @@ -63,7 +69,6 @@ protected: /// @name Parameters governing how triangulation result is treated /// @{ - const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false) const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false) /// @} @@ -85,13 +90,16 @@ public: * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations */ SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN, - double rankTolerance = 1, bool manageDegeneracy = false, bool enableEPI = - false, double landmarkDistanceThreshold = 1e10, + double rankTolerance = 1, DegeneracyMode degeneracyMode = + IGNORE_DEGENERACY, bool enableEPI = false, + double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - linearizeTo_(linearizationMode), parameters_(rankTolerance, enableEPI, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), // + linearizationMode_(linearizationMode), // + degeneracyMode_(degeneracyMode), // + parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold, + dynamicOutlierRejectionThreshold), // result_(TriangulationResult::Degenerate()), // - retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), // + retriangulationThreshold_(1e-5), // throwCheirality_(false), verboseCheirality_(false) { } @@ -107,7 +115,7 @@ public: void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { std::cout << s << "SmartProjectionFactor\n"; - std::cout << "linearizationMode:\n" << linearizeTo_ << std::endl; + std::cout << "linearizationMode:\n" << linearizationMode_ << std::endl; std::cout << "triangulationParameters:\n" << parameters_ << std::endl; std::cout << "result:\n" << result_ << std::endl; Base::print("", keyFormatter); @@ -116,7 +124,8 @@ public: /// equals virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const { const This *e = dynamic_cast(&p); - return e && linearizeTo_ == e->linearizeTo_ && Base::equals(p, tol); + return e && linearizationMode_ == e->linearizationMode_ + && Base::equals(p, tol); } /// Check if the new linearization point is the same as the one used for previous triangulation @@ -194,7 +203,7 @@ public: triangulateSafe(cameras); - if (!manageDegeneracy_ && !result_) { + if (degeneracyMode_ == ZERO_ON_DEGENERACY && !result_) { // put in "empty" Hessian BOOST_FOREACH(Matrix& m, Gs) m = zeros(Base::Dim, Base::Dim); @@ -281,7 +290,7 @@ public: const double lambda = 0.0) const { // depending on flag set on construction we may linearize to different linear factors Cameras cameras = this->cameras(values); - switch (linearizeTo_) { + switch (linearizationMode_) { case HESSIAN: return createHessianFactor(cameras, lambda); case IMPLICIT_SCHUR: @@ -381,7 +390,7 @@ public: if (result_) // All good, just use version in base class return Base::totalReprojectionError(cameras, *result_); - else if (manageDegeneracy_) { + else if (degeneracyMode_ == HANDLE_INFINITY) { // Otherwise, manage the exceptions with rotation-only factors const Point2& z0 = this->measured_.at(0); Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0); diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index b5ddc4e25..508182557 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -31,7 +31,6 @@ using namespace boost::assign; static bool isDebugTest = false; static double rankTol = 1.0; -static double linThreshold = -1.0; // Convenience for named keys using symbol_shorthand::X; @@ -79,7 +78,7 @@ TEST( SmartProjectionCameraFactor, Constructor) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { using namespace vanilla; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold); + SmartFactor factor1(SmartFactor::HESSIAN, rankTol); } /* ************************************************************************* */ @@ -92,7 +91,7 @@ TEST( SmartProjectionCameraFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { using namespace vanilla; - SmartFactor factor1(SmartFactor::HESSIAN, rankTol, linThreshold); + SmartFactor factor1(SmartFactor::HESSIAN, rankTol); factor1.add(measurement1, x1, unit2); } @@ -108,7 +107,6 @@ TEST( SmartProjectionCameraFactor, Equals ) { /* *************************************************************************/ TEST( SmartProjectionCameraFactor, noiseless ) { - // cout << " ************************ SmartProjectionCameraFactor: noisy ****************************" << endl; using namespace vanilla; Values values; @@ -153,7 +151,8 @@ TEST( SmartProjectionCameraFactor, noisy ) { // Check triangulated point CHECK(factor1->point()); - EXPECT(assert_equal(Point3(13.7587, 1.43851, -1.14274),*factor1->point(), 1e-4)); + EXPECT( + assert_equal(Point3(13.7587, 1.43851, -1.14274), *factor1->point(), 1e-4)); // Check whitened errors Vector expected(4); @@ -236,16 +235,23 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { CHECK(smartFactor2->point()); CHECK(smartFactor3->point()); - EXPECT(assert_equal(Point3(2.57696, -0.182566, 1.04085),*smartFactor1->point(), 1e-4)); - EXPECT(assert_equal(Point3(2.80114, -0.702153, 1.06594),*smartFactor2->point(), 1e-4)); - EXPECT(assert_equal(Point3(1.82593, -0.289569, 2.13438),*smartFactor3->point(), 1e-4)); + EXPECT( + assert_equal(Point3(2.57696, -0.182566, 1.04085), *smartFactor1->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(2.80114, -0.702153, 1.06594), *smartFactor2->point(), + 1e-4)); + EXPECT( + assert_equal(Point3(1.82593, -0.289569, 2.13438), *smartFactor3->point(), + 1e-4)); // Check whitened errors Vector expected(6); expected << 256, 29, -26, 29, -206, -202; Point3 point1 = *smartFactor1->point(); SmartFactor::Cameras cameras1 = smartFactor1->cameras(initial); - Vector reprojectionError = cameras1.reprojectionError(point1, measurements_cam1); + Vector reprojectionError = cameras1.reprojectionError(point1, + measurements_cam1); EXPECT(assert_equal(expected, reprojectionError, 1)); Vector actual = smartFactor1->whitenedError(cameras1, point1); EXPECT(assert_equal(expected, actual, 1)); @@ -259,9 +265,9 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { LevenbergMarquardtOptimizer optimizer(graph, initial, params); Values result = optimizer.optimize(); - EXPECT(assert_equal(landmark1,*smartFactor1->point(), 1e-7)); - EXPECT(assert_equal(landmark2,*smartFactor2->point(), 1e-7)); - EXPECT(assert_equal(landmark3,*smartFactor3->point(), 1e-7)); + EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7)); + EXPECT(assert_equal(landmark2, *smartFactor2->point(), 1e-7)); + EXPECT(assert_equal(landmark3, *smartFactor3->point(), 1e-7)); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(initial); // VectorValues delta = GFG->optimize(); @@ -796,15 +802,12 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { values.insert(c1, level_camera); values.insert(c2, level_camera_right); double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; bool useEPI = false; - bool isImplicit = false; // Hessian version SmartFactor::shared_ptr explicitFactor( - new SmartFactor(SmartFactor::HESSIAN, rankTol, linThreshold, manageDegeneracy, useEPI, - isImplicit)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::IGNORE_DEGENERACY, useEPI)); explicitFactor->add(level_uv, c1, unit2); explicitFactor->add(level_uv_right, c2, unit2); @@ -814,10 +817,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { dynamic_cast(*gaussianHessianFactor); // Implicit Schur version - isImplicit = true; SmartFactor::shared_ptr implicitFactor( - new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, linThreshold, manageDegeneracy, useEPI, - isImplicit)); + new SmartFactor(SmartFactor::IMPLICIT_SCHUR, rankTol, + SmartFactor::IGNORE_DEGENERACY, useEPI)); implicitFactor->add(level_uv, c1, unit2); implicitFactor->add(level_uv_right, c2, unit2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c77616793..6fb1652b0 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -31,8 +31,6 @@ using namespace boost::assign; static const double rankTol = 1.0; -static const bool manageDegeneracy = true; - // Create a noise model for the pixel error static const double sigma = 0.1; static SharedIsotropic model(noiseModel::Isotropic::Sigma(2, sigma)); @@ -501,15 +499,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -530,7 +528,6 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { values.insert(x3, Camera(pose_above * noise_pose, sharedK)); Values result; - params.verbosityLM = LevenbergMarquardtParams::SUMMARY; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3).pose(), 1e-8)); @@ -556,17 +553,20 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false, + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false, + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, -1, false, false, + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model); @@ -621,23 +621,27 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, + dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, + dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, + dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, false, false, - excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(SmartFactor::JACOBIAN_SVD, 1, + SmartFactor::IGNORE_DEGENERACY, false, excludeLandmarksFutherThanDist, + dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -680,15 +684,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::JACOBIAN_Q, 1, false, false)); + new SmartFactor(SmartFactor::JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -885,11 +889,13 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac double rankTol = 50; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -923,6 +929,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac values.at(x3).pose())); Values result; + params.verbosityLM = LevenbergMarquardtParams::SUMMARY; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); @@ -964,15 +971,18 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1265,15 +1275,18 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { double rankTol = 10; SmartFactor::shared_ptr smartFactor1( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(SmartFactor::HESSIAN, rankTol, manageDegeneracy)); + new SmartFactor(SmartFactor::HESSIAN, rankTol, + SmartFactor::ZERO_ON_DEGENERACY)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);