diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 369c54bea..474244550 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -382,7 +382,7 @@ private: */ class TriangulationResult: public boost::optional { enum Status { - VALID, DEGENERATE, BEHIND_CAMERA + VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status_; TriangulationResult(Status s) : @@ -405,12 +405,27 @@ public: static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } + static TriangulationResult Outlier() { + return TriangulationResult(OUTLIER); + } + static TriangulationResult FarPoint() { + return TriangulationResult(FAR_POINT); + } static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); } + bool valid() const { + return status_ == VALID; + } bool degenerate() const { return status_ == DEGENERATE; } + bool outlier() const { + return status_ == OUTLIER; + } + bool farPoint() const { + return status_ == FAR_POINT; + } bool behindCamera() const { return status_ == BEHIND_CAMERA; } @@ -459,7 +474,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.landmarkDistanceThreshold > 0 && distance(pose.translation(), point) > params.landmarkDistanceThreshold) - return TriangulationResult::Degenerate(); + return TriangulationResult::FarPoint(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // Only needed if this was not yet handled by exception @@ -478,7 +493,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, // Flag as degenerate if average reprojection error is too large if (params.dynamicOutlierRejectionThreshold > 0 && totalReprojError / m > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Degenerate(); + return TriangulationResult::Outlier(); // all good! return TriangulationResult(point);