1 /*M///////////////////////////////////////////////////////////////////////////////////////
2 //
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
4 //
5 // By downloading, copying, installing or using the software you agree to this license.
6 // If you do not agree to this license, do not download, install,
7 // copy or use the software.
8 //
9 //
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
15 //
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
18 //
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
21 //
22 // * Redistribution's in binary form must reproduce the above copyright notice,
23 // this list of conditions and the following disclaimer in the documentation
24 // and/or other materials provided with the distribution.
25 //
26 // * The name of Intel Corporation may not be used to endorse or promote products
27 // derived from this software without specific prior written permission.
28 //
29 // This software is provided by the copyright holders and contributors "as is" and
30 // any express or implied warranties, including, but not limited to, the implied
31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
32 // In no event shall the Intel Corporation or contributors be liable for any direct,
33 // indirect, incidental, special, exemplary, or consequential damages
34 // (including, but not limited to, procurement of substitute goods or services;
35 // loss of use, data, or profits; or business interruption) however caused
36 // and on any theory of liability, whether in contract, strict liability,
37 // or tort (including negligence or otherwise) arising in any way out of
38 // the use of this software, even if advised of the possibility of such damage.
39 //
40 //M*/
41
42 #include "_cv.h"
43
44 /**************************************************************************************\
45 * line samplers *
46 \**************************************************************************************/
47
48 CV_IMPL int
cvSampleLine(const void * img,CvPoint pt1,CvPoint pt2,void * _buffer,int connectivity)49 cvSampleLine( const void* img, CvPoint pt1, CvPoint pt2,
50 void* _buffer, int connectivity )
51 {
52 int count = -1;
53
54 CV_FUNCNAME( "cvSampleLine" );
55
56 __BEGIN__;
57
58 int i, coi = 0, pix_size;
59 CvMat stub, *mat = (CvMat*)img;
60 CvLineIterator iterator;
61 uchar* buffer = (uchar*)_buffer;
62
63 CV_CALL( mat = cvGetMat( mat, &stub, &coi ));
64
65 if( coi != 0 )
66 CV_ERROR( CV_BadCOI, "" );
67
68 if( !buffer )
69 CV_ERROR( CV_StsNullPtr, "" );
70
71 CV_CALL( count = cvInitLineIterator( mat, pt1, pt2, &iterator, connectivity ));
72
73 pix_size = CV_ELEM_SIZE(mat->type);
74 for( i = 0; i < count; i++ )
75 {
76 CV_MEMCPY_AUTO( buffer, iterator.ptr, pix_size );
77 buffer += pix_size;
78 CV_NEXT_LINE_POINT( iterator );
79 }
80
81 __END__;
82
83 return count;
84 }
85
86
87 static const void*
icvAdjustRect(const void * srcptr,int src_step,int pix_size,CvSize src_size,CvSize win_size,CvPoint ip,CvRect * pRect)88 icvAdjustRect( const void* srcptr, int src_step, int pix_size,
89 CvSize src_size, CvSize win_size,
90 CvPoint ip, CvRect* pRect )
91 {
92 CvRect rect;
93 const char* src = (const char*)srcptr;
94
95 if( ip.x >= 0 )
96 {
97 src += ip.x*pix_size;
98 rect.x = 0;
99 }
100 else
101 {
102 rect.x = -ip.x;
103 if( rect.x > win_size.width )
104 rect.x = win_size.width;
105 }
106
107 if( ip.x + win_size.width < src_size.width )
108 rect.width = win_size.width;
109 else
110 {
111 rect.width = src_size.width - ip.x - 1;
112 if( rect.width < 0 )
113 {
114 src += rect.width*pix_size;
115 rect.width = 0;
116 }
117 assert( rect.width <= win_size.width );
118 }
119
120 if( ip.y >= 0 )
121 {
122 src += ip.y * src_step;
123 rect.y = 0;
124 }
125 else
126 rect.y = -ip.y;
127
128 if( ip.y + win_size.height < src_size.height )
129 rect.height = win_size.height;
130 else
131 {
132 rect.height = src_size.height - ip.y - 1;
133 if( rect.height < 0 )
134 {
135 src += rect.height*src_step;
136 rect.height = 0;
137 }
138 }
139
140 *pRect = rect;
141 return src - rect.x*pix_size;
142 }
143
144
145 #define ICV_DEF_GET_RECT_SUB_PIX_FUNC( flavor, srctype, dsttype, worktype, \
146 cast_macro, scale_macro, cast_macro2 )\
147 CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C1R \
148 ( const srctype* src, int src_step, CvSize src_size, \
149 dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center ) \
150 { \
151 CvPoint ip; \
152 worktype a11, a12, a21, a22, b1, b2; \
153 float a, b; \
154 int i, j; \
155 \
156 center.x -= (win_size.width-1)*0.5f; \
157 center.y -= (win_size.height-1)*0.5f; \
158 \
159 ip.x = cvFloor( center.x ); \
160 ip.y = cvFloor( center.y ); \
161 \
162 a = center.x - ip.x; \
163 b = center.y - ip.y; \
164 a11 = scale_macro((1.f-a)*(1.f-b)); \
165 a12 = scale_macro(a*(1.f-b)); \
166 a21 = scale_macro((1.f-a)*b); \
167 a22 = scale_macro(a*b); \
168 b1 = scale_macro(1.f - b); \
169 b2 = scale_macro(b); \
170 \
171 src_step /= sizeof(src[0]); \
172 dst_step /= sizeof(dst[0]); \
173 \
174 if( 0 <= ip.x && ip.x + win_size.width < src_size.width && \
175 0 <= ip.y && ip.y + win_size.height < src_size.height ) \
176 { \
177 /* extracted rectangle is totally inside the image */ \
178 src += ip.y * src_step + ip.x; \
179 \
180 if( icvCopySubpix_##flavor##_C1R_p && \
181 icvCopySubpix_##flavor##_C1R_p( src, src_step*sizeof(src[0]), \
182 dst, dst_step*sizeof(dst[0]), \
183 win_size, a, b ) >= 0 ) \
184 return CV_OK; \
185 \
186 for( i = 0; i < win_size.height; i++, src += src_step, \
187 dst += dst_step ) \
188 { \
189 for( j = 0; j <= win_size.width - 2; j += 2 ) \
190 { \
191 worktype s0 = cast_macro(src[j])*a11 + \
192 cast_macro(src[j+1])*a12 + \
193 cast_macro(src[j+src_step])*a21 + \
194 cast_macro(src[j+src_step+1])*a22; \
195 worktype s1 = cast_macro(src[j+1])*a11 + \
196 cast_macro(src[j+2])*a12 + \
197 cast_macro(src[j+src_step+1])*a21 + \
198 cast_macro(src[j+src_step+2])*a22; \
199 \
200 dst[j] = (dsttype)cast_macro2(s0); \
201 dst[j+1] = (dsttype)cast_macro2(s1); \
202 } \
203 \
204 for( ; j < win_size.width; j++ ) \
205 { \
206 worktype s0 = cast_macro(src[j])*a11 + \
207 cast_macro(src[j+1])*a12 + \
208 cast_macro(src[j+src_step])*a21 + \
209 cast_macro(src[j+src_step+1])*a22; \
210 \
211 dst[j] = (dsttype)cast_macro2(s0); \
212 } \
213 } \
214 } \
215 else \
216 { \
217 CvRect r; \
218 \
219 src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src), \
220 sizeof(*src), src_size, win_size,ip, &r); \
221 \
222 for( i = 0; i < win_size.height; i++, dst += dst_step ) \
223 { \
224 const srctype *src2 = src + src_step; \
225 \
226 if( i < r.y || i >= r.height ) \
227 src2 -= src_step; \
228 \
229 for( j = 0; j < r.x; j++ ) \
230 { \
231 worktype s0 = cast_macro(src[r.x])*b1 + \
232 cast_macro(src2[r.x])*b2; \
233 \
234 dst[j] = (dsttype)cast_macro2(s0); \
235 } \
236 \
237 for( ; j < r.width; j++ ) \
238 { \
239 worktype s0 = cast_macro(src[j])*a11 + \
240 cast_macro(src[j+1])*a12 + \
241 cast_macro(src2[j])*a21 + \
242 cast_macro(src2[j+1])*a22; \
243 \
244 dst[j] = (dsttype)cast_macro2(s0); \
245 } \
246 \
247 for( ; j < win_size.width; j++ ) \
248 { \
249 worktype s0 = cast_macro(src[r.width])*b1 + \
250 cast_macro(src2[r.width])*b2; \
251 \
252 dst[j] = (dsttype)cast_macro2(s0); \
253 } \
254 \
255 if( i < r.height ) \
256 src = src2; \
257 } \
258 } \
259 \
260 return CV_OK; \
261 }
262
263
264 #define ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( flavor, srctype, dsttype, worktype, \
265 cast_macro, scale_macro, mul_macro )\
266 static CvStatus CV_STDCALL icvGetRectSubPix_##flavor##_C3R \
267 ( const srctype* src, int src_step, CvSize src_size, \
268 dsttype* dst, int dst_step, CvSize win_size, CvPoint2D32f center ) \
269 { \
270 CvPoint ip; \
271 worktype a, b; \
272 int i, j; \
273 \
274 center.x -= (win_size.width-1)*0.5f; \
275 center.y -= (win_size.height-1)*0.5f; \
276 \
277 ip.x = cvFloor( center.x ); \
278 ip.y = cvFloor( center.y ); \
279 \
280 a = scale_macro( center.x - ip.x ); \
281 b = scale_macro( center.y - ip.y ); \
282 \
283 src_step /= sizeof( src[0] ); \
284 dst_step /= sizeof( dst[0] ); \
285 \
286 if( 0 <= ip.x && ip.x + win_size.width < src_size.width && \
287 0 <= ip.y && ip.y + win_size.height < src_size.height ) \
288 { \
289 /* extracted rectangle is totally inside the image */ \
290 src += ip.y * src_step + ip.x*3; \
291 \
292 for( i = 0; i < win_size.height; i++, src += src_step, \
293 dst += dst_step ) \
294 { \
295 for( j = 0; j < win_size.width; j++ ) \
296 { \
297 worktype s0 = cast_macro(src[j*3]); \
298 worktype s1 = cast_macro(src[j*3 + src_step]); \
299 s0 += mul_macro( a, (cast_macro(src[j*3+3]) - s0)); \
300 s1 += mul_macro( a, (cast_macro(src[j*3+3+src_step]) - s1));\
301 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
302 \
303 s0 = cast_macro(src[j*3+1]); \
304 s1 = cast_macro(src[j*3+1 + src_step]); \
305 s0 += mul_macro( a, (cast_macro(src[j*3+4]) - s0)); \
306 s1 += mul_macro( a, (cast_macro(src[j*3+4+src_step]) - s1));\
307 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
308 \
309 s0 = cast_macro(src[j*3+2]); \
310 s1 = cast_macro(src[j*3+2 + src_step]); \
311 s0 += mul_macro( a, (cast_macro(src[j*3+5]) - s0)); \
312 s1 += mul_macro( a, (cast_macro(src[j*3+5+src_step]) - s1));\
313 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
314 } \
315 } \
316 } \
317 else \
318 { \
319 CvRect r; \
320 \
321 src = (const srctype*)icvAdjustRect( src, src_step*sizeof(*src), \
322 sizeof(*src)*3, src_size, win_size, ip, &r ); \
323 \
324 for( i = 0; i < win_size.height; i++, dst += dst_step ) \
325 { \
326 const srctype *src2 = src + src_step; \
327 \
328 if( i < r.y || i >= r.height ) \
329 src2 -= src_step; \
330 \
331 for( j = 0; j < r.x; j++ ) \
332 { \
333 worktype s0 = cast_macro(src[r.x*3]); \
334 worktype s1 = cast_macro(src2[r.x*3]); \
335 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
336 \
337 s0 = cast_macro(src[r.x*3+1]); \
338 s1 = cast_macro(src2[r.x*3+1]); \
339 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
340 \
341 s0 = cast_macro(src[r.x*3+2]); \
342 s1 = cast_macro(src2[r.x*3+2]); \
343 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
344 } \
345 \
346 for( ; j < r.width; j++ ) \
347 { \
348 worktype s0 = cast_macro(src[j*3]); \
349 worktype s1 = cast_macro(src2[j*3]); \
350 s0 += mul_macro( a, (cast_macro(src[j*3 + 3]) - s0)); \
351 s1 += mul_macro( a, (cast_macro(src2[j*3 + 3]) - s1)); \
352 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
353 \
354 s0 = cast_macro(src[j*3+1]); \
355 s1 = cast_macro(src2[j*3+1]); \
356 s0 += mul_macro( a, (cast_macro(src[j*3 + 4]) - s0)); \
357 s1 += mul_macro( a, (cast_macro(src2[j*3 + 4]) - s1)); \
358 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
359 \
360 s0 = cast_macro(src[j*3+2]); \
361 s1 = cast_macro(src2[j*3+2]); \
362 s0 += mul_macro( a, (cast_macro(src[j*3 + 5]) - s0)); \
363 s1 += mul_macro( a, (cast_macro(src2[j*3 + 5]) - s1)); \
364 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
365 } \
366 \
367 for( ; j < win_size.width; j++ ) \
368 { \
369 worktype s0 = cast_macro(src[r.width*3]); \
370 worktype s1 = cast_macro(src2[r.width*3]); \
371 dst[j*3] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
372 \
373 s0 = cast_macro(src[r.width*3+1]); \
374 s1 = cast_macro(src2[r.width*3+1]); \
375 dst[j*3+1] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
376 \
377 s0 = cast_macro(src[r.width*3+2]); \
378 s1 = cast_macro(src2[r.width*3+2]); \
379 dst[j*3+2] = (dsttype)(s0 + mul_macro( b, (s1 - s0))); \
380 } \
381 \
382 if( i < r.height ) \
383 src = src2; \
384 } \
385 } \
386 \
387 return CV_OK; \
388 }
389
390
391
icvGetRectSubPix_8u32f_C1R(const uchar * src,int src_step,CvSize src_size,float * dst,int dst_step,CvSize win_size,CvPoint2D32f center)392 CvStatus CV_STDCALL icvGetRectSubPix_8u32f_C1R
393 ( const uchar* src, int src_step, CvSize src_size,
394 float* dst, int dst_step, CvSize win_size, CvPoint2D32f center )
395 {
396 CvPoint ip;
397 float a12, a22, b1, b2;
398 float a, b;
399 double s = 0;
400 int i, j;
401
402 center.x -= (win_size.width-1)*0.5f;
403 center.y -= (win_size.height-1)*0.5f;
404
405 ip.x = cvFloor( center.x );
406 ip.y = cvFloor( center.y );
407
408 if( win_size.width <= 0 || win_size.height <= 0 )
409 return CV_BADRANGE_ERR;
410
411 a = center.x - ip.x;
412 b = center.y - ip.y;
413 a = MAX(a,0.0001f);
414 a12 = a*(1.f-b);
415 a22 = a*b;
416 b1 = 1.f - b;
417 b2 = b;
418 s = (1. - a)/a;
419
420 src_step /= sizeof(src[0]);
421 dst_step /= sizeof(dst[0]);
422
423 if( 0 <= ip.x && ip.x + win_size.width < src_size.width &&
424 0 <= ip.y && ip.y + win_size.height < src_size.height )
425 {
426 // extracted rectangle is totally inside the image
427 src += ip.y * src_step + ip.x;
428
429 #if 0
430 if( icvCopySubpix_8u32f_C1R_p &&
431 icvCopySubpix_8u32f_C1R_p( src, src_step, dst,
432 dst_step*sizeof(dst[0]), win_size, a, b ) >= 0 )
433 return CV_OK;
434 #endif
435
436 for( ; win_size.height--; src += src_step, dst += dst_step )
437 {
438 float prev = (1 - a)*(b1*CV_8TO32F(src[0]) + b2*CV_8TO32F(src[src_step]));
439 for( j = 0; j < win_size.width; j++ )
440 {
441 float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src[j+1+src_step]);
442 dst[j] = prev + t;
443 prev = (float)(t*s);
444 }
445 }
446 }
447 else
448 {
449 CvRect r;
450
451 src = (const uchar*)icvAdjustRect( src, src_step*sizeof(*src),
452 sizeof(*src), src_size, win_size,ip, &r);
453
454 for( i = 0; i < win_size.height; i++, dst += dst_step )
455 {
456 const uchar *src2 = src + src_step;
457
458 if( i < r.y || i >= r.height )
459 src2 -= src_step;
460
461 for( j = 0; j < r.x; j++ )
462 {
463 float s0 = CV_8TO32F(src[r.x])*b1 +
464 CV_8TO32F(src2[r.x])*b2;
465
466 dst[j] = (float)(s0);
467 }
468
469 if( j < r.width )
470 {
471 float prev = (1 - a)*(b1*CV_8TO32F(src[j]) + b2*CV_8TO32F(src2[j]));
472
473 for( ; j < r.width; j++ )
474 {
475 float t = a12*CV_8TO32F(src[j+1]) + a22*CV_8TO32F(src2[j+1]);
476 dst[j] = prev + t;
477 prev = (float)(t*s);
478 }
479 }
480
481 for( ; j < win_size.width; j++ )
482 {
483 float s0 = CV_8TO32F(src[r.width])*b1 +
484 CV_8TO32F(src2[r.width])*b2;
485
486 dst[j] = (float)(s0);
487 }
488
489 if( i < r.height )
490 src = src2;
491 }
492 }
493
494 return CV_OK;
495 }
496
497
498
499 #define ICV_SHIFT 16
500 #define ICV_SCALE(x) cvRound((x)*(1 << ICV_SHIFT))
501 #define ICV_MUL_SCALE(x,y) (((x)*(y) + (1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
502 #define ICV_DESCALE(x) (((x)+(1 << (ICV_SHIFT-1))) >> ICV_SHIFT)
503
504 icvCopySubpix_8u_C1R_t icvCopySubpix_8u_C1R_p = 0;
505 icvCopySubpix_8u32f_C1R_t icvCopySubpix_8u32f_C1R_p = 0;
506 icvCopySubpix_32f_C1R_t icvCopySubpix_32f_C1R_p = 0;
507
508 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_DESCALE )
509 //ICV_DEF_GET_RECT_SUB_PIX_FUNC( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_NOP )
510 ICV_DEF_GET_RECT_SUB_PIX_FUNC( 32f, float, float, float, CV_NOP, CV_NOP, CV_NOP )
511
512 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u, uchar, uchar, int, CV_NOP, ICV_SCALE, ICV_MUL_SCALE )
513 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 8u32f, uchar, float, float, CV_8TO32F, CV_NOP, CV_MUL )
514 ICV_DEF_GET_RECT_SUB_PIX_FUNC_C3( 32f, float, float, float, CV_NOP, CV_NOP, CV_MUL )
515
516
517 #define ICV_DEF_INIT_SUBPIX_TAB( FUNCNAME, FLAG ) \
518 static void icvInit##FUNCNAME##FLAG##Table( CvFuncTable* tab ) \
519 { \
520 tab->fn_2d[CV_8U] = (void*)icv##FUNCNAME##_8u_##FLAG; \
521 tab->fn_2d[CV_32F] = (void*)icv##FUNCNAME##_32f_##FLAG; \
522 \
523 tab->fn_2d[1] = (void*)icv##FUNCNAME##_8u32f_##FLAG; \
524 }
525
526
527 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C1R )
528 ICV_DEF_INIT_SUBPIX_TAB( GetRectSubPix, C3R )
529
530 typedef CvStatus (CV_STDCALL *CvGetRectSubPixFunc)( const void* src, int src_step,
531 CvSize src_size, void* dst,
532 int dst_step, CvSize win_size,
533 CvPoint2D32f center );
534
535 CV_IMPL void
cvGetRectSubPix(const void * srcarr,void * dstarr,CvPoint2D32f center)536 cvGetRectSubPix( const void* srcarr, void* dstarr, CvPoint2D32f center )
537 {
538 static CvFuncTable gr_tab[2];
539 static int inittab = 0;
540 CV_FUNCNAME( "cvGetRectSubPix" );
541
542 __BEGIN__;
543
544 CvMat srcstub, *src = (CvMat*)srcarr;
545 CvMat dststub, *dst = (CvMat*)dstarr;
546 CvSize src_size, dst_size;
547 CvGetRectSubPixFunc func;
548 int cn, src_step, dst_step;
549
550 if( !inittab )
551 {
552 icvInitGetRectSubPixC1RTable( gr_tab + 0 );
553 icvInitGetRectSubPixC3RTable( gr_tab + 1 );
554 inittab = 1;
555 }
556
557 if( !CV_IS_MAT(src))
558 CV_CALL( src = cvGetMat( src, &srcstub ));
559
560 if( !CV_IS_MAT(dst))
561 CV_CALL( dst = cvGetMat( dst, &dststub ));
562
563 cn = CV_MAT_CN( src->type );
564
565 if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
566 CV_ERROR( CV_StsUnsupportedFormat, "" );
567
568 src_size = cvGetMatSize( src );
569 dst_size = cvGetMatSize( dst );
570 src_step = src->step ? src->step : CV_STUB_STEP;
571 dst_step = dst->step ? dst->step : CV_STUB_STEP;
572
573 if( dst_size.width > src_size.width || dst_size.height > src_size.height )
574 CV_ERROR( CV_StsBadSize, "destination ROI must be smaller than source ROI" );
575
576 if( CV_ARE_DEPTHS_EQ( src, dst ))
577 {
578 func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
579 }
580 else
581 {
582 if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
583 CV_ERROR( CV_StsUnsupportedFormat, "" );
584
585 func = (CvGetRectSubPixFunc)(gr_tab[cn != 1].fn_2d[1]);
586 }
587
588 if( !func )
589 CV_ERROR( CV_StsUnsupportedFormat, "" );
590
591 IPPI_CALL( func( src->data.ptr, src_step, src_size,
592 dst->data.ptr, dst_step, dst_size, center ));
593
594 __END__;
595 }
596
597
598 #define ICV_32F8U(x) ((uchar)cvRound(x))
599
600 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( flavor, srctype, dsttype, \
601 worktype, cast_macro, cvt ) \
602 CvStatus CV_STDCALL \
603 icvGetQuadrangleSubPix_##flavor##_C1R \
604 ( const srctype * src, int src_step, CvSize src_size, \
605 dsttype *dst, int dst_step, CvSize win_size, const float *matrix ) \
606 { \
607 int x, y; \
608 double dx = (win_size.width - 1)*0.5; \
609 double dy = (win_size.height - 1)*0.5; \
610 double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
611 double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
612 \
613 src_step /= sizeof(srctype); \
614 dst_step /= sizeof(dsttype); \
615 \
616 for( y = 0; y < win_size.height; y++, dst += dst_step ) \
617 { \
618 double xs = A12*y + A13; \
619 double ys = A22*y + A23; \
620 double xe = A11*(win_size.width-1) + A12*y + A13; \
621 double ye = A21*(win_size.width-1) + A22*y + A23; \
622 \
623 if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) && \
624 (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) && \
625 (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) && \
626 (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3)) \
627 { \
628 for( x = 0; x < win_size.width; x++ ) \
629 { \
630 int ixs = cvFloor( xs ); \
631 int iys = cvFloor( ys ); \
632 const srctype *ptr = src + src_step*iys + ixs; \
633 double a = xs - ixs, b = ys - iys, a1 = 1.f - a; \
634 worktype p0 = cvt(ptr[0])*a1 + cvt(ptr[1])*a; \
635 worktype p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+1])*a;\
636 xs += A11; \
637 ys += A21; \
638 \
639 dst[x] = cast_macro(p0 + b * (p1 - p0)); \
640 } \
641 } \
642 else \
643 { \
644 for( x = 0; x < win_size.width; x++ ) \
645 { \
646 int ixs = cvFloor( xs ), iys = cvFloor( ys ); \
647 double a = xs - ixs, b = ys - iys, a1 = 1.f - a; \
648 const srctype *ptr0, *ptr1; \
649 worktype p0, p1; \
650 xs += A11; ys += A21; \
651 \
652 if( (unsigned)iys < (unsigned)(src_size.height-1) ) \
653 ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step; \
654 else \
655 ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
656 \
657 if( (unsigned)ixs < (unsigned)(src_size.width-1) ) \
658 { \
659 p0 = cvt(ptr0[ixs])*a1 + cvt(ptr0[ixs+1])*a; \
660 p1 = cvt(ptr1[ixs])*a1 + cvt(ptr1[ixs+1])*a; \
661 } \
662 else \
663 { \
664 ixs = ixs < 0 ? 0 : src_size.width - 1; \
665 p0 = cvt(ptr0[ixs]); p1 = cvt(ptr1[ixs]); \
666 } \
667 dst[x] = cast_macro(p0 + b * (p1 - p0)); \
668 } \
669 } \
670 } \
671 \
672 return CV_OK; \
673 }
674
675
676 #define ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( flavor, srctype, dsttype, \
677 worktype, cast_macro, cvt ) \
678 static CvStatus CV_STDCALL \
679 icvGetQuadrangleSubPix_##flavor##_C3R \
680 ( const srctype * src, int src_step, CvSize src_size, \
681 dsttype *dst, int dst_step, CvSize win_size, const float *matrix ) \
682 { \
683 int x, y; \
684 double dx = (win_size.width - 1)*0.5; \
685 double dy = (win_size.height - 1)*0.5; \
686 double A11 = matrix[0], A12 = matrix[1], A13 = matrix[2]-A11*dx-A12*dy; \
687 double A21 = matrix[3], A22 = matrix[4], A23 = matrix[5]-A21*dx-A22*dy; \
688 \
689 src_step /= sizeof(srctype); \
690 dst_step /= sizeof(dsttype); \
691 \
692 for( y = 0; y < win_size.height; y++, dst += dst_step ) \
693 { \
694 double xs = A12*y + A13; \
695 double ys = A22*y + A23; \
696 double xe = A11*(win_size.width-1) + A12*y + A13; \
697 double ye = A21*(win_size.width-1) + A22*y + A23; \
698 \
699 if( (unsigned)(cvFloor(xs)-1) < (unsigned)(src_size.width - 3) && \
700 (unsigned)(cvFloor(ys)-1) < (unsigned)(src_size.height - 3) && \
701 (unsigned)(cvFloor(xe)-1) < (unsigned)(src_size.width - 3) && \
702 (unsigned)(cvFloor(ye)-1) < (unsigned)(src_size.height - 3)) \
703 { \
704 for( x = 0; x < win_size.width; x++ ) \
705 { \
706 int ixs = cvFloor( xs ); \
707 int iys = cvFloor( ys ); \
708 const srctype *ptr = src + src_step*iys + ixs*3; \
709 double a = xs - ixs, b = ys - iys, a1 = 1.f - a; \
710 worktype p0, p1; \
711 xs += A11; \
712 ys += A21; \
713 \
714 p0 = cvt(ptr[0])*a1 + cvt(ptr[3])*a; \
715 p1 = cvt(ptr[src_step])*a1 + cvt(ptr[src_step+3])*a; \
716 dst[x*3] = cast_macro(p0 + b * (p1 - p0)); \
717 \
718 p0 = cvt(ptr[1])*a1 + cvt(ptr[4])*a; \
719 p1 = cvt(ptr[src_step+1])*a1 + cvt(ptr[src_step+4])*a; \
720 dst[x*3+1] = cast_macro(p0 + b * (p1 - p0)); \
721 \
722 p0 = cvt(ptr[2])*a1 + cvt(ptr[5])*a; \
723 p1 = cvt(ptr[src_step+2])*a1 + cvt(ptr[src_step+5])*a; \
724 dst[x*3+2] = cast_macro(p0 + b * (p1 - p0)); \
725 } \
726 } \
727 else \
728 { \
729 for( x = 0; x < win_size.width; x++ ) \
730 { \
731 int ixs = cvFloor(xs), iys = cvFloor(ys); \
732 double a = xs - ixs, b = ys - iys; \
733 const srctype *ptr0, *ptr1; \
734 xs += A11; ys += A21; \
735 \
736 if( (unsigned)iys < (unsigned)(src_size.height-1) ) \
737 ptr0 = src + src_step*iys, ptr1 = ptr0 + src_step; \
738 else \
739 ptr0 = ptr1 = src + (iys < 0 ? 0 : src_size.height-1)*src_step; \
740 \
741 if( (unsigned)ixs < (unsigned)(src_size.width - 1) ) \
742 { \
743 double a1 = 1.f - a; \
744 worktype p0, p1; \
745 ptr0 += ixs*3; ptr1 += ixs*3; \
746 p0 = cvt(ptr0[0])*a1 + cvt(ptr0[3])*a; \
747 p1 = cvt(ptr1[0])*a1 + cvt(ptr1[3])*a; \
748 dst[x*3] = cast_macro(p0 + b * (p1 - p0)); \
749 \
750 p0 = cvt(ptr0[1])*a1 + cvt(ptr0[4])*a; \
751 p1 = cvt(ptr1[1])*a1 + cvt(ptr1[4])*a; \
752 dst[x*3+1] = cast_macro(p0 + b * (p1 - p0)); \
753 \
754 p0 = cvt(ptr0[2])*a1 + cvt(ptr0[5])*a; \
755 p1 = cvt(ptr1[2])*a1 + cvt(ptr1[5])*a; \
756 dst[x*3+2] = cast_macro(p0 + b * (p1 - p0)); \
757 } \
758 else \
759 { \
760 double b1 = 1.f - b; \
761 ixs = ixs < 0 ? 0 : src_size.width - 1; \
762 ptr0 += ixs*3; ptr1 += ixs*3; \
763 \
764 dst[x*3] = cast_macro(cvt(ptr0[0])*b1 + cvt(ptr1[0])*b);\
765 dst[x*3+1]=cast_macro(cvt(ptr0[1])*b1 + cvt(ptr1[1])*b);\
766 dst[x*3+2]=cast_macro(cvt(ptr0[2])*b1 + cvt(ptr1[2])*b);\
767 } \
768 } \
769 } \
770 } \
771 \
772 return CV_OK; \
773 }
774
775
776 /*#define srctype uchar
777 #define dsttype uchar
778 #define worktype float
779 #define cvt CV_8TO32F
780 #define cast_macro ICV_32F8U
781
782 #undef srctype
783 #undef dsttype
784 #undef worktype
785 #undef cvt
786 #undef cast_macro*/
787
788 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
789 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 32f, float, float, double, CV_CAST_32F, CV_NOP )
790 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
791
792 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u, uchar, uchar, double, ICV_32F8U, CV_8TO32F )
793 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 32f, float, float, double, CV_CAST_32F, CV_NOP )
794 ICV_DEF_GET_QUADRANGLE_SUB_PIX_FUNC_C3( 8u32f, uchar, float, double, CV_CAST_32F, CV_8TO32F )
795
796 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C1R )
797 ICV_DEF_INIT_SUBPIX_TAB( GetQuadrangleSubPix, C3R )
798
799 typedef CvStatus (CV_STDCALL *CvGetQuadrangleSubPixFunc)(
800 const void* src, int src_step,
801 CvSize src_size, void* dst,
802 int dst_step, CvSize win_size,
803 const float* matrix );
804
805 CV_IMPL void
cvGetQuadrangleSubPix(const void * srcarr,void * dstarr,const CvMat * mat)806 cvGetQuadrangleSubPix( const void* srcarr, void* dstarr, const CvMat* mat )
807 {
808 static CvFuncTable gq_tab[2];
809 static int inittab = 0;
810 CV_FUNCNAME( "cvGetQuadrangleSubPix" );
811
812 __BEGIN__;
813
814 CvMat srcstub, *src = (CvMat*)srcarr;
815 CvMat dststub, *dst = (CvMat*)dstarr;
816 CvSize src_size, dst_size;
817 CvGetQuadrangleSubPixFunc func;
818 float m[6];
819 int k, cn;
820
821 if( !inittab )
822 {
823 icvInitGetQuadrangleSubPixC1RTable( gq_tab + 0 );
824 icvInitGetQuadrangleSubPixC3RTable( gq_tab + 1 );
825 inittab = 1;
826 }
827
828 if( !CV_IS_MAT(src))
829 CV_CALL( src = cvGetMat( src, &srcstub ));
830
831 if( !CV_IS_MAT(dst))
832 CV_CALL( dst = cvGetMat( dst, &dststub ));
833
834 if( !CV_IS_MAT(mat))
835 CV_ERROR( CV_StsBadArg, "map matrix is not valid" );
836
837 cn = CV_MAT_CN( src->type );
838
839 if( (cn != 1 && cn != 3) || !CV_ARE_CNS_EQ( src, dst ))
840 CV_ERROR( CV_StsUnsupportedFormat, "" );
841
842 src_size = cvGetMatSize( src );
843 dst_size = cvGetMatSize( dst );
844
845 /*if( dst_size.width > src_size.width || dst_size.height > src_size.height )
846 CV_ERROR( CV_StsBadSize, "destination ROI must not be larger than source ROI" );*/
847
848 if( mat->rows != 2 || mat->cols != 3 )
849 CV_ERROR( CV_StsBadArg,
850 "Transformation matrix must be 2x3" );
851
852 if( CV_MAT_TYPE( mat->type ) == CV_32FC1 )
853 {
854 for( k = 0; k < 3; k++ )
855 {
856 m[k] = mat->data.fl[k];
857 m[3 + k] = ((float*)(mat->data.ptr + mat->step))[k];
858 }
859 }
860 else if( CV_MAT_TYPE( mat->type ) == CV_64FC1 )
861 {
862 for( k = 0; k < 3; k++ )
863 {
864 m[k] = (float)mat->data.db[k];
865 m[3 + k] = (float)((double*)(mat->data.ptr + mat->step))[k];
866 }
867 }
868 else
869 CV_ERROR( CV_StsUnsupportedFormat,
870 "The transformation matrix should have 32fC1 or 64fC1 type" );
871
872 if( CV_ARE_DEPTHS_EQ( src, dst ))
873 {
874 func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[CV_MAT_DEPTH(src->type)]);
875 }
876 else
877 {
878 if( CV_MAT_DEPTH( src->type ) != CV_8U || CV_MAT_DEPTH( dst->type ) != CV_32F )
879 CV_ERROR( CV_StsUnsupportedFormat, "" );
880
881 func = (CvGetQuadrangleSubPixFunc)(gq_tab[cn != 1].fn_2d[1]);
882 }
883
884 if( !func )
885 CV_ERROR( CV_StsUnsupportedFormat, "" );
886
887 IPPI_CALL( func( src->data.ptr, src->step, src_size,
888 dst->data.ptr, dst->step, dst_size, m ));
889
890 __END__;
891 }
892
893
894 /* End of file. */
895