Go to the documentation of this file.
32 size_t m = projection_matrices.size();
37 for (
size_t i = 0;
i <
m;
i++) {
39 const Matrix34&
projection = projection_matrices.at(
i);
51 const auto [rank,
error,
v] =
DLT(
A, rank_tol);
61 const std::vector<Unit3>&
measurements,
double rank_tol) {
63 size_t m = projection_matrices.size();
68 for (
size_t i = 0;
i <
m;
i++) {
70 const Matrix34&
projection = projection_matrices.at(
i);
79 const auto [rank,
error,
v] =
DLT(
A, rank_tol);
90 size_t m = calibratedMeasurements.size();
91 assert(
m == poses.size());
97 for (
size_t i = 0;
i <
m;
i++) {
98 const Pose3& wTi = poses[
i];
101 const Pose3& wTj = poses[
j];
106 double num_i = wZi.cross(wZj).norm();
107 double den_i = d_ij.cross(wZj).norm();
112 if (num_i == 0 || den_i == 0) {
113 bool success =
false;
114 for (
size_t k = 2; k <
m; k++) {
116 const Pose3& wTj = poses[
j];
120 num_i = wZi.cross(wZj).norm();
121 den_i = d_ij.cross(wZj).norm();
122 if (num_i > 0 && den_i > 0) {
131 const double q_i = num_i / (measurementNoise->sigma() * den_i);
133 const Matrix23 coefficientMat =
134 q_i *
skewSymmetric(calibratedMeasurements[
i]).topLeftCorner(2, 3) *
137 A.block<2, 3>(2 *
i, 0) << coefficientMat;
138 b.block<2, 1>(2 *
i, 0) << coefficientMat * wTi.
translation();
146 return A_Qr.solve(
b);
162 const std::vector<Unit3>&
measurements,
double rank_tol) {
185 params.maxIterations = 100;
186 params.absoluteErrorTol = 1.0;
ColPivHouseholderQR & setThreshold(const RealScalar &threshold)
virtual const Values & optimize()
std::tuple< int, double, Vector > DLT(const Matrix &A, double rank_tol)
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Expression< Point2 > projection(f, p_cam)
static const SmartProjectionParams params
Matrix3 skewSymmetric(double wx, double wy, double wz)
Functions for triangulation.
const ValueType at(Key j) const
STL compatible allocator to use with types requiring a non standrad alignment.
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
const MATRIX::ConstRowXpr row(const MATRIX &A, size_t j)
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Base class for all pinhole cameras.
Householder rank-revealing QR decomposition of a matrix with column-pivoting.
Point3 triangulateDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
std::vector< double > measurements
Exception thrown by triangulateDLT when SVD returns rank < 3.
Array< int, Dynamic, 1 > v
std::vector< Point2, Eigen::aligned_allocator< Point2 > > Point2Vector
noiseModel::Isotropic::shared_ptr SharedIsotropic
Vector4 triangulateHomogeneousDLT(const std::vector< Matrix34, Eigen::aligned_allocator< Matrix34 >> &projection_matrices, const Point2Vector &measurements, double rank_tol)
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....
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
std::vector< Point3, Eigen::aligned_allocator< Point3 > > Point3Vector
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:09:31