gtsam 4.2.0
gtsam
Loading...
Searching...
No Matches
triangulation.h
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
21#pragma once
22
34#include <gtsam/slam/TriangulationFactor.h>
35
36namespace gtsam {
37
39class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
40public:
42 std::runtime_error("Triangulation Underconstrained Exception.") {
43 }
44};
45
47class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
48public:
50 std::runtime_error(
51 "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
52 }
53};
54
62GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
63 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
64 const Point2Vector& measurements, double rank_tol = 1e-9);
65
74GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
75 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
76 const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
77
85GTSAM_EXPORT Point3 triangulateDLT(
86 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
87 const Point2Vector& measurements,
88 double rank_tol = 1e-9);
89
93GTSAM_EXPORT Point3 triangulateDLT(
94 const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
95 const std::vector<Unit3>& measurements,
96 double rank_tol = 1e-9);
97
108GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
109 const Point3Vector& calibratedMeasurements,
110 const SharedIsotropic& measurementNoise);
111
121template<class CALIBRATION>
122std::pair<NonlinearFactorGraph, Values> triangulationGraph(
123 const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
124 const Point2Vector& measurements, Key landmarkKey,
125 const Point3& initialEstimate,
126 const SharedNoiseModel& model = noiseModel::Unit::Create(2)) {
127 Values values;
128 values.insert(landmarkKey, initialEstimate); // Initial landmark value
130 for (size_t i = 0; i < measurements.size(); i++) {
131 const Pose3& pose_i = poses[i];
132 typedef PinholePose<CALIBRATION> Camera;
133 Camera camera_i(pose_i, sharedCal);
134 graph.emplace_shared<TriangulationFactor<Camera> > //
135 (camera_i, measurements[i], model, landmarkKey);
136 }
137 return std::make_pair(graph, values);
138}
139
149template<class CAMERA>
150std::pair<NonlinearFactorGraph, Values> triangulationGraph(
151 const CameraSet<CAMERA>& cameras,
152 const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
153 const Point3& initialEstimate,
154 const SharedNoiseModel& model = nullptr) {
155 Values values;
156 values.insert(landmarkKey, initialEstimate); // Initial landmark value
160 for (size_t i = 0; i < measurements.size(); i++) {
161 const CAMERA& camera_i = cameras[i];
162 graph.emplace_shared<TriangulationFactor<CAMERA> > //
163 (camera_i, measurements[i], model? model : unit, landmarkKey);
164 }
165 return std::make_pair(graph, values);
166}
167
175GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
176 const Values& values, Key landmarkKey);
177
186template<class CALIBRATION>
187Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
188 boost::shared_ptr<CALIBRATION> sharedCal,
189 const Point2Vector& measurements, const Point3& initialEstimate,
190 const SharedNoiseModel& model = nullptr) {
191
192 // Create a factor graph and initial values
193 Values values;
195 boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
196 (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
197
198 return optimize(graph, values, Symbol('p', 0));
199}
200
208template<class CAMERA>
210 const CameraSet<CAMERA>& cameras,
211 const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
212 const SharedNoiseModel& model = nullptr) {
213
214 // Create a factor graph and initial values
215 Values values;
217 boost::tie(graph, values) = triangulationGraph<CAMERA> //
218 (cameras, measurements, Symbol('p', 0), initialEstimate, model);
219
220 return optimize(graph, values, Symbol('p', 0));
221}
222
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());
229 }
230 return projection_matrices;
231}
232
233// overload, assuming pinholePose
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());
241 }
242 return projection_matrices;
243}
244
252template <class CALIBRATION>
253Cal3_S2 createPinholeCalibration(const CALIBRATION& cal) {
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));
256}
257
260template <class CALIBRATION, class MEASUREMENT>
262 const CALIBRATION& cal, const MEASUREMENT& measurement,
263 boost::optional<Cal3_S2> pinholeCal = boost::none) {
264 if (!pinholeCal) {
265 pinholeCal = createPinholeCalibration(cal);
266 }
267 return pinholeCal->uncalibrate(cal.calibrate(measurement));
268}
269
281template <class CALIBRATION>
282Point2Vector undistortMeasurements(const CALIBRATION& cal,
283 const Point2Vector& measurements) {
284 Cal3_S2 pinholeCalibration = createPinholeCalibration(cal);
285 Point2Vector undistortedMeasurements;
286 // Calibrate with cal and uncalibrate with pinhole version of cal so that
287 // measurements are undistorted.
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);
293 });
294 return undistortedMeasurements;
295}
296
298template <>
299inline Point2Vector undistortMeasurements(const Cal3_S2& cal,
300 const Point2Vector& measurements) {
301 return measurements;
302}
303
315template <class CAMERA>
316typename CAMERA::MeasurementVector undistortMeasurements(
317 const CameraSet<CAMERA>& cameras,
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) {
323 // Calibrate with cal and uncalibrate with pinhole version of cal so that
324 // measurements are undistorted.
325 undistortedMeasurements[ii] =
326 undistortMeasurementInternal<typename CAMERA::CalibrationType>(
327 cameras[ii].calibration(), measurements[ii]);
328 }
329 return undistortedMeasurements;
330}
331
333template <class CAMERA = PinholeCamera<Cal3_S2>>
334inline PinholeCamera<Cal3_S2>::MeasurementVector undistortMeasurements(
335 const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
336 const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
337 return measurements;
338}
339
341template <class CAMERA = SphericalCamera>
342inline SphericalCamera::MeasurementVector undistortMeasurements(
343 const CameraSet<SphericalCamera>& cameras,
344 const SphericalCamera::MeasurementVector& measurements) {
345 return measurements;
346}
347
356template <class CALIBRATION>
357inline Point3Vector calibrateMeasurementsShared(
358 const CALIBRATION& cal, const Point2Vector& measurements) {
359 Point3Vector calibratedMeasurements;
360 // Calibrate with cal and uncalibrate with pinhole version of cal so that
361 // measurements are undistorted.
362 std::transform(measurements.begin(), measurements.end(),
363 std::back_inserter(calibratedMeasurements),
364 [&cal](const Point2& measurement) {
365 Point3 p;
366 p << cal.calibrate(measurement), 1.0;
367 return p;
368 });
369 return calibratedMeasurements;
370}
371
380template <class CAMERA>
381inline Point3Vector calibrateMeasurements(
382 const CameraSet<CAMERA>& cameras,
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]),
390 1.0;
391 }
392 return calibratedMeasurements;
393}
394
396template <class CAMERA = SphericalCamera>
397inline Point3Vector calibrateMeasurements(
398 const CameraSet<SphericalCamera>& cameras,
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();
403 }
404 return calibratedMeasurements;
405}
406
420template <class CALIBRATION>
421Point3 triangulatePoint3(const std::vector<Pose3>& poses,
422 boost::shared_ptr<CALIBRATION> sharedCal,
423 const Point2Vector& measurements,
424 double rank_tol = 1e-9, bool optimize = false,
425 const SharedNoiseModel& model = nullptr,
426 const bool useLOST = false) {
427 assert(poses.size() == measurements.size());
428 if (poses.size() < 2) throw(TriangulationUnderconstrainedException());
429
430 // Triangulate linearly
431 Point3 point;
432 if (useLOST) {
433 // Reduce input noise model to an isotropic noise model using the mean of
434 // the diagonal.
435 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
436 SharedIsotropic measurementNoise =
437 noiseModel::Isotropic::Sigma(2, measurementSigma);
438 // calibrate the measurements to obtain homogenous coordinates in image
439 // plane.
440 auto calibratedMeasurements =
441 calibrateMeasurementsShared<CALIBRATION>(*sharedCal, measurements);
442
443 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
444 } else {
445 // construct projection matrices from poses & calibration
446 auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
447
448 // Undistort the measurements, leaving only the pinhole elements in effect.
449 auto undistortedMeasurements =
450 undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
451
452 point =
453 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
454 }
455
456 // Then refine using non-linear optimization
457 if (optimize)
458 point = triangulateNonlinear<CALIBRATION> //
459 (poses, sharedCal, measurements, point, model);
460
461#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
462 // verify that the triangulated point lies in front of all cameras
463 for (const Pose3& pose : poses) {
464 const Point3& p_local = pose.transformTo(point);
465 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
466 }
467#endif
468
469 return point;
470}
471
486template <class CAMERA>
488 const typename CAMERA::MeasurementVector& measurements,
489 double rank_tol = 1e-9, bool optimize = false,
490 const SharedNoiseModel& model = nullptr,
491 const bool useLOST = false) {
492 size_t m = cameras.size();
493 assert(measurements.size() == m);
494
495 if (m < 2) throw(TriangulationUnderconstrainedException());
496
497 // Triangulate linearly
498 Point3 point;
499 if (useLOST) {
500 // Reduce input noise model to an isotropic noise model using the mean of
501 // the diagonal.
502 const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
503 SharedIsotropic measurementNoise =
504 noiseModel::Isotropic::Sigma(2, measurementSigma);
505
506 // construct poses from cameras.
507 std::vector<Pose3> poses;
508 poses.reserve(cameras.size());
509 for (const auto& camera : cameras) poses.push_back(camera.pose());
510
511 // calibrate the measurements to obtain homogenous coordinates in image
512 // plane.
513 auto calibratedMeasurements =
514 calibrateMeasurements<CAMERA>(cameras, measurements);
515
516 point = triangulateLOST(poses, calibratedMeasurements, measurementNoise);
517 } else {
518 // construct projection matrices from poses & calibration
519 auto projection_matrices = projectionMatricesFromCameras(cameras);
520
521 // Undistort the measurements, leaving only the pinhole elements in effect.
522 auto undistortedMeasurements =
523 undistortMeasurements<CAMERA>(cameras, measurements);
524
525 point =
526 triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
527 }
528
529 // Then refine using non-linear optimization
530 if (optimize) {
531 point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
532 }
533
534#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
535 // verify that the triangulated point lies in front of all cameras
536 for (const CAMERA& camera : cameras) {
537 const Point3& p_local = camera.pose().transformTo(point);
538 if (p_local.z() <= 0) throw(TriangulationCheiralityException());
539 }
540#endif
541
542 return point;
543}
544
546template <class CALIBRATION>
548 const Point2Vector& measurements,
549 double rank_tol = 1e-9, bool optimize = false,
550 const SharedNoiseModel& model = nullptr,
551 const bool useLOST = false) {
552 return triangulatePoint3<PinholeCamera<CALIBRATION>> //
553 (cameras, measurements, rank_tol, optimize, model, useLOST);
554}
555
556struct GTSAM_EXPORT TriangulationParameters {
557
561
567
574
576
586 TriangulationParameters(const double _rankTolerance = 1.0,
587 const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
588 double _dynamicOutlierRejectionThreshold = -1,
589 const SharedNoiseModel& _noiseModel = nullptr) :
590 rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
591 landmarkDistanceThreshold(_landmarkDistanceThreshold), //
592 dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
593 noiseModel(_noiseModel){
594 }
595
596 // stream to output
597 friend std::ostream &operator<<(std::ostream &os,
598 const TriangulationParameters& p) {
599 os << "rankTolerance = " << p.rankTolerance << std::endl;
600 os << "enableEPI = " << p.enableEPI << std::endl;
601 os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
602 << std::endl;
603 os << "dynamicOutlierRejectionThreshold = "
604 << p.dynamicOutlierRejectionThreshold << std::endl;
605 os << "noise model" << std::endl;
606 return os;
607 }
608
609private:
610
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);
619 }
620};
621
626class TriangulationResult : public boost::optional<Point3> {
627 public:
628 enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
629 Status status;
630
631 private:
632 TriangulationResult(Status s) : status(s) {}
633
634 public:
639
643 TriangulationResult(const Point3& p) : status(VALID) { reset(p); }
644 static TriangulationResult Degenerate() {
645 return TriangulationResult(DEGENERATE);
646 }
647 static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
648 static TriangulationResult FarPoint() {
649 return TriangulationResult(FAR_POINT);
650 }
651 static TriangulationResult BehindCamera() {
652 return TriangulationResult(BEHIND_CAMERA);
653 }
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; }
659 // stream to output
660 friend std::ostream& operator<<(std::ostream& os,
661 const TriangulationResult& result) {
662 if (result)
663 os << "point = " << *result << std::endl;
664 else
665 os << "no point, status = " << result.status << std::endl;
666 return os;
667 }
668
669 private:
672 template <class ARCHIVE>
673 void serialize(ARCHIVE& ar, const unsigned int version) {
674 ar& BOOST_SERIALIZATION_NVP(status);
675 }
676};
677
679template<class CAMERA>
681 const typename CAMERA::MeasurementVector& measured,
682 const TriangulationParameters& params) {
683
684 size_t m = cameras.size();
685
686 // if we have a single pose the corresponding factor is uninformative
687 if (m < 2)
688 return TriangulationResult::Degenerate();
689 else
690 // We triangulate the 3D position of the landmark
691 try {
692 Point3 point =
693 triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
694 params.enableEPI, params.noiseModel);
695
696 // Check landmark distance and re-projection errors to avoid outliers
697 size_t i = 0;
698 double maxReprojError = 0.0;
699 for(const CAMERA& camera: cameras) {
700 const Pose3& pose = camera.pose();
701 if (params.landmarkDistanceThreshold > 0
702 && distance3(pose.translation(), point)
704 return TriangulationResult::FarPoint();
705#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
706 // verify that the triangulated point lies in front of all cameras
707 // Only needed if this was not yet handled by exception
708 const Point3& p_local = pose.transformTo(point);
709 if (p_local.z() <= 0)
710 return TriangulationResult::BehindCamera();
711#endif
712 // Check reprojection error
713 if (params.dynamicOutlierRejectionThreshold > 0) {
714 const typename CAMERA::Measurement& zi = measured.at(i);
715 Point2 reprojectionError = camera.reprojectionError(point, zi);
716 maxReprojError = std::max(maxReprojError, reprojectionError.norm());
717 }
718 i += 1;
719 }
720 // Flag as degenerate if average reprojection error is too large
722 && maxReprojError > params.dynamicOutlierRejectionThreshold)
723 return TriangulationResult::Outlier();
724
725 // all good!
726 return TriangulationResult(point);
728 // This exception is thrown if
729 // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
730 // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
731 return TriangulationResult::Degenerate();
733 // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
734 return TriangulationResult::BehindCamera();
735 }
736}
737
738// Vector of Cameras - used by the Python/MATLAB wrapper
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>;
745} // \namespace gtsam
746
Calibration of a fisheye camera.
Calibration used by Bundler.
Unified Calibration Model, see Mei07icra for details.
Base class to create smart factors on poses or cameras.
Calibrated camera with spherical projection.
2D Pose
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
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
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 &params)
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...