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 static CvStatus
icvUnDistort_8u_CnR(const uchar * src,int srcstep,uchar * dst,int dststep,CvSize size,const float * intrinsic_matrix,const float * dist_coeffs,int cn)45 icvUnDistort_8u_CnR( const uchar* src, int srcstep,
46 uchar* dst, int dststep, CvSize size,
47 const float* intrinsic_matrix,
48 const float* dist_coeffs, int cn )
49 {
50 int u, v, i;
51 float u0 = intrinsic_matrix[2], v0 = intrinsic_matrix[5];
52 float x0 = (size.width-1)*0.5f, y0 = (size.height-1)*0.5f;
53 float fx = intrinsic_matrix[0], fy = intrinsic_matrix[4];
54 float ifx = 1.f/fx, ify = 1.f/fy;
55 float k1 = dist_coeffs[0], k2 = dist_coeffs[1], k3 = dist_coeffs[4];
56 float p1 = dist_coeffs[2], p2 = dist_coeffs[3];
57
58 srcstep /= sizeof(src[0]);
59 dststep /= sizeof(dst[0]);
60
61 for( v = 0; v < size.height; v++, dst += dststep )
62 {
63 float y = (v - v0)*ify, y2 = y*y;
64
65 for( u = 0; u < size.width; u++ )
66 {
67 float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y;
68 float kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
69 float _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0;
70 float _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0;
71 int ix = cvFloor(_x), iy = cvFloor(_y);
72
73 if( (unsigned)iy < (unsigned)(size.height - 1) &&
74 (unsigned)ix < (unsigned)(size.width - 1) )
75 {
76 const uchar* ptr = src + iy*srcstep + ix*cn;
77 _x -= ix; _y -= iy;
78 for( i = 0; i < cn; i++ )
79 {
80 float t0 = CV_8TO32F(ptr[i]), t1 = CV_8TO32F(ptr[i+srcstep]);
81 t0 += _x*(CV_8TO32F(ptr[i+cn]) - t0);
82 t1 += _x*(CV_8TO32F(ptr[i + srcstep + cn]) - t1);
83 dst[u*cn + i] = (uchar)cvRound(t0 + _y*(t1 - t0));
84 }
85 }
86 else
87 {
88 for( i = 0; i < cn; i++ )
89 dst[u*cn + i] = 0;
90 }
91
92 }
93 }
94
95 return CV_OK;
96 }
97
98
99 icvUndistortGetSize_t icvUndistortGetSize_p = 0;
100 icvCreateMapCameraUndistort_32f_C1R_t icvCreateMapCameraUndistort_32f_C1R_p = 0;
101 icvUndistortRadial_8u_C1R_t icvUndistortRadial_8u_C1R_p = 0;
102 icvUndistortRadial_8u_C3R_t icvUndistortRadial_8u_C3R_p = 0;
103
104 typedef CvStatus (CV_STDCALL * CvUndistortRadialIPPFunc)
105 ( const void* pSrc, int srcStep, void* pDst, int dstStep, CvSize roiSize,
106 float fx, float fy, float cx, float cy, float k1, float k2, uchar *pBuffer );
107
108 CV_IMPL void
cvUndistort2(const CvArr * _src,CvArr * _dst,const CvMat * A,const CvMat * dist_coeffs)109 cvUndistort2( const CvArr* _src, CvArr* _dst, const CvMat* A, const CvMat* dist_coeffs )
110 {
111 static int inittab = 0;
112
113 CV_FUNCNAME( "cvUndistort2" );
114
115 __BEGIN__;
116
117 float a[9], k[5]={0,0,0,0,0};
118 int coi1 = 0, coi2 = 0;
119 CvMat srcstub, *src = (CvMat*)_src;
120 CvMat dststub, *dst = (CvMat*)_dst;
121 CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
122 int cn, src_step, dst_step;
123 CvSize size;
124
125 if( !inittab )
126 {
127 icvInitLinearCoeffTab();
128 icvInitCubicCoeffTab();
129 inittab = 1;
130 }
131
132 CV_CALL( src = cvGetMat( src, &srcstub, &coi1 ));
133 CV_CALL( dst = cvGetMat( dst, &dststub, &coi2 ));
134
135 if( coi1 != 0 || coi2 != 0 )
136 CV_ERROR( CV_BadCOI, "The function does not support COI" );
137
138 if( CV_MAT_DEPTH(src->type) != CV_8U )
139 CV_ERROR( CV_StsUnsupportedFormat, "Only 8-bit images are supported" );
140
141 if( src->data.ptr == dst->data.ptr )
142 CV_ERROR( CV_StsNotImplemented, "In-place undistortion is not implemented" );
143
144 if( !CV_ARE_TYPES_EQ( src, dst ))
145 CV_ERROR( CV_StsUnmatchedFormats, "" );
146
147 if( !CV_ARE_SIZES_EQ( src, dst ))
148 CV_ERROR( CV_StsUnmatchedSizes, "" );
149
150 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
151 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )
152 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
153
154 if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) ||
155 (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 &&
156 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) ||
157 (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
158 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) )
159 CV_ERROR( CV_StsBadArg,
160 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" );
161
162 cvConvert( A, &_a );
163 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
164 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
165 cvConvert( dist_coeffs, &_k );
166
167 cn = CV_MAT_CN(src->type);
168 size = cvGetMatSize(src);
169 src_step = src->step ? src->step : CV_STUB_STEP;
170 dst_step = dst->step ? dst->step : CV_STUB_STEP;
171
172 icvUnDistort_8u_CnR( src->data.ptr, src_step,
173 dst->data.ptr, dst_step, size, a, k, cn );
174
175 __END__;
176 }
177
178
179 CV_IMPL void
cvInitUndistortMap(const CvMat * A,const CvMat * dist_coeffs,CvArr * mapxarr,CvArr * mapyarr)180 cvInitUndistortMap( const CvMat* A, const CvMat* dist_coeffs,
181 CvArr* mapxarr, CvArr* mapyarr )
182 {
183 CV_FUNCNAME( "cvInitUndistortMap" );
184
185 __BEGIN__;
186
187 float a[9], k[5]={0,0,0,0,0};
188 int coi1 = 0, coi2 = 0;
189 CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
190 CvMat mapystub, *_mapy = (CvMat*)mapyarr;
191 CvMat _a = cvMat( 3, 3, CV_32F, a ), _k;
192 int u, v;
193 float u0, v0, fx, fy, ifx, ify, x0, y0, k1, k2, k3, p1, p2;
194 CvSize size;
195
196 CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
197 CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
198
199 if( coi1 != 0 || coi2 != 0 )
200 CV_ERROR( CV_BadCOI, "The function does not support COI" );
201
202 if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
203 CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
204
205 if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
206 CV_ERROR( CV_StsUnmatchedFormats, "" );
207
208 if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
209 CV_ERROR( CV_StsUnmatchedSizes, "" );
210
211 size = cvGetMatSize(_mapx);
212
213 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
214 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )
215 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
216
217 if( !CV_IS_MAT(dist_coeffs) || (dist_coeffs->rows != 1 && dist_coeffs->cols != 1) ||
218 (dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 4 &&
219 dist_coeffs->rows*dist_coeffs->cols*CV_MAT_CN(dist_coeffs->type) != 5) ||
220 (CV_MAT_DEPTH(dist_coeffs->type) != CV_64F &&
221 CV_MAT_DEPTH(dist_coeffs->type) != CV_32F) )
222 CV_ERROR( CV_StsBadArg,
223 "Distortion coefficients must be 1x4, 4x1, 1x5 or 5x1 floating-point vector" );
224
225 cvConvert( A, &_a );
226 _k = cvMat( dist_coeffs->rows, dist_coeffs->cols,
227 CV_MAKETYPE(CV_32F, CV_MAT_CN(dist_coeffs->type)), k );
228 cvConvert( dist_coeffs, &_k );
229
230 u0 = a[2]; v0 = a[5];
231 fx = a[0]; fy = a[4];
232 ifx = 1.f/fx; ify = 1.f/fy;
233 k1 = k[0]; k2 = k[1]; k3 = k[4];
234 p1 = k[2]; p2 = k[3];
235 x0 = (size.width-1)*0.5f;
236 y0 = (size.height-1)*0.5f;
237
238 for( v = 0; v < size.height; v++ )
239 {
240 float* mapx = (float*)(_mapx->data.ptr + _mapx->step*v);
241 float* mapy = (float*)(_mapy->data.ptr + _mapy->step*v);
242 float y = (v - v0)*ify, y2 = y*y;
243
244 for( u = 0; u < size.width; u++ )
245 {
246 float x = (u - u0)*ifx, x2 = x*x, r2 = x2 + y2, _2xy = 2*x*y;
247 double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
248 double _x = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + x0;
249 double _y = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + y0;
250 mapx[u] = (float)_x;
251 mapy[u] = (float)_y;
252 }
253 }
254
255 __END__;
256 }
257
258
259 void
cvInitUndistortRectifyMap(const CvMat * A,const CvMat * distCoeffs,const CvMat * R,const CvMat * Ar,CvArr * mapxarr,CvArr * mapyarr)260 cvInitUndistortRectifyMap( const CvMat* A, const CvMat* distCoeffs,
261 const CvMat *R, const CvMat* Ar, CvArr* mapxarr, CvArr* mapyarr )
262 {
263 CV_FUNCNAME( "cvInitUndistortMap" );
264
265 __BEGIN__;
266
267 double a[9], ar[9], r[9], ir[9], k[5]={0,0,0,0,0};
268 int coi1 = 0, coi2 = 0;
269 CvMat mapxstub, *_mapx = (CvMat*)mapxarr;
270 CvMat mapystub, *_mapy = (CvMat*)mapyarr;
271 CvMat _a = cvMat( 3, 3, CV_64F, a );
272 CvMat _k = cvMat( 4, 1, CV_64F, k );
273 CvMat _ar = cvMat( 3, 3, CV_64F, ar );
274 CvMat _r = cvMat( 3, 3, CV_64F, r );
275 CvMat _ir = cvMat( 3, 3, CV_64F, ir );
276 int i, j;
277 double fx, fy, u0, v0, k1, k2, k3, p1, p2;
278 CvSize size;
279
280 CV_CALL( _mapx = cvGetMat( _mapx, &mapxstub, &coi1 ));
281 CV_CALL( _mapy = cvGetMat( _mapy, &mapystub, &coi2 ));
282
283 if( coi1 != 0 || coi2 != 0 )
284 CV_ERROR( CV_BadCOI, "The function does not support COI" );
285
286 if( CV_MAT_TYPE(_mapx->type) != CV_32FC1 )
287 CV_ERROR( CV_StsUnsupportedFormat, "Both maps must have 32fC1 type" );
288
289 if( !CV_ARE_TYPES_EQ( _mapx, _mapy ))
290 CV_ERROR( CV_StsUnmatchedFormats, "" );
291
292 if( !CV_ARE_SIZES_EQ( _mapx, _mapy ))
293 CV_ERROR( CV_StsUnmatchedSizes, "" );
294
295 if( A )
296 {
297 if( !CV_IS_MAT(A) || A->rows != 3 || A->cols != 3 ||
298 (CV_MAT_TYPE(A->type) != CV_32FC1 && CV_MAT_TYPE(A->type) != CV_64FC1) )
299 CV_ERROR( CV_StsBadArg, "Intrinsic matrix must be a valid 3x3 floating-point matrix" );
300 cvConvert( A, &_a );
301 }
302 else
303 cvSetIdentity( &_a );
304
305 if( Ar )
306 {
307 CvMat Ar33;
308 if( !CV_IS_MAT(Ar) || Ar->rows != 3 || (Ar->cols != 3 && Ar->cols != 4) ||
309 (CV_MAT_TYPE(Ar->type) != CV_32FC1 && CV_MAT_TYPE(Ar->type) != CV_64FC1) )
310 CV_ERROR( CV_StsBadArg, "The new intrinsic matrix must be a valid 3x3 floating-point matrix" );
311 cvGetCols( Ar, &Ar33, 0, 3 );
312 cvConvert( &Ar33, &_ar );
313 }
314 else
315 cvSetIdentity( &_ar );
316
317 if( !CV_IS_MAT(R) || R->rows != 3 || R->cols != 3 ||
318 (CV_MAT_TYPE(R->type) != CV_32FC1 && CV_MAT_TYPE(R->type) != CV_64FC1) )
319 CV_ERROR( CV_StsBadArg, "Rotaion/homography matrix must be a valid 3x3 floating-point matrix" );
320
321 if( distCoeffs )
322 {
323 CV_ASSERT( CV_IS_MAT(distCoeffs) &&
324 (distCoeffs->rows == 1 || distCoeffs->cols == 1) &&
325 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 4 ||
326 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) == 5) &&
327 (CV_MAT_DEPTH(distCoeffs->type) == CV_64F ||
328 CV_MAT_DEPTH(distCoeffs->type) == CV_32F) );
329 _k = cvMat( distCoeffs->rows, distCoeffs->cols,
330 CV_MAKETYPE(CV_64F, CV_MAT_CN(distCoeffs->type)), k );
331 cvConvert( distCoeffs, &_k );
332 }
333 else
334 cvZero( &_k );
335
336 cvConvert( R, &_r ); // rectification matrix
337 cvMatMul( &_ar, &_r, &_r ); // Ar*R
338 cvInvert( &_r, &_ir ); // inverse: R^-1*Ar^-1
339
340 u0 = a[2]; v0 = a[5];
341 fx = a[0]; fy = a[4];
342 k1 = k[0]; k2 = k[1]; k3 = k[4];
343 p1 = k[2]; p2 = k[3];
344
345 size = cvGetMatSize(_mapx);
346
347 for( i = 0; i < size.height; i++ )
348 {
349 float* mapx = (float*)(_mapx->data.ptr + _mapx->step*i);
350 float* mapy = (float*)(_mapy->data.ptr + _mapy->step*i);
351 double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8];
352
353 for( j = 0; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] )
354 {
355 double w = 1./_w, x = _x*w, y = _y*w;
356 double x2 = x*x, y2 = y*y;
357 double r2 = x2 + y2, _2xy = 2*x*y;
358 double kr = 1 + ((k3*r2 + k2)*r2 + k1)*r2;
359 double u = fx*(x*kr + p1*_2xy + p2*(r2 + 2*x2)) + u0;
360 double v = fy*(y*kr + p1*(r2 + 2*y2) + p2*_2xy) + v0;
361 mapx[j] = (float)u;
362 mapy[j] = (float)v;
363 }
364 }
365
366 __END__;
367 }
368
369
370 void
cvUndistortPoints(const CvMat * _src,CvMat * _dst,const CvMat * _cameraMatrix,const CvMat * _distCoeffs,const CvMat * _R,const CvMat * _P)371 cvUndistortPoints( const CvMat* _src, CvMat* _dst, const CvMat* _cameraMatrix,
372 const CvMat* _distCoeffs,
373 const CvMat* _R, const CvMat* _P )
374 {
375 CV_FUNCNAME( "cvUndistortPoints" );
376
377 __BEGIN__;
378
379 double A[3][3], RR[3][3], k[5]={0,0,0,0,0}, fx, fy, ifx, ify, cx, cy;
380 CvMat _A=cvMat(3, 3, CV_64F, A), _Dk;
381 CvMat _RR=cvMat(3, 3, CV_64F, RR);
382 const CvPoint2D32f* srcf;
383 const CvPoint2D64f* srcd;
384 CvPoint2D32f* dstf;
385 CvPoint2D64f* dstd;
386 int stype, dtype;
387 int sstep, dstep;
388 int i, j, n;
389
390 CV_ASSERT( CV_IS_MAT(_src) && CV_IS_MAT(_dst) &&
391 (_src->rows == 1 || _src->cols == 1) &&
392 (_dst->rows == 1 || _dst->cols == 1) &&
393 CV_ARE_SIZES_EQ(_src, _dst) &&
394 (CV_MAT_TYPE(_src->type) == CV_32FC2 || CV_MAT_TYPE(_src->type) == CV_64FC2) &&
395 (CV_MAT_TYPE(_dst->type) == CV_32FC2 || CV_MAT_TYPE(_dst->type) == CV_64FC2));
396
397 CV_ASSERT( CV_IS_MAT(_cameraMatrix) && CV_IS_MAT(_distCoeffs) &&
398 _cameraMatrix->rows == 3 && _cameraMatrix->cols == 3 &&
399 (_distCoeffs->rows == 1 || _distCoeffs->cols == 1) &&
400 (_distCoeffs->rows*_distCoeffs->cols == 4 ||
401 _distCoeffs->rows*_distCoeffs->cols == 5) );
402 _Dk = cvMat( _distCoeffs->rows, _distCoeffs->cols,
403 CV_MAKETYPE(CV_64F,CV_MAT_CN(_distCoeffs->type)), k);
404 cvConvert( _cameraMatrix, &_A );
405 cvConvert( _distCoeffs, &_Dk );
406
407 if( _R )
408 {
409 CV_ASSERT( CV_IS_MAT(_R) && _R->rows == 3 && _R->cols == 3 );
410 cvConvert( _R, &_RR );
411 }
412 else
413 cvSetIdentity(&_RR);
414
415 if( _P )
416 {
417 double PP[3][3];
418 CvMat _P3x3, _PP=cvMat(3, 3, CV_64F, PP);
419 CV_ASSERT( CV_IS_MAT(_P) && _P->rows == 3 && (_P->cols == 3 || _P->cols == 4));
420 cvConvert( cvGetCols(_P, &_P3x3, 0, 3), &_PP );
421 cvMatMul( &_PP, &_RR, &_RR );
422 }
423
424 srcf = (const CvPoint2D32f*)_src->data.ptr;
425 srcd = (const CvPoint2D64f*)_src->data.ptr;
426 dstf = (CvPoint2D32f*)_dst->data.ptr;
427 dstd = (CvPoint2D64f*)_dst->data.ptr;
428 stype = CV_MAT_TYPE(_src->type);
429 dtype = CV_MAT_TYPE(_dst->type);
430 sstep = _src->rows == 1 ? 1 : _src->step/CV_ELEM_SIZE(stype);
431 dstep = _dst->rows == 1 ? 1 : _dst->step/CV_ELEM_SIZE(dtype);
432
433 n = _src->rows + _src->cols - 1;
434
435 fx = A[0][0];
436 fy = A[1][1];
437 ifx = 1./fx;
438 ify = 1./fy;
439 cx = A[0][2];
440 cy = A[1][2];
441
442 for( i = 0; i < n; i++ )
443 {
444 double x, y, x0, y0;
445 if( stype == CV_32FC2 )
446 {
447 x = srcf[i*sstep].x;
448 y = srcf[i*sstep].y;
449 }
450 else
451 {
452 x = srcd[i*sstep].x;
453 y = srcd[i*sstep].y;
454 }
455
456 x0 = x = (x - cx)*ifx;
457 y0 = y = (y - cy)*ify;
458
459 // compensate distortion iteratively
460 for( j = 0; j < 5; j++ )
461 {
462 double r2 = x*x + y*y;
463 double icdist = 1./(1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2);
464 double deltaX = 2*k[2]*x*y + k[3]*(r2 + 2*x*x);
465 double deltaY = k[2]*(r2 + 2*y*y) + 2*k[3]*x*y;
466 x = (x0 - deltaX)*icdist;
467 y = (y0 - deltaY)*icdist;
468 }
469
470 double xx = RR[0][0]*x + RR[0][1]*y + RR[0][2];
471 double yy = RR[1][0]*x + RR[1][1]*y + RR[1][2];
472 double ww = 1./(RR[2][0]*x + RR[2][1]*y + RR[2][2]);
473 x = xx*ww;
474 y = yy*ww;
475
476 if( dtype == CV_32FC2 )
477 {
478 dstf[i*dstep].x = (float)x;
479 dstf[i*dstep].y = (float)y;
480 }
481 else
482 {
483 dstd[i*dstep].x = x;
484 dstd[i*dstep].y = y;
485 }
486 }
487
488 __END__;
489 }
490
491 /* End of file */
492