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) 2000-2008, Intel Corporation, all rights reserved.
14	// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
15	// Third party copyrights are property of their respective owners.
16	//
17	// Redistribution and use in source and binary forms, with or without modification,
18	// are permitted provided that the following conditions are met:
19	//
20	// * Redistribution's of source code must retain the above copyright notice,
21	// this list of conditions and the following disclaimer.
22	//
23	// * Redistribution's in binary form must reproduce the above copyright notice,
24	// this list of conditions and the following disclaimer in the documentation
25	// and/or other materials provided with the distribution.
26	//
27	// * The name of the copyright holders may not be used to endorse or promote products
28	// derived from this software without specific prior written permission.
29	//
30	// This software is provided by the copyright holders and contributors "as is" and
31	// any express or implied warranties, including, but not limited to, the implied
32	// warranties of merchantability and fitness for a particular purpose are disclaimed.
33	// In no event shall the Intel Corporation or contributors be liable for any direct,
34	// indirect, incidental, special, exemplary, or consequential damages
35	// (including, but not limited to, procurement of substitute goods or services;
36	// loss of use, data, or profits; or business interruption) however caused
37	// and on any theory of liability, whether in contract, strict liability,
38	// or tort (including negligence or otherwise) arising in any way out of
39	// the use of this software, even if advised of the possibility of such damage.
40	//
41	//M*/
42
43	#include "precomp.hpp"
44	#include "fisheye.hpp"
45	#include <limits>
46
47	namespace cv { namespace
48	{
49	struct JacobianRow
50	{
51	Vec2d df, dc;
52	Vec4d dk;
53	Vec3d dom, dT;
54	double dalpha;
55	};
56
57	void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& rows);
58	}}
59
60	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
61	/// cv::fisheye::projectPoints
62
63	void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine,
64	InputArray K, InputArray D, double alpha, OutputArray jacobian)
65	{
66	CV_INSTRUMENT_REGION();
67
68	projectPoints(objectPoints, imagePoints, rvec: affine.rvec(), tvec: affine.translation(), K, D, alpha, jacobian);
69	}
70
71	void cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec,
72	InputArray _tvec, InputArray _K, InputArray _D, double alpha, OutputArray jacobian)
73	{
74	CV_INSTRUMENT_REGION();
75
76	// will support only 3-channel data now for points
77	CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
78	imagePoints.create(sz: objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2));
79	size_t n = objectPoints.total();
80
81	CV_Assert(_rvec.total() * _rvec.channels() == 3 && (_rvec.depth() == CV_32F || _rvec.depth() == CV_64F));
82	CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F));
83	CV_Assert(_tvec.getMat().isContinuous() && _rvec.getMat().isContinuous());
84
85	Vec3d om = _rvec.depth() == CV_32F ? (Vec3d)*_rvec.getMat().ptr<Vec3f>() : *_rvec.getMat().ptr<Vec3d>();
86	Vec3d T = _tvec.depth() == CV_32F ? (Vec3d)*_tvec.getMat().ptr<Vec3f>() : *_tvec.getMat().ptr<Vec3d>();
87
88	CV_Assert(_K.size() == Size(3,3) && (_K.type() == CV_32F || _K.type() == CV_64F) && _D.type() == _K.type() && _D.total() == 4);
89
90	cv::Vec2d f, c;
91	if (_K.depth() == CV_32F)
92	{
93
94	Matx33f K = _K.getMat();
95	f = Vec2f(K(0, 0), K(1, 1));
96	c = Vec2f(K(0, 2), K(1, 2));
97	}
98	else
99	{
100	Matx33d K = _K.getMat();
101	f = Vec2d(K(0, 0), K(1, 1));
102	c = Vec2d(K(0, 2), K(1, 2));
103	}
104
105	Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr<Vec4f>(): *_D.getMat().ptr<Vec4d>();
106
107	const bool isJacobianNeeded = jacobian.needed();
108	JacobianRow *Jn = 0;
109	if (isJacobianNeeded)
110	{
111	int nvars = 2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T,
112	jacobian.create(rows: 2*(int)n, cols: nvars, CV_64F);
113	Jn = jacobian.getMat().ptr<JacobianRow>(y: 0);
114	}
115
116	Matx33d R;
117	Matx<double, 3, 9> dRdom;
118	Rodrigues(src: om, dst: R, jacobian: dRdom);
119	Affine3d aff(om, T);
120
121	const Vec3f* Xf = objectPoints.getMat().ptr<Vec3f>();
122	const Vec3d* Xd = objectPoints.getMat().ptr<Vec3d>();
123	Vec2f *xpf = imagePoints.getMat().ptr<Vec2f>();
124	Vec2d *xpd = imagePoints.getMat().ptr<Vec2d>();
125
126	for(size_t i = 0; i < n; ++i)
127	{
128	Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i];
129	Vec3d Y = aff*Xi;
130	if (fabs(x: Y[2]) < DBL_MIN)
131	Y[2] = 1;
132	Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);
133
134	double r2 = x.dot(M: x);
135	double r = std::sqrt(x: r2);
136
137	// Angle of the incoming ray:
138	double theta = atan(x: r);
139
140	double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
141	theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
142
143	double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
144
145	double inv_r = r > 1e-8 ? 1.0/r : 1;
146	double cdist = r > 1e-8 ? theta_d * inv_r : 1;
147
148	Vec2d xd1 = x * cdist;
149	Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
150	Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]);
151
152	if (objectPoints.depth() == CV_32F)
153	xpf[i] = final_point;
154	else
155	xpd[i] = final_point;
156
157	if (isJacobianNeeded)
158	{
159	//Vec3d Xi = pdepth == CV_32F ? (Vec3d)Xf[i] : Xd[i];
160	//Vec3d Y = aff*Xi;
161	double dYdR[] = { Xi[0], Xi[1], Xi[2], 0, 0, 0, 0, 0, 0,
162	0, 0, 0, Xi[0], Xi[1], Xi[2], 0, 0, 0,
163	0, 0, 0, 0, 0, 0, Xi[0], Xi[1], Xi[2] };
164
165	Matx33d dYdom_data = Matx<double, 3, 9>(dYdR) * dRdom.t();
166	const Vec3d *dYdom = (Vec3d*)dYdom_data.val;
167
168	Matx33d dYdT_data = Matx33d::eye();
169	const Vec3d *dYdT = (Vec3d*)dYdT_data.val;
170
171	//Vec2d x(Y[0]/Y[2], Y[1]/Y[2]);
172	Vec3d dxdom[2];
173	dxdom[0] = (1.0/Y[2]) * dYdom[0] - x[0]/Y[2] * dYdom[2];
174	dxdom[1] = (1.0/Y[2]) * dYdom[1] - x[1]/Y[2] * dYdom[2];
175
176	Vec3d dxdT[2];
177	dxdT[0] = (1.0/Y[2]) * dYdT[0] - x[0]/Y[2] * dYdT[2];
178	dxdT[1] = (1.0/Y[2]) * dYdT[1] - x[1]/Y[2] * dYdT[2];
179
180	//double r2 = x.dot(x);
181	Vec3d dr2dom = 2 * x[0] * dxdom[0] + 2 * x[1] * dxdom[1];
182	Vec3d dr2dT = 2 * x[0] * dxdT[0] + 2 * x[1] * dxdT[1];
183
184	//double r = std::sqrt(r2);
185	double drdr2 = r > 1e-8 ? 1.0/(2*r) : 1;
186	Vec3d drdom = drdr2 * dr2dom;
187	Vec3d drdT = drdr2 * dr2dT;
188
189	// Angle of the incoming ray:
190	//double theta = atan(r);
191	double dthetadr = 1.0/(1+r2);
192	Vec3d dthetadom = dthetadr * drdom;
193	Vec3d dthetadT = dthetadr * drdT;
194
195	//double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
196	double dtheta_ddtheta = 1 + 3*k[0]*theta2 + 5*k[1]*theta4 + 7*k[2]*theta6 + 9*k[3]*theta8;
197	Vec3d dtheta_ddom = dtheta_ddtheta * dthetadom;
198	Vec3d dtheta_ddT = dtheta_ddtheta * dthetadT;
199	Vec4d dtheta_ddk = Vec4d(theta3, theta5, theta7, theta9);
200
201	//double inv_r = r > 1e-8 ? 1.0/r : 1;
202	//double cdist = r > 1e-8 ? theta_d / r : 1;
203	Vec3d dcdistdom = inv_r * (dtheta_ddom - cdist*drdom);
204	Vec3d dcdistdT = inv_r * (dtheta_ddT - cdist*drdT);
205	Vec4d dcdistdk = inv_r * dtheta_ddk;
206
207	//Vec2d xd1 = x * cdist;
208	Vec4d dxd1dk[2];
209	Vec3d dxd1dom[2], dxd1dT[2];
210	dxd1dom[0] = x[0] * dcdistdom + cdist * dxdom[0];
211	dxd1dom[1] = x[1] * dcdistdom + cdist * dxdom[1];
212	dxd1dT[0] = x[0] * dcdistdT + cdist * dxdT[0];
213	dxd1dT[1] = x[1] * dcdistdT + cdist * dxdT[1];
214	dxd1dk[0] = x[0] * dcdistdk;
215	dxd1dk[1] = x[1] * dcdistdk;
216
217	//Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
218	Vec4d dxd3dk[2];
219	Vec3d dxd3dom[2], dxd3dT[2];
220	dxd3dom[0] = dxd1dom[0] + alpha * dxd1dom[1];
221	dxd3dom[1] = dxd1dom[1];
222	dxd3dT[0] = dxd1dT[0] + alpha * dxd1dT[1];
223	dxd3dT[1] = dxd1dT[1];
224	dxd3dk[0] = dxd1dk[0] + alpha * dxd1dk[1];
225	dxd3dk[1] = dxd1dk[1];
226
227	Vec2d dxd3dalpha(xd1[1], 0);
228
229	//final jacobian
230	Jn[0].dom = f[0] * dxd3dom[0];
231	Jn[1].dom = f[1] * dxd3dom[1];
232
233	Jn[0].dT = f[0] * dxd3dT[0];
234	Jn[1].dT = f[1] * dxd3dT[1];
235
236	Jn[0].dk = f[0] * dxd3dk[0];
237	Jn[1].dk = f[1] * dxd3dk[1];
238
239	Jn[0].dalpha = f[0] * dxd3dalpha[0];
240	Jn[1].dalpha = 0; //f[1] * dxd3dalpha[1];
241
242	Jn[0].df = Vec2d(xd3[0], 0);
243	Jn[1].df = Vec2d(0, xd3[1]);
244
245	Jn[0].dc = Vec2d(1, 0);
246	Jn[1].dc = Vec2d(0, 1);
247
248	//step to jacobian rows for next point
249	Jn += 2;
250	}
251	}
252	}
253
254	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
255	/// cv::fisheye::distortPoints
256
257	void cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha)
258	{
259	CV_INSTRUMENT_REGION();
260
261	// will support only 2-channel data now for points
262	CV_Assert(undistorted.type() == CV_32FC2 || undistorted.type() == CV_64FC2);
263	distorted.create(sz: undistorted.size(), type: undistorted.type());
264	size_t n = undistorted.total();
265
266	CV_Assert(K.size() == Size(3,3) && (K.type() == CV_32F || K.type() == CV_64F) && D.total() == 4);
267
268	cv::Vec2d f, c;
269	if (K.depth() == CV_32F)
270	{
271	Matx33f camMat = K.getMat();
272	f = Vec2f(camMat(0, 0), camMat(1, 1));
273	c = Vec2f(camMat(0, 2), camMat(1, 2));
274	}
275	else
276	{
277	Matx33d camMat = K.getMat();
278	f = Vec2d(camMat(0, 0), camMat(1, 1));
279	c = Vec2d(camMat(0 ,2), camMat(1, 2));
280	}
281
282	Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>();
283
284	const Vec2f* Xf = undistorted.getMat().ptr<Vec2f>();
285	const Vec2d* Xd = undistorted.getMat().ptr<Vec2d>();
286	Vec2f *xpf = distorted.getMat().ptr<Vec2f>();
287	Vec2d *xpd = distorted.getMat().ptr<Vec2d>();
288
289	for(size_t i = 0; i < n; ++i)
290	{
291	Vec2d x = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i];
292
293	double r2 = x.dot(M: x);
294	double r = std::sqrt(x: r2);
295
296	// Angle of the incoming ray:
297	double theta = atan(x: r);
298
299	double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta,
300	theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta;
301
302	double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9;
303
304	double inv_r = r > 1e-8 ? 1.0/r : 1;
305	double cdist = r > 1e-8 ? theta_d * inv_r : 1;
306
307	Vec2d xd1 = x * cdist;
308	Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]);
309	Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]);
310
311	if (undistorted.depth() == CV_32F)
312	xpf[i] = final_point;
313	else
314	xpd[i] = final_point;
315	}
316	}
317
318	void cv::fisheye::distortPoints(InputArray _undistorted, OutputArray distorted, InputArray Kundistorted, InputArray K, InputArray D, double alpha)
319	{
320	CV_INSTRUMENT_REGION();
321
322	CV_Assert(_undistorted.type() == CV_32FC2 || _undistorted.type() == CV_64FC2);
323	CV_Assert(Kundistorted.size() == Size(3,3) && (Kundistorted.type() == CV_32F || Kundistorted.type() == CV_64F));
324
325	cv::Mat undistorted = _undistorted.getMat();
326	cv::Mat normalized(undistorted.size(), CV_64FC2);
327
328	Mat Knew = Kundistorted.getMat();
329
330	double cx, cy, fx, fy;
331	if (Knew.depth() == CV_32F)
332	{
333	fx = (double)Knew.at<float>(i0: 0, i1: 0);
334	fy = (double)Knew.at<float>(i0: 1, i1: 1);
335	cx = (double)Knew.at<float>(i0: 0, i1: 2);
336	cy = (double)Knew.at<float>(i0: 1, i1: 2);
337	}
338	else
339	{
340	fx = Knew.at<double>(i0: 0, i1: 0);
341	fy = Knew.at<double>(i0: 1, i1: 1);
342	cx = Knew.at<double>(i0: 0, i1: 2);
343	cy = Knew.at<double>(i0: 1, i1: 2);
344	}
345
346	size_t n = undistorted.total();
347	const Vec2f* Xf = undistorted.ptr<Vec2f>();
348	const Vec2d* Xd = undistorted.ptr<Vec2d>();
349	Vec2d* normXd = normalized.ptr<Vec2d>();
350	for (size_t i = 0; i < n; i++)
351	{
352	Vec2d p = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i];
353	normXd[i][0] = (p[0] - cx) / fx;
354	normXd[i][1] = (p[1] - cy) / fy;
355	}
356
357	cv::fisheye::distortPoints(undistorted: normalized, distorted, K, D, alpha);
358	}
359
360	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
361	/// cv::fisheye::undistortPoints
362
363	void cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray K, InputArray D,
364	InputArray R, InputArray P, TermCriteria criteria)
365	{
366	CV_INSTRUMENT_REGION();
367
368	// will support only 2-channel data now for points
369	CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2);
370	undistorted.create(sz: distorted.size(), type: distorted.type());
371
372	CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3));
373	CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3);
374	CV_Assert(D.total() == 4 && K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F));
375
376	CV_Assert(criteria.isValid());
377
378	cv::Vec2d f, c;
379	if (K.depth() == CV_32F)
380	{
381	Matx33f camMat = K.getMat();
382	f = Vec2f(camMat(0, 0), camMat(1, 1));
383	c = Vec2f(camMat(0, 2), camMat(1, 2));
384	}
385	else
386	{
387	Matx33d camMat = K.getMat();
388	f = Vec2d(camMat(0, 0), camMat(1, 1));
389	c = Vec2d(camMat(0, 2), camMat(1, 2));
390	}
391
392	Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>();
393
394	cv::Matx33d RR = cv::Matx33d::eye();
395	if (!R.empty() && R.total() * R.channels() == 3)
396	{
397	cv::Vec3d rvec;
398	R.getMat().convertTo(m: rvec, CV_64F);
399	RR = cv::Affine3d(rvec).rotation();
400	}
401	else if (!R.empty() && R.size() == Size(3, 3))
402	R.getMat().convertTo(m: RR, CV_64F);
403
404	if(!P.empty())
405	{
406	cv::Matx33d PP;
407	P.getMat().colRange(startcol: 0, endcol: 3).convertTo(m: PP, CV_64F);
408	RR = PP * RR;
409	}
410
411	// start undistorting
412	const cv::Vec2f* srcf = distorted.getMat().ptr<cv::Vec2f>();
413	const cv::Vec2d* srcd = distorted.getMat().ptr<cv::Vec2d>();
414	cv::Vec2f* dstf = undistorted.getMat().ptr<cv::Vec2f>();
415	cv::Vec2d* dstd = undistorted.getMat().ptr<cv::Vec2d>();
416
417	size_t n = distorted.total();
418	int sdepth = distorted.depth();
419
420	const bool isEps = (criteria.type & TermCriteria::EPS) != 0;
421
422	/* Define max count for solver iterations */
423	int maxCount = std::numeric_limits<int>::max();
424	if (criteria.type & TermCriteria::MAX_ITER) {
425	maxCount = criteria.maxCount;
426	}
427
428
429	for(size_t i = 0; i < n; i++ )
430	{
431	Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point
432	Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point
433
434	double theta_d = sqrt(x: pw[0]*pw[0] + pw[1]*pw[1]);
435
436	// the current camera model is only valid up to 180 FOV
437	// for larger FOV the loop below does not converge
438	// clip values so we still get plausible results for super fisheye images > 180 grad
439	theta_d = min(a: max(a: -CV_PI/2., b: theta_d), CV_PI/2.);
440
441	bool converged = false;
442	double theta = theta_d;
443
444	double scale = 0.0;
445
446	if (!isEps || fabs(x: theta_d) > criteria.epsilon)
447	{
448	// compensate distortion iteratively using Newton method
449
450	for (int j = 0; j < maxCount; j++)
451	{
452	double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2;
453	double k0_theta2 = k[0] * theta2, k1_theta4 = k[1] * theta4, k2_theta6 = k[2] * theta6, k3_theta8 = k[3] * theta8;
454	/* new_theta = theta - theta_fix, theta_fix = f0(theta) / f0'(theta) */
455	double theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
456	(1 + 3*k0_theta2 + 5*k1_theta4 + 7*k2_theta6 + 9*k3_theta8);
457	theta = theta - theta_fix;
458
459	if (isEps && (fabs(x: theta_fix) < criteria.epsilon))
460	{
461	converged = true;
462	break;
463	}
464	}
465
466	scale = std::tan(x: theta) / theta_d;
467	}
468	else
469	{
470	converged = true;
471	}
472
473	// theta is monotonously increasing or decreasing depending on the sign of theta
474	// if theta has flipped, it might converge due to symmetry but on the opposite of the camera center
475	// so we can check whether theta has changed the sign during the optimization
476	bool theta_flipped = ((theta_d < 0 && theta > 0) || (theta_d > 0 && theta < 0));
477
478	if ((converged || !isEps) && !theta_flipped)
479	{
480	Vec2d pu = pw * scale; //undistorted point
481
482	// reproject
483	Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix
484	Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final
485
486	if( sdepth == CV_32F )
487	dstf[i] = fi;
488	else
489	dstd[i] = fi;
490	}
491	else
492	{
493	// Vec2d fi(std::numeric_limits<double>::quiet_NaN(), std::numeric_limits<double>::quiet_NaN());
494	Vec2d fi(-1000000.0, -1000000.0);
495
496	if( sdepth == CV_32F )
497	dstf[i] = fi;
498	else
499	dstd[i] = fi;
500	}
501	}
502	}
503
504	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
505	/// cv::fisheye::initUndistortRectifyMap
506
507	void cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArray R, InputArray P,
508	const cv::Size& size, int m1type, OutputArray map1, OutputArray map2 )
509	{
510	CV_INSTRUMENT_REGION();
511
512	CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 );
513	map1.create( sz: size, type: m1type <= 0 ? CV_16SC2 : m1type );
514	map2.create( sz: size, type: map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F );
515
516	CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F));
517	CV_Assert((P.empty() || P.depth() == CV_32F || P.depth() == CV_64F) && (R.empty() || R.depth() == CV_32F || R.depth() == CV_64F));
518	CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4));
519	CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3);
520	CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3));
521
522	cv::Vec2d f, c;
523	if (K.depth() == CV_32F)
524	{
525	Matx33f camMat = K.getMat();
526	f = Vec2f(camMat(0, 0), camMat(1, 1));
527	c = Vec2f(camMat(0, 2), camMat(1, 2));
528	}
529	else
530	{
531	Matx33d camMat = K.getMat();
532	f = Vec2d(camMat(0, 0), camMat(1, 1));
533	c = Vec2d(camMat(0, 2), camMat(1, 2));
534	}
535
536	Vec4d k = Vec4d::all(alpha: 0);
537	if (!D.empty())
538	k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>();
539
540	cv::Matx33d RR = cv::Matx33d::eye();
541	if (!R.empty() && R.total() * R.channels() == 3)
542	{
543	cv::Vec3d rvec;
544	R.getMat().convertTo(m: rvec, CV_64F);
545	RR = Affine3d(rvec).rotation();
546	}
547	else if (!R.empty() && R.size() == Size(3, 3))
548	R.getMat().convertTo(m: RR, CV_64F);
549
550	cv::Matx33d PP = cv::Matx33d::eye();
551	if (!P.empty())
552	P.getMat().colRange(startcol: 0, endcol: 3).convertTo(m: PP, CV_64F);
553
554	cv::Matx33d iR = (PP * RR).inv(method: cv::DECOMP_SVD);
555
556	for( int i = 0; i < size.height; ++i)
557	{
558	float* m1f = map1.getMat().ptr<float>(y: i);
559	float* m2f = map2.getMat().ptr<float>(y: i);
560	short* m1 = (short*)m1f;
561	ushort* m2 = (ushort*)m2f;
562
563	double _x = i*iR(0, 1) + iR(0, 2),
564	_y = i*iR(1, 1) + iR(1, 2),
565	_w = i*iR(2, 1) + iR(2, 2);
566
567	for( int j = 0; j < size.width; ++j)
568	{
569	double u, v;
570	if( _w <= 0)
571	{
572	u = (_x > 0) ? -std::numeric_limits<double>::infinity() : std::numeric_limits<double>::infinity();
573	v = (_y > 0) ? -std::numeric_limits<double>::infinity() : std::numeric_limits<double>::infinity();
574	}
575	else
576	{
577	double x = _x/_w, y = _y/_w;
578
579	double r = sqrt(x: x*x + y*y);
580	double theta = atan(x: r);
581
582	double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4;
583	double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8);
584
585	double scale = (r == 0) ? 1.0 : theta_d / r;
586	u = f[0]*x*scale + c[0];
587	v = f[1]*y*scale + c[1];
588	}
589
590	if( m1type == CV_16SC2 )
591	{
592	int iu = cv::saturate_cast<int>(v: u*static_cast<double>(cv::INTER_TAB_SIZE));
593	int iv = cv::saturate_cast<int>(v: v*static_cast<double>(cv::INTER_TAB_SIZE));
594	m1[j*2+0] = (short)(iu >> cv::INTER_BITS);
595	m1[j*2+1] = (short)(iv >> cv::INTER_BITS);
596	m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1)));
597	}
598	else if( m1type == CV_32FC1 )
599	{
600	m1f[j] = (float)u;
601	m2f[j] = (float)v;
602	}
603
604	_x += iR(0, 0);
605	_y += iR(1, 0);
606	_w += iR(2, 0);
607	}
608	}
609	}
610
611	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
612	/// cv::fisheye::undistortImage
613
614	void cv::fisheye::undistortImage(InputArray distorted, OutputArray undistorted,
615	InputArray K, InputArray D, InputArray Knew, const Size& new_size)
616	{
617	CV_INSTRUMENT_REGION();
618
619	Size size = !new_size.empty() ? new_size : distorted.size();
620
621	cv::Mat map1, map2;
622	fisheye::initUndistortRectifyMap(K, D, R: cv::Matx33d::eye(), P: Knew, size, CV_16SC2, map1, map2 );
623	cv::remap(src: distorted, dst: undistorted, map1, map2, interpolation: INTER_LINEAR, borderMode: BORDER_CONSTANT);
624	}
625
626
627	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
628	/// cv::fisheye::estimateNewCameraMatrixForUndistortRectify
629
630	void cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R,
631	OutputArray P, double balance, const Size& new_size, double fov_scale)
632	{
633	CV_INSTRUMENT_REGION();
634
635	CV_Assert( K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F));
636	CV_Assert(D.empty() || ((D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F)));
637
638	int w = image_size.width, h = image_size.height;
639	balance = std::min(a: std::max(a: balance, b: 0.0), b: 1.0);
640
641	cv::Mat points(1, 4, CV_64FC2);
642	Vec2d* pptr = points.ptr<Vec2d>();
643	pptr[0] = Vec2d(w/2, 0);
644	pptr[1] = Vec2d(w, h/2);
645	pptr[2] = Vec2d(w/2, h);
646	pptr[3] = Vec2d(0, h/2);
647
648	fisheye::undistortPoints(distorted: points, undistorted: points, K, D, R);
649	cv::Scalar center_mass = mean(src: points);
650	cv::Vec2d cn(center_mass.val);
651
652	double aspect_ratio = (K.depth() == CV_32F) ? K.getMat().at<float >(i0: 0,i1: 0)/K.getMat().at<float> (i0: 1,i1: 1)
653	: K.getMat().at<double>(i0: 0,i1: 0)/K.getMat().at<double>(i0: 1,i1: 1);
654
655	// convert to identity ratio
656	cn[1] *= aspect_ratio;
657	for(size_t i = 0; i < points.total(); ++i)
658	pptr[i][1] *= aspect_ratio;
659
660	double minx = DBL_MAX, miny = DBL_MAX, maxx = -DBL_MAX, maxy = -DBL_MAX;
661	for(size_t i = 0; i < points.total(); ++i)
662	{
663	miny = std::min(a: miny, b: pptr[i][1]);
664	maxy = std::max(a: maxy, b: pptr[i][1]);
665	minx = std::min(a: minx, b: pptr[i][0]);
666	maxx = std::max(a: maxx, b: pptr[i][0]);
667	}
668
669	double f1 = w * 0.5/(cn[0] - minx);
670	double f2 = w * 0.5/(maxx - cn[0]);
671	double f3 = h * 0.5 * aspect_ratio/(cn[1] - miny);
672	double f4 = h * 0.5 * aspect_ratio/(maxy - cn[1]);
673
674	double fmin = std::min(a: f1, b: std::min(a: f2, b: std::min(a: f3, b: f4)));
675	double fmax = std::max(a: f1, b: std::max(a: f2, b: std::max(a: f3, b: f4)));
676
677	double f = balance * fmin + (1.0 - balance) * fmax;
678	f *= fov_scale > 0 ? 1.0/fov_scale : 1.0;
679
680	cv::Vec2d new_f(f, f), new_c = -cn * f + Vec2d(w, h * aspect_ratio) * 0.5;
681
682	// restore aspect ratio
683	new_f[1] /= aspect_ratio;
684	new_c[1] /= aspect_ratio;
685
686	if (!new_size.empty())
687	{
688	double rx = new_size.width /(double)image_size.width;
689	double ry = new_size.height/(double)image_size.height;
690
691	new_f[0] *= rx; new_f[1] *= ry;
692	new_c[0] *= rx; new_c[1] *= ry;
693	}
694
695	Mat(Matx33d(new_f[0], 0, new_c[0],
696	0, new_f[1], new_c[1],
697	0, 0, 1)).convertTo(m: P, rtype: P.empty() ? K.type() : P.type());
698	}
699
700
701	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
702	/// cv::fisheye::calibrate
703
704	double cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size,
705	InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs,
706	int flags , cv::TermCriteria criteria)
707	{
708	CV_INSTRUMENT_REGION();
709
710	CV_Assert(!objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total());
711	CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
712	CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2);
713	CV_Assert(K.empty() || (K.size() == Size(3,3)));
714	CV_Assert(D.empty() || (D.total() == 4));
715	CV_Assert(rvecs.empty() || (rvecs.channels() == 3));
716	CV_Assert(tvecs.empty() || (tvecs.channels() == 3));
717
718	CV_Assert((!K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS));
719
720	using namespace cv::internal;
721	//-------------------------------Initialization
722	IntrinsicParams finalParam;
723	IntrinsicParams currentParam;
724	IntrinsicParams errors;
725
726	finalParam.isEstimate[0] = flags & CALIB_FIX_FOCAL_LENGTH ? 0 : 1;
727	finalParam.isEstimate[1] = flags & CALIB_FIX_FOCAL_LENGTH ? 0 : 1;
728	finalParam.isEstimate[2] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
729	finalParam.isEstimate[3] = flags & CALIB_FIX_PRINCIPAL_POINT ? 0 : 1;
730	finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1;
731	finalParam.isEstimate[5] = flags & CALIB_FIX_K1 ? 0 : 1;
732	finalParam.isEstimate[6] = flags & CALIB_FIX_K2 ? 0 : 1;
733	finalParam.isEstimate[7] = flags & CALIB_FIX_K3 ? 0 : 1;
734	finalParam.isEstimate[8] = flags & CALIB_FIX_K4 ? 0 : 1;
735
736	const int recompute_extrinsic = flags & CALIB_RECOMPUTE_EXTRINSIC ? 1: 0;
737	const int check_cond = flags & CALIB_CHECK_COND ? 1 : 0;
738
739	const double alpha_smooth = 0.4;
740	const double thresh_cond = 1e6;
741	double change = 1;
742	Vec2d err_std;
743
744	Matx33d _K;
745	Vec4d _D;
746	if (flags & CALIB_USE_INTRINSIC_GUESS)
747	{
748	K.getMat().convertTo(m: _K, CV_64FC1);
749	D.getMat().convertTo(m: _D, CV_64FC1);
750	finalParam.Init(f: Vec2d(_K(0,0), _K(1, 1)),
751	c: Vec2d(_K(0,2), _K(1, 2)),
752	k: Vec4d(flags & CALIB_FIX_K1 ? 0 : _D[0],
753	flags & CALIB_FIX_K2 ? 0 : _D[1],
754	flags & CALIB_FIX_K3 ? 0 : _D[2],
755	flags & CALIB_FIX_K4 ? 0 : _D[3]),
756	alpha: _K(0, 1) / _K(0, 0));
757	}
758	else
759	{
760	finalParam.Init(f: Vec2d(max(a: image_size.width, b: image_size.height) / 2., max(a: image_size.width, b: image_size.height) / 2.),
761	c: Vec2d(image_size.width / 2.0 - 0.5, image_size.height / 2.0 - 0.5));
762	}
763
764	errors.isEstimate = finalParam.isEstimate;
765
766	std::vector<Vec3d> omc(objectPoints.total()), Tc(objectPoints.total());
767
768	CalibrateExtrinsics(objectPoints, imagePoints, param: finalParam, check_cond, thresh_cond, omc, Tc);
769
770
771	//-------------------------------Optimization
772	for(int iter = 0; iter < std::numeric_limits<int>::max(); ++iter)
773	{
774	if ((criteria.type == 1 && iter >= criteria.maxCount) ||
775	(criteria.type == 2 && change <= criteria.epsilon) ||
776	(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
777	break;
778
779	double alpha_smooth2 = 1 - std::pow(x: 1 - alpha_smooth, y: iter + 1.0);
780
781	Mat JJ2, ex3;
782	ComputeJacobians(objectPoints, imagePoints, param: finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv&: JJ2, ex3);
783
784	Mat G;
785	solve(src1: JJ2, src2: ex3, dst: G);
786	currentParam = finalParam + alpha_smooth2*G;
787
788	change = norm(M: Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) -
789	Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1]))
790	/ norm(M: Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]));
791
792	finalParam = currentParam;
793
794	if (recompute_extrinsic)
795	{
796	CalibrateExtrinsics(objectPoints, imagePoints, param: finalParam, check_cond,
797	thresh_cond, omc, Tc);
798	}
799	}
800
801	//-------------------------------Validation
802	double rms;
803	EstimateUncertainties(objectPoints, imagePoints, params: finalParam, omc, Tc, errors, std_err&: err_std, thresh_cond,
804	check_cond, rms);
805
806	//-------------------------------
807	_K = Matx33d(finalParam.f[0], finalParam.f[0] * finalParam.alpha, finalParam.c[0],
808	0, finalParam.f[1], finalParam.c[1],
809	0, 0, 1);
810
811	if (K.needed()) cv::Mat(_K).convertTo(m: K, rtype: K.empty() ? CV_64FC1 : K.type());
812	if (D.needed()) cv::Mat(finalParam.k).convertTo(m: D, rtype: D.empty() ? CV_64FC1 : D.type());
813	if (rvecs.isMatVector())
814	{
815	int N = (int)objectPoints.total();
816
817	if(rvecs.empty())
818	rvecs.create(rows: N, cols: 1, CV_64FC3);
819
820	if(tvecs.empty())
821	tvecs.create(rows: N, cols: 1, CV_64FC3);
822
823	for(int i = 0; i < N; i++ )
824	{
825	rvecs.create(rows: 3, cols: 1, CV_64F, i, allowTransposed: true);
826	tvecs.create(rows: 3, cols: 1, CV_64F, i, allowTransposed: true);
827	memcpy(dest: rvecs.getMat(i).ptr(), src: omc[i].val, n: sizeof(Vec3d));
828	memcpy(dest: tvecs.getMat(i).ptr(), src: Tc[i].val, n: sizeof(Vec3d));
829	}
830	}
831	else
832	{
833	if (rvecs.needed()) cv::Mat(omc).convertTo(m: rvecs, rtype: rvecs.empty() ? CV_64FC3 : rvecs.type());
834	if (tvecs.needed()) cv::Mat(Tc).convertTo(m: tvecs, rtype: tvecs.empty() ? CV_64FC3 : tvecs.type());
835	}
836
837	return rms;
838	}
839
840	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
841	/// cv::fisheye::stereoCalibrate
842
843	double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
844	InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
845	OutputArray R, OutputArray T, int flags, TermCriteria criteria)
846	{
847	return cv::fisheye::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, K1, D1, K2, D2, imageSize, R, T, rvecs: noArray(), tvecs: noArray(), flags, criteria);
848	}
849
850	double cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2,
851	InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize,
852	OutputArray R, OutputArray T, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, int flags, TermCriteria criteria)
853	{
854	CV_INSTRUMENT_REGION();
855
856	CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty());
857	CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total());
858	CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3);
859	CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2);
860	CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2);
861
862	CV_Assert(K1.empty() || (K1.size() == Size(3,3)));
863	CV_Assert(D1.empty() || (D1.total() == 4));
864	CV_Assert(K2.empty() || (K2.size() == Size(3,3)));
865	CV_Assert(D2.empty() || (D2.total() == 4));
866
867	CV_Assert((!K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC));
868
869	//-------------------------------Initialization
870
871	const int threshold = 50;
872	const double thresh_cond = 1e6;
873	const int check_cond = 1;
874
875	int n_points = (int)objectPoints.getMat(i: 0).total();
876	int n_images = (int)objectPoints.total();
877
878	double change = 1;
879
880	cv::internal::IntrinsicParams intrinsicLeft;
881	cv::internal::IntrinsicParams intrinsicRight;
882
883	cv::internal::IntrinsicParams intrinsicLeft_errors;
884	cv::internal::IntrinsicParams intrinsicRight_errors;
885
886	Matx33d _K1, _K2;
887	Vec4d _D1, _D2;
888	if (!K1.empty()) K1.getMat().convertTo(m: _K1, CV_64FC1);
889	if (!D1.empty()) D1.getMat().convertTo(m: _D1, CV_64FC1);
890	if (!K2.empty()) K2.getMat().convertTo(m: _K2, CV_64FC1);
891	if (!D2.empty()) D2.getMat().convertTo(m: _D2, CV_64FC1);
892
893	std::vector<Vec3d> rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images);
894
895	if (!(flags & CALIB_FIX_INTRINSIC))
896	{
897	calibrate(objectPoints, imagePoints: imagePoints1, image_size: imageSize, K: _K1, D: _D1, rvecs: rvecs1, tvecs: tvecs1, flags, criteria: TermCriteria(3, 20, 1e-6));
898	calibrate(objectPoints, imagePoints: imagePoints2, image_size: imageSize, K: _K2, D: _D2, rvecs: rvecs2, tvecs: tvecs2, flags, criteria: TermCriteria(3, 20, 1e-6));
899	}
900
901	intrinsicLeft.Init(f: Vec2d(_K1(0,0), _K1(1, 1)), c: Vec2d(_K1(0,2), _K1(1, 2)),
902	k: Vec4d(_D1[0], _D1[1], _D1[2], _D1[3]), alpha: _K1(0, 1) / _K1(0, 0));
903
904	intrinsicRight.Init(f: Vec2d(_K2(0,0), _K2(1, 1)), c: Vec2d(_K2(0,2), _K2(1, 2)),
905	k: Vec4d(_D2[0], _D2[1], _D2[2], _D2[3]), alpha: _K2(0, 1) / _K2(0, 0));
906
907	if ((flags & CALIB_FIX_INTRINSIC))
908	{
909	cv::internal::CalibrateExtrinsics(objectPoints, imagePoints: imagePoints1, param: intrinsicLeft, check_cond, thresh_cond, omc: rvecs1, Tc: tvecs1);
910	cv::internal::CalibrateExtrinsics(objectPoints, imagePoints: imagePoints2, param: intrinsicRight, check_cond, thresh_cond, omc: rvecs2, Tc: tvecs2);
911	}
912
913	intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
914	intrinsicLeft.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
915	intrinsicLeft.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
916	intrinsicLeft.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
917	intrinsicLeft.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1;
918	intrinsicLeft.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1;
919	intrinsicLeft.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1;
920	intrinsicLeft.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1;
921	intrinsicLeft.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1;
922
923	intrinsicRight.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
924	intrinsicRight.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
925	intrinsicRight.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
926	intrinsicRight.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1;
927	intrinsicRight.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1;
928	intrinsicRight.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1;
929	intrinsicRight.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1;
930	intrinsicRight.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1;
931	intrinsicRight.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1;
932
933	intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate;
934	intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate;
935
936	std::vector<uchar> selectedParams;
937	std::vector<uchar> tmp(6 * (n_images + 1), 1);
938	selectedParams.insert(position: selectedParams.end(), first: intrinsicLeft.isEstimate.begin(), last: intrinsicLeft.isEstimate.end());
939	selectedParams.insert(position: selectedParams.end(), first: intrinsicRight.isEstimate.begin(), last: intrinsicRight.isEstimate.end());
940	selectedParams.insert(position: selectedParams.end(), first: tmp.begin(), last: tmp.end());
941
942	//Init values for rotation and translation between two views
943	cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3);
944	cv::Mat om_ref, R_ref, T_ref, R1, R2;
945	for (int image_idx = 0; image_idx < n_images; ++image_idx)
946	{
947	cv::Rodrigues(src: rvecs1[image_idx], dst: R1);
948	cv::Rodrigues(src: rvecs2[image_idx], dst: R2);
949	R_ref = R2 * R1.t();
950	T_ref = cv::Mat(tvecs2[image_idx]) - R_ref * cv::Mat(tvecs1[image_idx]);
951	cv::Rodrigues(src: R_ref, dst: om_ref);
952	om_ref.reshape(cn: 3, rows: 1).copyTo(m: om_list.col(x: image_idx));
953	T_ref.reshape(cn: 3, rows: 1).copyTo(m: T_list.col(x: image_idx));
954	}
955	cv::Vec3d omcur = cv::internal::median3d(m: om_list);
956	cv::Vec3d Tcur = cv::internal::median3d(m: T_list);
957
958	cv::Mat J = cv::Mat::zeros(rows: 4 * n_points * n_images, cols: 18 + 6 * (n_images + 1), CV_64FC1),
959	e = cv::Mat::zeros(rows: 4 * n_points * n_images, cols: 1, CV_64FC1), Jkk, ekk;
960
961	for(int iter = 0; ; ++iter)
962	{
963	if ((criteria.type == 1 && iter >= criteria.maxCount) ||
964	(criteria.type == 2 && change <= criteria.epsilon) ||
965	(criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount)))
966	break;
967
968	J.create(rows: 4 * n_points * n_images, cols: 18 + 6 * (n_images + 1), CV_64FC1);
969	e.create(rows: 4 * n_points * n_images, cols: 1, CV_64FC1);
970	Jkk.create(rows: 4 * n_points, cols: 18 + 6 * (n_images + 1), CV_64FC1);
971	ekk.create(rows: 4 * n_points, cols: 1, CV_64FC1);
972
973	cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT;
974
975	for (int image_idx = 0; image_idx < n_images; ++image_idx)
976	{
977	Jkk = cv::Mat::zeros(rows: 4 * n_points, cols: 18 + 6 * (n_images + 1), CV_64FC1);
978
979	cv::Mat object = objectPoints.getMat(i: image_idx).clone();
980	cv::Mat imageLeft = imagePoints1.getMat(i: image_idx).clone();
981	cv::Mat imageRight = imagePoints2.getMat(i: image_idx).clone();
982	cv::Mat jacobians, projected;
983
984	//left camera jacobian
985	cv::Mat rvec = cv::Mat(rvecs1[image_idx]);
986	cv::Mat tvec = cv::Mat(tvecs1[image_idx]);
987	cv::internal::projectPoints(objectPoints: object, imagePoints: projected, rvec: rvec, tvec: tvec, param: intrinsicLeft, jacobian: jacobians);
988	cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(cn: 1, rows: 1).t()).copyTo(m: ekk.rowRange(startrow: 0, endrow: 2 * n_points));
989	jacobians.colRange(startcol: 8, endcol: 11).copyTo(m: Jkk.colRange(startcol: 24 + image_idx * 6, endcol: 27 + image_idx * 6).rowRange(startrow: 0, endrow: 2 * n_points));
990	jacobians.colRange(startcol: 11, endcol: 14).copyTo(m: Jkk.colRange(startcol: 27 + image_idx * 6, endcol: 30 + image_idx * 6).rowRange(startrow: 0, endrow: 2 * n_points));
991	jacobians.colRange(startcol: 0, endcol: 2).copyTo(m: Jkk.colRange(startcol: 0, endcol: 2).rowRange(startrow: 0, endrow: 2 * n_points));
992	jacobians.colRange(startcol: 2, endcol: 4).copyTo(m: Jkk.colRange(startcol: 2, endcol: 4).rowRange(startrow: 0, endrow: 2 * n_points));
993	jacobians.colRange(startcol: 4, endcol: 8).copyTo(m: Jkk.colRange(startcol: 5, endcol: 9).rowRange(startrow: 0, endrow: 2 * n_points));
994	jacobians.col(x: 14).copyTo(m: Jkk.col(x: 4).rowRange(startrow: 0, endrow: 2 * n_points));
995
996	//right camera jacobian
997	cv::internal::compose_motion(om1: rvec, T1: tvec, om2: omcur, T2: Tcur, om3&: omr, T3&: Tr, dom3dom1&: domrdomckk, dom3dT1&: domrdTckk, dom3dom2&: domrdom, dom3dT2&: domrdT, dT3dom1&: dTrdomckk, dT3dT1&: dTrdTckk, dT3dom2&: dTrdom, dT3dT2&: dTrdT);
998	rvec = cv::Mat(rvecs2[image_idx]);
999	tvec = cv::Mat(tvecs2[image_idx]);
1000
1001	cv::internal::projectPoints(objectPoints: object, imagePoints: projected, rvec: omr, tvec: Tr, param: intrinsicRight, jacobian: jacobians);
1002	cv::Mat(cv::Mat((imageRight - projected).t()).reshape(cn: 1, rows: 1).t()).copyTo(m: ekk.rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1003	cv::Mat dxrdom = jacobians.colRange(startcol: 8, endcol: 11) * domrdom + jacobians.colRange(startcol: 11, endcol: 14) * dTrdom;
1004	cv::Mat dxrdT = jacobians.colRange(startcol: 8, endcol: 11) * domrdT + jacobians.colRange(startcol: 11, endcol: 14)* dTrdT;
1005	cv::Mat dxrdomckk = jacobians.colRange(startcol: 8, endcol: 11) * domrdomckk + jacobians.colRange(startcol: 11, endcol: 14) * dTrdomckk;
1006	cv::Mat dxrdTckk = jacobians.colRange(startcol: 8, endcol: 11) * domrdTckk + jacobians.colRange(startcol: 11, endcol: 14) * dTrdTckk;
1007
1008	dxrdom.copyTo(m: Jkk.colRange(startcol: 18, endcol: 21).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1009	dxrdT.copyTo(m: Jkk.colRange(startcol: 21, endcol: 24).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1010	dxrdomckk.copyTo(m: Jkk.colRange(startcol: 24 + image_idx * 6, endcol: 27 + image_idx * 6).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1011	dxrdTckk.copyTo(m: Jkk.colRange(startcol: 27 + image_idx * 6, endcol: 30 + image_idx * 6).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1012	jacobians.colRange(startcol: 0, endcol: 2).copyTo(m: Jkk.colRange(startcol: 9 + 0, endcol: 9 + 2).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1013	jacobians.colRange(startcol: 2, endcol: 4).copyTo(m: Jkk.colRange(startcol: 9 + 2, endcol: 9 + 4).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1014	jacobians.colRange(startcol: 4, endcol: 8).copyTo(m: Jkk.colRange(startcol: 9 + 5, endcol: 9 + 9).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1015	jacobians.col(x: 14).copyTo(m: Jkk.col(x: 9 + 4).rowRange(startrow: 2 * n_points, endrow: 4 * n_points));
1016
1017	//check goodness of sterepair
1018	double abs_max = 0;
1019	for (int i = 0; i < 4 * n_points; i++)
1020	{
1021	if (fabs(x: ekk.at<double>(i0: i)) > abs_max)
1022	{
1023	abs_max = fabs(x: ekk.at<double>(i0: i));
1024	}
1025	}
1026
1027	CV_Assert(abs_max < threshold); // bad stereo pair
1028
1029	Jkk.copyTo(m: J.rowRange(startrow: image_idx * 4 * n_points, endrow: (image_idx + 1) * 4 * n_points));
1030	ekk.copyTo(m: e.rowRange(startrow: image_idx * 4 * n_points, endrow: (image_idx + 1) * 4 * n_points));
1031	}
1032
1033	cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
1034
1035	//update all parameters
1036	cv::subMatrix(src: J, dst&: J, cols: selectedParams, rows: std::vector<uchar>(J.rows, 1));
1037	int a = cv::countNonZero(src: intrinsicLeft.isEstimate);
1038	int b = cv::countNonZero(src: intrinsicRight.isEstimate);
1039	cv::Mat deltas;
1040	solve(src1: J.t() * J, src2: J.t()*e, dst: deltas);
1041	if (a > 0)
1042	intrinsicLeft = intrinsicLeft + deltas.rowRange(startrow: 0, endrow: a);
1043	if (b > 0)
1044	intrinsicRight = intrinsicRight + deltas.rowRange(startrow: a, endrow: a + b);
1045	omcur = omcur + cv::Vec3d(deltas.rowRange(startrow: a + b, endrow: a + b + 3));
1046	Tcur = Tcur + cv::Vec3d(deltas.rowRange(startrow: a + b + 3, endrow: a + b + 6));
1047	for (int image_idx = 0; image_idx < n_images; ++image_idx)
1048	{
1049	rvecs1[image_idx] = cv::Mat(cv::Mat(rvecs1[image_idx]) + deltas.rowRange(startrow: a + b + 6 + image_idx * 6, endrow: a + b + 9 + image_idx * 6));
1050	tvecs1[image_idx] = cv::Mat(cv::Mat(tvecs1[image_idx]) + deltas.rowRange(startrow: a + b + 9 + image_idx * 6, endrow: a + b + 12 + image_idx * 6));
1051	}
1052
1053	cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]);
1054	change = cv::norm(M: newTom - oldTom) / cv::norm(M: newTom);
1055	}
1056
1057	double rms = 0;
1058	const Vec2d* ptr_e = e.ptr<Vec2d>();
1059	for (size_t i = 0; i < e.total() / 2; i++)
1060	{
1061	rms += ptr_e[i][0] * ptr_e[i][0] + ptr_e[i][1] * ptr_e[i][1];
1062	}
1063
1064	rms /= ((double)e.total() / 2.0);
1065	rms = sqrt(x: rms);
1066
1067	_K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0],
1068	0, intrinsicLeft.f[1], intrinsicLeft.c[1],
1069	0, 0, 1);
1070
1071	_K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0],
1072	0, intrinsicRight.f[1], intrinsicRight.c[1],
1073	0, 0, 1);
1074
1075	Mat _R;
1076	Rodrigues(src: omcur, dst: _R);
1077
1078	if (K1.needed()) cv::Mat(_K1).convertTo(m: K1, rtype: K1.empty() ? CV_64FC1 : K1.type());
1079	if (K2.needed()) cv::Mat(_K2).convertTo(m: K2, rtype: K2.empty() ? CV_64FC1 : K2.type());
1080	if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(m: D1, rtype: D1.empty() ? CV_64FC1 : D1.type());
1081	if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(m: D2, rtype: D2.empty() ? CV_64FC1 : D2.type());
1082	if (R.needed()) _R.convertTo(m: R, rtype: R.empty() ? CV_64FC1 : R.type());
1083	if (T.needed()) cv::Mat(Tcur).convertTo(m: T, rtype: T.empty() ? CV_64FC1 : T.type());
1084	if (rvecs.isMatVector())
1085	{
1086	if(rvecs.empty())
1087	rvecs.create(rows: n_images, cols: 1, CV_64FC3);
1088
1089	if(tvecs.empty())
1090	tvecs.create(rows: n_images, cols: 1, CV_64FC3);
1091
1092	for(int i = 0; i < n_images; i++ )
1093	{
1094	rvecs.create(rows: 3, cols: 1, CV_64F, i, allowTransposed: true);
1095	tvecs.create(rows: 3, cols: 1, CV_64F, i, allowTransposed: true);
1096	memcpy(dest: rvecs.getMat(i).ptr(), src: rvecs1[i].val, n: sizeof(Vec3d));
1097	memcpy(dest: tvecs.getMat(i).ptr(), src: tvecs1[i].val, n: sizeof(Vec3d));
1098	}
1099	}
1100	else
1101	{
1102	if (rvecs.needed()) cv::Mat(rvecs1).convertTo(m: rvecs, rtype: rvecs.empty() ? CV_64FC3 : rvecs.type());
1103	if (tvecs.needed()) cv::Mat(tvecs1).convertTo(m: tvecs, rtype: tvecs.empty() ? CV_64FC3 : tvecs.type());
1104	}
1105
1106	return rms;
1107	}
1108
1109	//////////////////////////////////////////////////////////////////////////////////////////////////////////////
1110	/// cv::fisheye::solvePnP
1111
1112	bool cv::fisheye::solvePnP( InputArray opoints, InputArray ipoints,
1113	InputArray cameraMatrix, InputArray distCoeffs,
1114	OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess,
1115	int flags, TermCriteria criteria)
1116	{
1117
1118	Mat imagePointsNormalized;
1119	cv::fisheye::undistortPoints(distorted: ipoints, undistorted: imagePointsNormalized, K: cameraMatrix, D: distCoeffs, R: noArray(), P: cameraMatrix, criteria);
1120	return cv::solvePnP(objectPoints: opoints, imagePoints: imagePointsNormalized, cameraMatrix, distCoeffs: noArray(), rvec, tvec, useExtrinsicGuess, flags);
1121	}
1122
1123	namespace cv{ namespace {
1124	void subMatrix(const Mat& src, Mat& dst, const std::vector<uchar>& cols, const std::vector<uchar>& rows)
1125	{
1126	CV_Assert(src.channels() == 1);
1127
1128	int nonzeros_cols = cv::countNonZero(src: cols);
1129	Mat tmp(src.rows, nonzeros_cols, CV_64F);
1130
1131	for (int i = 0, j = 0; i < (int)cols.size(); i++)
1132	{
1133	if (cols[i])
1134	{
1135	src.col(x: i).copyTo(m: tmp.col(x: j++));
1136	}
1137	}
1138
1139	int nonzeros_rows = cv::countNonZero(src: rows);
1140	dst.create(rows: nonzeros_rows, cols: nonzeros_cols, CV_64F);
1141	for (int i = 0, j = 0; i < (int)rows.size(); i++)
1142	{
1143	if (rows[i])
1144	{
1145	tmp.row(y: i).copyTo(m: dst.row(y: j++));
1146	}
1147	}
1148	}
1149
1150	}}
1151
1152	cv::internal::IntrinsicParams::IntrinsicParams():
1153	f(Vec2d::all(alpha: 0)), c(Vec2d::all(alpha: 0)), k(Vec4d::all(alpha: 0)), alpha(0), isEstimate(9,0)
1154	{
1155	}
1156
1157	cv::internal::IntrinsicParams::IntrinsicParams(Vec2d _f, Vec2d _c, Vec4d _k, double _alpha):
1158	f(_f), c(_c), k(_k), alpha(_alpha), isEstimate(9,0)
1159	{
1160	}
1161
1162	cv::internal::IntrinsicParams cv::internal::IntrinsicParams::operator+(const Mat& a)
1163	{
1164	CV_Assert(a.type() == CV_64FC1);
1165	IntrinsicParams tmp;
1166	const double* ptr = a.ptr<double>();
1167
1168	int j = 0;
1169	tmp.f[0] = this->f[0] + (isEstimate[0] ? ptr[j++] : 0);
1170	tmp.f[1] = this->f[1] + (isEstimate[1] ? ptr[j++] : 0);
1171	tmp.c[0] = this->c[0] + (isEstimate[2] ? ptr[j++] : 0);
1172	tmp.c[1] = this->c[1] + (isEstimate[3] ? ptr[j++] : 0);
1173	tmp.alpha = this->alpha + (isEstimate[4] ? ptr[j++] : 0);
1174	tmp.k[0] = this->k[0] + (isEstimate[5] ? ptr[j++] : 0);
1175	tmp.k[1] = this->k[1] + (isEstimate[6] ? ptr[j++] : 0);
1176	tmp.k[2] = this->k[2] + (isEstimate[7] ? ptr[j++] : 0);
1177	tmp.k[3] = this->k[3] + (isEstimate[8] ? ptr[j++] : 0);
1178
1179	tmp.isEstimate = isEstimate;
1180	return tmp;
1181	}
1182
1183	cv::internal::IntrinsicParams& cv::internal::IntrinsicParams::operator =(const Mat& a)
1184	{
1185	CV_Assert(a.type() == CV_64FC1);
1186	const double* ptr = a.ptr<double>();
1187
1188	int j = 0;
1189
1190	this->f[0] = isEstimate[0] ? ptr[j++] : 0;
1191	this->f[1] = isEstimate[1] ? ptr[j++] : 0;
1192	this->c[0] = isEstimate[2] ? ptr[j++] : 0;
1193	this->c[1] = isEstimate[3] ? ptr[j++] : 0;
1194	this->alpha = isEstimate[4] ? ptr[j++] : 0;
1195	this->k[0] = isEstimate[5] ? ptr[j++] : 0;
1196	this->k[1] = isEstimate[6] ? ptr[j++] : 0;
1197	this->k[2] = isEstimate[7] ? ptr[j++] : 0;
1198	this->k[3] = isEstimate[8] ? ptr[j++] : 0;
1199
1200	return *this;
1201	}
1202
1203	void cv::internal::IntrinsicParams::Init(const cv::Vec2d& _f, const cv::Vec2d& _c, const cv::Vec4d& _k, const double& _alpha)
1204	{
1205	this->c = _c;
1206	this->f = _f;
1207	this->k = _k;
1208	this->alpha = _alpha;
1209	}
1210
1211	void cv::internal::projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints,
1212	cv::InputArray _rvec,cv::InputArray _tvec,
1213	const IntrinsicParams& param, cv::OutputArray jacobian)
1214	{
1215	CV_INSTRUMENT_REGION();
1216
1217	CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
1218	Matx33d K(param.f[0], param.f[0] * param.alpha, param.c[0],
1219	0, param.f[1], param.c[1],
1220	0, 0, 1);
1221	fisheye::projectPoints(objectPoints, imagePoints, _rvec, _tvec, K: K, D: param.k, alpha: param.alpha, jacobian);
1222	}
1223
1224	void cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec,
1225	Mat& tvec, Mat& J, const int MaxIter,
1226	const IntrinsicParams& param, const double thresh_cond)
1227	{
1228	CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3);
1229	CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
1230	CV_Assert(rvec.total() > 2 && tvec.total() > 2);
1231	Vec6d extrinsics(rvec.at<double>(i0: 0), rvec.at<double>(i0: 1), rvec.at<double>(i0: 2),
1232	tvec.at<double>(i0: 0), tvec.at<double>(i0: 1), tvec.at<double>(i0: 2));
1233	double change = 1;
1234	int iter = 0;
1235
1236	while (change > 1e-10 && iter < MaxIter)
1237	{
1238	std::vector<Point2d> x;
1239	Mat jacobians;
1240	projectPoints(objectPoints, imagePoints: x, rvec: rvec, tvec: tvec, param, jacobian: jacobians);
1241
1242	Mat ex = imagePoints - Mat(x).t();
1243	ex = ex.reshape(cn: 1, rows: 2);
1244
1245	J = jacobians.colRange(startcol: 8, endcol: 14).clone();
1246
1247	SVD svd(J, SVD::NO_UV);
1248	double condJJ = svd.w.at<double>(i0: 0)/svd.w.at<double>(i0: 5);
1249
1250	if (condJJ > thresh_cond)
1251	change = 0;
1252	else
1253	{
1254	Vec6d param_innov;
1255	solve(src1: J, src2: ex.reshape(cn: 1, rows: (int)ex.total()), dst: param_innov, flags: DECOMP_SVD + DECOMP_NORMAL);
1256
1257	Vec6d param_up = extrinsics + param_innov;
1258	change = norm(M: param_innov)/norm(M: param_up);
1259	extrinsics = param_up;
1260	iter = iter + 1;
1261
1262	rvec = Mat(Vec3d(extrinsics.val));
1263	tvec = Mat(Vec3d(extrinsics.val+3));
1264	}
1265	}
1266	}
1267
1268	cv::Mat cv::internal::ComputeHomography(Mat m, Mat M)
1269	{
1270	CV_INSTRUMENT_REGION();
1271
1272	int Np = m.cols;
1273
1274	if (m.rows < 3)
1275	{
1276	vconcat(src1: m, src2: Mat::ones(rows: 1, cols: Np, CV_64FC1), dst: m);
1277	}
1278	if (M.rows < 3)
1279	{
1280	vconcat(src1: M, src2: Mat::ones(rows: 1, cols: Np, CV_64FC1), dst: M);
1281	}
1282
1283	divide(src1: m, src2: Mat::ones(rows: 3, cols: 1, CV_64FC1) * m.row(y: 2), dst: m);
1284	divide(src1: M, src2: Mat::ones(rows: 3, cols: 1, CV_64FC1) * M.row(y: 2), dst: M);
1285
1286	Mat ax = m.row(y: 0).clone();
1287	Mat ay = m.row(y: 1).clone();
1288
1289	double mxx = mean(src: ax)[0];
1290	double myy = mean(src: ay)[0];
1291
1292	ax = ax - mxx;
1293	ay = ay - myy;
1294
1295	double scxx = mean(src: abs(m: ax))[0];
1296	double scyy = mean(src: abs(m: ay))[0];
1297
1298	Mat Hnorm (Matx33d( 1/scxx, 0.0, -mxx/scxx,
1299	0.0, 1/scyy, -myy/scyy,
1300	0.0, 0.0, 1.0 ));
1301
1302	Mat inv_Hnorm (Matx33d( scxx, 0, mxx,
1303	0, scyy, myy,
1304	0, 0, 1 ));
1305	Mat mn = Hnorm * m;
1306
1307	Mat L = Mat::zeros(rows: 2*Np, cols: 9, CV_64FC1);
1308
1309	for (int i = 0; i < Np; ++i)
1310	{
1311	for (int j = 0; j < 3; j++)
1312	{
1313	L.at<double>(i0: 2 * i, i1: j) = M.at<double>(i0: j, i1: i);
1314	L.at<double>(i0: 2 * i + 1, i1: j + 3) = M.at<double>(i0: j, i1: i);
1315	L.at<double>(i0: 2 * i, i1: j + 6) = -mn.at<double>(i0: 0,i1: i) * M.at<double>(i0: j, i1: i);
1316	L.at<double>(i0: 2 * i + 1, i1: j + 6) = -mn.at<double>(i0: 1,i1: i) * M.at<double>(i0: j, i1: i);
1317	}
1318	}
1319
1320	if (Np > 4) L = L.t() * L;
1321	SVD svd(L);
1322	Mat hh = svd.vt.row(y: 8) / svd.vt.row(y: 8).at<double>(i0: 8);
1323	Mat Hrem = hh.reshape(cn: 1, rows: 3);
1324	Mat H = inv_Hnorm * Hrem;
1325
1326	if (Np > 4)
1327	{
1328	Mat hhv = H.reshape(cn: 1, rows: 9)(Rect(0, 0, 1, 8)).clone();
1329	for (int iter = 0; iter < 10; iter++)
1330	{
1331	Mat mrep = H * M;
1332	Mat J = Mat::zeros(rows: 2 * Np, cols: 8, CV_64FC1);
1333	Mat MMM;
1334	divide(src1: M, src2: Mat::ones(rows: 3, cols: 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), dst: MMM);
1335	divide(src1: mrep, src2: Mat::ones(rows: 3, cols: 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), dst: mrep);
1336	Mat m_err = m(Rect(0,0, m.cols, 2)) - mrep(Rect(0,0, mrep.cols, 2));
1337	m_err = Mat(m_err.t()).reshape(cn: 1, rows: m_err.cols * m_err.rows);
1338	Mat MMM2, MMM3;
1339	multiply(src1: Mat::ones(rows: 3, cols: 1, CV_64FC1) * mrep(Rect(0, 0, mrep.cols, 1)), src2: MMM, dst: MMM2);
1340	multiply(src1: Mat::ones(rows: 3, cols: 1, CV_64FC1) * mrep(Rect(0, 1, mrep.cols, 1)), src2: MMM, dst: MMM3);
1341
1342	for (int i = 0; i < Np; ++i)
1343	{
1344	for (int j = 0; j < 3; ++j)
1345	{
1346	J.at<double>(i0: 2 * i, i1: j) = -MMM.at<double>(i0: j, i1: i);
1347	J.at<double>(i0: 2 * i + 1, i1: j + 3) = -MMM.at<double>(i0: j, i1: i);
1348	}
1349
1350	for (int j = 0; j < 2; ++j)
1351	{
1352	J.at<double>(i0: 2 * i, i1: j + 6) = MMM2.at<double>(i0: j, i1: i);
1353	J.at<double>(i0: 2 * i + 1, i1: j + 6) = MMM3.at<double>(i0: j, i1: i);
1354	}
1355	}
1356	divide(src1: M, src2: Mat::ones(rows: 3, cols: 1, CV_64FC1) * mrep(Rect(0,2,mrep.cols,1)), dst: MMM);
1357	Mat hh_innov = (J.t() * J).inv() * (J.t()) * m_err;
1358	Mat hhv_up = hhv - hh_innov;
1359	Mat tmp;
1360	vconcat(src1: hhv_up, src2: Mat::ones(rows: 1,cols: 1,CV_64FC1), dst: tmp);
1361	Mat H_up = tmp.reshape(cn: 1,rows: 3);
1362	hhv = hhv_up;
1363	H = H_up;
1364	}
1365	}
1366	return H;
1367	}
1368
1369	cv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param)
1370	{
1371	CV_INSTRUMENT_REGION();
1372
1373	CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2);
1374
1375	Mat distorted((int)imagePoints.total(), 1, CV_64FC2), undistorted;
1376	const Vec2d* ptr = imagePoints.ptr<Vec2d>();
1377	Vec2d* ptr_d = distorted.ptr<Vec2d>();
1378	for (size_t i = 0; i < imagePoints.total(); ++i)
1379	{
1380	ptr_d[i] = (ptr[i] - param.c).mul(v: Vec2d(1.0 / param.f[0], 1.0 / param.f[1]));
1381	ptr_d[i][0] -= param.alpha * ptr_d[i][1];
1382	}
1383	cv::fisheye::undistortPoints(distorted, undistorted, K: Matx33d::eye(), D: param.k);
1384	return undistorted;
1385	}
1386
1387	void cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk)
1388	{
1389	CV_Assert(!_objectPoints.empty() && _objectPoints.type() == CV_64FC3);
1390	CV_Assert(!_imagePoints.empty() && _imagePoints.type() == CV_64FC2);
1391
1392	Mat imagePointsNormalized = NormalizePixels(imagePoints: _imagePoints, param).reshape(cn: 1).t();
1393	Mat objectPoints = _objectPoints.reshape(cn: 1).t();
1394	Mat objectPointsMean, covObjectPoints;
1395	Mat Rckk;
1396	int Np = imagePointsNormalized.cols;
1397	calcCovarMatrix(samples: objectPoints, covar: covObjectPoints, mean: objectPointsMean, flags: COVAR_NORMAL | COVAR_COLS);
1398	SVD svd(covObjectPoints);
1399	Mat R(svd.vt);
1400	if (norm(src1: R(Rect(2, 0, 1, 2))) < 1e-6)
1401	R = Mat::eye(rows: 3,cols: 3, CV_64FC1);
1402	if (determinant(mtx: R) < 0)
1403	R = -R;
1404	Mat T = -R * objectPointsMean;
1405	Mat X_new = R * objectPoints + T * Mat::ones(rows: 1, cols: Np, CV_64FC1);
1406	Mat H = ComputeHomography(m: imagePointsNormalized, M: X_new(Rect(0,0,X_new.cols,2)));
1407	double sc = .5 * (norm(src1: H.col(x: 0)) + norm(src1: H.col(x: 1)));
1408	H = H / sc;
1409	Mat u1 = H.col(x: 0).clone();
1410	double norm_u1 = norm(src1: u1);
1411	CV_Assert(fabs(norm_u1) > 0);
1412	u1 = u1 / norm_u1;
1413	Mat u2 = H.col(x: 1).clone() - u1.dot(m: H.col(x: 1).clone()) * u1;
1414	double norm_u2 = norm(src1: u2);
1415	CV_Assert(fabs(norm_u2) > 0);
1416	u2 = u2 / norm_u2;
1417	Mat u3 = u1.cross(m: u2);
1418	Mat RRR;
1419	hconcat(src1: u1, src2: u2, dst: RRR);
1420	hconcat(src1: RRR, src2: u3, dst: RRR);
1421	Rodrigues(src: RRR, dst: omckk);
1422	Rodrigues(src: omckk, dst: Rckk);
1423	Tckk = H.col(x: 2).clone();
1424	Tckk = Tckk + Rckk * T;
1425	Rckk = Rckk * R;
1426	Rodrigues(src: Rckk, dst: omckk);
1427	}
1428
1429	void cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
1430	const IntrinsicParams& param, const int check_cond,
1431	const double thresh_cond, InputOutputArray omc, InputOutputArray Tc)
1432	{
1433	CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
1434	CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
1435	CV_Assert(omc.type() == CV_64FC3 || Tc.type() == CV_64FC3);
1436
1437	if (omc.empty()) omc.create(rows: 1, cols: (int)objectPoints.total(), CV_64FC3);
1438	if (Tc.empty()) Tc.create(rows: 1, cols: (int)objectPoints.total(), CV_64FC3);
1439
1440	const int maxIter = 20;
1441
1442	for(int image_idx = 0; image_idx < (int)imagePoints.total(); ++image_idx)
1443	{
1444	Mat omckk, Tckk, JJ_kk;
1445	Mat image, object;
1446
1447	objectPoints.getMat(i: image_idx).convertTo(m: object, CV_64FC3);
1448	imagePoints.getMat (i: image_idx).convertTo(m: image, CV_64FC2);
1449
1450	bool imT = image.rows < image.cols;
1451	bool obT = object.rows < object.cols;
1452
1453	InitExtrinsics(imagePoints: imT ? image.t() : image, objectPoints: obT ? object.t() : object, param, omckk, Tckk);
1454
1455	ComputeExtrinsicRefine(imagePoints: !imT ? image.t() : image, objectPoints: !obT ? object.t() : object, rvec&: omckk, tvec&: Tckk, J&: JJ_kk, MaxIter: maxIter, param, thresh_cond);
1456	if (check_cond)
1457	{
1458	SVD svd(JJ_kk, SVD::NO_UV);
1459	if(svd.w.at<double>(i0: 0) / svd.w.at<double>(i0: (int)svd.w.total() - 1) > thresh_cond )
1460	CV_Error( cv::Error::StsInternal, format("CALIB_CHECK_COND - Ill-conditioned matrix for input array %d",image_idx));
1461	}
1462	omckk.reshape(cn: 3,rows: 1).copyTo(m: omc.getMat().col(x: image_idx));
1463	Tckk.reshape(cn: 3,rows: 1).copyTo(m: Tc.getMat().col(x: image_idx));
1464	}
1465	}
1466
1467	void cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
1468	const IntrinsicParams& param, InputArray omc, InputArray Tc,
1469	const int& check_cond, const double& thresh_cond, Mat& JJ2, Mat& ex3)
1470	{
1471	CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
1472	CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
1473
1474	CV_Assert(!omc.empty() && omc.type() == CV_64FC3);
1475	CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3);
1476
1477	int n = (int)objectPoints.total();
1478
1479	JJ2 = Mat::zeros(rows: 9 + 6 * n, cols: 9 + 6 * n, CV_64FC1);
1480	ex3 = Mat::zeros(rows: 9 + 6 * n, cols: 1, CV_64FC1 );
1481
1482	for (int image_idx = 0; image_idx < n; ++image_idx)
1483	{
1484	Mat image, object;
1485	objectPoints.getMat(i: image_idx).convertTo(m: object, CV_64FC3);
1486	imagePoints.getMat (i: image_idx).convertTo(m: image, CV_64FC2);
1487
1488	bool imT = image.rows < image.cols;
1489	Mat om(omc.getMat().col(x: image_idx)), T(Tc.getMat().col(x: image_idx));
1490
1491	std::vector<Point2d> x;
1492	Mat jacobians;
1493	projectPoints(objectPoints: object, imagePoints: x, rvec: om, tvec: T, param, jacobian: jacobians);
1494	Mat exkk = (imT ? image.t() : image) - Mat(x);
1495
1496	Mat A(jacobians.rows, 9, CV_64FC1);
1497	jacobians.colRange(startcol: 0, endcol: 4).copyTo(m: A.colRange(startcol: 0, endcol: 4));
1498	jacobians.col(x: 14).copyTo(m: A.col(x: 4));
1499	jacobians.colRange(startcol: 4, endcol: 8).copyTo(m: A.colRange(startcol: 5, endcol: 9));
1500
1501	A = A.t();
1502
1503	Mat B = jacobians.colRange(startcol: 8, endcol: 14).clone();
1504	B = B.t();
1505
1506	JJ2(Rect(0, 0, 9, 9)) += A * A.t();
1507	JJ2(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t();
1508
1509	JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)) = A * B.t();
1510	JJ2(Rect(0, 9 + 6 * image_idx, 9, 6)) = JJ2(Rect(9 + 6 * image_idx, 0, 6, 9)).t();
1511
1512	ex3.rowRange(startrow: 0, endrow: 9) += A * exkk.reshape(cn: 1, rows: 2 * exkk.rows);
1513	ex3.rowRange(startrow: 9 + 6 * image_idx, endrow: 9 + 6 * (image_idx + 1)) = B * exkk.reshape(cn: 1, rows: 2 * exkk.rows);
1514
1515	if (check_cond)
1516	{
1517	Mat JJ_kk = B.t();
1518	SVD svd(JJ_kk, SVD::NO_UV);
1519	CV_Assert(svd.w.at<double>(0) / svd.w.at<double>(svd.w.rows - 1) < thresh_cond);
1520	}
1521	}
1522
1523	std::vector<uchar> idxs(param.isEstimate);
1524	idxs.insert(position: idxs.end(), n: 6 * n, x: 1);
1525
1526	subMatrix(src: JJ2, dst&: JJ2, cols: idxs, rows: idxs);
1527	subMatrix(src: ex3, dst&: ex3, cols: std::vector<uchar>(1, 1), rows: idxs);
1528	}
1529
1530	void cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints,
1531	const IntrinsicParams& params, InputArray omc, InputArray Tc,
1532	IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms)
1533	{
1534	CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3));
1535	CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2));
1536
1537	CV_Assert(!omc.empty() && omc.type() == CV_64FC3);
1538	CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3);
1539
1540	int total_ex = 0;
1541	for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx)
1542	{
1543	total_ex += (int)objectPoints.getMat(i: image_idx).total();
1544	}
1545	Mat ex(total_ex, 1, CV_64FC2);
1546	int insert_idx = 0;
1547	for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx)
1548	{
1549	Mat image, object;
1550	objectPoints.getMat(i: image_idx).convertTo(m: object, CV_64FC3);
1551	imagePoints.getMat (i: image_idx).convertTo(m: image, CV_64FC2);
1552
1553	bool imT = image.rows < image.cols;
1554
1555	Mat om(omc.getMat().col(x: image_idx)), T(Tc.getMat().col(x: image_idx));
1556
1557	std::vector<Point2d> x;
1558	projectPoints(objectPoints: object, imagePoints: x, rvec: om, tvec: T, param: params, jacobian: noArray());
1559	Mat ex_ = (imT ? image.t() : image) - Mat(x);
1560	ex_.copyTo(m: ex.rowRange(startrow: insert_idx, endrow: insert_idx + ex_.rows));
1561	insert_idx += ex_.rows;
1562	}
1563
1564	meanStdDev(src: ex, mean: noArray(), stddev: std_err);
1565	std_err *= sqrt(x: (double)ex.total()/((double)ex.total() - 1.0));
1566
1567	Vec<double, 1> sigma_x;
1568	meanStdDev(src: ex.reshape(cn: 1, rows: 1), mean: noArray(), stddev: sigma_x);
1569
1570	Mat JJ2, ex3;
1571	ComputeJacobians(objectPoints, imagePoints, param: params, omc, Tc, check_cond, thresh_cond, JJ2, ex3);
1572
1573	sqrt(src: JJ2.inv(), dst: JJ2);
1574
1575	int nParams = JJ2.rows;
1576	// an explanation of that denominator correction can be found here:
1577	// R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, 2004, section 5.1.3, page 134
1578	// see the discussion for more details: https://github.com/opencv/opencv/pull/22992
1579	sigma_x *= sqrt(x: 2.0 * (double)ex.total()/(2.0 * (double)ex.total() - nParams));
1580
1581	errors = 3 * sigma_x(0) * JJ2.diag();
1582	rms = sqrt(x: norm(src1: ex, normType: NORM_L2SQR)/ex.total());
1583	}
1584
1585	void cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB)
1586	{
1587	CV_Assert(A.getMat().cols == B.getMat().rows);
1588	CV_Assert(A.type() == CV_64FC1 && B.type() == CV_64FC1);
1589
1590	int p = A.getMat().rows;
1591	int n = A.getMat().cols;
1592	int q = B.getMat().cols;
1593
1594	dABdA.create(rows: p * q, cols: p * n, CV_64FC1);
1595	dABdB.create(rows: p * q, cols: q * n, CV_64FC1);
1596
1597	dABdA.getMat() = Mat::zeros(rows: p * q, cols: p * n, CV_64FC1);
1598	dABdB.getMat() = Mat::zeros(rows: p * q, cols: q * n, CV_64FC1);
1599
1600	for (int i = 0; i < q; ++i)
1601	{
1602	for (int j = 0; j < p; ++j)
1603	{
1604	int ij = j + i * p;
1605	for (int k = 0; k < n; ++k)
1606	{
1607	int kj = j + k * p;
1608	dABdA.getMat().at<double>(i0: ij, i1: kj) = B.getMat().at<double>(i0: k, i1: i);
1609	}
1610	}
1611	}
1612
1613	for (int i = 0; i < q; ++i)
1614	{
1615	A.getMat().copyTo(m: dABdB.getMat().rowRange(startrow: i * p, endrow: i * p + p).colRange(startcol: i * n, endcol: i * n + n));
1616	}
1617	}
1618
1619	void cv::internal::JRodriguesMatlab(const Mat& src, Mat& dst)
1620	{
1621	Mat tmp(src.cols, src.rows, src.type());
1622	if (src.rows == 9)
1623	{
1624	Mat(src.row(y: 0).t()).copyTo(m: tmp.col(x: 0));
1625	Mat(src.row(y: 1).t()).copyTo(m: tmp.col(x: 3));
1626	Mat(src.row(y: 2).t()).copyTo(m: tmp.col(x: 6));
1627	Mat(src.row(y: 3).t()).copyTo(m: tmp.col(x: 1));
1628	Mat(src.row(y: 4).t()).copyTo(m: tmp.col(x: 4));
1629	Mat(src.row(y: 5).t()).copyTo(m: tmp.col(x: 7));
1630	Mat(src.row(y: 6).t()).copyTo(m: tmp.col(x: 2));
1631	Mat(src.row(y: 7).t()).copyTo(m: tmp.col(x: 5));
1632	Mat(src.row(y: 8).t()).copyTo(m: tmp.col(x: 8));
1633	}
1634	else
1635	{
1636	Mat(src.col(x: 0).t()).copyTo(m: tmp.row(y: 0));
1637	Mat(src.col(x: 1).t()).copyTo(m: tmp.row(y: 3));
1638	Mat(src.col(x: 2).t()).copyTo(m: tmp.row(y: 6));
1639	Mat(src.col(x: 3).t()).copyTo(m: tmp.row(y: 1));
1640	Mat(src.col(x: 4).t()).copyTo(m: tmp.row(y: 4));
1641	Mat(src.col(x: 5).t()).copyTo(m: tmp.row(y: 7));
1642	Mat(src.col(x: 6).t()).copyTo(m: tmp.row(y: 2));
1643	Mat(src.col(x: 7).t()).copyTo(m: tmp.row(y: 5));
1644	Mat(src.col(x: 8).t()).copyTo(m: tmp.row(y: 8));
1645	}
1646	dst = tmp.clone();
1647	}
1648
1649	void cv::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2,
1650	Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2,
1651	Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2)
1652	{
1653	Mat om1 = _om1.getMat();
1654	Mat om2 = _om2.getMat();
1655	Mat T1 = _T1.getMat().reshape(cn: 1, rows: 3);
1656	Mat T2 = _T2.getMat().reshape(cn: 1, rows: 3);
1657
1658	//% Rotations:
1659	Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2;
1660	Rodrigues(src: om1, dst: R1, jacobian: dR1dom1);
1661	Rodrigues(src: om2, dst: R2, jacobian: dR2dom2);
1662	JRodriguesMatlab(src: dR1dom1, dst&: dR1dom1);
1663	JRodriguesMatlab(src: dR2dom2, dst&: dR2dom2);
1664	R3 = R2 * R1;
1665	Mat dR3dR2, dR3dR1;
1666	dAB(A: R2, B: R1, dABdA: dR3dR2, dABdB: dR3dR1);
1667	Mat dom3dR3;
1668	Rodrigues(src: R3, dst: om3, jacobian: dom3dR3);
1669	JRodriguesMatlab(src: dom3dR3, dst&: dom3dR3);
1670	dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
1671	dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;
1672	dom3dT1 = Mat::zeros(rows: 3, cols: 3, CV_64FC1);
1673	dom3dT2 = Mat::zeros(rows: 3, cols: 3, CV_64FC1);
1674
1675	//% Translations:
1676	Mat T3t = R2 * T1;
1677	Mat dT3tdR2, dT3tdT1;
1678	dAB(A: R2, B: T1, dABdA: dT3tdR2, dABdB: dT3tdT1);
1679	Mat dT3tdom2 = dT3tdR2 * dR2dom2;
1680	T3 = T3t + T2;
1681	dT3dT1 = dT3tdT1;
1682	dT3dT2 = Mat::eye(rows: 3, cols: 3, CV_64FC1);
1683	dT3dom2 = dT3tdom2;
1684	dT3dom1 = Mat::zeros(rows: 3, cols: 3, CV_64FC1);
1685	}
1686
1687	double cv::internal::median(const Mat& row)
1688	{
1689	CV_Assert(row.type() == CV_64FC1);
1690	CV_Assert(!row.empty() && row.rows == 1);
1691	Mat tmp = row.clone();
1692	sort(src: tmp, dst: tmp, flags: 0);
1693	if ((int)tmp.total() % 2) return tmp.at<double>(i0: (int)tmp.total() / 2);
1694	else return 0.5 *(tmp.at<double>(i0: (int)tmp.total() / 2) + tmp.at<double>(i0: (int)tmp.total() / 2 - 1));
1695	}
1696
1697	cv::Vec3d cv::internal::median3d(InputArray m)
1698	{
1699	CV_Assert(m.depth() == CV_64F && m.getMat().rows == 1);
1700	Mat M = Mat(m.getMat().t()).reshape(cn: 1).t();
1701	return Vec3d(median(row: M.row(y: 0)), median(row: M.row(y: 1)), median(row: M.row(y: 2)));
1702	}
1703