Basic Image AlgorithmS Library  2.8.0
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
TFTensorEstimation.cpp
1 #include "TFTensorEstimation.hh"
2 #include "TrifocalTensor.hh"
3 #include <Base/Geometry/HomgPoint2D.hh>
4 #include <MathAlgo/SVD.hh>
5 #include <Base/Math/Matrix.hh>
6 #include <iostream>
7 
8 using namespace std;
9 using namespace BIAS;
10 
11 #define USE_FOUR_EQUATIONS_PER_POINT
12 #define TFT_EST_CHECK_EQUATIONS
13 #define TFT_EST_USE_SVD_HACK
14 
15 
16 TFTensorEstimation::TFTensorEstimation()
17 {
18 }
19 
20 
21 TFTensorEstimation::~TFTensorEstimation(){}
22 
23 
24 int TFTensorEstimation::Compute( TrifocalTensor &T,
25  const vector<HomgPoint2D> &x1,
26  const vector<HomgPoint2D> &x2,
27  const vector<HomgPoint2D> &x3,
28  bool HartleyNormalization /* = true */ )
29 {
30  BIASASSERT( x1.size()==x2.size() && x2.size()==x3.size() );
31  if ( x1.size() < 7 )
32  {
33  BIASERR("Need at least 7 correspondences for computation!");
34  return -1;
35  }
36 
37  int n = x1.size();
38  vector<HomgPoint2D> p1, p1norm;
39  vector<HomgPoint2D> p2, p2norm;
40  vector<HomgPoint2D> p3, p3norm;
41  KMatrix K1, K2, K3;
42 
43  // original points shall not be touched
44  p1 = x1;
45  p2 = x2;
46  p3 = x3;
47 
48  if (HartleyNormalization) // normalize points following Hartley-normalization
49  {
50  cout << "Performing Hartley normalization..." << endl;
51  Norm_.Hartley(p1, p1norm, K1);
52  Norm_.Hartley(p2, p2norm, K2);
53  Norm_.Hartley(p3, p3norm, K3);
54  p1 = p1norm;
55  p2 = p2norm;
56  p3 = p3norm;
57  cout << "K1 = " << K1 << endl;
58  cout << "K2 = " << K2 << endl;
59  cout << "K3 = " << K3 << endl;
60  }
61  else
62  {
63  cerr << "Hartley normalization skipped." << endl;
64  }
65 
66 
67 
68 #ifdef USE_FOUR_EQUATIONS_PER_POINT
69  Matrix<TRIFOCALTENSOR_TYPE> A(4*n, 27);
70  for(int corr=0; corr<(int)p1.size(); corr++) {
71  // first equation for i=1 and l=1 ///////////////////////////////////////
72  /* T_1^{11} */ A[corr][0] = p1[corr][0];
73  /* T_1^{12} */ A[corr][1] = 0.0;
74  /* T_1^{13} */ A[corr][2] = - p1[corr][0] * p3[corr][0];
75  /* T_1^{21} */ A[corr][3] = 0.0;
76  /* T_1^{22} */ A[corr][4] = 0.0;
77  /* T_1^{23} */ A[corr][5] = 0.0;
78  /* T_1^{31} */ A[corr][6] = - p1[corr][0] * p2[corr][0];
79  /* T_1^{32} */ A[corr][7] = 0.0;
80  /* T_1^{33} */ A[corr][8] = p1[corr][0] * p2[corr][0] * p3[corr][0];
81 
82  /* T_2^{11} */ A[corr][9] = p1[corr][1];
83  /* T_2^{12} */ A[corr][10] = 0.0;
84  /* T_2^{13} */ A[corr][11] = - p1[corr][1] * p3[corr][0];
85  /* T_2^{21} */ A[corr][12] = 0.0;
86  /* T_2^{22} */ A[corr][13] = 0.0;
87  /* T_2^{23} */ A[corr][14] = 0.0;
88  /* T_2^{31} */ A[corr][15] = - p1[corr][1] * p2[corr][0];
89  /* T_2^{32} */ A[corr][16] = 0.0;
90  /* T_2^{33} */ A[corr][17] = p1[corr][1] * p2[corr][0] * p3[corr][0];
91 
92  /* T_3^{11} */ A[corr][18] = p1[corr][2];
93  /* T_3^{12} */ A[corr][19] = 0.0;
94  /* T_3^{13} */ A[corr][20] = - p1[corr][2] * p3[corr][0];
95  /* T_3^{21} */ A[corr][21] = 0.0;
96  /* T_3^{22} */ A[corr][22] = 0.0;
97  /* T_3^{23} */ A[corr][23] = 0.0;
98  /* T_3^{31} */ A[corr][24] = - p1[corr][2] * p2[corr][0];
99  /* T_3^{32} */ A[corr][25] = 0.0;
100  /* T_3^{33} */ A[corr][26] = p1[corr][2] * p2[corr][0] * p3[corr][0];
101 
102  // second equation for i=1 and l=2 //////////////////////////////////////
103  /* T_1^{11} */ A[corr+n][0] = 0.0;
104  /* T_1^{12} */ A[corr+n][1] = p1[corr][0];
105  /* T_1^{13} */ A[corr+n][2] = - p1[corr][0] * p3[corr][1];
106  /* T_1^{21} */ A[corr+n][3] = 0.0;
107  /* T_1^{22} */ A[corr+n][4] = 0.0;
108  /* T_1^{23} */ A[corr+n][5] = 0.0;
109  /* T_1^{31} */ A[corr+n][6] = 0.0;
110  /* T_1^{32} */ A[corr+n][7] = - p1[corr][0] * p2[corr][0];
111  /* T_1^{33} */ A[corr+n][8] = p1[corr][0] * p2[corr][0] * p3[corr][1];
112 
113  /* T_2^{11} */ A[corr+n][9] = 0.0;
114  /* T_2^{12} */ A[corr+n][10]= p1[corr][1];
115  /* T_2^{13} */ A[corr+n][11]= - p1[corr][1] * p3[corr][1];
116  /* T_2^{21} */ A[corr+n][12]= 0.0;
117  /* T_2^{22} */ A[corr+n][13]= 0.0;
118  /* T_2^{23} */ A[corr+n][14]= 0.0;
119  /* T_2^{31} */ A[corr+n][15]= 0.0;
120  /* T_2^{32} */ A[corr+n][16]= - p1[corr][1] * p2[corr][0];
121  /* T_2^{33} */ A[corr+n][17]= p1[corr][1] * p2[corr][0] * p3[corr][1];
122 
123  /* T_3^{11} */ A[corr+n][18]= 0.0;
124  /* T_3^{12} */ A[corr+n][19]= p1[corr][2];
125  /* T_3^{13} */ A[corr+n][20]= - p1[corr][2] * p3[corr][1];
126  /* T_3^{21} */ A[corr+n][21]= 0.0;
127  /* T_3^{22} */ A[corr+n][22]= 0.0;
128  /* T_3^{23} */ A[corr+n][23]= 0.0;
129  /* T_3^{31} */ A[corr+n][24]= 0.0;
130  /* T_3^{32} */ A[corr+n][25]= - p1[corr][2] * p2[corr][0];
131  /* T_3^{33} */ A[corr+n][26]= p1[corr][2] * p2[corr][0] * p3[corr][1];
132 
133  // third equation for i=2 and l=1 ///////////////////////////////////////
134  /* T_1^{11} */ A[corr+2*n][0] = 0.0;
135  /* T_1^{12} */ A[corr+2*n][1] = 0.0;
136  /* T_1^{13} */ A[corr+2*n][2] = 0.0;
137  /* T_1^{21} */ A[corr+2*n][3] = p1[corr][0];
138  /* T_1^{22} */ A[corr+2*n][4] = 0.0;
139  /* T_1^{23} */ A[corr+2*n][5] = - p1[corr][0] * p3[corr][0];
140  /* T_1^{31} */ A[corr+2*n][6] = - p1[corr][0] * p2[corr][1];
141  /* T_1^{32} */ A[corr+2*n][7] = 0.0;
142  /* T_1^{33} */ A[corr+2*n][8] = p1[corr][0] * p2[corr][1] * p3[corr][0];
143 
144  /* T_2^{11} */ A[corr+2*n][9] = 0.0;
145  /* T_2^{12} */ A[corr+2*n][10]= 0.0;
146  /* T_2^{13} */ A[corr+2*n][11]= 0.0;
147  /* T_2^{21} */ A[corr+2*n][12]= p1[corr][1];
148  /* T_2^{22} */ A[corr+2*n][13]= 0.0;
149  /* T_2^{23} */ A[corr+2*n][14]= - p1[corr][1] * p3[corr][0];
150  /* T_2^{31} */ A[corr+2*n][15]= - p1[corr][1] * p2[corr][1];
151  /* T_2^{32} */ A[corr+2*n][16]= 0.0;
152  /* T_2^{33} */ A[corr+2*n][17]= p1[corr][1] * p2[corr][1] * p3[corr][0];
153 
154  /* T_3^{11} */ A[corr+2*n][18]= 0.0;
155  /* T_3^{12} */ A[corr+2*n][19]= 0.0;
156  /* T_3^{13} */ A[corr+2*n][20]= 0.0;
157  /* T_3^{21} */ A[corr+2*n][21]= p1[corr][2];
158  /* T_3^{22} */ A[corr+2*n][22]= 0.0;
159  /* T_3^{23} */ A[corr+2*n][23]= - p1[corr][2] * p3[corr][0];
160  /* T_3^{31} */ A[corr+2*n][24]= - p1[corr][2] * p2[corr][1];
161  /* T_3^{32} */ A[corr+2*n][25]= 0.0;
162  /* T_3^{33} */ A[corr+2*n][26]= p1[corr][2] * p2[corr][1] * p3[corr][0];
163 
164  // fourth equation for i=2 and l=2 //////////////////////////////////
165  /* T_1^{11} */ A[corr+3*n][0] = 0.0;
166  /* T_1^{12} */ A[corr+3*n][1] = 0.0;
167  /* T_1^{13} */ A[corr+3*n][2] = 0.0;
168  /* T_1^{21} */ A[corr+3*n][3] = 0.0;
169  /* T_1^{22} */ A[corr+3*n][4] = p1[corr][0];
170  /* T_1^{23} */ A[corr+3*n][5] = - p1[corr][0] * p3[corr][1];
171  /* T_1^{31} */ A[corr+3*n][6] = 0.0;
172  /* T_1^{32} */ A[corr+3*n][7] = - p1[corr][0] * p2[corr][1];
173  /* T_1^{33} */ A[corr+3*n][8] = p1[corr][0] * p2[corr][1] * p3[corr][1];
174 
175  /* T_2^{11} */ A[corr+3*n][9] = 0.0;
176  /* T_2^{12} */ A[corr+3*n][10]= 0.0;
177  /* T_2^{13} */ A[corr+3*n][11]= 0.0;
178  /* T_2^{21} */ A[corr+3*n][12]= 0.0;
179  /* T_2^{22} */ A[corr+3*n][13]= p1[corr][1];
180  /* T_2^{23} */ A[corr+3*n][14]= - p1[corr][1] * p3[corr][1];
181  /* T_2^{31} */ A[corr+3*n][15]= 0.0;
182  /* T_2^{32} */ A[corr+3*n][16]= - p1[corr][1] * p2[corr][1];
183  /* T_2^{33} */ A[corr+3*n][17]= p1[corr][1] * p2[corr][1] * p3[corr][1];
184 
185  /* T_3^{11} */ A[corr+3*n][18]= 0.0;
186  /* T_3^{12} */ A[corr+3*n][19]= 0.0;
187  /* T_3^{13} */ A[corr+3*n][20]= 0.0;
188  /* T_3^{21} */ A[corr+3*n][21]= 0.0;
189  /* T_3^{22} */ A[corr+3*n][22]= p1[corr][2];
190  /* T_3^{23} */ A[corr+3*n][23]= - p1[corr][2] * p3[corr][1];
191  /* T_3^{31} */ A[corr+3*n][24]= 0.0;
192  /* T_3^{32} */ A[corr+3*n][25]= - p1[corr][2] * p2[corr][1];
193  /* T_3^{33} */ A[corr+3*n][26]= p1[corr][2] * p2[corr][1] * p3[corr][1];
194  }
195 #else
196  Matrix<TRIFOCALTENSOR_TYPE> A(p1.size(), 27);
197  for (int i=0; i<(int)p1.size(); i++)
198  {
199  // T_1^{11}
200  A[i][0] = p1[i][0];
201  // T_1^{12}
202  A[i][1] = p1[i][0];
203  // T_1^{13}
204  A[i][2] = -p1[i][0] * p3[i][0] - p1[i][0] * p3[i][1];
205  // T_1^{21}
206  A[i][3] = p1[i][0];
207  // T_1^{22}
208  A[i][4] = p1[i][0];
209  // T_1^{23}
210  A[i][5] = -p1[i][0] * p3[i][0] - p1[i][0] * p3[i][1];
211  // T_1^{31}
212  A[i][6] = -p1[i][0] * p2[i][0] - p1[i][0] * p2[i][1];
213  // T_1^{32}
214  A[i][7] = -p1[i][0] * p2[i][0] - p1[i][0] * p2[i][1];
215  // T_1^{33}
216  A[i][8] =
217  p1[i][0] * p2[i][0] * p3[i][0] +
218  p1[i][0] * p2[i][0] * p3[i][1] +
219  p1[i][0] * p2[i][1] * p3[i][0] +
220  p1[i][0] * p2[i][1] * p3[i][1] ;
221  // T_2^{11}
222  A[i][9] = p1[i][1];
223  // T_2^{12}
224  A[i][10] = p1[i][1];
225  // T_2^{13}
226  A[i][11] = -p1[i][1] * p3[i][0] - p1[i][1] * p3[i][1];
227  // T_2^{21}
228  A[i][12] = p1[i][1];
229  // T_2^{22}
230  A[i][13] = p1[i][1];
231  // T_2^{23}
232  A[i][14] = -p1[i][1] * p3[i][0] - p1[i][1] * p3[i][1];
233  // T_2^{31}
234  A[i][15] = -p1[i][1] * p2[i][0] - p1[i][1] * p2[i][1];
235  // T_2^{32}
236  A[i][16] = -p1[i][1] * p2[i][0] - p1[i][1] * p2[i][1];
237  // T_2^{33}
238  A[i][17] =
239  p1[i][1] * p2[i][0] * p3[i][0] +
240  p1[i][1] * p2[i][0] * p3[i][1] +
241  p1[i][1] * p2[i][1] * p3[i][0] +
242  p1[i][1] * p2[i][1] * p3[i][1] ;
243  // T_3^{11}
244  A[i][18] = p1[i][2];
245  // T_3^{12}
246  A[i][19] = p1[i][2];
247  // T_3^{13}
248  A[i][20] = -p1[i][2] * p3[i][0] - p1[i][2] * p3[i][1];
249  // T_3^{21}
250  A[i][21] = p1[i][2];
251  // T_3^{22}
252  A[i][22] = p1[i][2];
253  // T_3^{23}
254  A[i][23] = -p1[i][2] * p3[i][0] - p1[i][2] * p3[i][1];
255  // T_3^{31}
256  A[i][24] = -p1[i][2] * p2[i][0] - p1[i][2] * p2[i][1];
257  // T_3^{32}
258  A[i][25] = -p1[i][2] * p2[i][0] - p1[i][2] * p2[i][1];
259  // T_3^{33}
260  A[i][26] =
261  p1[i][2] * p2[i][0] * p3[i][0] +
262  p1[i][2] * p2[i][0] * p3[i][1] +
263  p1[i][2] * p2[i][1] * p3[i][0] +
264  p1[i][2] * p2[i][1] * p3[i][1] ;
265  }
266 #endif
267 
268 #ifdef TFT_EST_CHECK_EQUATIONS
269  cout << "A = " << A << endl;
270 #endif
271 
272  SVD svda(A);
273  cout << "LeftNullspaceDim = " << svda.LeftNullspaceDim() << endl;
274  cout << "RightNullspaceDim = " << svda.RightNullspaceDim() << endl;
275 
276 #ifdef TFT_EST_USE_SVD_HACK
277  Matrix<double> VT = svda.GetVT();
278  Vector<TRIFOCALTENSOR_TYPE> Tvec(VT.num_cols());
279 
280  for (int col=0; col < VT.num_cols(); col++)
281  {
282  Tvec[col] = VT[ VT.num_cols() - 1 ][col];
283  }
284 #else
285  Vector<TRIFOCALTENSOR_TYPE> Tvec = svda.GetNullvector();
286 #endif
287 
288  cout << "Tvec = " << Tvec << endl;
289 
290 #ifdef TFT_EST_CHECK_EQUATIONS
291  cout << A * Tvec << endl;
292 #endif
293 
294  T.SetFromVector(Tvec);
295 
296 #ifdef TFT_EST_CHECK_EQUATIONS
297  double result;
298  for (unsigned int i=0; i<p1.size(); i++)
299  {
300  cout << "\nChecking correspondence " << i << endl;
301  result =
302  p1[i][0] * p2[i][0] * p3[i][0] * T(0,2,2)
303  - p1[i][0] * p3[i][0] * T(0,0,2)
304  - p1[i][0] * p2[i][0] * T(0,2,0)
305  + p1[i][0] * T(0,0,0);
306  result +=
307  p1[i][1] * p2[i][0] * p3[i][0] * T(1,2,2)
308  - p1[i][1] * p3[i][0] * T(1,0,2)
309  - p1[i][1] * p2[i][0] * T(1,2,0)
310  + p1[i][1] * T(1,0,0);
311  result +=
312  p1[i][2] * p2[i][0] * p3[i][0] * T(2,2,2)
313  - p1[i][2] * p3[i][0] * T(2,0,2)
314  - p1[i][2] * p2[i][0] * T(2,2,0)
315  + p1[i][2] * T(2,0,0);
316  cout <<" result 1 is "<<result<<endl;
317 
318  result =
319  p1[i][0] * p2[i][0] * p3[i][1] * T(0,2,2)
320  - p1[i][0] * p3[i][1] * T(0,0,2)
321  - p1[i][0] * p2[i][0] * T(0,2,1)
322  + p1[i][0] * T(0,0,1);
323  result +=
324  p1[i][1] * p2[i][0] * p3[i][1] * T(1,2,2)
325  - p1[i][1] * p3[i][1] * T(1,0,2)
326  - p1[i][1] * p2[i][0] * T(1,2,1)
327  + p1[i][1] * T(1,0,1);
328  result +=
329  p1[i][2] * p2[i][0] * p3[i][1] * T(2,2,2)
330  - p1[i][2] * p3[i][1] * T(2,0,2)
331  - p1[i][2] * p2[i][0] * T(2,2,1)
332  + p1[i][2] * T(2,0,1);
333  cout <<" result 2 is "<<result<<endl;
334 
335  result =
336  p1[i][0] * p2[i][1] * p3[i][0] * T(0,2,2)
337  - p1[i][0] * p3[i][0] * T(0,1,2)
338  - p1[i][0] * p2[i][1] * T(0,2,0)
339  + p1[i][0] * T(0,1,0);
340  result +=
341  p1[i][1] * p2[i][1] * p3[i][0] * T(1,2,2)
342  - p1[i][1] * p3[i][0] * T(1,1,2)
343  - p1[i][1] * p2[i][1] * T(1,2,0)
344  + p1[i][1] * T(1,1,0);
345  result +=
346  p1[i][2] * p2[i][1] * p3[i][0] * T(2,2,2)
347  - p1[i][2] * p3[i][0] * T(2,1,2)
348  - p1[i][2] * p2[i][1] * T(2,2,0)
349  + p1[i][2] * T(2,1,0);
350  cout <<" result 3 is "<<result<<endl;
351 
352  result =
353  p1[i][0] * p2[i][1] * p3[i][1] * T(0,2,2)
354  - p1[i][0] * p3[i][1] * T(0,1,2)
355  - p1[i][0] * p2[i][1] * T(0,2,1)
356  + p1[i][0] * T(0,1,1);
357  result +=
358  p1[i][1] * p2[i][1] * p3[i][1] * T(1,2,2)
359  - p1[i][1] * p3[i][1] * T(1,1,2)
360  - p1[i][1] * p2[i][1] * T(1,2,1)
361  + p1[i][1] * T(1,1,1);
362  result +=
363  p1[i][2] * p2[i][1] * p3[i][1] * T(2,2,2)
364  - p1[i][2] * p3[i][1] * T(2,1,2)
365  - p1[i][2] * p2[i][1] * T(2,2,1)
366  + p1[i][2] * T(2,1,1);
367  cout <<" result 4 is "<<result<<endl;
368  }
369 #endif
370 
371  TrifocalTensor T2;
372  AlgebraicMinimization(T,T2);
373 
374  if (HartleyNormalization)
375  {
376  T2.premultiply3(K3.Invert(),T);
377  T. premultiply2(K2.Invert(),T2);
378  T2.postmultiply1(K1,T);
379  }
380  else
381  {
382  T = T2;
383  }
384 
385  return 0;
386 }
387 
388 /* The algebraic minimization is described on page 384 of Hartley/Zisserman's
389  book Multiple View Geometry in Computer Vision */
390 int TFTensorEstimation::AlgebraicMinimization(TrifocalTensor &Initial,
391  TrifocalTensor &Result)
392 {
393  cerr << "The algebraic minimization is not fully implemented yet.\n"
394  << "So the result is the initial tensor!" << endl;
395 
396  Result = Initial;
397  return -1;
398  ////////////////////////////////////////////////////////////////////
399 /*
400  cout << "Starting algebraic minimization..." << endl;
401  HomgPoint2D e2 = Initial.GetEpipole21(true);
402  HomgPoint2D e3 = Initial.GetEpipole31();
403 
404  cout << "Epipole in second image is " << e2 << endl;
405  cout << "Epipole in third image is " << e3 << endl;
406 
407  Matrix<TRIFOCALTENSOR_TYPE> E(27,18);
408  E.SetZero();
409 
410  // build matrix E with t=Ea
411 
412  for( int i=0; i<3; i++)
413  for( int j=0; j<3; j++)
414  for( int k=0; k<3; k++)
415  {
416  E[9*i+3*j+k][3*i+j] = e3[k];
417  E[9*i+3*j+k][9+3*i+k] = -e2[j];
418  }
419 
420  cout << "E = " << E << endl;
421 
422 
423  // hier fehlt noch was...
424 
425 
426  cout << "Algebraic minimization finished." << endl;
427  return 0;
428  */
429 }
430 
431 
432 
BIAS::SVD::GetNullvector
Vector< double > GetNullvector(const int last_offset=0)
return one of the nullvectors.
Definition: SVD.hh:404
BIAS::SVD
computes and holds the singular value decomposition of a rectangular (not necessarily quadratic) Matr...
Definition: SVD.hh:92
TNT::Matrix::num_cols
Subscript num_cols() const
Definition: cmat.h:320
BIAS::Vector
class for column vectors with arbitrary size
Definition: GenerateGauss.hh:30
BIAS::Tensor3D3x3x3::postmultiply1
void postmultiply1(const BIAS::Matrix3x3< T > &M, Tensor3D3x3x3< T > &result) const
Compute S_{ijk} = T_{pjk} M_{pi}.
Definition: Tensor3D3x3x3.cpp:66
BIAS::Tensor3D::SetFromVector
void SetFromVector(const BIAS::Vector< T > &v)
set the tensor by the rows of the vector.
Definition: Tensor3D.cpp:165
BIAS::Tensor3D3x3x3::premultiply3
void premultiply3(const BIAS::Matrix3x3< T > &M, Tensor3D3x3x3< T > &result) const
compute S_{ijk} = M_{kp} T_{ijp}
Definition: Tensor3D3x3x3.cpp:146
BIAS::SVD::RightNullspaceDim
const int RightNullspaceDim() const
returns the dim of the matrix&#39;s right nullspace
Definition: SVD.hh:216
BIAS::SVD::GetVT
const Matrix< double > & GetVT() const
return VT (=transposed(V))
Definition: SVD.hh:177
BIAS::SVD::LeftNullspaceDim
const int LeftNullspaceDim() const
returns the dim of the matrix&#39;s left nullspace
Definition: SVD.hh:223
BIAS::Matrix
matrix class with arbitrary size, indexing is row major.
Definition: GenerateGauss.hh:31
BIAS::KMatrix
K describes the mapping from world coordinates (wcs) to pixel coordinates (pcs).
Definition: KMatrix.hh:48
BIAS::KMatrix::Invert
KMatrix Invert() const
returns analyticaly inverted matrix
Definition: KMatrix.cpp:31
Generated on Tue Feb 6 2018 02:01:37 for Basic Image AlgorithmS Library by   doxygen 1.8.5