From 4495efe2335b87ac68f8a5ad5138600cc0b87edf Mon Sep 17 00:00:00 2001 From: kartik arcot Date: Thu, 12 Jan 2023 08:34:54 -0800 Subject: [PATCH] triangulation.h --- gtsam/geometry/tests/testTriangulation.cpp | 82 +++++++++---------- gtsam/geometry/triangulation.h | 6 +- ...martProjectionPoseFactorRollingShutter.cpp | 2 +- 3 files changed, 45 insertions(+), 45 deletions(-) diff --git a/gtsam/geometry/tests/testTriangulation.cpp b/gtsam/geometry/tests/testTriangulation.cpp index 95397d5b3..89b320e94 100644 --- a/gtsam/geometry/tests/testTriangulation.cpp +++ b/gtsam/geometry/tests/testTriangulation.cpp @@ -67,13 +67,13 @@ TEST(triangulation, twoPoses) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -82,13 +82,13 @@ TEST(triangulation, twoPoses) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, kSharedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); } @@ -103,7 +103,7 @@ TEST(triangulation, twoCamerasUsingLOST) { double rank_tol = 1e-9; // 1. Test simple triangulation, perfect in no noise situation - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3>(cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -114,7 +114,7 @@ TEST(triangulation, twoCamerasUsingLOST) { // 0.499167, 1.19814) measurements[0] += Point2(0.1, 0.5); measurements[1] += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3>( cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -140,7 +140,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { const double rank_tol = 1e-9; SharedNoiseModel measurementNoise = noiseModel::Isotropic::Sigma(2, 1e-2); - boost::optional estimateLOST = // + std::optional estimateLOST = // triangulatePoint3>(cameras, measurements, rank_tol, /*optimize=*/false, measurementNoise, @@ -149,7 +149,7 @@ TEST(triangulation, twoCamerasLOSTvsDLT) { // These values are from a MATLAB implementation. EXPECT(assert_equal(Point3(0.007, 0.011, 0.945), *estimateLOST, 1e-3)); - boost::optional estimateDLT = + std::optional estimateDLT = triangulatePoint3(cameras, measurements, rank_tol, false); // The LOST estimate should have a smaller error. @@ -177,14 +177,14 @@ TEST(triangulation, twoPosesCal3DS2) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -194,14 +194,14 @@ TEST(triangulation, twoPosesCal3DS2) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); @@ -230,14 +230,14 @@ TEST(triangulation, twoPosesFisheye) { // 1. Test simple DLT, perfect in no noise situation bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 2. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -247,14 +247,14 @@ TEST(triangulation, twoPosesFisheye) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-3)); // 4. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(kPoses, sharedDistortedCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-3)); @@ -277,7 +277,7 @@ TEST(triangulation, twoPosesBundler) { bool optimize = true; double rank_tol = 1e-9; - boost::optional actual = // + std::optional actual = // triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual, 1e-7)); @@ -286,7 +286,7 @@ TEST(triangulation, twoPosesBundler) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(kPoses, bundlerCal, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-3)); @@ -296,7 +296,7 @@ TEST(triangulation, twoPosesBundler) { TEST(triangulation, fourPoses) { Pose3Vector poses = kPoses; Point2Vector measurements = kMeasurements; - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -305,7 +305,7 @@ TEST(triangulation, fourPoses) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); @@ -317,12 +317,12 @@ TEST(triangulation, fourPoses) { poses.push_back(pose3); measurements.push_back(z3 + Point2(0.1, -0.1)); - boost::optional triangulated_3cameras = // + std::optional triangulated_3cameras = // triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = + std::optional triangulated_3cameras_opt = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); @@ -352,7 +352,7 @@ TEST(triangulation, threePoses_robustNoiseModel) { Point2Vector measurements{kZ1, kZ2, z3}; // noise free, so should give exactly the landmark - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -360,14 +360,14 @@ TEST(triangulation, threePoses_robustNoiseModel) { measurements.at(0) += Point2(100, 120); // very large pixel noise! // now estimate does not match landmark - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.26m): EXPECT( (kLandmark - *actual2).norm() >= 0.2); EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization - boost::optional actual3 = + std::optional actual3 = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -375,7 +375,7 @@ TEST(triangulation, threePoses_robustNoiseModel) { // Again with nonlinear optimization, this time with robust loss auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); - boost::optional actual4 = triangulatePoint3( + std::optional actual4 = triangulatePoint3( poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! EXPECT(assert_equal(kLandmark, *actual4, 0.05)); @@ -393,7 +393,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) { Point2Vector measurements{kZ1, kZ1, kZ2, z3}; // noise free, so should give exactly the landmark - boost::optional actual = + std::optional actual = triangulatePoint3(poses, kSharedCal, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -405,14 +405,14 @@ TEST(triangulation, fourPoses_robustNoiseModel) { measurements.at(3) += Point2(0.3, 0.1); // now estimate does not match landmark - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(poses, kSharedCal, measurements); // DLT is surprisingly robust, but still off (actual error is around 0.17m): EXPECT( (kLandmark - *actual2).norm() >= 0.1); EXPECT( (kLandmark - *actual2).norm() <= 0.5); // Again with nonlinear optimization - boost::optional actual3 = + std::optional actual3 = triangulatePoint3(poses, kSharedCal, measurements, 1e-9, true); // result from nonlinear (but non-robust optimization) is close to DLT and still off EXPECT(assert_equal(*actual2, *actual3, 0.1)); @@ -420,7 +420,7 @@ TEST(triangulation, fourPoses_robustNoiseModel) { // Again with nonlinear optimization, this time with robust loss auto model = noiseModel::Robust::Create( noiseModel::mEstimator::Huber::Create(1.345), noiseModel::Unit::Create(2)); - boost::optional actual4 = triangulatePoint3( + std::optional actual4 = triangulatePoint3( poses, kSharedCal, measurements, 1e-9, true, model); // using the Huber loss we now have a quite small error!! nice! EXPECT(assert_equal(kLandmark, *actual4, 0.05)); @@ -443,7 +443,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { CameraSet> cameras{camera1, camera2}; Point2Vector measurements{z1, z2}; - boost::optional actual = // + std::optional actual = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); @@ -452,7 +452,7 @@ TEST(triangulation, fourPoses_distinct_Ks) { measurements.at(0) += Point2(0.1, 0.5); measurements.at(1) += Point2(-0.2, 0.3); - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual2, 1e-2)); @@ -465,12 +465,12 @@ TEST(triangulation, fourPoses_distinct_Ks) { cameras.push_back(camera3); measurements.push_back(z3 + Point2(0.1, -0.1)); - boost::optional triangulated_3cameras = // + std::optional triangulated_3cameras = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *triangulated_3cameras, 1e-2)); // Again with nonlinear optimization - boost::optional triangulated_3cameras_opt = + std::optional triangulated_3cameras_opt = triangulatePoint3(cameras, measurements, 1e-9, true); EXPECT(assert_equal(kLandmark, *triangulated_3cameras_opt, 1e-2)); @@ -506,7 +506,7 @@ TEST(triangulation, fourPoses_distinct_Ks_distortion) { const CameraSet> cameras{camera1, camera2}; const Point2Vector measurements{z1, z2}; - boost::optional actual = // + std::optional actual = // triangulatePoint3(cameras, measurements); EXPECT(assert_equal(kLandmark, *actual, 1e-2)); } @@ -727,14 +727,14 @@ TEST(triangulation, twoPoses_sphericalCamera) { // 3. Test simple DLT, now within triangulatePoint3 bool optimize = false; - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual1, 1e-7)); // 4. test with optimization on, same answer optimize = true; - boost::optional actual2 = // + std::optional actual2 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(kLandmark, *actual2, 1e-7)); @@ -745,14 +745,14 @@ TEST(triangulation, twoPoses_sphericalCamera) { u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3 measurements.at(1) = u2.retract(Vector2(-0.02, 0.03)); optimize = false; - boost::optional actual3 = // + std::optional actual3 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3)); // 6. Now with optimization on optimize = true; - boost::optional actual4 = // + std::optional actual4 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3)); @@ -795,7 +795,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { rank_tol, optimize), TriangulationCheiralityException); #else // otherwise project should not throw the exception - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); @@ -809,7 +809,7 @@ TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) { rank_tol, optimize), TriangulationCheiralityException); #else // otherwise project should not throw the exception - boost::optional actual1 = // + std::optional actual1 = // triangulatePoint3(cameras, measurements, rank_tol, optimize); EXPECT(assert_equal(landmarkL, *actual1, 1e-7)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e0473f8c3..660d9fa3c 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -260,7 +260,7 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { template MEASUREMENT undistortMeasurementInternal( const CALIBRATION& cal, const MEASUREMENT& measurement, - boost::optional pinholeCal = boost::none) { + std::optional pinholeCal = {}) { if (!pinholeCal) { pinholeCal = createPinholeCalibration(cal); } @@ -623,7 +623,7 @@ private: * TriangulationResult is an optional point, along with the reasons why it is * invalid. */ -class TriangulationResult : public boost::optional { +class TriangulationResult : public std::optional { public: enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT }; Status status; @@ -640,7 +640,7 @@ class TriangulationResult : public boost::optional { /** * Constructor */ - TriangulationResult(const Point3& p) : status(VALID) { reset(p); } + TriangulationResult(const Point3& p) : status(VALID) { emplace(p); } static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index cad1feae1..098d33744 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -664,7 +664,7 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { EXPECT(smartFactor1->triangulateSafe(cameras)); EXPECT(!smartFactor1->isDegenerate()); EXPECT(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); + std::optional p = smartFactor1->point(); EXPECT(p); EXPECT(assert_equal(landmark1, *p));