- メインページ
- モジュール
- ネームスペース
- データ構造
- ファイル

- ファイル一覧
- グローバル

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