From 43a1e290b3a51eed98584897485a849f42b3bd04 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sun, 29 May 2016 23:32:30 -0400 Subject: [PATCH 01/12] added working SFM smart factor example --- .../SFMExample_SmartProjectionPoseFactor.cpp | 132 ++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 examples/SFMExample_SmartProjectionPoseFactor.cpp diff --git a/examples/SFMExample_SmartProjectionPoseFactor.cpp b/examples/SFMExample_SmartProjectionPoseFactor.cpp new file mode 100644 index 000000000..794bd3901 --- /dev/null +++ b/examples/SFMExample_SmartProjectionPoseFactor.cpp @@ -0,0 +1,132 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file SFMExample_SmartFactor.cpp + * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor + * @author Luca Carlone + * @author Frank Dellaert + */ + +// In GTSAM, measurement functions are represented as 'factors'. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. +#include + +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" +#include + +using namespace std; +using namespace gtsam; + +// Make the typename short so it looks much cleaner +typedef SmartProjectionPoseFactor SmartFactor; + +// create a typedef to the camera type +typedef PinholePose Camera; + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Define the camera calibration parameters + Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); + + // Define the camera observation noise model + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + + // Create the set of ground-truth landmarks and poses + vector points = createPoints(); + vector poses = createPoses(); + + // Create a factor graph + NonlinearFactorGraph graph; + + // Simulated measurements from each camera pose, adding them to the factor graph + for (size_t j = 0; j < points.size(); ++j) { + + // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. + SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); + + // for each measurement of landmark j + for (size_t i = 0; i < poses.size(); ++i) { + + // generate the 2D measurement + Camera camera(poses[i], K); + Point2 measurement = camera.project(points[j]); + + // call add() function to add measurement into a single factor, here we need to add: + // 1. the 2D measurement + // 2. the corresponding camera's key + // 3. camera noise model + // 4. camera calibration + smartfactor->add(measurement, i); + } + + // insert the smart factor in the graph + graph.push_back(smartfactor); + } + + // Add a prior on pose x0. This indirectly specifies where the origin is. + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3)).finished()); + graph.push_back(PriorFactor(0, poses[0], noise)); + + // Because the structure-from-motion problem has a scale ambiguity, the problem is + // still under-constrained. Here we add a prior on the second pose x1, so this will + // fix the scale by indicating the distance between x0 and x1. + // Because these two are fixed, the rest of the poses will be also be fixed. + graph.push_back(PriorFactor(1, poses[1], noise)); // add directly to graph + + graph.print("Factor Graph:\n"); + + // Create the initial estimate to the solution + // Intentionally initialize the variables off from the ground truth + Values initialEstimate; + Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); + for (size_t i = 0; i < poses.size(); ++i) + initialEstimate.insert(i, poses[i].compose(delta)); + initialEstimate.print("Initial Estimates:\n"); + + // Optimize the graph and print results + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); + Values result = optimizer.optimize(); + result.print("Final results:\n"); + + // A smart factor represent the 3D point inside the factor, not as a variable. + // The 3D position of the landmark is not explicitly calculated by the optimizer. + // To obtain the landmark's 3D position, we use the "point" method of the smart factor. + Values landmark_result; + for (size_t j = 0; j < points.size(); ++j) { + + // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first + SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); + if (smart) { + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. + boost::optional point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } + } + + landmark_result.print("Landmark results:\n"); + cout << "final error: " << graph.error(result) << endl; + cout << "number of iterations: " << optimizer.iterations() << endl; + + return 0; +} +/* ************************************************************************* */ + From eb9cda7c92671442f3db061006c98cb12e83cc58 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 1 Jun 2016 11:30:47 -0400 Subject: [PATCH 02/12] exposed retriangulationThreshold in SmartProjectionParams constructor --- gtsam/slam/SmartProjectionFactor.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index af7ca64c6..9e3feb238 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; } From 66d42a336fcec9e5c90bb61a99e65252a31f80fa Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 29 Jun 2016 12:26:21 -0400 Subject: [PATCH 03/12] adding 2 more status items for triangulation results: outliers (dynamicOutlierRejection triggered) and farPoint (maxLandmarkDistance triggered) --- gtsam/geometry/triangulation.h | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) 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); From 245e802959f238a113ad1d5863122714f7236425 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 29 Jun 2016 12:34:18 -0400 Subject: [PATCH 04/12] added comment on the function of rankTolerance --- gtsam/geometry/triangulation.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 474244550..29f87c215 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -321,6 +321,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 /** From 03fac5cd02c8fe9507bef1114fc70aedaf96aba7 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Wed, 13 Jul 2016 17:04:23 -0400 Subject: [PATCH 05/12] using max reprojection error (rather than average) for outlier rejection during triangulation --- gtsam/geometry/triangulation.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 29f87c215..73eb9063b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -469,7 +469,7 @@ 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 @@ -487,13 +487,13 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.dynamicOutlierRejectionThreshold > 0) { const Point2& zi = measured.at(i); Point2 reprojectionError(camera.project(point) - zi); - totalReprojError += reprojectionError.vector().norm(); + maxReprojError = std::max(maxReprojError, reprojectionError.vector().norm()); } i += 1; } // Flag as degenerate if average reprojection error is too large if (params.dynamicOutlierRejectionThreshold > 0 - && totalReprojError / m > params.dynamicOutlierRejectionThreshold) + && maxReprojError > params.dynamicOutlierRejectionThreshold) return TriangulationResult::Outlier(); // all good! From 97712b39ccb89813f561a95138fe77dedac1f796 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 14:00:31 -0400 Subject: [PATCH 06/12] added unit test for get/set triangulation threshold --- gtsam/slam/tests/testSmartProjectionPoseFactor.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) 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; From 4f37edaeb9a7c5fc31d118452080f88daffa571d Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 14:42:25 -0400 Subject: [PATCH 07/12] 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) From 5e9dfdd0b65316e7ac576adfbdf6a3cf6c436a3f Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 24 Jul 2016 14:57:59 -0400 Subject: [PATCH 08/12] deleted example (that I added at some point) since it is identical to SFMExample_SMartFactor --- .../SFMExample_SmartProjectionPoseFactor.cpp | 132 ------------------ 1 file changed, 132 deletions(-) delete mode 100644 examples/SFMExample_SmartProjectionPoseFactor.cpp diff --git a/examples/SFMExample_SmartProjectionPoseFactor.cpp b/examples/SFMExample_SmartProjectionPoseFactor.cpp deleted file mode 100644 index 794bd3901..000000000 --- a/examples/SFMExample_SmartProjectionPoseFactor.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file SFMExample_SmartFactor.cpp - * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor - * @author Luca Carlone - * @author Frank Dellaert - */ - -// In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. -// Every smart factor represent a single landmark, seen from multiple cameras. -// The SmartProjectionPoseFactor only optimizes for the poses of a camera, -// not the calibration, which is assumed known. -#include - -// For an explanation of these headers, see SFMExample.cpp -#include "SFMdata.h" -#include - -using namespace std; -using namespace gtsam; - -// Make the typename short so it looks much cleaner -typedef SmartProjectionPoseFactor SmartFactor; - -// create a typedef to the camera type -typedef PinholePose Camera; - -/* ************************************************************************* */ -int main(int argc, char* argv[]) { - - // Define the camera calibration parameters - Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); - - // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = - noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - - // Create the set of ground-truth landmarks and poses - vector points = createPoints(); - vector poses = createPoses(); - - // Create a factor graph - NonlinearFactorGraph graph; - - // Simulated measurements from each camera pose, adding them to the factor graph - for (size_t j = 0; j < points.size(); ++j) { - - // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. - SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K)); - - // for each measurement of landmark j - for (size_t i = 0; i < poses.size(); ++i) { - - // generate the 2D measurement - Camera camera(poses[i], K); - Point2 measurement = camera.project(points[j]); - - // call add() function to add measurement into a single factor, here we need to add: - // 1. the 2D measurement - // 2. the corresponding camera's key - // 3. camera noise model - // 4. camera calibration - smartfactor->add(measurement, i); - } - - // insert the smart factor in the graph - graph.push_back(smartfactor); - } - - // Add a prior on pose x0. This indirectly specifies where the origin is. - // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas( - (Vector(6) << Vector3::Constant(0.3)).finished()); - graph.push_back(PriorFactor(0, poses[0], noise)); - - // Because the structure-from-motion problem has a scale ambiguity, the problem is - // still under-constrained. Here we add a prior on the second pose x1, so this will - // fix the scale by indicating the distance between x0 and x1. - // Because these two are fixed, the rest of the poses will be also be fixed. - graph.push_back(PriorFactor(1, poses[1], noise)); // add directly to graph - - graph.print("Factor Graph:\n"); - - // Create the initial estimate to the solution - // Intentionally initialize the variables off from the ground truth - Values initialEstimate; - Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); - for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(i, poses[i].compose(delta)); - initialEstimate.print("Initial Estimates:\n"); - - // Optimize the graph and print results - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate); - Values result = optimizer.optimize(); - result.print("Final results:\n"); - - // A smart factor represent the 3D point inside the factor, not as a variable. - // The 3D position of the landmark is not explicitly calculated by the optimizer. - // To obtain the landmark's 3D position, we use the "point" method of the smart factor. - Values landmark_result; - for (size_t j = 0; j < points.size(); ++j) { - - // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first - SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); - if (smart) { - // The output of point() is in boost::optional, as sometimes - // the triangulation operation inside smart factor will encounter degeneracy. - boost::optional point = smart->point(result); - if (point) // ignore if boost::optional return NULL - landmark_result.insert(j, *point); - } - } - - landmark_result.print("Landmark results:\n"); - cout << "final error: " << graph.error(result) << endl; - cout << "number of iterations: " << optimizer.iterations() << endl; - - return 0; -} -/* ************************************************************************* */ - From b82af4f5cccefff3e7bfabfeb54ff2a833c402d2 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 26 Jul 2016 10:54:07 -0400 Subject: [PATCH 09/12] got rid of vector() as suggested by Chris --- gtsam/geometry/triangulation.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index ea0ecca8a..81c8ed2f3 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -488,7 +488,7 @@ TriangulationResult triangulateSafe(const std::vector& cameras, if (params.dynamicOutlierRejectionThreshold > 0) { const Point2& zi = measured.at(i); Point2 reprojectionError(camera.project(point) - zi); - maxReprojError = std::max(maxReprojError, reprojectionError.vector().norm()); + maxReprojError = std::max(maxReprojError, reprojectionError.norm()); } i += 1; } From 0b4927cafe2133b87f0058dc4f8669c404f62bdc Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:22:49 -0400 Subject: [PATCH 10/12] exposed new degeneracy flags in smart factors (they were already exposed in triangulation and only partially exposed in smart factors) --- gtsam/slam/SmartProjectionFactor.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 65a532dc0..d34ba11e3 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -534,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: From 6a21f1b7303bf788df267b0cdf7988455fe7b032 Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:31:38 -0400 Subject: [PATCH 11/12] adapted stereoSmart factors to use "outlier" and "far point" statuses --- gtsam_unstable/slam/SmartStereoProjectionFactor.h | 4 ++-- .../slam/tests/testSmartStereoProjectionPoseFactor.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index bede012d8..77c1cd3ed 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_; } 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 From f009633384e2cf7eed7139986092984226bb00cc Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 31 Jul 2016 18:33:58 -0400 Subject: [PATCH 12/12] exposed point status in smart factors to keep uniformity with mono smart factors --- .../slam/SmartStereoProjectionFactor.h | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 77c1cd3ed..21b0c5eb7 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -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: