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,
90  const Point2Vector& measurements,
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>
282 Point2Vector 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 
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,
336  const PinholeCamera<Cal3_S2>::MeasurementVector& measurements) {
337  return measurements;
338 }
339 
341 template <class CAMERA = SphericalCamera>
343  const CameraSet<SphericalCamera>& cameras,
344  const SphericalCamera::MeasurementVector& measurements) {
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.
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 
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,
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 
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 
578 
588  TriangulationParameters(const double _rankTolerance = 1.0,
589  const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
590  double _dynamicOutlierRejectionThreshold = -1,
591  const SharedNoiseModel& _noiseModel = nullptr) :
592  rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
593  landmarkDistanceThreshold(_landmarkDistanceThreshold), //
594  dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold),
595  noiseModel(_noiseModel){
596  }
597 
598  // stream to output
599  friend std::ostream &operator<<(std::ostream &os,
600  const TriangulationParameters& p) {
601  os << "rankTolerance = " << p.rankTolerance << std::endl;
602  os << "enableEPI = " << p.enableEPI << std::endl;
603  os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
604  << std::endl;
605  os << "dynamicOutlierRejectionThreshold = "
606  << p.dynamicOutlierRejectionThreshold << std::endl;
607  os << "noise model" << std::endl;
608  return os;
609  }
610 
611 private:
612 
613 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
614  friend class boost::serialization::access;
616  template<class ARCHIVE>
617  void serialize(ARCHIVE & ar, const unsigned int version) {
618  ar & BOOST_SERIALIZATION_NVP(rankTolerance);
619  ar & BOOST_SERIALIZATION_NVP(enableEPI);
620  ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
621  ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
622  }
623 #endif
624 };
625 
630 class TriangulationResult : public std::optional<Point3> {
631  public:
632  enum Status { VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT };
634 
635  private:
636  TriangulationResult(Status s) : status(s) {}
637 
638  public:
643 
647  TriangulationResult(const Point3& p) : status(VALID) { emplace(p); }
649  return TriangulationResult(DEGENERATE);
650  }
651  static TriangulationResult Outlier() { return TriangulationResult(OUTLIER); }
653  return TriangulationResult(FAR_POINT);
654  }
656  return TriangulationResult(BEHIND_CAMERA);
657  }
658  bool valid() const { return status == VALID; }
659  bool degenerate() const { return status == DEGENERATE; }
660  bool outlier() const { return status == OUTLIER; }
661  bool farPoint() const { return status == FAR_POINT; }
662  bool behindCamera() const { return status == BEHIND_CAMERA; }
663  const gtsam::Point3& get() const {
664  if (!has_value()) throw std::runtime_error("TriangulationResult has no value");
665  return value();
666  }
667  // stream to output
668  friend std::ostream& operator<<(std::ostream& os,
669  const TriangulationResult& result) {
670  if (result)
671  os << "point = " << *result << std::endl;
672  else
673  os << "no point, status = " << result.status << std::endl;
674  return os;
675  }
676 
677  private:
678 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
679  friend class boost::serialization::access;
681  template <class ARCHIVE>
682  void serialize(ARCHIVE& ar, const unsigned int version) {
683  ar& BOOST_SERIALIZATION_NVP(status);
684  }
685 #endif
686 };
687 
689 template<class CAMERA>
691  const typename CAMERA::MeasurementVector& measured,
693 
694  size_t m = cameras.size();
695 
696  // if we have a single pose the corresponding factor is uninformative
697  if (m < 2)
699  else
700  // We triangulate the 3D position of the landmark
701  try {
702  Point3 point =
703  triangulatePoint3<CAMERA>(cameras, measured, params.rankTolerance,
704  params.enableEPI, params.noiseModel);
705 
706  // Check landmark distance and re-projection errors to avoid outliers
707  size_t i = 0;
708  double maxReprojError = 0.0;
709  for(const CAMERA& camera: cameras) {
710  const Pose3& pose = camera.pose();
711  if (params.landmarkDistanceThreshold > 0
712  && distance3(pose.translation(), point)
713  > params.landmarkDistanceThreshold)
715 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
716  // verify that the triangulated point lies in front of all cameras
717  // Only needed if this was not yet handled by exception
718  const Point3& p_local = pose.transformTo(point);
719  if (p_local.z() <= 0)
721 #endif
722  // Check reprojection error
723  if (params.dynamicOutlierRejectionThreshold > 0) {
724  const typename CAMERA::Measurement& zi = measured.at(i);
725  Point2 reprojectionError = camera.reprojectionError(point, zi);
726  maxReprojError = std::max(maxReprojError, reprojectionError.norm());
727  }
728  i += 1;
729  }
730  // Flag as degenerate if average reprojection error is too large
731  if (params.dynamicOutlierRejectionThreshold > 0
732  && maxReprojError > params.dynamicOutlierRejectionThreshold)
734 
735  // all good!
736  return TriangulationResult(point);
738  // This exception is thrown if
739  // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
740  // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
743  // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
745  }
746 }
747 
748 // Vector of Cameras - used by the Python/MATLAB wrapper
755 } // \namespace gtsam
Point2 measured(-17, 30)
Matrix3f m
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1, const SharedNoiseModel &_noiseModel=nullptr)
std::vector< Point3, Eigen::aligned_allocator< Point3 > > Point3Vector
Definition: Point3.h:39
#define max(a, b)
Definition: datatypes.h:20
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))
TriangulationResult(const Point3 &p)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
SharedNoiseModel noiseModel
used in the nonlinear triangulation
static TriangulationResult Outlier()
Factor Graph consisting of non-linear factors.
std::string serialize(const T &input)
serializes to a string
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromPoses(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal)
noiseModel::Diagonal::shared_ptr model
Base class to create smart factors on poses or cameras.
Vector2 Point2
Definition: Point2.h:32
PinholeCamera< Cal3Bundler0 > Camera
leaf::MyValues values
Point3Vector calibrateMeasurements(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measurements)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
noiseModel::Isotropic::shared_ptr SharedIsotropic
Definition: NoiseModel.h:745
Calibrated camera with spherical projection.
Definition: BFloat16.h:88
Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base.
NonlinearFactorGraph graph
A set of cameras, all with their own calibration.
Definition: CameraSet.h:36
static TriangulationResult BehindCamera()
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:610
static const SmartProjectionParams params
friend std::ostream & operator<<(std::ostream &os, const TriangulationResult &result)
Base class for all pinhole cameras.
static Point2 measurement(323.0, 240.0)
std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 > > projectionMatricesFromCameras(const CameraSet< CAMERA > &cameras)
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...
Values result
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
Cal3_S2 createPinholeCalibration(const CALIBRATION &cal)
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
bool enableEPI
if set to true, will refine triangulation using LM
double rankTolerance
(the rank is the number of singular values of the triangulation matrix which are larger than rankTole...
GenericMeasurement< Point2, Point2 > Measurement
Definition: simulated2D.h:278
static const std::shared_ptr< Cal3_S2 > sharedCal
Point2Vector undistortMeasurements(const CALIBRATION &cal, const Point2Vector &measurements)
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition: triangulation.h:42
The most common 5DOF 3D->2D calibration.
Definition: Cal3_S2.h:34
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)
EIGEN_DONT_INLINE void transform(const Transformation &t, Data &data)
Definition: geometry.cpp:25
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition: triangulation.h:50
static TriangulationResult FarPoint()
traits
Definition: chartTesting.h:28
friend std::ostream & operator<<(std::ostream &os, const TriangulationParameters &p)
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
ofstream os("timeSchurFactors.csv")
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
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
static TriangulationResult Degenerate()
3D Point
float * p
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
MEASUREMENT undistortMeasurementInternal(const CALIBRATION &cal, const MEASUREMENT &measurement, std::optional< Cal3_S2 > pinholeCal={})
void insert(Key j, const Value &val)
Definition: Values.cpp:155
Point2Vector MeasurementVector
Definition: PinholeCamera.h:42
Calibration of a fisheye camera.
Vector3 Point3
Definition: Point3.h:38
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, std::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate, const SharedNoiseModel &model=nullptr)
Unified Calibration Model, see Mei07icra for details.
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:27
2D Pose
static const CalibratedCamera camera(kDefaultPose)
std::vector< Unit3 > MeasurementVector
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Calibration used by Bundler.
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:741
The most common 5DOF 3D->2D calibration.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:594
Point3Vector calibrateMeasurementsShared(const CALIBRATION &cal, const Point2Vector &measurements)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:40:33