Now complete functional triangulateSafe

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
+  /// @name Triangulation result
-  typedef boost::shared_ptr<This> shared_ptr;
+  /// @{
+  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_(
+      rankTolerance_(rankTol), enableEPI_(enableEPI), landmarkDistanceThreshold_(
-          false), landmarkDistanceThreshold_(landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
+          landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
           dynamicOutlierRejectionThreshold) {
   }
 
@@ -81,17 +86,14 @@ public:
   virtual ~PinholeSet() {
   }
 
-  /**
+  /// @name Testable
-   * print
+  /// @{
-   * @param s optional string naming the factor
+
-   * @param keyFormatter optional formatter useful for printing Symbols
+  /// print
-   */
+  virtual void print(const std::string& s = "") const {
-  void print(const std::string& s = "") const {
+    Base::print(s);
-    std::cout << s << "PinholeSet, z = \n";
+    std::cout << s << "PinholeSet\n";
-    std::cout << "rankTolerance_ = " << rankTolerance_ << std::endl;
+    std::cout << "rankTolerance = " << rankTolerance_ << std::endl;
-    std::cout << "degenerate_ = " << degenerate_ << std::endl;
-    std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
-    Base::print("");
   }
 
   /// equals
@@ -99,22 +101,28 @@ public:
     return Base::equals(p, tol); // TODO all flags
   }
 
+  /// @}
+
   /// triangulateSafe
-  size_t triangulateSafe() const {
+  Result triangulateSafe(const std::vector<typename Base::Z>& 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;
+    // if we have a single pose the corresponding factor is uninformative
-      return m;
+    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<CAMERA>(*this, this->measured_, rankTolerance_,
+      result.point = triangulatePoint3<CAMERA>(*this, measured, rankTolerance_,
           enableEPI_);
-      degenerate_ = false;
+      result.degenerate = false;
-      cheiralityException_ = 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_) {
+        if (cameraTranslation.distance(result.point)
-          degenerate_ = true;
+            > 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;
+      result.degenerate = true;
-      cheiralityException_ = false;
+      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;
+    return result;
-  }
-
-  /// 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<Point3> point() const {
-    return point_;
-  }
-
-  /** COMPUTE the landmark */
-  boost::optional<Point3> 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_;
   }
 
 private:

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<CalibratedCamera>::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<CalibratedCamera>::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<Camera>::Result actual = set.triangulateSafe(z);
+  CHECK(actual.degenerate);
 }
 
 /* ************************************************************************* */