SLIP  1.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
camera_algo.hpp
Go to the documentation of this file.
1 /*
2  * Copyright(c):
3  * Signal Image and Communications (SIC) Department
4  * http://www.sic.sp2mi.univ-poitiers.fr/
5  * - University of Poitiers, France http://www.univ-poitiers.fr
6  * - XLIM Institute UMR CNRS 7252 http://www.xlim.fr/
7  *
8  * and
9  *
10  * D2 Fluid, Thermic and Combustion
11  * - University of Poitiers, France http://www.univ-poitiers.fr
12  * - PPRIME Institute - UPR CNRS 3346 http://www.pprime.fr
13  * - ISAE-ENSMA http://www.ensma.fr
14  *
15  * Contributor(s):
16  * The SLIP team,
17  * Benoit Tremblais <tremblais_AT_sic.univ-poitiers.fr>,
18  * Laurent David <laurent.david_AT_lea.univ-poitiers.fr>,
19  * Ludovic Chatellier <ludovic.chatellier_AT_univ-poitiers.fr>,
20  * Lionel Thomas <lionel.thomas_AT_univ-poitiers.fr>,
21  * Denis Arrivault <arrivault_AT_sic.univ-poitiers.fr>,
22  * Julien Dombre <julien.dombre_AT_univ-poitiers.fr>.
23  *
24  * Description:
25  * The Simple Library of Image Processing (SLIP) is a new image processing
26  * library. It is written in the C++ language following as much as possible
27  * the ISO/ANSI C++ standard. It is consequently compatible with any system
28  * satisfying the ANSI C++ complience. It works on different Unix , Linux ,
29  * Mircrosoft Windows and Mac OS X plateforms. SLIP is a research library that
30  * was created by the Signal, Image and Communications (SIC) departement of
31  * the XLIM, UMR 7252 CNRS Institute in collaboration with the Fluids, Thermic
32  * and Combustion departement of the P', UPR 3346 CNRS Institute of the
33  * University of Poitiers.
34  *
35  * The SLIP Library source code has been registered to the APP (French Agency
36  * for the Protection of Programs) by the University of Poitiers and CNRS,
37  * under registration number IDDN.FR.001.300034.000.S.P.2010.000.21000.
38 
39  * http://www.sic.sp2mi.univ-poitiers.fr/slip/
40  *
41  * This software is governed by the CeCILL-C license under French law and
42  * abiding by the rules of distribution of free software. You can use,
43  * modify and/ or redistribute the software under the terms of the CeCILL-C
44  * license as circulated by CEA, CNRS and INRIA at the following URL
45  * http://www.cecill.info.
46  * As a counterpart to the access to the source code and rights to copy,
47  * modify and redistribute granted by the license, users are provided only
48  * with a limited warranty and the software's author, the holder of the
49  * economic rights, and the successive licensors have only limited
50  * liability.
51  *
52  * In this respect, the user's attention is drawn to the risks associated
53  * with loading, using, modifying and/or developing or reproducing the
54  * software by the user in light of its specific status of free software,
55  * that may mean that it is complicated to manipulate, and that also
56  * therefore means that it is reserved for developers and experienced
57  * professionals having in-depth computer knowledge. Users are therefore
58  * encouraged to load and test the software's suitability as regards their
59  * requirements in conditions enabling the security of their systems and/or
60  * data to be ensured and, more generally, to use and operate it in the
61  * same conditions as regards security.
62  *
63  * The fact that you are presently reading this means that you have had
64  * knowledge of the CeCILL-C license and that you accept its terms.
65  */
66 
67 
68 
76 #ifndef SLIP_CAMERA_ALGO_HPP
77 #define SLIP_CAMERA_ALGO_HPP
78 
79 #include "Point2d.hpp"
80 #include "Point3d.hpp"
81 #include "Vector3d.hpp"
82 #include "Vector.hpp"
83 #include "Matrix.hpp"
84 #include "io_tools.hpp"
85 #include "linear_algebra_eigen.hpp"
86 #include "linear_algebra_qr.hpp"
87 #include "norms.hpp"
88 #include "macros.hpp"
89 #include "MultivariatePolynomial.hpp"
90 namespace slip
91 {
92  enum DECOMP_TYPE
93  {
94  RQ,
95  direct
96  };
97 }
98 
99 namespace slip
100 {
101 
131 template <class Matrix1, class Matrix2, class Matrix3>
132  inline
133  int rq_decomp(const Matrix1 & A, Matrix2 & R, Matrix3 & Q)
134  {
135  assert(A.dim1()==A.dim2());
136  assert(A.dim1()==R.dim1());
137  assert(A.dim2()==R.dim2());
138  assert(A.dim1()==Q.dim1());
139  assert(A.dim2()==Q.dim2());
140  typedef typename Matrix1::value_type Type;
141  const std::size_t n = A.dim1();
142 
143  slip::Matrix<Type> P(n,n);
144  for(std::size_t i = 0; i < n; ++i)
145  {
146  P[i][n-1-i] = static_cast<Type>(1.0);
147  }
148  slip::Matrix<Type> At(n,n);
149  slip::transpose(A,At);
150  slip::Matrix<Type> AtP(n,n);
151  slip::multiplies(At,P,AtP);
152  slip::Matrix<Type> Q2(n,n);
153  slip::Matrix<Type> R2(n,n);
154  slip::householder_qr(AtP,Q2,R2);
155  //slip::qr_decomp(AtP,Q2,R2);
156  slip::Matrix<Type> H(n,n);
157  slip::multiplies(Q2,P,H);
158  slip::transpose(H,Q);
159  slip::multiplies(R2,P,H);
160  slip::multiplies(P,H,R2);
161  slip::transpose(R2,R);
162 
163  return 0;
164  }
165 
186  template <typename Type>
187  void getpars_DLT(const slip::Matrix<Type>& P,
188  slip::Matrix<Type>& Mat)
189  {
190  const std::size_t P_rows = P.rows();
191  Type zero = static_cast<Type>(0.0);
192  const std::size_t A_rows = 2*P_rows;
193  const std::size_t A_cols = 11;
194  slip::Matrix<Type> A(A_rows,A_cols,zero);
195  slip::Vector<Type> Y(A_rows);
196  // const Type m34 = static_cast<Type>(1.0);
197 
198  for(std::size_t i = 0; i < P_rows; ++i)
199  {
200  std::size_t k = 2*i;
201  A[k][0]=P[i][2]; //X_i
202  A[k][1]=P[i][3]; //Y_i
203  A[k][2]=P[i][4]; //Z_i
204  A[k][3] = slip::constants<Type>::one();
205  A[k][8]=-P[i][0]*P[i][2]; //-x_i*X_i
206  A[k][9]=-P[i][0]*P[i][3]; //-x_i*Y_i
207  A[k][10]=-P[i][0]*P[i][4]; //-x_i*Z_i
208  A[k+1][4]=P[i][2]; //X_i
209  A[k+1][5]=P[i][3]; //Y_i
210  A[k+1][6]=P[i][4]; //Z_i
211  A[k+1][7]=slip::constants<Type>::one();
212  A[k+1][8]=-P[i][1]*P[i][2]; //-y_i*X_i
213  A[k+1][9]=-P[i][1]*P[i][3]; //-y_i*Y_i
214  A[k+1][10]=-P[i][1]*P[i][4]; //-y_i*Z_i
215 
216  Y[k]=P[i][0];//*m34; //x_i
217  Y[k+1]=P[i][1];//*m34; //y_i
218  }
219 
220  //compute pseudo-inverse
221  slip::Matrix<Type> ATA(A_cols,A_cols);
222  slip::hmatrix_matrix_multiplies(A,A,ATA);
223  slip::Matrix<Type> ATAm1(A_cols,A_cols);
224  slip::inverse(ATA,ATAm1);
225  slip::Matrix<Type> ATAm1AT(A_cols,A_rows);
226  for(std::size_t i = 0; i < A_cols; ++i)
227  {
228  for(std::size_t j = 0; j < A_rows; ++j)
229  {
230  ATAm1AT[i][j] = std::inner_product(ATAm1.row_begin(i),ATAm1.row_end(i),
231  A.row_begin(j),
232  Type());
233  }
234  }
235  slip::matrix_vector_multiplies(ATAm1AT.upper_left(),
236  ATAm1AT.bottom_right(),
237  Y.begin(),Y.end(),
238  Mat.begin(),Mat.begin()+A_cols);
239 
240 
241  Mat[2][3] = static_cast<Type>(1.0);
242  }
243 
261  template <typename Type>
262  void getpars_DLT_norm(const slip::Matrix<Type>& P,
263  slip::Matrix<Type>& Mat)
264  {
265  slip::getpars_DLT(P,Mat);
266  Type norm = std::sqrt( Mat[2][0]*Mat[2][0]
267  + Mat[2][1]*Mat[2][1]
268  + Mat[2][2]*Mat[2][2]);
269  slip::matrix_scalar_multiplies(Mat,
270  static_cast<Type>(-1.0)/norm,
271  Mat);
272  }
273 
274 
296  template <typename Type>
297  void getpars_Faugeras(const slip::Matrix<Type>& P,
298  slip::Matrix<Type>& Mat)
299  {
300  const std::size_t P_rows = P.rows();
301  Matrix<Type> B(2*P_rows,9,static_cast<Type>(0.0));
302  Matrix<Type> C(2*P_rows,3);
303 
304 
305  for(std::size_t i = 0; i < P_rows; ++i)
306  {
307  std::size_t k=2*i;
308 
309  B[k][0]=P[i][2]; //X_i
310  B[k][1]=P[i][3]; //Y_i
311  B[k][2]=P[i][4]; //Z_i
312  B[k][3]=static_cast<Type>(1.0);
313  B[k][8]=-P[i][0]; //-x_i
314  B[k+1][4]=P[i][2]; //X_i
315  B[k+1][5]=P[i][3]; //Y_i
316  B[k+1][6]=P[i][4]; //Z_i
317  B[k+1][7]=static_cast<Type>(1.0);
318  B[k+1][8]=-P[i][1]; //-y_i
319 
320  C[k][0]=-P[i][0]*P[i][2]; //-x_i*X_i
321  C[k][1]=-P[i][0]*P[i][3]; //-x_i*Y_i
322  C[k][2]=-P[i][0]*P[i][4]; //-x_i*Z_i
323 
324  C[k+1][0]=-P[i][1]*P[i][2]; //-y_i*X_i
325  C[k+1][1]=-P[i][1]*P[i][3]; //-y_i*Y_i
326  C[k+1][2]=-P[i][1]*P[i][4]; //-y_i*Z_i
327  }
328 
329 
330 
331  Matrix<Type> Ct(3,2*P_rows);
332  slip::transpose(C,Ct);
333 
334  Matrix<Type> Bt(9,2*P_rows);
335  slip::transpose(B,Bt);
336 
337  Matrix<Type> CtC(3,3);
338  slip::matrix_matrix_multiplies(Ct,C,CtC);
339 
340  Matrix<Type> BtC(9,3);
341  slip::matrix_matrix_multiplies(Bt,C,BtC);
342 
343  Matrix<Type> BtC_t(3,9);
344  slip::transpose(BtC,BtC_t);
345 
346  Matrix<Type> CtB(3,9);
347  slip::matrix_matrix_multiplies(Ct,B,CtB);
348 
349  Matrix<Type> BtB(9,9);
350  slip::matrix_matrix_multiplies(Bt,B,BtB);
351 
352  Matrix<Type> BtB_inv(9,9);
353  slip::inverse(BtB,BtB_inv);
354 
355  Matrix<Type> Tmp(9,3);
356  slip::matrix_matrix_multiplies(BtB_inv,BtC,Tmp);
357 
358 
359  Matrix<Type> D2(3,3);
360  slip::matrix_matrix_multiplies(CtB,Tmp,D2);
361 
362  Matrix<Type> D(3,3);
363  D = CtC-D2;
364 
365  slip::Vector<Type> EigenValues(3);
366  slip::Matrix<Type> EigenVectors(3,3);
367  slip::hermitian_eigen(D,EigenValues,EigenVectors);
368 
369  Vector<Type> x3_1(3);
370  x3_1[0]=EigenVectors[0][2]; x3_1[1]=EigenVectors[1][2]; x3_1[2]=EigenVectors[2][2];
371 
372  Type norm=std::sqrt(x3_1[0]*x3_1[0]+x3_1[1]*x3_1[1]+x3_1[2]*x3_1[2]);
373 
374  x3_1/=norm;
375  Vector<Type> x9_1(9);
376  slip::matrix_vector_multiplies(Tmp,x3_1,x9_1);
377  x9_1=-x9_1;
378 
379  Vector<Type> x3(3); Vector<Type> x9(9);
380  x9=x9_1[8]>0 ? -x9_1 : x9_1;
381  x3=x9_1[8]>0 ? -x3_1 : x3_1;
382 
383  Mat[0][0]=x9[0]; Mat[0][1]=x9[1]; Mat[0][2]=x9[2]; Mat[0][3]=x9[3];
384  Mat[1][0]=x9[4]; Mat[1][1]=x9[5]; Mat[1][2]=x9[6]; Mat[1][3]=x9[7];
385  Mat[2][0]=x3[0]; Mat[2][1]=x3[1]; Mat[2][2]=x3[2]; Mat[2][3]=x9[8];
386 
387  }
388 
389 
412  template <typename Type>
413  void getpars_SoloffUV(const slip::Matrix<Type>& P,
414  slip::MultivariatePolynomial<Type,3>& Pol_x,
415  slip::MultivariatePolynomial<Type,3>& Pol_y)
416  {
417 
418  const std::size_t P_rows = P.rows();
419  slip::Matrix<Type> A(P_rows,19);
420  slip::Vector<Type> bx(P_rows,1);
421  slip::Vector<Type> by(P_rows,1);
422 
423  for(size_t i = 0; i < P_rows; i++)
424  {
425  A[i][0]=static_cast<Type>(1.0);
426  A[i][1]=P[i][2]; // X
427  A[i][2]=P[i][2]*P[i][2]; // XX
428  A[i][3]=A[i][2]*P[i][2]; // XXX
429  A[i][4]=P[i][3]; // Y
430  A[i][5]=P[i][2]*P[i][3]; // XY
431  A[i][6]=P[i][2]*A[i][5]; // XXY
432  A[i][7]=P[i][3]*P[i][3]; // YY
433  A[i][8]=P[i][2]*A[i][7]; // XYY
434  A[i][9]=A[i][7]*P[i][3]; // YYY
435  A[i][10]=P[i][4]; // Z
436  A[i][11]=P[i][2]*P[i][4]; // XZ
437  A[i][12]=P[i][2]*A[i][11]; // XXZ
438  A[i][13]=P[i][3]*P[i][4]; // YZ
439  A[i][14]=P[i][2]*A[i][13]; // XYZ
440  A[i][15]=P[i][3]*A[i][13]; // YYZ
441  A[i][16]=P[i][4]*P[i][4]; // ZZ
442  A[i][17]=P[i][2]*A[i][16]; // XZZ
443  A[i][18]=P[i][3]*A[i][16]; // YZZ
444 
445  bx[i]=P[i][0]; // x
446  by[i]=P[i][1]; // y
447  }
448 
449  // calculate the pseudo-inverse
450  slip::Matrix<Type> AT(A.dim2(),A.dim1());
451  slip::transpose(A,AT);
452  slip::Matrix<Type> ATA(A.dim2(),A.dim2());
453  slip::matrix_matrix_multiplies(AT,A,ATA);
454  slip::Matrix<Type> ATAm1(A.dim2(),A.dim2());
455  slip::inverse(ATA,ATAm1);
456  slip::Matrix<Type> ATAm1AT(A.dim2(),A.dim1());
457  slip::matrix_matrix_multiplies(ATAm1,AT,ATAm1AT);
458 
459  // calculate parameters m
460  slip::Vector<Type> mx(19);
461  slip::Vector<Type> my(19);
462  slip::matrix_vector_multiplies(ATAm1AT,bx,mx);
463  slip::matrix_vector_multiplies(ATAm1AT,by,my);
464 
465  slip::Vector<Type> mx2(20);
466  slip::Vector<Type> my2(20);
467  for(int i=0; i<19; i++)
468  {
469  mx2[i]=mx[i]; my2[i]=my[i];
470  }
471 
472  slip::MultivariatePolynomial<Type,3> Px(3,mx2.begin(),mx2.end());
473  slip::MultivariatePolynomial<Type,3> Py(3,my2.begin(),my2.end());
474  Pol_x=Px;
475  Pol_y=Py;
476  }
477 
478 
479 
500  template <typename Type>
501  void getpars_SoloffXY(const slip::Matrix<Type>& P,
502  slip::MultivariatePolynomial<Type,3>& Pol_x,
503  slip::MultivariatePolynomial<Type,3>& Pol_y)
504  {
505 
506  const std::size_t P_rows = P.rows();
507  slip::Matrix<Type> A(P_rows,19);
508  slip::Vector<Type> bx(P_rows,1);
509  slip::Vector<Type> by(P_rows,1);
510 
511  for(std::size_t i = 0; i < P_rows; ++i)
512  {
513  A[i][0]=static_cast<Type>(1.0);
514  A[i][1]=P[i][0]; // x
515  A[i][2]=P[i][0]*P[i][0]; // xx
516  A[i][3]=P[i][0]*A[i][2]; // xxx
517  A[i][4]=P[i][1]; // Y
518  A[i][5]=P[i][0]*P[i][1]; // xY
519  A[i][6]=P[i][0]*A[i][5]; // xxY
520  A[i][7]=P[i][1]*P[i][1]; // YY
521  A[i][8]=P[i][0]*A[i][7]; // xYY
522  A[i][9]=P[i][1]*A[i][7]; // YYY
523  A[i][10]=P[i][4]; // Z
524  A[i][11]=P[i][0]*P[i][4]; // xZ
525  A[i][12]=P[i][0]*A[i][11]; // xxZ
526  A[i][13]=P[i][1]*P[i][4]; // YZ
527  A[i][14]=P[i][0]*A[i][13]; // xYZ
528  A[i][15]=P[i][1]*A[i][13]; // YYZ
529  A[i][16]=P[i][4]*P[i][4]; // ZZ
530  A[i][17]=P[i][0]*A[i][16]; // xZZ
531  A[i][18]=P[i][1]*A[i][16]; // YZZ
532 
533  bx[i]=P[i][2]; // X
534  by[i]=P[i][3]; // Y
535  }
536 
537  // calculate the pseudo-inverse
538  slip::Matrix<Type> AT(A.dim2(),A.dim1());
539  slip::transpose(A,AT);
540  slip::Matrix<Type> ATA(A.dim2(),A.dim2());
541  slip::matrix_matrix_multiplies(AT,A,ATA);
542  slip::Matrix<Type> ATAm1(A.dim2(),A.dim2());
543  slip::inverse(ATA,ATAm1);
544  slip::Matrix<Type> ATAm1AT(A.dim2(),A.dim1());
545  slip::matrix_matrix_multiplies(ATAm1,AT,ATAm1AT);
546 
547  // calculate parameters m
548  slip::Vector<Type> mx(19);
549  slip::Vector<Type> my(19);
550  slip::matrix_vector_multiplies(ATAm1AT,bx,mx);
551  slip::matrix_vector_multiplies(ATAm1AT,by,my);
552 
553  slip::Vector<Type> mx2(20);
554  slip::Vector<Type> my2(20);
555  for(std::size_t i=0; i<19; ++i)
556  {
557  mx2[i]=mx[i];
558  my2[i]=my[i];
559  }
560 
561  slip::MultivariatePolynomial<Type,3> Px(3,mx2.begin(),mx2.end());
562  slip::MultivariatePolynomial<Type,3> Py(3,my2.begin(),my2.end());
563  Pol_x=Px;
564  Pol_y=Py;
565  }
566 
567 
581  template <typename Type>
582  int decompose_RQ(const slip::Matrix<Type>& M,
583  slip::Matrix<Type>& K,
584  slip::Matrix<Type>& R,
585  slip::Vector3d<Type>& c)
586  {
587 
588  slip::Matrix<Type> Mat(3,3);
589  //
590  Type D1 = M[1][2]*M[2][3] - M[2][2]*M[1][3];
591  Type D2 = M[1][1]*M[2][3] - M[2][1]*M[1][3];
592  Type D3 = M[1][1]*M[2][2] - M[2][1]*M[1][2];
593  Type D4 = M[1][0]*M[2][3] - M[2][0]*M[1][3];
594  Type D5 = M[1][0]*M[2][1] - M[2][0]*M[1][1];
595  Type D6 = M[1][0]*M[2][2] - M[2][0]*M[1][2];
596 
597 
598  c[0]= M[0][1]*D1 - M[0][2]*D2 + M[0][3]*D3;
599  c[1]= -M[0][0]*D1 + M[0][2]*D4 - M[0][3]*D6;
600  c[2]= M[0][0]*D2 - M[0][1]*D4 + M[0][3]*D5;
601  c[3]= -M[0][0]*D3 + M[0][1]*D6 - M[0][2]*D5;
602  c[0]=c[0]/c[3];
603  c[1]=c[1]/c[3];
604  c[2]=c[2]/c[3];
605  std::copy(M.row_begin(0),M.row_begin(0)+3,Mat.row_begin(0));
606  std::copy(M.row_begin(1),M.row_begin(1)+3,Mat.row_begin(1));
607  std::copy(M.row_begin(2),M.row_begin(2)+3,Mat.row_begin(2));
608 
609  int sing = slip::rq_decomp(Mat,K,R);
610  R=-R;
611  K/=K[2][2];
612  return sing;
613  }
614 
615 
629  template <typename Type>
630  int decompose_direct(const slip::Matrix<Type>& M,
631  slip::Matrix<Type>& K,
632  slip::Matrix<Type>& R,
633  slip::Vector3d<Type>& c)
634  {
635 
636 
637 
638  slip::Vector3d<Type> m1(M[0][0],M[0][1],M[0][2]);
639  slip::Vector3d<Type> m2(M[1][0],M[1][1],M[1][2]);
640  slip::Vector3d<Type> m3(M[2][0],M[2][1],M[2][2]);
641 
642  slip::Vector3d<Type> r3(m3);
643 
644  Type u0 = std::inner_product(m1.begin(),m1.end(),m3.begin(),
645  Type());
646  Type v0 = std::inner_product(m2.begin(),m2.end(),m3.begin(),
647  Type());
648  slip::Vector3d<Type> tmp;
649  tmp = m1.product(m3);
650  Type alpha_u = -tmp.Euclidean_norm();
651  tmp = m2.product(m3);
652  Type alpha_v = tmp.Euclidean_norm();
653  Type zero = static_cast<Type>(0.0);
654  Type s = zero;
655 
656  slip::Vector3d<Type> r1;
657  slip::Vector3d<Type> r2;
658 
659  r1 = (m1-(u0*m3))/alpha_u;
660  r2 = (m2-(v0*m3))/alpha_v;
661 
662  slip::Vector3d<Type> t;
663  t[0] = (M[0][3]-u0*M[2][3])/alpha_u;
664  t[1] = (M[1][3]-v0*M[2][3])/alpha_v;
665  t[2] = M[2][3];
666 
667  R[0][0]=r1[0]; R[0][1]=r1[1]; R[0][2]=r1[2];
668  R[1][0]=r2[0]; R[1][1]=r2[1]; R[1][2]=r2[2];
669  R[2][0]=r3[0]; R[2][1]=r3[1]; R[2][2]=r3[2];
670 
671  K[0][0]=alpha_u; K[0][1]=s; K[0][2]=u0;
672  K[1][0]=zero; K[1][1]=alpha_v; K[1][2]=v0;
673  K[2][0]=zero; K[2][1]=zero; K[2][2]=static_cast<Type>(1.0);
674  //PR(u0)PR(v0)PR(alpha_u)PR(alpha_v)
675  slip::Matrix<Type> Rinv(3,3);
676  slip::inverse(R,Rinv);
677  slip::matrix_vector_multiplies(Rinv,t,c);
678  c = -c;
679 
680  return 1;
681  }
682 
683 
695  template <typename Type>
696  slip::Point2d<Type> invert_distortion_model(const slip::Point2d<Type> &pd,
697  const slip::Vector<Type> &p) {
698 
699  Type EPS = 1E-13;
700 
701  Type xd = pd[0];
702  Type yd = pd[1];
703  Type x = xd;
704  Type y = yd;
705 
706  std::size_t ctr = 0;
707  Type epsilon = static_cast<Type>(1);
708  Type one = static_cast<Type>(1);
709  Type two = static_cast<Type>(2);
710  Type three = static_cast<Type>(3);
711  Type six = static_cast<Type>(6);
712  bool done = 0;
713  while(!done)
714  {
715  Type x_m_U0 = (x-p[12]);
716  Type y_m_V0 = (y-p[13]);
717  Type x_m_U02 = x_m_U0 * x_m_U0;
718  Type y_m_V02 = y_m_V0 * y_m_V0;
719  Type x_m_U0_y_m_V0 = x_m_U0 * y_m_V0;
720  Type r = x_m_U02 + y_m_V02;
721  Type r2 = r * r;
722  Type z1 = p[14]*r + p[15]*r2 + p[16]*r2*r;
723  Type z2 = p[14]
724  + two*p[15]*r
725  + three*p[16]*r2;
726  Type two_z2 = two*z2;
727  Type two_d1 = two*p[17];
728  Type two_d2 = two*p[18];
729  Type one_p_z1 = one + z1;
730  //J matrix
731  Type J00 = one_p_z1
732  + two_z2*x_m_U02
733  + two_d1*y_m_V0
734  + (six*p[18]+two*p[19])*x_m_U0;
735  Type J01 = two_z2*x_m_U0_y_m_V0
736  + two_d1*x_m_U0
737  + (two*(p[18] + p[19]))*y_m_V0 ;
738  Type J10 = two_z2*x_m_U0_y_m_V0
739  + two_d2*y_m_V0
740  + (two*(p[17] + p[20]))*x_m_U0;
741  Type J11 = one_p_z1
742  + two_z2*y_m_V02
743  + two_d2*x_m_U0
744  + (six*p[17]+ two*p[20])*y_m_V0;
745  //J inverse matrix
746  Type det = J00*J11-J01*J10;
747  Type Jinv00 = J11/det;
748  Type Jinv01 = -J01/det;
749  Type Jinv10 = -J10/det;
750  Type Jinv11 = J00/det;
751 
752  Type f = x + ( x_m_U0*z1
753  + two_d1*x_m_U0_y_m_V0
754  + p[18]*(three*x_m_U02 + y_m_V02)
755  + p[19]*r)-xd;
756  Type g = y + ( y_m_V0*z1
757  + two_d2*x_m_U0_y_m_V0
758  + p[17]*(three*y_m_V02 + x_m_U02)
759  + p[20]*r)-yd;
760 
761  x -= (Jinv00*f + Jinv01*g);
762  y -= (Jinv10*f + Jinv11*g);
763  epsilon = std::sqrt(f*f+g*g);
764 
765  if (epsilon < EPS)
766  {
767  done = 1;
768  }
769  if (ctr > 10000)
770  {
771  done = 1;
772  }
773  ctr++;
774  }
775  return slip::Point2d<Type>(x,y);
776  }
777 
778 
779 
780 
781 } //slip::
782 #endif //SLIP_CAMERA_ALGO_HPP
slip::hmatrix_matrix_multiplies
void hmatrix_matrix_multiplies(RandomAccessIterator2d1 M1_up, RandomAccessIterator2d1 M1_bot, RandomAccessIterator2d2 M2_up, RandomAccessIterator2d2 M2_bot, RandomAccessIterator2d3 result_up, RandomAccessIterator2d3 result_bot)
Computes the hermitian left multiplication of a matrix: .
Definition: linear_algebra.hpp:1289
Point3d.hpp
Provides a class to modelize 3d points.
slip::Matrix::upper_left
iterator2d upper_left()
Returns a read/write iterator2d that points to the first element of the Matrix. It points to the uppe...
Definition: Matrix.hpp:3135
slip::invert_distortion_model
slip::Point2d< Type > invert_distortion_model(const slip::Point2d< Type > &pd, const slip::Vector< Type > &p)
Inverts distortion model using the Newton-method.
Definition: camera_algo.hpp:696
slip::Matrix::dim1
size_type dim1() const
Returns the number of rows (first dimension size) in the Matrix.
Definition: Matrix.hpp:3495
slip::decompose_direct
int decompose_direct(const slip::Matrix< Type > &M, slip::Matrix< Type > &K, slip::Matrix< Type > &R, slip::Vector3d< Type > &c)
Direct-Decomposition a 3x4-Matrix into internal and external parameters.
Definition: camera_algo.hpp:630
slip::Vector::end
iterator end()
Returns a read/write iterator that points one past the last element in the Vector. Iteration is done in ordinary element order.
Definition: Vector.hpp:1481
slip::Vector3d
This is a Vector3d struct. It is a specialization of kvector. It implements some specific 3d operatio...
Definition: Vector3d.hpp:96
slip::Matrix::bottom_right
iterator2d bottom_right()
Returns a read/write iterator2d that points to the past the end element of the Matrix. It points to past the end element of the bottom right element of the Matrix.
Definition: Matrix.hpp:3151
slip::Matrix::begin
iterator begin()
Returns a read/write iterator that points to the first element in the Matrix. Iteration is done in or...
Definition: Matrix.hpp:2789
slip::epsilon
slip::lin_alg_traits< T >::value_type epsilon()
Returns the epsilon value of a real or a complex.
Definition: linear_algebra_traits.hpp:211
slip::matrix_vector_multiplies
void matrix_vector_multiplies(RandomAccessIterator2d M_upper_left, RandomAccessIterator2d M_bottom_right, RandomAccessIterator1 first1, RandomAccessIterator1 last1, RandomAccessIterator2 result_first, RandomAccessIterator2 result_last)
Computes the multiplication of a Matrix and a Vector: Result=MV.
Definition: linear_algebra.hpp:963
slip::decompose_RQ
int decompose_RQ(const slip::Matrix< Type > &M, slip::Matrix< Type > &K, slip::Matrix< Type > &R, slip::Vector3d< Type > &c)
RQ-Decomposition 3x4-Matrix into internal and external parameters.
Definition: camera_algo.hpp:582
slip::getpars_SoloffUV
void getpars_SoloffUV(const slip::Matrix< Type > &P, slip::MultivariatePolynomial< Type, 3 > &Pol_x, slip::MultivariatePolynomial< Type, 3 > &Pol_y)
Get calibration parameters using a polynomial fit (computation "by hand")
Definition: camera_algo.hpp:413
slip::Point2d
This is a point2d class, a specialized version of Point<CoordType,DIM> with DIM = 2...
Definition: Array2d.hpp:132
slip::getpars_DLT
void getpars_DLT(const slip::Matrix< Type > &P, slip::Matrix< Type > &Mat)
Get calibration parameters using the DLT.
Definition: camera_algo.hpp:187
slip::hermitian_eigen
void hermitian_eigen(const Matrix1 &A, Vector1 &EigenValues, Matrix2 &EigenVectors)
Eigen Values Computation of an hermitian semi-definite positive matrix.
Definition: linear_algebra_eigen.hpp:124
slip::kvector< T, 3 >::begin
iterator begin()
Returns a read/write iterator that points to the first element in the kvector. Iteration is done in o...
Definition: KVector.hpp:196
slip::multiplies
void multiplies(InputIterator1 __first1, InputIterator1 __last1, InputIterator2 __first2, OutputIterator __result)
Computes the pointwise product of two ranges.
Definition: arithmetic_op.hpp:508
linear_algebra_eigen.hpp
Provides some algorithms to computes eigenvalues and eigenvectors.
slip::copy
void copy(_II first, _II last, _OI output_first)
Copy algorithm optimized for slip iterators.
Definition: copy_ext.hpp:177
Vector3d.hpp
Provides a class to manipulate 3d Vector.
macros.hpp
Provides some mathematical functors and constants.
slip::sqrt
HyperVolume< T > sqrt(const HyperVolume< T > &M)
Definition: HyperVolume.hpp:5074
slip::matrix_scalar_multiplies
void matrix_scalar_multiplies(RandomAccessIterator2d1 M_upper_left, RandomAccessIterator2d1 M_bottom_right, const T &scal, RandomAccessIterator2d2 R_upper_left, RandomAccessIterator2d2 R_bottom_right)
Computes the multiplication of a Matrix by a scalar R = M*scal.
Definition: linear_algebra.hpp:856
slip::Matrix::row_end
row_iterator row_end(const size_type row)
Returns a read/write iterator that points one past the end element of the row row in the Matrix...
slip::matrix_matrix_multiplies
void matrix_matrix_multiplies(MatrixIterator1 M1_up, MatrixIterator1 M1_bot, MatrixIterator2 M2_up, MatrixIterator2 M2_bot, MatrixIterator3 R_up, MatrixIterator3 R_bot)
Computes the matrix matrix multiplication of 2 2d ranges.
Definition: linear_algebra.hpp:1621
io_tools.hpp
Provides some input/ouput algorithms.
slip::constants::one
static _Tp one()
Constant .
Definition: macros.hpp:420
slip::Vector3d::product
self product(const self &other) const
Computes the vector cross product betwin the current Vector3d and the Vector3d other.
Definition: Vector3d.hpp:608
slip::getpars_Faugeras
void getpars_Faugeras(const slip::Matrix< Type > &P, slip::Matrix< Type > &Mat)
Get calibration parameters using the Faugeras-Algorithm.
Definition: camera_algo.hpp:297
slip::inverse
void inverse(const Matrix1 &M, const std::size_t nr1, const std::size_t nc1, Matrix2 &IM, const std::size_t nr2, const std::size_t nc2)
Computes the inverse of a matrix using gaussian elimination.
Definition: linear_algebra.hpp:4963
slip::Matrix::rows
size_type rows() const
Returns the number of rows (first dimension size) in the Matrix.
Definition: Matrix.hpp:3499
slip::kvector< T, 3 >::end
iterator end()
Returns a read/write iterator that points one past the last element in the kvector. Iteration is done in ordinary element order.
Definition: KVector.hpp:217
slip::householder_qr
void householder_qr(Matrix1 &M, Vector &V0)
in place Householder QR decomposition M = QR Q is a rotation matrix and R is an upper triangular ma...
Definition: linear_algebra_qr.hpp:301
slip::inner_product
std::iterator_traits< RandomAccessIterator1 >::value_type inner_product(RandomAccessIterator1 first1, RandomAccessIterator1 last1, RandomAccessIterator2 first2, typename std::iterator_traits< RandomAccessIterator1 >::value_type init=typename std::iterator_traits< RandomAccessIterator1 >::value_type())
Computes the inner_product of two ranges X and Y: .
Definition: linear_algebra.hpp:323
linear_algebra_qr.hpp
Provides some QR decomposition algorithms.
slip::getpars_DLT_norm
void getpars_DLT_norm(const slip::Matrix< Type > &P, slip::Matrix< Type > &Mat)
Get calibration parameters using the DLT.
Definition: camera_algo.hpp:262
slip::rq_decomp
int rq_decomp(const Matrix1 &A, Matrix2 &R, Matrix3 &Q)
Computes the RQ decomposition of a matrix.
Definition: camera_algo.hpp:133
Matrix.hpp
Provides a class to manipulate Numerical Matrix.
Point2d.hpp
Provides a class to modelize 2d points.
slip::transpose
void transpose(const Matrix1 &M, const std::size_t nr1, const std::size_t nc1, Matrix2 &TM, const std::size_t nr2, const std::size_t nc2)
Computes the transpose of a matrix .
Definition: linear_algebra.hpp:2516
slip::Matrix::dim2
size_type dim2() const
Returns the number of columns (second dimension size) in the Matrix.
Definition: Matrix.hpp:3504
slip::Matrix::row_begin
row_iterator row_begin(const size_type row)
Returns a read/write iterator that points to the first element of the row row in the Matrix...
slip::getpars_SoloffXY
void getpars_SoloffXY(const slip::Matrix< Type > &P, slip::MultivariatePolynomial< Type, 3 > &Pol_x, slip::MultivariatePolynomial< Type, 3 > &Pol_y)
Get calibration parameters using a polynomial fit (computation "by hand")
Definition: camera_algo.hpp:501
Vector.hpp
Provides a class to manipulate numerical vectors.
slip::Vector::begin
iterator begin()
Returns a read/write iterator that points to the first element in the Vector. Iteration is done in or...
Definition: Vector.hpp:1474
MultivariatePolynomial.hpp
Provides a class to manipulate multivariate polynomial.
norms.hpp
Provides some algorithms to compute norms.
Generated on Tue Sep 30 2014 10:05:56 for SLIP by   doxygen 1.8.6