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 
19 #pragma once
20 
21 #include <gtsam/geometry/Cal3_S2.h>
25 #include <gtsam/geometry/Pose2.h>
28 #include <gtsam/inference/Symbol.h>
29 
30 namespace gtsam {
31 
33 class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
34 public:
36  std::runtime_error("Triangulation Underconstrained Exception.") {
37  }
38 };
39 
41 class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
42 public:
44  std::runtime_error(
45  "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
46  }
47 };
48 
56 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
57  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
58  const Point2Vector& measurements, double rank_tol = 1e-9);
59 
67 GTSAM_EXPORT Point3 triangulateDLT(
68  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
69  const Point2Vector& measurements,
70  double rank_tol = 1e-9);
71 
81 template<class CALIBRATION>
82 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
83  const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
84  const Point2Vector& measurements, Key landmarkKey,
85  const Point3& initialEstimate) {
86  Values values;
87  values.insert(landmarkKey, initialEstimate); // Initial landmark value
90  static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
91  for (size_t i = 0; i < measurements.size(); i++) {
92  const Pose3& pose_i = poses[i];
94  Camera camera_i(pose_i, sharedCal);
96  (camera_i, measurements[i], unit2, landmarkKey);
97  }
98  return std::make_pair(graph, values);
99 }
100 
110 template<class CAMERA>
111 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
112  const CameraSet<CAMERA>& cameras,
113  const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
114  const Point3& initialEstimate) {
115  Values values;
116  values.insert(landmarkKey, initialEstimate); // Initial landmark value
120  for (size_t i = 0; i < measurements.size(); i++) {
121  const CAMERA& camera_i = cameras[i];
123  (camera_i, measurements[i], unit, landmarkKey);
124  }
125  return std::make_pair(graph, values);
126 }
127 
135 GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
136  const Values& values, Key landmarkKey);
137 
146 template<class CALIBRATION>
147 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
148  boost::shared_ptr<CALIBRATION> sharedCal,
149  const Point2Vector& measurements, const Point3& initialEstimate) {
150 
151  // Create a factor graph and initial values
152  Values values;
154  boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
155  (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
156 
157  return optimize(graph, values, Symbol('p', 0));
158 }
159 
167 template<class CAMERA>
169  const CameraSet<CAMERA>& cameras,
170  const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
171 
172  // Create a factor graph and initial values
173  Values values;
175  boost::tie(graph, values) = triangulationGraph<CAMERA> //
176  (cameras, measurements, Symbol('p', 0), initialEstimate);
177 
178  return optimize(graph, values, Symbol('p', 0));
179 }
180 
188 template<class CALIBRATION>
190  CameraProjectionMatrix(const CALIBRATION& calibration) :
191  K_(calibration.K()) {
192  }
193  Matrix34 operator()(const Pose3& pose) const {
194  return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
195  }
196 private:
197  const Matrix3 K_;
198 public:
200 };
201 
214 template<class CALIBRATION>
215 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
216  boost::shared_ptr<CALIBRATION> sharedCal,
217  const Point2Vector& measurements, double rank_tol = 1e-9,
218  bool optimize = false) {
219 
220  assert(poses.size() == measurements.size());
221  if (poses.size() < 2)
223 
224  // construct projection matrices from poses & calibration
225  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
226  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
227  for(const Pose3& pose: poses)
228  projection_matrices.push_back(createP(pose));
229 
230  // Triangulate linearly
231  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
232 
233  // Then refine using non-linear optimization
234  if (optimize)
235  point = triangulateNonlinear<CALIBRATION> //
236  (poses, sharedCal, measurements, point);
237 
238 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
239  // verify that the triangulated point lies in front of all cameras
240  for(const Pose3& pose: poses) {
241  const Point3& p_local = pose.transformTo(point);
242  if (p_local.z() <= 0)
244  }
245 #endif
246 
247  return point;
248 }
249 
262 template<class CAMERA>
264  const CameraSet<CAMERA>& cameras,
265  const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
266  bool optimize = false) {
267 
268  size_t m = cameras.size();
269  assert(measurements.size() == m);
270 
271  if (m < 2)
273 
274  // construct projection matrices from poses & calibration
275  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
276  for(const CAMERA& camera: cameras)
277  projection_matrices.push_back(
279  camera.pose()));
280  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
281 
282  // The n refine using non-linear optimization
283  if (optimize)
284  point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
285 
286 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
287  // verify that the triangulated point lies in front of all cameras
288  for(const CAMERA& camera: cameras) {
289  const Point3& p_local = camera.pose().transformTo(point);
290  if (p_local.z() <= 0)
292  }
293 #endif
294 
295  return point;
296 }
297 
299 template<class CALIBRATION>
301  const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
302  const Point2Vector& measurements, double rank_tol = 1e-9,
303  bool optimize = false) {
304  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
305  (cameras, measurements, rank_tol, optimize);
306 }
307 
308 struct GTSAM_EXPORT TriangulationParameters {
309 
310  double rankTolerance;
311  bool enableEPI;
313 
319 
326 
335  TriangulationParameters(const double _rankTolerance = 1.0,
336  const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
337  double _dynamicOutlierRejectionThreshold = -1) :
338  rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
339  landmarkDistanceThreshold(_landmarkDistanceThreshold), //
340  dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
341  }
342 
343  // stream to output
344  friend std::ostream &operator<<(std::ostream &os,
345  const TriangulationParameters& p) {
346  os << "rankTolerance = " << p.rankTolerance << std::endl;
347  os << "enableEPI = " << p.enableEPI << std::endl;
348  os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
349  << std::endl;
350  os << "dynamicOutlierRejectionThreshold = "
351  << p.dynamicOutlierRejectionThreshold << std::endl;
352  return os;
353  }
354 
355 private:
356 
358  friend class boost::serialization::access;
359  template<class ARCHIVE>
360  void serialize(ARCHIVE & ar, const unsigned int version) {
361  ar & BOOST_SERIALIZATION_NVP(rankTolerance);
362  ar & BOOST_SERIALIZATION_NVP(enableEPI);
363  ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
364  ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
365  }
366 };
367 
371 class TriangulationResult: public boost::optional<Point3> {
372  enum Status {
373  VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
374  };
377  status_(s) {
378  }
379 public:
380 
385 
390  status_(VALID) {
391  reset(p);
392  }
394  return TriangulationResult(DEGENERATE);
395  }
397  return TriangulationResult(OUTLIER);
398  }
400  return TriangulationResult(FAR_POINT);
401  }
403  return TriangulationResult(BEHIND_CAMERA);
404  }
405  bool valid() const {
406  return status_ == VALID;
407  }
408  bool degenerate() const {
409  return status_ == DEGENERATE;
410  }
411  bool outlier() const {
412  return status_ == OUTLIER;
413  }
414  bool farPoint() const {
415  return status_ == FAR_POINT;
416  }
417  bool behindCamera() const {
418  return status_ == BEHIND_CAMERA;
419  }
420  // stream to output
421  friend std::ostream &operator<<(std::ostream &os,
422  const TriangulationResult& result) {
423  if (result)
424  os << "point = " << *result << std::endl;
425  else
426  os << "no point, status = " << result.status_ << std::endl;
427  return os;
428  }
429 
430 private:
431 
433  friend class boost::serialization::access;
434  template<class ARCHIVE>
435  void serialize(ARCHIVE & ar, const unsigned int version) {
436  ar & BOOST_SERIALIZATION_NVP(status_);
437  }
438 };
439 
441 template<class CAMERA>
443  const typename CAMERA::MeasurementVector& measured,
445 
446  size_t m = cameras.size();
447 
448  // if we have a single pose the corresponding factor is uninformative
449  if (m < 2)
451  else
452  // We triangulate the 3D position of the landmark
453  try {
454  Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
455  params.rankTolerance, params.enableEPI);
456 
457  // Check landmark distance and re-projection errors to avoid outliers
458  size_t i = 0;
459  double maxReprojError = 0.0;
460  for(const CAMERA& camera: cameras) {
461  const Pose3& pose = camera.pose();
462  if (params.landmarkDistanceThreshold > 0
463  && distance3(pose.translation(), point)
464  > params.landmarkDistanceThreshold)
466 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
467  // verify that the triangulated point lies in front of all cameras
468  // Only needed if this was not yet handled by exception
469  const Point3& p_local = pose.transformTo(point);
470  if (p_local.z() <= 0)
472 #endif
473  // Check reprojection error
474  if (params.dynamicOutlierRejectionThreshold > 0) {
475  const Point2& zi = measured.at(i);
476  Point2 reprojectionError(camera.project(point) - zi);
477  maxReprojError = std::max(maxReprojError, reprojectionError.norm());
478  }
479  i += 1;
480  }
481  // Flag as degenerate if average reprojection error is too large
482  if (params.dynamicOutlierRejectionThreshold > 0
483  && maxReprojError > params.dynamicOutlierRejectionThreshold)
485 
486  // all good!
487  return TriangulationResult(point);
489  // This exception is thrown if
490  // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
491  // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
494  // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
496  }
497 }
498 
499 // Vector of Cameras - used by the Python/MATLAB wrapper
502 
503 } // \namespace gtsam
504 
Matrix3f m
#define max(a, b)
Definition: datatypes.h:20
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
TriangulationResult(const Point3 &p)
static TriangulationResult Outlier()
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
Definition: Values.cpp:140
Base class to create smart factors on poses or cameras.
Vector2 Point2
Definition: Point2.h:27
PinholeCamera< Cal3Bundler0 > Camera
leaf::MyValues values
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate)
Definition: Half.h:150
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
NonlinearFactorGraph graph
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
static const boost::shared_ptr< Cal3_S2 > sharedCal
static TriangulationResult BehindCamera()
static shared_ptr Create(size_t dim)
Definition: NoiseModel.h:608
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
Definition: Point3.cpp:26
friend std::ostream & operator<<(std::ostream &os, const TriangulationResult &result)
Point3 point(10, 0,-5)
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1)
Base class for all pinhole cameras.
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:314
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:172
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:50
Values result
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false)
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:721
void serialize(ARCHIVE &ar, const unsigned int version)
static const Pose3 pose(Rot3(Vector3(1,-1,-1).asDiagonal()), Point3(0, 0, 0.5))
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...
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:42
Matrix34 operator()(const Pose3 &pose) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition: triangulation.h:33
static SmartStereoProjectionParams params
TriangulationResult triangulateSafe(const CameraSet< CAMERA > &cameras, const typename CAMERA::MeasurementVector &measured, const TriangulationParameters &params)
triangulateSafe: extensive checking of the outcome
void serialize(ARCHIVE &ar, const unsigned int version)
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition: triangulation.h:41
static TriangulationResult FarPoint()
static SharedNoiseModel unit2(noiseModel::Unit::Create(2))
traits
Definition: chartTesting.h:28
friend std::ostream & operator<<(std::ostream &os, const TriangulationParameters &p)
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate)
Definition: triangulation.h:82
Matrix4 matrix() const
Definition: Pose3.cpp:274
ofstream os("timeSchurFactors.csv")
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
static TriangulationResult Degenerate()
CameraProjectionMatrix(const CALIBRATION &calibration)
float * p
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
Definition: types.h:269
Vector3 Point3
Definition: Point3.h:35
2D Pose
Point3 measured
static const CalibratedCamera camera(kDefaultPose)
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
Calibration used by Bundler.
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:734
The most common 5DOF 3D->2D calibration.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Definition: NoiseModel.cpp:567


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:51:16