diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 86b7734f7..f57ca60d3 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -246,6 +246,59 @@ TEST( triangulation, fourPoses_distinct_Ks) { #endif } +//****************************************************************************** +TEST( triangulation, outliersAndFarLandmarks) { + Cal3_S2 K1(1500, 1200, 0, 640, 480); + // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) + SimpleCamera camera1(pose1, K1); + + // create second camera 1 meter to the right of first camera + Cal3_S2 K2(1600, 1300, 0, 650, 440); + SimpleCamera camera2(pose2, K2); + + // 1. Project two landmarks into two cameras and triangulate + Point2 z1 = camera1.project(landmark); + Point2 z2 = camera2.project(landmark); + + vector cameras; + vector measurements; + + cameras += camera1, camera2; + measurements += z1, z2; + + double landmarkDistanceThreshold = 10; // landmark is closer than that + TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold + TriangulationResult actual = triangulateSafe(cameras,measurements,params); + EXPECT(assert_equal(landmark, *actual, 1e-2)); + EXPECT(actual.valid()); + + landmarkDistanceThreshold = 4; // landmark is farther than that + TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold + actual = triangulateSafe(cameras,measurements,params2); + EXPECT(actual.farPoint()); + + // 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) + Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); + Cal3_S2 K3(700, 500, 0, 640, 480); + SimpleCamera camera3(pose3, K3); + Point2 z3 = camera3.project(landmark); + + cameras += camera3; + measurements += z3 + Point2(10, -10); + + landmarkDistanceThreshold = 10; // landmark is closer than that + double outlierThreshold = 100; // loose, the outlier is going to pass + TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold); + actual = triangulateSafe(cameras,measurements,params3); + EXPECT(actual.valid()); + + // now set stricter threshold for outlier rejection + outlierThreshold = 5; // tighter, the outlier is not going to pass + TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold); + actual = triangulateSafe(cameras,measurements,params4); + EXPECT(actual.outlier()); +} + //****************************************************************************** TEST( triangulation, twoIdenticalPoses) { // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 880c23fd1..81c8ed2f3 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -322,6 +322,7 @@ Point3 triangulatePoint3( struct TriangulationParameters { double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) bool enableEPI; ///< if set to true, will refine triangulation using LM /** @@ -383,7 +384,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) : @@ -406,12 +407,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; } @@ -454,13 +470,13 @@ TriangulationResult triangulateSafe(const std::vector& cameras, // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; - double totalReprojError = 0.0; + double maxReprojError = 0.0; for(const CAMERA& camera: cameras) { const Pose3& pose = camera.pose(); if (params.landmarkDistanceThreshold > 0 && distance3(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 @@ -472,14 +488,14 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.dynamicOutlierRejectionThreshold > 0) { const Point2& zi = measured.at(i); Point2 reprojectionError(camera.project(point) - zi); - totalReprojError += reprojectionError.norm(); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); } i += 1; } // Flag as degenerate if average reprojection error is too large if (params.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params.dynamicOutlierRejectionThreshold) - return TriangulationResult::Degenerate(); + && maxReprojError > params.dynamicOutlierRejectionThreshold) + return TriangulationResult::Outlier(); // all good! return TriangulationResult(point); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 64b300b86..d34ba11e3 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -64,9 +64,9 @@ struct GTSAM_EXPORT SmartProjectionParams { // Constructor SmartProjectionParams(LinearizationMode linMode = HESSIAN, DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false, - bool verboseCheirality = false) : + bool verboseCheirality = false, double retriangulationTh = 1e-5) : linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold( - 1e-5), throwCheirality(throwCheirality), verboseCheirality( + retriangulationTh), throwCheirality(throwCheirality), verboseCheirality( verboseCheirality) { } @@ -94,9 +94,15 @@ struct GTSAM_EXPORT SmartProjectionParams { bool getThrowCheirality() const { return throwCheirality; } + double getRetriangulationThreshold() const { + return retriangulationThreshold; + } void setLinearizationMode(LinearizationMode linMode) { linearizationMode = linMode; } + void setRetriangulationThreshold(double retriangulationTh) { + retriangulationThreshold = retriangulationTh; + } void setDegeneracyMode(DegeneracyMode degMode) { degeneracyMode = degMode; } @@ -528,19 +534,19 @@ public: } /// Is result valid? - bool isValid() const { - return result_; - } + bool isValid() const { return result_.valid(); } /** return the degenerate state */ - bool isDegenerate() const { - return result_.degenerate(); - } + bool isDegenerate() const { return result_.degenerate(); } /** return the cheirality status flag */ - bool isPointBehindCamera() const { - return result_.behindCamera(); - } + bool isPointBehindCamera() const { return result_.behindCamera(); } + + /** return the outlier state */ + bool isOutlier() const { return result_.outlier(); } + + /** return the farPoint state */ + bool isFarPoint() const { return result_.farPoint(); } private: diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index ace75f80f..f0a864fdd 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -81,6 +81,17 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1); } +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactor, params) { + using namespace vanillaPose; + SmartProjectionParams params; + double rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-5, rt, 1e-7); + params.setRetriangulationThreshold(1e-3); + rt = params.getRetriangulationThreshold(); + EXPECT_DOUBLES_EQUAL(1e-3, rt, 1e-7); +} + /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index bede012d8..21b0c5eb7 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -274,7 +274,7 @@ public: // check landmark distance if (params_.triangulation.landmarkDistanceThreshold > 0 && pl.norm() > params_.triangulation.landmarkDistanceThreshold) { - result_ = TriangulationResult::Degenerate(); + result_ = TriangulationResult::FarPoint(); return result_; } @@ -287,7 +287,7 @@ public: if (params_.triangulation.dynamicOutlierRejectionThreshold > 0 && totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) { - result_ = TriangulationResult::Degenerate(); + result_ = TriangulationResult::Outlier(); return result_; } @@ -582,19 +582,19 @@ public: } /// Is result valid? - bool isValid() const { - return bool(result_); - } + bool isValid() const { return result_.valid(); } /** return the degenerate state */ - bool isDegenerate() const { - return result_.degenerate(); - } + bool isDegenerate() const { return result_.degenerate(); } /** return the cheirality status flag */ - bool isPointBehindCamera() const { - return result_.behindCamera(); - } + bool isPointBehindCamera() const { return result_.behindCamera(); } + + /** return the outlier state */ + bool isOutlier() const { return result_.outlier(); } + + /** return the farPoint state */ + bool isFarPoint() const { return result_.farPoint(); } private: diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 5bad0e171..8051e238a 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -568,7 +568,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { EXPECT(smartFactor1->point()); EXPECT(smartFactor2->point()); EXPECT(smartFactor3->point()); - EXPECT(smartFactor4->point().degenerate()); + EXPECT(smartFactor4->point().outlier()); EXPECT(smartFactor4b->point()); // Factor 4 is disabled, pose 3 stays put