diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 37489355d..5101e9fc8 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -13,8 +13,6 @@ * @file PinholeSet.h * @brief A CameraSet of either CalibratedCamera, PinholePose, or PinholeCamera * @author Frank Dellaert - * @author Luca Carlone - * @author Zsolt Kira */ #pragma once @@ -38,50 +36,8 @@ private: protected: - /// @name Triangulation parameters - /// @{ - - const double rankTolerance_; ///< threshold to decide whether triangulation is result.degenerate - const bool enableEPI_; ///< if set to true, will refine triangulation using LM - - /** - * if the landmark is triangulated at distance larger than this, - * result is flagged as degenerate. - */ - const double landmarkDistanceThreshold_; // - - /** - * If this is nonnegative the we will check if the average reprojection error - * is smaller than this threshold after triangulation, otherwise result is - * flagged as degenerate. - */ - const double dynamicOutlierRejectionThreshold_; - /// @} - public: - /// @name Triangulation result - /// @{ - struct Result { - Point3 point; - bool degenerate; - bool cheiralityException; - }; - /// @} - - /** - * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - */ - PinholeSet(const double rankTol = 1.0, const bool enableEPI = false, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - rankTolerance_(rankTol), enableEPI_(enableEPI), landmarkDistanceThreshold_( - landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( - dynamicOutlierRejectionThreshold) { - } - /** Virtual destructor */ virtual ~PinholeSet() { } @@ -92,8 +48,6 @@ public: /// print virtual void print(const std::string& s = "") const { Base::print(s); - std::cout << s << "PinholeSet\n"; - std::cout << "rankTolerance = " << rankTolerance_ << std::endl; } /// equals @@ -104,64 +58,10 @@ public: /// @} /// triangulateSafe - Result triangulateSafe(const std::vector& measured) const { - - Result result; - - size_t m = this->size(); - - // if we have a single pose the corresponding factor is uninformative - if (m < 2) { - result.degenerate = true; - return result; - } - - // We triangulate the 3D position of the landmark - try { - // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - result.point = triangulatePoint3(*this, measured, rankTolerance_, - enableEPI_); - result.degenerate = false; - result.cheiralityException = false; - - // Check landmark distance and reprojection errors to avoid outliers - double totalReprojError = 0.0; - size_t i = 0; - BOOST_FOREACH(const CAMERA& camera, *this) { - Point3 cameraTranslation = camera.pose().translation(); - // we discard smart factors corresponding to points that are far away - if (cameraTranslation.distance(result.point) - > landmarkDistanceThreshold_) { - result.degenerate = true; - break; - } - const Point2& zi = measured.at(i); - try { - Point2 reprojectionError(camera.project(result.point) - zi); - totalReprojError += reprojectionError.vector().norm(); - } catch (CheiralityException) { - result.cheiralityException = true; - } - i += 1; - } - // we discard smart factors that have large reprojection error - if (dynamicOutlierRejectionThreshold_ > 0 - && totalReprojError / m > dynamicOutlierRejectionThreshold_) - result.degenerate = true; - - } catch (TriangulationUnderconstrainedException&) { - // 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 *this (or motion towards the landmark) - // in the second case we want to use a rotation-only smart factor - result.degenerate = true; - result.cheiralityException = false; - } catch (TriangulationCheiralityException&) { - // point is behind one of the *this: can be the case of close-to-parallel *this or may depend on outliers - // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - result.cheiralityException = true; - } - return result; + TriangulationResult triangulateSafe( + const std::vector& measured, + const TriangulationParameters& params) const { + return gtsam::triangulateSafe(*this, measured, params); } private: diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index b4b065802..5de2b5f4d 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -127,7 +127,8 @@ TEST(PinholeSet, Pinhole) { EXPECT(assert_equal(expectedV, actualV)); // Instantiate triangulateSafe - PinholeSet::Result actual = set.triangulateSafe(z); + TriangulationParameters params; + TriangulationResult actual = set.triangulateSafe(z,params); CHECK(actual.degenerate); } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 5b8be0168..15721e81c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -221,7 +221,7 @@ Point3 triangulatePoint3(const std::vector& poses, BOOST_FOREACH(const Pose3& pose, poses) { const Point3& p_local = pose.transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -269,7 +269,7 @@ Point3 triangulatePoint3(const std::vector& cameras, BOOST_FOREACH(const CAMERA& camera, cameras) { const Point3& p_local = camera.pose().transform_to(point); if (p_local.z() <= 0) - throw(TriangulationCheiralityException()); + throw(TriangulationCheiralityException()); } #endif @@ -286,5 +286,107 @@ Point3 triangulatePoint3( (cameras, measurements, rank_tol, optimize); } +struct TriangulationParameters { + + const double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + const bool enableEPI; ///< if set to true, will refine triangulation using LM + + /** + * if the landmark is triangulated at distance larger than this, + * result is flagged as degenerate. + */ + const double landmarkDistanceThreshold; // + + /** + * If this is nonnegative the we will check if the average reprojection error + * is smaller than this threshold after triangulation, otherwise result is + * flagged as degenerate. + */ + const double dynamicOutlierRejectionThreshold; + + /** + * Constructor + * @param rankTol tolerance used to check if point triangulation is degenerate + * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations + */ + TriangulationParameters(const double _rankTolerance = 1.0, + const bool _enableEPI = false, double _landmarkDistanceThreshold = 1e10, + double _dynamicOutlierRejectionThreshold = -1) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), landmarkDistanceThreshold( + _landmarkDistanceThreshold), dynamicOutlierRejectionThreshold( + _dynamicOutlierRejectionThreshold) { + } +}; + +struct TriangulationResult { + Point3 point; + bool degenerate; + bool cheiralityException; +}; + +/// triangulateSafe: extensive checking of the outcome +template +TriangulationResult triangulateSafe(const std::vector& cameras, + const std::vector& measured, + const TriangulationParameters& params) { + + TriangulationResult result; + + size_t m = cameras.size(); + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) { + result.degenerate = true; + return result; + } + + // We triangulate the 3D position of the landmark + try { + // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; + result.point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); + result.degenerate = false; + result.cheiralityException = false; + + // Check landmark distance and reprojection errors to avoid outliers + double totalReprojError = 0.0; + size_t i = 0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + Point3 cameraTranslation = camera.pose().translation(); + // we discard smart factors corresponding to points that are far away + if (cameraTranslation.distance(result.point) + > params.landmarkDistanceThreshold) { + result.degenerate = true; + break; + } + const Point2& zi = measured.at(i); + try { + Point2 reprojectionError(camera.project(result.point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + result.cheiralityException = true; + } + i += 1; + } + // we discard smart factors that have large reprojection error + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + result.degenerate = true; + + } catch (TriangulationUnderconstrainedException&) { + // 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 + result.degenerate = true; + result.cheiralityException = false; + } catch (TriangulationCheiralityException&) { + // 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 + result.cheiralityException = true; + } + return result; +} + } // \namespace gtsam