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 used by Bundler.
Base class to create smart factors on poses or cameras.
The most common 5DOF 3D->2D calibration.
Calibration of a fisheye camera.
Unified Calibration Model, see Mei07icra for details.
2D Pose
Base class for all pinhole cameras.
Calibrated camera with spherical projection.
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
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...