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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:22