Merge pull request #1194 from borglab/feature/saveTriangulation

gtsam/geometry/geometry.i

@@ -1088,58 +1088,149 @@ class StereoCamera {
 };
 
 #include <gtsam/geometry/triangulation.h>
+class TriangulationResult {
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
+  Status status;
+  TriangulationResult(const gtsam::Point3& p);
+  const gtsam::Point3& get() const;
+  static TriangulationResult Degenerate();
+  static TriangulationResult Outlier();
+  static TriangulationResult FarPoint();
+  static TriangulationResult BehindCamera();
+  bool valid() const;
+  bool degenerate() const;
+  bool outlier() const;
+  bool farPoint() const;
+  bool behindCamera() const;
+};
+
+class TriangulationParameters {
+  double rankTolerance;
+  bool enableEPI;
+  double landmarkDistanceThreshold;
+  double dynamicOutlierRejectionThreshold;
+  SharedNoiseModel noiseModel;
+  TriangulationParameters(const double rankTolerance = 1.0,
+                          const bool enableEPI = false,
+                          double landmarkDistanceThreshold = -1,
+                          double dynamicOutlierRejectionThreshold = -1,
+                          const gtsam::SharedNoiseModel& noiseModel = nullptr);
+};
 
 // Templates appear not yet supported for free functions - issue raised at
 // borglab/wrap#14 to add support
+
+// Cal3_S2 versions
 gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
                                 gtsam::Cal3_S2* sharedCal,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3DS2* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
-gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
-                                gtsam::Cal3Bundler* sharedCal,
-                                const gtsam::Point2Vector& measurements,
-                                double rank_tol, bool optimize,
-                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3_S2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3_S2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3DS2 versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3DS2* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2& cameras,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3DS2* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3DS2& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Bundler versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Bundler* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Bundler* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Bundler& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Fisheye versions
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Fisheye* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
+gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+                                   gtsam::Cal3Fisheye* sharedCal,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Fisheye& cameras,
+                                   const gtsam::Point2Vector& measurements,
+                                   const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Fisheye& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+// Cal3Unified versions                                
+gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,
+                                gtsam::Cal3Unified* sharedCal,
+                                const gtsam::Point2Vector& measurements,
+                                double rank_tol, bool optimize,
+                                const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
                                 const gtsam::Point2Vector& measurements,
                                 double rank_tol, bool optimize,
                                 const gtsam::SharedNoiseModel& model = nullptr);
 gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3_S2* sharedCal,
+                                   gtsam::Cal3Unified* sharedCal,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
+gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& cameras,
-                                   gtsam::Cal3DS2* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses,
-                                   gtsam::Cal3Bundler* sharedCal,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3_S2& cameras,
-                                   const gtsam::Point2Vector& measurements,
-                                   const gtsam::Point3& initialEstimate);
-gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Bundler& cameras,
                                    const gtsam::Point2Vector& measurements,
                                    const gtsam::Point3& initialEstimate);
+gtsam::TriangulationResult triangulateSafe(
+    const gtsam::CameraSetCal3Unified& cameras,
+    const gtsam::Point2Vector& measurements,
+    const gtsam::TriangulationParameters& params);
+
+
 
 #include <gtsam/geometry/BearingRange.h>
 template <POSE, POINT, BEARING, RANGE>

gtsam/geometry/triangulation.h

@@ -23,6 +23,7 @@
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -510,18 +511,18 @@ private:
 };
 
 /**
- * TriangulationResult is an optional point, along with the reasons why it is invalid.
+ * TriangulationResult is an optional point, along with the reasons why it is
+ * invalid.
  */
-class TriangulationResult: public boost::optional<Point3> {
+class TriangulationResult : public boost::optional<Point3> {
-  enum Status {
+ public:
-    VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
+  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
-  };
+  Status status;
-  Status status_;
-  TriangulationResult(Status s) :
-      status_(s) {
-  }
-public:
 
+ private:
+  TriangulationResult(Status s) : status(s) {}
+
+ public:
   /**
    * Default constructor, only for serialization
    */
@@ -530,54 +531,38 @@ public:
   /**
    * Constructor
    */
-  TriangulationResult(const Point3& p) :
+  TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
-      status_(VALID) {
-    reset(p);
-  }
   static TriangulationResult Degenerate() {
     return TriangulationResult(DEGENERATE);
   }
-  static TriangulationResult Outlier() {
+  static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
-    return TriangulationResult(OUTLIER);
-  }
   static TriangulationResult FarPoint() {
     return TriangulationResult(FAR_POINT);
   }
   static TriangulationResult BehindCamera() {
     return TriangulationResult(BEHIND_CAMERA);
   }
-  bool valid() const {
+  bool valid() const { return status == VALID; }
-    return status_ == VALID;
+  bool degenerate() const { return status == DEGENERATE; }
-  }
+  bool outlier() const { return status == OUTLIER; }
-  bool degenerate() const {
+  bool farPoint() const { return status == FAR_POINT; }
-    return status_ == DEGENERATE;
+  bool behindCamera() const { return status == BEHIND_CAMERA; }
-  }
-  bool outlier() const {
-    return status_ == OUTLIER;
-  }
-  bool farPoint() const {
-    return status_ == FAR_POINT;
-  }
-  bool behindCamera() const {
-    return status_ == BEHIND_CAMERA;
-  }
   // stream to output
-  friend std::ostream &operator<<(std::ostream &os,
+  friend std::ostream& operator<<(std::ostream& os,
-      const TriangulationResult& result) {
+                                  const TriangulationResult& result) {
     if (result)
       os << "point = " << *result << std::endl;
     else
-      os << "no point, status = " << result.status_ << std::endl;
+      os << "no point, status = " << result.status << std::endl;
     return os;
   }
 
-private:
+ private:
-
   /// Serialization function
   friend class boost::serialization::access;
-  template<class ARCHIVE>
+  template <class ARCHIVE>
-  void serialize(ARCHIVE & ar, const unsigned int version) {
+  void serialize(ARCHIVE& ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(status_);
+    ar& BOOST_SERIALIZATION_NVP(status);
   }
 };
 
@@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 // Vector of Cameras - used by the Python/MATLAB wrapper
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
+using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;

gtsam/linear/SubgraphBuilder.cpp

@@ -337,7 +337,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
   DSFVector dsf(n);
 
   size_t count = 0;
-  double sum = 0.0;
   for (const size_t index : sortedIndices) {
     const GaussianFactor &gf = *gfg[index];
     const auto keys = gf.keys();
@@ -347,7 +346,6 @@ vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg,
     if (dsf.find(u) != dsf.find(v)) {
       dsf.merge(u, v);
       treeIndices.push_back(index);
-      sum += weights[index];
       if (++count == n - 1) break;
     }
   }

python/gtsam/specializations/geometry.h

@@ -21,6 +21,8 @@ py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
     m_, "CameraSetCal3_S2");
+py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3DS2>>>(
+    m_, "CameraSetCal3DS2");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>>(
     m_, "CameraSetCal3Bundler");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>>(

python/gtsam/tests/test_Triangulation.py

@@ -8,26 +8,17 @@ See LICENSE for the license information
 Test Triangulation
 Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
 """
+# pylint: disable=no-name-in-module, invalid-name, no-member
 import unittest
 from typing import Iterable, List, Optional, Tuple, Union
-
 import numpy as np
 
 import gtsam
-from gtsam import (
+from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
-    Cal3_S2,
+                   CameraSetCal3Bundler, PinholeCameraCal3_S2,
-    Cal3Bundler,
+                   PinholeCameraCal3Bundler, Point2, Point2Vector, Point3,
-    CameraSetCal3_S2,
+                   Pose3, Pose3Vector, Rot3, TriangulationParameters,
-    CameraSetCal3Bundler,
+                   TriangulationResult)
-    PinholeCameraCal3_S2,
-    PinholeCameraCal3Bundler,
-    Point2,
-    Point2Vector,
-    Point3,
-    Pose3,
-    Pose3Vector,
-    Rot3,
-)
 from gtsam.utils.test_case import GtsamTestCase
 
 UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
@@ -218,6 +209,68 @@ class TestTriangulationExample(GtsamTestCase):
         # using the Huber loss we now have a quite small error!! nice!
         self.assertTrue(np.allclose(landmark, actual4, atol=0.05))
 
+    def test_outliers_and_far_landmarks(self) -> None:
+        """Check safe triangulation function."""
+        pose1, pose2 = self.poses
+
+        K1 = Cal3_S2(1500, 1200, 0, 640, 480)
+        # create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
+        camera1 = PinholeCameraCal3_S2(pose1, K1)
+
+        # create second camera 1 meter to the right of first camera
+        K2 = Cal3_S2(1600, 1300, 0, 650, 440)
+        camera2 = PinholeCameraCal3_S2(pose2, K2)
+
+        # 1. Project two landmarks into two cameras and triangulate
+        z1 = camera1.project(self.landmark)
+        z2 = camera2.project(self.landmark)
+
+        cameras = CameraSetCal3_S2()
+        measurements = Point2Vector()
+
+        cameras.append(camera1)
+        cameras.append(camera2)
+        measurements.append(z1)
+        measurements.append(z2)
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        # all default except landmarkDistanceThreshold: 
+        params = TriangulationParameters(1.0, False, landmarkDistanceThreshold)
+        actual: TriangulationResult = gtsam.triangulateSafe(
+            cameras, measurements, params)
+        self.gtsamAssertEquals(actual.get(), self.landmark, 1e-2)
+        self.assertTrue(actual.valid())
+
+        landmarkDistanceThreshold = 4  # landmark is farther than that
+        params2 = TriangulationParameters(
+            1.0, False, landmarkDistanceThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params2)
+        self.assertTrue(actual.farPoint())
+
+        # 3. Add a slightly rotated third camera above with a wrong measurement
+        # (OUTLIER)
+        pose3 = pose1 * Pose3(Rot3.Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1))
+        K3 = Cal3_S2(700, 500, 0, 640, 480)
+        camera3 = PinholeCameraCal3_S2(pose3, K3)
+        z3 = camera3.project(self.landmark)
+
+        cameras.append(camera3)
+        measurements.append(z3 + Point2(10, -10))
+
+        landmarkDistanceThreshold = 10  # landmark is closer than that
+        outlierThreshold = 100   # loose, the outlier is going to pass
+        params3 = TriangulationParameters(1.0, False, landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params3)
+        self.assertTrue(actual.valid())
+
+        # now set stricter threshold for outlier rejection
+        outlierThreshold = 5  # tighter, the outlier is not going to pass
+        params4 = TriangulationParameters(1.0, False, landmarkDistanceThreshold,
+                                          outlierThreshold)
+        actual = gtsam.triangulateSafe(cameras, measurements, params4)
+        self.assertTrue(actual.outlier())
+
 
 if __name__ == "__main__":
     unittest.main()