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

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

epipolar.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
33 #ifndef __INCLUDE_MIST_EPIPOLAR__
34 #define __INCLUDE_MIST_EPIPOLAR__
35
36 #ifndef __INCLUDE_MIST_H__
37 #include <mist/mist.h>
38 #endif
39
40 #ifndef __INCLUDE_MIST_MATRIX__
41 #include <mist/matrix.h>
42 #endif
43
44 #ifndef __INCLUDE_NUMERIC__
45 #include <mist/numeric.h>
46 #endif
47
48 #ifndef __INCLUDE_MIST_RANDOM__
49 #include <mist/random.h>
50 #endif
51
52 #include <queue>
53 #include <cmath>
54
55 _MIST_BEGIN
56
57
58 namespace __epipolar__
59 {
60
61
72 template < typename T >
73 inline bool enforce_rank2( matrix< T > &in, matrix< T > &out )
74 {
75 matrix < T > u, s, vt;
76
77 if ( (in.rows() != 3) || (out.cols() != 3) )
78 {
79 return false;
80 }
81
82 svd( in, u, s, vt );
83 s( 2, 2 ) = static_cast< T > ( 0.0 );
84 out = u * s * vt;
85
86 return true;
87 }
88
89
90 mist::matrix< double >
91 skew_symmetric_matrix3( mist::matrix< double > &vec )
92 {
93 mist::matrix< double > mat(3, 3);
94 mat(0, 0) = 0.0; mat(0, 1) = -vec[2]; mat(0, 2) = vec[1];
95 mat(1, 0) = vec[2]; mat(1, 1) = 0.0; mat(1, 2) = -vec[0];
96 mat(2, 0) = -vec[1]; mat(2, 1) = vec[0]; mat(2, 2) = 0.0;
97 return mat;
98 }
99
100 double levi_civita3( int i, int j, int k )
101 {
102 if ( (i == j) || (j == k) || (k == i) )
103 {
104 return 0.0;
105 }
106
107 if ( ((j - i + 3) % 3 == 1 ) &&
108 ((k - j + 3) % 3 == 1 ) &&
109 ((i - k + 3) % 3 == 1 ) )
110 {
111 return 1.0;
112 }
113
114 return -1.0;
115 }
116
130 bool line_triangle_intersection( matrix< double > &origin,
131 matrix< double > &direction,
132 matrix< double > &vert0,
133 matrix< double > &vert1,
134 matrix< double > &vert2,
135 matrix< double > &intersection )
136 {
137 matrix< double > e1 = vert1 - vert0;
138 matrix< double > e2 = vert2 - vert0;
139 matrix< double > t = origin - vert0;
140 matrix< double > p = skew_symmetric_matrix3( direction ) * e2;
141 matrix< double > q = skew_symmetric_matrix3( t ) * e1;
142
143 intersection.resize(3, 1);
144 intersection[0] = (q.t() * e2)[0];
145 intersection[1] = (p.t() * t)[0];
146 intersection[2] = (q.t() * direction)[0];
147
148 intersection /= (p.t() * e1)[0];
149
150 if ( (intersection[1] >= 0.0) && (intersection[2] >= 0.0) &&
151 (intersection[1] + intersection[2] <= 1 ) )
152 {
153 return true;
154 }
155
156 return false;
157 }
158
159
160
161 } // namespace __epipolar__
162
163
165 template < typename T >
166 class trifocal_tensor
167 {
168 private:
169 array1< matrix< T > > data_;
170
171 public:
172
174 trifocal_tensor()
175 {
176 data_.resize( 3 );
177 data_[0].resize( 3, 3 );
178 data_[1].resize( 3, 3 );
179 data_[2].resize( 3, 3 );
180 }
181
183 matrix< T >& operator[] (size_t index)
184 {
185 return data_[ index ];
186 }
187
189 T& operator() ( size_t i, size_t q, size_t r )
190 {
191 return data_[i](q, r);
192 }
193
194 };
195
196
207 template < typename T1, typename T2 >
208 inline bool compute_fundamental_matrix( matrix< T1 > &points1,
209 matrix< T1 > &points2,
210 matrix< T2 > &fundamental_matrix )
211 {
212
213 if ( (points1.cols() != points2.cols()) || (points1.cols() < 8) )
214 {
215 return false;
216 }
217
218 size_t point_num = points1.cols();
219
220 matrix< double > m( point_num, 9 );
221
222 for ( size_t i = 0; i < point_num; i++ )
223 {
224 m( i, 0 ) = points1( 0, i ) * points2( 0, i );
225 m( i, 1 ) = points1( 1, i ) * points2( 0, i );
226 m( i, 2 ) = points2( 0, i );
227 m( i, 3 ) = points1( 0, i ) * points2( 1, i );
228 m( i, 4 ) = points1( 1, i ) * points2( 1, i );
229 m( i, 5 ) = points2( 1, i );
230 m( i, 6 ) = points1( 0, i );
231 m( i, 7 ) = points1( 1, i );
232 m( i, 8 ) = 1.0;
233 }
234
235 m = m.t() * m;
236
237 // compute an eigen vector for the minimnum eigen value
238 matrix< double > eval, evec;
239
240 eigen( m, eval, evec );
241
242 fundamental_matrix.resize( 3, 3 );
243
244 if ( _DESCENDING_ORDER_EIGEN_VALUE_ == 0 )
245 {
246 fundamental_matrix( 0, 0 ) = static_cast< T2 > ( evec( 0, 0 ) );
247 fundamental_matrix( 0, 1 ) = static_cast< T2 > ( evec( 1, 0 ) );
248 fundamental_matrix( 0, 2 ) = static_cast< T2 > ( evec( 2, 0 ) );
249 fundamental_matrix( 1, 0 ) = static_cast< T2 > ( evec( 3, 0 ) );
250 fundamental_matrix( 1, 1 ) = static_cast< T2 > ( evec( 4, 0 ) );
251 fundamental_matrix( 1, 2 ) = static_cast< T2 > ( evec( 5, 0 ) );
252 fundamental_matrix( 2, 0 ) = static_cast< T2 > ( evec( 6, 0 ) );
253 fundamental_matrix( 2, 1 ) = static_cast< T2 > ( evec( 7, 0 ) );
254 fundamental_matrix( 2, 2 ) = static_cast< T2 > ( evec( 8, 0 ) );
255 }
256 else
257 {
258 fundamental_matrix( 0, 0 ) = static_cast< T2 > ( evec( 0, evec.cols() - 1 ) );
259 fundamental_matrix( 0, 1 ) = static_cast< T2 > ( evec( 1, evec.cols() - 1 ) );
260 fundamental_matrix( 0, 2 ) = static_cast< T2 > ( evec( 2, evec.cols() - 1 ) );
261 fundamental_matrix( 1, 0 ) = static_cast< T2 > ( evec( 3, evec.cols() - 1 ) );
262 fundamental_matrix( 1, 1 ) = static_cast< T2 > ( evec( 4, evec.cols() - 1 ) );
263 fundamental_matrix( 1, 2 ) = static_cast< T2 > ( evec( 5, evec.cols() - 1 ) );
264 fundamental_matrix( 2, 0 ) = static_cast< T2 > ( evec( 6, evec.cols() - 1 ) );
265 fundamental_matrix( 2, 1 ) = static_cast< T2 > ( evec( 7, evec.cols() - 1 ) );
266 fundamental_matrix( 2, 2 ) = static_cast< T2 > ( evec( 8, evec.cols() - 1 ) );
267 }
268
269 __epipolar__::enforce_rank2( fundamental_matrix, fundamental_matrix );
270
271 return true;
272 }
273
274
275
276
289 template < typename T1, typename T2 >
290 inline bool compute_correspond_epilines( matrix< T1 > &points,
291 int which_image,
292 matrix< T2 > &fundamental_matrix,
293 matrix< T1 > &correspondent_lines )
294 {
295
296 size_t point_num = points.cols();
297
298 matrix< T1 > p( 3, point_num );
299
300 for ( size_t i = 0; i < point_num; i++ )
301 {
302 p( 0, i ) = points( 0, i );
303 p( 1, i ) = points( 1, i );
304 p( 2, i ) = static_cast< T1 > ( 1.0 );
305 }
306
307
308 if ( which_image == 0 )
309 {
310 correspondent_lines = fundamental_matrix * p;
311 }
312 else
313 {
314 correspondent_lines = fundamental_matrix.t() * p;
315 }
316
317 return true;
318 }
319
320
328 template < typename T1, typename T2 >
329 inline bool compute_epipole( matrix< T1 > &fundamental_matrix,
330 matrix< T2 > &epipole )
331 {
332
333 if ( (fundamental_matrix.rows() != 3) ||
334 (fundamental_matrix.cols() != 3) )
335 {
336 return false;
337 }
338
339 epipole.resize(3, 1);
340
341 mist::matrix< double > u, s, vt;
342 svd( fundamental_matrix, u, s, vt );
343 epipole[0] = u(0, 2);
344 epipole[1] = u(1, 2);
345 epipole[2] = u(2, 2);
346
347 // normalize the norm
348 T2 norm = 0.0;
349 norm += epipole[0] * epipole[0];
350 norm += epipole[1] * epipole[1];
351 norm += epipole[2] * epipole[2];
352 norm = sqrt( norm );
353 epipole /= norm;
354
355 return true;
356 }
357
358
376 template < typename T1, typename T2 >
377 inline bool factorize_essential_matrix( matrix< T1 > &essential_matrix,
378 matrix< T2 > &t,
379 matrix< T2 > &r1,
380 matrix< T2 > &r2 )
381 {
382
383
384
385 if ( (essential_matrix.rows() != 3) ||
386 (essential_matrix.cols() != 3) )
387 {
388 return false;
389 }
390
391
392 matrix< T1 > essential = essential_matrix;
393
394 mist::matrix< T1 > du, ds, dvt;
395 svd( essential, du, ds, dvt );
396 ds = mist::matrix< T1 >::_33( (ds(0, 0) + ds(1, 1)) * 0.5, 0.0, 0.0,
397 0.0, (ds(0, 0) + ds(1, 1)) * 0.5, 0.0,
398 0.0, 0.0, 0.0 );
399 essential = du * ds * dvt;
400 svd( essential, du, ds, dvt );
401
402
403 mist::matrix< T1 > w;
404 w = matrix< T1 >::_33( 0.0, -1.0, 0.0,
405 1.0, 0.0, 0.0,
406 0.0, 0.0, 1.0 );
407 mist::matrix< T1 > z;
408 z = matrix< T1 >::_33( 0.0, 1.0, 0.0,
409 -1.0, 0.0, 0.0,
410 0.0, 0.0, 0.0 );
411
412 mist::matrix< T1 > s = du * z * du.t();
413 t.resize(3, 1);
414 t[0] = s(2, 1);
415 t[1] = s(0, 2);
416 t[2] = s(1, 0);
417
418 // normalize t
419 double norm = sqrt(t[0] * t[0] + t[1] * t[1] + t[2] * t[2]);
420 t[0] /= norm;
421 t[1] /= norm;
422 t[2] /= norm;
423
424 r1 = du * w * dvt;
425 r2 = du * w.t() * dvt;
426
427
428 return true;
429 }
430
431
432
443 template < typename T1, typename T2 >
444 inline bool triangulation_dlt( matrix< T1 > &xc1,
445 matrix< T1 > &xc2,
446 matrix< T2 > &camera_matrix1,
447 matrix< T2 > &camera_matrix2,
448 matrix< T1 > &xw )
449 {
450
451 if ( (camera_matrix1.rows() != 3) || (camera_matrix1.cols() != 4) ||
452 (camera_matrix2.rows() != 3) || (camera_matrix2.cols() != 4) ||
453 (xc1.size() < 3) || (xc2.size() < 3) )
454 {
455 return false;
456 }
457
458 matrix< T1 > mat( 4, 4 );
459
460 matrix< T1 > row;
461 matrix< T1 > pt1( 1, 4 ), pt2( 1, 4 );
462
463 // use four constraints
464
465 // x coordinate, first camera
466 for (size_t i = 0; i < 4; i++)
467 {
468 pt1(0, i) = camera_matrix1( 2, i );
469 }
470 for (size_t i = 0; i < 4; i++)
471 {
472 pt2(0, i) = camera_matrix1( 0, i );
473 }
474 row = xc1[0] * pt1 - pt2;
475 for (size_t i = 0; i < 4; i++)
476 {
477 mat(0, i) = row[i];
478 }
479
480 // y coordinate, first camera
481 for (size_t i = 0; i < 4; i++)
482 {
483 pt2(0, i) = camera_matrix1( 1, i );
484 }
485 row = xc1[1] * pt1 - pt2;
486 for (size_t i = 0; i < 4; i++)
487 {
488 mat(1, i) = row[i];
489 }
490
491 // x coordinate, second camera
492 for (size_t i = 0; i < 4; i++)
493 {
494 pt1(0, i) = camera_matrix2( 2, i );
495 }
496 for (size_t i = 0; i < 4; i++)
497 {
498 pt2(0, i) = camera_matrix2( 0, i );
499 }
500 row = xc2[0] * pt1 - pt2;
501 for (size_t i = 0; i < 4; i++)
502 {
503 mat(2, i) = row[i];
504 }
505
506 // y coordinate, first camera
507 for (size_t i = 0; i < 4; i++)
508 {
509 pt2(0, i) = camera_matrix2( 1, i );
510 }
511 row = xc2[1] * pt1 - pt2;
512 for (size_t i = 0; i < 4; i++)
513 {
514 mat(3, i) = row[i];
515 }
516
517 // the equation is solve by SVD
518 matrix< T2 > u, s, vt;
519 svd( mat, u, s, vt );
520
521 xw.resize( 4, 1 );
522 xw[0] = vt( 3, 0 ) / vt(3, 3);
523 xw[1] = vt( 3, 1 ) / vt(3, 3);
524 xw[2] = vt( 3, 2 ) / vt(3, 3);
525 xw[3] = 1.0;
526
527
528 return true;
529 }
530
531
532
533
544 template < typename T1 >
545 matrix< T1 > trilinear_three_points( matrix< T1 > &p1,
546 matrix< T1 > &p2,
547 matrix< T1 > &p3 )
548 {
549 matrix< T1 > mat(9, 27);
550
551 for (int s = 0; s < 3; s++)
552 {
553 for (int t = 0; t < 3; t++)
554 {
555 // each equation
556 for (int q = 0; q < 3; q++)
557 {
558 for (int r = 0; r < 3; r++)
559 {
560 for (int i = 0; i < 3; i++)
561 {
562 // each tensor coefficient
563 double sum = 0.0;
564
565 for (int j = 0; j < 3; j++)
566 {
567 for (int k = 0; k < 3; k++)
568 {
569 double val = 1.0;
570 val *= p1[i];
571 val *= p2[j];
572 val *= p3[k];
573 val *= __epipolar__::levi_civita3( j, q, s );
574 val *= __epipolar__::levi_civita3( k, r, t );
575 sum += val;
576 }
577 }
578 mat( 3 * s + t, 3 * 3 * i + 3 * q + r ) = sum;
579 }
580 }
581 }
582 }
583 }
584
585 return mat;
586 }
587
588
599 template < typename T1 >
600 matrix< T1 > trilinear_three_lines( matrix< T1 > &l1,
601 matrix< T1 > &l2,
602 matrix< T1 > &l3 )
603
604 {
605 matrix< T1 > mat(3, 27);
606
607 for (int w = 0; w < 3; w++)
608 {
609 // each equation
610 for (int i = 0; i < 3; i++)
611 {
612 for (int q = 0; q < 3; q++)
613 {
614 for (int r = 0; r < 3; r++)
615 {
616 // each tensor coefficient
617 double sum = 0.0;
618 for (int p = 0; p < 3; p++)
619 {
620 double val = 1.0;
621 val *= l1[p];
622 val *= l2[q];
623 val *= l3[r];
624 val *= __epipolar__::levi_civita3(p, i, w);
625 sum += val;
626 }
627 mat( w, i * 3 * 3 + q * 3 + r ) = sum;
628 }
629 }
630 }
631 }
632
633 return mat;
634 }
635
636
637
647 template < typename T1, typename T2 >
648 bool compute_trifocal_tensor( mist::matrix< T1 > &relations,
649 trifocal_tensor< T2 > &tensor )
650 {
651 if ( relations.cols() != 27 )
652 {
653 return false;
654 }
655
656
657 mist::matrix< T1 > u, s, vt;
658 mist::svd( relations, u, s, vt );
659
660
661 for (size_t i = 0; i < 3; i++)
662 {
663 for (size_t q = 0; q < 3; q++)
664 {
665 for (size_t r = 0; r < 3; r++)
666 {
667 tensor[i].resize(3, 3);
668 tensor[i](q, r) = vt( vt.rows() - 1, i * 3 * 3 + q * 3 + r);
669 }
670 }
671 }
672
673 return true;
674 }
675
676
685 template < typename T1, typename T2 >
686 bool trifocal_tensor_to_epipoles( trifocal_tensor< T1 > &tensor,
687 mist::matrix< T2 > &epi21,
688 mist::matrix< T2 > &epi31 )
689 {
690 epi21.resize(1, 3);
691 epi31.resize(1, 3);
692
693 mist::matrix< T1 > u, v;
694
695 u.resize(3, 3);
696 for (int i = 0; i < 3; i++)
697 {
698
699 mist::matrix< T1 > uu, s, vt;
700 mist::svd( tensor[i], uu, s, vt );
701 for (int j = 0; j < 3; j++)
702 {
703 u(j, i) = uu(j, 2);
704 }
705
706 mist::matrix< T1 > vec(3, 1);
707 vec[0] = u(0, i);
708 vec[1] = u(1, i);
709 vec[2] = u(2, i);
710 }
711
712
713 v.resize(3, 3);
714 for (int i = 0; i < 3; i++)
715 {
716 mist::matrix< T1 > uu, s, vt;
717 mist::svd( tensor[i], uu, s, vt );
718 for (int j = 0; j < 3; j++)
719 {
720 v(j, i) = vt(2, j);
721 }
722 }
723
724
725 epi21.resize(3, 1);
726 {
727
728 mist::matrix< T1 > uu, s, vt;
729 mist::svd( u, uu, s, vt );
730 for (int j = 0; j < 3; j++)
731 {
732 epi21[j] = uu(j, 2);
733 }
734
735 double norm = sqrt( epi21[0] * epi21[0]
736 + epi21[1] * epi21[1]
737 + epi21[2] * epi21[2] );
738 epi21 /= norm;
739
740 }
741 epi31.resize(3, 1);
742 {
743
744 mist::matrix< T1 > uu, s, vt;
745 mist::svd( v, uu, s, vt );
746 for (int j = 0; j < 3; j++)
747 {
748 epi31[j] = uu(j, 2);
749 }
750 double norm = sqrt( epi31[0] * epi31[0]
751 + epi31[1] * epi31[1]
752 + epi31[2] * epi31[2] );
753 epi31 /= norm;
754
755
756 }
757
758 return true;
759 }
760
761
762
771 template <typename T1, typename T2>
772 bool compute_fundamental_matrices( trifocal_tensor< T1 > &tensor,
773 mist::matrix< T2 > &fundamental21,
774 mist::matrix< T2 > &fundamental31 )
775 {
776
777 // compute epipoles
778 mist::matrix< T2 > epi21, epi31;
779 trifocal_tensor_to_epipoles(tensor, epi21, epi31);
780
781 fundamental21.resize(3, 3);
782 fundamental31.resize(3, 3);
783
784 for (size_t i = 0; i < 3; i++)
785 {
786 mist::matrix< T2 > temp;
787
788 temp = __epipolar__::skew_symmetric_matrix3( epi21 ) * tensor[i] * epi31;
789 for (int j = 0; j < 3; j++)
790 {
791 fundamental21(j, i) = temp(j, 0);
792 }
793
794 temp = __epipolar__::skew_symmetric_matrix3( epi31 ) * tensor[i].t() * epi21;
795 for (int j = 0; j < 3; j++)
796 {
797 fundamental31(j, i) = temp(j, 0);
798 }
799 }
800
801 return true;
802 }
803
804
805
817 template < typename T1, typename T2 >
818 bool trifocal_tensor_to_camera_matrices( trifocal_tensor< T1 > &tensor,
819 mist::matrix< T2 > &mat2,
820 mist::matrix< T2 > &mat3 )
821 {
822
823 mat2.resize( 3, 4 );
824 mat3.resize( 3, 4 );
825 matrix< T1 > vec1( 3, 1 ), vec2( 3, 1 );
826
827 // epipoles
828 matrix< T1 > epi21, epi31;
829 if ( !trifocal_tensor_to_epipoles( tensor, epi21, epi31 ) )
830 {
831 return false;
832 }
833
834
835 // camera matrix for the second camera
836 for (size_t i = 0; i < 3; i++)
837 {
838 vec2 = tensor[i] * epi31;
839 mat2( 0, i ) = vec2[0];
840 mat2( 1, i ) = vec2[1];
841 mat2( 2, i ) = vec2[2];
842 }
843 for (size_t i = 0; i < 3; i++)
844 {
845 mat2( i, 3 ) = epi21[i];
846 }
847
848 // camera matrix for third camera
849 mist::matrix< T1 > temp_mat;
850 temp_mat = epi31 * epi31.t() - matrix< T1 >::_33( 1.0, 0.0, 0.0,
851 0.0, 1.0, 0.0,
852 0.0, 0.0, 1.0 );
853 for (size_t i = 0; i < 3; i++)
854 {
855 vec1 = temp_mat * tensor[i].t() * epi21;
856 mat3( 0, i ) = vec1[0];
857 mat3( 1, i ) = vec1[1];
858 mat3( 2, i ) = vec1[2];
859 }
860 for (size_t i = 0; i < 3; i++)
861 {
862 mat3( i, 3 ) = epi31[i];
863 }
864
865 return true;
866 }
867
868
869 _MIST_END
870
871
872
873
874 #endif // __INCLUDE_MIST_EPIPOLAR__
875