From d10ffee4d43f63d47654176877fccdd797bf17f4 Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Sat, 28 Sep 2013 00:49:37 +0000 Subject: [PATCH] management of degeneracy --- gtsam_unstable/geometry/triangulation.cpp | 2 +- gtsam_unstable/slam/SmartProjectionFactor.h | 95 +++++++++++-------- .../slam/tests/testSmartProjectionFactor.cpp | 38 ++++---- 3 files changed, 76 insertions(+), 59 deletions(-) diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 3b4bdad17..fa45951be 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; if(rank < 3) throw(TriangulationUnderconstrainedException()); diff --git a/gtsam_unstable/slam/SmartProjectionFactor.h b/gtsam_unstable/slam/SmartProjectionFactor.h index 8bb6c78ab..7232670c4 100644 --- a/gtsam_unstable/slam/SmartProjectionFactor.h +++ b/gtsam_unstable/slam/SmartProjectionFactor.h @@ -34,6 +34,10 @@ namespace gtsam { static double defaultLinThreshold = -1; // 1e-7; // 0.01 // default threshold for retriangulation static double defaultTriangThreshold = 1e-7; + // 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 @@ -327,6 +331,13 @@ namespace gtsam { cameraPoses.push_back(cameraPose); } + if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative + state_->degenerate = true; + BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); + BOOST_FOREACH(Vector& v, gs) v = zero(6); + return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed + } + bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold); if(retriangulate) {// we store the current poses used for triangulation @@ -336,39 +347,34 @@ namespace gtsam { if (retriangulate) { // We triangulate the 3D position of the landmark try { - Point3 newPoint = triangulatePoint3(cameraPoses, measured_, *K_); - // changeLinPoint = newPoint - state_->point; // TODO: implement this check for the degenerate case - state_->point = newPoint; + state_->point = triangulatePoint3(cameraPoses, measured_, *K_, rankTolerance); state_->degenerate = false; state_->cheiralityException = false; } catch( TriangulationUnderconstrainedException& e) { - // point is triangulated at infinity - //std::cout << "Triangulation failed " << e.what() << std::endl; - BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); - BOOST_FOREACH(Vector& v, gs) v = zero(6); + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + //std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity state_->degenerate = true; state_->cheiralityException = false; - dim_landmark = 2; - return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed } catch( TriangulationCheiralityException& e) { - // point is behind one of the cameras, turn factor off by setting everything to 0 + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint //std::cout << e.what() << std::end; - BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); - BOOST_FOREACH(Vector& v, gs) v = zero(6); - state_->cheiralityException = true; // TODO: Debug condition, uncomment when fixed - return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); // TODO: Debug condition, uncomment when fixed - // TODO: this is a debug condition, should be removed the comment + state_->cheiralityException = true; } } -// state_->degenerate = true; // TODO: this is a debug condition, should be removed -// dim_landmark = 2; // TODO: this is a debug condition, should be removed the comment - if (!retriangulate && state_->cheiralityException) { + if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){ + std::cout << "In linearize: exception" << std::endl; BOOST_FOREACH(gtsam::Matrix& m, Gs) m = zeros(6, 6); BOOST_FOREACH(Vector& v, gs) v = zero(6); return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } - if (!retriangulate && state_->degenerate) { + + if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors + state_->degenerate = true; dim_landmark = 2; } @@ -388,8 +394,8 @@ namespace gtsam { if(!doLinearize){ // return the previous Hessian factor return HessianFactor::shared_ptr(new HessianFactor(keys_, state_->Gs, state_->gs, state_->f)); } - //otherwise redo linearization + //otherwise redo linearization if (blockwise){ // ========================================================================================================== std::cout << "Deprecated use of blockwise version. This is slower and no longer supported" << std::endl; @@ -498,6 +504,7 @@ namespace gtsam { subInsert(b2,bi,2*i); } + } // Shur complement trick @@ -524,12 +531,12 @@ namespace gtsam { } // ========================================================================================================== -// if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables -// state_->calculatedHessian = true; -// state_->Gs = Gs; -// state_->gs = gs; -// state_->f = f; -// } + if(linearizationThreshold >= 0){ // if we do not use selective relinearization we don't need to store these variables + state_->calculatedHessian = true; + state_->Gs = Gs; + state_->gs = gs; + state_->f = f; + } return HessianFactor::shared_ptr(new HessianFactor(keys_, Gs, gs, f)); } @@ -553,37 +560,47 @@ namespace gtsam { cameraPoses.push_back(cameraPose); } + if(cameraPoses.size() < 2){ // if we have a single pose the corresponding factor is uninformative + return 0.0; + } + bool retriangulate = decideIfTriangulate(cameraPoses, state_->cameraPosesTriangulation, retriangulationThreshold); if(retriangulate) {// we store the current poses used for triangulation state_->cameraPosesTriangulation = cameraPoses; } - // We triangulate the 3D position of the landmark if (retriangulate) { + // We triangulate the 3D position of the landmark try { - state_->point = triangulatePoint3(cameraPoses, measured_, *K_); + state_->point = triangulatePoint3(cameraPoses, measured_, *K_, rankTolerance); state_->degenerate = false; state_->cheiralityException = false; - } catch( TriangulationCheiralityException& e) { - // std::cout << "TriangulationCheiralityException " << std::endl; - // point is behind one of the cameras, turn factor off by setting everything to 0 - //std::cout << e.what() << std::end; - state_->cheiralityException = true; // TODO: Debug condition, remove comment - return 0.0; // TODO: this is a debug condition, should be removed the comment } catch( TriangulationUnderconstrainedException& e) { - // point is triangulated at infinity - //std::cout << e.what() << std::endl; + // if TriangulationUnderconstrainedException can be + // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before + // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) + // in the second case we want to use a rotation-only smart factor + //std::cout << "Triangulation failed " << e.what() << std::endl; // point triangulated at infinity state_->degenerate = true; state_->cheiralityException = false; + } catch( TriangulationCheiralityException& e) { + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint + //std::cout << e.what() << std::end; + state_->cheiralityException = true; } } - // state_->degenerate = true; // TODO: this is a debug condition, should be removed - if (!retriangulate && state_->cheiralityException) { + if (!manageDegeneracy && (state_->cheiralityException || state_->degenerate) ){ // if we want to manage the exceptions with rotation-only factors + std::cout << "In error evaluation: exception" << std::endl; return 0.0; } + if (state_->cheiralityException || state_->degenerate){ // if we want to manage the exceptions with rotation-only factors + state_->degenerate = true; + } + if(state_->degenerate){ for(size_t i = 0; i < measured_.size(); i++) { Pose3 pose = cameraPoses.at(i); @@ -604,7 +621,7 @@ namespace gtsam { try { Point2 reprojectionError(camera.project(state_->point) - measured_.at(i)); -//std::cout << "Reprojection error: " << reprojectionError << std::endl; + //std::cout << "Reprojection error: " << reprojectionError << std::endl; overallError += 0.5 * noise_->distance( reprojectionError.vector() ); //overallError += reprojectionError.vector().norm(); } catch ( CheiralityException& e) { diff --git a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp index f493316c1..a9407e854 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp @@ -120,7 +120,7 @@ TEST( SmartProjectionFactor, EqualsWithTransform ) { } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, noisy ){ cout << " ************************ SmartProjectionFactor: noisy ****************************" << endl; @@ -159,7 +159,7 @@ TEST( SmartProjectionFactor, noisy ){ measurements += level_uv, level_uv_right; SmartProjectionFactor::shared_ptr - smartFactor(new SmartProjectionFactor(measurements, noiseProjection, views, K)); + smartFactor(new SmartProjectionFactor(views, measurements, noiseProjection, K)); double actualError = smartFactor->error(values); std::cout << "Error: " << actualError << std::endl; @@ -224,9 +224,9 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ typedef SmartProjectionFactor SmartFactor; typedef GenericProjectionFactor ProjectionFactor; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K, boost::make_optional(landmark1) )); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K, boost::make_optional(landmark2) )); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K, boost::make_optional(landmark3) )); + SmartFactor::shared_ptr smartFactor1(new SmartFactor(views, measurements_cam1, noiseProjection, K, boost::make_optional(landmark1) )); + SmartFactor::shared_ptr smartFactor2(new SmartFactor(views, measurements_cam2, noiseProjection, K, boost::make_optional(landmark2) )); + SmartFactor::shared_ptr smartFactor3(new SmartFactor(views, measurements_cam3, noiseProjection, K, boost::make_optional(landmark3) )); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -311,7 +311,7 @@ TEST( SmartProjectionFactor, 3poses_1iteration_projection_factor_comparison ){ } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; @@ -365,9 +365,9 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K)); + 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)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -405,7 +405,7 @@ TEST( SmartProjectionFactor, 3poses_smart_projection_factor ){ } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks **********************" << endl; @@ -510,7 +510,7 @@ TEST( SmartProjectionFactor, 3poses_iterative_smart_projection_factor ){ } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_projection_factor ){ // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; @@ -583,7 +583,7 @@ TEST( SmartProjectionFactor, 3poses_projection_factor ){ } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor ){ cout << " ************************ SmartProjectionFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl; @@ -670,7 +670,7 @@ TEST( SmartProjectionFactor, 3poses_2land_rotation_only_smart_projection_factor tictoc_print_(); } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ cout << " ************************ SmartProjectionFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl; @@ -724,9 +724,9 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ typedef SmartProjectionFactor SmartFactor; - SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K)); - SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K)); + 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)); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10); @@ -767,7 +767,7 @@ TEST( SmartProjectionFactor, 3poses_rotation_only_smart_projection_factor ){ } -/* ************************************************************************* +/* *************************************************************************/ TEST( SmartProjectionFactor, Hessian ){ cout << " ************************ SmartProjectionFactor: Hessian **********************" << endl; @@ -797,7 +797,7 @@ TEST( SmartProjectionFactor, Hessian ){ vector measurements_cam1; measurements_cam1 += cam1_uv1, cam2_uv1; - SmartProjectionFactor smartFactor(measurements_cam1, noiseProjection, views, K); + SmartProjectionFactor smartFactor(views, measurements_cam1, noiseProjection, K); Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); Values values; @@ -806,7 +806,7 @@ TEST( SmartProjectionFactor, Hessian ){ // values.insert(L(1), landmark1); boost::shared_ptr hessianFactor = smartFactor.linearize(values); - hessianFactor->print("Hessian factor \n"); + // hessianFactor->print("Hessian factor \n"); // compute triangulation from linearization point // compute reprojection errors (sum squared)