triangulation.cpp
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 
24 
25 namespace gtsam {
26 
28  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
29  projection_matrices,
30  const Point2Vector& measurements, double rank_tol) {
31  // number of cameras
32  size_t m = projection_matrices.size();
33 
34  // Allocate DLT matrix
35  Matrix A = Matrix::Zero(m * 2, 4);
36 
37  for (size_t i = 0; i < m; i++) {
38  size_t row = i * 2;
39  const Matrix34& projection = projection_matrices.at(i);
40  const Point2& p = measurements.at(i);
41 
42  // build system of equations
43  // [A_1; A_2; A_3] x = [b_1; b_2; b_3]
44  // [b_3 * A_1 - b_1 * A_3] x = 0
45  // [b_3 * A_2 - b_2 * A_3] x = 0
46  // A' x = 0
47  // A' 2x4 = [b_3 * A_1 - b_1 * A_3; b_3 * A_2 - b_2 * A_3]
48  A.row(row) = p.x() * projection.row(2) - projection.row(0);
49  A.row(row + 1) = p.y() * projection.row(2) - projection.row(1);
50  }
51  const auto [rank, error, v] = DLT(A, rank_tol);
52 
53  if (rank < 3) throw(TriangulationUnderconstrainedException());
54 
55  return v;
56 }
57 
59  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
60  projection_matrices,
61  const std::vector<Unit3>& measurements, double rank_tol) {
62  // number of cameras
63  size_t m = projection_matrices.size();
64 
65  // Allocate DLT matrix
66  Matrix A = Matrix::Zero(m * 2, 4);
67 
68  for (size_t i = 0; i < m; i++) {
69  size_t row = i * 2;
70  const Matrix34& projection = projection_matrices.at(i);
71  const Point3& p =
72  measurements.at(i)
73  .point3(); // to get access to x,y,z of the bearing vector
74 
75  // build system of equations
76  A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
77  A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
78  }
79  const auto [rank, error, v] = DLT(A, rank_tol);
80 
81  if (rank < 3) throw(TriangulationUnderconstrainedException());
82 
83  return v;
84 }
85 
86 Point3 triangulateLOST(const std::vector<Pose3>& poses,
87  const Point3Vector& calibratedMeasurements,
88  const SharedIsotropic& measurementNoise,
89  double rank_tol) {
90  size_t m = calibratedMeasurements.size();
91  assert(m == poses.size());
92 
93  // Construct the system matrices.
94  Matrix A = Matrix::Zero(m * 2, 3);
95  Matrix b = Matrix::Zero(m * 2, 1);
96 
97  for (size_t i = 0; i < m; i++) {
98  const Pose3& wTi = poses[i];
99  // TODO(akshay-krishnan): are there better ways to select j?
100  int j = (i + 1) % m;
101  const Pose3& wTj = poses[j];
102 
103  Point3 d_ij = wTj.translation() - wTi.translation();
104  Point3 wZi = wTi.rotation().rotate(calibratedMeasurements[i]);
105  Point3 wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
106  double num_i = wZi.cross(wZj).norm();
107  double den_i = d_ij.cross(wZj).norm();
108 
109  // Handle q_i = 0 (or NaN), which arises if the measurement vectors, wZi and
110  // wZj, coincide (or the baseline vector coincides with the jth measurement
111  // vector).
112  if (num_i == 0 || den_i == 0) {
113  bool success = false;
114  for (size_t k = 2; k < m; k++) {
115  j = (i + k) % m;
116  const Pose3& wTj = poses[j];
117 
118  d_ij = wTj.translation() - wTi.translation();
119  wZj = wTj.rotation().rotate(calibratedMeasurements[j]);
120  num_i = wZi.cross(wZj).norm();
121  den_i = d_ij.cross(wZj).norm();
122  if (num_i > 0 && den_i > 0) {
123  success = true;
124  break;
125  }
126  }
127  if (!success) throw(TriangulationUnderconstrainedException());
128  }
129 
130  // Note: Setting q_i = 1.0 gives same results as DLT.
131  const double q_i = num_i / (measurementNoise->sigma() * den_i);
132 
133  const Matrix23 coefficientMat =
134  q_i * skewSymmetric(calibratedMeasurements[i]).topLeftCorner(2, 3) *
135  wTi.rotation().matrix().transpose();
136 
137  A.block<2, 3>(2 * i, 0) << coefficientMat;
138  b.block<2, 1>(2 * i, 0) << coefficientMat * wTi.translation();
139  }
140 
141  Eigen::ColPivHouseholderQR<Matrix> A_Qr = A.colPivHouseholderQr();
142  A_Qr.setThreshold(rank_tol);
143 
144  if (A_Qr.rank() < 3) throw(TriangulationUnderconstrainedException());
145 
146  return A_Qr.solve(b);
147 }
148 
150  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
151  projection_matrices,
152  const Point2Vector& measurements, double rank_tol) {
153  Vector4 v =
154  triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
155  // Create 3D point from homogeneous coordinates
156  return Point3(v.head<3>() / v[3]);
157 }
158 
160  const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>&
161  projection_matrices,
162  const std::vector<Unit3>& measurements, double rank_tol) {
163  // contrary to previous triangulateDLT, this is now taking Unit3 inputs
164  Vector4 v =
165  triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol);
166  // Create 3D point from homogeneous coordinates
167  return Point3(v.head<3>() / v[3]);
168 }
169 
178  Key landmarkKey) {
179  // Maybe we should consider Gauss-Newton?
183  params.lambdaInitial = 1;
184  params.lambdaFactor = 10;
185  params.maxIterations = 100;
186  params.absoluteErrorTol = 1.0;
190 
192  Values result = optimizer.optimize();
193 
194  return result.at<Point3>(landmarkKey);
195 }
196 
197 } // namespace gtsam
Eigen::ColPivHouseholderQR::setThreshold
ColPivHouseholderQR & setThreshold(const RealScalar &threshold)
Definition: ColPivHouseholderQR.h:353
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
gtsam::DLT
std::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Definition: Matrix.cpp:566
Eigen::ColPivHouseholderQR::rank
Index rank() const
Definition: ColPivHouseholderQR.h:255
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Definition: triangulation.cpp:177
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
tree::projection
Expression< Point2 > projection(f, p_cam)
gtsam::NonlinearOptimizerParams::SILENT
@ SILENT
Definition: NonlinearOptimizerParams.h:39
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
different_sigmas::values
HybridValues values
Definition: testHybridBayesNet.cpp:245
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
gtsam::skewSymmetric
Matrix3 skewSymmetric(double wx, double wy, double wz)
Definition: base/Matrix.h:400
result
Values result
Definition: OdometryOptimize.cpp:8
triangulation.h
Functions for triangulation.
gtsam::Values::at
const ValueType at(Key j) const
Definition: Values-inl.h:261
A
Definition: test_numpy_dtypes.cpp:298
Eigen::aligned_allocator
STL compatible allocator to use with types requiring a non standrad alignment.
Definition: Memory.h:878
j
std::ptrdiff_t j
Definition: tut_arithmetic_redux_minmax.cpp:2
gtsam::Rot3::rotate
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Definition: Rot3M.cpp:148
gtsam::row
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
Definition: base/Matrix.h:221
gtsam::Pose3
Definition: Pose3.h:37
gtsam::NonlinearFactorGraph
Definition: NonlinearFactorGraph.h:55
gtsam::LevenbergMarquardtParams::TRYLAMBDA
@ TRYLAMBDA
Definition: LevenbergMarquardtParams.h:40
gtsam::Pose3::translation
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Definition: Pose3.cpp:308
PinholeCamera.h
Base class for all pinhole cameras.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
m
Matrix3f m
Definition: AngleAxis_mimic_euler.cpp:1
gtsam::Point2
Vector2 Point2
Definition: Point2.h:32
Eigen::ColPivHouseholderQR
Householder rank-revealing QR decomposition of a matrix with column-pivoting.
Definition: ForwardDeclarations.h:274
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::b
const G & b
Definition: Group.h:79
gtsam::Pose3::rotation
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
Definition: Pose3.cpp:315
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
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
gtsam::Values
Definition: Values.h:65
p
float * p
Definition: Tutorial_Map_using.cpp:9
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::Rot3::matrix
Matrix3 matrix() const
Definition: Rot3M.cpp:218
gtsam::Point2Vector
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
Definition: Point2.h:49
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
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::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
gtsam::NonlinearOptimizerParams::ERROR
@ ERROR
Definition: NonlinearOptimizerParams.h:39
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LevenbergMarquardtParams::SILENT
@ SILENT
Definition: LevenbergMarquardtParams.h:40
i
int i
Definition: BiCGSTAB_step_by_step.cpp:9
gtsam::Point3Vector
std::vector< Point3, Eigen::aligned_allocator< Point3 > > Point3Vector
Definition: Point3.h:39
gtsam::NonlinearOptimizerParams::MULTIFRONTAL_CHOLESKY
@ MULTIFRONTAL_CHOLESKY
Definition: NonlinearOptimizerParams.h:99


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