1 #include "TriangulationMidPoint.hh"
2 #include <Base/Math/Matrix.hh>
3 #include <MathAlgo/LeastSquares.hh>
4 #include <MathAlgo/SVD.hh>
25 const bool computeDist)
27 const unsigned int num = pos.size();
28 BIASASSERT(dir.size() == pos.size());
61 for (
unsigned int i = 0; i < num; i++) {
62 for (
unsigned int k = 0; k < 3; k++, j++) {
64 A[j][i+3] = -dir[i][k];
69 if (solver.
Solve(A, b, x) != 0)
return -1;
72 for (
unsigned int k = 0; k < 3; k++) {
74 if (BIAS_ISNAN(x[k]))
return -1;
81 for (
unsigned int i = 0; i < num; i++) {
82 d = pos[i] + x[i+3]*dir[i] - res;
89 for (
unsigned int i = 0; i < num; i++) {
90 if (x[i+3] < 0)
return i+1;
97 Triangulate_(
const std::vector< BIAS::ProjectionParametersBase* >& cams,
98 const std::vector< BIAS::HomgPoint2D >& points2d,
100 const bool computeDist)
102 const unsigned int num = cams.size();
103 BIASASSERT(points2d.size() == num);
104 std::vector< BIAS::Vector3<double> > pos;
105 std::vector< BIAS::Vector3<double> > dir;
110 for (
unsigned int i = 0; i < num; i++) {
111 BIASASSERT(points2d[i].IsHomogenized());
112 cams[i]->UnProjectToRay(points2d[i], p, d);
119 return Intersect_(pos, dir, point3d, dist, computeDist);
125 const std::vector< BIAS::HomgPoint2D >& rays,
127 const bool computeDist)
129 const unsigned int num = poses.size();
130 BIASASSERT(rays.size() == num);
131 std::vector< BIAS::Vector3<double> > pos;
132 std::vector< BIAS::Vector3<double> > dir;
136 for (
unsigned int i = 0; i < num; i++) {
137 pos.push_back(poses[i].GetPosition());
141 return Intersect_(pos, dir, point3d, dist, computeDist);
int Intersect_(const std::vector< BIAS::Vector3< double > > &pos, const std::vector< BIAS::Vector3< double > > &dir, BIAS::Vector3< double > &res, double &dist, const bool computeDist)
Linear least squares solver based on Lapack routines.
bool Equal(const T left, const T right, const T eps)
comparison function for floating point values See http://www.boost.org/libs/test/doc/components/test_...
virtual int Solve(Matrix< double > &A, Vector< double > &b, Vector< double > &x)
LeastSquaresLapack.
int Triangulate_(const std::vector< BIAS::ProjectionParametersBase * > &cams, const std::vector< BIAS::HomgPoint2D > &points2d, BIAS::Vector3< double > &point3d, double &dist, const bool computeDist)
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)