From 262b42e82973c9add110ef286c0205359943fdfa Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 26 Feb 2015 15:33:43 +0100 Subject: [PATCH] Now complete functional triangulateSafe --- gtsam/geometry/PinholeSet.h | 145 +++++++++--------------- gtsam/geometry/tests/testPinholeSet.cpp | 9 ++ 2 files changed, 64 insertions(+), 90 deletions(-) diff --git a/gtsam/geometry/PinholeSet.h b/gtsam/geometry/PinholeSet.h index 5a1378782..37489355d 100644 --- a/gtsam/geometry/PinholeSet.h +++ b/gtsam/geometry/PinholeSet.h @@ -41,39 +41,44 @@ protected: /// @name Triangulation parameters /// @{ - const double rankTolerance_; ///< threshold to decide whether triangulation is degenerate_ + const double rankTolerance_; ///< threshold to decide whether triangulation is result.degenerate const bool enableEPI_; ///< if set to true, will refine triangulation using LM + /** + * if the landmark is triangulated at distance larger than this, + * result is flagged as degenerate. + */ + const double landmarkDistanceThreshold_; // + + /** + * If this is nonnegative the we will check if the average reprojection error + * is smaller than this threshold after triangulation, otherwise result is + * flagged as degenerate. + */ + const double dynamicOutlierRejectionThreshold_; /// @} - mutable Point3 point_; ///< Current estimate of the 3D point - - mutable bool degenerate_; - mutable bool cheiralityException_; - - double landmarkDistanceThreshold_; // if the landmark is triangulated at a - // distance larger than that the factor is considered degenerate - - double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the - // average reprojection error is smaller than this threshold after triangulation, - // and the factor is disregarded if the error is large - public: - /// shorthand for a smart pointer to a factor - typedef boost::shared_ptr shared_ptr; + /// @name Triangulation result + /// @{ + struct Result { + Point3 point; + bool degenerate; + bool cheiralityException; + }; + /// @} /** * Constructor * @param rankTol tolerance used to check if point triangulation is degenerate - * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations */ PinholeSet(const double rankTol = 1.0, const bool enableEPI = false, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - rankTolerance_(rankTol), enableEPI_(enableEPI), degenerate_(false), cheiralityException_( - false), landmarkDistanceThreshold_(landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + rankTolerance_(rankTol), enableEPI_(enableEPI), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( dynamicOutlierRejectionThreshold) { } @@ -81,17 +86,14 @@ public: virtual ~PinholeSet() { } - /** - * print - * @param s optional string naming the factor - * @param keyFormatter optional formatter useful for printing Symbols - */ - void print(const std::string& s = "") const { - std::cout << s << "PinholeSet, z = \n"; - std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl; - std::cout << "degenerate_ = " << degenerate_ << std::endl; - std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl; - Base::print(""); + /// @name Testable + /// @{ + + /// print + virtual void print(const std::string& s = "") const { + Base::print(s); + std::cout << s << "PinholeSet\n"; + std::cout << "rankTolerance = " << rankTolerance_ << std::endl; } /// equals @@ -99,22 +101,28 @@ public: return Base::equals(p, tol); // TODO all flags } + /// @} + /// triangulateSafe - size_t triangulateSafe() const { + Result triangulateSafe(const std::vector& measured) const { + + Result result; size_t m = this->size(); - if (m < 2) { // if we have a single pose the corresponding factor is uninformative - degenerate_ = true; - return m; + + // if we have a single pose the corresponding factor is uninformative + if (m < 2) { + result.degenerate = true; + return result; } // We triangulate the 3D position of the landmark try { // std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl; - point_ = triangulatePoint3(*this, this->measured_, rankTolerance_, + result.point = triangulatePoint3(*this, measured, rankTolerance_, enableEPI_); - degenerate_ = false; - cheiralityException_ = false; + result.degenerate = false; + result.cheiralityException = false; // Check landmark distance and reprojection errors to avoid outliers double totalReprojError = 0.0; @@ -122,81 +130,38 @@ public: BOOST_FOREACH(const CAMERA& camera, *this) { Point3 cameraTranslation = camera.pose().translation(); // we discard smart factors corresponding to points that are far away - if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) { - degenerate_ = true; + if (cameraTranslation.distance(result.point) + > landmarkDistanceThreshold_) { + result.degenerate = true; break; } - const Point2& zi = this->measured_.at(i); + const Point2& zi = measured.at(i); try { - Point2 reprojectionError(camera.project(point_) - zi); + Point2 reprojectionError(camera.project(result.point) - zi); totalReprojError += reprojectionError.vector().norm(); } catch (CheiralityException) { - cheiralityException_ = true; + result.cheiralityException = true; } i += 1; } // we discard smart factors that have large reprojection error if (dynamicOutlierRejectionThreshold_ > 0 && totalReprojError / m > dynamicOutlierRejectionThreshold_) - degenerate_ = true; + 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 *this (or motion towards the landmark) // in the second case we want to use a rotation-only smart factor - degenerate_ = true; - cheiralityException_ = false; + result.degenerate = true; + result.cheiralityException = false; } catch (TriangulationCheiralityException&) { // point is behind one of the *this: can be the case of close-to-parallel *this or may depend on outliers // we manage this case by either discarding the smart factor, or imposing a rotation-only constraint - cheiralityException_ = true; + result.cheiralityException = true; } - return m; - } - - /// triangulate - bool triangulateForLinearize() const { - - bool isDebug = false; - size_t nrCameras = this->triangulateSafe(*this); - - if (nrCameras < 2 || (this->cheiralityException_ || this->degenerate_)) { - if (isDebug) { - std::cout - << "createRegularImplicitSchurFactor: degenerate configuration" - << std::endl; - } - return false; - } else { - - // instead, if we want to manage the exception.. - if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors - this->degenerate_ = true; - } - return true; - } - } - - /** return the landmark */ - boost::optional point() const { - return point_; - } - - /** COMPUTE the landmark */ - boost::optional point(const Values& values) const { - triangulateSafe(values); - return point_; - } - - /** return the degenerate state */ - inline bool isDegenerate() const { - return (cheiralityException_ || degenerate_); - } - - /** return the cheirality status flag */ - inline bool isPointBehindCamera() const { - return cheiralityException_; + return result; } private: diff --git a/gtsam/geometry/tests/testPinholeSet.cpp b/gtsam/geometry/tests/testPinholeSet.cpp index 079833ec4..b4b065802 100644 --- a/gtsam/geometry/tests/testPinholeSet.cpp +++ b/gtsam/geometry/tests/testPinholeSet.cpp @@ -33,6 +33,7 @@ TEST(PinholeSet, Stereo) { CalibratedCamera camera; set.push_back(camera); set.push_back(camera); + // set.print("set: "); Point3 p(0, 0, 1); EXPECT_LONGS_EQUAL(6, traits::dimension); @@ -59,6 +60,10 @@ TEST(PinholeSet, Stereo) { EXPECT(assert_equal(F1, F[0])); EXPECT(assert_equal(F1, F[1])); EXPECT(assert_equal(actualE, E)); + + // Instantiate triangulateSafe + // TODO triangulation does not work yet for CalibratedCamera + // PinholeSet::Result actual = set.triangulateSafe(z); } /* ************************************************************************* */ @@ -120,6 +125,10 @@ TEST(PinholeSet, Pinhole) { camera.backprojectPointAtInfinity(Point2()))); actualV = set.reprojectionErrorAtInfinity(p, measured); EXPECT(assert_equal(expectedV, actualV)); + + // Instantiate triangulateSafe + PinholeSet::Result actual = set.triangulateSafe(z); + CHECK(actual.degenerate); } /* ************************************************************************* */