gtsam  4.1.0
gtsam
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 
23 #include <gtsam/geometry/Pose2.h>
24 #include <gtsam/slam/TriangulationFactor.h>
26 #include <gtsam/inference/Symbol.h>
27 
28 namespace gtsam {
29 
31 class GTSAM_EXPORT TriangulationUnderconstrainedException: public std::runtime_error {
32 public:
34  std::runtime_error("Triangulation Underconstrained Exception.") {
35  }
36 };
37 
39 class GTSAM_EXPORT TriangulationCheiralityException: public std::runtime_error {
40 public:
42  std::runtime_error(
43  "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
44  }
45 };
46 
54 GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
55  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
56  const Point2Vector& measurements, double rank_tol = 1e-9);
57 
65 GTSAM_EXPORT Point3 triangulateDLT(
66  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
67  const Point2Vector& measurements,
68  double rank_tol = 1e-9);
69 
79 template<class CALIBRATION>
80 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
81  const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
82  const Point2Vector& measurements, Key landmarkKey,
83  const Point3& initialEstimate) {
84  Values values;
85  values.insert(landmarkKey, initialEstimate); // Initial landmark value
88  static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
89  for (size_t i = 0; i < measurements.size(); i++) {
90  const Pose3& pose_i = poses[i];
91  typedef PinholePose<CALIBRATION> Camera;
92  Camera camera_i(pose_i, sharedCal);
93  graph.emplace_shared<TriangulationFactor<Camera> > //
94  (camera_i, measurements[i], unit2, landmarkKey);
95  }
96  return std::make_pair(graph, values);
97 }
98 
108 template<class CAMERA>
109 std::pair<NonlinearFactorGraph, Values> triangulationGraph(
110  const CameraSet<CAMERA>& cameras,
111  const typename CAMERA::MeasurementVector& measurements, Key landmarkKey,
112  const Point3& initialEstimate) {
113  Values values;
114  values.insert(landmarkKey, initialEstimate); // Initial landmark value
115  NonlinearFactorGraph graph;
118  for (size_t i = 0; i < measurements.size(); i++) {
119  const CAMERA& camera_i = cameras[i];
120  graph.emplace_shared<TriangulationFactor<CAMERA> > //
121  (camera_i, measurements[i], unit, landmarkKey);
122  }
123  return std::make_pair(graph, values);
124 }
125 
133 GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
134  const Values& values, Key landmarkKey);
135 
144 template<class CALIBRATION>
145 Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
146  boost::shared_ptr<CALIBRATION> sharedCal,
147  const Point2Vector& measurements, const Point3& initialEstimate) {
148 
149  // Create a factor graph and initial values
150  Values values;
151  NonlinearFactorGraph graph;
152  boost::tie(graph, values) = triangulationGraph<CALIBRATION> //
153  (poses, sharedCal, measurements, Symbol('p', 0), initialEstimate);
154 
155  return optimize(graph, values, Symbol('p', 0));
156 }
157 
165 template<class CAMERA>
167  const CameraSet<CAMERA>& cameras,
168  const typename CAMERA::MeasurementVector& measurements, const Point3& initialEstimate) {
169 
170  // Create a factor graph and initial values
171  Values values;
172  NonlinearFactorGraph graph;
173  boost::tie(graph, values) = triangulationGraph<CAMERA> //
174  (cameras, measurements, Symbol('p', 0), initialEstimate);
175 
176  return optimize(graph, values, Symbol('p', 0));
177 }
178 
186 template<class CALIBRATION>
188  CameraProjectionMatrix(const CALIBRATION& calibration) :
189  K_(calibration.K()) {
190  }
191  Matrix34 operator()(const Pose3& pose) const {
192  return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
193  }
194 private:
195  const Matrix3 K_;
196 public:
198 };
199 
212 template<class CALIBRATION>
213 Point3 triangulatePoint3(const std::vector<Pose3>& poses,
214  boost::shared_ptr<CALIBRATION> sharedCal,
215  const Point2Vector& measurements, double rank_tol = 1e-9,
216  bool optimize = false) {
217 
218  assert(poses.size() == measurements.size());
219  if (poses.size() < 2)
221 
222  // construct projection matrices from poses & calibration
223  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
224  CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
225  for(const Pose3& pose: poses)
226  projection_matrices.push_back(createP(pose));
227 
228  // Triangulate linearly
229  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
230 
231  // Then refine using non-linear optimization
232  if (optimize)
233  point = triangulateNonlinear<CALIBRATION> //
234  (poses, sharedCal, measurements, point);
235 
236 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
237  // verify that the triangulated point lies in front of all cameras
238  for(const Pose3& pose: poses) {
239  const Point3& p_local = pose.transformTo(point);
240  if (p_local.z() <= 0)
242  }
243 #endif
244 
245  return point;
246 }
247 
260 template<class CAMERA>
262  const CameraSet<CAMERA>& cameras,
263  const typename CAMERA::MeasurementVector& measurements, double rank_tol = 1e-9,
264  bool optimize = false) {
265 
266  size_t m = cameras.size();
267  assert(measurements.size() == m);
268 
269  if (m < 2)
271 
272  // construct projection matrices from poses & calibration
273  std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
274  for(const CAMERA& camera: cameras)
275  projection_matrices.push_back(
277  camera.pose()));
278  Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
279 
280  // The n refine using non-linear optimization
281  if (optimize)
282  point = triangulateNonlinear<CAMERA>(cameras, measurements, point);
283 
284 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
285  // verify that the triangulated point lies in front of all cameras
286  for(const CAMERA& camera: cameras) {
287  const Point3& p_local = camera.pose().transformTo(point);
288  if (p_local.z() <= 0)
290  }
291 #endif
292 
293  return point;
294 }
295 
297 template<class CALIBRATION>
299  const CameraSet<PinholeCamera<CALIBRATION> >& cameras,
300  const Point2Vector& measurements, double rank_tol = 1e-9,
301  bool optimize = false) {
302  return triangulatePoint3<PinholeCamera<CALIBRATION> > //
303  (cameras, measurements, rank_tol, optimize);
304 }
305 
306 struct GTSAM_EXPORT TriangulationParameters {
307 
308  double rankTolerance;
309  bool enableEPI;
311 
317 
324 
333  TriangulationParameters(const double _rankTolerance = 1.0,
334  const bool _enableEPI = false, double _landmarkDistanceThreshold = -1,
335  double _dynamicOutlierRejectionThreshold = -1) :
336  rankTolerance(_rankTolerance), enableEPI(_enableEPI), //
337  landmarkDistanceThreshold(_landmarkDistanceThreshold), //
338  dynamicOutlierRejectionThreshold(_dynamicOutlierRejectionThreshold) {
339  }
340 
341  // stream to output
342  friend std::ostream &operator<<(std::ostream &os,
343  const TriangulationParameters& p) {
344  os << "rankTolerance = " << p.rankTolerance << std::endl;
345  os << "enableEPI = " << p.enableEPI << std::endl;
346  os << "landmarkDistanceThreshold = " << p.landmarkDistanceThreshold
347  << std::endl;
348  os << "dynamicOutlierRejectionThreshold = "
349  << p.dynamicOutlierRejectionThreshold << std::endl;
350  return os;
351  }
352 
353 private:
354 
356  friend class boost::serialization::access;
357  template<class ARCHIVE>
358  void serialize(ARCHIVE & ar, const unsigned int version) {
359  ar & BOOST_SERIALIZATION_NVP(rankTolerance);
360  ar & BOOST_SERIALIZATION_NVP(enableEPI);
361  ar & BOOST_SERIALIZATION_NVP(landmarkDistanceThreshold);
362  ar & BOOST_SERIALIZATION_NVP(dynamicOutlierRejectionThreshold);
363  }
364 };
365 
369 class TriangulationResult: public boost::optional<Point3> {
370  enum Status {
371  VALID, DEGENERATE, BEHIND_CAMERA, OUTLIER, FAR_POINT
372  };
373  Status status_;
374  TriangulationResult(Status s) :
375  status_(s) {
376  }
377 public:
378 
383 
388  status_(VALID) {
389  reset(p);
390  }
391  static TriangulationResult Degenerate() {
392  return TriangulationResult(DEGENERATE);
393  }
394  static TriangulationResult Outlier() {
395  return TriangulationResult(OUTLIER);
396  }
397  static TriangulationResult FarPoint() {
398  return TriangulationResult(FAR_POINT);
399  }
400  static TriangulationResult BehindCamera() {
401  return TriangulationResult(BEHIND_CAMERA);
402  }
403  bool valid() const {
404  return status_ == VALID;
405  }
406  bool degenerate() const {
407  return status_ == DEGENERATE;
408  }
409  bool outlier() const {
410  return status_ == OUTLIER;
411  }
412  bool farPoint() const {
413  return status_ == FAR_POINT;
414  }
415  bool behindCamera() const {
416  return status_ == BEHIND_CAMERA;
417  }
418  // stream to output
419  friend std::ostream &operator<<(std::ostream &os,
420  const TriangulationResult& result) {
421  if (result)
422  os << "point = " << *result << std::endl;
423  else
424  os << "no point, status = " << result.status_ << std::endl;
425  return os;
426  }
427 
428 private:
429 
432  template<class ARCHIVE>
433  void serialize(ARCHIVE & ar, const unsigned int version) {
434  ar & BOOST_SERIALIZATION_NVP(status_);
435  }
436 };
437 
439 template<class CAMERA>
441  const typename CAMERA::MeasurementVector& measured,
442  const TriangulationParameters& params) {
443 
444  size_t m = cameras.size();
445 
446  // if we have a single pose the corresponding factor is uninformative
447  if (m < 2)
448  return TriangulationResult::Degenerate();
449  else
450  // We triangulate the 3D position of the landmark
451  try {
452  Point3 point = triangulatePoint3<CAMERA>(cameras, measured,
453  params.rankTolerance, params.enableEPI);
454 
455  // Check landmark distance and re-projection errors to avoid outliers
456  size_t i = 0;
457  double maxReprojError = 0.0;
458  for(const CAMERA& camera: cameras) {
459  const Pose3& pose = camera.pose();
460  if (params.landmarkDistanceThreshold > 0
461  && distance3(pose.translation(), point)
462  > params.landmarkDistanceThreshold)
463  return TriangulationResult::FarPoint();
464 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
465  // verify that the triangulated point lies in front of all cameras
466  // Only needed if this was not yet handled by exception
467  const Point3& p_local = pose.transformTo(point);
468  if (p_local.z() <= 0)
469  return TriangulationResult::BehindCamera();
470 #endif
471  // Check reprojection error
472  if (params.dynamicOutlierRejectionThreshold > 0) {
473  const Point2& zi = measured.at(i);
474  Point2 reprojectionError(camera.project(point) - zi);
475  maxReprojError = std::max(maxReprojError, reprojectionError.norm());
476  }
477  i += 1;
478  }
479  // Flag as degenerate if average reprojection error is too large
480  if (params.dynamicOutlierRejectionThreshold > 0
481  && maxReprojError > params.dynamicOutlierRejectionThreshold)
482  return TriangulationResult::Outlier();
483 
484  // all good!
485  return TriangulationResult(point);
487  // This exception is thrown if
488  // 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
489  // 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
490  return TriangulationResult::Degenerate();
492  // point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
493  return TriangulationResult::BehindCamera();
494  }
495 }
496 
497 } // \namespace gtsam
498 
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself=boost::none) const
get translation
Definition: Pose3.cpp:259
gtsam::NonlinearFactorGraph
A non-linear factor graph is a graph of non-Gaussian, i.e.
Definition: NonlinearFactorGraph.h:78
gtsam::noiseModel::Unit::Create
static shared_ptr Create(size_t dim)
Create a unit covariance noise model.
Definition: NoiseModel.h:608
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::CameraProjectionMatrix
Create a 3*4 camera projection matrix from calibration and pose.
Definition: triangulation.h:187
gtsam::triangulatePoint3
Point3 triangulatePoint3(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, double rank_tol=1e-9, bool optimize=false)
Function to triangulate 3D landmark point from an arbitrary number of poses (at least 2) using the DL...
Definition: triangulation.h:213
gtsam::Pose3::transformTo
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
gtsam::TriangulationFactor
Definition: TriangulationFactor.h:31
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Note, deliberately not in noiseModel namespace.
Definition: NoiseModel.h:734
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::TriangulationParameters::enableEPI
bool enableEPI
if set to true, will refine triangulation using LM
Definition: triangulation.h:310
gtsam::TriangulationResult
TriangulationResult is an optional point, along with the reasons why it is invalid.
Definition: triangulation.h:369
gtsam::TriangulationParameters::dynamicOutlierRejectionThreshold
double dynamicOutlierRejectionThreshold
If this is nonnegative the we will check if the average reprojection error is smaller than this thres...
Definition: triangulation.h:323
gtsam::serialize
std::string serialize(const T &input)
serializes to a string
Definition: serialization.h:100
gtsam::CameraSet
A set of cameras, all with their own calibration.
Definition: CameraSet.h:35
NonlinearFactorGraph.h
Factor Graph Constsiting of non-linear factors.
gtsam::TriangulationParameters::rankTolerance
double rankTolerance
threshold to decide whether triangulation is result.degenerate
Definition: triangulation.h:308
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
CameraSet.h
Base class to create smart factors on poses or cameras.
Pose2.h
2D Pose
gtsam::Pose3::inverse
Pose3 inverse() const
inverse transformation with derivatives
Definition: Pose3.cpp:50
gtsam::Point2
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
gtsam::PinholePose
Definition: PinholePose.h:237
gtsam::Values::insert
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:140
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
#define GTSAM_MAKE_ALIGNED_OPERATOR_NEW
This marks a GTSAM object to require alignment.
Definition: types.h:269
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:26
gtsam::PinholeCamera
Definition: PinholeCamera.h:33
gtsam::Pose3::matrix
Matrix4 matrix() const
convert to 4*4 matrix
Definition: Pose3.cpp:274
gtsam::Pose3
Definition: Pose3.h:37
gtsam::triangulateDLT
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:56
gtsam::TriangulationCheiralityException
Exception thrown by triangulateDLT when landmark is behind one or more of the cameras.
Definition: triangulation.h:39
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:440
gtsam::Point3
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:35
gtsam::TriangulationResult::TriangulationResult
TriangulationResult()
Default constructor, only for serialization.
Definition: triangulation.h:382
gtsam::triangulateHomogeneousDLT
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:26
Symbol.h
gtsam::Values
A non-templated config holding any types of Manifold-group elements.
Definition: Values.h:71
gtsam::triangulationGraph
std::pair< NonlinearFactorGraph, Values > triangulationGraph(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, Key landmarkKey, const Point3 &initialEstimate)
Create a factor graph with projection factors from poses and one calibration.
Definition: triangulation.h:80
gtsam::noiseModel::Isotropic::Sigma
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:567
gtsam::triangulateNonlinear
Point3 triangulateNonlinear(const std::vector< Pose3 > &poses, boost::shared_ptr< CALIBRATION > sharedCal, const Point2Vector &measurements, const Point3 &initialEstimate)
Given an initial estimate , refine a point using measurements in several cameras.
Definition: triangulation.h:145
gtsam::TriangulationResult::TriangulationResult
TriangulationResult(const Point3 &p)
Constructor.
Definition: triangulation.h:387
gtsam::TriangulationResult::access
friend class boost::serialization::access
Serialization function.
Definition: triangulation.h:431
gtsam::TriangulationParameters::TriangulationParameters
TriangulationParameters(const double _rankTolerance=1.0, const bool _enableEPI=false, double _landmarkDistanceThreshold=-1, double _dynamicOutlierRejectionThreshold=-1)
Constructor.
Definition: triangulation.h:333
gtsam::TriangulationParameters
Definition: triangulation.h:306
gtsam::TriangulationParameters::landmarkDistanceThreshold
double landmarkDistanceThreshold
if the landmark is triangulated at distance larger than this, result is flagged as degenerate.
Definition: triangulation.h:316
gtsam::Symbol
Character and index key used to refer to variables.
Definition: Symbol.h:33
gtsam::TriangulationUnderconstrainedException
Exception thrown by triangulateDLT when SVD returns rank < 3.
Definition: triangulation.h:31