34#include <gtsam/slam/TriangulationFactor.h>
42 std::runtime_error(
"Triangulation Underconstrained Exception.") {
51 "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
63 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
64 const Point2Vector& measurements,
double rank_tol = 1e-9);
75 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
76 const std::vector<Unit3>& measurements,
double rank_tol = 1e-9);
86 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
87 const Point2Vector& measurements,
88 double rank_tol = 1e-9);
94 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
95 const std::vector<Unit3>& measurements,
96 double rank_tol = 1e-9);
109 const Point3Vector& calibratedMeasurements,
110 const SharedIsotropic& measurementNoise);
121template<
class CALIBRATION>
123 const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
124 const Point2Vector& measurements,
Key landmarkKey,
125 const Point3& initialEstimate,
128 values.
insert(landmarkKey, initialEstimate);
130 for (
size_t i = 0; i < measurements.size(); i++) {
131 const Pose3& pose_i = poses[i];
133 Camera camera_i(pose_i, sharedCal);
135 (camera_i, measurements[i], model, landmarkKey);
137 return std::make_pair(graph, values);
149template<
class CAMERA>
152 const typename CAMERA::MeasurementVector& measurements,
Key landmarkKey,
153 const Point3& initialEstimate,
156 values.
insert(landmarkKey, initialEstimate);
160 for (
size_t i = 0; i < measurements.size(); i++) {
161 const CAMERA& camera_i = cameras[i];
163 (camera_i, measurements[i], model? model : unit, landmarkKey);
165 return std::make_pair(graph, values);
186template<
class CALIBRATION>
188 boost::shared_ptr<CALIBRATION> sharedCal,
189 const Point2Vector& measurements,
const Point3& initialEstimate,
195 boost::tie(graph, values) = triangulationGraph<CALIBRATION>
196 (poses, sharedCal, measurements,
Symbol(
'p', 0), initialEstimate, model);
208template<
class CAMERA>
211 const typename CAMERA::MeasurementVector& measurements,
const Point3& initialEstimate,
217 boost::tie(graph, values) = triangulationGraph<CAMERA>
218 (cameras, measurements,
Symbol(
'p', 0), initialEstimate, model);
223template<
class CAMERA>
224std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
225projectionMatricesFromCameras(
const CameraSet<CAMERA> &cameras) {
226 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
227 for (
const CAMERA &camera: cameras) {
228 projection_matrices.push_back(camera.cameraProjectionMatrix());
230 return projection_matrices;
234template<
class CALIBRATION>
235std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
236 const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
237 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
238 for (
size_t i = 0; i < poses.size(); i++) {
239 PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
240 projection_matrices.push_back(camera.cameraProjectionMatrix());
242 return projection_matrices;
252template <
class CALIBRATION>
254 const auto& K = cal.K();
255 return Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2));
260template <
class CALIBRATION,
class MEASUREMENT>
262 const CALIBRATION& cal,
const MEASUREMENT& measurement,
263 boost::optional<Cal3_S2> pinholeCal = boost::none) {
267 return pinholeCal->uncalibrate(cal.calibrate(measurement));
281template <
class CALIBRATION>
283 const Point2Vector& measurements) {
285 Point2Vector undistortedMeasurements;
288 std::transform(measurements.begin(), measurements.end(),
289 std::back_inserter(undistortedMeasurements),
290 [&cal, &pinholeCalibration](
const Point2& measurement) {
291 return undistortMeasurementInternal<CALIBRATION>(
292 cal, measurement, pinholeCalibration);
294 return undistortedMeasurements;
300 const Point2Vector& measurements) {
315template <
class CAMERA>
318 const typename CAMERA::MeasurementVector& measurements) {
319 const size_t nrMeasurements = measurements.size();
320 assert(nrMeasurements == cameras.size());
321 typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
322 for (
size_t ii = 0; ii < nrMeasurements; ++ii) {
325 undistortedMeasurements[ii] =
326 undistortMeasurementInternal<typename CAMERA::CalibrationType>(
327 cameras[ii].calibration(), measurements[ii]);
329 return undistortedMeasurements;
333template <
class CAMERA = PinholeCamera<Cal3_S2>>
336 const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
341template <
class CAMERA = SphericalCamera>
344 const SphericalCamera::MeasurementVector& measurements) {
356template <
class CALIBRATION>
358 const CALIBRATION& cal,
const Point2Vector& measurements) {
359 Point3Vector calibratedMeasurements;
362 std::transform(measurements.begin(), measurements.end(),
363 std::back_inserter(calibratedMeasurements),
364 [&cal](
const Point2& measurement) {
366 p << cal.calibrate(measurement), 1.0;
369 return calibratedMeasurements;
380template <
class CAMERA>
383 const typename CAMERA::MeasurementVector& measurements) {
384 const size_t nrMeasurements = measurements.size();
385 assert(nrMeasurements == cameras.size());
386 Point3Vector calibratedMeasurements(nrMeasurements);
387 for (
size_t ii = 0; ii < nrMeasurements; ++ii) {
388 calibratedMeasurements[ii]
389 << cameras[ii].calibration().calibrate(measurements[ii]),
392 return calibratedMeasurements;
396template <
class CAMERA = SphericalCamera>
399 const SphericalCamera::MeasurementVector& measurements) {
400 Point3Vector calibratedMeasurements(measurements.size());
401 for (
size_t ii = 0; ii < measurements.size(); ++ii) {
402 calibratedMeasurements[ii] << measurements[ii].point3();
404 return calibratedMeasurements;
420template <
class CALIBRATION>
422 boost::shared_ptr<CALIBRATION> sharedCal,
423 const Point2Vector& measurements,
424 double rank_tol = 1e-9,
bool optimize =
false,
426 const bool useLOST =
false) {
427 assert(poses.size() == measurements.size());
435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
436 SharedIsotropic measurementNoise =
440 auto calibratedMeasurements =
441 calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
443 point =
triangulateLOST(poses, calibratedMeasurements, measurementNoise);
446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
449 auto undistortedMeasurements =
450 undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
453 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
458 point = triangulateNonlinear<CALIBRATION>
459 (poses, sharedCal, measurements, point, model);
461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
463 for (
const Pose3& pose : poses) {
464 const Point3& p_local = pose.transformTo(point);
486template <
class CAMERA>
488 const typename CAMERA::MeasurementVector& measurements,
489 double rank_tol = 1e-9,
bool optimize =
false,
491 const bool useLOST =
false) {
492 size_t m = cameras.size();
493 assert(measurements.size() == m);
502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
503 SharedIsotropic measurementNoise =
507 std::vector<Pose3> poses;
508 poses.reserve(cameras.size());
509 for (
const auto& camera : cameras) poses.push_back(camera.pose());
513 auto calibratedMeasurements =
514 calibrateMeasurements<CAMERA>(cameras, measurements);
516 point =
triangulateLOST(poses, calibratedMeasurements, measurementNoise);
519 auto projection_matrices = projectionMatricesFromCameras(cameras);
522 auto undistortedMeasurements =
523 undistortMeasurements<CAMERA>(cameras, measurements);
526 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
531 point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
536 for (
const CAMERA& camera : cameras) {
537 const Point3& p_local = camera.pose().transformTo(point);
546template <
class CALIBRATION>
548 const Point2Vector& measurements,
549 double rank_tol = 1e-9,
bool optimize =
false,
551 const bool useLOST =
false) {
552 return triangulatePoint3<PinholeCamera<CALIBRATION>>
553 (cameras, measurements, rank_tol,
optimize, model, useLOST);
587 const bool _enableEPI =
false,
double _landmarkDistanceThreshold = -1,
588 double _dynamicOutlierRejectionThreshold = -1,
590 rankTolerance(_rankTolerance), enableEPI(_enableEPI),
591 landmarkDistanceThreshold(_landmarkDistanceThreshold),
592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
593 noiseModel(_noiseModel){
597 friend std::ostream &operator<<(std::ostream &os,
600 os <<
"enableEPI = " << p.
enableEPI << std::endl;
603 os <<
"dynamicOutlierRejectionThreshold = "
605 os <<
"noise model" << std::endl;
612 friend class boost::serialization::access;
613 template<
class ARCHIVE>
614 void serialize(ARCHIVE & ar,
const unsigned int version) {
615 ar & BOOST_SERIALIZATION_NVP(rankTolerance);
616 ar & BOOST_SERIALIZATION_NVP(enableEPI);
617 ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
618 ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
654 bool valid()
const {
return status == VALID; }
655 bool degenerate()
const {
return status == DEGENERATE; }
656 bool outlier()
const {
return status == OUTLIER; }
657 bool farPoint()
const {
return status == FAR_POINT; }
658 bool behindCamera()
const {
return status == BEHIND_CAMERA; }
660 friend std::ostream& operator<<(std::ostream& os,
663 os <<
"point = " << *result << std::endl;
665 os <<
"no point, status = " << result.status << std::endl;
672 template <
class ARCHIVE>
673 void serialize(ARCHIVE& ar,
const unsigned int version) {
674 ar& BOOST_SERIALIZATION_NVP(status);
679template<
class CAMERA>
681 const typename CAMERA::MeasurementVector& measured,
684 size_t m = cameras.size();
688 return TriangulationResult::Degenerate();
693 triangulatePoint3<CAMERA>(cameras, measured, params.
rankTolerance,
698 double maxReprojError = 0.0;
699 for(
const CAMERA& camera: cameras) {
700 const Pose3& pose = camera.pose();
704 return TriangulationResult::FarPoint();
705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
709 if (p_local.z() <= 0)
710 return TriangulationResult::BehindCamera();
714 const typename CAMERA::Measurement& zi = measured.at(i);
715 Point2 reprojectionError = camera.reprojectionError(point, zi);
716 maxReprojError = std::max(maxReprojError, reprojectionError.norm());
723 return TriangulationResult::Outlier();
731 return TriangulationResult::Degenerate();
734 return TriangulationResult::BehindCamera();
739using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
740using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
741using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
742using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
743using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
744using CameraSetSpherical = CameraSet<SphericalCamera>;
Calibrated camera with spherical projection.
Calibration of a fisheye camera.
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
Unified Calibration Model, see Mei07icra for details.
Calibration used by Bundler.
Base class to create smart factors on poses or cameras.
Base class for all pinhole cameras.
The most common 5DOF 3D->2D calibration.
Factor Graph consisting of non-linear factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using shared camer...
Definition: triangulation.h:357
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
Definition: triangulation.cpp:92
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Remove distortion for measurements so as if the measurements came from a pinhole camera.
Definition: triangulation.h:282
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:113
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Create a pinhole calibration from a different Cal3 object, removing distortion.
Definition: triangulation.h:253
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, boost::optional< Cal3_S2 > pinholeCal=boost::none)
Internal undistortMeasurement to be used by undistortMeasurement and undistortMeasurements.
Definition: triangulation.h:261
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:155
Vector2 Point2
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point2 to Vector2...
Definition: Point2.h:27
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters ¶ms)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:680
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Given an initial estimate , refine a point using measurements in several cameras.
Definition: triangulation.h:187
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
Vector3 Point3
As of GTSAM 4, in order to make GTSAM more lean, it is now possible to just typedef Point3 to Vector3...
Definition: Point3.h:36
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition: NoiseModel.h:724
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DL...
Definition: triangulation.h:421
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
Create a factor graph with projection factors from poses and one calibration.
Definition: triangulation.h:122
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:100
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Convert pixel measurements in image to homogeneous measurements in the image plane using camera intri...
Definition: triangulation.h:381
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition: triangulation.cpp:127
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > &projection_matrices, const Point2Vector &measurements, double rank_tol)
DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312.
Definition: triangulation.cpp:27
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
A pinhole camera class that has a Pose3 and a Calibration.
Definition: PinholeCamera.h:33
A pinhole camera class that has a Pose3 and a fixed Calibration.
Definition: PinholePose.h:243
A 3D pose (R,t) : (Rot3,Point3)
Definition: Pose3.h:37
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:308
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition: triangulation.h:39
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition: triangulation.h:47
Definition: triangulation.h:556
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel &_noiseModel=nullptr)
Constructor.
Definition: triangulation.h:586
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition: triangulation.h:573
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition: triangulation.h:558
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition: triangulation.h:566
bool enableEPI
if set to true, will refine triangulation using LM
Definition: triangulation.h:560
SharedNoiseModel noiseModel
used in the nonlinear triangulation
Definition: triangulation.h:575
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition: triangulation.h:626
TriangulationResult()
Default constructor, only for serialization.
Definition: triangulation.h:638
TriangulationResult(const Point3 &p)
Constructor.
Definition: triangulation.h:643
friend class boost::serialization::access
Serialization function.
Definition: triangulation.h:671
Character and index key used to refer to variables.
Definition: Symbol.h:35
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
An isotropic noise model created by specifying a standard devation sigma.
Definition: NoiseModel.cpp:597
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:597
Definition: NonlinearFactorGraph.h:55
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:65
void insert(Key j, const Value &val)
Add a variable with the given j, throws KeyAlreadyExists<J> if j is already present.
Definition: Values.cpp:157
Non-linear factor for a constraint derived from a 2D measurement.
Definition: TriangulationFactor.h:33
In nonlinear factors, the error function returns the negative log-likelihood as a non-linear function...