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 
Generated on Wed Nov 12 2014 19:44:15 for MIST by doxygen 1.8.1.2