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 // License Agreement
11 // For Open Source Computer Vision Library
12 //
13 // Copyright (C) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University,
14 // all rights reserved.
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 the copyright holders 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 /**
43 * This module was accepted as a GSoC 2015 project for OpenCV, authored by
44 * Baisheng Lai, mentored by Bo Li.
45 *
46 * The omnidirectional camera in this module is denoted by the catadioptric
47 * model. Please refer to Mei's paper for details of the camera model:
48 *
49 * C. Mei and P. Rives, "Single view point omnidirectional camera
50 * calibration from planar grids", in ICRA 2007.
51 *
52 * The implementation of the calibration part is based on Li's calibration
53 * toolbox:
54 *
55 * B. Li, L. Heng, K. Kevin and M. Pollefeys, "A Multiple-Camera System
56 * Calibration Toolbox Using A Feature Descriptor-Based Calibration
57 * Pattern", in IROS 2013.
58 */
59 #include "precomp.hpp"
60 #include "opencv2/ccalib/omnidir.hpp"
61 #include <fstream>
62 #include <iostream>
63 namespace cv { namespace
64 {
65 struct JacobianRow
66 {
67 Matx13d dom,dT;
68 Matx12d df;
69 double ds;
70 Matx12d dc;
71 double dxi;
72 Matx14d dkp; // distortion k1,k2,p1,p2
73 };
74 }}
75
76 /////////////////////////////////////////////////////////////////////////////
77 //////// projectPoints
projectPoints(InputArray objectPoints,OutputArray imagePoints,const Affine3d & affine,InputArray K,double xi,InputArray D,OutputArray jacobian)78 void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints,
79 const Affine3d& affine, InputArray K, double xi, InputArray D, OutputArray jacobian)
80 {
81 projectPoints(objectPoints, imagePoints, affine.rvec(), affine.translation(), K, xi, D, jacobian);
82 }
83
projectPoints(InputArray objectPoints,OutputArray imagePoints,InputArray rvec,InputArray tvec,InputArray K,double xi,InputArray D,OutputArray jacobian)84 void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints,
85 InputArray rvec, InputArray tvec, InputArray K, double xi, InputArray D, OutputArray jacobian)
86 {
87
88 CV_Assert(objectPoints.type() == CV_64FC3 || objectPoints.type() == CV_32FC3);
89 CV_Assert((rvec.depth() == CV_64F || rvec.depth() == CV_32F) && rvec.total() == 3);
90 CV_Assert((tvec.depth() == CV_64F || tvec.depth() == CV_32F) && tvec.total() == 3);
91 CV_Assert((K.type() == CV_64F || K.type() == CV_32F) && K.size() == Size(3,3));
92 CV_Assert((D.type() == CV_64F || D.type() == CV_32F) && D.total() == 4);
93
94 imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
95
96 int n = (int)objectPoints.total();
97
98 Vec3d om = rvec.depth() == CV_32F ? (Vec3d)*rvec.getMat().ptr<Vec3f>() : *rvec.getMat().ptr<Vec3d>();
99 Vec3d T = tvec.depth() == CV_32F ? (Vec3d)*tvec.getMat().ptr<Vec3f>() : *tvec.getMat().ptr<Vec3d>();
100
101 Vec2d f,c;
102 double s;
103 if (K.depth() == CV_32F)
104 {
105 Matx33f Kc = K.getMat();
106 f = Vec2f(Kc(0,0), Kc(1,1));
107 c = Vec2f(Kc(0,2),Kc(1,2));
108 s = (double)Kc(0,1);
109 }
110 else
111 {
112 Matx33d Kc = K.getMat();
113 f = Vec2d(Kc(0,0), Kc(1,1));
114 c = Vec2d(Kc(0,2),Kc(1,2));
115 s = Kc(0,1);
116 }
117
118 Vec4d kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>() : *D.getMat().ptr<Vec4d>();
119 //Vec<double, 4> kp= (Vec<double,4>)*D.getMat().ptr<Vec<double,4> >();
120
121 const Vec3d* Xw_alld = objectPoints.getMat().ptr<Vec3d>();
122 const Vec3f* Xw_allf = objectPoints.getMat().ptr<Vec3f>();
123 Vec2d* xpd = imagePoints.getMat().ptr<Vec2d>();
124 Vec2f* xpf = imagePoints.getMat().ptr<Vec2f>();
125
126 Matx33d R;
127 Matx<double, 3, 9> dRdom;
128 Rodrigues(om, R, dRdom);
129
130 JacobianRow *Jn = 0;
131 if (jacobian.needed())
132 {
133 int nvars = 2+2+1+4+3+3+1; // f,c,s,kp,om,T,xi
134 jacobian.create(2*int(n), nvars, CV_64F);
135 Jn = jacobian.getMat().ptr<JacobianRow>(0);
136 }
137
138 double k1=kp[0],k2=kp[1];
139 double p1 = kp[2], p2 = kp[3];
140
141 for (int i = 0; i < n; i++)
142 {
143 // convert to camera coordinate
144 Vec3d Xw = objectPoints.depth() == CV_32F ? (Vec3d)Xw_allf[i] : Xw_alld[i];
145
146 Vec3d Xc = (Vec3d)(R*Xw + T);
147
148 // convert to unit sphere
149 Vec3d Xs = Xc/cv::norm(Xc);
150
151 // convert to normalized image plane
152 Vec2d xu = Vec2d(Xs[0]/(Xs[2]+xi), Xs[1]/(Xs[2]+xi));
153
154 // add distortion
155 Vec2d xd;
156 double r2 = xu[0]*xu[0]+xu[1]*xu[1];
157 double r4 = r2*r2;
158
159 xd[0] = xu[0]*(1+k1*r2+k2*r4) + 2*p1*xu[0]*xu[1] + p2*(r2+2*xu[0]*xu[0]);
160 xd[1] = xu[1]*(1+k1*r2+k2*r4) + p1*(r2+2*xu[1]*xu[1]) + 2*p2*xu[0]*xu[1];
161
162 // convert to pixel coordinate
163 Vec2d final;
164 final[0] = f[0]*xd[0]+s*xd[1]+c[0];
165 final[1] = f[1]*xd[1]+c[1];
166
167 if (objectPoints.depth() == CV_32F)
168 {
169 xpf[i] = final;
170 }
171 else
172 {
173 xpd[i] = final;
174 }
175 /*xpd[i][0] = f[0]*xd[0]+s*xd[1]+c[0];
176 xpd[i][1] = f[1]*xd[1]+c[1];*/
177
178 if (jacobian.needed())
179 {
180 double dXcdR_a[] = {Xw[0],Xw[1],Xw[2],0,0,0,0,0,0,
181 0,0,0,Xw[0],Xw[1],Xw[2],0,0,0,
182 0,0,0,0,0,0,Xw[0],Xw[1],Xw[2]};
183 Matx<double,3, 9> dXcdR(dXcdR_a);
184 Matx33d dXcdom = dXcdR * dRdom.t();
185 double r_1 = 1.0/norm(Xc);
186 double r_3 = pow(r_1,3);
187 Matx33d dXsdXc(r_1-Xc[0]*Xc[0]*r_3, -(Xc[0]*Xc[1])*r_3, -(Xc[0]*Xc[2])*r_3,
188 -(Xc[0]*Xc[1])*r_3, r_1-Xc[1]*Xc[1]*r_3, -(Xc[1]*Xc[2])*r_3,
189 -(Xc[0]*Xc[2])*r_3, -(Xc[1]*Xc[2])*r_3, r_1-Xc[2]*Xc[2]*r_3);
190 Matx23d dxudXs(1/(Xs[2]+xi), 0, -Xs[0]/(Xs[2]+xi)/(Xs[2]+xi),
191 0, 1/(Xs[2]+xi), -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi));
192 // pre-compute some reusable things
193 double temp1 = 2*k1*xu[0] + 4*k2*xu[0]*r2;
194 double temp2 = 2*k1*xu[1] + 4*k2*xu[1]*r2;
195 Matx22d dxddxu(k2*r4+6*p2*xu[0]+2*p1*xu[1]+xu[0]*temp1+k1*r2+1, 2*p1*xu[0]+2*p2*xu[1]+xu[0]*temp2,
196 2*p1*xu[0]+2*p2*xu[1]+xu[1]*temp1, k2*r4+2*p2*xu[0]+6*p1*xu[1]+xu[1]*temp2+k1*r2+1);
197 Matx22d dxpddxd(f[0], s,
198 0, f[1]);
199 Matx23d dxpddXc = dxpddxd * dxddxu * dxudXs * dXsdXc;
200
201 // derivative of xpd respect to om
202 Matx23d dxpddom = dxpddXc * dXcdom;
203 Matx33d dXcdT(1.0,0.0,0.0,
204 0.0,1.0,0.0,
205 0.0,0.0,1.0);
206 // derivative of xpd respect to T
207
208 Matx23d dxpddT = dxpddXc * dXcdT;
209 Matx21d dxudxi(-Xs[0]/(Xs[2]+xi)/(Xs[2]+xi),
210 -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi));
211
212 // derivative of xpd respect to xi
213 Matx21d dxpddxi = dxpddxd * dxddxu * dxudxi;
214 Matx<double,2,4> dxddkp(xu[0]*r2, xu[0]*r4, 2*xu[0]*xu[1], r2+2*xu[0]*xu[0],
215 xu[1]*r2, xu[1]*r4, r2+2*xu[1]*xu[1], 2*xu[0]*xu[1]);
216
217 // derivative of xpd respect to kp
218 Matx<double,2,4> dxpddkp = dxpddxd * dxddkp;
219
220 // derivative of xpd respect to f
221 Matx22d dxpddf(xd[0], 0,
222 0, xd[1]);
223
224 // derivative of xpd respect to c
225 Matx22d dxpddc(1, 0,
226 0, 1);
227
228 Jn[0].dom = dxpddom.row(0);
229 Jn[1].dom = dxpddom.row(1);
230 Jn[0].dT = dxpddT.row(0);
231 Jn[1].dT = dxpddT.row(1);
232 Jn[0].dkp = dxpddkp.row(0);
233 Jn[1].dkp = dxpddkp.row(1);
234 Jn[0].dxi = dxpddxi(0,0);
235 Jn[1].dxi = dxpddxi(1,0);
236 Jn[0].df = dxpddf.row(0);
237 Jn[1].df = dxpddf.row(1);
238 Jn[0].dc = dxpddc.row(0);
239 Jn[1].dc = dxpddc.row(1);
240 Jn[0].ds = xd[1];
241 Jn[1].ds = 0;
242 Jn += 2;
243 }
244 }
245 }
246
247 /////////////////////////////////////////////////////////////////////////////
248 //////// undistortPoints
undistortPoints(InputArray distorted,OutputArray undistorted,InputArray K,InputArray D,InputArray xi,InputArray R)249 void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted,
250 InputArray K, InputArray D, InputArray xi, InputArray R)
251 {
252 CV_Assert(distorted.type() == CV_64FC2 || distorted.type() == CV_32FC2);
253 CV_Assert(R.empty() || (!R.empty() && (R.size() == Size(3, 3) || R.total() * R.channels() == 3)
254 && (R.depth() == CV_64F || R.depth() == CV_32F)));
255 CV_Assert((D.depth() == CV_64F || D.depth() == CV_32F) && D.total() == 4);
256 CV_Assert(K.size() == Size(3, 3) && (K.depth() == CV_64F || K.depth() == CV_32F));
257 CV_Assert(xi.total() == 1 && (xi.depth() == CV_64F || xi.depth() == CV_32F));
258
259 undistorted.create(distorted.size(), distorted.type());
260
261 cv::Vec2d f, c;
262 double s = 0.0;
263 if (K.depth() == CV_32F)
264 {
265 Matx33f camMat = K.getMat();
266 f = Vec2f(camMat(0,0), camMat(1,1));
267 c = Vec2f(camMat(0,2), camMat(1,2));
268 s = (double)camMat(0,1);
269 }
270 else if (K.depth() == CV_64F)
271 {
272 Matx33d camMat = K.getMat();
273 f = Vec2d(camMat(0,0), camMat(1,1));
274 c = Vec2d(camMat(0,2), camMat(1,2));
275 s = camMat(0,1);
276 }
277
278 Vec4d kp = D.depth()==CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>():(Vec4d)*D.getMat().ptr<Vec4d>();
279 Vec2d k = Vec2d(kp[0], kp[1]);
280 Vec2d p = Vec2d(kp[2], kp[3]);
281
282 double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr<float>() : *xi.getMat().ptr<double>();
283 cv::Matx33d RR = cv::Matx33d::eye();
284 // R is om
285 if(!R.empty() && R.total()*R.channels() == 3)
286 {
287 cv::Vec3d rvec;
288 R.getMat().convertTo(rvec, CV_64F);
289 cv::Rodrigues(rvec, RR);
290 }
291 else if (!R.empty() && R.size() == Size(3,3))
292 {
293 R.getMat().convertTo(RR, CV_64F);
294 }
295
296 const cv::Vec2d *srcd = distorted.getMat().ptr<cv::Vec2d>();
297 const cv::Vec2f *srcf = distorted.getMat().ptr<cv::Vec2f>();
298
299 cv::Vec2d *dstd = undistorted.getMat().ptr<cv::Vec2d>();
300 cv::Vec2f *dstf = undistorted.getMat().ptr<cv::Vec2f>();
301
302 int n = (int)distorted.total();
303 for (int i = 0; i < n; i++)
304 {
305 Vec2d pi = distorted.depth() == CV_32F ? (Vec2d)srcf[i]:(Vec2d)srcd[i]; // image point
306 Vec2d pp((pi[0]*f[1]-c[0]*f[1]-s*(pi[1]-c[1]))/(f[0]*f[1]), (pi[1]-c[1])/f[1]); //plane
307 Vec2d pu = pp; // points without distortion
308
309 // remove distortion iteratively
310 for (int j = 0; j < 20; j++)
311 {
312 double r2 = pu[0]*pu[0] + pu[1]*pu[1];
313 double r4 = r2*r2;
314 pu[0] = (pp[0] - 2*p[0]*pu[0]*pu[1] - p[1]*(r2+2*pu[0]*pu[0])) / (1 + k[0]*r2 + k[1]*r4);
315 pu[1] = (pp[1] - 2*p[1]*pu[0]*pu[1] - p[0]*(r2+2*pu[1]*pu[1])) / (1 + k[0]*r2 + k[1]*r4);
316 }
317
318 // project to unit sphere
319 double r2 = pu[0]*pu[0] + pu[1]*pu[1];
320 double a = (r2 + 1);
321 double b = 2*_xi*r2;
322 double cc = r2*_xi*_xi-1;
323 double Zs = (-b + sqrt(b*b - 4*a*cc))/(2*a);
324 Vec3d Xw = Vec3d(pu[0]*(Zs + _xi), pu[1]*(Zs +_xi), Zs);
325
326 // rotate
327 Xw = RR * Xw;
328
329 // project back to sphere
330 Vec3d Xs = Xw / cv::norm(Xw);
331
332 // reproject to camera plane
333 Vec3d ppu = Vec3d(Xs[0]/Xs[2], Xs[1]/Xs[2], 1.0);
334 if (undistorted.depth() == CV_32F)
335 {
336 dstf[i] = Vec2f((float)ppu[0], (float)ppu[1]);
337 }
338 else if (undistorted.depth() == CV_64F)
339 {
340 dstd[i] = Vec2d(ppu[0], ppu[1]);
341 }
342 }
343 }
344
345
346 /////////////////////////////////////////////////////////////////////////////
347 //////// cv::omnidir::initUndistortRectifyMap
initUndistortRectifyMap(InputArray K,InputArray D,InputArray xi,InputArray R,InputArray P,const cv::Size & size,int m1type,OutputArray map1,OutputArray map2,int flags)348 void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, InputArray xi, InputArray R, InputArray P,
349 const cv::Size& size, int m1type, OutputArray map1, OutputArray map2, int flags)
350 {
351 CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 );
352 map1.create( size, m1type <= 0 ? CV_16SC2 : m1type );
353 map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F );
354
355 CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F));
356 CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4));
357 CV_Assert(P.empty()|| (P.depth() == CV_32F || P.depth() == CV_64F));
358 CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3));
359 CV_Assert(R.empty() || (R.depth() == CV_32F || R.depth() == CV_64F));
360 CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3);
361 CV_Assert(flags == RECTIFY_PERSPECTIVE || flags == RECTIFY_CYLINDRICAL || flags == RECTIFY_LONGLATI
362 || flags == RECTIFY_STEREOGRAPHIC);
363 CV_Assert(xi.total() == 1 && (xi.depth() == CV_32F || xi.depth() == CV_64F));
364
365 cv::Vec2d f, c;
366 double s;
367 if (K.depth() == CV_32F)
368 {
369 Matx33f camMat = K.getMat();
370 f = Vec2f(camMat(0, 0), camMat(1, 1));
371 c = Vec2f(camMat(0, 2), camMat(1, 2));
372 s = (double)camMat(0,1);
373 }
374 else
375 {
376 Matx33d camMat = K.getMat();
377 f = Vec2d(camMat(0, 0), camMat(1, 1));
378 c = Vec2d(camMat(0, 2), camMat(1, 2));
379 s = camMat(0,1);
380 }
381
382 Vec4d kp = Vec4d::all(0);
383 if (!D.empty())
384 kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>();
385 double _xi = xi.depth() == CV_32F ? (double)*xi.getMat().ptr<float>() : *xi.getMat().ptr<double>();
386 Vec2d k = Vec2d(kp[0], kp[1]);
387 Vec2d p = Vec2d(kp[2], kp[3]);
388 cv::Matx33d RR = cv::Matx33d::eye();
389 if (!R.empty() && R.total() * R.channels() == 3)
390 {
391 cv::Vec3d rvec;
392 R.getMat().convertTo(rvec, CV_64F);
393 cv::Rodrigues(rvec, RR);
394 }
395 else if (!R.empty() && R.size() == Size(3, 3))
396 R.getMat().convertTo(RR, CV_64F);
397
398 cv::Matx33d PP = cv::Matx33d::eye();
399 if (!P.empty())
400 P.getMat().colRange(0, 3).convertTo(PP, CV_64F);
401 else
402 PP = K.getMat();
403
404 cv::Matx33d iKR = (PP*RR).inv(cv::DECOMP_SVD);
405 cv::Matx33d iK = PP.inv(cv::DECOMP_SVD);
406 cv::Matx33d iR = RR.inv(cv::DECOMP_SVD);
407
408 if (flags == omnidir::RECTIFY_PERSPECTIVE)
409 {
410 for (int i = 0; i < size.height; ++i)
411 {
412 float* m1f = map1.getMat().ptr<float>(i);
413 float* m2f = map2.getMat().ptr<float>(i);
414 short* m1 = (short*)m1f;
415 ushort* m2 = (ushort*)m2f;
416
417 double _x = i*iKR(0, 1) + iKR(0, 2),
418 _y = i*iKR(1, 1) + iKR(1, 2),
419 _w = i*iKR(2, 1) + iKR(2, 2);
420 for(int j = 0; j < size.width; ++j, _x+=iKR(0,0), _y+=iKR(1,0), _w+=iKR(2,0))
421 {
422 // project back to unit sphere
423 double r = sqrt(_x*_x + _y*_y + _w*_w);
424 double Xs = _x / r;
425 double Ys = _y / r;
426 double Zs = _w / r;
427 // project to image plane
428 double xu = Xs / (Zs + _xi),
429 yu = Ys / (Zs + _xi);
430 // add distortion
431 double r2 = xu*xu + yu*yu;
432 double r4 = r2*r2;
433 double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu);
434 double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu;
435 // to image pixel
436 double u = f[0]*xd + s*yd + c[0];
437 double v = f[1]*yd + c[1];
438
439 if( m1type == CV_16SC2 )
440 {
441 int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE);
442 int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE);
443 m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
444 m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
445 m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
446 }
447 else if( m1type == CV_32FC1 )
448 {
449 m1f[j] = (float)u;
450 m2f[j] = (float)v;
451 }
452 }
453 }
454 }
455 else if(flags == omnidir::RECTIFY_CYLINDRICAL || flags == omnidir::RECTIFY_LONGLATI ||
456 flags == omnidir::RECTIFY_STEREOGRAPHIC)
457 {
458 for (int i = 0; i < size.height; ++i)
459 {
460 float* m1f = map1.getMat().ptr<float>(i);
461 float* m2f = map2.getMat().ptr<float>(i);
462 short* m1 = (short*)m1f;
463 ushort* m2 = (ushort*)m2f;
464
465 // for RECTIFY_LONGLATI, theta and h are longittude and latitude
466 double theta = i*iK(0, 1) + iK(0, 2),
467 h = i*iK(1, 1) + iK(1, 2);
468
469 for (int j = 0; j < size.width; ++j, theta+=iK(0,0), h+=iK(1,0))
470 {
471 double _xt = 0.0, _yt = 0.0, _wt = 0.0;
472 if (flags == omnidir::RECTIFY_CYLINDRICAL)
473 {
474 //_xt = std::sin(theta);
475 //_yt = h;
476 //_wt = std::cos(theta);
477 _xt = std::cos(theta);
478 _yt = std::sin(theta);
479 _wt = h;
480 }
481 else if (flags == omnidir::RECTIFY_LONGLATI)
482 {
483 _xt = -std::cos(theta);
484 _yt = -std::sin(theta) * std::cos(h);
485 _wt = std::sin(theta) * std::sin(h);
486 }
487 else if (flags == omnidir::RECTIFY_STEREOGRAPHIC)
488 {
489 double a = theta*theta + h*h + 4;
490 double b = -2*theta*theta - 2*h*h;
491 double c2 = theta*theta + h*h -4;
492
493 _yt = (-b-std::sqrt(b*b - 4*a*c2))/(2*a);
494 _xt = theta*(1 - _yt) / 2;
495 _wt = h*(1 - _yt) / 2;
496 }
497 double _x = iR(0,0)*_xt + iR(0,1)*_yt + iR(0,2)*_wt;
498 double _y = iR(1,0)*_xt + iR(1,1)*_yt + iR(1,2)*_wt;
499 double _w = iR(2,0)*_xt + iR(2,1)*_yt + iR(2,2)*_wt;
500
501 double r = sqrt(_x*_x + _y*_y + _w*_w);
502 double Xs = _x / r;
503 double Ys = _y / r;
504 double Zs = _w / r;
505 // project to image plane
506 double xu = Xs / (Zs + _xi),
507 yu = Ys / (Zs + _xi);
508 // add distortion
509 double r2 = xu*xu + yu*yu;
510 double r4 = r2*r2;
511 double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu);
512 double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu;
513 // to image pixel
514 double u = f[0]*xd + s*yd + c[0];
515 double v = f[1]*yd + c[1];
516
517 if( m1type == CV_16SC2 )
518 {
519 int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE);
520 int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE);
521 m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
522 m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
523 m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
524 }
525 else if( m1type == CV_32FC1 )
526 {
527 m1f[j] = (float)u;
528 m2f[j] = (float)v;
529 }
530 }
531 }
532 }
533 }
534
535 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
536 /// cv::omnidir::undistortImage
537
undistortImage(InputArray distorted,OutputArray undistorted,InputArray K,InputArray D,InputArray xi,int flags,InputArray Knew,const Size & new_size,InputArray R)538 void cv::omnidir::undistortImage(InputArray distorted, OutputArray undistorted,
539 InputArray K, InputArray D, InputArray xi, int flags, InputArray Knew, const Size& new_size, InputArray R)
540 {
541 Size size = new_size.empty() ? distorted.size() : new_size;
542
543 cv::Mat map1, map2;
544 omnidir::initUndistortRectifyMap(K, D, xi, R, Knew, size, CV_16SC2, map1, map2, flags);
545 cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT);
546 }
547
548 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
549 /// cv::omnidir::internal::initializeCalibration
550
initializeCalibration(InputArrayOfArrays patternPoints,InputArrayOfArrays imagePoints,Size size,OutputArrayOfArrays omAll,OutputArrayOfArrays tAll,OutputArray K,double & xi,OutputArray idx)551 void cv::omnidir::internal::initializeCalibration(InputArrayOfArrays patternPoints, InputArrayOfArrays imagePoints, Size size,
552 OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray K, double& xi, OutputArray idx)
553 {
554 // For details please refer to Section III from Li's IROS 2013 paper
555
556 double u0 = size.width / 2;
557 double v0 = size.height / 2;
558
559 int n_img = (int)imagePoints.total();
560
561 std::vector<cv::Vec3d> v_omAll(n_img), v_tAll(n_img);
562
563 std::vector<double> gammaAll(n_img);
564
565 K.create(3, 3, CV_64F);
566 Mat _K;
567 for (int image_idx = 0; image_idx < n_img; ++image_idx)
568 {
569 cv::Mat objPoints, imgPoints;
570 patternPoints.getMat(image_idx).copyTo(objPoints);
571 imagePoints.getMat(image_idx).copyTo(imgPoints);
572
573 int n_point = imgPoints.rows * imgPoints.cols;
574 if (objPoints.rows != n_point)
575 objPoints = objPoints.reshape(3, n_point);
576 if (imgPoints.rows != n_point)
577 imgPoints = imgPoints.reshape(2, n_point);
578
579 // objectPoints should be 3-channel data, imagePoints should be 2-channel data
580 CV_Assert(objPoints.type() == CV_64FC3 && imgPoints.type() == CV_64FC2 );
581
582 std::vector<cv::Mat> xy, uv;
583 cv::split(objPoints, xy);
584 cv::split(imgPoints, uv);
585
586
587 cv::Mat x = xy[0].reshape(1, n_point), y = xy[1].reshape(1, n_point),
588 u = uv[0].reshape(1, n_point) - u0, v = uv[1].reshape(1, n_point) - v0;
589
590 cv::Mat sqrRho = u.mul(u) + v.mul(v);
591 // compute extrinsic parameters
592 cv::Mat M(n_point, 6, CV_64F);
593 Mat(-v.mul(x)).copyTo(M.col(0));
594 Mat(-v.mul(y)).copyTo(M.col(1));
595 Mat(u.mul(x)).copyTo(M.col(2));
596 Mat(u.mul(y)).copyTo(M.col(3));
597 Mat(-v).copyTo(M.col(4));
598 Mat(u).copyTo(M.col(5));
599
600 Mat W,U,V;
601 cv::SVD::compute(M, W, U, V,SVD::FULL_UV);
602 V = V.t();
603
604 double miniReprojectError = 1e5;
605 // the signs of r1, r2, r3 are unknown, so they can be flipped.
606 for (int coef = 1; coef >= -1; coef-=2)
607 {
608 double r11 = V.at<double>(0, 5) * coef;
609 double r12 = V.at<double>(1, 5) * coef;
610 double r21 = V.at<double>(2, 5) * coef;
611 double r22 = V.at<double>(3, 5) * coef;
612 double t1 = V.at<double>(4, 5) * coef;
613 double t2 = V.at<double>(5, 5) * coef;
614
615 Mat roots;
616 double r31s;
617 solvePoly(Matx13d(-(r11*r12+r21*r22)*(r11*r12+r21*r22), r11*r11+r21*r21-r12*r12-r22*r22, 1), roots);
618
619 if (roots.at<Vec2d>(0)[0] > 0)
620 r31s = sqrt(roots.at<Vec2d>(0)[0]);
621 else
622 r31s = sqrt(roots.at<Vec2d>(1)[0]);
623
624 for (int coef2 = 1; coef2 >= -1; coef2-=2)
625 {
626 double r31 = r31s * coef2;
627 double r32 = -(r11*r12 + r21*r22) / r31;
628
629 cv::Vec3d r1(r11, r21, r31);
630 cv::Vec3d r2(r12, r22, r32);
631 cv::Vec3d t(t1, t2, 0);
632 double scale = 1 / cv::norm(r1);
633 r1 = r1 * scale;
634 r2 = r2 * scale;
635 t = t * scale;
636
637 // compute intrisic parameters
638 // Form equations in Scaramuzza's paper
639 // A Toolbox for Easily Calibrating Omnidirectional Cameras
640 Mat A(n_point*2, 3, CV_64F);
641 Mat((r1[1]*x + r2[1]*y + t[1])/2).copyTo(A.rowRange(0, n_point).col(0));
642 Mat((r1[0]*x + r2[0]*y + t[0])/2).copyTo(A.rowRange(n_point, 2*n_point).col(0));
643 Mat(-A.col(0).rowRange(0, n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(0, n_point));
644 Mat(-A.col(0).rowRange(n_point, 2*n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(n_point, 2*n_point));
645 Mat(-v).copyTo(A.rowRange(0, n_point).col(2));
646 Mat(-u).copyTo(A.rowRange(n_point, 2*n_point).col(2));
647
648 // Operation to avoid bad numerical-condition of A
649 Vec3d maxA, minA;
650 for (int j = 0; j < A.cols; j++)
651 {
652 cv::minMaxLoc(cv::abs(A.col(j)), &minA[j], &maxA[j]);
653 A.col(j) = A.col(j) / maxA[j];
654 }
655
656 Mat B(n_point*2 , 1, CV_64F);
657 Mat(v.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(0, n_point));
658 Mat(u.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(n_point, 2*n_point));
659
660 Mat res = A.inv(DECOMP_SVD) * B;
661 res = res.mul(1/Mat(maxA));
662
663 double gamma = sqrt(res.at<double>(0) / res.at<double>(1));
664 t[2] = res.at<double>(2);
665
666 cv::Vec3d r3 = r1.cross(r2);
667
668 Matx33d R(r1[0], r2[0], r3[0],
669 r1[1], r2[1], r3[1],
670 r1[2], r2[2], r3[2]);
671 Vec3d om;
672 Rodrigues(R, om);
673
674 // project pattern points to images
675 Mat projedImgPoints;
676 Matx33d Kc(gamma, 0, u0, 0, gamma, v0, 0, 0, 1);
677
678 // reproject error
679 cv::omnidir::projectPoints(objPoints, projedImgPoints, om, t, Kc, 1, Matx14d(0, 0, 0, 0), cv::noArray());
680 double reprojectError = omnidir::internal::computeMeanReproErr(imgPoints, projedImgPoints);
681
682 // if this reproject error is smaller
683 if (reprojectError < miniReprojectError)
684 {
685 miniReprojectError = reprojectError;
686 v_omAll[image_idx] = om;
687 v_tAll[image_idx] = t;
688 gammaAll[image_idx] = gamma;
689 }
690 }
691 }
692 }
693
694 // filter initial results whose reproject errors are too large
695 std::vector<double> reProjErrorFilter,v_gammaFilter;
696 std::vector<Vec3d> omFilter, tFilter;
697 double gammaFinal = 0;
698
699 // choose median value
700 size_t n = gammaAll.size() / 2;
701 std::nth_element(gammaAll.begin(), gammaAll.begin()+n, gammaAll.end());
702 gammaFinal = gammaAll[n];
703
704 _K = Mat(Matx33d(gammaFinal, 0, u0, 0, gammaFinal, v0, 0, 0, 1));
705 _K.convertTo(K, CV_64F);
706 std::vector<int> _idx;
707 // recompute reproject error using the final gamma
708 for (int i = 0; i< n_img; i++)
709 {
710 Mat _projected;
711 cv::omnidir::projectPoints(patternPoints.getMat(i), _projected, v_omAll[i], v_tAll[i], _K, 1, Matx14d(0, 0, 0, 0), cv::noArray());
712 double _error = omnidir::internal::computeMeanReproErr(imagePoints.getMat(i), _projected);
713 if(_error < 100)
714 {
715 _idx.push_back(i);
716 omFilter.push_back(v_omAll[i]);
717 tFilter.push_back(v_tAll[i]);
718 }
719 }
720
721 if (idx.needed())
722 {
723 idx.create(1, (int)_idx.size(), CV_32S);
724 Mat idx_m = idx.getMat();
725 for (int j = 0; j < (int)idx_m.total(); j++)
726 {
727 idx_m.at<int>(j) = _idx[j];
728 }
729 }
730
731 if(omAll.kind() == _InputArray::STD_VECTOR_MAT)
732 {
733 for (int i = 0; i < (int)omFilter.size(); ++i)
734 {
735 omAll.getMat(i) = Mat(omFilter[i]);
736 tAll.getMat(i) = Mat(tFilter[i]);
737 }
738 }
739 else
740 {
741 cv::Mat(omFilter).convertTo(omAll, CV_64FC3);
742 cv::Mat(tFilter).convertTo(tAll, CV_64FC3);
743 }
744 xi = 1;
745 }
746
747 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
748 /// cv::omnidir::internal::initializeStereoCalibration
749
initializeStereoCalibration(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,const Size & size1,const Size & size2,OutputArray om,OutputArray T,OutputArrayOfArrays omL,OutputArrayOfArrays tL,OutputArray K1,OutputArray D1,OutputArray K2,OutputArray D2,double & xi1,double & xi2,int flags,OutputArray idx)750 void cv::omnidir::internal::initializeStereoCalibration(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
751 const Size& size1, const Size& size2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, OutputArray K1, OutputArray D1, OutputArray K2, OutputArray D2,
752 double &xi1, double &xi2, int flags, OutputArray idx)
753 {
754 Mat idx1, idx2;
755 Matx33d _K1, _K2;
756 Matx14d _D1, _D2;
757 Mat _xi1m, _xi2m;
758
759 std::vector<Vec3d> omAllTemp1, omAllTemp2, tAllTemp1, tAllTemp2;
760
761 omnidir::calibrate(objectPoints, imagePoints1, size1, _K1, _xi1m, _D1, omAllTemp1, tAllTemp1, flags, TermCriteria(3, 100, 1e-6), idx1);
762 omnidir::calibrate(objectPoints, imagePoints2, size2, _K2, _xi2m, _D2, omAllTemp2, tAllTemp2, flags, TermCriteria(3, 100, 1e-6), idx2);
763
764 // find the intersection idx
765 Mat interIdx1, interIdx2, interOri;
766
767 getInterset(idx1, idx2, interIdx1, interIdx2, interOri);
768 if (idx.empty())
769 idx.create(1, (int)interOri.total(), CV_32S);
770 interOri.copyTo(idx.getMat());
771
772 int n_inter = (int)interIdx1.total();
773
774 std::vector<Vec3d> omAll1(n_inter), omAll2(n_inter), tAll1(n_inter), tAll2(n_inter);
775 for(int i = 0; i < (int)interIdx1.total(); ++i)
776 {
777 omAll1[i] = omAllTemp1[interIdx1.at<int>(i)];
778 tAll1[i] = tAllTemp1[interIdx1.at<int>(i)];
779 omAll2[i] = omAllTemp2[interIdx2.at<int>(i)];
780 tAll2[i] = tAllTemp2[interIdx2.at<int>(i)];
781 }
782
783 // initialize R,T
784 Mat omEstAll(1, n_inter, CV_64FC3), tEstAll(1, n_inter, CV_64FC3);
785 Mat R1, R2, T1, T2, omLR, TLR, RLR;
786 for (int i = 0; i < n_inter; ++i)
787 {
788 Rodrigues(omAll1[i], R1);
789 Rodrigues(omAll2[i], R2);
790 T1 = Mat(tAll1[i]).reshape(1, 3);
791 T2 = Mat(tAll2[i]).reshape(1, 3);
792 RLR = R2 * R1.t();
793 TLR = T2 - RLR*T1;
794 Rodrigues(RLR, omLR);
795 omLR.reshape(3, 1).copyTo(omEstAll.col(i));
796 TLR.reshape(3, 1).copyTo(tEstAll.col(i));
797 }
798 Vec3d omEst = internal::findMedian3(omEstAll);
799 Vec3d tEst = internal::findMedian3(tEstAll);
800
801 Mat(omEst).copyTo(om.getMat());
802 Mat(tEst).copyTo(T.getMat());
803
804 if (omL.empty())
805 {
806 omL.create((int)omAll1.size(), 1, CV_64FC3);
807 }
808 if (tL.empty())
809 {
810 tL.create((int)tAll1.size(), 1, CV_64FC3);
811 }
812
813 if(omL.kind() == _InputArray::STD_VECTOR_MAT)
814 {
815 for(int i = 0; i < n_inter; ++i)
816 {
817 omL.create(3, 1, CV_64F, i, true);
818 tL.create(3, 1, CV_64F, i, true);
819 omL.getMat(i) = Mat(omAll1[i]);
820 tL.getMat(i) = Mat(tAll1[i]);
821 }
822 }
823 else
824 {
825 cv::Mat(omAll1).convertTo(omL, CV_64FC3);
826 cv::Mat(tAll1).convertTo(tL, CV_64FC3);
827 }
828 if (K1.empty())
829 {
830 K1.create(3, 3, CV_64F);
831 K2.create(3, 3, CV_64F);
832 }
833 if (D1.empty())
834 {
835 D1.create(1, 4, CV_64F);
836 D2.create(1, 4, CV_64F);
837 }
838 Mat(_K1).copyTo(K1.getMat());
839 Mat(_K2).copyTo(K2.getMat());
840
841 Mat(_D1).copyTo(D1.getMat());
842 Mat(_D2).copyTo(D2.getMat());
843
844 xi1 = _xi1m.at<double>(0);
845 xi2 = _xi2m.at<double>(0);
846 }
847
848 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
849 /// cv::omnidir::internal::computeJacobian
850
computeJacobian(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints,InputArray parameters,Mat & JTJ_inv,Mat & JTE,int flags,double epsilon)851 void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
852 InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
853 {
854 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
855 CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
856
857 int n = (int)objectPoints.total();
858
859 Mat JTJ = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F);
860 JTJ_inv = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F);
861 JTE = Mat::zeros(10 + 6*n, 1, CV_64F);
862
863 int nPointsAll = 0;
864 for (int i = 0; i < n; ++i)
865 {
866 nPointsAll += (int)objectPoints.getMat(i).total();
867 }
868
869 Mat J = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F);
870 Mat exAll = Mat::zeros(2*nPointsAll, 10+6*n, CV_64F);
871 double *para = parameters.getMat().ptr<double>();
872 Matx33d K(para[6*n], para[6*n+2], para[6*n+3],
873 0, para[6*n+1], para[6*n+4],
874 0, 0, 1);
875 Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
876 double xi = para[6*n+5];
877 for (int i = 0; i < n; i++)
878 {
879 Mat objPoints, imgPoints, om, T;
880 objectPoints.getMat(i).copyTo(objPoints);
881 imagePoints.getMat(i).copyTo(imgPoints);
882 objPoints = objPoints.reshape(3, objPoints.rows*objPoints.cols);
883 imgPoints = imgPoints.reshape(2, imgPoints.rows*imgPoints.cols);
884
885 om = parameters.getMat().colRange(i*6, i*6+3);
886 T = parameters.getMat().colRange(i*6+3, (i+1)*6);
887 Mat imgProj, jacobian;
888 omnidir::projectPoints(objPoints, imgProj, om, T, K, xi, D, jacobian);
889 Mat projError = imgPoints - imgProj;
890
891 // The intrinsic part of Jacobian
892 Mat JIn(jacobian.rows, 10, CV_64F);
893 Mat JEx(jacobian.rows, 6, CV_64F);
894
895 jacobian.colRange(6, 16).copyTo(JIn);
896 jacobian.colRange(0, 6).copyTo(JEx);
897
898 JTJ(Rect(6*n, 6*n, 10, 10)) = JTJ(Rect(6*n, 6*n, 10, 10)) + JIn.t()*JIn;
899
900 JTJ(Rect(i*6, i*6, 6, 6)) = JEx.t() * JEx;
901
902 Mat JExTIn = JEx.t() * JIn;
903
904 JExTIn.copyTo(JTJ(Rect(6*n, i*6, 10, 6)));
905
906 Mat(JIn.t()*JEx).copyTo(JTJ(Rect(i*6, 6*n, 6, 10)));
907
908 JTE(Rect(0, 6*n, 1, 10)) = JTE(Rect(0, 6*n,1, 10)) + JIn.t() * projError.reshape(1, 2*(int)projError.total());
909 JTE(Rect(0, i*6, 1, 6)) = JEx.t() * projError.reshape(1, 2*(int)projError.total());
910
911 //int nPoints = objectPoints.getMat(i).total();
912 //JIn.copyTo(J(Rect(6*n, i*nPoints*2, 10, nPoints*2)));
913 //JEx.copyTo(J(Rect(6*i, i*nPoints*2, 6, nPoints*2)));
914 //projError.reshape(1, 2*projError.rows).copyTo(exAll.rowRange(i*2*nPoints, (i+1)*2*nPoints));
915 }
916 //JTJ = J.t()*J;
917 //JTE = J.t()*exAll;
918 std::vector<int> _idx(6*n+10, 1);
919 flags2idx(flags, _idx, n);
920
921 subMatrix(JTJ, JTJ, _idx, _idx);
922 subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
923 // in case JTJ is singular
924 //SVD svd(JTJ, SVD::NO_UV);
925 //double cond = svd.w.at<double>(0)/svd.w.at<double>(5);
926
927 //if (cond_JTJ.needed())
928 //{
929 // cond_JTJ.create(1, 1, CV_64F);
930 // cond_JTJ.getMat().at<double>(0) = cond;
931 //}
932
933 //double epsilon = 1e-4*std::exp(cond);
934 JTJ_inv = Mat(JTJ+epsilon).inv();
935 }
936
computeJacobianStereo(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,InputArray parameters,Mat & JTJ_inv,Mat & JTE,int flags,double epsilon)937 void cv::omnidir::internal::computeJacobianStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
938 InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags, double epsilon)
939 {
940 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
941 CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2);
942 CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2);
943 CV_Assert((imagePoints1.total() == imagePoints2.total()) && (imagePoints1.total() == objectPoints.total()));
944
945 // compute Jacobian matrix by naive way
946 int n_img = (int)objectPoints.total();
947 int n_points = (int)objectPoints.getMat(0).total();
948 Mat J = Mat::zeros(4 * n_points * n_img, 20 + 6 * (n_img + 1), CV_64F);
949 Mat exAll = Mat::zeros(4 * n_points * n_img, 1, CV_64F);
950 double *para = parameters.getMat().ptr<double>();
951 int offset1 = (n_img + 1) * 6;
952 int offset2 = offset1 + 10;
953 Matx33d K1(para[offset1], para[offset1+2], para[offset1+3],
954 0, para[offset1+1], para[offset1+4],
955 0, 0, 1);
956 Matx14d D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]);
957 double xi1 = para[offset1+5];
958
959 Matx33d K2(para[offset2], para[offset2+2], para[offset2+3],
960 0, para[offset2+1], para[offset2+4],
961 0, 0, 1);
962 Matx14d D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]);
963 double xi2 = para[offset2+5];
964
965 Mat om = parameters.getMat().reshape(1, 1).colRange(0, 3);
966 Mat T = parameters.getMat().reshape(1, 1).colRange(3, 6);
967
968 for (int i = 0; i < n_img; i++)
969 {
970 Mat objPointsi, imgPoints1i, imgPoints2i, om1, T1;
971 objectPoints.getMat(i).copyTo(objPointsi);
972 imagePoints1.getMat(i).copyTo(imgPoints1i);
973 imagePoints2.getMat(i).copyTo(imgPoints2i);
974 objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
975 imgPoints1i = imgPoints1i.reshape(2, imgPoints1i.rows*imgPoints1i.cols);
976 imgPoints2i = imgPoints2i.reshape(2, imgPoints2i.rows*imgPoints2i.cols);
977
978 om1 = parameters.getMat().reshape(1, 1).colRange((1 + i) * 6, (1 + i) * 6 + 3);
979 T1 = parameters.getMat().reshape(1, 1).colRange((1 + i) * 6 + 3, (i + 1) * 6 + 6);
980
981
982 Mat imgProj1, imgProj2, jacobian1, jacobian2;
983
984 // jacobian for left image
985 cv::omnidir::projectPoints(objPointsi, imgProj1, om1, T1, K1, xi1, D1, jacobian1);
986 Mat projError1 = imgPoints1i - imgProj1;
987 //Mat JIn1(jacobian1.rows, 10, CV_64F);
988 //Mat JEx1(jacobian1.rows, 6, CV_64F);
989 jacobian1.colRange(6, 16).copyTo(J(Rect(6*(n_img+1), i*n_points*4, 10, n_points*2)));
990 jacobian1.colRange(0, 6).copyTo(J(Rect(6+i*6, i*n_points*4, 6, n_points*2)));
991 projError1.reshape(1, 2*n_points).copyTo(exAll.rowRange(i*4*n_points, (i*4+2)*n_points));
992
993 //jacobian for right image
994 Mat om2, T2, dom2dom1, dom2dT1, dom2dom, dom2dT, dT2dom1, dT2dT1, dT2dom, dT2dT;
995 cv::omnidir::internal::compose_motion(om1, T1, om, T, om2, T2, dom2dom1, dom2dT1, dom2dom, dom2dT, dT2dom1, dT2dT1, dT2dom, dT2dT);
996 cv::omnidir::projectPoints(objPointsi, imgProj2, om2, T2, K2, xi2, D2, jacobian2);
997 Mat projError2 = imgPoints2i - imgProj2;
998 projError2.reshape(1, 2*n_points).copyTo(exAll.rowRange((i*4+2)*n_points, (i*4+4)*n_points));
999 Mat dxrdom = jacobian2.colRange(0, 3) * dom2dom + jacobian2.colRange(3, 6) * dT2dom;
1000 Mat dxrdT = jacobian2.colRange(0, 3) * dom2dT + jacobian2.colRange(3, 6) * dT2dT;
1001 Mat dxrdom1 = jacobian2.colRange(0, 3) * dom2dom1 + jacobian2.colRange(3, 6) * dT2dom1;
1002 Mat dxrdT1 = jacobian2.colRange(0, 3) * dom2dT1 + jacobian2.colRange(3, 6) * dT2dT1;
1003
1004 dxrdom.copyTo(J(Rect(0, (i*4+2)*n_points, 3, n_points*2)));
1005 dxrdT.copyTo(J(Rect(3, (i*4+2)*n_points, 3, n_points*2)));
1006 dxrdom1.copyTo(J(Rect(6+i*6, (i*4+2)*n_points, 3, n_points*2)));
1007 dxrdT1.copyTo(J(Rect(6+i*6+3, (i*4+2)*n_points, 3, n_points*2)));
1008 jacobian2.colRange(6, 16).copyTo(J(Rect(6*(n_img+1)+10, (4*i+2)*n_points, 10, n_points*2)));
1009 }
1010
1011 std::vector<int> _idx(6*(n_img+1)+20, 1);
1012 flags2idxStereo(flags, _idx, n_img);
1013
1014 Mat JTJ = J.t()*J;
1015 JTE = J.t()*exAll;
1016 subMatrix(JTJ, JTJ, _idx, _idx);
1017 subMatrix(JTE, JTE, std::vector<int>(1, 1), _idx);
1018
1019 JTJ_inv = Mat(JTJ+epsilon).inv();
1020 }
1021
1022 // This function is from fisheye.cpp
compose_motion(InputArray _om1,InputArray _T1,InputArray _om2,InputArray _T2,Mat & om3,Mat & T3,Mat & dom3dom1,Mat & dom3dT1,Mat & dom3dom2,Mat & dom3dT2,Mat & dT3dom1,Mat & dT3dT1,Mat & dT3dom2,Mat & dT3dT2)1023 void cv::omnidir::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, Mat& om3, Mat& T3, Mat& dom3dom1,
1024 Mat& dom3dT1, Mat& dom3dom2, Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
1025 {
1026 Mat om1 = _om1.getMat();
1027 Mat om2 = _om2.getMat();
1028 Mat T1 = _T1.getMat().reshape(1, 3);
1029 Mat T2 = _T2.getMat().reshape(1, 3);
1030
1031 //% Rotations:
1032 Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
1033 Rodrigues(om1, R1, dR1dom1);
1034 Rodrigues(om2, R2, dR2dom2);
1035 //JRodriguesMatlab(dR1dom1, dR1dom1);
1036 //JRodriguesMatlab(dR2dom2, dR2dom2);
1037 dR1dom1 = dR1dom1.t();
1038 dR2dom2 = dR2dom2.t();
1039
1040 R3 = R2 * R1;
1041 Mat dR3dR2, dR3dR1;
1042 //dAB(R2, R1, dR3dR2, dR3dR1);
1043 matMulDeriv(R2, R1, dR3dR2, dR3dR1);
1044
1045 Mat dom3dR3;
1046 Rodrigues(R3, om3, dom3dR3);
1047 //JRodriguesMatlab(dom3dR3, dom3dR3);
1048 dom3dR3 = dom3dR3.t();
1049 dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
1050 dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
1051 dom3dT1 = Mat::zeros(3, 3, CV_64FC1);
1052 dom3dT2 = Mat::zeros(3, 3, CV_64FC1);
1053
1054 //% Translations:
1055 Mat T3t = R2 * T1;
1056 Mat dT3tdR2, dT3tdT1;
1057 //dAB(R2, T1, dT3tdR2, dT3tdT1);
1058 matMulDeriv(R2, T1, dT3tdR2, dT3tdT1);
1059 Mat dT3tdom2 = dT3tdR2 * dR2dom2;
1060 T3 = T3t + T2;
1061 dT3dT1 = dT3tdT1;
1062 dT3dT2 = Mat::eye(3, 3, CV_64FC1);
1063 dT3dom2 = dT3tdom2;
1064 dT3dom1 = Mat::zeros(3, 3, CV_64FC1);
1065 }
1066
calibrate(InputArrayOfArrays patternPoints,InputArrayOfArrays imagePoints,Size size,InputOutputArray K,InputOutputArray xi,InputOutputArray D,OutputArrayOfArrays omAll,OutputArrayOfArrays tAll,int flags,TermCriteria criteria,OutputArray idx)1067 double cv::omnidir::calibrate(InputArrayOfArrays patternPoints, InputArrayOfArrays imagePoints, Size size,
1068 InputOutputArray K, InputOutputArray xi, InputOutputArray D, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll,
1069 int flags, TermCriteria criteria, OutputArray idx)
1070 {
1071 CV_Assert(!patternPoints.empty() && !imagePoints.empty() && patternPoints.total() == imagePoints.total());
1072 CV_Assert((patternPoints.type() == CV_64FC3 && imagePoints.type() == CV_64FC2) ||
1073 (patternPoints.type() == CV_32FC3 && imagePoints.type() == CV_32FC2));
1074 CV_Assert(patternPoints.getMat(0).channels() == 3 && imagePoints.getMat(0).channels() == 2);
1075 CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty());
1076 CV_Assert((!D.empty() && D.total() == 4) || D.empty());
1077 CV_Assert((!xi.empty() && xi.total() == 1) || xi.empty());
1078 CV_Assert((!omAll.empty() && omAll.depth() == patternPoints.depth()) || omAll.empty());
1079 CV_Assert((!tAll.empty() && tAll.depth() == patternPoints.depth()) || tAll.empty());
1080 int depth = patternPoints.depth();
1081
1082 std::vector<Mat> _patternPoints, _imagePoints;
1083
1084 for (int i = 0; i < (int)patternPoints.total(); ++i)
1085 {
1086 _patternPoints.push_back(patternPoints.getMat(i));
1087 _imagePoints.push_back(imagePoints.getMat(i));
1088 if (depth == CV_32F)
1089 {
1090 _patternPoints[i].convertTo(_patternPoints[i], CV_64FC3);
1091 _imagePoints[i].convertTo(_imagePoints[i], CV_64FC2);
1092 }
1093 }
1094
1095 double _xi;
1096 // initialization
1097 std::vector<Vec3d> _omAll, _tAll;
1098 Matx33d _K;
1099 Matx14d _D;
1100 Mat _idx;
1101 cv::omnidir::internal::initializeCalibration(_patternPoints, _imagePoints, size, _omAll, _tAll, _K, _xi, _idx);
1102 std::vector<Mat> _patternPointsTmp = _patternPoints;
1103 std::vector<Mat> _imagePointsTmp = _imagePoints;
1104
1105 _patternPoints.clear();
1106 _imagePoints.clear();
1107 // erase
1108 for (int i = 0; i < (int)_idx.total(); i++)
1109 {
1110 _patternPoints.push_back(_patternPointsTmp[_idx.at<int>(i)]);
1111 _imagePoints.push_back(_imagePointsTmp[_idx.at<int>(i)]);
1112 }
1113
1114 int n = (int)_patternPoints.size();
1115 Mat finalParam(1, 10 + 6*n, CV_64F);
1116 Mat currentParam(1, 10 + 6*n, CV_64F);
1117 cv::omnidir::internal::encodeParameters(_K, _omAll, _tAll, Mat::zeros(1,4,CV_64F), _xi, currentParam);
1118
1119 // optimization
1120 const double alpha_smooth = 0.01;
1121 //const double thresh_cond = 1e6;
1122 double change = 1;
1123 for(int iter = 0; ; ++iter)
1124 {
1125 if ((criteria.type == 1 && iter >= criteria.maxCount) ||
1126 (criteria.type == 2 && change <= criteria.epsilon) ||
1127 (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
1128 break;
1129 double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
1130 Mat JTJ_inv, JTError;
1131 double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
1132 cv::omnidir::internal::computeJacobian(_patternPoints, _imagePoints, currentParam, JTJ_inv, JTError, flags, epsilon);
1133
1134 // Gauss - Newton
1135 Mat G = alpha_smooth2*JTJ_inv * JTError;
1136
1137 omnidir::internal::fillFixed(G, flags, n);
1138
1139 finalParam = currentParam + G.t();
1140
1141 change = norm(G) / norm(currentParam);
1142
1143 currentParam = finalParam.clone();
1144
1145 cv::omnidir::internal::decodeParameters(currentParam, _K, _omAll, _tAll, _D, _xi);
1146 //double repr = internal::computeMeanReproErr(_patternPoints, _imagePoints, _K, _D, _xi, _omAll, _tAll);
1147 }
1148 cv::omnidir::internal::decodeParameters(currentParam, _K, _omAll, _tAll, _D, _xi);
1149
1150 //double repr = internal::computeMeanReproErr(_patternPoints, _imagePoints, _K, _D, _xi, _omAll, _tAll);
1151
1152 if (omAll.needed())
1153 {
1154 omAll.create((int)_omAll.size(), 1, CV_64FC3);
1155 }
1156 if (tAll.needed())
1157 {
1158 tAll.create((int)_tAll.size(), 1, CV_64FC3);
1159 }
1160 if (omAll.kind() == _InputArray::STD_VECTOR_MAT)
1161 {
1162 for (int i = 0; i < n; ++i)
1163 {
1164 omAll.create(3, 1, CV_64F, i, true);
1165 tAll.create(3, 1, CV_64F, i, true);
1166 Mat tmpom = Mat(_omAll[i]);
1167 Mat tmpt = Mat(_tAll[i]);
1168 tmpom.convertTo(tmpom, CV_64F);
1169 tmpt.convertTo(tmpt, CV_64F);
1170 tmpom.copyTo(omAll.getMat(i));
1171 tmpt.copyTo(tAll.getMat(i));
1172 }
1173 }
1174 else
1175 {
1176 Mat(_omAll).convertTo(omAll, CV_64FC3);
1177 Mat(_tAll).convertTo(tAll, CV_64FC3);
1178 }
1179
1180 if(K.empty())
1181 {
1182 K.create(3, 3, CV_64F);
1183 }
1184 if (D.empty())
1185 {
1186 D.create(1, 4, CV_64F);
1187 }
1188
1189 Mat(_K).convertTo(K.getMat(), K.empty()? CV_64F : K.type());
1190 Mat(_D).convertTo(D.getMat(), D.empty() ? CV_64F: D.type());
1191
1192 if (xi.empty())
1193 {
1194 xi.create(1, 1, CV_64F);
1195 }
1196 Mat xi_m = Mat(1, 1, CV_64F);
1197 xi_m.at<double>(0) = _xi;
1198 xi_m.convertTo(xi.getMat(), xi.empty() ? CV_64F : xi.type());
1199
1200 if (idx.needed())
1201 {
1202 idx.create(1, (int)_idx.total(), CV_32S);
1203 _idx.copyTo(idx.getMat());
1204 }
1205
1206 Vec2d std_error;
1207 double rms;
1208 Mat errors;
1209 cv::omnidir::internal::estimateUncertainties(_patternPoints, _imagePoints, finalParam, errors, std_error, rms, flags);
1210 return rms;
1211 }
1212
stereoCalibrate(InputOutputArrayOfArrays objectPoints,InputOutputArrayOfArrays imagePoints1,InputOutputArrayOfArrays imagePoints2,const Size & imageSize1,const Size & imageSize2,InputOutputArray K1,InputOutputArray xi1,InputOutputArray D1,InputOutputArray K2,InputOutputArray xi2,InputOutputArray D2,OutputArray om,OutputArray T,OutputArrayOfArrays omL,OutputArrayOfArrays tL,int flags,TermCriteria criteria,OutputArray idx)1213 double cv::omnidir::stereoCalibrate(InputOutputArrayOfArrays objectPoints, InputOutputArrayOfArrays imagePoints1, InputOutputArrayOfArrays imagePoints2,
1214 const Size& imageSize1, const Size& imageSize2, InputOutputArray K1, InputOutputArray xi1, InputOutputArray D1, InputOutputArray K2, InputOutputArray xi2,
1215 InputOutputArray D2, OutputArray om, OutputArray T, OutputArrayOfArrays omL, OutputArrayOfArrays tL, int flags, TermCriteria criteria, OutputArray idx)
1216 {
1217 CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_64FC3 || objectPoints.type() == CV_32FC3));
1218 CV_Assert(!imagePoints1.empty() && (imagePoints1.type() == CV_64FC2 || imagePoints1.type() == CV_32FC2));
1219 CV_Assert(!imagePoints2.empty() && (imagePoints2.type() == CV_64FC2 || imagePoints2.type() == CV_32FC2));
1220
1221 CV_Assert(((flags & CALIB_USE_GUESS) && !K1.empty() && !D1.empty() && !K2.empty() && !D2.empty()) || !(flags & CALIB_USE_GUESS));
1222
1223 int depth = objectPoints.depth();
1224
1225 std::vector<Mat> _objectPoints, _imagePoints1, _imagePoints2,
1226 _objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt;
1227 for (int i = 0; i < (int)objectPoints.total(); ++i)
1228 {
1229 _objectPoints.push_back(objectPoints.getMat(i));
1230 _imagePoints1.push_back(imagePoints1.getMat(i));
1231 _imagePoints2.push_back(imagePoints2.getMat(i));
1232 if (depth == CV_32F)
1233 {
1234 _objectPoints[i].convertTo(_objectPoints[i], CV_64FC3);
1235 _imagePoints1[i].convertTo(_imagePoints1[i], CV_64FC2);
1236 _imagePoints2[i].convertTo(_imagePoints2[i], CV_64FC2);
1237 }
1238 }
1239
1240 Matx33d _K1, _K2;
1241 Matx14d _D1, _D2;
1242
1243 double _xi1, _xi2;
1244
1245 std::vector<Vec3d> _omL, _TL;
1246 Vec3d _om, _T;
1247
1248 // initializaition
1249 Mat _idx;
1250 internal::initializeStereoCalibration(_objectPoints, _imagePoints1, _imagePoints2, imageSize1, imageSize2, _om, _T, _omL, _TL, _K1, _D1, _K2, _D2, _xi1, _xi2, flags, _idx);
1251 if(idx.needed())
1252 {
1253 idx.create(1, (int)_idx.total(), CV_32S);
1254 _idx.copyTo(idx.getMat());
1255 }
1256 for (int i = 0; i < (int)_idx.total(); ++i)
1257 {
1258 _objectPointsFilt.push_back(_objectPoints[_idx.at<int>(i)]);
1259 _imagePoints1Filt.push_back(_imagePoints1[_idx.at<int>(i)]);
1260 _imagePoints2Filt.push_back(_imagePoints2[_idx.at<int>(i)]);
1261 }
1262
1263 int n = (int)_objectPointsFilt.size();
1264 Mat finalParam(1, 10 + 6*n, CV_64F);
1265 Mat currentParam(1, 10 + 6*n, CV_64F);
1266
1267 //double repr1 = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
1268 // _T, _omL, _TL);
1269 cv::omnidir::internal::encodeParametersStereo(_K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2, currentParam);
1270
1271 // optimization
1272 const double alpha_smooth = 0.01;
1273 double change = 1;
1274 for(int iter = 0; ; ++iter)
1275 {
1276 if ((criteria.type == 1 && iter >= criteria.maxCount) ||
1277 (criteria.type == 2 && change <= criteria.epsilon) ||
1278 (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
1279 break;
1280 double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter + 1.0);
1281 Mat JTJ_inv, JTError;
1282 double epsilon = 0.01 * std::pow(0.9, (double)iter/10);
1283
1284 cv::omnidir::internal::computeJacobianStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt, currentParam,
1285 JTJ_inv, JTError, flags, epsilon);
1286
1287 // Gauss - Newton
1288 Mat G = alpha_smooth2*JTJ_inv * JTError;
1289
1290 omnidir::internal::fillFixedStereo(G, flags, n);
1291
1292 finalParam = currentParam + G.t();
1293
1294 change = norm(G) / norm(currentParam);
1295
1296 currentParam = finalParam.clone();
1297 cv::omnidir::internal::decodeParametersStereo(currentParam, _K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2);
1298 //double repr = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
1299 // _T, _omL, _TL);
1300
1301 }
1302 cv::omnidir::internal::decodeParametersStereo(finalParam, _K1, _K2, _om, _T, _omL, _TL, _D1, _D2, _xi1, _xi2);
1303 //double repr = internal::computeMeanReproErrStereo(_objectPoints, _imagePoints1, _imagePoints2, _K1, _K2, _D1, _D2, _xi1, _xi2, _om,
1304 // _T, _omL, _TL);
1305
1306 if (K1.empty())
1307 {
1308 K1.create(3, 3, CV_64F);
1309 D1.create(1, 4, CV_64F);
1310 K2.create(3, 3, CV_64F);
1311 D2.create(1, 4, CV_64F);
1312 }
1313 if (om.empty())
1314 {
1315 om.create(3, 1, CV_64F);
1316 T.create(3, 1, CV_64F);
1317 }
1318 if (omL.empty())
1319 {
1320 omL.create(1, n, CV_64FC3);
1321 tL.create(1, n, CV_64FC3);
1322 }
1323
1324 Mat(_K1).convertTo(K1.getMat(), K1.empty() ? CV_64F : K1.type());
1325 Mat(_D1).convertTo(D1.getMat(), D1.empty() ? CV_64F : D1.type());
1326 Mat(_K2).convertTo(K2.getMat(), K2.empty() ? CV_64F : K2.type());
1327 Mat(_D2).convertTo(D2.getMat(), D2.empty() ? CV_64F : D2.type());
1328
1329 Mat(_om).convertTo(om.getMat(), om.empty() ? CV_64F: om.type());
1330 Mat(_T).convertTo(T.getMat(), T.empty() ? CV_64F: T.type());
1331
1332 if (omL.needed())
1333 {
1334 omL.create((int)_omL.size(), 1, CV_64FC3);
1335 }
1336 if (tL.needed())
1337 {
1338 tL.create((int)_TL.size(), 1, CV_64FC3);
1339 }
1340
1341 if (omL.kind() == _InputArray::STD_VECTOR_MAT)
1342 {
1343 for (int i = 0; i < n; ++i)
1344 {
1345 omL.create(3, 1, CV_64F, i, true);
1346 tL.create(3, 1, CV_64F, i, true);
1347 Mat(_omL[i]).copyTo(omL.getMat(i));
1348 Mat(_TL[i]).copyTo(tL.getMat(i));
1349 }
1350 }
1351 else
1352 {
1353 Mat(_omL).convertTo(omL, omL.empty() ? CV_64FC3 : omL.type());
1354 Mat(_TL).convertTo(tL, tL.empty() ? CV_64FC3 : tL.type());
1355 }
1356
1357 Mat xi1_m = Mat(1, 1, CV_64F),
1358 xi2_m = Mat(1, 1, CV_64F);
1359 xi1_m.at<double>(0) = _xi1;
1360 xi2_m.at<double>(0) = _xi2;
1361
1362 if (xi1.empty())
1363 {
1364 xi1.create(1, 1, CV_64F);
1365 }
1366 if (xi2.empty())
1367 {
1368 xi2.create(1, 1, CV_64F);
1369 }
1370 xi1_m.convertTo(xi1, xi1.empty() ? CV_64F : xi1.type());
1371 xi2_m.convertTo(xi2, xi2.empty() ? CV_64F : xi2.type());
1372
1373 // compute uncertainty
1374 Vec2d std_error;
1375 double rms;
1376 Mat errors;
1377
1378 cv::omnidir::internal::estimateUncertaintiesStereo(_objectPointsFilt, _imagePoints1Filt, _imagePoints2Filt,
1379 finalParam, errors, std_error, rms, flags);
1380 return rms;
1381 }
1382
stereoReconstruct(InputArray image1,InputArray image2,InputArray K1,InputArray D1,InputArray xi1,InputArray K2,InputArray D2,InputArray xi2,InputArray R,InputArray T,int flag,int numDisparities,int SADWindowSize,OutputArray disparity,OutputArray image1Rec,OutputArray image2Rec,const Size & newSize,InputArray Knew,OutputArray pointCloud,int pointType)1383 void cv::omnidir::stereoReconstruct(InputArray image1, InputArray image2, InputArray K1, InputArray D1,
1384 InputArray xi1, InputArray K2, InputArray D2, InputArray xi2, InputArray R, InputArray T, int flag,
1385 int numDisparities, int SADWindowSize, OutputArray disparity, OutputArray image1Rec, OutputArray image2Rec,
1386 const Size& newSize, InputArray Knew, OutputArray pointCloud, int pointType)
1387 {
1388 CV_Assert(!K1.empty() && K1.size() == Size(3,3) && (K1.type() == CV_64F || K1.type() == CV_32F));
1389 CV_Assert(!K2.empty() && K2.size() == Size(3,3) && (K2.type() == CV_64F || K2.type() == CV_32F));
1390 CV_Assert(!D1.empty() && D1.total() == 4 && (D1.type() == CV_64F || D1.type() == CV_32F));
1391 CV_Assert(!D2.empty() && D2.total() == 4 && (D2.type() == CV_64F || D2.type() == CV_32F));
1392 CV_Assert(!R.empty() && (R.size() == Size(3,3) || R.total() == 3) && (R.type() == CV_64F || R.type() == CV_32F));
1393 CV_Assert(!T.empty() && T.total() == 3 && (T.type() == CV_64F || T.type() == CV_32F));
1394 CV_Assert(!image1.empty() && (image1.type() == CV_8U || image1.type() == CV_8UC3));
1395 CV_Assert(!image2.empty() && (image2.type() == CV_8U || image2.type() == CV_8UC3));
1396 CV_Assert(flag == omnidir::RECTIFY_LONGLATI || flag == omnidir::RECTIFY_PERSPECTIVE);
1397
1398 Mat _K1, _D1, _K2, _D2, _R, _T;
1399
1400 K1.getMat().convertTo(_K1, CV_64F);
1401 K2.getMat().convertTo(_K2, CV_64F);
1402 D1.getMat().convertTo(_D1, CV_64F);
1403 D2.getMat().convertTo(_D2, CV_64F);
1404 T.getMat().reshape(1, 3).convertTo(_T, CV_64F);
1405
1406 if (R.size() == Size(3, 3))
1407 {
1408 R.getMat().convertTo(_R, CV_64F);
1409 }
1410 else if (R.total() == 3)
1411 {
1412 Rodrigues(R.getMat(), _R);
1413 _R.convertTo(_R, CV_64F);
1414 }
1415 // stereo rectify so that stereo matching can be applied in one line
1416 Mat R1, R2;
1417 stereoRectify(_R, _T, R1, R2);
1418 Mat undis1, undis2;
1419 Matx33d _Knew = Matx33d(_K1);
1420 if (!Knew.empty())
1421 {
1422 Knew.getMat().convertTo(_Knew, CV_64F);
1423 }
1424
1425 undistortImage(image1.getMat(), undis1, _K1, _D1, xi1, flag, _Knew, newSize, R1);
1426 undistortImage(image2.getMat(), undis2, _K2, _D2, xi2, flag, _Knew, newSize, R2);
1427
1428 undis1.copyTo(image1Rec);
1429 undis2.copyTo(image2Rec);
1430
1431 // stereo matching by semi-global
1432 Mat _disMap;
1433 int channel = image1.channels();
1434
1435 //cv::StereoSGBM matching(0, numDisparities, SADWindowSize, 8*channel*SADWindowSize*SADWindowSize, 32*channel*SADWindowSize*SADWindowSize);
1436 //matching(undis1, undis2, _depthMap);
1437 Ptr<StereoSGBM> sgbm = StereoSGBM::create(0, numDisparities, SADWindowSize, 8 * channel*SADWindowSize*SADWindowSize, 32 * channel*SADWindowSize*SADWindowSize);
1438
1439 sgbm->compute(undis1, undis2, _disMap);
1440
1441 // some regions of image1 is black, the corresponding regions of disparity map is also invalid.
1442 Mat realDis;
1443 _disMap.convertTo(_disMap, CV_32F);
1444 Mat(_disMap/16.0f).convertTo(realDis, CV_32F);
1445
1446 Mat grayImg, binaryImg, idx;
1447 if (undis1.channels() == 3)
1448 {
1449 cvtColor(undis1, grayImg, COLOR_RGB2GRAY);
1450 }
1451 else
1452 {
1453 grayImg = undis1;
1454 }
1455
1456 binaryImg = (grayImg <= 0);
1457 findNonZero(binaryImg, idx);
1458 for (int i = 0; i < (int)idx.total(); ++i)
1459 {
1460 Vec2i _idx = idx.at<Vec2i>(i);
1461 realDis.at<float>(_idx[1], _idx[0]) = 0.0f;
1462 }
1463
1464 disparity.create(realDis.size(), realDis.type());
1465 realDis.copyTo(disparity.getMat());
1466
1467 std::vector<Vec3f> _pointCloud;
1468 std::vector<Vec6f> _pointCloudColor;
1469 double baseline = cv::norm(T);
1470 double f = _Knew(0, 0);
1471 Matx33d K_inv = _Knew.inv();
1472
1473 std::vector<Mat> rgb;
1474 if (undis1.channels() == 3)
1475 {
1476 split(undis1, rgb);
1477 }
1478
1479 if (pointCloud.needed())
1480 {
1481 for (int i = 0; i < newSize.width; ++i)
1482 {
1483 for(int j = 0; j < newSize.height; ++j)
1484 {
1485 Vec3f point;
1486 Vec6f pointColor;
1487 if (realDis.at<float>(j, i) > 15)
1488 {
1489 float depth = float(baseline * f /realDis.at<float>(j, i));
1490 // for RECTIFY_PERSPECTIVE, (x,y) are image plane points,
1491 // for RECTIFY_LONGLATI, (x,y) are (theta, phi) angles
1492 float x = float(K_inv(0,0) * i + K_inv(0,1) * j + K_inv(0,2));
1493 float y = float(K_inv(1,0) * i + K_inv(1,1) * j + K_inv(1,2));
1494 if (flag == omnidir::RECTIFY_LONGLATI)
1495 {
1496 point = Vec3f((float)-std::cos(x), (float)(-std::sin(x)*std::cos(y)), (float)(std::sin(x)*std::sin(y))) * depth;
1497 }
1498 else if(flag == omnidir::RECTIFY_PERSPECTIVE)
1499 {
1500 point = Vec3f(float(x), float(y), 1.0f) * depth;
1501 }
1502 if (pointType == XYZ)
1503 {
1504 _pointCloud.push_back(point);
1505 }
1506 else if (pointType == XYZRGB)
1507 {
1508 pointColor[0] = point[0];
1509 pointColor[1] = point[1];
1510 pointColor[2] = point[2];
1511
1512 if (undis1.channels() == 1)
1513 {
1514 pointColor[3] = float(undis1.at<uchar>(j, i));
1515 pointColor[4] = pointColor[3];
1516 pointColor[5] = pointColor[3];
1517 }
1518 else if (undis1.channels() == 3)
1519 {
1520 pointColor[3] = rgb[0].at<uchar>(j, i);
1521 pointColor[4] = rgb[1].at<uchar>(j, i);
1522 pointColor[5] = rgb[2].at<uchar>(j, i);
1523 }
1524 _pointCloudColor.push_back(pointColor);
1525 }
1526 }
1527 }
1528 }
1529
1530 if (pointType == XYZ)
1531 {
1532 Mat(_pointCloud).convertTo(pointCloud, CV_MAKE_TYPE(CV_32F, 3));
1533 }
1534 else if (pointType == XYZRGB)
1535 {
1536 Mat(_pointCloudColor).convertTo(pointCloud, CV_MAKE_TYPE(CV_32F, 6));
1537 }
1538 }
1539 }
1540
encodeParameters(InputArray K,InputArrayOfArrays omAll,InputArrayOfArrays tAll,InputArray distoaration,double xi,OutputArray parameters)1541 void cv::omnidir::internal::encodeParameters(InputArray K, InputArrayOfArrays omAll, InputArrayOfArrays tAll, InputArray distoaration, double xi, OutputArray parameters)
1542 {
1543 CV_Assert(K.type() == CV_64F && K.size() == Size(3,3));
1544 CV_Assert(distoaration.total() == 4 && distoaration.type() == CV_64F);
1545 int n = (int)omAll.total();
1546 Mat _omAll = omAll.getMat(), _tAll = tAll.getMat();
1547
1548 Matx33d _K = K.getMat();
1549 Vec4d _D = (Vec4d)distoaration.getMat();
1550 parameters.create(1, 10+6*n,CV_64F);
1551 Mat _params = parameters.getMat();
1552 for (int i = 0; i < n; i++)
1553 {
1554 Mat(_omAll.at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(i*6, i*6+3));
1555 Mat(_tAll.at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(i*6+3, (i+1)*6));
1556 }
1557
1558 _params.at<double>(0, 6*n) = _K(0,0);
1559 _params.at<double>(0, 6*n+1) = _K(1,1);
1560 _params.at<double>(0, 6*n+2) = _K(0,1);
1561 _params.at<double>(0, 6*n+3) = _K(0,2);
1562 _params.at<double>(0, 6*n+4) = _K(1,2);
1563 _params.at<double>(0, 6*n+5) = xi;
1564 _params.at<double>(0, 6*n+6) = _D[0];
1565 _params.at<double>(0, 6*n+7) = _D[1];
1566 _params.at<double>(0, 6*n+8) = _D[2];
1567 _params.at<double>(0, 6*n+9) = _D[3];
1568 }
1569
encodeParametersStereo(InputArray K1,InputArray K2,InputArray om,InputArray T,InputArrayOfArrays omL,InputArrayOfArrays tL,InputArray D1,InputArray D2,double xi1,double xi2,OutputArray parameters)1570 void cv::omnidir::internal::encodeParametersStereo(InputArray K1, InputArray K2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays tL,
1571 InputArray D1, InputArray D2, double xi1, double xi2, OutputArray parameters)
1572 {
1573 CV_Assert(!K1.empty() && K1.type() == CV_64F && K1.size() == Size(3,3));
1574 CV_Assert(!K2.empty() && K2.type() == CV_64F && K2.size() == Size(3,3));
1575 CV_Assert(!om.empty() && om.type() == CV_64F && om.total() == 3);
1576 CV_Assert(!T.empty() && T.type() == CV_64F && T.total() == 3);
1577 CV_Assert(omL.total() == tL.total() && omL.type() == CV_64FC3 && tL.type() == CV_64FC3);
1578 CV_Assert(D1.type() == CV_64F && D1.total() == 4 && D2.type() == CV_64F && D2.total() == 4);
1579
1580 int n = (int)omL.total();
1581 // om, T, omL, tL, intrinsic left, intrinsic right
1582 parameters.create(1, 20 + 6 * (n + 1), CV_64F);
1583
1584 Mat _params = parameters.getMat();
1585
1586 om.getMat().reshape(1, 1).copyTo(_params.colRange(0, 3));
1587 T.getMat().reshape(1, 1).copyTo(_params.colRange(3, 6));
1588 for(int i = 0; i < n; ++i)
1589 {
1590 Mat(omL.getMat().at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(6 + i*6, 6 + i*6 + 3));
1591 Mat(tL.getMat().at<Vec3d>(i)).reshape(1, 1).copyTo(_params.colRange(6 + i*6 + 3, 6 + i*6 + 6));
1592 }
1593
1594 Matx33d _K1 = K1.getMat();
1595 Matx33d _K2 = K2.getMat();
1596 Vec4d _D1 = D1.getMat();
1597 Vec4d _D2 = D2.getMat();
1598 _params.at<double>(0, 6*(n+1)) = _K1(0,0);
1599 _params.at<double>(0, 6*(n+1)+1) = _K1(1,1);
1600 _params.at<double>(0, 6*(n+1)+2) = _K1(0,1);
1601 _params.at<double>(0, 6*(n+1)+3) = _K1(0,2);
1602 _params.at<double>(0, 6*(n+1)+4) = _K1(1,2);
1603 _params.at<double>(0, 6*(n+1)+5) = xi1;
1604 _params.at<double>(0, 6*(n+1)+6) = _D1[0];
1605 _params.at<double>(0, 6*(n+1)+7) = _D1[1];
1606 _params.at<double>(0, 6*(n+1)+8) = _D1[2];
1607 _params.at<double>(0, 6*(n+1)+9) = _D1[3];
1608
1609 _params.at<double>(0, 6*(n+1)+10) = _K2(0,0);
1610 _params.at<double>(0, 6*(n+1)+11) = _K2(1,1);
1611 _params.at<double>(0, 6*(n+1)+12) = _K2(0,1);
1612 _params.at<double>(0, 6*(n+1)+13) = _K2(0,2);
1613 _params.at<double>(0, 6*(n+1)+14) = _K2(1,2);
1614 _params.at<double>(0, 6*(n+1)+15) = xi2;
1615 _params.at<double>(0, 6*(n+1)+16) = _D2[0];
1616 _params.at<double>(0, 6*(n+1)+17) = _D2[1];
1617 _params.at<double>(0, 6*(n+1)+18) = _D2[2];
1618 _params.at<double>(0, 6*(n+1)+19) = _D2[3];
1619 }
1620
1621
decodeParameters(InputArray parameters,OutputArray K,OutputArrayOfArrays omAll,OutputArrayOfArrays tAll,OutputArray distoration,double & xi)1622 void cv::omnidir::internal::decodeParameters(InputArray parameters, OutputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray distoration, double& xi)
1623 {
1624 if(K.empty())
1625 K.create(3,3,CV_64F);
1626 Matx33d _K;
1627 int n = (int)(parameters.total()-10)/6;
1628 if(omAll.empty())
1629 omAll.create(1, n, CV_64FC3);
1630 if(tAll.empty())
1631 tAll.create(1, n, CV_64FC3);
1632 if(distoration.empty())
1633 distoration.create(1, 4, CV_64F);
1634 Matx14d _D = distoration.getMat();
1635 Mat param = parameters.getMat();
1636 double *para = param.ptr<double>();
1637 _K = Matx33d(para[6*n], para[6*n+2], para[6*n+3],
1638 0, para[6*n+1], para[6*n+4],
1639 0, 0, 1);
1640 _D = Matx14d(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
1641 xi = para[6*n+5];
1642 std::vector<Vec3d> _omAll(n), _tAll(n);
1643 for (int i = 0; i < n; i++)
1644 {
1645 _omAll[i] = Vec3d(param.colRange(i*6, i*6+3));
1646 _tAll[i] = Vec3d(param.colRange(i*6+3, i*6+6));
1647 }
1648 Mat(_D).convertTo(distoration, CV_64F);
1649 Mat(_K).convertTo(K, CV_64F);
1650
1651 if (omAll.kind() == _InputArray::STD_VECTOR_MAT)
1652 {
1653 for (int i = 0; i < n; ++i)
1654 {
1655 Mat(_omAll[i]).copyTo(omAll.getMat(i));
1656 Mat(_tAll[i]).copyTo(tAll.getMat(i));
1657 }
1658 }
1659 else
1660 {
1661 Mat(_omAll).convertTo(omAll, CV_64FC3);
1662 Mat(_tAll).convertTo(tAll, CV_64FC3);
1663 }
1664 }
1665
decodeParametersStereo(InputArray parameters,OutputArray K1,OutputArray K2,OutputArray om,OutputArray T,OutputArrayOfArrays omL,OutputArrayOfArrays tL,OutputArray D1,OutputArray D2,double & xi1,double & xi2)1666 void cv::omnidir::internal::decodeParametersStereo(InputArray parameters, OutputArray K1, OutputArray K2, OutputArray om, OutputArray T, OutputArrayOfArrays omL,
1667 OutputArrayOfArrays tL, OutputArray D1, OutputArray D2, double& xi1, double& xi2)
1668 {
1669 if(K1.empty())
1670 K1.create(3, 3, CV_64F);
1671 if(K2.empty())
1672 K2.create(3, 3, CV_64F);
1673 if(om.empty())
1674 om.create(3, 1, CV_64F);
1675 if(T.empty())
1676 T.create(3, 1, CV_64F);
1677
1678 int n = ((int)parameters.total() - 20) / 6 - 1;
1679
1680 if(omL.empty())
1681 omL.create(1, n, CV_64FC3);
1682 if(tL.empty())
1683 tL.create(1, n, CV_64FC3);
1684 if(D1.empty())
1685 D1.create(1, 4, CV_64F);
1686 if(D2.empty())
1687 D2.create(1, 4, CV_64F);
1688
1689 Mat param = parameters.getMat().reshape(1, 1);
1690 param.colRange(0, 3).reshape(1, 3).copyTo(om.getMat());
1691 param.colRange(3, 6).reshape(1, 3).copyTo(T.getMat());
1692 std::vector<Vec3d> _omL, _tL;
1693
1694 for(int i = 0; i < n; i++)
1695 {
1696 _omL.push_back(Vec3d(param.colRange(6 + i*6, 6 + i*6 + 3)));
1697 _tL.push_back(Vec3d(param.colRange(6 + i*6 + 3, 6 + i*6 + 6)));
1698 }
1699
1700 double* para = param.ptr<double>();
1701 int offset1 = (n + 1)*6;
1702 Matx33d _K1(para[offset1], para[offset1+2], para[offset1+3],
1703 0, para[offset1+1], para[offset1+4],
1704 0, 0, 1);
1705 xi1 = para[offset1+5];
1706 Matx14d _D1(para[offset1+6], para[offset1+7], para[offset1+8], para[offset1+9]);
1707
1708 int offset2 = (n + 1)*6 + 10;
1709 Matx33d _K2(para[offset2], para[offset2+2], para[offset2+3],
1710 0, para[offset2+1], para[offset2+4],
1711 0, 0, 1);
1712 xi2 = para[offset2+5];
1713 Matx14d _D2(para[offset2+6], para[offset2+7], para[offset2+8], para[offset2+9]);
1714
1715 Mat(_K1).convertTo(K1, CV_64F);
1716 Mat(_D1).convertTo(D1, CV_64F);
1717 Mat(_K2).convertTo(K2, CV_64F);
1718 Mat(_D2).convertTo(D2, CV_64F);
1719 if(omL.kind() == _InputArray::STD_VECTOR_MAT)
1720 {
1721 for(int i = 0; i < n; ++i)
1722 {
1723 Mat(_omL[i]).copyTo(omL.getMat(i));
1724 Mat(_tL[i]).copyTo(tL.getMat(i));
1725 }
1726 }
1727 else
1728 {
1729 Mat(_omL).convertTo(omL, CV_64FC3);
1730 Mat(_tL).convertTo(tL, CV_64FC3);
1731 }
1732 }
1733
estimateUncertainties(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints,InputArray parameters,Mat & errors,Vec2d & std_error,double & rms,int flags)1734 void cv::omnidir::internal::estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters,
1735 Mat& errors, Vec2d& std_error, double& rms, int flags)
1736 {
1737 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
1738 CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
1739 CV_Assert(!parameters.empty() && parameters.type() == CV_64F);
1740
1741 int n = (int) objectPoints.total();
1742 // assume every image has the same number of objectpoints
1743 int nPointsAll = 0;
1744 for (int i = 0; i < n; ++i)
1745 {
1746 nPointsAll += (int)objectPoints.getMat(i).total();
1747 }
1748
1749 Mat reprojError = Mat(nPointsAll, 1, CV_64FC2);
1750
1751 double* para = parameters.getMat().ptr<double>();
1752 Matx33d K(para[6*n], para[6*n+2], para[6*n+3],
1753 0, para[6*n+1], para[6*n+4],
1754 0, 0, 1);
1755 Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]);
1756 double xi = para[6*n+5];
1757 int nPointsAccu = 0;
1758
1759 for(int i=0; i < n; ++i)
1760 {
1761 Mat imgPoints, objPoints;
1762 imagePoints.getMat(i).copyTo(imgPoints);
1763 objectPoints.getMat(i).copyTo(objPoints);
1764 imgPoints = imgPoints.reshape(2, imgPoints.rows*imgPoints.cols);
1765 objPoints = objPoints.reshape(3, objPoints.rows*objPoints.cols);
1766
1767 Mat om = parameters.getMat().colRange(i*6, i*6+3);
1768 Mat T = parameters.getMat().colRange(i*6+3, (i+1)*6);
1769
1770 Mat x;
1771 omnidir::projectPoints(objPoints, x, om, T, K, xi, D, cv::noArray());
1772
1773 Mat errorx = (imgPoints - x);
1774
1775 //reprojError.rowRange(errorx.rows*i, errorx.rows*(i+1)) = errorx.clone();
1776 errorx.copyTo(reprojError.rowRange(nPointsAccu, nPointsAccu + (int)errorx.total()));
1777 nPointsAccu += (int)errorx.total();
1778 }
1779
1780 meanStdDev(reprojError, noArray(), std_error);
1781 std_error *= sqrt((double)reprojError.total()/((double)reprojError.total() - 1.0));
1782
1783 Mat sigma_x;
1784 meanStdDev(reprojError.reshape(1,1), noArray(), sigma_x);
1785 sigma_x *= sqrt(2.0*(double)reprojError.total()/(2.0*(double)reprojError.total() - 1.0));
1786 double s = sigma_x.at<double>(0);
1787
1788 Mat _JTJ_inv, _JTE;
1789 computeJacobian(objectPoints, imagePoints, parameters, _JTJ_inv, _JTE, flags, 0.0);
1790 sqrt(_JTJ_inv, _JTJ_inv);
1791
1792 errors = 3 * s * _JTJ_inv.diag();
1793
1794 rms = 0;
1795 const Vec2d* ptr_ex = reprojError.ptr<Vec2d>();
1796 for (int i = 0; i < (int)reprojError.total(); i++)
1797 {
1798 rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
1799 }
1800
1801 rms /= (double)reprojError.total();
1802 rms = sqrt(rms);
1803 }
1804
1805 // estimateUncertaintiesStereo
estimateUncertaintiesStereo(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,InputArray parameters,Mat & errors,Vec2d & std_error,double & rms,int flags)1806 void cv::omnidir::internal::estimateUncertaintiesStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
1807 InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags)
1808 {
1809 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
1810 CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2 && imagePoints1.total() == objectPoints.total());
1811 CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2 && imagePoints1.total() == imagePoints2.total());
1812 int n_img = (int)objectPoints.total();
1813 CV_Assert((int)parameters.total() == (6*(n_img+1)+20));
1814
1815 Mat _K1, _K2, _D1, _D2;
1816 Vec3d _om, _T;
1817 std::vector<Vec3d> _omL(n_img), _tL(n_img);
1818 Mat _parameters = parameters.getMat().reshape(1, 1);
1819 double _xi1, _xi2;
1820 internal::decodeParametersStereo(_parameters, _K1, _K2, _om, _T, _omL, _tL, _D1, _D2, _xi1, _xi2);
1821
1822 int n_points = (int)objectPoints.getMat(0).total();
1823 Mat reprojErrorAll = Mat::zeros(2*n_points*n_img, 1, CV_64FC2);
1824
1825 // error for left image
1826 for (int i = 0; i < n_img; ++i)
1827 {
1828 Mat objPointsi, imgPointsi;
1829 objectPoints.getMat(i).copyTo(objPointsi);
1830 imagePoints1.getMat(i).copyTo(imgPointsi);
1831 objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
1832 imgPointsi = imgPointsi.reshape(2, imgPointsi.rows*imgPointsi.cols);
1833
1834 Mat x;
1835 omnidir::projectPoints(objPointsi, x, _omL[i], _tL[i], _K1, _xi1, _D1, cv::noArray());
1836
1837 Mat errorx = imgPointsi - x;
1838
1839 errorx.copyTo(reprojErrorAll.rowRange(i*2*n_points, (i*2+1)*n_points));
1840 }
1841 // error for right image
1842 for (int i = 0; i < n_img; ++i)
1843 {
1844 Mat objPointsi, imgPointsi;
1845 objectPoints.getMat(i).copyTo(objPointsi);
1846 imagePoints2.getMat(i).copyTo(imgPointsi);
1847 objPointsi = objPointsi.reshape(3, objPointsi.rows*objPointsi.cols);
1848 imgPointsi = imgPointsi.reshape(2, imgPointsi.rows*imgPointsi.cols);
1849
1850 Mat x;
1851 Mat _R, _R2, _R1, _T2, _T1, _om2;
1852 Rodrigues(_om, _R);
1853 Rodrigues(_omL[i], _R1);
1854 _T1 = Mat(_tL[i]);
1855 _R2 = _R * _R1;
1856 _T2 = _R * _T1 + Mat(_T);
1857 Rodrigues(_R2, _om2);
1858
1859 omnidir::projectPoints(objPointsi, x, _om2, _T2, _K2, _xi2, _D2, cv::noArray());
1860
1861 Mat errorx = imgPointsi - x;
1862
1863 errorx.copyTo(reprojErrorAll.rowRange((i*2+1)*n_points, (i+1)*2*n_points));
1864 }
1865
1866 meanStdDev(reprojErrorAll, cv::noArray(), std_error);
1867 std_error *= sqrt((double)reprojErrorAll.total()/((double)reprojErrorAll.total() - 1.0));
1868
1869 Mat sigma_x;
1870 meanStdDev(reprojErrorAll.reshape(1,1), noArray(), sigma_x);
1871 sigma_x *= sqrt(2.0*(double)reprojErrorAll.total()/(2.0*(double)reprojErrorAll.total() - 1.0));
1872 double s = sigma_x.at<double>(0);
1873
1874 Mat _JTJ_inv, _JTE;
1875 computeJacobianStereo(objectPoints, imagePoints1, imagePoints2, _parameters, _JTJ_inv, _JTE, flags, 0.0);
1876 cv::sqrt(_JTJ_inv, _JTJ_inv);
1877
1878 errors = 3 * s * _JTJ_inv.diag();
1879
1880 rms = 0;
1881
1882 const Vec2d* ptr_ex = reprojErrorAll.ptr<Vec2d>();
1883 for (int i = 0; i < (int)reprojErrorAll.total(); i++)
1884 {
1885 rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1];
1886 }
1887 rms /= (double)reprojErrorAll.total();
1888 rms = sqrt(rms);
1889 }
1890
1891 //
computeMeanReproErr(InputArrayOfArrays imagePoints,InputArrayOfArrays proImagePoints)1892 double cv::omnidir::internal::computeMeanReproErr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints)
1893 {
1894 CV_Assert(!imagePoints.empty() && imagePoints.type()==CV_64FC2);
1895 CV_Assert(!proImagePoints.empty() && proImagePoints.type() == CV_64FC2);
1896 CV_Assert(imagePoints.total() == proImagePoints.total());
1897
1898 int n = (int)imagePoints.total();
1899 double reprojError = 0;
1900 int totalPoints = 0;
1901 if (imagePoints.kind() == _InputArray::STD_VECTOR_MAT)
1902 {
1903 for (int i = 0; i < n; i++)
1904 {
1905 Mat x, proj_x;
1906 imagePoints.getMat(i).copyTo(x);
1907 proImagePoints.getMat(i).copyTo(proj_x);
1908 Mat errorI = x.reshape(2, x.rows*x.cols) - proj_x.reshape(2, proj_x.rows*proj_x.cols);
1909 //Mat errorI = imagePoints.getMat(i) - proImagePoints.getMat(i);
1910 totalPoints += (int)errorI.total();
1911 Vec2d* ptr_err = errorI.ptr<Vec2d>();
1912 for (int j = 0; j < (int)errorI.total(); j++)
1913 {
1914 reprojError += sqrt(ptr_err[j][0]*ptr_err[j][0] + ptr_err[j][1]*ptr_err[j][1]);
1915 }
1916 }
1917 }
1918 else
1919 {
1920 Mat x, proj_x;
1921 imagePoints.getMat().copyTo(x);
1922 proImagePoints.getMat().copyTo(proj_x);
1923 Mat errorI = x.reshape(2, x.rows*x.cols) - proj_x.reshape(2, proj_x.rows*proj_x.cols);
1924 //Mat errorI = imagePoints.getMat() - proImagePoints.getMat();
1925 totalPoints += (int)errorI.total();
1926 Vec2d* ptr_err = errorI.ptr<Vec2d>();
1927 for (int j = 0; j < (int)errorI.total(); j++)
1928 {
1929 reprojError += sqrt(ptr_err[j][0]*ptr_err[j][0] + ptr_err[j][1]*ptr_err[j][1]);
1930 }
1931 }
1932 double meanReprojError = reprojError / totalPoints;
1933 return meanReprojError;
1934 }
1935
computeMeanReproErr(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints,InputArray K,InputArray D,double xi,InputArrayOfArrays omAll,InputArrayOfArrays tAll)1936 double cv::omnidir::internal::computeMeanReproErr(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray K, InputArray D, double xi, InputArrayOfArrays omAll,
1937 InputArrayOfArrays tAll)
1938 {
1939 CV_Assert(objectPoints.total() == imagePoints.total());
1940 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
1941 CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
1942 std::vector<Mat> proImagePoints;
1943 int n = (int)objectPoints.total();
1944 Mat _omAll = omAll.getMat();
1945 Mat _tAll = tAll.getMat();
1946 for(int i = 0; i < n; ++i)
1947 {
1948 Mat imgPoint;
1949 //cv::omnidir::projectPoints(objetPoints.getMat(i), imgPoint, omAll.getMat(i), tAll.getMat(i), K.getMat(), xi, D.getMat(), noArray());
1950 cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoint, _omAll.at<Vec3d>(i), _tAll.at<Vec3d>(i), K.getMat(), xi, D.getMat(), noArray());
1951 proImagePoints.push_back(imgPoint);
1952 }
1953
1954 return internal::computeMeanReproErr(imagePoints, proImagePoints);
1955 }
1956
computeMeanReproErrStereo(InputArrayOfArrays objectPoints,InputArrayOfArrays imagePoints1,InputArrayOfArrays imagePoints2,InputArray K1,InputArray K2,InputArray D1,InputArray D2,double xi1,double xi2,InputArray om,InputArray T,InputArrayOfArrays omL,InputArrayOfArrays TL)1957 double cv::omnidir::internal::computeMeanReproErrStereo(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, InputArray K1, InputArray K2,
1958 InputArray D1, InputArray D2, double xi1, double xi2, InputArray om, InputArray T, InputArrayOfArrays omL, InputArrayOfArrays TL)
1959 {
1960 CV_Assert(objectPoints.total() == imagePoints1.total() && imagePoints1.total() == imagePoints2.total());
1961 CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
1962 CV_Assert(!imagePoints1.empty() && imagePoints1.type() == CV_64FC2);
1963 CV_Assert(!imagePoints2.empty() && imagePoints2.type() == CV_64FC2);
1964
1965 std::vector<Mat> proImagePoints1, proImagePoints2;
1966 int n = (int)objectPoints.total();
1967 Mat _omL = omL.getMat(), _TL = TL.getMat();
1968 Mat _om = om.getMat(), _R, _T = T.getMat();
1969 Rodrigues(_om, _R);
1970 Mat _K1 = K1.getMat(), _K2 = K2.getMat();
1971 Mat _D1 = D1.getMat(), _D2 = D2.getMat();
1972
1973 // reprojection error for left image
1974 for (int i = 0; i < n; ++i)
1975 {
1976 Mat imgPoints;
1977 cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoints, _omL.at<Vec3d>(i), _TL.at<Vec3d>(i), _K1, xi1, _D1, cv::noArray());
1978 proImagePoints1.push_back(imgPoints);
1979 }
1980
1981 // reprojection error for right image
1982 for (int i = 0; i < n; ++i)
1983 {
1984 Mat imgPoints;
1985 Mat _omRi,_RRi,_TRi,_RLi, _TLi;
1986 Rodrigues(_omL.at<Vec3d>(i), _RLi);
1987 _TLi = Mat(_TL.at<Vec3d>(i)).reshape(1, 3);
1988 _RRi = _R * _RLi;
1989 _TRi = _R * _TLi + _T;
1990 Rodrigues(_RRi, _omRi);
1991 cv::omnidir::projectPoints(objectPoints.getMat(i), imgPoints, _omRi, _TRi, _K2, xi2, _D2, cv::noArray());
1992 proImagePoints2.push_back(imgPoints);
1993 }
1994 double reProErr1 = internal::computeMeanReproErr(imagePoints1, proImagePoints1);
1995 double reProErr2 = internal::computeMeanReproErr(imagePoints2, proImagePoints2);
1996
1997 double reProErr = (reProErr1 + reProErr2) / 2.0;
1998 //double reProErr = reProErr1*reProErr1 + reProErr2* reProErr2;
1999 return reProErr;
2000 }
2001
2002 // This function is from fisheye.cpp
subMatrix(const Mat & src,Mat & dst,const std::vector<int> & cols,const std::vector<int> & rows)2003 void cv::omnidir::internal::subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows)
2004 {
2005 CV_Assert(src.type() == CV_64FC1);
2006
2007 int nonzeros_cols = cv::countNonZero(cols);
2008 Mat tmp(src.rows, nonzeros_cols, CV_64FC1);
2009
2010 for (int i = 0, j = 0; i < (int)cols.size(); i++)
2011 {
2012 if (cols[i])
2013 {
2014 src.col(i).copyTo(tmp.col(j++));
2015 }
2016 }
2017
2018 int nonzeros_rows = cv::countNonZero(rows);
2019 Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1);
2020 for (int i = 0, j = 0; i < (int)rows.size(); i++)
2021 {
2022 if (rows[i])
2023 {
2024 tmp.row(i).copyTo(tmp1.row(j++));
2025 }
2026 }
2027
2028 dst = tmp1.clone();
2029 }
2030
flags2idx(int flags,std::vector<int> & idx,int n)2031 void cv::omnidir::internal::flags2idx(int flags, std::vector<int>& idx, int n)
2032 {
2033 idx = std::vector<int>(6*n+10,1);
2034 int _flags = flags;
2035 if(_flags >= omnidir::CALIB_FIX_CENTER)
2036 {
2037 idx[6*n+3] = 0;
2038 idx[6*n+4] = 0;
2039 _flags -= omnidir::CALIB_FIX_CENTER;
2040 }
2041 if(_flags >= omnidir::CALIB_FIX_GAMMA)
2042 {
2043 idx[6*n] = 0;
2044 idx[6*n+1] = 0;
2045 _flags -= omnidir::CALIB_FIX_GAMMA;
2046 }
2047 if(_flags >= omnidir::CALIB_FIX_XI)
2048 {
2049 idx[6*n + 5] = 0;
2050 _flags -= omnidir::CALIB_FIX_XI;
2051 }
2052 if(_flags >= omnidir::CALIB_FIX_P2)
2053 {
2054 idx[6*n + 9] = 0;
2055 _flags -= omnidir::CALIB_FIX_P2;
2056 }
2057 if(_flags >= omnidir::CALIB_FIX_P1)
2058 {
2059 idx[6*n + 8] = 0;
2060 _flags -= omnidir::CALIB_FIX_P1;
2061 }
2062 if(_flags >= omnidir::CALIB_FIX_K2)
2063 {
2064 idx[6*n + 7] = 0;
2065 _flags -= omnidir::CALIB_FIX_K2;
2066 }
2067 if(_flags >= omnidir::CALIB_FIX_K1)
2068 {
2069 idx[6*n + 6] = 0;
2070 _flags -= omnidir::CALIB_FIX_K1;
2071 }
2072 if(_flags >= omnidir::CALIB_FIX_SKEW)
2073 {
2074 idx[6*n + 2] = 0;
2075 }
2076 }
2077
flags2idxStereo(int flags,std::vector<int> & idx,int n)2078 void cv::omnidir::internal::flags2idxStereo(int flags, std::vector<int>& idx, int n)
2079 {
2080 idx = std::vector<int>(6*(n+1)+20, 1);
2081 int _flags = flags;
2082 int offset1 = 6*(n+1);
2083 int offset2 = offset1 + 10;
2084 if(_flags >= omnidir::CALIB_FIX_CENTER)
2085 {
2086 idx[offset1+3] = 0;
2087 idx[offset1+4] = 0;
2088 idx[offset2+3] = 0;
2089 idx[offset2+4] = 0;
2090 _flags -= omnidir::CALIB_FIX_CENTER;
2091 }
2092 if(_flags >= omnidir::CALIB_FIX_GAMMA)
2093 {
2094 idx[offset1] = 0;
2095 idx[offset1+1] = 0;
2096 idx[offset2] = 0;
2097 idx[offset2+1] = 0;
2098 _flags -= omnidir::CALIB_FIX_GAMMA;
2099 }
2100 if(_flags >= omnidir::CALIB_FIX_XI)
2101 {
2102 idx[offset1 + 5] = 0;
2103 idx[offset2 + 5] = 0;
2104 _flags -= omnidir::CALIB_FIX_XI;
2105 }
2106 if(_flags >= omnidir::CALIB_FIX_P2)
2107 {
2108 idx[offset1 + 9] = 0;
2109 idx[offset2 + 9] = 0;
2110 _flags -= omnidir::CALIB_FIX_P2;
2111 }
2112 if(_flags >= omnidir::CALIB_FIX_P1)
2113 {
2114 idx[offset1 + 8] = 0;
2115 idx[offset2 + 8] = 0;
2116 _flags -= omnidir::CALIB_FIX_P1;
2117 }
2118 if(_flags >= omnidir::CALIB_FIX_K2)
2119 {
2120 idx[offset1 + 7] = 0;
2121 idx[offset2 + 7] = 0;
2122 _flags -= omnidir::CALIB_FIX_K2;
2123 }
2124 if(_flags >= omnidir::CALIB_FIX_K1)
2125 {
2126 idx[offset1 + 6] = 0;
2127 idx[offset2 + 6] = 0;
2128 _flags -= omnidir::CALIB_FIX_K1;
2129 }
2130 if(_flags >= omnidir::CALIB_FIX_SKEW)
2131 {
2132 idx[offset1 + 2] = 0;
2133 idx[offset2 + 2] = 0;
2134 }
2135 }
2136
2137 // fill in zerso for fixed parameters
fillFixed(Mat & G,int flags,int n)2138 void cv::omnidir::internal::fillFixed(Mat&G, int flags, int n)
2139 {
2140 Mat tmp = G.clone();
2141 std::vector<int> idx(6*n + 10, 1);
2142 flags2idx(flags, idx, n);
2143 G.release();
2144 G.create(6*n +10, 1, CV_64F);
2145 G = cv::Mat::zeros(6*n +10, 1, CV_64F);
2146 for (int i = 0,j=0; i < (int)idx.size(); i++)
2147 {
2148 if (idx[i])
2149 {
2150 G.at<double>(i) = tmp.at<double>(j++);
2151 }
2152 }
2153 }
2154
fillFixedStereo(Mat & G,int flags,int n)2155 void cv::omnidir::internal::fillFixedStereo(Mat& G, int flags, int n)
2156 {
2157 Mat tmp = G.clone();
2158 std::vector<int> idx(6*(n+1)+20, 1);
2159 flags2idxStereo(flags, idx, n);
2160 G.release();
2161 G.create(6 * (n+1) + 20, 1, CV_64F);
2162 G = cv::Mat::zeros(6* (n + 1) + 20, 1, CV_64F);
2163 for (int i = 0,j=0; i < (int)idx.size(); i++)
2164 {
2165 if (idx[i])
2166 {
2167 G.at<double>(i) = tmp.at<double>(j++);
2168 }
2169 }
2170 }
2171
findMedian(const Mat & row)2172 double cv::omnidir::internal::findMedian(const Mat& row)
2173 {
2174 CV_Assert(!row.empty() && row.rows == 1 && row.type() == CV_64F);
2175 Mat tmp = row.clone();
2176 cv::sort(tmp, tmp, 0);
2177 if((int)tmp.total()%2 == 0)
2178 return tmp.at<double>((int)tmp.total() / 2);
2179 else
2180 return 0.5 * (tmp.at<double>((int)tmp.total() / 2) + tmp.at<double>((int)tmp.total()/2 - 1));
2181 }
2182
findMedian3(InputArray mat)2183 cv::Vec3d cv::omnidir::internal::findMedian3(InputArray mat)
2184 {
2185 CV_Assert(mat.depth() == CV_64F && mat.getMat().rows == 1);
2186 Mat M = Mat(mat.getMat().t()).reshape(1).t();
2187 return Vec3d(findMedian(M.row(0)), findMedian(M.row(1)), findMedian(M.row(2)));
2188 }
2189
stereoRectify(InputArray R,InputArray T,OutputArray R1,OutputArray R2)2190 void cv::omnidir::stereoRectify(InputArray R, InputArray T, OutputArray R1, OutputArray R2)
2191 {
2192 CV_Assert((R.size() == Size(3,3) || R.total() == 3) && (R.depth() == CV_32F || R.depth() == CV_64F));
2193 CV_Assert(T.total() == 3 && (T.depth() == CV_32F || T.depth() == CV_64F));
2194
2195 Mat _R, _T;
2196 if (R.size() == Size(3, 3))
2197 {
2198 R.getMat().convertTo(_R, CV_64F);
2199 }
2200 else if (R.total() == 3)
2201 {
2202 Rodrigues(R.getMat(), _R);
2203 _R.convertTo(_R, CV_64F);
2204 }
2205
2206 T.getMat().reshape(1, 3).convertTo(_T, CV_64F);
2207
2208 R1.create(3, 3, CV_64F);
2209 Mat _R1 = R1.getMat();
2210 R2.create(3, 3, CV_64F);
2211 Mat _R2 = R2.getMat();
2212
2213 Mat R21 = _R.t();
2214 Mat T21 = -_R.t() * _T;
2215
2216 Mat e1, e2, e3;
2217 e1 = T21.t() / norm(T21);
2218 e2 = Mat(Matx13d(T21.at<double>(1)*-1, T21.at<double>(0), 0.0));
2219 e2 = e2 / norm(e2);
2220 e3 = e1.cross(e2);
2221 e3 = e3 / norm(e3);
2222 e1.copyTo(_R1.row(0));
2223 e2.copyTo(_R1.row(1));
2224 e3.copyTo(_R1.row(2));
2225 _R2 = _R1 * R21;
2226
2227 }
2228
getInterset(InputArray idx1,InputArray idx2,OutputArray inter1,OutputArray inter2,OutputArray inter_ori)2229 void cv::omnidir::internal::getInterset(InputArray idx1, InputArray idx2, OutputArray inter1, OutputArray inter2,
2230 OutputArray inter_ori)
2231 {
2232 Mat _idx1 = idx1.getMat();
2233 Mat _idx2 = idx2.getMat();
2234
2235 int n1 = (int)idx1.total();
2236 int n2 = (int)idx2.total();
2237
2238 std::vector<int> _inter1, _inter2, _inter_ori;
2239 for (int i = 0; i < n1; ++i)
2240 {
2241 for (int j = 0; j < n2; ++j)
2242 {
2243 if(_idx1.at<int>(i) == _idx2.at<int>(j))
2244 {
2245 _inter1.push_back(i);
2246 _inter2.push_back(j);
2247 _inter_ori.push_back(_idx1.at<int>(i));
2248 }
2249 }
2250 }
2251
2252 inter1.create(1, (int)_inter1.size(), CV_32S);
2253 inter2.create(1, (int)_inter2.size(), CV_32S);
2254 inter_ori.create(1, (int)_inter_ori.size(), CV_32S);
2255
2256 for (int i = 0; i < (int)_inter1.size(); ++i)
2257 {
2258 inter1.getMat().at<int>(i) = _inter1[i];
2259 }
2260 for (int i = 0; i < (int)_inter2.size(); ++i)
2261 {
2262 inter2.getMat().at<int>(i) = _inter2[i];
2263 }
2264 for (int i = 0; i < (int)_inter_ori.size(); ++i)
2265 {
2266 inter_ori.getMat().at<int>(i) = _inter_ori[i];
2267 }
2268 }
2269