diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0dc23c160..8e3c93224 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1088,58 +1088,149 @@ class StereoCamera { }; #include +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::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, +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3Unified& 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 template diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 49b5aef04..401fd2d0b 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -23,6 +23,7 @@ #include #include #include +#include #include #include #include @@ -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 { - enum Status { - VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT - }; - Status status_; - TriangulationResult(Status s) : - status_(s) { - } -public: +class TriangulationResult : public boost::optional { + public: + enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; + Status status; + private: + TriangulationResult(Status s) : status(s) {} + + public: /** * Default constructor, only for serialization */ @@ -530,54 +531,38 @@ public: /** * Constructor */ - TriangulationResult(const Point3& p) : - status_(VALID) { - reset(p); - } + TriangulationResult(const Point3& p) : status(VALID) { reset(p); } static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } - static TriangulationResult Outlier() { - return TriangulationResult(OUTLIER); - } + 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; - } + 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; } // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationResult& result) { + friend std::ostream& operator<<(std::ostream& os, + 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 - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(status_); + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_NVP(status); } }; @@ -644,6 +629,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; diff --git a/gtsam/linear/SubgraphBuilder.cpp b/gtsam/linear/SubgraphBuilder.cpp index 18e19cd20..de7ae7060 100644 --- a/gtsam/linear/SubgraphBuilder.cpp +++ b/gtsam/linear/SubgraphBuilder.cpp @@ -337,7 +337,6 @@ vector 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 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; } } diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 5a0ea35ef..99f40253f 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,6 +21,8 @@ py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); py::bind_vector>>( m_, "CameraSetCal3_S2"); +py::bind_vector>>( + m_, "CameraSetCal3DS2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); py::bind_vector>>( diff --git a/python/gtsam/tests/test_Triangulation.py b/python/gtsam/tests/test_Triangulation.py index 0a258a0af..8630e1da7 100644 --- a/python/gtsam/tests/test_Triangulation.py +++ b/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 ( - Cal3_S2, - Cal3Bundler, - CameraSetCal3_S2, - CameraSetCal3Bundler, - PinholeCameraCal3_S2, - PinholeCameraCal3Bundler, - Point2, - Point2Vector, - Point3, - Pose3, - Pose3Vector, - Rot3, -) +from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, + CameraSetCal3Bundler, PinholeCameraCal3_S2, + PinholeCameraCal3Bundler, Point2, Point2Vector, Point3, + Pose3, Pose3Vector, Rot3, TriangulationParameters, + TriangulationResult) 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()