RTAB-Map 0.23.10
Real-Time Appearance-Based Mapping
Loading...
Searching...
No Matches
stereoRectifyFisheye.h
1/*
2Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3All rights reserved.
4
5This code is the same has cv::stereoRectify() but accepting fisheye distortion model:
6All cvUndistortPoints() have been replaced by cv::fisheye::undistortPoints()
7See https://github.com/opencv/opencv/blob/master/modules/calib3d/src/calibration.cpp
8
9Redistribution and use in source and binary forms, with or without
10modification, are permitted provided that the following conditions are met:
11 * Redistributions of source code must retain the above copyright
12 notice, this list of conditions and the following disclaimer.
13 * Redistributions in binary form must reproduce the above copyright
14 notice, this list of conditions and the following disclaimer in the
15 documentation and/or other materials provided with the distribution.
16 * Neither the name of the Universite de Sherbrooke nor the
17 names of its contributors may be used to endorse or promote products
18 derived from this software without specific prior written permission.
19
20THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
21ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
22WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
24DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
27ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30*/
31
32#ifndef CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
33#define CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_
34
35// This header relies on the OpenCV C API (cvRodrigues2, cvProjectPoints2, ...)
36// which was removed in OpenCV 5. Pull in only the version macros (available in
37// all OpenCV versions) so we can fail early with a clear message rather than
38// with cryptic errors from the includes below.
39#include <opencv2/core/version.hpp>
40#if CV_MAJOR_VERSION >= 5
41#error "stereoRectifyFisheye.h is not supported with OpenCV 5 or later (it uses the removed OpenCV C API). Use cv::fisheye::stereoRectify() instead, or guard your include with '#if CV_MAJOR_VERSION < 5'."
42#endif
43
44#include <opencv2/calib3d/calib3d.hpp>
45#if CV_MAJOR_VERSION >= 3
46#include <opencv2/calib3d/calib3d_c.h>
47
48#if CV_MAJOR_VERSION >= 4
49#include <opencv2/core/core_c.h>
50
51// Opencv4 doesn't expose those functions below anymore, we should recopy all of them!
52int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian CV_DEFAULT(0))
53{
54 int depth, elem_size;
55 int i, k;
56 double J[27] = {0};
57 CvMat matJ = cvMat( 3, 9, CV_64F, J );
58
59 if( !CV_IS_MAT(src) )
60 CV_Error( !src ? CV_StsNullPtr : CV_StsBadArg, "Input argument is not a valid matrix" );
61
62 if( !CV_IS_MAT(dst) )
63 CV_Error( !dst ? CV_StsNullPtr : CV_StsBadArg,
64 "The first output argument is not a valid matrix" );
65
66 depth = CV_MAT_DEPTH(src->type);
67 elem_size = CV_ELEM_SIZE(depth);
68
69 if( depth != CV_32F && depth != CV_64F )
70 CV_Error( CV_StsUnsupportedFormat, "The matrices must have 32f or 64f data type" );
71
72 if( !CV_ARE_DEPTHS_EQ(src, dst) )
73 CV_Error( CV_StsUnmatchedFormats, "All the matrices must have the same data type" );
74
75 if( jacobian )
76 {
77 if( !CV_IS_MAT(jacobian) )
78 CV_Error( CV_StsBadArg, "Jacobian is not a valid matrix" );
79
80 if( !CV_ARE_DEPTHS_EQ(src, jacobian) || CV_MAT_CN(jacobian->type) != 1 )
81 CV_Error( CV_StsUnmatchedFormats, "Jacobian must have 32fC1 or 64fC1 datatype" );
82
83 if( (jacobian->rows != 9 || jacobian->cols != 3) &&
84 (jacobian->rows != 3 || jacobian->cols != 9))
85 CV_Error( CV_StsBadSize, "Jacobian must be 3x9 or 9x3" );
86 }
87
88 if( src->cols == 1 || src->rows == 1 )
89 {
90 int step = src->rows > 1 ? src->step / elem_size : 1;
91
92 if( src->rows + src->cols*CV_MAT_CN(src->type) - 1 != 3 )
93 CV_Error( CV_StsBadSize, "Input matrix must be 1x3, 3x1 or 3x3" );
94
95 if( dst->rows != 3 || dst->cols != 3 || CV_MAT_CN(dst->type) != 1 )
96 CV_Error( CV_StsBadSize, "Output matrix must be 3x3, single-channel floating point matrix" );
97
98 cv::Point3d r;
99 if( depth == CV_32F )
100 {
101 r.x = src->data.fl[0];
102 r.y = src->data.fl[step];
103 r.z = src->data.fl[step*2];
104 }
105 else
106 {
107 r.x = src->data.db[0];
108 r.y = src->data.db[step];
109 r.z = src->data.db[step*2];
110 }
111
112 double theta = cv::norm(r);
113
114 if( theta < DBL_EPSILON )
115 {
116 cvSetIdentity( dst );
117
118 if( jacobian )
119 {
120 memset( J, 0, sizeof(J) );
121 J[5] = J[15] = J[19] = -1;
122 J[7] = J[11] = J[21] = 1;
123 }
124 }
125 else
126 {
127 double c = cos(theta);
128 double s = sin(theta);
129 double c1 = 1. - c;
130 double itheta = theta ? 1./theta : 0.;
131
132 r *= itheta;
133
134 cv::Matx33d rrt( r.x*r.x, r.x*r.y, r.x*r.z, r.x*r.y, r.y*r.y, r.y*r.z, r.x*r.z, r.y*r.z, r.z*r.z );
135 cv::Matx33d r_x( 0, -r.z, r.y,
136 r.z, 0, -r.x,
137 -r.y, r.x, 0 );
138
139 // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
140 cv::Matx33d R = c*cv::Matx33d::eye() + c1*rrt + s*r_x;
141
142 cv::Mat(R).convertTo(cv::cvarrToMat(dst), dst->type);
143
144 if( jacobian )
145 {
146 const double I[] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
147 double drrt[] = { r.x+r.x, r.y, r.z, r.y, 0, 0, r.z, 0, 0,
148 0, r.x, 0, r.x, r.y+r.y, r.z, 0, r.z, 0,
149 0, 0, r.x, 0, 0, r.y, r.x, r.y, r.z+r.z };
150 double d_r_x_[] = { 0, 0, 0, 0, 0, -1, 0, 1, 0,
151 0, 0, 1, 0, 0, 0, -1, 0, 0,
152 0, -1, 0, 1, 0, 0, 0, 0, 0 };
153 for( i = 0; i < 3; i++ )
154 {
155 double ri = i == 0 ? r.x : i == 1 ? r.y : r.z;
156 double a0 = -s*ri, a1 = (s - 2*c1*itheta)*ri, a2 = c1*itheta;
157 double a3 = (c - s*itheta)*ri, a4 = s*itheta;
158 for( k = 0; k < 9; k++ )
159 J[i*9+k] = a0*I[k] + a1*rrt.val[k] + a2*drrt[i*9+k] +
160 a3*r_x.val[k] + a4*d_r_x_[i*9+k];
161 }
162 }
163 }
164 }
165 else if( src->cols == 3 && src->rows == 3 )
166 {
167 cv::Matx33d U, Vt;
168 cv::Vec3d W;
169 double theta, s, c;
170 int step = dst->rows > 1 ? dst->step / elem_size : 1;
171
172 if( (dst->rows != 1 || dst->cols*CV_MAT_CN(dst->type) != 3) &&
173 (dst->rows != 3 || dst->cols != 1 || CV_MAT_CN(dst->type) != 1))
174 CV_Error( CV_StsBadSize, "Output matrix must be 1x3 or 3x1" );
175
176 cv::Matx33d R = cv::cvarrToMat(src);
177
178 if( !cv::checkRange(R, true, NULL, -100, 100) )
179 {
180 cvZero(dst);
181 if( jacobian )
182 cvZero(jacobian);
183 return 0;
184 }
185
186 cv::SVD::compute(R, W, U, Vt);
187 R = U*Vt;
188
189 cv::Point3d r(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
190
191 s = std::sqrt((r.x*r.x + r.y*r.y + r.z*r.z)*0.25);
192 c = (R(0, 0) + R(1, 1) + R(2, 2) - 1)*0.5;
193 c = c > 1. ? 1. : c < -1. ? -1. : c;
194 theta = acos(c);
195
196 if( s < 1e-5 )
197 {
198 double t;
199
200 if( c > 0 )
201 r = cv::Point3d(0, 0, 0);
202 else
203 {
204 t = (R(0, 0) + 1)*0.5;
205 r.x = std::sqrt(MAX(t,0.));
206 t = (R(1, 1) + 1)*0.5;
207 r.y = std::sqrt(MAX(t,0.))*(R(0, 1) < 0 ? -1. : 1.);
208 t = (R(2, 2) + 1)*0.5;
209 r.z = std::sqrt(MAX(t,0.))*(R(0, 2) < 0 ? -1. : 1.);
210 if( fabs(r.x) < fabs(r.y) && fabs(r.x) < fabs(r.z) && (R(1, 2) > 0) != (r.y*r.z > 0) )
211 r.z = -r.z;
212 theta /= cv::norm(r);
213 r *= theta;
214 }
215
216 if( jacobian )
217 {
218 memset( J, 0, sizeof(J) );
219 if( c > 0 )
220 {
221 J[5] = J[15] = J[19] = -0.5;
222 J[7] = J[11] = J[21] = 0.5;
223 }
224 }
225 }
226 else
227 {
228 double vth = 1/(2*s);
229
230 if( jacobian )
231 {
232 double t, dtheta_dtr = -1./s;
233 // var1 = [vth;theta]
234 // var = [om1;var1] = [om1;vth;theta]
235 double dvth_dtheta = -vth*c/s;
236 double d1 = 0.5*dvth_dtheta*dtheta_dtr;
237 double d2 = 0.5*dtheta_dtr;
238 // dvar1/dR = dvar1/dtheta*dtheta/dR = [dvth/dtheta; 1] * dtheta/dtr * dtr/dR
239 double dvardR[5*9] =
240 {
241 0, 0, 0, 0, 0, 1, 0, -1, 0,
242 0, 0, -1, 0, 0, 0, 1, 0, 0,
243 0, 1, 0, -1, 0, 0, 0, 0, 0,
244 d1, 0, 0, 0, d1, 0, 0, 0, d1,
245 d2, 0, 0, 0, d2, 0, 0, 0, d2
246 };
247 // var2 = [om;theta]
248 double dvar2dvar[] =
249 {
250 vth, 0, 0, r.x, 0,
251 0, vth, 0, r.y, 0,
252 0, 0, vth, r.z, 0,
253 0, 0, 0, 0, 1
254 };
255 double domegadvar2[] =
256 {
257 theta, 0, 0, r.x*vth,
258 0, theta, 0, r.y*vth,
259 0, 0, theta, r.z*vth
260 };
261
262 CvMat _dvardR = cvMat( 5, 9, CV_64FC1, dvardR );
263 CvMat _dvar2dvar = cvMat( 4, 5, CV_64FC1, dvar2dvar );
264 CvMat _domegadvar2 = cvMat( 3, 4, CV_64FC1, domegadvar2 );
265 double t0[3*5];
266 CvMat _t0 = cvMat( 3, 5, CV_64FC1, t0 );
267
268 cvMatMul( &_domegadvar2, &_dvar2dvar, &_t0 );
269 cvMatMul( &_t0, &_dvardR, &matJ );
270
271 // transpose every row of matJ (treat the rows as 3x3 matrices)
272 CV_SWAP(J[1], J[3], t); CV_SWAP(J[2], J[6], t); CV_SWAP(J[5], J[7], t);
273 CV_SWAP(J[10], J[12], t); CV_SWAP(J[11], J[15], t); CV_SWAP(J[14], J[16], t);
274 CV_SWAP(J[19], J[21], t); CV_SWAP(J[20], J[24], t); CV_SWAP(J[23], J[25], t);
275 }
276
277 vth *= theta;
278 r *= vth;
279 }
280
281 if( depth == CV_32F )
282 {
283 dst->data.fl[0] = (float)r.x;
284 dst->data.fl[step] = (float)r.y;
285 dst->data.fl[step*2] = (float)r.z;
286 }
287 else
288 {
289 dst->data.db[0] = r.x;
290 dst->data.db[step] = r.y;
291 dst->data.db[step*2] = r.z;
292 }
293 }
294
295 if( jacobian )
296 {
297 if( depth == CV_32F )
298 {
299 if( jacobian->rows == matJ.rows )
300 cvConvert( &matJ, jacobian );
301 else
302 {
303 float Jf[3*9];
304 CvMat _Jf = cvMat( matJ.rows, matJ.cols, CV_32FC1, Jf );
305 cvConvert( &matJ, &_Jf );
306 cvTranspose( &_Jf, jacobian );
307 }
308 }
309 else if( jacobian->rows == matJ.rows )
310 cvCopy( &matJ, jacobian );
311 else
312 cvTranspose( &matJ, jacobian );
313 }
314
315 return 1;
316}
317
318template <typename FLOAT>
319void computeTiltProjectionMatrix(FLOAT tauX,
320 FLOAT tauY,
321 cv::Matx<FLOAT, 3, 3>* matTilt = 0,
322 cv::Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
323 cv::Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
324 cv::Matx<FLOAT, 3, 3>* invMatTilt = 0)
325{
326 FLOAT cTauX = cos(tauX);
327 FLOAT sTauX = sin(tauX);
328 FLOAT cTauY = cos(tauY);
329 FLOAT sTauY = sin(tauY);
330 cv::Matx<FLOAT, 3, 3> matRotX = cv::Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
331 cv::Matx<FLOAT, 3, 3> matRotY = cv::Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
332 cv::Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
333 cv::Matx<FLOAT, 3, 3> matProjZ = cv::Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
334 if (matTilt)
335 {
336 // Matrix for trapezoidal distortion of tilted image sensor
337 *matTilt = matProjZ * matRotXY;
338 }
339 if (dMatTiltdTauX)
340 {
341 // Derivative with respect to tauX
342 cv::Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * cv::Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
343 cv::Matx<FLOAT, 3, 3> dMatProjZdTauX = cv::Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
344 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
345 *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
346 }
347 if (dMatTiltdTauY)
348 {
349 // Derivative with respect to tauY
350 cv::Matx<FLOAT, 3, 3> dMatRotXYdTauY = cv::Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
351 cv::Matx<FLOAT, 3, 3> dMatProjZdTauY = cv::Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
352 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
353 *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
354 }
355 if (invMatTilt)
356 {
357 FLOAT inv = 1./matRotXY(2,2);
358 cv::Matx<FLOAT, 3, 3> invMatProjZ = cv::Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
359 *invMatTilt = matRotXY.t()*invMatProjZ;
360 }
361}
362
363void cvProjectPoints2Internal( const CvMat* objectPoints,
364 const CvMat* r_vec,
365 const CvMat* t_vec,
366 const CvMat* A,
367 const CvMat* distCoeffs,
368 CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
369 CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
370 CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
371 CvMat* dpdo CV_DEFAULT(NULL),
372 double aspectRatio CV_DEFAULT(0) )
373{
374 cv::Ptr<CvMat> matM, _m;
375 cv::Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
376 cv::Ptr<CvMat> _dpdo;
377
378 int i, j, count;
379 int calc_derivatives;
380 const CvPoint3D64f* M;
381 CvPoint2D64f* m;
382 double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
383 cv::Matx33d matTilt = cv::Matx33d::eye();
384 cv::Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
385 cv::Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
386 CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
387 CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
388 double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
389 double* dpdo_p = 0;
390 int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
391 int dpdo_step = 0;
392 bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
393
394 if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
395 !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
!CV_IS_MAT(imagePoints) )
397 CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
398
399 int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
400 if(total % 3 != 0)
401 {
402 //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
403 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
404 }
405 count = total / 3;
406
407 if( CV_IS_CONT_MAT(objectPoints->type) &&
408 (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
409 ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
410 (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
411 (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
412 {
413 matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
414 cvConvert(objectPoints, matM);
415 }
416 else
417 {
418// matM = cvCreateMat( 1, count, CV_64FC3 );
419// cvConvertPointsHomogeneous( objectPoints, matM );
420 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
421 }
422
423 if( CV_IS_CONT_MAT(imagePoints->type) &&
424 (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
425 ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 2) ||
426 (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 2) ||
427 (imagePoints->rows == 2 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
428 {
429 _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
430 cvConvert(imagePoints, _m);
431 }
432 else
433 {
434// _m = cvCreateMat( 1, count, CV_64FC2 );
435 CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
436 }
437
438 M = (CvPoint3D64f*)matM->data.db;
439 m = (CvPoint2D64f*)_m->data.db;
440
441 if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
442 (((r_vec->rows != 1 && r_vec->cols != 1) ||
443 r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
444 ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
445 CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
446 "floating-point rotation vector, or 3x3 rotation matrix" );
447
448 if( r_vec->rows == 3 && r_vec->cols == 3 )
449 {
450 _r = cvMat( 3, 1, CV_64FC1, r );
451 cvRodrigues2( r_vec, &_r );
452 cvRodrigues2( &_r, &matR, &_dRdr );
453 cvCopy( r_vec, &matR );
454 }
455 else
456 {
457 _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
458 cvConvert( r_vec, &_r );
459 cvRodrigues2( &_r, &matR, &_dRdr );
460 }
461
462 if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
463 (t_vec->rows != 1 && t_vec->cols != 1) ||
464 t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
465 CV_Error( CV_StsBadArg,
466 "Translation vector must be 1x3 or 3x1 floating-point vector" );
467
468 _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
469 cvConvert( t_vec, &_t );
470
471 if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
472 A->rows != 3 || A->cols != 3 )
473 CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
474
475 cvConvert( A, &_a );
476 fx = a[0]; fy = a[4];
477 cx = a[2]; cy = a[5];
478
479 if( fixedAspectRatio )
480 fx = fy*aspectRatio;
481
482 if( distCoeffs )
483 {
484 if( !CV_IS_MAT(distCoeffs) ||
485 (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
486 CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
487 (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
488 (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
489 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
490 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
491 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
492 distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
493 CV_Error( CV_StsBadArg, "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector");
494
495 _k = cvMat( distCoeffs->rows, distCoeffs->cols,
496 CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
497 cvConvert( distCoeffs, &_k );
498 if(k[12] != 0 || k[13] != 0)
499 {
500 computeTiltProjectionMatrix(k[12], k[13],
501 &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
502 }
503 }
504
505 if( dpdr )
506 {
507 if( !CV_IS_MAT(dpdr) ||
508 (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
509 CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
510 dpdr->rows != count*2 || dpdr->cols != 3 )
511 CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
512
513 if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
514 {
515 _dpdr.reset(cvCloneMat(dpdr));
516 }
517 else
518 _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
519 dpdr_p = _dpdr->data.db;
520 dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
521 }
522
523 if( dpdt )
524 {
525 if( !CV_IS_MAT(dpdt) ||
526 (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
527 CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
528 dpdt->rows != count*2 || dpdt->cols != 3 )
529 CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
530
531 if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
532 {
533 _dpdt.reset(cvCloneMat(dpdt));
534 }
535 else
536 _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
537 dpdt_p = _dpdt->data.db;
538 dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
539 }
540
541 if( dpdf )
542 {
543 if( !CV_IS_MAT(dpdf) ||
544 (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
545 dpdf->rows != count*2 || dpdf->cols != 2 )
546 CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
547
548 if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
549 {
550 _dpdf.reset(cvCloneMat(dpdf));
551 }
552 else
553 _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
554 dpdf_p = _dpdf->data.db;
555 dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
556 }
557
558 if( dpdc )
559 {
560 if( !CV_IS_MAT(dpdc) ||
561 (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
562 dpdc->rows != count*2 || dpdc->cols != 2 )
563 CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
564
565 if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
566 {
567 _dpdc.reset(cvCloneMat(dpdc));
568 }
569 else
570 _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
571 dpdc_p = _dpdc->data.db;
572 dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
573 }
574
575 if( dpdk )
576 {
577 if( !CV_IS_MAT(dpdk) ||
578 (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
579 dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
580 CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
581
582 if( !distCoeffs )
583 CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
584
585 if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
586 {
587 _dpdk.reset(cvCloneMat(dpdk));
588 }
589 else
590 _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
591 dpdk_p = _dpdk->data.db;
592 dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
593 }
594
595 if( dpdo )
596 {
597 if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
598 && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
599 || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
600 CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
601
602 if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
603 {
604 _dpdo.reset( cvCloneMat( dpdo ) );
605 }
606 else
607 _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
608 cvZero(_dpdo);
609 dpdo_p = _dpdo->data.db;
610 dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
611 }
612
613 calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
614
615 for( i = 0; i < count; i++ )
616 {
617 double X = M[i].x, Y = M[i].y, Z = M[i].z;
618 double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
619 double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
620 double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
621 double r2, r4, r6, a1, a2, a3, cdist, icdist2;
622 double xd, yd, xd0, yd0, invProj;
623 cv::Vec3d vecTilt;
624 cv::Vec3d dVecTilt;
625 cv::Matx22d dMatTilt;
626 cv::Vec2d dXdYd;
627
628 double z0 = z;
629 z = z ? 1./z : 1;
630 x *= z; y *= z;
631
632 r2 = x*x + y*y;
633 r4 = r2*r2;
634 r6 = r4*r2;
635 a1 = 2*x*y;
636 a2 = r2 + 2*x*x;
637 a3 = r2 + 2*y*y;
638 cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
639 icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
640 xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
641 yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
642
643 // additional distortion by projecting onto a tilt plane
644 vecTilt = matTilt*cv::Vec3d(xd0, yd0, 1);
645 invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
646 xd = invProj * vecTilt(0);
647 yd = invProj * vecTilt(1);
648
649 m[i].x = xd*fx + cx;
650 m[i].y = yd*fy + cy;
651
652 if( calc_derivatives )
653 {
654 if( dpdc_p )
655 {
656 dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
657 dpdc_p[dpdc_step] = 0;
658 dpdc_p[dpdc_step+1] = 1;
659 dpdc_p += dpdc_step*2;
660 }
661
662 if( dpdf_p )
663 {
664 if( fixedAspectRatio )
665 {
666 dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
667 dpdf_p[dpdf_step] = 0;
668 dpdf_p[dpdf_step+1] = yd;
669 }
670 else
671 {
672 dpdf_p[0] = xd; dpdf_p[1] = 0;
673 dpdf_p[dpdf_step] = 0;
674 dpdf_p[dpdf_step+1] = yd;
675 }
676 dpdf_p += dpdf_step*2;
677 }
678 for (int row = 0; row < 2; ++row)
679 for (int col = 0; col < 2; ++col)
680 dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
681 - matTilt(2,col)*vecTilt(row);
682 double invProjSquare = (invProj*invProj);
683 dMatTilt *= invProjSquare;
684 if( dpdk_p )
685 {
686 dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r2, y*icdist2*r2);
687 dpdk_p[0] = fx*dXdYd(0);
688 dpdk_p[dpdk_step] = fy*dXdYd(1);
689 dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r4, y*icdist2*r4);
690 dpdk_p[1] = fx*dXdYd(0);
691 dpdk_p[dpdk_step+1] = fy*dXdYd(1);
692 if( _dpdk->cols > 2 )
693 {
694 dXdYd = dMatTilt*cv::Vec2d(a1, a3);
695 dpdk_p[2] = fx*dXdYd(0);
696 dpdk_p[dpdk_step+2] = fy*dXdYd(1);
697 dXdYd = dMatTilt*cv::Vec2d(a2, a1);
698 dpdk_p[3] = fx*dXdYd(0);
699 dpdk_p[dpdk_step+3] = fy*dXdYd(1);
700 if( _dpdk->cols > 4 )
701 {
702 dXdYd = dMatTilt*cv::Vec2d(x*icdist2*r6, y*icdist2*r6);
703 dpdk_p[4] = fx*dXdYd(0);
704 dpdk_p[dpdk_step+4] = fy*dXdYd(1);
705
706 if( _dpdk->cols > 5 )
707 {
708 dXdYd = dMatTilt*cv::Vec2d(
709 x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
710 dpdk_p[5] = fx*dXdYd(0);
711 dpdk_p[dpdk_step+5] = fy*dXdYd(1);
712 dXdYd = dMatTilt*cv::Vec2d(
713 x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
714 dpdk_p[6] = fx*dXdYd(0);
715 dpdk_p[dpdk_step+6] = fy*dXdYd(1);
716 dXdYd = dMatTilt*cv::Vec2d(
717 x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
718 dpdk_p[7] = fx*dXdYd(0);
719 dpdk_p[dpdk_step+7] = fy*dXdYd(1);
720 if( _dpdk->cols > 8 )
721 {
722 dXdYd = dMatTilt*cv::Vec2d(r2, 0);
723 dpdk_p[8] = fx*dXdYd(0); //s1
724 dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
725 dXdYd = dMatTilt*cv::Vec2d(r4, 0);
726 dpdk_p[9] = fx*dXdYd(0); //s2
727 dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
728 dXdYd = dMatTilt*cv::Vec2d(0, r2);
729 dpdk_p[10] = fx*dXdYd(0);//s3
730 dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
731 dXdYd = dMatTilt*cv::Vec2d(0, r4);
732 dpdk_p[11] = fx*dXdYd(0);//s4
733 dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
734 if( _dpdk->cols > 12 )
735 {
736 dVecTilt = dMatTiltdTauX * cv::Vec3d(xd0, yd0, 1);
737 dpdk_p[12] = fx * invProjSquare * (
738 dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
739 dpdk_p[dpdk_step+12] = fy*invProjSquare * (
740 dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
741 dVecTilt = dMatTiltdTauY * cv::Vec3d(xd0, yd0, 1);
742 dpdk_p[13] = fx * invProjSquare * (
743 dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
744 dpdk_p[dpdk_step+13] = fy * invProjSquare * (
745 dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
746 }
747 }
748 }
749 }
750 }
751 dpdk_p += dpdk_step*2;
752 }
753
754 if( dpdt_p )
755 {
756 double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
757 for( j = 0; j < 3; j++ )
758 {
759 double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
760 double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
761 double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
762 double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
763 double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
764 k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
765 double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
766 k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
767 dXdYd = dMatTilt*cv::Vec2d(dmxdt, dmydt);
768 dpdt_p[j] = fx*dXdYd(0);
769 dpdt_p[dpdt_step+j] = fy*dXdYd(1);
770 }
771 dpdt_p += dpdt_step*2;
772 }
773
774 if( dpdr_p )
775 {
776 double dx0dr[] =
777 {
778 X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
779 X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
780 X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
781 };
782 double dy0dr[] =
783 {
784 X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
785 X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
786 X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
787 };
788 double dz0dr[] =
789 {
790 X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
791 X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
792 X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
793 };
794 for( j = 0; j < 3; j++ )
795 {
796 double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
797 double dydr = z*(dy0dr[j] - y*dz0dr[j]);
798 double dr2dr = 2*x*dxdr + 2*y*dydr;
799 double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
800 double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
801 double da1dr = 2*(x*dydr + y*dxdr);
802 double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
803 k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
804 double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
805 k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
806 dXdYd = dMatTilt*cv::Vec2d(dmxdr, dmydr);
807 dpdr_p[j] = fx*dXdYd(0);
808 dpdr_p[dpdr_step+j] = fy*dXdYd(1);
809 }
810 dpdr_p += dpdr_step*2;
811 }
812
813 if( dpdo_p )
814 {
815 double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
816 z * ( R[1] - x * z * z0 * R[7] ),
817 z * ( R[2] - x * z * z0 * R[8] ) };
818 double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
819 z * ( R[4] - y * z * z0 * R[7] ),
820 z * ( R[5] - y * z * z0 * R[8] ) };
821 for( j = 0; j < 3; j++ )
822 {
823 double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
824 double dr4do = 2 * r2 * dr2do;
825 double dr6do = 3 * r4 * dr2do;
826 double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
827 double da2do = dr2do + 4 * x * dxdo[j];
828 double da3do = dr2do + 4 * y * dydo[j];
829 double dcdist_do
830 = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
831 double dicdist2_do = -icdist2 * icdist2
832 * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
833 double dxd0_do = cdist * icdist2 * dxdo[j]
834 + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
835 + k[2] * da1do + k[3] * da2do + k[8] * dr2do
836 + k[9] * dr4do;
837 double dyd0_do = cdist * icdist2 * dydo[j]
838 + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
839 + k[2] * da3do + k[3] * da1do + k[10] * dr2do
840 + k[11] * dr4do;
841 dXdYd = dMatTilt * cv::Vec2d( dxd0_do, dyd0_do );
842 dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
843 dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
844 }
845 dpdo_p += dpdo_step * 2;
846 }
847 }
848 }
849
850 if( _m != imagePoints )
851 cvConvert( _m, imagePoints );
852
853 if( _dpdr != dpdr )
854 cvConvert( _dpdr, dpdr );
855
856 if( _dpdt != dpdt )
857 cvConvert( _dpdt, dpdt );
858
859 if( _dpdf != dpdf )
860 cvConvert( _dpdf, dpdf );
861
862 if( _dpdc != dpdc )
863 cvConvert( _dpdc, dpdc );
864
865 if( _dpdk != dpdk )
866 cvConvert( _dpdk, dpdk );
867
868 if( _dpdo != dpdo )
869 cvConvert( _dpdo, dpdo );
870}
871
872void cvProjectPoints2( const CvMat* objectPoints,
873 const CvMat* r_vec,
874 const CvMat* t_vec,
875 const CvMat* A,
876 const CvMat* distCoeffs,
877 CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
878 CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
879 CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
880 double aspectRatio CV_DEFAULT(0))
881{
882 cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
883 dpdf, dpdc, dpdk, NULL, aspectRatio );
884}
885
886void cvConvertPointsHomogeneous( const CvMat* _src, CvMat* _dst )
887{
888 cv::Mat src = cv::cvarrToMat(_src), dst = cv::cvarrToMat(_dst);
889 const cv::Mat dst0 = dst;
890
891 int d0 = src.channels() > 1 ? src.channels() : MIN(src.cols, src.rows);
892
893 if( src.channels() == 1 && src.cols > d0 )
894 cv::transpose(src, src);
895
896 int d1 = dst.channels() > 1 ? dst.channels() : MIN(dst.cols, dst.rows);
897
898 if( d0 == d1 )
899 src.copyTo(dst);
900 else if( d0 < d1 )
901 cv::convertPointsToHomogeneous(src, dst);
902 else
903 cv::convertPointsFromHomogeneous(src, dst);
904
905 bool tflag = dst0.channels() == 1 && dst0.cols > d1;
906 dst = dst.reshape(dst0.channels(), (tflag ? dst0.cols : dst0.rows));
907
908 if( tflag )
909 {
910 CV_Assert( dst.rows == dst0.cols && dst.cols == dst0.rows );
911 if( dst0.type() == dst.type() )
912 transpose( dst, dst0 );
913 else
914 {
915 transpose( dst, dst );
916 dst.convertTo( dst0, dst0.type() );
917 }
918 }
919 else
920 {
921 CV_Assert( dst.size() == dst0.size() );
922 if( dst.data != dst0.data )
923 dst.convertTo(dst0, dst0.type());
924 }
925}
926
927#endif // OpenCV4
928
929#endif // OpenCV3
930
931namespace rtabmap
932{
933
934void
935icvGetRectanglesFisheye( const CvMat* cameraMatrix, const CvMat* distCoeffs,
936 const CvMat* R, const CvMat* newCameraMatrix, CvSize imgSize,
937 cv::Rect_<float>& inner, cv::Rect_<float>& outer )
938{
939 const int N = 9;
940 int x, y, k;
941 cv::Mat _pts(1, N*N, CV_32FC2);
942 CvPoint2D32f* pts = (CvPoint2D32f*)(_pts.data);
943
944 for( y = k = 0; y < N; y++ )
945 for( x = 0; x < N; x++ )
946 pts[k++] = cvPoint2D32f((float)x*imgSize.width/(N-1),
947 (float)y*imgSize.height/(N-1));
948
949 cv::Mat cameraMatrixM(cameraMatrix->rows, cameraMatrix->cols, cameraMatrix->type, cameraMatrix->data.ptr);
950 cv::Mat distCoeffsM(distCoeffs->rows, distCoeffs->cols, distCoeffs->type, distCoeffs->data.ptr);
951 cv::Mat RM(R->rows, R->cols, R->type, R->data.ptr);
952 cv::Mat newCameraMatrixM(newCameraMatrix->rows, newCameraMatrix->cols, newCameraMatrix->type, newCameraMatrix->data.ptr);
953 cv::fisheye::undistortPoints(_pts, _pts, cameraMatrixM, distCoeffsM, RM, newCameraMatrixM);
954 float iX0=-FLT_MAX, iX1=FLT_MAX, iY0=-FLT_MAX, iY1=FLT_MAX;
955 float oX0=FLT_MAX, oX1=-FLT_MAX, oY0=FLT_MAX, oY1=-FLT_MAX;
956 // find the inscribed rectangle.
957 // the code will likely not work with extreme rotation matrices (R) (>45%)
958 for( y = k = 0; y < N; y++ )
959 for( x = 0; x < N; x++ )
960 {
961 CvPoint2D32f p = pts[k++];
962 oX0 = MIN(oX0, p.x);
963 oX1 = MAX(oX1, p.x);
964 oY0 = MIN(oY0, p.y);
965 oY1 = MAX(oY1, p.y);
966
967 if( x == 0 )
968 iX0 = MAX(iX0, p.x);
969 if( x == N-1 )
970 iX1 = MIN(iX1, p.x);
971 if( y == 0 )
972 iY0 = MAX(iY0, p.y);
973 if( y == N-1 )
974 iY1 = MIN(iY1, p.y);
975 }
976 inner = cv::Rect_<float>(iX0, iY0, iX1-iX0, iY1-iY0);
977 outer = cv::Rect_<float>(oX0, oY0, oX1-oX0, oY1-oY0);
978}
979
980void cvStereoRectifyFisheye( const CvMat* _cameraMatrix1, const CvMat* _cameraMatrix2,
981 const CvMat* _distCoeffs1, const CvMat* _distCoeffs2,
982 CvSize imageSize, const CvMat* matR, const CvMat* matT,
983 CvMat* _R1, CvMat* _R2, CvMat* _P1, CvMat* _P2,
984 CvMat* matQ, int flags, double alpha, CvSize newImgSize )
985{
986 double _om[3], _t[3] = {0}, _uu[3]={0,0,0}, _r_r[3][3], _pp[3][4];
987 double _ww[3], _wr[3][3], _z[3] = {0,0,0}, _ri[3][3], _w3[3];
988 cv::Rect_<float> inner1, inner2, outer1, outer2;
989
990 CvMat om = cvMat(3, 1, CV_64F, _om);
991 CvMat t = cvMat(3, 1, CV_64F, _t);
992 CvMat uu = cvMat(3, 1, CV_64F, _uu);
993 CvMat r_r = cvMat(3, 3, CV_64F, _r_r);
994 CvMat pp = cvMat(3, 4, CV_64F, _pp);
995 CvMat ww = cvMat(3, 1, CV_64F, _ww); // temps
996 CvMat w3 = cvMat(3, 1, CV_64F, _w3); // temps
997 CvMat wR = cvMat(3, 3, CV_64F, _wr);
998 CvMat Z = cvMat(3, 1, CV_64F, _z);
999 CvMat Ri = cvMat(3, 3, CV_64F, _ri);
1000 double nx = imageSize.width, ny = imageSize.height;
1001 int i, k;
1002 double nt, nw;
1003
1004 if( matR->rows == 3 && matR->cols == 3 )
1005 cvRodrigues2(matR, &om); // get vector rotation
1006 else
1007 cvConvert(matR, &om); // it's already a rotation vector
1008 cvConvertScale(&om, &om, -0.5); // get average rotation
1009 cvRodrigues2(&om, &r_r); // rotate cameras to same orientation by averaging
1010
1011 cvMatMul(&r_r, matT, &t);
1012 int idx = fabs(_t[0]) > fabs(_t[1]) ? 0 : 1;
1013 // if idx == 0
1014 // e1 = T / ||T||
1015 // e2 = e1 x [0,0,1]
1016
1017 // if idx == 1
1018 // e2 = T / ||T||
1019 // e1 = e2 x [0,0,1]
1020
1021 // e3 = e1 x e2
1022 _uu[2] = 1;
1023 cvCrossProduct(&uu, &t, &ww);
1024 nt = cvNorm(&t, 0, CV_L2);
1025 CV_Assert(fabs(nt) > 0);
1026 nw = cvNorm(&ww, 0, CV_L2);
1027 CV_Assert(fabs(nw) > 0);
1028 cvConvertScale(&ww, &ww, 1 / nw);
1029 cvCrossProduct(&t, &ww, &w3);
1030 nw = cvNorm(&w3, 0, CV_L2);
1031 CV_Assert(fabs(nw) > 0);
1032 cvConvertScale(&w3, &w3, 1 / nw);
1033 _uu[2] = 0;
1034 for (i = 0; i < 3; ++i)
1035 {
1036 _wr[idx][i] = -_t[i] / nt;
1037 _wr[idx ^ 1][i] = -_ww[i];
1038 _wr[2][i] = _w3[i] * (1 - 2 * idx); // if idx == 1 -> opposite direction
1039 }
1040 // apply to both views
1041 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, CV_GEMM_B_T);
1042 cvConvert( &Ri, _R1 );
1043 cvGEMM(&wR, &r_r, 1, 0, 0, &Ri, 0);
1044 cvConvert( &Ri, _R2 );
1045 cvMatMul(&Ri, matT, &t);
1046 // calculate projection/camera matrices
1047 // these contain the relevant rectified image internal params (fx, fy=fx, cx, cy)
1048 double fc_new = DBL_MAX;
1049 CvPoint2D64f cc_new[2] = {};
1050
1051 newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imageSize;
1052 const double ratio_x = (double)newImgSize.width / imageSize.width / 2;
1053 const double ratio_y = (double)newImgSize.height / imageSize.height / 2;
1054 const double ratio = idx == 1 ? ratio_x : ratio_y;
1055 fc_new = (cvmGet(_cameraMatrix1, idx ^ 1, idx ^ 1) + cvmGet(_cameraMatrix2, idx ^ 1, idx ^ 1)) * ratio;
1056 for( k = 0; k < 2; k++ )
1057 {
1058 const CvMat* A = k == 0 ? _cameraMatrix1 : _cameraMatrix2;
1059 const CvMat* Dk = k == 0 ? _distCoeffs1 : _distCoeffs2;
1060 CvPoint2D32f _pts[4] = {};
1061 CvPoint3D32f _pts_3[4] = {};
1062 CvMat pts = cvMat(1, 4, CV_32FC2, _pts);
1063 CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
1064
1065 for( i = 0; i < 4; i++ )
1066 {
1067 int j = (i<2) ? 0 : 1;
1068 _pts[i].x = (float)((i % 2)*(nx));
1069 _pts[i].y = (float)(j*(ny));
1070 }
1071 cv::Mat ptsM(pts.rows, pts.cols, pts.type, pts.data.ptr);
1072 cv::Mat A_m(A->rows, A->cols, A->type, A->data.ptr);
1073 cv::Mat Dk_m(Dk->rows, Dk->cols, Dk->type, Dk->data.ptr);
1074 cv::fisheye::undistortPoints( ptsM, ptsM, A_m, Dk_m, cv::Mat(), cv::Mat() );
1075 cvConvertPointsHomogeneous( &pts, &pts_3 );
1076
1077 //Change camera matrix to have cc=[0,0] and fc = fc_new
1078 double _a_tmp[3][3];
1079 CvMat A_tmp = cvMat(3, 3, CV_64F, _a_tmp);
1080 _a_tmp[0][0]=fc_new;
1081 _a_tmp[1][1]=fc_new;
1082 _a_tmp[0][2]=0.0;
1083 _a_tmp[1][2]=0.0;
1084
1085 cvProjectPoints2( &pts_3, k == 0 ? _R1 : _R2, &Z, &A_tmp, 0, &pts );
1086 CvScalar avg = cvAvg(&pts);
1087
1088 cc_new[k].x = (nx)/2 - avg.val[0];
1089 cc_new[k].y = (ny)/2 - avg.val[1];
1090 }
1091
1092 // vertical focal length must be the same for both images to keep the epipolar constraint
1093 // (for horizontal epipolar lines -- TBD: check for vertical epipolar lines)
1094 // use fy for fx also, for simplicity
1095
1096 // For simplicity, set the principal points for both cameras to be the average
1097 // of the two principal points (either one of or both x- and y- coordinates)
1098 if( flags & cv::CALIB_ZERO_DISPARITY )
1099 {
1100 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
1101 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
1102 }
1103 else if( idx == 0 ) // horizontal stereo
1104 cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5;
1105 else // vertical stereo
1106 cc_new[0].x = cc_new[1].x = (cc_new[0].x + cc_new[1].x)*0.5;
1107
1108 cvZero( &pp );
1109 _pp[0][0] = _pp[1][1] = fc_new;
1110 _pp[0][2] = cc_new[0].x;
1111 _pp[1][2] = cc_new[0].y;
1112 _pp[2][2] = 1;
1113 cvConvert(&pp, _P1);
1114
1115 _pp[0][2] = cc_new[1].x;
1116 _pp[1][2] = cc_new[1].y;
1117 _pp[idx][3] = _t[idx]*fc_new; // baseline * focal length
1118 cvConvert(&pp, _P2);
1119
1120 alpha = MIN(alpha, 1.);
1121
1122 icvGetRectanglesFisheye( _cameraMatrix1, _distCoeffs1, _R1, _P1, imageSize, inner1, outer1 );
1123 icvGetRectanglesFisheye( _cameraMatrix2, _distCoeffs2, _R2, _P2, imageSize, inner2, outer2 );
1124
1125 {
1126 newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
1127 double cx1_0 = cc_new[0].x;
1128 double cy1_0 = cc_new[0].y;
1129 double cx2_0 = cc_new[1].x;
1130 double cy2_0 = cc_new[1].y;
1131 double cx1 = newImgSize.width*cx1_0/imageSize.width;
1132 double cy1 = newImgSize.height*cy1_0/imageSize.height;
1133 double cx2 = newImgSize.width*cx2_0/imageSize.width;
1134 double cy2 = newImgSize.height*cy2_0/imageSize.height;
1135 double s = 1.;
1136
1137 if( alpha >= 0 )
1138 {
1139 double s0 = std::max(std::max(std::max((double)cx1/(cx1_0 - inner1.x), (double)cy1/(cy1_0 - inner1.y)),
1140 (double)(newImgSize.width - cx1)/(inner1.x + inner1.width - cx1_0)),
1141 (double)(newImgSize.height - cy1)/(inner1.y + inner1.height - cy1_0));
1142 s0 = std::max(std::max(std::max(std::max((double)cx2/(cx2_0 - inner2.x), (double)cy2/(cy2_0 - inner2.y)),
1143 (double)(newImgSize.width - cx2)/(inner2.x + inner2.width - cx2_0)),
1144 (double)(newImgSize.height - cy2)/(inner2.y + inner2.height - cy2_0)),
1145 s0);
1146
1147 double s1 = std::min(std::min(std::min((double)cx1/(cx1_0 - outer1.x), (double)cy1/(cy1_0 - outer1.y)),
1148 (double)(newImgSize.width - cx1)/(outer1.x + outer1.width - cx1_0)),
1149 (double)(newImgSize.height - cy1)/(outer1.y + outer1.height - cy1_0));
1150 s1 = std::min(std::min(std::min(std::min((double)cx2/(cx2_0 - outer2.x), (double)cy2/(cy2_0 - outer2.y)),
1151 (double)(newImgSize.width - cx2)/(outer2.x + outer2.width - cx2_0)),
1152 (double)(newImgSize.height - cy2)/(outer2.y + outer2.height - cy2_0)),
1153 s1);
1154
1155 s = s0*(1 - alpha) + s1*alpha;
1156 }
1157
1158 fc_new *= s;
1159 cc_new[0] = cvPoint2D64f(cx1, cy1);
1160 cc_new[1] = cvPoint2D64f(cx2, cy2);
1161
1162 cvmSet(_P1, 0, 0, fc_new);
1163 cvmSet(_P1, 1, 1, fc_new);
1164 cvmSet(_P1, 0, 2, cx1);
1165 cvmSet(_P1, 1, 2, cy1);
1166
1167 cvmSet(_P2, 0, 0, fc_new);
1168 cvmSet(_P2, 1, 1, fc_new);
1169 cvmSet(_P2, 0, 2, cx2);
1170 cvmSet(_P2, 1, 2, cy2);
1171 cvmSet(_P2, idx, 3, s*cvmGet(_P2, idx, 3));
1172
1173 }
1174
1175 if( matQ )
1176 {
1177 double q[] =
1178 {
1179 1, 0, 0, -cc_new[0].x,
1180 0, 1, 0, -cc_new[0].y,
1181 0, 0, 0, fc_new,
1182 0, 0, -1./_t[idx],
1183 (idx == 0 ? cc_new[0].x - cc_new[1].x : cc_new[0].y - cc_new[1].y)/_t[idx]
1184 };
1185 CvMat Q = cvMat(4, 4, CV_64F, q);
1186 cvConvert( &Q, matQ );
1187 }
1188}
1189
1190void stereoRectifyFisheye( cv::InputArray _cameraMatrix1, cv::InputArray _distCoeffs1,
1191 cv::InputArray _cameraMatrix2, cv::InputArray _distCoeffs2,
1192 cv::Size imageSize, cv::InputArray _Rmat, cv::InputArray _Tmat,
1193 cv::OutputArray _Rmat1, cv::OutputArray _Rmat2,
1194 cv::OutputArray _Pmat1, cv::OutputArray _Pmat2,
1195 cv::OutputArray _Qmat, int flags,
1196 double alpha, cv::Size newImageSize)
1197{
1198 cv::Mat cameraMatrix1 = _cameraMatrix1.getMat(), cameraMatrix2 = _cameraMatrix2.getMat();
1199 cv::Mat distCoeffs1 = _distCoeffs1.getMat(), distCoeffs2 = _distCoeffs2.getMat();
1200 cv::Mat Rmat = _Rmat.getMat(), Tmat = _Tmat.getMat();
1201
1202#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
1203 CvMat c_cameraMatrix1 = cvMat(cameraMatrix1);
1204 CvMat c_cameraMatrix2 = cvMat(cameraMatrix2);
1205 CvMat c_distCoeffs1 = cvMat(distCoeffs1);
1206 CvMat c_distCoeffs2 = cvMat(distCoeffs2);
1207 CvMat c_R = cvMat(Rmat), c_T = cvMat(Tmat);
1208#else
1209 CvMat c_cameraMatrix1 = CvMat(cameraMatrix1);
1210 CvMat c_cameraMatrix2 = CvMat(cameraMatrix2);
1211 CvMat c_distCoeffs1 = CvMat(distCoeffs1);
1212 CvMat c_distCoeffs2 = CvMat(distCoeffs2);
1213 CvMat c_R = CvMat(Rmat), c_T = CvMat(Tmat);
1214#endif
1215
1216 int rtype = CV_64F;
1217 _Rmat1.create(3, 3, rtype);
1218 _Rmat2.create(3, 3, rtype);
1219 _Pmat1.create(3, 4, rtype);
1220 _Pmat2.create(3, 4, rtype);
1221 cv::Mat R1 = _Rmat1.getMat(), R2 = _Rmat2.getMat(), P1 = _Pmat1.getMat(), P2 = _Pmat2.getMat(), Q;
1222#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
1223 CvMat c_R1 = cvMat(R1), c_R2 = cvMat(R2), c_P1 = cvMat(P1), c_P2 = cvMat(P2);
1224#else
1225 CvMat c_R1 = CvMat(R1), c_R2 = CvMat(R2), c_P1 = CvMat(P1), c_P2 = CvMat(P2);
1226#endif
1227 CvMat c_Q, *p_Q = 0;
1228
1229 if( _Qmat.needed() )
1230 {
1231 _Qmat.create(4, 4, rtype);
1232#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
1233 p_Q = &(c_Q = cvMat(Q = _Qmat.getMat()));
1234#else
1235 p_Q = &(c_Q = CvMat(Q = _Qmat.getMat()));
1236#endif
1237 }
1238
1239 CvMat *p_distCoeffs1 = distCoeffs1.empty() ? NULL : &c_distCoeffs1;
1240 CvMat *p_distCoeffs2 = distCoeffs2.empty() ? NULL : &c_distCoeffs2;
1241 cvStereoRectifyFisheye( &c_cameraMatrix1, &c_cameraMatrix2, p_distCoeffs1, p_distCoeffs2,
1242#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION >= 3 && (CV_MINOR_VERSION>4 || (CV_MINOR_VERSION>=4 && CV_SUBMINOR_VERSION>=4)))
1243 cvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
1244 cvSize(newImageSize));
1245#else
1246 CvSize(imageSize), &c_R, &c_T, &c_R1, &c_R2, &c_P1, &c_P2, p_Q, flags, alpha,
1247 CvSize(newImageSize));
1248#endif
1249}
1250
1251}
1252
1253
1254#endif /* CORELIB_SRC_OPENCV_STEREORECTIFYFISHEYE_H_ */