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 |
|