12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added rank threshold to triangulateLOST

release/4.3a0
Travis Driver 2023-06-02 15:44:51 -07:00
parent 0a7c1d8712
commit 3eec88f60e
2 changed files with 173 additions and 189 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
gtsam/geometry/triangulation.cpp
View File

@ -25,9 +25,9 @@
namespace gtsam { namespace gtsam {
Vector4 triangulateHomogeneousDLT( Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
projection_matrices, const Point2Vector& measurements,
const Point2Vector& measurements, double rank_tol) { double rank_tol) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -56,9 +56,9 @@ Vector4 triangulateHomogeneousDLT(
} }
Vector4 triangulateHomogeneousDLT( Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
projection_matrices, const std::vector<Unit3>& measurements,
const std::vector<Unit3>& measurements, double rank_tol) { double rank_tol) {
// number of cameras // number of cameras
size_t m = projection_matrices.size(); size_t m = projection_matrices.size();
@ -68,9 +68,7 @@ Vector4 triangulateHomogeneousDLT(
for (size_t i = 0; i < m; i++) { for (size_t i = 0; i < m; i++) {
size_t row = i * 2; size_t row = i * 2;
const Matrix34& projection = projection_matrices.at(i); 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
measurements.at(i)
.point3(); // to get access to x,y,z of the bearing vector
// build system of equations // build system of equations
A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0); A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
@ -85,7 +83,8 @@ Vector4 triangulateHomogeneousDLT(
Point3 triangulateLOST(const std::vector<Pose3>& poses, Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3Vector& calibratedMeasurements, const Point3Vector& calibratedMeasurements,
const SharedIsotropic& measurementNoise) { const SharedIsotropic& measurementNoise,
double rank_tol) {
size_t m = calibratedMeasurements.size(); size_t m = calibratedMeasurements.size();
assert(m == poses.size()); assert(m == poses.size());
@ -105,36 +104,39 @@ Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]); const Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
// Note: Setting q_i = 1.0 gives same results as DLT. // Note: Setting q_i = 1.0 gives same results as DLT.
const double q_i = wZi.cross(wZj).norm() / const double q_i = wZi.cross(wZj).norm() / (measurementNoise->sigma() * d_ij.cross(wZj).norm());
(measurementNoise->sigma() * d_ij.cross(wZj).norm());
const Matrix23 coefficientMat = const Matrix23 coefficientMat = q_i *
q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
wTi.rotation().matrix().transpose(); wTi.rotation().matrix().transpose();
A.block<2, 3>(2 * i, 0) << coefficientMat; A.block<2, 3>(2 * i, 0) << coefficientMat;
b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation(); b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
} }
return A.colPivHouseholderQr().solve(b);
Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
A_Qr.setThreshold(rank_tol);
// if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
return A_Qr.solve(b);
} }
Point3 triangulateDLT( Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
projection_matrices, const Point2Vector& measurements,
const Point2Vector& measurements, double rank_tol) { double rank_tol) {
Vector4 v = Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(v.head<3>() / v[3]); return Point3(v.head<3>() / v[3]);
} }
Point3 triangulateDLT( Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
projection_matrices, const std::vector<Unit3>& measurements,
const std::vector<Unit3>& measurements, double rank_tol) { double rank_tol) {
// contrary to previous triangulateDLT, this is now taking Unit3 inputs // contrary to previous triangulateDLT, this is now taking Unit3 inputs
Vector4 v = Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(v.head<3>() / v[3]); return Point3(v.head<3>() / v[3]);
} }
@ -146,8 +148,7 @@ Point3 triangulateDLT(
* @param landmarkKey to refer to landmark * @param landmarkKey to refer to landmark
* @return refined Point3 * @return refined Point3
*/ */
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Key landmarkKey) {
Key landmarkKey) {
// Maybe we should consider Gauss-Newton? // Maybe we should consider Gauss-Newton?
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

305
gtsam/geometry/triangulation.h
View File

@ -20,39 +20,38 @@
#pragma once #pragma once
#include "gtsam/geometry/Point3.h"
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h> #include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h> #include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h> #include <gtsam/slam/TriangulationFactor.h>
#include "gtsam/geometry/Point3.h"
#include <optional> #include <optional>
namespace gtsam { namespace gtsam {
/// Exception thrown by triangulateDLT when SVD returns rank < 3 /// Exception thrown by triangulateDLT when SVD returns rank < 3
class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error { class GTSAM_EXPORT TriangulationUnderconstrainedException : public std::runtime_error {
public: public:
TriangulationUnderconstrainedException() : TriangulationUnderconstrainedException()
std::runtime_error("Triangulation Underconstrained Exception.") { : std::runtime_error("Triangulation Underconstrained Exception.") {}
}
}; };
/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error { class GTSAM_EXPORT TriangulationCheiralityException : public std::runtime_error {
public: public:
TriangulationCheiralityException() : TriangulationCheiralityException()
std::runtime_error( : std::runtime_error(
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { "Triangulation Cheirality Exception: The resulting landmark is behind one or more "
} "cameras.") {}
}; };
/** /**
@ -64,11 +63,13 @@ public:
*/ */
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& 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 * 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 projection_matrices Projection matrices (K*P^-1)
* @param measurements Unit3 bearing measurements * @param measurements Unit3 bearing measurements
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
@ -76,7 +77,8 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
*/ */
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol = 1e-9); const std::vector<Unit3>& measurements,
double rank_tol = 1e-9);
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@ -85,18 +87,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @return Triangulated Point3 * @return Triangulated Point3
*/ */
GTSAM_EXPORT Point3 triangulateDLT( GTSAM_EXPORT Point3
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements, const Point2Vector& measurements,
double rank_tol = 1e-9); double rank_tol = 1e-9);
/** /**
* overload of previous function to work with Unit3 (projected to canonical camera) * overload of previous function to work with Unit3 (projected to canonical camera)
*/ */
GTSAM_EXPORT Point3 triangulateDLT( GTSAM_EXPORT Point3
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, const std::vector<Unit3>& measurements,
double rank_tol = 1e-9); double rank_tol = 1e-9);
/** /**
* @brief Triangulation using the LOST (Linear Optimal Sine Triangulation) * @brief Triangulation using the LOST (Linear Optimal Sine Triangulation)
@ -110,7 +112,8 @@ GTSAM_EXPORT Point3 triangulateDLT(
*/ */
GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses, GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
const Point3Vector& calibratedMeasurements, 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 * Create a factor graph with projection factors from poses and one calibration
@ -121,20 +124,22 @@ GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
* @param initialEstimate * @param initialEstimate
* @return graph and initial values * @return graph and initial values
*/ */
template<class CALIBRATION> template <class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal, const std::vector<Pose3>& poses,
const Point2Vector& measurements, Key landmarkKey, std::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements,
Key landmarkKey,
const Point3& initialEstimate, const Point3& initialEstimate,
const SharedNoiseModel& model = noiseModel::Unit::Create(2)) { const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i]; const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera; typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal); Camera camera_i(pose_i, sharedCal);
graph.emplace_shared<TriangulationFactor<Camera> > // graph.emplace_shared<TriangulationFactor<Camera>> //
(camera_i, measurements[i], model, landmarkKey); (camera_i, measurements[i], model, landmarkKey);
} }
return {graph, values}; return {graph, values};
@ -149,21 +154,22 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @param initialEstimate * @param initialEstimate
* @return graph and initial values * @return graph and initial values
*/ */
template<class CAMERA> template <class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, Key landmarkKey, const typename CAMERA::MeasurementVector& measurements,
Key landmarkKey,
const Point3& initialEstimate, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) { const SharedNoiseModel& model = nullptr) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create( static SharedNoiseModel unit(
traits<typename CAMERA::Measurement>::dimension)); noiseModel::Unit::Create(traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i]; const CAMERA& camera_i = cameras[i];
graph.emplace_shared<TriangulationFactor<CAMERA> > // graph.emplace_shared<TriangulationFactor<CAMERA>> //
(camera_i, measurements[i], model? model : unit, landmarkKey); (camera_i, measurements[i], model ? model : unit, landmarkKey);
} }
return {graph, values}; return {graph, values};
} }
@ -176,7 +182,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @return refined Point3 * @return refined Point3
*/ */
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, 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 * 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 * @param initialEstimate
* @return refined Point3 * @return refined Point3
*/ */
template<class CALIBRATION> template <class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses, Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
std::shared_ptr<CALIBRATION> sharedCal, std::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point3& initialEstimate, const Point2Vector& measurements,
const SharedNoiseModel& model = nullptr) { const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values // Create a factor graph and initial values
const auto [graph, values] = triangulationGraph<CALIBRATION> // const auto [graph, values] = triangulationGraph<CALIBRATION> //
(poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model); (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
@ -206,33 +213,32 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
* @param initialEstimate * @param initialEstimate
* @return refined Point3 * @return refined Point3
*/ */
template<class CAMERA> template <class CAMERA>
Point3 triangulateNonlinear( Point3 triangulateNonlinear(const CameraSet<CAMERA>& cameras,
const CameraSet<CAMERA>& cameras, const typename CAMERA::MeasurementVector& measurements,
const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate, const Point3& initialEstimate,
const SharedNoiseModel& model = nullptr) { const SharedNoiseModel& model = nullptr) {
// Create a factor graph and initial values // Create a factor graph and initial values
const auto [graph, values] = triangulationGraph<CAMERA> // const auto [graph, values] = triangulationGraph<CAMERA> //
(cameras, measurements, Symbol('p', 0), initialEstimate, model); (cameras, measurements, Symbol('p', 0), initialEstimate, model);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
template<class CAMERA> template <class CAMERA>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromCameras(
projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) { const CameraSet<CAMERA>& cameras) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (const CAMERA &camera: cameras) { for (const CAMERA& camera : cameras) {
projection_matrices.push_back(camera.cameraProjectionMatrix()); projection_matrices.push_back(camera.cameraProjectionMatrix());
} }
return projection_matrices; return projection_matrices;
} }
// overload, assuming pinholePose // overload, assuming pinholePose
template<class CALIBRATION> template <class CALIBRATION>
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses( std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION> sharedCal) { const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal) {
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (size_t i = 0; i < poses.size(); i++) { for (size_t i = 0; i < poses.size(); i++) {
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal); PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
@ -257,9 +263,9 @@ Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
/** Internal undistortMeasurement to be used by undistortMeasurement and /** Internal undistortMeasurement to be used by undistortMeasurement and
* undistortMeasurements */ * undistortMeasurements */
template <class CALIBRATION, class MEASUREMENT> template <class CALIBRATION, class MEASUREMENT>
MEASUREMENT undistortMeasurementInternal( MEASUREMENT undistortMeasurementInternal(const CALIBRATION& cal,
const CALIBRATION& cal, const MEASUREMENT& measurement, const MEASUREMENT& measurement,
std::optional<Cal3_S2> pinholeCal = {}) { std::optional<Cal3_S2> pinholeCal = {}) {
if (!pinholeCal) { if (!pinholeCal) {
pinholeCal = createPinholeCalibration(cal); pinholeCal = createPinholeCalibration(cal);
} }
@ -278,13 +284,13 @@ MEASUREMENT undistortMeasurementInternal(
* @return measurements with the effect of the distortion of sharedCal removed. * @return measurements with the effect of the distortion of sharedCal removed.
*/ */
template <class CALIBRATION> template <class CALIBRATION>
Point2Vector undistortMeasurements(const CALIBRATION& cal, Point2Vector undistortMeasurements(const CALIBRATION& cal, const Point2Vector& measurements) {
const Point2Vector& measurements) {
Cal3_S2 pinholeCalibration = createPinholeCalibration(cal); Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
Point2Vector undistortedMeasurements; Point2Vector undistortedMeasurements;
// Calibrate with cal and uncalibrate with pinhole version of cal so that // Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted. // measurements are undistorted.
std::transform(measurements.begin(), measurements.end(), std::transform(measurements.begin(),
measurements.end(),
std::back_inserter(undistortedMeasurements), std::back_inserter(undistortedMeasurements),
[&cal, &pinholeCalibration](const Point2& measurement) { [&cal, &pinholeCalibration](const Point2& measurement) {
return undistortMeasurementInternal<CALIBRATION>( return undistortMeasurementInternal<CALIBRATION>(
@ -295,8 +301,7 @@ Point2Vector undistortMeasurements(const CALIBRATION& cal,
/** Specialization for Cal3_S2 as it doesn't need to be undistorted. */ /** Specialization for Cal3_S2 as it doesn't need to be undistorted. */
template <> template <>
inline Point2Vector undistortMeasurements(const Cal3_S2& cal, inline Point2Vector undistortMeasurements(const Cal3_S2& cal, const Point2Vector& measurements) {
const Point2Vector& measurements) {
return measurements; return measurements;
} }
@ -313,17 +318,15 @@ inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
*/ */
template <class CAMERA> template <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements( typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA>& cameras, const typename CAMERA::MeasurementVector& measurements) {
const typename CAMERA::MeasurementVector& measurements) {
const size_t nrMeasurements = measurements.size(); const size_t nrMeasurements = measurements.size();
assert(nrMeasurements == cameras.size()); assert(nrMeasurements == cameras.size());
typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < nrMeasurements; ++ii) { for (size_t ii = 0; ii < nrMeasurements; ++ii) {
// Calibrate with cal and uncalibrate with pinhole version of cal so that // Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted. // measurements are undistorted.
undistortedMeasurements[ii] = undistortedMeasurements[ii] = undistortMeasurementInternal<typename CAMERA::CalibrationType>(
undistortMeasurementInternal<typename CAMERA::CalibrationType>( cameras[ii].calibration(), measurements[ii]);
cameras[ii].calibration(), measurements[ii]);
} }
return undistortedMeasurements; return undistortedMeasurements;
} }
@ -353,12 +356,13 @@ inline SphericalCamera::MeasurementVector undistortMeasurements(
* @return homogeneous measurements in image plane * @return homogeneous measurements in image plane
*/ */
template <class CALIBRATION> template <class CALIBRATION>
inline Point3Vector calibrateMeasurementsShared( inline Point3Vector calibrateMeasurementsShared(const CALIBRATION& cal,
const CALIBRATION& cal, const Point2Vector& measurements) { const Point2Vector& measurements) {
Point3Vector calibratedMeasurements; Point3Vector calibratedMeasurements;
// Calibrate with cal and uncalibrate with pinhole version of cal so that // Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted. // measurements are undistorted.
std::transform(measurements.begin(), measurements.end(), std::transform(measurements.begin(),
measurements.end(),
std::back_inserter(calibratedMeasurements), std::back_inserter(calibratedMeasurements),
[&cal](const Point2& measurement) { [&cal](const Point2& measurement) {
Point3 p; Point3 p;
@ -377,25 +381,21 @@ inline Point3Vector calibrateMeasurementsShared(
* @return homogeneous measurements in image plane * @return homogeneous measurements in image plane
*/ */
template <class CAMERA> template <class CAMERA>
inline Point3Vector calibrateMeasurements( inline Point3Vector calibrateMeasurements(const CameraSet<CAMERA>& cameras,
const CameraSet<CAMERA>& cameras, const typename CAMERA::MeasurementVector& measurements) {
const typename CAMERA::MeasurementVector& measurements) {
const size_t nrMeasurements = measurements.size(); const size_t nrMeasurements = measurements.size();
assert(nrMeasurements == cameras.size()); assert(nrMeasurements == cameras.size());
Point3Vector calibratedMeasurements(nrMeasurements); Point3Vector calibratedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < nrMeasurements; ++ii) { for (size_t ii = 0; ii < nrMeasurements; ++ii) {
calibratedMeasurements[ii] calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0;
<< cameras[ii].calibration().calibrate(measurements[ii]),
1.0;
} }
return calibratedMeasurements; return calibratedMeasurements;
} }
/** Specialize for SphericalCamera to do nothing. */ /** Specialize for SphericalCamera to do nothing. */
template <class CAMERA = SphericalCamera> template <class CAMERA = SphericalCamera>
inline Point3Vector calibrateMeasurements( inline Point3Vector calibrateMeasurements(const CameraSet<SphericalCamera>& cameras,
const CameraSet<SphericalCamera>& cameras, const SphericalCamera::MeasurementVector& measurements) {
const SphericalCamera::MeasurementVector& measurements) {
Point3Vector calibratedMeasurements(measurements.size()); Point3Vector calibratedMeasurements(measurements.size());
for (size_t ii = 0; ii < measurements.size(); ++ii) { for (size_t ii = 0; ii < measurements.size(); ++ii) {
calibratedMeasurements[ii] << measurements[ii].point3(); calibratedMeasurements[ii] << measurements[ii].point3();
@ -420,7 +420,8 @@ template <class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, Point3 triangulatePoint3(const std::vector<Pose3>& poses,
std::shared_ptr<CALIBRATION> sharedCal, std::shared_ptr<CALIBRATION> sharedCal,
const Point2Vector& measurements, const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr, const SharedNoiseModel& model = nullptr,
const bool useLOST = false) { const bool useLOST = false) {
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
@ -432,24 +433,21 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// Reduce input noise model to an isotropic noise model using the mean of // Reduce input noise model to an isotropic noise model using the mean of
// the diagonal. // the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4; const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
SharedIsotropic measurementNoise = SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
noiseModel::Isotropic::Sigma(2, measurementSigma);
// calibrate the measurements to obtain homogenous coordinates in image // calibrate the measurements to obtain homogenous coordinates in image
// plane. // plane.
auto calibratedMeasurements = auto calibratedMeasurements =
calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements); calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
} else { } else {
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal); auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
// Undistort the measurements, leaving only the pinhole elements in effect. // Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements = auto undistortedMeasurements = undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
point = point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
} }
// Then refine using non-linear optimization // Then refine using non-linear optimization
@ -485,7 +483,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
template <class CAMERA> template <class CAMERA>
Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras, Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements, 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 SharedNoiseModel& model = nullptr,
const bool useLOST = false) { const bool useLOST = false) {
size_t m = cameras.size(); size_t m = cameras.size();
@ -499,8 +498,7 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
// Reduce input noise model to an isotropic noise model using the mean of // Reduce input noise model to an isotropic noise model using the mean of
// the diagonal. // the diagonal.
const double measurementSigma = model ? model->sigmas().mean() : 1e-4; const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
SharedIsotropic measurementNoise = SharedIsotropic measurementNoise = noiseModel::Isotropic::Sigma(2, measurementSigma);
noiseModel::Isotropic::Sigma(2, measurementSigma);
// construct poses from cameras. // construct poses from cameras.
std::vector<Pose3> poses; std::vector<Pose3> poses;
@ -509,20 +507,17 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
// calibrate the measurements to obtain homogenous coordinates in image // calibrate the measurements to obtain homogenous coordinates in image
// plane. // plane.
auto calibratedMeasurements = auto calibratedMeasurements = calibrateMeasurements<CAMERA>(cameras, measurements);
calibrateMeasurements<CAMERA>(cameras, measurements);
point = triangulateLOST(poses, calibratedMeasurements, measurementNoise); point = triangulateLOST(poses, calibratedMeasurements, measurementNoise, rank_tol);
} else { } else {
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
auto projection_matrices = projectionMatricesFromCameras(cameras); auto projection_matrices = projectionMatricesFromCameras(cameras);
// Undistort the measurements, leaving only the pinhole elements in effect. // Undistort the measurements, leaving only the pinhole elements in effect.
auto undistortedMeasurements = auto undistortedMeasurements = undistortMeasurements<CAMERA>(cameras, measurements);
undistortMeasurements<CAMERA>(cameras, measurements);
point = point = triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
} }
// Then refine using non-linear optimization // Then refine using non-linear optimization
@ -545,7 +540,8 @@ Point3 triangulatePoint3(const CameraSet<CAMERA>& cameras,
template <class CALIBRATION> template <class CALIBRATION>
Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras, Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
const Point2Vector& measurements, const Point2Vector& measurements,
double rank_tol = 1e-9, bool optimize = false, double rank_tol = 1e-9,
bool optimize = false,
const SharedNoiseModel& model = nullptr, const SharedNoiseModel& model = nullptr,
const bool useLOST = false) { const bool useLOST = false) {
return triangulatePoint3<PinholeCamera<CALIBRATION>> // return triangulatePoint3<PinholeCamera<CALIBRATION>> //
@ -553,16 +549,16 @@ Point3 triangulatePoint3(const CameraSet<PinholeCamera<CALIBRATION>>& cameras,
} }
struct GTSAM_EXPORT TriangulationParameters { struct GTSAM_EXPORT TriangulationParameters {
double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
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
///< (the rank is the number of singular values of the triangulation matrix which are larger than rankTolerance) ///< rankTolerance)
bool enableEPI; ///< if set to true, will refine triangulation using LM bool enableEPI; ///< if set to true, will refine triangulation using LM
/** /**
* if the landmark is triangulated at distance larger than this, * if the landmark is triangulated at distance larger than this,
* result is flagged as degenerate. * result is flagged as degenerate.
*/ */
double landmarkDistanceThreshold; // double landmarkDistanceThreshold; //
/** /**
* If this is nonnegative the we will check if the average reprojection error * If this is nonnegative the we will check if the average reprojection error
@ -571,7 +567,7 @@ struct GTSAM_EXPORT TriangulationParameters {
*/ */
double dynamicOutlierRejectionThreshold; double dynamicOutlierRejectionThreshold;
SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation SharedNoiseModel noiseModel; ///< used in the nonlinear triangulation
/** /**
* Constructor * Constructor
@ -583,39 +579,36 @@ struct GTSAM_EXPORT TriangulationParameters {
* *
*/ */
TriangulationParameters(const double _rankTolerance = 1.0, TriangulationParameters(const double _rankTolerance = 1.0,
const bool _enableEPI = false, double _landmarkDistanceThreshold = -1, const bool _enableEPI = false,
double _dynamicOutlierRejectionThreshold = -1, double _landmarkDistanceThreshold = -1,
const SharedNoiseModel& _noiseModel = nullptr) : double _dynamicOutlierRejectionThreshold = -1,
rankTolerance(_rankTolerance), enableEPI(_enableEPI), // const SharedNoiseModel& _noiseModel = nullptr)
landmarkDistanceThreshold(_landmarkDistanceThreshold), // : rankTolerance(_rankTolerance),
dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold), enableEPI(_enableEPI), //
noiseModel(_noiseModel){ landmarkDistanceThreshold(_landmarkDistanceThreshold), //
} dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
noiseModel(_noiseModel) {}
// stream to output // stream to output
friend std::ostream &operator<<(std::ostream &os, friend std::ostream& operator<<(std::ostream& os, const TriangulationParameters& p) {
const TriangulationParameters& p) {
os << "rankTolerance = " << p.rankTolerance << std::endl; os << "rankTolerance = " << p.rankTolerance << std::endl;
os << "enableEPI = " << p.enableEPI << std::endl; os << "enableEPI = " << p.enableEPI << std::endl;
os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold << std::endl;
<< std::endl; os << "dynamicOutlierRejectionThreshold = " << p.dynamicOutlierRejectionThreshold << std::endl;
os << "dynamicOutlierRejectionThreshold = "
<< p.dynamicOutlierRejectionThreshold << std::endl;
os << "noise model" << std::endl; os << "noise model" << std::endl;
return os; return os;
} }
private: private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION ///
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE& ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(rankTolerance); ar& BOOST_SERIALIZATION_NVP(rankTolerance);
ar & BOOST_SERIALIZATION_NVP(enableEPI); ar& BOOST_SERIALIZATION_NVP(enableEPI);
ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold); ar& BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold); ar& BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
} }
#endif #endif
}; };
@ -642,16 +635,10 @@ class TriangulationResult : public std::optional<Point3> {
* Constructor * Constructor
*/ */
TriangulationResult(const Point3& p) : status(VALID) { emplace(p); } TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
static TriangulationResult Degenerate() { static TriangulationResult Degenerate() { return TriangulationResult(DEGENERATE); }
return TriangulationResult(DEGENERATE);
}
static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); } static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
static TriangulationResult FarPoint() { static TriangulationResult FarPoint() { return TriangulationResult(FAR_POINT); }
return TriangulationResult(FAR_POINT); static TriangulationResult BehindCamera() { return TriangulationResult(BEHIND_CAMERA); }
}
static TriangulationResult BehindCamera() {
return TriangulationResult(BEHIND_CAMERA);
}
bool valid() const { return status == VALID; } bool valid() const { return status == VALID; }
bool degenerate() const { return status == DEGENERATE; } bool degenerate() const { return status == DEGENERATE; }
bool outlier() const { return status == OUTLIER; } bool outlier() const { return status == OUTLIER; }
@ -662,8 +649,7 @@ class TriangulationResult : public std::optional<Point3> {
return value(); return value();
} }
// stream to output // stream to output
friend std::ostream& operator<<(std::ostream& os, friend std::ostream& operator<<(std::ostream& os, const TriangulationResult& result) {
const TriangulationResult& result) {
if (result) if (result)
os << "point = " << *result << std::endl; os << "point = " << *result << std::endl;
else else
@ -683,11 +669,10 @@ class TriangulationResult : public std::optional<Point3> {
}; };
/// triangulateSafe: extensive checking of the outcome /// triangulateSafe: extensive checking of the outcome
template<class CAMERA> template <class CAMERA>
TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras, TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measured, const typename CAMERA::MeasurementVector& measured,
const TriangulationParameters& params) { const TriangulationParameters& params) {
size_t m = cameras.size(); size_t m = cameras.size();
// if we have a single pose the corresponding factor is uninformative // if we have a single pose the corresponding factor is uninformative
@ -696,25 +681,22 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
else else
// We triangulate the 3D position of the landmark // We triangulate the 3D position of the landmark
try { try {
Point3 point = Point3 point = triangulatePoint3<CAMERA>(
triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance, cameras, measured, params.rankTolerance, params.enableEPI, params.noiseModel);
params.enableEPI, params.noiseModel);
// Check landmark distance and re-projection errors to avoid outliers // Check landmark distance and re-projection errors to avoid outliers
size_t i = 0; size_t i = 0;
double maxReprojError = 0.0; double maxReprojError = 0.0;
for(const CAMERA& camera: cameras) { for (const CAMERA& camera : cameras) {
const Pose3& pose = camera.pose(); const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0 if (params.landmarkDistanceThreshold > 0 &&
&& distance3(pose.translation(), point) distance3(pose.translation(), point) > params.landmarkDistanceThreshold)
> params.landmarkDistanceThreshold)
return TriangulationResult::FarPoint(); return TriangulationResult::FarPoint();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
// verify that the triangulated point lies in front of all cameras // verify that the triangulated point lies in front of all cameras
// Only needed if this was not yet handled by exception // Only needed if this was not yet handled by exception
const Point3& p_local = pose.transformTo(point); const Point3& p_local = pose.transformTo(point);
if (p_local.z() <= 0) if (p_local.z() <= 0) return TriangulationResult::BehindCamera();
return TriangulationResult::BehindCamera();
#endif #endif
// Check reprojection error // Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) { if (params.dynamicOutlierRejectionThreshold > 0) {
@ -725,19 +707,21 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
i += 1; i += 1;
} }
// Flag as degenerate if average reprojection error is too large // Flag as degenerate if average reprojection error is too large
if (params.dynamicOutlierRejectionThreshold > 0 if (params.dynamicOutlierRejectionThreshold > 0 &&
&& maxReprojError > params.dynamicOutlierRejectionThreshold) maxReprojError > params.dynamicOutlierRejectionThreshold)
return TriangulationResult::Outlier(); return TriangulationResult::Outlier();
// all good! // all good!
return TriangulationResult(point); return TriangulationResult(point);
} catch (TriangulationUnderconstrainedException&) { } catch (TriangulationUnderconstrainedException&) {
// This exception is thrown if // 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 // 1) There is a single pose for triangulation - this should not happen because we checked the
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark) // 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(); return TriangulationResult::Degenerate();
} catch (TriangulationCheiralityException&) { } 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(); return TriangulationResult::BehindCamera();
} }
} }
@ -749,5 +733,4 @@ using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>; using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>; using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>; using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam } // namespace gtsam