Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
ExampleFMatrix2.cpp
1 /*
2  This file is part of the BIAS library (Basic ImageAlgorithmS).
3 
4  Copyright (C) 2003-2009 (see file CONTACT for details)
5  Multimediale Systeme der Informationsverarbeitung
6  Institut fuer Informatik
7  Christian-Albrechts-Universitaet Kiel
8 
9 
10  BIAS is free software; you can redistribute it and/or modify
11  it under the terms of the GNU Lesser General Public License as published by
12  the Free Software Foundation; either version 2.1 of the License, or
13  (at your option) any later version.
14 
15  BIAS is distributed in the hope that it will be useful,
16  but WITHOUT ANY WARRANTY; without even the implied warranty of
17  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  GNU Lesser General Public License for more details.
19 
20  You should have received a copy of the GNU Lesser General Public License
21  along with BIAS; if not, write to the Free Software
22  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23  */
24 
25 /**
26  @example ExampleFMatrix2
27  @relates FMatrix
28  @brief Example computing F matrix with 8-point, non-linear optimize, and gold standard
29  @ingroup g_examples
30  @author MIP
31 */
32 
33 #include "../FMatrix.hh"
34 #include "../FMatrixEstimation.hh"
35 #include "../PMatrixEstimation.hh"
36 #include "../PMatrixLinear.hh"
37 #include "../Triangulation.hh"
38 #include "../../Base/Math/Random.hh"
39 
40 using namespace BIAS;
41 using namespace std;
42 
43 // ---------------------------------------------------------------------------
44 // testing eight point and Gold Standard algorithm for F matrix estimation
45 
46 int main() {
47  BIAS::FMatrix FEightPoint, FOptimize, FGoldSt, TheoF;
48 
49  BIAS::PMatrix P1, P2;
50  BIAS::Random Randomizer;
51 
52  BIAS::RMatrix R1, R2;
54  BIAS::Vector3<double> C1, C2;
55 
56  // set rotation matrices
57  R1.SetIdentity();
58  //R2.SetXYZ(20.0*M_PI/180.0, 10.0*M_PI/180.0, -60*M_PI/180.0);
59  R2.SetXYZ(0.0, 5.0*M_PI/180.0, 0.0);
60  // camera center coordiantes
61  C1[0] = 0.0;
62  C1[1] = 0.0;
63  C1[2] = 0.0;
64 
65  C2[0] = -1.0/sqrt(2.0);
66  C2[1] = 1.0/sqrt(2.0);
67  // C2[0] = -1.0;
68  // C2[1] = 3.0;
69  C2[2] = 0.0;
70 
71  // // camera matrix for both cameras
72  K.SetFx(400.0);
73  K.SetFy(400.0);
74  // in image coordinates
75  K.SetHx(100.0);
76  K.SetHy(100.0);
77  K.SetSkew(0.0);
78 
79  K[2][2] = 1;
80 
81  // combine camera matrix, rotation matrix and camera center to projection matrices
82  P1.Compose(K, R1, C1);
83  P2.Compose(K, R2, C2);
84 
87 
88  // compute true F matrix
89  TheoF.ComputeFromPMatrices(P1, P2);
90 
91  vector<HomgPoint3D> points;
92  vector<HomgPoint2D> p1, p2;
93 
94  // noise is added to point correspondences
95  vector<HomgPoint2D> p1Noisy, p2Noisy;
96  double noiseB = 10;
97  double testDist1, testDist2, testDist3, testDist4;
98 
99  unsigned int numPoints = 80;
100  cout << "Randomized correspondences are "<<endl;
101  for (unsigned int p=0; p<numPoints; p++) {
102  cout << "number " << p << endl;
103  points.push_back(HomgPoint3D(Randomizer.GetUniformDistributed(-1.0, 1.0),
104  Randomizer.GetUniformDistributed(-1.0, 1.0), Randomizer.GetUniformDistributed(-1.0, 1.0)));
105  p1.push_back(P1 * points[p]);
106  p1[p].Homogenize();
107  cout << "{ "<<p1[p]<<" ";
108  p2.push_back(P2 * points[p]);
109  p2[p].Homogenize();
110  cout << p2[p]<<" } "<<endl;
111 
112 // if (p < 8) {
113  testDist1 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
114  testDist2 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
115  testDist3 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
116  testDist4 = Randomizer.GetUniformDistributed(-noiseB, noiseB);
117  p1Noisy.push_back(HomgPoint2D((p1[p])[0] + testDist1, (p1[p])[1] + testDist2));
118  p2Noisy.push_back(HomgPoint2D((p2[p])[0] + testDist3, (p2[p])[1] + testDist4));
119 // } else {
120 // p1Noisy.push_back(p1[p]);
121 // p2Noisy.push_back(p2[p]);
122 // }
123  cout << "{ " << p1Noisy[p] << " " << p2Noisy[p] << " }" << endl;
124  }
125 
126  bool NormalizeHartley = true;
127  FMatrixEstimation FEstimator(NormalizeHartley);
128 
129  // vars for epipole computation
130  HomgPoint2D e8Point1, e8Point2, theoe1, theoe2, eGoldSt1, eGoldSt2, eOpt1, eOpt2;
131  double residualError8Point, residualErrorGoldStandard, residualErrorTest;
132  double residualError8Point_n, residualErrorGoldStandard_n, residualErrorTest_n;
133  double residualErrorOpt_n, residualErrorOpt;
134 
135  residualErrorTest_n = TheoF.GetResidualError(p1Noisy, p2Noisy);
136  residualErrorTest = TheoF.GetResidualError(p1, p2);
137 
138  // use eight point algorithm and the optimize function applying the
139  // levenberg marquardt algorithm to optimize the parameters of the Fmatrix
140  // using the sum of squared distances to the epipolar line as an error function
141  FEightPoint.SetIdentity();
142  FEstimator.Linear(FEightPoint, p1Noisy, p2Noisy);
143  residualError8Point_n = FEightPoint.GetResidualError(p1Noisy, p2Noisy);
144  residualError8Point = FEightPoint.GetResidualError(p1, p2);
145  FEightPoint.GetEpipolesHomogenized(e8Point1, e8Point2);
146  e8Point1.Normalize();
147  e8Point2.Normalize();
148 
149  // use optimize on 8Point result
150  FOptimize = FEightPoint;
151  FEstimator.Optimize(FOptimize, p1Noisy, p2Noisy);
152  residualErrorOpt_n = FOptimize.GetResidualError(p1Noisy, p2Noisy);
153  residualErrorOpt = FOptimize.GetResidualError(p1, p2);
154  FOptimize.GetEpipolesHomogenized(eOpt1, eOpt2);
155  if(!eOpt1.IsZero()) eOpt1.Normalize();
156  if(!eOpt2.IsZero())eOpt2.Normalize();
157 
158  // use Gold Standard algorithm
159  FGoldSt.SetIdentity();
160  FEstimator.GoldStandard(FGoldSt, p1Noisy, p2Noisy);
161  residualErrorGoldStandard_n = FGoldSt.GetResidualError(p1Noisy, p2Noisy);
162  residualErrorGoldStandard = FGoldSt.GetResidualError(p1, p2);
163  FGoldSt.GetEpipolesHomogenized(eGoldSt1, eGoldSt2);
164  if(!eGoldSt1.IsZero()) eGoldSt1.Normalize();
165  if(!eGoldSt2.IsZero()) eGoldSt2.Normalize();
166 
167  // cout results
168  cout << "True F matrix " << 1/TheoF[2][2] * TheoF << endl;
169  cout << "Residual Error " << residualErrorTest << " noisy " << residualErrorTest_n << endl;
170  TheoF.GetEpipolesHomogenized(theoe1, theoe2);
171  theoe1.Normalize();
172  theoe2.Normalize();
173  cout << "True epipoles are "<<theoe1<<" and "<<theoe2<<endl;
174  cout << endl;
175 
176  cout << "Eight point algorithm " << endl;
177  cout << "Fmatrix " << 1/FEightPoint[2][2] * FEightPoint << endl;
178  cout << "Residual Error " << residualError8Point << " noisy " << residualError8Point_n << endl;
179  cout << "est. epipoles are "<<e8Point1<<" and "<<e8Point2<<endl;
180 
181  cout << endl;
182 
183  cout << "Optimize algorithm " << endl;
184  cout << "Fmatrix " << 1/FOptimize[2][2] * FOptimize << endl;
185  cout << "Residual Error " << residualErrorOpt << " noisy " << residualErrorOpt_n << endl;
186  cout << "est. epipoles are "<<eOpt1<<" and "<<eOpt2<<endl;
187 
188  cout << endl;
189 
190 
191  cout << "Gold Standard algorithm" << endl;
192  cout << "Fmatrix " << 1/FGoldSt[2][2] * FGoldSt << endl;
193  cout << "Residual Error " << residualErrorGoldStandard << " noisy " << residualErrorGoldStandard_n << endl;
194  cout << "est. epipoles are "<<eGoldSt1<<" and "<<eGoldSt2<<endl;
195 
196  return 0;
197 }
class HomgPoint2D describes a point with 2 degrees of freedom in projective coordinates.
Definition: HomgPoint2D.hh:67
void SetXYZ(ROTATION_MATRIX_TYPE PhiX, ROTATION_MATRIX_TYPE PhiY, ROTATION_MATRIX_TYPE PhiZ)
Set Euler angles (in rad) in order XYZ.
void SetSkew(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:81
double GetUniformDistributed(const double min, const double max)
on succesive calls return uniform distributed random variable between min and max ...
Definition: Random.hh:84
3D rotation matrix
Definition: RMatrix.hh:49
double GetResidualError(const std::vector< BIAS::HomgPoint2D > &p1, const std::vector< BIAS::HomgPoint2D > &p2)
returns residual error as in Zisserman p.
Definition: FMatrix.cpp:148
bool IsZero() const
Definition: Vector3.hh:561
class representing a Fundamental matrix
Definition: FMatrix.hh:46
void InvalidateDecomposition()
to re-Decompose_() after filling with data use this.
Definition: PMatrix.hh:499
functions for estimating a fundamental matrix (FMatrix) given a set of 2d-2d correspondences (no outl...
void ComputeFromPMatrices(BIAS::PMatrix &P1, BIAS::PMatrix &P2)
computes an F matrix from two cameras (defined by arbitrary P matrices, not only metric ones) ...
Definition: FMatrix.cpp:38
void GetEpipolesHomogenized(HomgPoint2D &E1, HomgPoint2D &E2) const
same as above, additionally homogenizes epipoles
Definition: FMatrix.hh:83
void SetFx(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:75
void SetHy(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:87
void SetFy(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:78
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Definition: KMatrix.hh:48
describes a projective 3D -&gt; 2D mapping in homogenous coordinates
Definition: PMatrix.hh:88
void SetHx(const KMATRIX_TYPE &val)
Definition: KMatrix.cpp:84
Vector3< T > & Normalize()
normalize this vector to length 1
Definition: Vector3.hh:653
class for producing random numbers from different distributions
Definition: Random.hh:51
class BIASGeometryBase_EXPORT HomgPoint2D
void Compose(const Matrix3x3< double > &K, const Matrix3x3< double > &R, const Vector3< double > &C)
composes this from K, R and C using P = [ K R&#39; | -K R&#39; C ] with R&#39; = transpose(R) ...
Definition: PMatrix.cpp:581
class BIASGeometryBase_EXPORT HomgPoint3D
void SetIdentity()
set the elements of this matrix to the identity matrix (possibly overriding the inherited method) ...
Definition: Matrix3x3.hh:429