calibration.h
説明を見る。
1 //
2 // Copyright (c) 2003-2011, MIST Project, Nagoya University
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without modification,
6 // are permitted provided that the following conditions are met:
7 //
8 // 1. Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright notice,
12 // this list of conditions and the following disclaimer in the documentation
13 // and/or other materials provided with the distribution.
14 //
15 // 3. Neither the name of the Nagoya University nor the names of its contributors
16 // may be used to endorse or promote products derived from this software
17 // without specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
20 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
21 // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
22 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
25 // IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
26 // THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 
36 
37 #ifndef __INCLUDE_MIST_CALIBRATION__
38 #define __INCLUDE_MIST_CALIBRATION__
39 
40 
41 #ifndef __INCLUDE_MIST_CONF_H__
42 #include "config/mist_conf.h"
43 #endif
44 
45 #ifndef __INCLUDE_MIST_MATRIX__
46 #include "matrix.h"
47 #endif
48 
49 #ifndef __INCLUDE_MIST_VECTOR__
50 #include "vector.h"
51 #endif
52 
53 #ifndef __INCLUDE_NUMERIC__
54 #include "numeric.h"
55 #endif
56 
57 #ifndef __INCLUDE_MIST_MINIMIZATION__
58 #include "minimization.h"
59 #endif
60 
61 
62 #include <vector>
63 
64 
65 // mist名前空間の始まり
66 _MIST_BEGIN
67 
68 
69 
77 
78 
80 namespace Tsai
81 {
83  struct parameter
84  {
85  // キャリブレーション前に入力する必要がある変数
86  double Ncx;
87  double Ncy;
88  double Nfx;
89  double Nfy;
90  double dx;
91  double dy;
92  double Cx;
93  double Cy;
94  double sx;
95 
96  // キャリブレーション後に得られるパラメータ
97  double dpx;
98  double dpy;
99  double r1;
100  double r2;
101  double r3;
102  double r4;
103  double r5;
104  double r6;
105  double r7;
106  double r8;
107  double r9;
108  double Tx;
109  double Ty;
110  double Tz;
111  double f;
112  double ka1;
113 
114  parameter( )
115  : Ncx( 640 ), Ncy( 480 ), Nfx( 640 ), Nfy( 640 ), dx( 1.0 ), dy( 1.0 ), Cx( 320 ), Cy( 240 ), sx( 1.0 ),
116  r1( 1.0 ), r2( 0.0 ), r3( 0.0 ), r4( 0.0 ), r5( 1.0 ), r6( 0.0 ), r7( 0.0 ), r8( 0.0 ), r9( 1.0 ),
117  Tx( 0.0 ), Ty( 0.0 ), Tz( 0.0 ), f( 1.0 ), ka1( 0.0 )
118  {
119  }
120  };
121 
122 
123  inline std::string to_string( double v, int f1, int f2 )
124  {
125  char format[ 20 ];
126  char buff[ 256 ];
127 
128  if( f2 == 0 )
129  {
130  sprintf( format, "%%f" );
131  }
132  else
133  {
134  sprintf( format, "%%%d.%df", f1 + f2 + 1, f2 );
135  }
136 
137  sprintf( buff, format, v );
138 
139  return( buff );
140  }
141 
142  inline std::string fixed_string( double v, int f1, int f2, size_t len )
143  {
144  std::string str = to_string( v, f1, f2 );
145  size_t i = str.length( );
146  for( ; i < len ; i++ )
147  {
148  str += " ";
149  }
150 
151  return( str );
152  }
153 
161  inline ::std::ostream &operator <<( ::std::ostream &out, const parameter &p )
162  {
163  out << "Ncx : " << fixed_string( p.Ncx, 4, 6, 12 ) << " [cell]" << ::std::endl;
164  out << "Ncy : " << fixed_string( p.Ncy, 4, 6, 12 ) << " [cell]" << ::std::endl;
165  out << "Nfx : " << fixed_string( p.Nfx, 4, 6, 12 ) << " [pixels]" << ::std::endl;
166  out << "Nfy : " << fixed_string( p.Nfy, 4, 6, 12 ) << " [pixels]" << ::std::endl;
167  out << "dx : " << fixed_string( p.dx, 4, 6, 12 ) << " [mm/cell]" << ::std::endl;
168  out << "dy : " << fixed_string( p.dy, 4, 6, 12 ) << " [mm/cell]" << ::std::endl;
169  out << "dpx : " << fixed_string( p.dpx, 4, 6, 12 ) << " [mm/pixel]" << ::std::endl;
170  out << "dpy : " << fixed_string( p.dpy, 4, 6, 12 ) << " [mm/pixel]" << ::std::endl;
171  out << "Cx : " << fixed_string( p.Cx, 4, 6, 12 ) << " [pixel]" << ::std::endl;
172  out << "Cy : " << fixed_string( p.Cy, 4, 6, 12 ) << " [pixel]" << ::std::endl;
173  out << "sx : " << fixed_string( p.sx, 4, 6, 12 ) << ::std::endl;
174 
175  out << ::std::endl;
176  out << "R =" << ::std::endl;
177  out << to_string( p.r1, 2, 6 ) << ", " << to_string( p.r2, 2, 6 ) << ", " << to_string( p.r3, 2, 6 ) << ::std::endl;
178  out << to_string( p.r4, 2, 6 ) << ", " << to_string( p.r5, 2, 6 ) << ", " << to_string( p.r6, 2, 6 ) << ::std::endl;
179  out << to_string( p.r7, 2, 6 ) << ", " << to_string( p.r8, 2, 6 ) << ", " << to_string( p.r2, 2, 6 ) << ::std::endl;
180 
181  out << ::std::endl;
182  out << "T [mm] = ( " << p.Tx << ", " << p.Ty << ", " << p.Tz << " )" << ::std::endl;
183  out << "focal length = " << p.f << " [mm]" << ::std::endl;
184  out << "Tz / f = " << p.Tz / p.f << ::std::endl;
185  out << "kappa1 = " << p.ka1 << " [1/mm^2]" << ::std::endl;
186 
187  return( out );
188  }
189 
190 
191  template < class T1, class T2 >
192  struct __parameter__ : public parameter
193  {
194  typedef parameter base;
195  typedef vector3< T1 > point3;
196  typedef vector2< T2 > point2;
197 
198  const std::vector< point3 > &world;
199  const std::vector< point2 > &image;
200 
201  __parameter__( const parameter &p, const std::vector< point3 > &w, const std::vector< point2 > &i )
202  : base( p ), world( w ), image( i )
203  {
204  }
205 
206  template < class TT >
207  double operator( )( const matrix< TT > &v ) const
208  {
209  typedef matrix< TT >::size_type size_type;
210 
211  double err = 0.0;
212  double f = v[ 0 ];
213  double Tz = v[ 1 ];
214  double ka1 = v[ 2 ];
215 
216  for( size_type i = 0 ; i < world.size( ) ; i++ )
217  {
218  const point3 &w = world[ i ];
219  const point2 &p = image[ i ];
220  double Xd = ( p.x - Cx ) * dpx / sx;
221  double Yd = ( p.y - Cy ) * dpy;
222  double x = r1 * w.x + r2 * w.y + r3 * w.z + Tx;
223  double y = r4 * w.x + r5 * w.y + r6 * w.z + Ty;
224  double z = r7 * w.x + r8 * w.y + r9 * w.z + Tz;
225  double rr = Xd * Xd + Yd * Yd;
226 
227  double e1 = f * x - Xd * ( 1 + ka1 * rr ) * z;
228  double e2 = f * y - Yd * ( 1 + ka1 * rr ) * z;
229 
230  //std::cout << x << ", " << y << ", " << z << std::endl;
231 
232  //err += std::sqrt( e1 * e1 );
233  //err += std::sqrt( e2 * e2 );
234  err += std::sqrt( e1 * e1 + e2 * e2 );
235  }
236  return( err );
237  }
238  };
239 
240 
251  template < class T1, class T2 >
252  bool calibration( parameter &p, const std::vector< vector3< T1 > > &world, const std::vector< vector2< T2 > > &image )
253  {
254  typedef vector3< T1 > point3;
255  typedef vector2< T2 > point2;
256  typedef matrix< double > matrix_type;
257  typedef matrix< double >::size_type size_type;
258 
259  double r1, r2, r3, r4, r5, r6, r7, r8, r9;
260  double Tx, Ty, Tz;
261  double f;
262  double ka1 = 0.0;
263 
264  double Ncx = p.Ncx; // カメラのX軸方向のセンサー素子数 [cell]
265  double Ncy = p.Ncy; // カメラのY軸方向のセンサー素子数 [cell]
266  double Nfx = p.Nfx; // 撮影される画像のX軸方向の画素数 [pixel]
267  double Nfy = p.Nfy; // 撮影される画像のY軸方向の画素数 [pixel]
268  double dx = p.dx; // カメラのX軸方向のセンサー素子の大きさ [mm/cell]
269  double dy = p.dy; // カメラのY軸方向のセンサー素子の大きさ [mm/cell]
270  double Cx = p.Cx; // カメラ座標系におけるZ軸と画像平面の交点のX座標（画像中心を与える）[pixel]
271  double Cy = p.Cy; // カメラ座標系におけるZ軸と画像平面の交点のY座標（画像中心を与える）[pixel]
272  double sx = p.sx; // 複数平面を用いたキャリブレーションの際に用いる，X軸方向の解像度を調整する係数
273 
274  double dpx = dx * Ncx / Nfx; // カメラのX軸方向の1画素あたりの大きさ [mm/pixel]
275  double dpy = dy * Ncy / Nfy; // カメラのY軸方向の1画素あたりの大きさ [mm/pixel]
276 
277  size_type i, num = image.size( );
278 
279  matrix_type A( num, 5 ), B( num, 1 );
280 
281  // (i) 画像座標系における点を計算する
282  for( i = 0 ; i < num ; i++ )
283  {
284  const point3 &w = world[ i ];
285  const point2 &p = image[ i ];
286  double Xd = ( p.x - Cx ) * dpx / sx;
287  double Yd = ( p.y - Cy ) * dpy;
288 
289  A( i, 0 ) = Yd * w.x;
290  A( i, 1 ) = Yd * w.y;
291  A( i, 2 ) = Yd;
292  A( i, 3 ) = - Xd * w.x;
293  A( i, 4 ) = - Xd * w.y;
294  B( i, 0 ) = Xd;
295  }
296 
297  // (ii) 5つの未知数を解く
298  matrix_type L = inverse( A ) * B;
299 
300  // (iii.1) Tyを求める
301  double r1_ = L[ 0 ];
302  double r2_ = L[ 1 ];
303  double Ty_Tx = L[ 2 ];
304  double r4_ = L[ 3 ];
305  double r5_ = L[ 4 ];
306 
307  bool b1 = std::abs( r1_ ) > 1.0e-12;
308  bool b2 = std::abs( r2_ ) > 1.0e-12;
309  bool b4 = std::abs( r4_ ) > 1.0e-12;
310  bool b5 = std::abs( r5_ ) > 1.0e-12;
311 
312  if( b1 && b2 && b4 && b5 )
313  {
314  double Sr = r1_ * r1_ + r2_ * r2_ + r4_ * r4_ + r5_ * r5_;
315  double Br = r1_ * r5_ - r4_ * r2_;
316  Ty = ( Sr - std::sqrt( Sr * Sr - 4.0 * Br * Br ) ) / ( 2.0 * Br * Br );
317  }
318  else
319  {
320  double rr[ 4 ];
321  size_type count = 0;
322  if( b1 )
323  {
324  rr[ count++ ] = r1_;
325  }
326  if( b2 )
327  {
328  rr[ count++ ] = r2_;
329  }
330  if( b4 )
331  {
332  rr[ count++ ] = r4_;
333  }
334  if( b5 )
335  {
336  rr[ count++ ] = r5_;
337  }
338  Ty = 1.0 / ( rr[ 0 ] * rr[ 0 ] + rr[ 1 ] * rr[ 1 ] );
339  }
340 
341 
342  // (iii.2) Tyの符合を決定する
343  {
344  // 画像中心から最も離れているものを選択する
345  point3 w = world[ 0 ];
346  point2 p = image[ 0 ];
347  double len = ( p.x - Cx ) * ( p.x - Cx ) + ( p.y - Cy ) * ( p.y - Cy );
348  for( i = 1 ; i < num ; i++ )
349  {
350  point3 ww = world[ i ];
351  point2 pp = image[ i ];
352  double l = ( pp.x - Cx ) * ( pp.x - Cx ) + ( pp.y - Cy ) * ( pp.y - Cy );
353  if( len < l )
354  {
355  w = ww;
356  p = pp;
357  }
358  }
359 
360  // Tyの符号を正にする
361  Ty = std::sqrt( Ty );
362  r1 = r1_ * Ty;
363  r2 = r2_ * Ty;
364  r4 = r4_ * Ty;
365  r5 = r5_ * Ty;
366  Tx = Ty_Tx * Ty;
367 
368  double x = r1 * w.x + r2 * w.y + Tx;
369  double y = r4 * w.x + r5 * w.y + Ty;
370  if( x * p.x > 0 && y * p.y > 0 )
371  {
372  // Tyの符号は正にする
373  }
374  else
375  {
376  // Tyの符号は負にする
377  Ty = -Ty;
378  }
379  }
380 
381  // Tyの計算結果を用いて値を更新する
382  r1 = r1_ * Ty;
383  r2 = r2_ * Ty;
384  r4 = r4_ * Ty;
385  r5 = r5_ * Ty;
386  Tx = Ty_Tx * Ty;
387 
388  //std::cout << "( Tx, Ty ) = ( " << Tx << ", " << Ty << " )" << std::endl;
389 
390  // (iii.3) 回転行列Rを決定する
391  {
392  double s = -1.0 * ( r1 * r4 + r2 * r5 < 0.0 ? -1.0 : 1.0 );
393 
394  r3 = std::sqrt( 1.0 - r1 * r1 - r2 * r2 );
395  r6 = s * std::sqrt( 1.0 - r4 * r4 - r5 * r5 );
396  r7 = r2 * r6 - r3 * r5;
397 
398  // 直行行列になるように変形を行う
399  double Rz = std::atan2( r4, r1 );
400 
401  double s1, s2, s3, c1, c2, c3;
402 
403  s3 = std::sin( Rz );
404  c3 = std::cos( Rz );
405 
406  double Ry = std::atan2( -r7, r1 * c3 + r4 * s3 );
407  double Rx = std::atan2( r3 * s3 - r6 * c3, r5 * c3 - r2 * s3 );
408 
409  s1 = std::sin( Rx );
410  c1 = std::cos( Rx );
411  s2 = std::sin( Ry );
412  c2 = std::cos( Ry );
413 
414  r1 = c2 * c3;
415  r2 = c3 * s1 * s2 - c1 * s3;
416  r3 = s1 * s3 + c1 * c3 * s2;
417  r4 = c2 * s3;
418  r5 = s1 * s2 * s3 + c1 * c3;
419  r6 = c1 * s2 * s3 - c3 * s1;
420  r7 = -s2;
421  r8 = c2 * s1;
422  r9 = c1 * c2;
423  }
424 
425 
426  A.resize( num, 2 );
427 
428  // (iv) 焦点距離とTzの近似値を，レンズ歪を無視して求める
429  for( i = 0 ; i < num ; i++ )
430  {
431  const point3 &w = world[ i ];
432  const point2 &p = image[ i ];
433  double Yd = ( p.y - Cy ) * dpy;
434 
435  A( i, 0 ) = r4 * w.x + r5 * w.y + r6 * 0 + Ty;
436  A( i, 1 ) = -Yd;
437  B( i, 0 ) = ( r7 * w.x + r8 * w.y + r9 * 0 ) * Yd;
438  }
439 
440  L = inverse( A ) * B;
441 
442  f = L[ 0 ];
443  Tz = L[ 1 ];
444 
445  if( f < 0.0 )
446  {
447  // 求まった焦点距離が負となるため，回転行列を再設定して計算しなおす
448  r3 = -r3;
449  r6 = -r6;
450  r7 = -r7;
451  r8 = -r8;
452 
453  // 回転行列Rを再決定する
454  {
455  double Rz = atan2( r4, r1 );
456  double s3 = std::sin( Rz );
457  double c3 = std::cos( Rz );
458  double Ry = atan2( -r7, r1 * c3 + r4 * s3 );
459  double Rx = atan2( r3 * s3 - r6 * c3, r5 * c3 - r2 * s3 );
460 
461  double s1 = std::sin( Rx );
462  double c1 = std::cos( Rx );
463  double s2 = std::sin( Ry );
464  double c2 = std::cos( Ry );
465 
466  r1 = c2 * c3;
467  r2 = c3 * s1 * s2 - c1 * s3;
468  r3 = s1 * s3 + c1 * c3 * s2;
469  r4 = c2 * s3;
470  r5 = s1 * s2 * s3 + c1 * c3;
471  r6 = c1 * s2 * s3 - c3 * s1;
472  r7 = -s2;
473  r8 = c2 * s1;
474  r9 = c1 * c2;
475  }
476 
477  for( i = 0 ; i < num ; i++ )
478  {
479  const point3 &w = world[ i ];
480  const point2 &p = image[ i ];
481  double Yd = ( p.y - Cy ) * dpy;
482 
483  A( i, 0 ) = r4 * w.x + r5 * w.y + r6 * 0 + Ty;
484  A( i, 1 ) = -Yd;
485  B( i, 0 ) = ( r7 * w.x + r8 * w.y + r9 * 0 ) * Yd;
486  }
487 
488  L = inverse( A ) * B;
489 
490  f = L[ 0 ];
491  Tz = L[ 1 ];
492 
493  if( f < 0 )
494  {
495  // なんかおかしいね・・・
496  return( false );
497  }
498  }
499 
500  // (v) 最急降下法を用いて正確な焦点距離とTzの値を求める
501  __parameter__< T1, T2 > param( p, world, image );
502  param.dpx = dpx; // カメラのX軸方向の1画素あたりの大きさ [mm/pixel]
503  param.dpy = dpy; // カメラのY軸方向の1画素あたりの大きさ [mm/pixel]
504  param.r1 = r1; // キャリブレーションの結果得られる回転行列の ( 0, 0 ) 成分
505  param.r2 = r2; // キャリブレーションの結果得られる回転行列の ( 0, 1 ) 成分
506  param.r3 = r3; // キャリブレーションの結果得られる回転行列の ( 0, 2 ) 成分
507  param.r4 = r4; // キャリブレーションの結果得られる回転行列の ( 1, 0 ) 成分
508  param.r5 = r5; // キャリブレーションの結果得られる回転行列の ( 1, 1 ) 成分
509  param.r6 = r6; // キャリブレーションの結果得られる回転行列の ( 1, 2 ) 成分
510  param.r7 = r7; // キャリブレーションの結果得られる回転行列の ( 2, 0 ) 成分
511  param.r8 = r8; // キャリブレーションの結果得られる回転行列の ( 2, 1 ) 成分
512  param.r9 = r9; // キャリブレーションの結果得られる回転行列の ( 2, 2 ) 成分
513  param.Tx = Tx; // キャリブレーションの結果得られるカメラの平行移動のX成分
514  param.Ty = Ty; // キャリブレーションの結果得られるカメラの平行移動のY成分
515  param.Tz = Tz; // キャリブレーションの結果得られるカメラの平行移動のZ成分
516  param.f = f; // キャリブレーションの結果得られる焦点距離
517  param.ka1 = ka1; // キャリブレーションの結果得られる円形の歪成分
518 
519  {
520  matrix< double > ppp( 3, 1 ), dirs = matrix_type::identity( 3, 3 );
521  ppp[ 0 ] = f;
522  ppp[ 1 ] = Tz;
523  ppp[ 2 ] = ka1;
524 
525  //std::cout << "( f, Tz, ka1 ) = " << ppp.t( ) << std::endl;
526 
527  lucidi::minimization( ppp, dirs, param, 1.0e-16 );
528  //powell::minimization( ppp, dirs, param, 0.0000001 );
529  //gradient::minimization( ppp, param, 0.0000001, 0.01 );
530 
531  //std::cout << "( f, Tz, ka1 ) = " << ppp.t( ) << std::endl;
532 
533  param.f = ppp[ 0 ];
534  param.Tz = ppp[ 1 ];
535  param.ka1 = ppp[ 2 ];
536  }
537 
538  p = param;
539 
540  if( p.Tz < 0 )
541  {
542  // カメラの正方向の符号が逆転しているので，その他の符号を正しく設定する
543  // なんか知らないけどこれでうまく行くみたい
544  // どっかで座標系がひっくり返ったのか？
545  p.r1 = -p.r1;
546  p.r2 = -p.r2;
547  p.r4 = -p.r4;
548  p.r5 = -p.r5;
549  p.r7 = -p.r7;
550  p.r8 = -p.r8;
551  p.Tx = -p.Tx;
552  p.Ty = -p.Ty;
553  p.Tz = -p.Tz;
554  }
555 
556  return( true );
557  }
558 }
559 
560 
561 
563 // キャリブレーションの終わり
564 
565 
566 
567 // mist名前空間の終わり
568 _MIST_END
569 
570 
571 #endif // __INCLUDE_MIST_CALIBRATION__
572 
Generated on Wed Nov 12 2014 19:44:13 for MIST by doxygen 1.8.1.2