25 #include "FMatrixEstimation.hh"
26 #include "MathAlgo/SVD.hh"
28 #include "MathAlgo/Minpack.hh"
29 #include "Base/Geometry/KMatrix.hh"
30 #include "PMatrixEstimation.hh"
32 #include <Base/Common/CompareFloatingPoint.hh>
34 #include <Base/Common/BIASpragma.hh>
40 FErrorFunction_(
void *p,
int m,
int n,
const double *x,
double *fvec,
int iflag) {
44 vector<double> params;
46 for (
unsigned int i=0; i<7; i++){
47 params.push_back(x[i]);
56 const unsigned int uiDataSize = pCurrentObject->
p1_->size();
57 const vector<BIAS::HomgPoint2D> &p1 = *pCurrentObject->
p1_;
58 const vector<BIAS::HomgPoint2D> &p2 = *pCurrentObject->
p2_;
67 for (
unsigned int j=0; j<uiDataSize; j++) {
77 FGoldStandardErrorFunction_(
void *p,
int m,
int n,
const double *x,
double *fvec,
int iflag) {
87 for (
int i = 0; i < 3; i++) {
88 for (
int j = 0; j < 4; j++) {
89 P2[i][j] = x[i*4 + j];
94 const unsigned int uiDataSize = pCurrentObject->
p1_->size();
95 const std::vector<BIAS::HomgPoint2D> &p1 = *pCurrentObject->
p1_;
96 const std::vector<BIAS::HomgPoint2D> &p2 = *pCurrentObject->
p2_;
99 vector<HomgPoint3D> points3D;
100 for (
unsigned int i = 0; i < uiDataSize; i++) {
101 points3D.push_back(
HomgPoint3D(x[12 + i*3], x[12 + i*3 + 1], x[12 + i*3 + 2], 1.0));
107 double error1x, error2x, error1y, error2y;
110 for (
unsigned int i = 0; i < uiDataSize; i++) {
112 p1Est = P1 * points3D[i];
114 p2Est = P2 * points3D[i];
117 error1x = (p1Est[0] - p1[i][0]) * (p1Est[0] - p1[i][0]);
118 error1y = (p1Est[1] - p1[i][1]) * (p1Est[1] - p1[i][1]);
119 error2x = (p2Est[0] - p2[i][0]) * (p2Est[0] - p2[i][0]);
120 error2y = (p2Est[1] - p2[i][1]) * (p2Est[1] - p2[i][1]);
122 fvec[i*4 ] = error1x;
123 fvec[i*4+1] = error1y;
124 fvec[i*4+2] = error2x;
125 fvec[i*4+3] = error2y;
131 const std::vector<BIAS::HomgPoint2D> &p1,
132 const std::vector<BIAS::HomgPoint2D> &p2) {
134 if ((p1.size()!=7) || (p2.size()!=7)) {
135 BIASERR(
"Must provide 7 point correspondences for 7-Point algorithm"
136 <<
"p1.size="<<p1.size()<<
"p2.size="<<p2.size());
141 const std::vector<BIAS::HomgPoint2D> *pp1 = &p1;
142 const std::vector<BIAS::HomgPoint2D> *pp2 = &p2;
143 std::vector<BIAS::HomgPoint2D> p1norm;
144 std::vector<BIAS::HomgPoint2D> p2norm;
146 if (NormalizeHartley_) {
148 N_.Hartley(p1, p1norm, K1);
150 N_.Hartley(p2, p2norm, K2);
158 for (
unsigned int i=0; i<7; i++) {
160 A[i][0] = (*pp1)[i][0] * (*pp2)[i][0];
161 A[i][1] = (*pp1)[i][1] * (*pp2)[i][0];
162 A[i][2] = (*pp2)[i][0];
163 A[i][3] = (*pp1)[i][0] * (*pp2)[i][1];
164 A[i][4] = (*pp1)[i][1] * (*pp2)[i][1];
165 A[i][5] = (*pp2)[i][1];
166 A[i][6] = (*pp1)[i][0];
167 A[i][7] = (*pp1)[i][1];
174 BIASERR(
"Error retrieving first F !");
177 BIASERR(
"Error retrieving second F !");
183 std::vector<POLYNOMIALSOLVE_TYPE> AlphaPolynomial, Alphas;
184 GetDetPolynomial(f1, f2, AlphaPolynomial);
189 const unsigned int uiNumberSolutions = Solver.
Analytic(AlphaPolynomial, Alphas);
191 vecF.resize(uiNumberSolutions);
192 if (uiNumberSolutions==0)
196 for (
unsigned int i = 0; i<uiNumberSolutions; i++) {
197 vecF[i].SetFromVector(f1*Alphas[i] + f2*(1.0-Alphas[i]));
198 if (NormalizeHartley_)
206 const std::vector<BIAS::HomgPoint2D> &p2) {
208 const std::vector<BIAS::HomgPoint2D> *pp1 = &p1;
209 const std::vector<BIAS::HomgPoint2D> *pp2 = &p2;
210 std::vector<BIAS::HomgPoint2D> p1norm;
211 std::vector<BIAS::HomgPoint2D> p2norm;
213 if (NormalizeHartley_) {
214 N_.Hartley(p1, p1norm, K1);
215 N_.Hartley(p2, p2norm, K2);
223 for (
unsigned int i=0; i<p1.size(); i++) {
225 A[i][0] = (*pp1)[i][0] * (*pp2)[i][0];
226 A[i][1] = (*pp1)[i][1] * (*pp2)[i][0];
227 A[i][2] = (*pp2)[i][0];
228 A[i][3] = (*pp1)[i][0] * (*pp2)[i][1];
229 A[i][4] = (*pp1)[i][1] * (*pp2)[i][1];
230 A[i][5] = (*pp2)[i][1];
231 A[i][6] = (*pp1)[i][0];
232 A[i][7] = (*pp1)[i][1];
248 for (
unsigned int i=0; i<9; i++)
249 F[i/3][i%3] = VT[8][i];
252 if (NormalizeHartley_)
262 const std::vector<BIAS::HomgPoint2D> &p2) {
264 if (p1.size()!=p2.size()) {
265 BIASERR(
"Vectors of corresponding points have different sizes !!!");
269 BIASERR(
"Too few points to optimize: "<<p1.size());
288 ComputeNormFParam(F, F_normed);
290 vector<double> vecFParamsStart, vecFParamsResult;
294 for (
unsigned int i=0; i<7; i++)
295 FParamsStart[i] = vecFParamsStart[i];
298 p1.size(), 7, FParamsStart, FParamsResult,
299 MINPACK_DEF_TOLERANCE))!=0) {
303 vecFParamsResult.resize(7);
304 for (
unsigned int i=0; i<7; i++)
305 vecFParamsResult[i] = FParamsResult[i];
306 currentPara_->ParamsToFMatrix(F, vecFParamsResult);
314 F =
FMatrix(NormF2_.Transpose() * F * NormF1_);
320 const std::vector<BIAS::HomgPoint2D> &p2) {
322 NormalizeHartley_ =
true;
324 int p1Size = p1.size();
325 BIASASSERT(p1Size == (
int) p2.size());
326 BIASASSERT(p1Size >= 8);
330 std::vector<BIAS::HomgPoint2D> p1_rest;
331 std::vector<BIAS::HomgPoint2D> p2_rest;
333 for (
int i = 0; i < p1Size; i++) {
337 p1_rest.push_back(p1[i]);
338 p2_rest.push_back(p2[i]);
340 p1_rest.push_back(p1[i]);
341 p2_rest.push_back(p2[i]);
356 if (epipole1[1]!=0.0)
358 if (epipole2[1]!=0.0)
364 for (
int i=0; i<3; i++) {
365 for (
int j=0; j<3; j++) {
366 P2[i][j] = mMatrix[i][j];
373 vector<HomgPoint3D> points3D;
376 for (
int i = 0; i < p1Size; i++) {
377 triangulator.
Optimal(P1, P2, p1_rest[i], p2_rest[i], tempPoint3D);
379 points3D.push_back(tempPoint3D);
393 Vector<double> FParamsStart(12 + 3*p1Size), FParamsResult(12 + 3*p1Size);
396 for (
int i = 0; i < 3; i++) {
397 for (
int j = 0; j < 4; j++) {
398 FParamsStart[i*4 + j] = P2[i][j];
402 for (
int i = 0; i < p1Size; i++) {
403 if (!points3D[i].IsHomogenized())
404 points3D[i].Homogenize();
405 FParamsStart[12 + i*3 ] = (points3D[i])[0];
406 FParamsStart[12 + i*3 + 1] = (points3D[i])[1];
407 FParamsStart[12 + i*3 + 2] = (points3D[i])[2];
413 this, 4*p1Size, 12 + 3*p1Size, FParamsStart,
414 FParamsResult, MINPACK_DEF_TOLERANCE))!=0) {
415 BIASERR(
"Error in Levenberg-Marquardt-Optimization: "<<res);
420 for (
int i = 0; i < 3; i++) {
421 for (
int j = 0; j < 4; j++) {
422 P2[i][j]= FParamsResult[i*4 + j];
429 epipole2[0] = FParamsResult[3];
430 epipole2[1] = FParamsResult[7];
431 epipole2[2] = FParamsResult[11];
433 for (
int i=0; i<3; i++) {
434 for (
int j=0; j<3; j++) {
435 mMatrix[i][j] = FParamsResult[i*4 + j];
446 std::vector<POLYNOMIALSOLVE_TYPE> &v) {
449 BIASERR(
"Vector has wrong size "<<F1.
size());
453 BIASERR(
"Vector has wrong size "<<F2.
size());
457 BIASERR(
"Vector v is not empty: "<<v.size());
470 const double d[9] = { (F1[0]-F2[0]), (F1[1]-F2[1]), (F1[2]-F2[2]), (F1[3]-F2[3]), (F1[4]-F2[4]),
471 (F1[5]-F2[5]), (F1[6]-F2[6]), (F1[7]-F2[7]), (F1[8]-F2[8]) };
480 unsigned int x, y, z;
481 for (
unsigned int i=0; i<3; i++) {
486 v[3] += d[x] * d[y] * d[z];
487 v[2] += d[x] * d[y] * F2[z] + d[x] * F2[y] * d[z] + F2[x] * d[y] * d[z];
488 v[1] += d[x] * F2[y] * F2[z] + F2[x] * d[y] * F2[z] + F2[x] * F2[y] * d[z];
489 v[0] += F2[x] * F2[y] * F2[z];
495 v[3] -= d[x] * d[y] * d[z];
496 v[2] -= d[x] * d[y] * F2[z] + d[x] * F2[y] * d[z] + F2[x] * d[y] * d[z];
497 v[1] -= d[x] * F2[y] * F2[z] + F2[x] * d[y] * F2[z] + F2[x] * F2[y] * d[z];
498 v[0] -= F2[x] * F2[y] * F2[z];
545 NormF1_.SetIdentity();
546 NormF2_.SetIdentity();
550 double largest = 0.0;
553 for(
int i = 0; i < 3; i++){
554 for(
int j = 0; j < 3; j++){
555 if(initialF[i][j] > largest){
556 largest = initialF[i][j];
571 row1 = NormF1_.GetRow(0);
572 row2 = NormF1_.GetRow(indexCol);
573 NormF1_.SetRow(0, row2);
574 NormF1_.SetRow(indexCol, row1);
576 ep2 = epi1[indexCol];
577 epi1[indexCol] = ep1;
583 row1 = NormF2_.GetRow(0);
584 row2 = NormF2_.GetRow(indexRow);
585 NormF2_.SetRow(0, row2);
586 NormF2_.SetRow(indexRow, row1);
588 ep2 = epi2[indexRow];
589 epi2[indexRow] = ep1;
594 if(fabs((
float)epi1[1]) > fabs((
float)epi1[2])){
595 row1 = NormF1_.GetRow(1);
596 row2 = NormF1_.GetRow(2);
597 NormF1_.SetRow(1, row2);
598 NormF1_.SetRow(2, row1);
606 if(fabs((
float)epi2[1]) > fabs((
float)epi2[2])){
607 row1 = NormF2_.GetRow(1);
608 row2 = NormF2_.GetRow(2);
609 NormF2_.SetRow(1, row2);
610 NormF2_.SetRow(2, row1);
Vector< double > GetNullvector(const int last_offset=0)
return one of the nullvectors.
class BIASGeometry_EXPORT FMatrix
int Optimize(BIAS::FMatrix &F, const std::vector< BIAS::HomgPoint2D > &p1, const std::vector< BIAS::HomgPoint2D > &p2)
optimize a given FMatrix regarding n 2d point correspondences
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
void CheckCoefficients(std::vector< POLYNOMIALSOLVE_TYPE > &coeff, double eps=DBL_EPSILON)
removes leading zeros in coefficients
std::vector< BIAS::HomgPoint2D > const * p2_
pointer to vector of points in image2 (hack for Optimize)
bool ParamsToFMatrix(BIAS::FMatrix &Mat, const std::vector< double > &Params)
composes an F-matrix from the seven given parameters The deparametrization only works with the previo...
void ComputeNormFParam(const BIAS::FMatrix &initialF, BIAS::FMatrix &normF)
rank 2 F matrix parametrization as implemented in Parametrization does not work in some cases...
This class is used for parametrizing F- and H-matrices, generally for optimization purposes...
HomgPoint2D & Homogenize()
homogenize class data member elements to W==1 by divison by W
std::vector< BIAS::HomgPoint2D > const * p1_
pointer to vector of points in image1 (hack for Optimize)
void Homogenize()
homogenize class data member elements to W==1 by divison by W
void EnforceMaxRank2(BIAS::FMatrix &F)
enforces a maximum rank of two by setting the smallest singular value to zero
int Linear(BIAS::FMatrix &F, const std::vector< BIAS::HomgPoint2D > &p1, const std::vector< BIAS::HomgPoint2D > &p2)
compute an FMatrix from more than seven point correspondences
BIAS::Parametrization * currentPara_
base class for solving polynomial equations
Matrix< T > GetSkewSymmetricMatrix() const
constructs a skew symmetric 3x3 matrix from (*this), which can be used instead of the cross product ...
Class for triangulation of 3Dpoints from 2D matches. Covariance matrix (refering to an uncertainty el...
void SetCol(const int row, const Vector< T > &data)
set a col of matrix from vector
double GetEpipolarErrorHomogenized(const BIAS::HomgPoint2D &P1, const BIAS::HomgPoint2D &P2) const
computes error in epipolar geometry for the given correspondence P1;P2
void SetIdentity()
set the elements of this matrix to the matrix 1 0 0 0 0 1 0 0 0 0 1 0 which is no strict identity (!)...
Matrix3x3< double > NormF2_
matrix for saving normalization of 2D points - important for f parametrization
const Matrix< double > & GetVT() const
return VT (=transposed(V))
class representing a Fundamental matrix
functions for estimating a fundamental matrix (FMatrix) given a set of 2d-2d correspondences (no outl...
long int LevenbergMarquardt(minpack_funcder_mn fun, void *p, long int m, long int n, Vector< double > &InitialGuess, Vector< double > &Result, double Tolerance)
Solves an overdetermined system f(x) = 0 of m non-linear equations with n parameters using the Levenb...
const Vector< double > & GetS() const
return S which is a vector of the singular values of A in descending order.
void GetEpipolesHomogenized(HomgPoint2D &E1, HomgPoint2D &E2) const
same as above, additionally homogenizes epipoles
class HomgPoint3D describes a point with 3 degrees of freedom in projective coordinates.
bool FMatrixToParams(const BIAS::FMatrix &Mat, std::vector< double > &Params)
parametizes an F-matrix to seven parameters
void GetDetPolynomial(const BIAS::Vector< double > &f1, const BIAS::Vector< double > &f2, std::vector< POLYNOMIALSOLVE_TYPE > &v)
set up a polynomial in alpha given Det(alpha*F1+(1-alpha)*F2)=0, where the f vectors are interpreted ...
int Optimal(PMatrix &P1, PMatrix &P2, HomgPoint2D &p1, HomgPoint2D &p2, HomgPoint3D &point3d)
method from Hartley Zisserman chapter 12.5 pp 315 first uses CorrectCorrespondences() and then calls ...
int Analytic(const std::vector< POLYNOMIALSOLVE_TYPE > &coeff, std::vector< POLYNOMIALSOLVE_TYPE > &sol)
solves polynomial of arbitrary order n<=4 analytically coeff[n]x^n+...+coeff[2]x^2+coeff[1]x+coeff[0]...
Matrix3x3< T > Transpose() const
returns transposed matrix tested 12.06.2002
const Matrix< double > & GetU() const
return U U is a m x m orthogonal matrix
int SevenPoint(std::vector< BIAS::FMatrix > &Fvec, const std::vector< BIAS::HomgPoint2D > &p1, const std::vector< BIAS::HomgPoint2D > &p2)
compute a vector of FMatrices from seven point correspondences
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
int GetEpipoles(HomgPoint2D &Epipole1, HomgPoint2D &Epipole2) const
Computes the epipoles from this F Matrix.
describes a projective 3D -> 2D mapping in homogenous coordinates
int GoldStandard(BIAS::FMatrix &F, const std::vector< BIAS::HomgPoint2D > &p1, const std::vector< BIAS::HomgPoint2D > &p2)
Compute FMatrix using the Gold Standard algorithm as in Hartley and Zisserman p.
Vector3< T > & Normalize()
normalize this vector to length 1
Matrix3x3< double > NormF1_
matrix for saving normalization of 2D points - important for f parametrization
T Normalize()
divide this by biggest absolute entry, returns biggest entry
class BIASGeometryBase_EXPORT HomgPoint3D