From 3eec88f60eaae0c1db1cecdab6d20d24a64c9030 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Fri, 2 Jun 2023 15:44:51 -0700 Subject: [PATCH 1/6] Added rank threshold to triangulateLOST --- gtsam/geometry/triangulation.cpp | 57 +++--- gtsam/geometry/triangulation.h | 305 +++++++++++++++---------------- 2 files changed, 173 insertions(+), 189 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 06fb8fafe..2049ce384 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -25,9 +25,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& - projection_matrices, - const Point2Vector& measurements, double rank_tol) { + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT( } Vector4 triangulateHomogeneousDLT( - const std::vector>& - projection_matrices, - const std::vector& measurements, double rank_tol) { + const std::vector>& projection_matrices, + const std::vector& measurements, + double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -68,9 +68,7 @@ Vector4 triangulateHomogeneousDLT( for (size_t i = 0; i < m; i++) { size_t row = i * 2; const Matrix34& projection = projection_matrices.at(i); - const Point3& p = - measurements.at(i) - .point3(); // to get access to x,y,z of the bearing vector + const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector // build system of equations A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); @@ -85,7 +83,8 @@ Vector4 triangulateHomogeneousDLT( Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, - const SharedIsotropic& measurementNoise) { + const SharedIsotropic& measurementNoise, + double rank_tol) { size_t m = calibratedMeasurements.size(); assert(m == poses.size()); @@ -105,36 +104,39 @@ Point3 triangulateLOST(const std::vector& poses, const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); // Note: Setting q_i = 1.0 gives same results as DLT. - const double q_i = wZi.cross(wZj).norm() / - (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm()); - const Matrix23 coefficientMat = - q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * - wTi.rotation().matrix().transpose(); + const Matrix23 coefficientMat = q_i * + skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); A.block<2, 3>(2 * i, 0) << coefficientMat; b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); } - return A.colPivHouseholderQr().solve(b); + + Eigen::ColPivHouseholderQR A_Qr = A.colPivHouseholderQr(); + A_Qr.setThreshold(rank_tol); + + // if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException()); + + return A_Qr.solve(b); } Point3 triangulateDLT( - const std::vector>& - projection_matrices, - const Point2Vector& measurements, double rank_tol) { - Vector4 v = - triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol) { + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& - projection_matrices, - const std::vector& measurements, double rank_tol) { + const std::vector>& projection_matrices, + const std::vector& measurements, + double rank_tol) { // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = - triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } @@ -146,8 +148,7 @@ Point3 triangulateDLT( * @param landmarkKey to refer to landmark * @return refined Point3 */ -Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, - Key landmarkKey) { +Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) { // Maybe we should consider Gauss-Newton? LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 7e58cee2d..61ca5588a 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,39 +20,38 @@ #pragma once -#include "gtsam/geometry/Point3.h" #include +#include #include #include #include -#include #include #include -#include #include +#include #include #include #include +#include "gtsam/geometry/Point3.h" #include namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 -class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { -public: - TriangulationUnderconstrainedException() : - std::runtime_error("Triangulation Underconstrained Exception.") { - } +class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error { + public: + TriangulationUnderconstrainedException() + : std::runtime_error("Triangulation Underconstrained Exception.") {} }; /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras -class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { -public: - TriangulationCheiralityException() : - std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { - } +class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error { + public: + TriangulationCheiralityException() + : std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more " + "cameras.") {} }; /** @@ -64,11 +63,13 @@ public: */ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, - const Point2Vector& measurements, double rank_tol = 1e-9); + const Point2Vector& measurements, + double rank_tol = 1e-9); /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors - * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) + * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and + * Zisserman) * @param projection_matrices Projection matrices (K*P^-1) * @param measurements Unit3 bearing measurements * @param rank_tol SVD rank tolerance @@ -76,7 +77,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, - const std::vector& measurements, double rank_tol = 1e-9); + const std::vector& measurements, + double rank_tol = 1e-9); /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 @@ -85,18 +87,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_EXPORT Point3 triangulateDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 +triangulateDLT(const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol = 1e-9); /** * overload of previous function to work with Unit3 (projected to canonical camera) */ -GTSAM_EXPORT Point3 triangulateDLT( - const std::vector>& projection_matrices, - const std::vector& measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 +triangulateDLT(const std::vector>& projection_matrices, + const std::vector& measurements, + double rank_tol = 1e-9); /** * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) @@ -110,7 +112,8 @@ GTSAM_EXPORT Point3 triangulateDLT( */ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, const Point3Vector& calibratedMeasurements, - const SharedIsotropic& measurementNoise); + const SharedIsotropic& measurementNoise, + double rank_tol = 1e-9); /** * Create a factor graph with projection factors from poses and one calibration @@ -121,20 +124,22 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector& poses, std::shared_ptr sharedCal, - const Point2Vector& measurements, Key landmarkKey, + const std::vector& poses, + std::shared_ptr sharedCal, + const Point2Vector& measurements, + Key landmarkKey, const Point3& initialEstimate, const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value + values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.emplace_shared > // + graph.emplace_shared> // (camera_i, measurements[i], model, landmarkKey); } return {graph, values}; @@ -149,21 +154,22 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, + const typename CAMERA::MeasurementVector& measurements, + Key landmarkKey, const Point3& initialEstimate, const SharedNoiseModel& model = nullptr) { Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value + values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit(noiseModel::Unit::Create( - traits::dimension)); + static SharedNoiseModel unit( + noiseModel::Unit::Create(traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.emplace_shared > // - (camera_i, measurements[i], model? model : unit, landmarkKey); + graph.emplace_shared> // + (camera_i, measurements[i], model ? model : unit, landmarkKey); } return {graph, values}; } @@ -176,7 +182,8 @@ std::pair triangulationGraph( * @return refined Point3 */ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, - const Values& values, Key landmarkKey); + const Values& values, + Key landmarkKey); /** * Given an initial estimate , refine a point using measurements in several cameras @@ -186,14 +193,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear(const std::vector& poses, - std::shared_ptr sharedCal, - const Point2Vector& measurements, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { - + std::shared_ptr sharedCal, + const Point2Vector& measurements, + const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { // Create a factor graph and initial values - const auto [graph, values] = triangulationGraph // + const auto [graph, values] = triangulationGraph // (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); @@ -206,33 +213,32 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template -Point3 triangulateNonlinear( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { - +template +Point3 triangulateNonlinear(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, + const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { // Create a factor graph and initial values - const auto [graph, values] = triangulationGraph // + const auto [graph, values] = triangulationGraph // (cameras, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); } -template -std::vector> -projectionMatricesFromCameras(const CameraSet &cameras) { +template +std::vector> projectionMatricesFromCameras( + const CameraSet& cameras) { std::vector> projection_matrices; - for (const CAMERA &camera: cameras) { + for (const CAMERA& camera : cameras) { projection_matrices.push_back(camera.cameraProjectionMatrix()); } return projection_matrices; } // overload, assuming pinholePose -template +template std::vector> projectionMatricesFromPoses( - const std::vector &poses, std::shared_ptr sharedCal) { + const std::vector& poses, std::shared_ptr sharedCal) { std::vector> projection_matrices; for (size_t i = 0; i < poses.size(); i++) { PinholePose camera(poses.at(i), sharedCal); @@ -257,9 +263,9 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { /** Internal undistortMeasurement to be used by undistortMeasurement and * undistortMeasurements */ template -MEASUREMENT undistortMeasurementInternal( - const CALIBRATION& cal, const MEASUREMENT& measurement, - std::optional pinholeCal = {}) { +MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, + const MEASUREMENT& measurement, + std::optional pinholeCal = {}) { if (!pinholeCal) { pinholeCal = createPinholeCalibration(cal); } @@ -278,13 +284,13 @@ MEASUREMENT undistortMeasurementInternal( * @return measurements with the effect of the distortion of sharedCal removed. */ template -Point2Vector undistortMeasurements(const CALIBRATION& cal, - const Point2Vector& measurements) { +Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& measurements) { Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); Point2Vector undistortedMeasurements; // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - std::transform(measurements.begin(), measurements.end(), + std::transform(measurements.begin(), + measurements.end(), std::back_inserter(undistortedMeasurements), [&cal, &pinholeCalibration](const Point2& measurement) { return undistortMeasurementInternal( @@ -295,8 +301,7 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal, /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ template <> -inline Point2Vector undistortMeasurements(const Cal3_S2& cal, - const Point2Vector& measurements) { +inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) { return measurements; } @@ -313,17 +318,15 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal, */ template typename CAMERA::MeasurementVector undistortMeasurements( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements) { + const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); assert(nrMeasurements == cameras.size()); typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - undistortedMeasurements[ii] = - undistortMeasurementInternal( - cameras[ii].calibration(), measurements[ii]); + undistortedMeasurements[ii] = undistortMeasurementInternal( + cameras[ii].calibration(), measurements[ii]); } return undistortedMeasurements; } @@ -353,12 +356,13 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @return homogeneous measurements in image plane */ template -inline Point3Vector calibrateMeasurementsShared( - const CALIBRATION& cal, const Point2Vector& measurements) { +inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal, + const Point2Vector& measurements) { Point3Vector calibratedMeasurements; // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - std::transform(measurements.begin(), measurements.end(), + std::transform(measurements.begin(), + measurements.end(), std::back_inserter(calibratedMeasurements), [&cal](const Point2& measurement) { Point3 p; @@ -377,25 +381,21 @@ inline Point3Vector calibrateMeasurementsShared( * @return homogeneous measurements in image plane */ template -inline Point3Vector calibrateMeasurements( - const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements) { +inline Point3Vector calibrateMeasurements(const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); assert(nrMeasurements == cameras.size()); Point3Vector calibratedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { - calibratedMeasurements[ii] - << cameras[ii].calibration().calibrate(measurements[ii]), - 1.0; + calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0; } return calibratedMeasurements; } /** Specialize for SphericalCamera to do nothing. */ template -inline Point3Vector calibrateMeasurements( - const CameraSet& cameras, - const SphericalCamera::MeasurementVector& measurements) { +inline Point3Vector calibrateMeasurements(const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { Point3Vector calibratedMeasurements(measurements.size()); for (size_t ii = 0; ii < measurements.size(); ++ii) { calibratedMeasurements[ii] << measurements[ii].point3(); @@ -420,7 +420,8 @@ template Point3 triangulatePoint3(const std::vector& poses, std::shared_ptr sharedCal, const Point2Vector& measurements, - double rank_tol = 1e-9, bool optimize = false, + double rank_tol = 1e-9, + bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { assert(poses.size() == measurements.size()); @@ -432,24 +433,21 @@ Point3 triangulatePoint3(const std::vector& poses, // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; - SharedIsotropic measurementNoise = - noiseModel::Isotropic::Sigma(2, measurementSigma); + SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); // calibrate the measurements to obtain homogenous coordinates in image // plane. auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(*sharedCal, measurements); + auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); - point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -485,7 +483,8 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, - double rank_tol = 1e-9, bool optimize = false, + double rank_tol = 1e-9, + bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { size_t m = cameras.size(); @@ -499,8 +498,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; - SharedIsotropic measurementNoise = - noiseModel::Isotropic::Sigma(2, measurementSigma); + SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); // construct poses from cameras. std::vector poses; @@ -509,20 +507,17 @@ Point3 triangulatePoint3(const CameraSet& cameras, // calibrate the measurements to obtain homogenous coordinates in image // plane. - auto calibratedMeasurements = - calibrateMeasurements(cameras, measurements); + auto calibratedMeasurements = calibrateMeasurements(cameras, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromCameras(cameras); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = - undistortMeasurements(cameras, measurements); + auto undistortedMeasurements = undistortMeasurements(cameras, measurements); - point = - triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -545,7 +540,8 @@ Point3 triangulatePoint3(const CameraSet& cameras, template Point3 triangulatePoint3(const CameraSet>& cameras, const Point2Vector& measurements, - double rank_tol = 1e-9, bool optimize = false, + double rank_tol = 1e-9, + bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { return triangulatePoint3> // @@ -553,16 +549,16 @@ Point3 triangulatePoint3(const CameraSet>& cameras, } struct GTSAM_EXPORT TriangulationParameters { - - double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) - bool enableEPI; ///< if set to true, will refine triangulation using LM + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than + ///< rankTolerance) + 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. */ - double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // /** * If this is nonnegative the we will check if the average reprojection error @@ -571,7 +567,7 @@ struct GTSAM_EXPORT TriangulationParameters { */ double dynamicOutlierRejectionThreshold; - SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation + SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation /** * Constructor @@ -583,39 +579,36 @@ struct GTSAM_EXPORT TriangulationParameters { * */ TriangulationParameters(const double _rankTolerance = 1.0, - const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1, - const SharedNoiseModel& _noiseModel = nullptr) : - rankTolerance(_rankTolerance), enableEPI(_enableEPI), // - landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), - noiseModel(_noiseModel){ - } + const bool _enableEPI = false, + double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1, + const SharedNoiseModel& _noiseModel = nullptr) + : rankTolerance(_rankTolerance), + enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), + noiseModel(_noiseModel) {} // stream to output - friend std::ostream &operator<<(std::ostream &os, - const TriangulationParameters& p) { + friend std::ostream& operator<<(std::ostream& os, const TriangulationParameters& p) { os << "rankTolerance = " << p.rankTolerance << std::endl; os << "enableEPI = " << p.enableEPI << std::endl; - os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold - << std::endl; - os << "dynamicOutlierRejectionThreshold = " - << p.dynamicOutlierRejectionThreshold << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl; + os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl; os << "noise model" << std::endl; return os; } -private: - + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & BOOST_SERIALIZATION_NVP(rankTolerance); - ar & BOOST_SERIALIZATION_NVP(enableEPI); - ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); - ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + template + void serialize(ARCHIVE& ar, const unsigned int version) { + ar& BOOST_SERIALIZATION_NVP(rankTolerance); + ar& BOOST_SERIALIZATION_NVP(enableEPI); + ar& BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar& BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); } #endif }; @@ -642,16 +635,10 @@ class TriangulationResult : public std::optional { * Constructor */ TriangulationResult(const Point3& p) : status(VALID) { emplace(p); } - static TriangulationResult Degenerate() { - return TriangulationResult(DEGENERATE); - } + static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); } - static TriangulationResult FarPoint() { - return TriangulationResult(FAR_POINT); - } - static TriangulationResult BehindCamera() { - return TriangulationResult(BEHIND_CAMERA); - } + 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; } @@ -662,8 +649,7 @@ class TriangulationResult : public std::optional { return value(); } // 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 @@ -683,11 +669,10 @@ class TriangulationResult : public std::optional { }; /// triangulateSafe: extensive checking of the outcome -template +template TriangulationResult triangulateSafe(const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measured, - const TriangulationParameters& params) { - + const typename CAMERA::MeasurementVector& measured, + const TriangulationParameters& params) { size_t m = cameras.size(); // if we have a single pose the corresponding factor is uninformative @@ -696,25 +681,22 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, else // We triangulate the 3D position of the landmark try { - Point3 point = - triangulatePoint3(cameras, measured, params.rankTolerance, - params.enableEPI, params.noiseModel); + Point3 point = triangulatePoint3( + cameras, measured, params.rankTolerance, params.enableEPI, params.noiseModel); // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; double maxReprojError = 0.0; - for(const CAMERA& camera: cameras) { + for (const CAMERA& camera : cameras) { const Pose3& pose = camera.pose(); - if (params.landmarkDistanceThreshold > 0 - && distance3(pose.translation(), point) - > params.landmarkDistanceThreshold) + if (params.landmarkDistanceThreshold > 0 && + distance3(pose.translation(), point) > params.landmarkDistanceThreshold) return TriangulationResult::FarPoint(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // Only needed if this was not yet handled by exception const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) - return TriangulationResult::BehindCamera(); + if (p_local.z() <= 0) return TriangulationResult::BehindCamera(); #endif // Check reprojection error if (params.dynamicOutlierRejectionThreshold > 0) { @@ -725,19 +707,21 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, i += 1; } // Flag as degenerate if average reprojection error is too large - if (params.dynamicOutlierRejectionThreshold > 0 - && maxReprojError > params.dynamicOutlierRejectionThreshold) + if (params.dynamicOutlierRejectionThreshold > 0 && + maxReprojError > params.dynamicOutlierRejectionThreshold) return TriangulationResult::Outlier(); // all good! return TriangulationResult(point); } catch (TriangulationUnderconstrainedException&) { // This exception is thrown if - // 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 cameras (or motion towards the landmark) + // 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 cameras (or motion towards the landmark) return TriangulationResult::Degenerate(); } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may + // depend on outliers return TriangulationResult::BehindCamera(); } } @@ -749,5 +733,4 @@ using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; -} // \namespace gtsam - +} // namespace gtsam From fcee29e62898d15bf64d93fafbb35f6735d997d1 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Mon, 5 Jun 2023 19:11:41 -0700 Subject: [PATCH 2/6] Handle q_i = 0 (or NaN) for LOST --- gtsam/geometry/triangulation.cpp | 33 ++++++++++++++++++++++++++------ 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 2049ce384..d7be29a9f 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -95,16 +95,37 @@ Point3 triangulateLOST(const std::vector& poses, for (size_t i = 0; i < m; i++) { const Pose3& wTi = poses[i]; // TODO(akshay-krishnan): are there better ways to select j? - const int j = (i + 1) % m; + int j = (i + 1) % m; const Pose3& wTj = poses[j]; - const Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 d_ij = wTj.translation() - wTi.translation(); + Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); + Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + double num_i = wZi.cross(wZj).norm(); + double den_i = d_ij.cross(wZj).norm(); - const Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]); - const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and wZj, coincide + // (or the baseline vector coincides with the jth measurement vector). + if (num_i == 0 || den_i == 0) { + bool success = false; + for (size_t k = 2; k < m; k++) { + j = (i + k) % m; + const Pose3& wTj = poses[j]; + + d_ij = wTj.translation() - wTi.translation(); + wZj = wTj.rotation().rotate(calibratedMeasurements[j]); + num_i = wZi.cross(wZj).norm(); + den_i = d_ij.cross(wZj).norm(); + if (num_i > 0 && den_i > 0) { + success = true; + break; + } + } + if (!success) throw(TriangulationUnderconstrainedException()); + } // Note: Setting q_i = 1.0 gives same results as DLT. - const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm()); + const double q_i = num_i / (measurementNoise->sigma() * den_i); const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * @@ -117,7 +138,7 @@ Point3 triangulateLOST(const std::vector& poses, Eigen::ColPivHouseholderQR A_Qr = A.colPivHouseholderQr(); A_Qr.setThreshold(rank_tol); - // if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException()); + if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException()); return A_Qr.solve(b); } From f6f91ce231eef1855085444031470856bd42e1af Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Tue, 6 Jun 2023 09:38:19 -0700 Subject: [PATCH 3/6] Revert formatting for triangulation.cpp --- gtsam/geometry/triangulation.cpp | 48 ++++++++++++++++++-------------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index d7be29a9f..e714bb432 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -25,9 +25,9 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol) { + const std::vector>& + projection_matrices, + const Point2Vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT( } Vector4 triangulateHomogeneousDLT( - const std::vector>& projection_matrices, - const std::vector& measurements, - double rank_tol) { + const std::vector>& + projection_matrices, + const std::vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -68,7 +68,9 @@ Vector4 triangulateHomogeneousDLT( for (size_t i = 0; i < m; i++) { size_t row = i * 2; const Matrix34& projection = projection_matrices.at(i); - const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector + const Point3& p = + measurements.at(i) + .point3(); // to get access to x,y,z of the bearing vector // build system of equations A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); @@ -104,8 +106,9 @@ Point3 triangulateLOST(const std::vector& poses, double num_i = wZi.cross(wZj).norm(); double den_i = d_ij.cross(wZj).norm(); - // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and wZj, coincide - // (or the baseline vector coincides with the jth measurement vector). + // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and + // wZj, coincide (or the baseline vector coincides with the jth measurement + // vector). if (num_i == 0 || den_i == 0) { bool success = false; for (size_t k = 2; k < m; k++) { @@ -127,9 +130,9 @@ Point3 triangulateLOST(const std::vector& poses, // Note: Setting q_i = 1.0 gives same results as DLT. const double q_i = num_i / (measurementNoise->sigma() * den_i); - const Matrix23 coefficientMat = q_i * - skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * - wTi.rotation().matrix().transpose(); + const Matrix23 coefficientMat = + q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * + wTi.rotation().matrix().transpose(); A.block<2, 3>(2 * i, 0) << coefficientMat; b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); @@ -144,20 +147,22 @@ Point3 triangulateLOST(const std::vector& poses, } Point3 triangulateDLT( - const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol) { - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + const std::vector>& + projection_matrices, + const Point2Vector& measurements, double rank_tol) { + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& projection_matrices, - const std::vector& measurements, - double rank_tol) { + const std::vector>& + projection_matrices, + const std::vector& measurements, double rank_tol) { // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); + Vector4 v = + triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } @@ -169,7 +174,8 @@ Point3 triangulateDLT( * @param landmarkKey to refer to landmark * @return refined Point3 */ -Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) { +Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, + Key landmarkKey) { // Maybe we should consider Gauss-Newton? LevenbergMarquardtParams params; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; From 8d134fd120525562dead2f9e9cbe3e93bc7e4395 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Tue, 6 Jun 2023 09:48:05 -0700 Subject: [PATCH 4/6] Revert formatting for triangulation.h --- gtsam/geometry/triangulation.h | 303 ++++++++++++++++++--------------- 1 file changed, 161 insertions(+), 142 deletions(-) diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 61ca5588a..3a8398804 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -20,38 +20,39 @@ #pragma once +#include "gtsam/geometry/Point3.h" #include -#include #include #include #include +#include #include #include -#include #include +#include #include #include #include -#include "gtsam/geometry/Point3.h" #include namespace gtsam { /// Exception thrown by triangulateDLT when SVD returns rank < 3 -class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error { - public: - TriangulationUnderconstrainedException() - : std::runtime_error("Triangulation Underconstrained Exception.") {} +class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { +public: + TriangulationUnderconstrainedException() : + std::runtime_error("Triangulation Underconstrained Exception.") { + } }; /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras -class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error { - public: - TriangulationCheiralityException() - : std::runtime_error( - "Triangulation Cheirality Exception: The resulting landmark is behind one or more " - "cameras.") {} +class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { +public: + TriangulationCheiralityException() : + std::runtime_error( + "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { + } }; /** @@ -63,13 +64,11 @@ class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error */ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol = 1e-9); + const Point2Vector& measurements, double rank_tol = 1e-9); /** * Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors - * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and - * Zisserman) + * (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman) * @param projection_matrices Projection matrices (K*P^-1) * @param measurements Unit3 bearing measurements * @param rank_tol SVD rank tolerance @@ -77,8 +76,7 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( */ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( const std::vector>& projection_matrices, - const std::vector& measurements, - double rank_tol = 1e-9); + const std::vector& measurements, double rank_tol = 1e-9); /** * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 @@ -87,18 +85,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_EXPORT Point3 -triangulateDLT(const std::vector>& projection_matrices, - const Point2Vector& measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 triangulateDLT( + const std::vector>& projection_matrices, + const Point2Vector& measurements, + double rank_tol = 1e-9); /** * overload of previous function to work with Unit3 (projected to canonical camera) */ -GTSAM_EXPORT Point3 -triangulateDLT(const std::vector>& projection_matrices, - const std::vector& measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Point3 triangulateDLT( + const std::vector>& projection_matrices, + const std::vector& measurements, + double rank_tol = 1e-9); /** * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) @@ -124,22 +122,20 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector& poses, * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( - const std::vector& poses, - std::shared_ptr sharedCal, - const Point2Vector& measurements, - Key landmarkKey, + const std::vector& poses, std::shared_ptr sharedCal, + const Point2Vector& measurements, Key landmarkKey, const Point3& initialEstimate, const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value + values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; for (size_t i = 0; i < measurements.size(); i++) { const Pose3& pose_i = poses[i]; typedef PinholePose Camera; Camera camera_i(pose_i, sharedCal); - graph.emplace_shared> // + graph.emplace_shared > // (camera_i, measurements[i], model, landmarkKey); } return {graph, values}; @@ -154,22 +150,21 @@ std::pair triangulationGraph( * @param initialEstimate * @return graph and initial values */ -template +template std::pair triangulationGraph( const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, - Key landmarkKey, + const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, const Point3& initialEstimate, const SharedNoiseModel& model = nullptr) { Values values; - values.insert(landmarkKey, initialEstimate); // Initial landmark value + values.insert(landmarkKey, initialEstimate); // Initial landmark value NonlinearFactorGraph graph; - static SharedNoiseModel unit( - noiseModel::Unit::Create(traits::dimension)); + static SharedNoiseModel unit(noiseModel::Unit::Create( + traits::dimension)); for (size_t i = 0; i < measurements.size(); i++) { const CAMERA& camera_i = cameras[i]; - graph.emplace_shared> // - (camera_i, measurements[i], model ? model : unit, landmarkKey); + graph.emplace_shared > // + (camera_i, measurements[i], model? model : unit, landmarkKey); } return {graph, values}; } @@ -182,8 +177,7 @@ std::pair triangulationGraph( * @return refined Point3 */ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, - const Values& values, - Key landmarkKey); + const Values& values, Key landmarkKey); /** * Given an initial estimate , refine a point using measurements in several cameras @@ -193,14 +187,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, * @param initialEstimate * @return refined Point3 */ -template +template Point3 triangulateNonlinear(const std::vector& poses, - std::shared_ptr sharedCal, - const Point2Vector& measurements, - const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { + std::shared_ptr sharedCal, + const Point2Vector& measurements, const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { + // Create a factor graph and initial values - const auto [graph, values] = triangulationGraph // + const auto [graph, values] = triangulationGraph // (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); @@ -213,32 +207,33 @@ Point3 triangulateNonlinear(const std::vector& poses, * @param initialEstimate * @return refined Point3 */ -template -Point3 triangulateNonlinear(const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements, - const Point3& initialEstimate, - const SharedNoiseModel& model = nullptr) { +template +Point3 triangulateNonlinear( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate, + const SharedNoiseModel& model = nullptr) { + // Create a factor graph and initial values - const auto [graph, values] = triangulationGraph // + const auto [graph, values] = triangulationGraph // (cameras, measurements, Symbol('p', 0), initialEstimate, model); return optimize(graph, values, Symbol('p', 0)); } -template -std::vector> projectionMatricesFromCameras( - const CameraSet& cameras) { +template +std::vector> +projectionMatricesFromCameras(const CameraSet &cameras) { std::vector> projection_matrices; - for (const CAMERA& camera : cameras) { + for (const CAMERA &camera: cameras) { projection_matrices.push_back(camera.cameraProjectionMatrix()); } return projection_matrices; } // overload, assuming pinholePose -template +template std::vector> projectionMatricesFromPoses( - const std::vector& poses, std::shared_ptr sharedCal) { + const std::vector &poses, std::shared_ptr sharedCal) { std::vector> projection_matrices; for (size_t i = 0; i < poses.size(); i++) { PinholePose camera(poses.at(i), sharedCal); @@ -263,9 +258,9 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) { /** Internal undistortMeasurement to be used by undistortMeasurement and * undistortMeasurements */ template -MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, - const MEASUREMENT& measurement, - std::optional pinholeCal = {}) { +MEASUREMENT undistortMeasurementInternal( + const CALIBRATION& cal, const MEASUREMENT& measurement, + std::optional pinholeCal = {}) { if (!pinholeCal) { pinholeCal = createPinholeCalibration(cal); } @@ -284,13 +279,13 @@ MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal, * @return measurements with the effect of the distortion of sharedCal removed. */ template -Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& measurements) { +Point2Vector undistortMeasurements(const CALIBRATION& cal, + const Point2Vector& measurements) { Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); Point2Vector undistortedMeasurements; // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - std::transform(measurements.begin(), - measurements.end(), + std::transform(measurements.begin(), measurements.end(), std::back_inserter(undistortedMeasurements), [&cal, &pinholeCalibration](const Point2& measurement) { return undistortMeasurementInternal( @@ -301,7 +296,8 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& m /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ template <> -inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) { +inline Point2Vector undistortMeasurements(const Cal3_S2& cal, + const Point2Vector& measurements) { return measurements; } @@ -318,15 +314,17 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector */ template typename CAMERA::MeasurementVector undistortMeasurements( - const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); assert(nrMeasurements == cameras.size()); typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - undistortedMeasurements[ii] = undistortMeasurementInternal( - cameras[ii].calibration(), measurements[ii]); + undistortedMeasurements[ii] = + undistortMeasurementInternal( + cameras[ii].calibration(), measurements[ii]); } return undistortedMeasurements; } @@ -356,13 +354,12 @@ inline SphericalCamera::MeasurementVector undistortMeasurements( * @return homogeneous measurements in image plane */ template -inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal, - const Point2Vector& measurements) { +inline Point3Vector calibrateMeasurementsShared( + const CALIBRATION& cal, const Point2Vector& measurements) { Point3Vector calibratedMeasurements; // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. - std::transform(measurements.begin(), - measurements.end(), + std::transform(measurements.begin(), measurements.end(), std::back_inserter(calibratedMeasurements), [&cal](const Point2& measurement) { Point3 p; @@ -381,21 +378,25 @@ inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal, * @return homogeneous measurements in image plane */ template -inline Point3Vector calibrateMeasurements(const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measurements) { +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const typename CAMERA::MeasurementVector& measurements) { const size_t nrMeasurements = measurements.size(); assert(nrMeasurements == cameras.size()); Point3Vector calibratedMeasurements(nrMeasurements); for (size_t ii = 0; ii < nrMeasurements; ++ii) { - calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0; + calibratedMeasurements[ii] + << cameras[ii].calibration().calibrate(measurements[ii]), + 1.0; } return calibratedMeasurements; } /** Specialize for SphericalCamera to do nothing. */ template -inline Point3Vector calibrateMeasurements(const CameraSet& cameras, - const SphericalCamera::MeasurementVector& measurements) { +inline Point3Vector calibrateMeasurements( + const CameraSet& cameras, + const SphericalCamera::MeasurementVector& measurements) { Point3Vector calibratedMeasurements(measurements.size()); for (size_t ii = 0; ii < measurements.size(); ++ii) { calibratedMeasurements[ii] << measurements[ii].point3(); @@ -420,8 +421,7 @@ template Point3 triangulatePoint3(const std::vector& poses, std::shared_ptr sharedCal, const Point2Vector& measurements, - double rank_tol = 1e-9, - bool optimize = false, + double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { assert(poses.size() == measurements.size()); @@ -433,21 +433,25 @@ Point3 triangulatePoint3(const std::vector& poses, // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; - SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); // calibrate the measurements to obtain homogenous coordinates in image // plane. auto calibratedMeasurements = calibrateMeasurementsShared(*sharedCal, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, + rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = undistortMeasurements(*sharedCal, measurements); + auto undistortedMeasurements = + undistortMeasurements(*sharedCal, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -483,8 +487,7 @@ Point3 triangulatePoint3(const std::vector& poses, template Point3 triangulatePoint3(const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements, - double rank_tol = 1e-9, - bool optimize = false, + double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { size_t m = cameras.size(); @@ -498,7 +501,8 @@ Point3 triangulatePoint3(const CameraSet& cameras, // Reduce input noise model to an isotropic noise model using the mean of // the diagonal. const double measurementSigma = model ? model->sigmas().mean() : 1e-4; - SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma); + SharedIsotropic measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); // construct poses from cameras. std::vector poses; @@ -507,17 +511,21 @@ Point3 triangulatePoint3(const CameraSet& cameras, // calibrate the measurements to obtain homogenous coordinates in image // plane. - auto calibratedMeasurements = calibrateMeasurements(cameras, measurements); + auto calibratedMeasurements = + calibrateMeasurements(cameras, measurements); - point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol); + point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, + rank_tol); } else { // construct projection matrices from poses & calibration auto projection_matrices = projectionMatricesFromCameras(cameras); // Undistort the measurements, leaving only the pinhole elements in effect. - auto undistortedMeasurements = undistortMeasurements(cameras, measurements); + auto undistortedMeasurements = + undistortMeasurements(cameras, measurements); - point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); + point = + triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol); } // Then refine using non-linear optimization @@ -540,8 +548,7 @@ Point3 triangulatePoint3(const CameraSet& cameras, template Point3 triangulatePoint3(const CameraSet>& cameras, const Point2Vector& measurements, - double rank_tol = 1e-9, - bool optimize = false, + double rank_tol = 1e-9, bool optimize = false, const SharedNoiseModel& model = nullptr, const bool useLOST = false) { return triangulatePoint3> // @@ -549,16 +556,16 @@ Point3 triangulatePoint3(const CameraSet>& cameras, } struct GTSAM_EXPORT TriangulationParameters { - double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - ///< (the rank is the number of singular values of the triangulation matrix which are larger than - ///< rankTolerance) - bool enableEPI; ///< if set to true, will refine triangulation using LM + + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + ///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) + 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. */ - double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // /** * If this is nonnegative the we will check if the average reprojection error @@ -567,7 +574,7 @@ struct GTSAM_EXPORT TriangulationParameters { */ double dynamicOutlierRejectionThreshold; - SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation + SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation /** * Constructor @@ -579,36 +586,39 @@ struct GTSAM_EXPORT TriangulationParameters { * */ TriangulationParameters(const double _rankTolerance = 1.0, - const bool _enableEPI = false, - double _landmarkDistanceThreshold = -1, - double _dynamicOutlierRejectionThreshold = -1, - const SharedNoiseModel& _noiseModel = nullptr) - : rankTolerance(_rankTolerance), - enableEPI(_enableEPI), // - landmarkDistanceThreshold(_landmarkDistanceThreshold), // - dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), - noiseModel(_noiseModel) {} + const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, + double _dynamicOutlierRejectionThreshold = -1, + const SharedNoiseModel& _noiseModel = nullptr) : + rankTolerance(_rankTolerance), enableEPI(_enableEPI), // + landmarkDistanceThreshold(_landmarkDistanceThreshold), // + dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), + noiseModel(_noiseModel){ + } // stream to output - friend std::ostream& operator<<(std::ostream& os, const TriangulationParameters& p) { + friend std::ostream &operator<<(std::ostream &os, + const TriangulationParameters& p) { os << "rankTolerance = " << p.rankTolerance << std::endl; os << "enableEPI = " << p.enableEPI << std::endl; - os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl; - os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl; + os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold + << std::endl; + os << "dynamicOutlierRejectionThreshold = " + << p.dynamicOutlierRejectionThreshold << std::endl; os << "noise model" << std::endl; return os; } - private: +private: + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// /// Serialization function friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int version) { - ar& BOOST_SERIALIZATION_NVP(rankTolerance); - ar& BOOST_SERIALIZATION_NVP(enableEPI); - ar& BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); - ar& BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(rankTolerance); + ar & BOOST_SERIALIZATION_NVP(enableEPI); + ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); + ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); } #endif }; @@ -635,10 +645,16 @@ class TriangulationResult : public std::optional { * Constructor */ TriangulationResult(const Point3& p) : status(VALID) { emplace(p); } - static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); } + static TriangulationResult Degenerate() { + return TriangulationResult(DEGENERATE); + } static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); } - static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); } - static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); } + 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; } @@ -649,7 +665,8 @@ class TriangulationResult : public std::optional { return value(); } // 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 @@ -669,10 +686,11 @@ class TriangulationResult : public std::optional { }; /// triangulateSafe: extensive checking of the outcome -template +template TriangulationResult triangulateSafe(const CameraSet& cameras, - const typename CAMERA::MeasurementVector& measured, - const TriangulationParameters& params) { + const typename CAMERA::MeasurementVector& measured, + const TriangulationParameters& params) { + size_t m = cameras.size(); // if we have a single pose the corresponding factor is uninformative @@ -681,22 +699,25 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, else // We triangulate the 3D position of the landmark try { - Point3 point = triangulatePoint3( - cameras, measured, params.rankTolerance, params.enableEPI, params.noiseModel); + Point3 point = + triangulatePoint3(cameras, measured, params.rankTolerance, + params.enableEPI, params.noiseModel); // Check landmark distance and re-projection errors to avoid outliers size_t i = 0; double maxReprojError = 0.0; - for (const CAMERA& camera : cameras) { + for(const CAMERA& camera: cameras) { const Pose3& pose = camera.pose(); - if (params.landmarkDistanceThreshold > 0 && - distance3(pose.translation(), point) > params.landmarkDistanceThreshold) + if (params.landmarkDistanceThreshold > 0 + && distance3(pose.translation(), point) + > params.landmarkDistanceThreshold) return TriangulationResult::FarPoint(); #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION // verify that the triangulated point lies in front of all cameras // Only needed if this was not yet handled by exception const Point3& p_local = pose.transformTo(point); - if (p_local.z() <= 0) return TriangulationResult::BehindCamera(); + if (p_local.z() <= 0) + return TriangulationResult::BehindCamera(); #endif // Check reprojection error if (params.dynamicOutlierRejectionThreshold > 0) { @@ -707,21 +728,19 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, i += 1; } // Flag as degenerate if average reprojection error is too large - if (params.dynamicOutlierRejectionThreshold > 0 && - maxReprojError > params.dynamicOutlierRejectionThreshold) + if (params.dynamicOutlierRejectionThreshold > 0 + && maxReprojError > params.dynamicOutlierRejectionThreshold) return TriangulationResult::Outlier(); // all good! return TriangulationResult(point); } catch (TriangulationUnderconstrainedException&) { // This exception is thrown if - // 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 cameras (or motion towards the landmark) + // 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 cameras (or motion towards the landmark) return TriangulationResult::Degenerate(); } catch (TriangulationCheiralityException&) { - // point is behind one of the cameras: can be the case of close-to-parallel cameras or may - // depend on outliers + // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers return TriangulationResult::BehindCamera(); } } @@ -733,4 +752,4 @@ using CameraSetCal3DS2 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; -} // namespace gtsam +} // \namespace gtsam From bba4b8731f68652398e4d1b85224e13e89517917 Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Tue, 6 Jun 2023 10:01:46 -0700 Subject: [PATCH 5/6] Final cleanup --- gtsam/geometry/triangulation.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index e714bb432..c8af2ea72 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -25,8 +25,8 @@ namespace gtsam { Vector4 triangulateHomogeneousDLT( - const std::vector>& - projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -56,8 +56,8 @@ Vector4 triangulateHomogeneousDLT( } Vector4 triangulateHomogeneousDLT( - const std::vector>& - projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras size_t m = projection_matrices.size(); @@ -68,7 +68,7 @@ Vector4 triangulateHomogeneousDLT( for (size_t i = 0; i < m; i++) { size_t row = i * 2; const Matrix34& projection = projection_matrices.at(i); - const Point3& p = + const Point3& p = measurements.at(i) .point3(); // to get access to x,y,z of the bearing vector @@ -130,7 +130,7 @@ Point3 triangulateLOST(const std::vector& poses, // Note: Setting q_i = 1.0 gives same results as DLT. const double q_i = num_i / (measurementNoise->sigma() * den_i); - const Matrix23 coefficientMat = + const Matrix23 coefficientMat = q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * wTi.rotation().matrix().transpose(); @@ -147,8 +147,8 @@ Point3 triangulateLOST(const std::vector& poses, } Point3 triangulateDLT( - const std::vector>& - projection_matrices, + const std::vector>& + projection_matrices, const Point2Vector& measurements, double rank_tol) { Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); @@ -161,7 +161,7 @@ Point3 triangulateDLT( projection_matrices, const std::vector& measurements, double rank_tol) { // contrary to previous triangulateDLT, this is now taking Unit3 inputs - Vector4 v = + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); @@ -174,7 +174,7 @@ Point3 triangulateDLT( * @param landmarkKey to refer to landmark * @return refined Point3 */ -Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, +Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) { // Maybe we should consider Gauss-Newton? LevenbergMarquardtParams params; From 1e6f1b757cdcc02a810d3e425009017fcf62bbcc Mon Sep 17 00:00:00 2001 From: Travis Driver Date: Tue, 6 Jun 2023 10:11:49 -0700 Subject: [PATCH 6/6] Final final cleanup --- gtsam/geometry/triangulation.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index c8af2ea72..039dfd74d 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -150,15 +150,15 @@ Point3 triangulateDLT( const std::vector>& projection_matrices, const Point2Vector& measurements, double rank_tol) { - Vector4 v = + Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); // Create 3D point from homogeneous coordinates return Point3(v.head<3>() / v[3]); } Point3 triangulateDLT( - const std::vector>& - projection_matrices, + const std::vector>& + projection_matrices, const std::vector& measurements, double rank_tol) { // contrary to previous triangulateDLT, this is now taking Unit3 inputs Vector4 v =