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

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

geometry.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_GEOMETRY__
34 #define __INCLUDE_GEOMETRY__
35
36 #ifndef __INCLUDE_MIST_MATRIX__
37 #include "matrix.h"
38 #endif
39
40 #ifndef __INCLUDE_MIST_NUMERIC__
41 #include "numeric.h"
42 #endif
43
44 #ifndef __INCLUDE_MIST_VECTOR__
45 #include "vector.h"
46 #endif
47
48 _MIST_BEGIN
49
50
55 template< class T, class Allocator >
56 void translation_matrix( matrix< T, Allocator > &mat, typename matrix< T, Allocator >::value_type x, typename matrix< T, Allocator >::value_type y )
57 {
58 typedef matrix< T > matrix_type;
59 mat = matrix_type::_33(
60 1.0, 0.0, x,
61 0.0, 1.0, y,
62 0.0, 0.0, 1.0
63 );
64 }
65
70 template< class T, class Allocator >
71 void rotation_matrix( matrix< T, Allocator > &mat, typename matrix< T, Allocator >::value_type radian )
72 {
73 typedef matrix< T > matrix_type;
74 mat = matrix_type::_33(
75 cos( radian ), -sin( radian ), 0.0,
76 sin( radian ), cos( radian ), 0.0,
77 0.0, 0.0, 1.0
78 );
79 }
80
87 template< class T, class Allocator >
88 void rotation_matrix( matrix< T, Allocator > &mat, typename matrix< T, Allocator >::value_type radian, typename matrix< T, Allocator >::value_type cx, typename matrix< T, Allocator >::value_type cy )
89 {
90 typedef matrix< T > matrix_type;
91 mat = matrix_type::_33(
92 1.0, 0.0, cx,
93 0.0, 1.0, cy,
94 0.0, 0.0, 1.0
95 );
96 mat *= matrix_type::_33(
97 cos( radian ), -sin( radian ), 0.0,
98 sin( radian ), cos( radian ), 0.0,
99 0.0, 0.0, 1.0
100 );
101 mat *= matrix_type::_33(
102 1.0, 0.0, -cx,
103 0.0, 1.0, -cy,
104 0.0, 0.0, 1.0
105 );
106 }
107
112 template< class T, class Allocator >
113 void scaling_matrix( matrix< T, Allocator > &mat, typename matrix< T, Allocator >::value_type x, typename matrix< T, Allocator >::value_type y )
114 {
115 typedef matrix< T > matrix_type;
116 mat = matrix_type::_33(
117 x, 0.0, 0.0,
118 0.0, y, 0.0,
119 0.0, 0.0, 1.0
120 );
121 }
122
128 template< class T, class Allocator >
129 bool homography_matrix( matrix< T, Allocator > &mat, const matrix< T, Allocator > &p1, const matrix< T, Allocator > &p2 )
130 {
131 if( p1.rows() != 2 || p1.cols() != 4 ||
132 p2.rows() != 2 || p2.cols() != 4 )
133 {
134 throw( numerical_exception( 1, "Incorrect input matrix size is specified." ) );
135 return( false );
136 }
137
138 typedef matrix< double > matrix_type;
139
140 matrix_type tm( 8, 9 );
141 double w = 1.0;
142 for( int i = 0 ; i < 4 ; ++i )
143 {
144 tm( i * 2 + 0, 0 ) = 0;
145 tm( i * 2 + 0, 1 ) = 0;
146 tm( i * 2 + 0, 2 ) = 0;
147 tm( i * 2 + 0, 3 ) = - w * p1( 0, i );
148 tm( i * 2 + 0, 4 ) = - w * p1( 1, i );
149 tm( i * 2 + 0, 5 ) = -w;
150 tm( i * 2 + 0, 6 ) = p2( 1, i ) * p1( 0, i );
151 tm( i * 2 + 0, 7 ) = p2( 1, i ) * p1( 1, i );
152 tm( i * 2 + 0, 8 ) = p2( 1, i ) * w;
153
154 tm( i * 2 + 1, 0 ) = w * p1( 0, i );
155 tm( i * 2 + 1, 1 ) = w * p1( 1, i );
156 tm( i * 2 + 1, 2 ) = w;
157 tm( i * 2 + 1, 3 ) = 0;
158 tm( i * 2 + 1, 4 ) = 0;
159 tm( i * 2 + 1, 5 ) = 0;
160 tm( i * 2 + 1, 6 ) = -p2( 0, i ) * p1( 0, i );
161 tm( i * 2 + 1, 7 ) = -p2( 0, i ) * p1( 1, i );
162 tm( i * 2 + 1, 8 ) = -p2( 0, i ) * w;
163 }
164
165 // Singular Value Decomposition
166 matrix_type u, s, vt;
167 svd( tm, u, s, vt );
168
169 // Compute projection matrix from last row of the matrix
170 mat.resize( 3, 3 );
171 w = 1.0 / vt( 8, 8 );
172 mat( 0, 0 ) = vt( 8, 0 ) * w;
173 mat( 0, 1 ) = vt( 8, 1 ) * w;
174 mat( 0, 2 ) = vt( 8, 2 ) * w;
175
176 mat( 1, 0 ) = vt( 8, 3 ) * w;
177 mat( 1, 1 ) = vt( 8, 4 ) * w;
178 mat( 1, 2 ) = vt( 8, 5 ) * w;
179
180 mat( 2, 0 ) = vt( 8, 6 ) * w;
181 mat( 2, 1 ) = vt( 8, 7 ) * w;
182 mat( 2, 2 ) = vt( 8, 8 ) * w;
183
184 return( true );
185 }
186
187 namespace detail
188 {
189 template< class T, class Allocator >
190 void transform_point( int sx, int sy, double &dx, double &dy, const matrix< T, Allocator > &mat )
191 {
192 dx = ( mat( 0, 0 ) * sx + mat( 0, 1 ) * sy + mat( 0, 2 ) ) / ( mat( 2, 0 ) * sx + mat( 2, 1 ) * sy + mat( 2, 2 ) );
193 dy = ( mat( 1, 0 ) * sx + mat( 1, 1 ) * sy + mat( 1, 2 ) ) / ( mat( 2, 0 ) * sx + mat( 2, 1 ) * sy + mat( 2, 2 ) );
194 }
195
196 template< class T, class Allocator >
197 void clipping( int sx0, int sy0, int sx1, int sy1,
198 int &dx0, int &dy0, int &dx1, int &dy1,
199 const matrix< T, Allocator > &mat )
200 {
201 double tx[ 4 ], ty[ 4 ];
202 transform_point( sx0, sy0, tx[ 0 ], ty[ 0 ], mat );
203 transform_point( sx1, sy0, tx[ 1 ], ty[ 1 ], mat );
204 transform_point( sx0, sy1, tx[ 2 ], ty[ 2 ], mat );
205 transform_point( sx1, sy1, tx[ 3 ], ty[ 3 ], mat );
206
207 dx0 = 1000000;
208 dx1 = 0;
209 dy0 = 1000000;
210 dy1 = 0;
211 for( int i = 0 ; i < 4 ; ++i )
212 {
213 dx0 = ( dx0 < static_cast< int >( tx[ i ] ) ) ? dx0 : static_cast< int >( tx[ i ] );
214 dx1 = ( dx1 > static_cast< int >( tx[ i ] ) ) ? dx1 : static_cast< int >( tx[ i ] );
215 dy0 = ( dy0 < static_cast< int >( ty[ i ] ) ) ? dy0 : static_cast< int >( ty[ i ] );
216 dy1 = ( dy1 > static_cast< int >( ty[ i ] ) ) ? dy1 : static_cast< int >( ty[ i ] );
217 }
218 }
219 }
220
221 namespace nearest
222 {
228 template< class T1, class Allocator1, class T2, class Allocator2 >
229 void transform( const array2< T1, Allocator1 > &in, array2< T1, Allocator1 > &out, const matrix< T2, Allocator2 > &mat )
230 {
231 matrix< double > m = inverse( mat );
232
233 int w = static_cast< int >( out.width() );
234 int h = static_cast< int >( out.height() );
235 int iw = static_cast< int >( in.width( ) );
236 int ih = static_cast< int >( in.height( ) );
237
238 int dx0, dy0, dx1, dy1;
239 detail::clipping( 0, 0, iw, ih, dx0, dy0, dx1, dy1, mat );
240
241 // for( int y = ( ( dy0 > 0 ) ? dy0 : 0 ) ; y < ( ( dy1 < h ) ? dy1 : h ) ; ++y )
242 for( int y = 0 ; y < h ; ++y )
243 {
244 // for( int x = ( ( dx0 > 0 ) ? dx0 : 0 ) ; x < ( ( dx1 < w ) ? dx1 : w ) ; ++x )
245 for( int x = 0 ; x < w ; ++x )
246 {
247 double sx, sy;
248 detail::transform_point( x, y, sx, sy, m );
249 int ix = static_cast< int >( sx );
250 int iy = static_cast< int >( sy );
251
252 if( 0 <= ix && ix < iw && 0 <= iy && iy < ih )
253 {
254 out( x, y ) = in( ix, iy );
255 }
256 }
257 }
258 }
259 }
260
261 namespace linear
262 {
268 template< class T1, class Allocator1, class T2, class Allocator2 >
269 void transform( const array2< T1, Allocator1 > &in, array2< T1, Allocator1 > &out, const matrix< T2, Allocator2 > &mat )
270 {
271 matrix< double > m = inverse( mat );
272
273 int w = static_cast< int >( out.width( ) );
274 int h = static_cast< int >( out.height( ) );
275 int iw = static_cast< int >( in.width( ) );
276 int ih = static_cast< int >( in.height( ) );
277
278 int dx0, dy0, dx1, dy1;
279 detail::clipping( 0, 0, iw, ih, dx0, dy0, dx1, dy1, mat );
280
281 #pragma omp parallel for schedule( guided )
282 for( int y = 0 ; y < h ; ++y )
283 // for( int y = ( ( dy0 > 0 ) ? dy0 : 0 ) ; y < ( ( dy1 < h ) ? dy1 : h ) ; ++y )
284 {
285 for( int x = 0 ; x < w ; ++x )
286 // for( int x = ( ( dx0 > 0 ) ? dx0 : 0 ) ; x < ( ( dx1 < w ) ? dx1 : w ) ; ++x )
287 {
288 double sx, sy;
289 detail::transform_point( x, y, sx, sy, m );
290
291 int ix = static_cast< int >( sx );
292 int iy = static_cast< int >( sy );
293 double dx = sx - ix;
294 double dy = sy - iy;
295
296 if( 0 <= ix && ix < iw - 1 && 0 <= iy && iy < ih - 1 )
297 {
298 typedef typename array2< T1, Allocator1 >::value_type value_type;
299 value_type tmp = in( ix, iy ) * ( 1.0 - dx ) * ( 1.0 - dy )
300 + in( ix + 1, iy ) * dx * ( 1.0 - dy )
301 + in( ix, iy + 1 ) * ( 1.0 - dx ) * dy
302 + in( ix + 1, iy + 1 ) * dx * dy;
303 out( x, y ) = tmp;
304 }
305 }
306 }
307 }
308
319 template< class Value_type1, class Allocator1, class Value_type2, class Allocator2 >
320 void transform( const mist::array2< Value_type1, Allocator1 > &in, mist::array2< Value_type2, Allocator2 > &out, const double &delta_x, const double &delta_y )
321 {
322 vector2< double > factor( out.width() / in.width(), out.height() / in.height() );
323 //out.fill( 0 );
324
325 #pragma omp parallel for schedule( guided )
326 for( int y = 0 ; y < static_cast< int >(out.height()) ; y++ )
327 {
328 size_t ii[ 4 ], jj[ 4 ];
329 double fy = y / factor.y - delta_y;
330 if( fy < 0 )
331 {
332 fy = 0;
333 }
334 else if( fy > static_cast< int >(in.height()) - 1 )
335 {
336 fy = in.height() - 1;
337 }
338
339 jj[ 1 ] = static_cast< size_t >( fy );
340 jj[ 2 ] = jj[ 1 ] < in.height() - 1 ? jj[ 1 ] + 1 : jj[ 1 ];
341 fy -= jj[ 1 ];
342 for( size_t x = 0 ; x < out.width() ; x++ )
343 {
344 double fx = x / factor.x - delta_x;
345
346 if( fx < 0 )
347 {
348 fx = 0;
349 }
350 else if( fx > in.width() - 1 )
351 {
352 fx = in.width() - 1;
353 }
354 ii[ 1 ] = static_cast< size_t >( fx );
355 ii[ 2 ] = ii[ 1 ] < in.width() - 1 ? ii[ 1 ] + 1 : ii[ 1 ];
356 fx -= ii[ 1 ];
357
358 out( x, y ) = mist::__linear__::_linear_< is_color< Value_type2 >::value >::interpolate( in, ii[ 1 ], ii[ 2 ], jj[ 1 ], jj[ 2 ], 0, 0, fx, fy, 0 ) ;
359 }
360 }
361 }
362 }
363
364 namespace cubic
365 {
371 template< class T1, class Allocator1, class T2, class Allocator2 >
372 void transform( const array2< T1, Allocator1 > &in, array2< T1, Allocator1 > &out, const matrix< T2, Allocator2 > &mat )
373 {
374 matrix< double > m = inverse( mat );
375
376 int w = static_cast< int >( out.width( ) );
377 int h = static_cast< int >( out.height( ) );
378 int iw = static_cast< int >( in.width( ) );
379 int ih = static_cast< int >( in.height( ) );
380
381 int dx0, dy0, dx1, dy1;
382 detail::clipping( 0, 0, iw, ih, dx0, dy0, dx1, dy1, mat );
383
384 #pragma omp parallel for schedule( guided )
385 for( int y = 0 ; y < h ; ++y )
386 // for( int y = ( ( dy0 > 0 ) ? dy0 : 0 ) ; y < ( ( dy1 < h ) ? dy1 : h ) ; ++y )
387 {
388 size_t ii[ 4 ], jj[ 4 ], kk[ 4 ];
389 for( int x = 0 ; x < w ; ++x )
390 // for( int x = ( ( dx0 > 0 ) ? dx0 : 0 ) ; x < ( ( dx1 < w ) ? dx1 : w ) ; ++x )
391 {
392 double sx, sy;
393 detail::transform_point( x, y, sx, sy, m );
394
395 int ix = static_cast< int >( sx );
396 int iy = static_cast< int >( sy );
397 jj[ 1 ] = static_cast< size_t >( sy );
398 jj[ 0 ] = jj[ 1 ] > 0 ? jj[ 1 ] - 1 : jj[ 1 ];
399 jj[ 2 ] = jj[ 1 ] < ih - 1 ? jj[ 1 ] + 1 : jj[ 1 ];
400 jj[ 3 ] = jj[ 2 ] < ih - 1 ? jj[ 2 ] + 1 : jj[ 2 ];
401 sy -= jj[ 1 ];
402
403 ii[ 1 ] = static_cast< size_t >( sx );
404 ii[ 0 ] = ii[ 1 ] > 0 ? ii[ 1 ] - 1 : ii[ 1 ];
405 ii[ 2 ] = ii[ 1 ] < iw - 1 ? ii[ 1 ] + 1 : ii[ 1 ];
406 ii[ 3 ] = ii[ 2 ] < iw - 1 ? ii[ 2 ] + 1 : ii[ 2 ];
407 sx -= ii[ 1 ];
408
409 if( 0 <= ix && ix < iw && 0 <= iy && iy < ih )
410 {
411 out( x, y ) = mist::__cubic__::_cubic_< false >::interpolate( in, ii, jj, kk, sx, sy, 0 ) ;
412 }
413 }
414 }
415 }
416
427 template< class Value_type1, class Allocator1, class Value_type2, class Allocator2 >
428 void transform( const mist::array2< Value_type1, Allocator1 > &in, mist::array2< Value_type2, Allocator2 > &out, const double &delta_x, const double &delta_y )
429 {
430 vector2< double > factor( out.width() / in.width(), out.height() / in.height() );
431 //out.fill( 0 );
432 #pragma omp parallel for schedule( guided )
433 for( int y = 0 ; y < static_cast< int >(out.height()) ; y++ )
434 {
435 size_t ii[ 4 ], jj[ 4 ], kk[ 4 ];
436 double fy = y / factor.y - delta_y;
437 if( fy < 0 )
438 {
439 fy = 0;
440 }
441 else if( fy > static_cast< int >(in.height()) - 1 )
442 {
443 fy = in.height() - 1;
444 }
445
446 jj[ 1 ] = static_cast< size_t >( fy );
447 jj[ 0 ] = jj[ 1 ] > 0 ? jj[ 1 ] - 1 : jj[ 1 ];
448 jj[ 2 ] = jj[ 1 ] < in.height() - 1 ? jj[ 1 ] + 1 : jj[ 1 ];
449 jj[ 3 ] = jj[ 2 ] < in.height() - 1 ? jj[ 2 ] + 1 : jj[ 2 ];
450 fy -= jj[ 1 ];
451 for( size_t x = 0 ; x < out.width() ; x++ )
452 {
453 double fx = x / factor.x - delta_x;
454
455 if( fx < 0 )
456 {
457 fx = 0;
458 }
459 else if( fx > in.width() - 1 )
460 {
461 fx = in.width() - 1;
462 }
463 ii[ 1 ] = static_cast< size_t >( fx );
464 ii[ 0 ] = ii[ 1 ] > 0 ? ii[ 1 ] - 1 : ii[ 1 ];
465 ii[ 2 ] = ii[ 1 ] < in.width() - 1 ? ii[ 1 ] + 1 : ii[ 1 ];
466 ii[ 3 ] = ii[ 2 ] < in.width() - 1 ? ii[ 2 ] + 1 : ii[ 2 ];
467 fx -= ii[ 1 ];
468
469 out( x, y ) = mist::__cubic__::_cubic_< is_color< Value_type2 >::value >::interpolate( in, ii, jj, kk, fx, fy, 0 ) ;
470 }
471 }
472 }
473 }
474
475 _MIST_END
476
477
478 #endif
479