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 
23 #include "gtsam/geometry/Point3.h"
27 #include <gtsam/geometry/Cal3_S2.h>
28 #include <gtsam/geometry/Cal3DS2.h>
32 #include <gtsam/geometry/Pose2.h>
33 #include <gtsam/inference/Symbol.h>
36 
37 #include <optional>
38 
39 namespace gtsam {
40 
42 class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
43 public:
45  std::runtime_error("Triangulation Underconstrained Exception.") {
46  }
47 };
48 
50 class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
51 public:
53  std::runtime_error(
54  "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
55  }
56 };
57 
65 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
66  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
67  const Point2Vector& measurements, double rank_tol = 1e-9);
68 
77 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
78  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
79  const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
80 
88 GTSAM_EXPORT Point3 triangulateDLT(
89  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
91  double rank_tol = 1e-9);
92 
96 GTSAM_EXPORT Point3 triangulateDLT(
97  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
98  const std::vector<Unit3>& measurements,
99  double rank_tol = 1e-9);
100 
111 GTSAM_EXPORT Point3 triangulateLOST(const std::vector<Pose3>& poses,
112  const Point3Vector& calibratedMeasurements,
113  const SharedIsotropic& measurementNoise,
114  double rank_tol = 1e-9);
115 
125 template<class CALIBRATION>
126 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
127  const std::vector<Pose3>& poses, std::shared_ptr<CALIBRATION> sharedCal,
128  const Point2Vector& measurements, Key landmarkKey,
129  const Point3& initialEstimate,
131  Values values;
132  values.insert(landmarkKey, initialEstimate); // Initial landmark value
134  for (size_t i = 0; i < measurements.size(); i++) {
135  const Pose3& pose_i = poses[i];
137  Camera camera_i(pose_i, sharedCal);
139  (camera_i, measurements[i], model, landmarkKey);
140  }
141  return {graph, values};
142 }
143 
153 template<class CAMERA>
154 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
155  const CameraSet<CAMERA>& cameras,
156  const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
157  const Point3& initialEstimate,
158  const SharedNoiseModel& model = nullptr) {
159  Values values;
160  values.insert(landmarkKey, initialEstimate); // Initial landmark value
164  for (size_t i = 0; i < measurements.size(); i++) {
165  const CAMERA& camera_i = cameras[i];
167  (camera_i, measurements[i], model? model : unit, landmarkKey);
168  }
169  return {graph, values};
170 }
171 
179 GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
180  const Values& values, Key landmarkKey);
181 
190 template<class CALIBRATION>
191 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
192  std::shared_ptr<CALIBRATION> sharedCal,
193  const Point2Vector& measurements, const Point3& initialEstimate,
194  const SharedNoiseModel& model = nullptr) {
195 
196  // Create a factor graph and initial values
197  const auto [graph, values] = triangulationGraph<CALIBRATION> //
198  (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate, model);
199 
200  return optimize(graph, values, Symbol('p', 0));
201 }
202 
210 template<class CAMERA>
212  const CameraSet<CAMERA>& cameras,
213  const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate,
214  const SharedNoiseModel& model = nullptr) {
215 
216  // Create a factor graph and initial values
217  const auto [graph, values] = triangulationGraph<CAMERA> //
218  (cameras, measurements, Symbol('p', 0), initialEstimate, model);
219 
220  return optimize(graph, values, Symbol('p', 0));
221 }
222 
223 template<class CAMERA>
224 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
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
234 template<class CALIBRATION>
235 std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
236  const std::vector<Pose3> &poses, std::shared_ptr<CALIBRATION> sharedCal) {
237  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
238  for (size_t i = 0; i < poses.size(); i++) {
240  projection_matrices.push_back(camera.cameraProjectionMatrix());
241  }
242  return projection_matrices;
243 }
244 
252 template <class CALIBRATION>
253 Cal3_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 
260 template <class CALIBRATION, class MEASUREMENT>
262  const CALIBRATION& cal, const MEASUREMENT& measurement,
263  std::optional<Cal3_S2> pinholeCal = {}) {
264  if (!pinholeCal) {
265  pinholeCal = createPinholeCalibration(cal);
266  }
267  return pinholeCal->uncalibrate(cal.calibrate(measurement));
268 }
269 
281 template <class CALIBRATION>
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.
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 
298 template <>
300  const Point2Vector& measurements) {
301  return measurements;
302 }
303 
315 template <class CAMERA>
316 typename 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 
333 template <class CAMERA = PinholeCamera<Cal3_S2>>
335  const CameraSet<PinholeCamera<Cal3_S2>>& cameras,
337  return measurements;
338 }
339 
341 template <class CAMERA = SphericalCamera>
343  const CameraSet<SphericalCamera>& cameras,
345  return measurements;
346 }
347 
356 template <class CALIBRATION>
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.
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 
380 template <class CAMERA>
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 
396 template <class CAMERA = SphericalCamera>
398  const CameraSet<SphericalCamera>& cameras,
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 
420 template <class CALIBRATION>
421 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
422  std::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  rank_tol);
445  } else {
446  // construct projection matrices from poses & calibration
447  auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
448 
449  // Undistort the measurements, leaving only the pinhole elements in effect.
450  auto undistortedMeasurements =
451  undistortMeasurements<CALIBRATION>(*sharedCal, measurements);
452 
453  point =
454  triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
455  }
456 
457  // Then refine using non-linear optimization
458  if (optimize)
459  point = triangulateNonlinear<CALIBRATION> //
460  (poses, sharedCal, measurements, point, model);
461 
462 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
463  // verify that the triangulated point lies in front of all cameras
464  for (const Pose3& pose : poses) {
465  const Point3& p_local = pose.transformTo(point);
466  if (p_local.z() <= 0) throw(TriangulationCheiralityException());
467  }
468 #endif
469 
470  return point;
471 }
472 
487 template <class CAMERA>
489  const typename CAMERA::MeasurementVector& measurements,
490  double rank_tol = 1e-9, bool optimize = false,
491  const SharedNoiseModel& model = nullptr,
492  const bool useLOST = false) {
493  size_t m = cameras.size();
494  assert(measurements.size() == m);
495 
496  if (m < 2) throw(TriangulationUnderconstrainedException());
497 
498  // Triangulate linearly
499  Point3 point;
500  if (useLOST) {
501  // Reduce input noise model to an isotropic noise model using the mean of
502  // the diagonal.
503  const double measurementSigma = model ? model->sigmas().mean() : 1e-4;
504  SharedIsotropic measurementNoise =
505  noiseModel::Isotropic::Sigma(2, measurementSigma);
506 
507  // construct poses from cameras.
508  std::vector<Pose3> poses;
509  poses.reserve(cameras.size());
510  for (const auto& camera : cameras) poses.push_back(camera.pose());
511 
512  // calibrate the measurements to obtain homogenous coordinates in image
513  // plane.
514  auto calibratedMeasurements =
515  calibrateMeasurements<CAMERA>(cameras, measurements);
516 
517  point = triangulateLOST(poses, calibratedMeasurements, measurementNoise,
518  rank_tol);
519  } else {
520  // construct projection matrices from poses & calibration
521  auto projection_matrices = projectionMatricesFromCameras(cameras);
522 
523  // Undistort the measurements, leaving only the pinhole elements in effect.
524  auto undistortedMeasurements =
525  undistortMeasurements<CAMERA>(cameras, measurements);
526 
527  point =
528  triangulateDLT(projection_matrices, undistortedMeasurements, rank_tol);
529  }
530 
531  // Then refine using non-linear optimization
532  if (optimize) {
533  point = triangulateNonlinear<CAMERA>(cameras, measurements, point, model);
534  }
535 
536 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
537  // verify that the triangulated point lies in front of all cameras
538  for (const CAMERA& camera : cameras) {
539  const Point3& p_local = camera.pose().transformTo(point);
540  if (p_local.z() <= 0) throw(TriangulationCheiralityException());
541  }
542 #endif
543 
544  return point;
545 }
546 
548 template <class CALIBRATION>
550  const Point2Vector& measurements,
551  double rank_tol = 1e-9, bool optimize = false,
552  const SharedNoiseModel& model = nullptr,
553  const bool useLOST = false) {
554  return triangulatePoint3<PinholeCamera<CALIBRATION>> //
555  (cameras, measurements, rank_tol, optimize, model, useLOST);
556 }
557 
558 struct GTSAM_EXPORT TriangulationParameters {
559 
560  double rankTolerance;
561  bool enableEPI;
563 
569 
576 
580  bool useLOST;
581 
583 
593  TriangulationParameters(const double _rankTolerance = 1.0,
594  const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
595  double _dynamicOutlierRejectionThreshold = -1,
596  const bool _useLOST = false,
597  const SharedNoiseModel& _noiseModel = nullptr) :
598  rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
599  landmarkDistanceThreshold(_landmarkDistanceThreshold), //
600  dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
601  useLOST(_useLOST),
602  noiseModel(_noiseModel){
603  }
604 
605  // stream to output
606  friend std::ostream &operator<<(std::ostream &os,
607  const TriangulationParameters& p) {
608  os << "rankTolerance = " << p.rankTolerance << std::endl;
609  os << "enableEPI = " << p.enableEPI << std::endl;
610  os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
611  << std::endl;
612  os << "dynamicOutlierRejectionThreshold = "
613  << p.dynamicOutlierRejectionThreshold << std::endl;
614  os << "useLOST = " << p.useLOST << std::endl;
615  os << "noise model" << std::endl;
616  return os;
617  }
618 
619 private:
620 
621 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
622  friend class boost::serialization::access;
624  template<class ARCHIVE>
625  void serialize(ARCHIVE & ar, const unsigned int version) {
626  ar & BOOST_SERIALIZATION_NVP(rankTolerance);
627  ar & BOOST_SERIALIZATION_NVP(enableEPI);
628  ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
629  ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
630  }
631 #endif
632 };
633 
638 class TriangulationResult : public std::optional<Point3> {
639  public:
642 
643  private:
645 
646  public:
651 
655  TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
658  }
662  }
665  }
666  bool valid() const { return status == VALID; }
667  bool degenerate() const { return status == DEGENERATE; }
668  bool outlier() const { return status == OUTLIER; }
669  bool farPoint() const { return status == FAR_POINT; }
670  bool behindCamera() const { return status == BEHIND_CAMERA; }
671  const gtsam::Point3& get() const {
672  if (!has_value()) throw std::runtime_error("TriangulationResult has no value");
673  return value();
674  }
675  // stream to output
676  GTSAM_EXPORT friend std::ostream& operator<<(
677  std::ostream& os, const TriangulationResult& result) {
678  if (result)
679  os << "point = " << *result << std::endl;
680  else
681  os << "no point, status = " << result.status << std::endl;
682  return os;
683  }
684 
685  private:
686 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
687  friend class boost::serialization::access;
689  template <class ARCHIVE>
690  void serialize(ARCHIVE& ar, const unsigned int version) {
691  ar& BOOST_SERIALIZATION_NVP(status);
692  }
693 #endif
694 };
695 
697 template<class CAMERA>
699  const typename CAMERA::MeasurementVector& measured,
701 
702  size_t m = cameras.size();
703 
704  // if we have a single pose the corresponding factor is uninformative
705  if (m < 2)
707  else
708  // We triangulate the 3D position of the landmark
709  try {
710  Point3 point =
711  triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
712  params.enableEPI, params.noiseModel, params.useLOST);
713 
714  // Check landmark distance and re-projection errors to avoid outliers
715  size_t i = 0;
716  double maxReprojError = 0.0;
717  for(const CAMERA& camera: cameras) {
718  const Pose3& pose = camera.pose();
719  if (params.landmarkDistanceThreshold > 0
721  > params.landmarkDistanceThreshold)
723 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
724  // verify that the triangulated point lies in front of all cameras
725  // Only needed if this was not yet handled by exception
726  const Point3& p_local = pose.transformTo(point);
727  if (p_local.z() <= 0)
729 #endif
730  // Check reprojection error
731  if (params.dynamicOutlierRejectionThreshold > 0) {
732  const typename CAMERA::Measurement& zi = measured.at(i);
733  Point2 reprojectionError = camera.reprojectionError(point, zi);
734  maxReprojError = std::max(maxReprojError, reprojectionError.norm());
735  }
736  i += 1;
737  }
738  // Flag as degenerate if average reprojection error is too large
739  if (params.dynamicOutlierRejectionThreshold > 0
740  && maxReprojError > params.dynamicOutlierRejectionThreshold)
742 
743  // all good!
744  return TriangulationResult(point);
746  // This exception is thrown if
747  // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
748  // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
751  // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
753  }
754 }
755 
756 // Vector of Cameras - used by the Python/MATLAB wrapper
763 } // \namespace gtsam
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.cal
cal
Definition: test_ProjectionFactorRollingShutter.py:27
gtsam::calibrateMeasurementsShared
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)
Definition: triangulation.h:357
gtsam::projectionMatricesFromCameras
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromCameras(const CameraSet< CAMERA > &cameras)
Definition: triangulation.h:225
Pose2.h
2D Pose
gtsam::TriangulationResult::DEGENERATE
@ DEGENERATE
Definition: triangulation.h:640
gtsam::PinholeCamera::MeasurementVector
Point2Vector MeasurementVector
Definition: PinholeCamera.h:42
pose
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
CameraSet.h
Base class to create smart factors on poses or cameras.
gtsam::Pose3::transformTo
Point3 transformTo(const Point3 &point, OptionalJacobian< 3, 6 > Hself={}, OptionalJacobian< 3, 3 > Hpoint={}) const
takes point in world coordinates and transforms it to Pose coordinates
Definition: Pose3.cpp:371
gtsam::TriangulationFactor
Definition: TriangulationFactor.h:31
s
RealScalar s
Definition: level1_cplx_impl.h:126
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
SphericalCamera.h
Calibrated camera with spherical projection.
gtsam::undistortMeasurements
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Definition: triangulation.h:282
Cal3_S2.h
The most common 5DOF 3D->2D calibration.
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
Definition: triangulation.h:575
gtsam::TriangulationResult::outlier
bool outlier() const
Definition: triangulation.h:668
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Definition: triangulation.cpp:177
gtsam::TriangulationResult::FarPoint
static TriangulationResult FarPoint()
Definition: triangulation.h:660
Values
measured
Point2 measured(-17, 30)
gtsam::TriangulationUnderconstrainedException::TriangulationUnderconstrainedException
TriangulationUnderconstrainedException()
Definition: triangulation.h:44
gtsam::undistortMeasurementInternal
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, std::optional< Cal3_S2 > pinholeCal={})
Definition: triangulation.h:261
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
Point3.h
3D Point
os
ofstream os("timeSchurFactors.csv")
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
gtsam::TriangulationResult::status
Status status
Definition: triangulation.h:641
gtsam::TriangulationResult::TriangulationResult
TriangulationResult()
Definition: triangulation.h:650
simulated2D::Measurement
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
gtsam_unstable.tests.test_ProjectionFactorRollingShutter.point
point
Definition: test_ProjectionFactorRollingShutter.py:25
gtsam::TriangulationResult::Outlier
static TriangulationResult Outlier()
Definition: triangulation.h:659
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
(the rank is the number of singular values of the triangulation matrix which are larger than rankTole...
Definition: triangulation.h:560
gtsam::TriangulationParameters
Definition: triangulation.h:558
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(const Point3 &p)
Definition: triangulation.h:655
gtsam::TriangulationResult::get
const gtsam::Point3 & get() const
Definition: triangulation.h:671
gtsam::Pose3
Definition: Pose3.h:37
gtsam::triangulateNonlinear
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Definition: triangulation.h:191
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::TriangulationParameters::useLOST
bool useLOST
Definition: triangulation.h:580
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:631
gtsam::createPinholeCalibration
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Definition: triangulation.h:253
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
gtsam::TriangulationResult::valid
bool valid() const
Definition: triangulation.h:666
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::TriangulationParameters::noiseModel
SharedNoiseModel noiseModel
used in the nonlinear triangulation
Definition: triangulation.h:582
Symbol.h
gtsam::TriangulationResult::OUTLIER
@ OUTLIER
Definition: triangulation.h:640
gtsam::projectionMatricesFromPoses
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromPoses(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal)
Definition: triangulation.h:235
gtsam::triangulateSafe
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Definition: triangulation.h:698
conf.version
version
Definition: gtsam/3rdparty/GeographicLib/python/doc/conf.py:67
gtsam::HybridValues::insert
void insert(Key j, const Vector &value)
Definition: HybridValues.cpp:85
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
transform
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
sharedCal
static const std::shared_ptr< Cal3_S2 > sharedCal
Definition: testTriangulationFactor.cpp:33
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
gtsam::TriangulationResult::operator<<
GTSAM_EXPORT friend std::ostream & operator<<(std::ostream &os, const TriangulationResult &result)
Definition: triangulation.h:676
gtsam::TriangulationResult::Degenerate
static TriangulationResult Degenerate()
Definition: triangulation.h:656
Cal3Unified.h
Unified Calibration Model, see Mei07icra for details.
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::calibrateMeasurements
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
Definition: triangulation.h:381
gtsam::triangulatePoint3
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false, const SharedNoiseModel &model=nullptr, const bool useLOST=false)
Definition: triangulation.h:421
gtsam::triangulateDLT
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
Definition: triangulation.cpp:149
gtsam::SphericalCamera::MeasurementVector
std::vector< Unit3 > MeasurementVector
Definition: SphericalCamera.h:79
gtsam::TriangulationResult::degenerate
bool degenerate() const
Definition: triangulation.h:667
gtsam::TriangulationParameters::operator<<
friend std::ostream & operator<<(std::ostream &os, const TriangulationParameters &p)
Definition: triangulation.h:606
TriangulationFactor.h
gtsam::triangulationGraph
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate, const SharedNoiseModel &model=noiseModel::Unit::Create(2))
Definition: triangulation.h:126
gtsam
traits
Definition: SFMdata.h:40
gtsam::TriangulationResult::BehindCamera
static TriangulationResult BehindCamera()
Definition: triangulation.h:663
gtsam::TriangulationResult::BEHIND_CAMERA
@ BEHIND_CAMERA
Definition: triangulation.h:640
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(Status s)
Definition: triangulation.h:644
gtsam::traits
Definition: Group.h:36
K
#define K
Definition: igam.h:8
estimation_fixture::measurements
std::vector< double > measurements
Definition: testHybridEstimation.cpp:51
gtsam::TriangulationUnderconstrainedException
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition: triangulation.h:42
NonlinearFactorGraph.h
Factor Graph consisting of non-linear factors.
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
Definition: triangulation.h:568
gtsam::Values
Definition: Values.h:65
gtsam::TriangulationResult::behindCamera
bool behindCamera() const
Definition: triangulation.h:670
Cal3DS2.h
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
std
Definition: BFloat16.h:88
gtsam::TriangulationCheiralityException::TriangulationCheiralityException
TriangulationCheiralityException()
Definition: triangulation.h:52
p
float * p
Definition: Tutorial_Map_using.cpp:9
gtsam::PinholeBase::pose
const Pose3 & pose() const
return pose, constant version
Definition: CalibratedCamera.h:154
gtsam::Camera
PinholeCamera< Cal3Bundler0 > Camera
Definition: testAdaptAutoDiff.cpp:52
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
gtsam::Cal3_S2
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
gtsam::TriangulationResult
Definition: triangulation.h:638
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
gtsam::TriangulationResult::FAR_POINT
@ FAR_POINT
Definition: triangulation.h:640
gtsam::SharedIsotropic
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:766
gtsam::triangulateHomogeneousDLT
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
Definition: triangulation.cpp:27
gtsam::noiseModel::Isotropic::Sigma
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:624
gtsam::TriangulationResult::VALID
@ VALID
Definition: triangulation.h:640
gtsam::triangulateLOST
Point3 triangulateLOST(const std::vector< Pose3 > &poses, const Point3Vector &calibratedMeasurements, const SharedIsotropic &measurementNoise, double rank_tol)
Triangulation using the LOST (Linear Optimal Sine Triangulation) algorithm proposed in https://arxiv....
Definition: triangulation.cpp:86
camera
static const CalibratedCamera camera(kDefaultPose)
gtsam::TriangulationCheiralityException
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition: triangulation.h:50
gtsam::PinholePose
Definition: PinholePose.h:239
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
max
#define max(a, b)
Definition: datatypes.h:20
gtsam::distance3
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
gtsam::TriangulationParameters::TriangulationParameters
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const bool _useLOST=false, const SharedNoiseModel &_noiseModel=nullptr)
Definition: triangulation.h:593
Cal3Bundler.h
Calibration used by Bundler.
test_callbacks.value
value
Definition: test_callbacks.py:160
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::TriangulationResult::farPoint
bool farPoint() const
Definition: triangulation.h:669
measurement
static Point2 measurement(323.0, 240.0)
gtsam::FactorGraph::emplace_shared
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:153
gtsam::TriangulationResult::Status
Status
Definition: triangulation.h:640
gtsam::Symbol
Definition: inference/Symbol.h:37
gtsam::Point3Vector
std::vector< Point3, Eigen::aligned_allocator< Point3 > > Point3Vector
Definition: Point3.h:39
Cal3Fisheye.h
Calibration of a fisheye camera.


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:54