From 83d0bd414db0bce122271f793473f6564caa2094 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 1 Mar 2015 12:06:16 +0100 Subject: [PATCH] Introduced TriangulationResult --- gtsam/geometry/triangulation.h | 144 ++++++++++++++++++++------------- 1 file changed, 90 insertions(+), 54 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 15721e81c..a67f26bf2 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -316,12 +316,57 @@ struct TriangulationParameters { _landmarkDistanceThreshold), dynamicOutlierRejectionThreshold( _dynamicOutlierRejectionThreshold) { } + + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { + os << "rankTolerance = " << p.rankTolerance << std::endl; + os << "enableEPI = " << p.enableEPI << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; + return os; + } }; -struct TriangulationResult { - Point3 point; - bool degenerate; - bool cheiralityException; +/** + * TriangulationResult is an optional point, along with the reasons why it is invalid. + */ +class TriangulationResult: public boost::optional { + enum Status { + VALID, DEGENERATE, BEHIND_CAMERA + }; + Status status_; + TriangulationResult(Status s) : + status_(s) { + } +public: + TriangulationResult(const Point3& p) : + status_(VALID) { + reset(p); + } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } + static TriangulationResult BehindCamera() { + return TriangulationResult(BEHIND_CAMERA); + } + bool degenerate() const { + return status_ == DEGENERATE; + } + bool behindCamera() const { + return status_ == BEHIND_CAMERA; + } + // stream to output + friend std::ostream &operator<<(std::ostream &os, + const TriangulationResult& result) { + if (result) + os << "point = " << *result << std::endl; + else + os << "no point, status = " << result.status_ << std::endl; + return os; + } }; /// triangulateSafe: extensive checking of the outcome @@ -330,62 +375,53 @@ 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; - } + if (m < 2) + return TriangulationResult::Degenerate(); + else + // We triangulate the 3D position of the landmark + try { + Point3 point = triangulatePoint3(cameras, measured, + params.rankTolerance, params.enableEPI); - // 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 + size_t i = 0; + double totalReprojError = 0.0; + BOOST_FOREACH(const CAMERA& camera, cameras) { + // we discard smart factors corresponding to points that are far away + Point3 cameraTranslation = camera.pose().translation(); + if (cameraTranslation.distance(point) > params.landmarkDistanceThreshold) + return TriangulationResult::Degenerate(); + // Also flag if point is behind any of the cameras + try { + const Point2& zi = measured.at(i); + Point2 reprojectionError(camera.project(point) - zi); + totalReprojError += reprojectionError.vector().norm(); + } catch (CheiralityException) { + return TriangulationResult::BehindCamera(); + } + i += 1; + } + // we discard smart factors that have large reprojection error + if (params.dynamicOutlierRejectionThreshold > 0 + && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Degenerate(); - // 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; + // all good! + return TriangulationResult(point); + } 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 + return TriangulationResult::Degenerate(); + } 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 + return TriangulationResult::BehindCamera(); } - // 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