Rev Author Line No. Line
178 kaklik 1 #include <boost/multi_array.hpp>
2 #include <complex>
3 #include <fstream>
4 #include "fourier.h"
5 #include "linalg.h"
6 #include "image_op.h"
7 #include "cameraProjectorCalibration.hh"
8  
9 using namespace boost;
10 using namespace mimas;
11 using namespace std;
12  
13 CameraProjectorCalibration::CameraProjectorCalibration
14 ( const image< unsigned char > &_searchPattern ):
15 searchPattern( _searchPattern ),
16 pointPairsModified(false)
17 {
18 }
19  
20 void CameraProjectorCalibration::addCalibFrame
21 ( const mimas::image< unsigned char > &_frame,
22 const Vector &_pos )
23 {
24 patternFrame.push_back( _frame );
25 pointPairs.push_back( make_pair( _pos, findPatternImg( _frame ) ) );
26 pointPairsModified = true;
27 }
28  
29 CameraProjectorCalibration::Matrix
30 CameraProjectorCalibration::getHomography(void)
31 {
32 if ( pointPairsModified ) {
33 homography = genHomography( pointPairs );
34 pointPairsModified = false;
35 };
36 return homography;
37 }
38  
39 multi_array< double, 2 >::array_view< 2 >::type CameraProjectorCalibration::view
40 ( multi_array< double, 2 > &in, int x, int y, int w, int h )
41 {
42 typedef multi_array< double, 2 >::index_range range;
43 typedef multi_array< double, 2 >::array_view< 2 >::type array_view;
44 multi_array< double, 2 >::index_gen indices;
45 array_view out =
46 in[ indices[ range( y, y + h ) ][ range( x, x + w ) ] ];
47 return out;
48 }
49  
50 CameraProjectorCalibration::Matrix CameraProjectorCalibration::genHomography
51 ( const std::vector< std::pair< Vector, Vector > > &pointPairs )
52 {
53 Matrix system( 2 * pointPairs.size(), 9 );
54 Matrix Vt;
55  
56 // Fill in the matrix to solve the system
57 for(int i=0; i<(signed)pointPairs.size(); i++ ) {
58  
59 double
60 x1 = pointPairs[i].first[0],
61 x2 = pointPairs[i].first[1],
62 xs1 = pointPairs[i].second[0],
63 xs2 = pointPairs[i].second[1];
64  
65 system(2*i,0) = x1;
66 system(2*i,1) = x2;
67 system(2*i,2) = 1;
68 system(2*i,3) = 0;
69 system(2*i,4) = 0;
70 system(2*i,5) = 0;
71 system(2*i,6) = -x1 * xs1;
72 system(2*i,7) = -x2 * xs1;
73 system(2*i,8) = -xs1;
74  
75 system(2*i+1,0) = 0;
76 system(2*i+1,1) = 0;
77 system(2*i+1,2) = 0;
78 system(2*i+1,3) = -x1;
79 system(2*i+1,4) = -x2;
80 system(2*i+1,5) = -1;
81 system(2*i+1,6) = x1 * xs2;
82 system(2*i+1,7) = x2 * xs2;
83 system(2*i+1,8) = xs2;
84 }
85  
86 // calculate the SVD of the matrix A = U.sigma.Vt
87 // we don't need to compute U or sigma.
88 gesvd< double >( system, NULL, &Vt );
89  
90 // the result is the last column of the matrix V, which is the
91 // last line of Vt.
92 Matrix retVal( 3, 3 );
93 // we put the result into the homography matrix
94 retVal(0,0) = Vt(8,0);
95 retVal(0,1) = Vt(8,1);
96 retVal(0,2) = Vt(8,2);
97 retVal(1,0) = Vt(8,3);
98 retVal(1,1) = Vt(8,4);
99 retVal(1,2) = Vt(8,5);
100 retVal(2,0) = Vt(8,6);
101 retVal(2,1) = Vt(8,7);
102 retVal(2,2) = Vt(8,8);
103  
104 return retVal;
105 }
106  
107 CameraProjectorCalibration::Vector CameraProjectorCalibration::findPatternDiffImg
108 ( const image< double > &diffImg,
109 const image< double > &tpl )
110 {
111 multi_array< double, 2 > imgField
112 ( extents[ diffImg.getHeight() + tpl.getWidth() ]
113 [ diffImg.getWidth() + tpl.getHeight() ] );
114 view( imgField, 0, 0, diffImg.getWidth(), diffImg.getHeight() ) =
115 const_multi_array_ref< double, 2 >( diffImg.rawData(),
116 extents[ diffImg.getHeight() ]
117 [ diffImg.getWidth() ] );
118 multi_array< double, 2 > tplField
119 ( extents[ diffImg.getHeight() + tpl.getWidth() ]
120 [ diffImg.getWidth() + tpl.getHeight() ] );
121 view( tplField, 0, 0, tpl.getWidth(), tpl.getHeight() ) =
122 const_multi_array_ref< double, 2 >( tpl.rawData(),
123 extents[ tpl.getHeight() ]
124 [ tpl.getWidth() ] );
125  
126 multi_array< std::complex< double >, 2 > ffff( rfft( tplField ) );
127 multi_array< double, 2 > crossCorrelation
128 ( invrfft( rfft( imgField ) *
129 conj( ffff ) ) );
130 int maxIndex =
131 max_element( crossCorrelation.data(),
132 crossCorrelation.data() + crossCorrelation.num_elements() ) -
133 crossCorrelation.data();
134  
135 Vector retVal( 3 );
136 retVal[0] = maxIndex % crossCorrelation.shape()[1] + tpl.getWidth() / 2;
137 retVal[1] = maxIndex / crossCorrelation.shape()[1] + tpl.getHeight() / 2;
138 retVal[2] = 1.0;
139 return retVal;
140 }
141  
142 CameraProjectorCalibration::Vector CameraProjectorCalibration::findPatternImg
143 ( const image< unsigned char > &img )
144 {
145 assert( searchPattern.initialised() );
146 return findPatternDiffImg( image< double >( img ) -
147 image< double >( backgroundFrame ),
148 image< double >( searchPattern ) );
149 }
150