From 4f37edaeb9a7c5fc31d118452080f88daffa571d Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 14:42:25 -0400 Subject: [PATCH] added tests to check correct use of landmarkDistanceThreshold and dynamicOutlierRejectionThreshold --- gtsam/geometry/tests/testTriangulation.cpp | 53 ++++++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index c3df95abc..a61456441 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)