1793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/*M/////////////////////////////////////////////////////////////////////////////////////// 2793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 3793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 5793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// By downloading, copying, installing or using the software you agree to this license. 6793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// If you do not agree to this license, do not download, install, 7793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// copy or use the software. 8793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 9793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 10793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// License Agreement 11793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// For Open Source Computer Vision Library 12793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 13793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. 15793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Third party copyrights are property of their respective owners. 16793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 17793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// Redistribution and use in source and binary forms, with or without modification, 18793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// are permitted provided that the following conditions are met: 19793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 20793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * Redistribution's of source code must retain the above copyright notice, 21793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// this list of conditions and the following disclaimer. 22793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 23793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * Redistribution's in binary form must reproduce the above copyright notice, 24793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// this list of conditions and the following disclaimer in the documentation 25793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// and/or other materials provided with the distribution. 26793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 27793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// * The name of the copyright holders may not be used to endorse or promote products 28793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// derived from this software without specific prior written permission. 29793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 30793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// This software is provided by the copyright holders and contributors "as is" and 31793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// any express or implied warranties, including, but not limited to, the implied 32793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// warranties of merchantability and fitness for a particular purpose are disclaimed. 33793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// In no event shall the Intel Corporation or contributors be liable for any direct, 34793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// indirect, incidental, special, exemplary, or consequential damages 35793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// (including, but not limited to, procurement of substitute goods or services; 36793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// loss of use, data, or profits; or business interruption) however caused 37793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// and on any theory of liability, whether in contract, strict liability, 38793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// or tort (including negligence or otherwise) arising in any way out of 39793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// the use of this software, even if advised of the possibility of such damage. 40793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler// 41793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler//M*/ 42793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 43793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#include "precomp.hpp" 44793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#include "fisheye.hpp" 45793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 46793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslernamespace cv { namespace 47793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 48793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler struct JacobianRow 49793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 50793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d df, dc; 51793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d dk; 52793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dom, dT; 53793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double dalpha; 54793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler }; 55793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 56793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler void subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows); 57793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler}} 58793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 59793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 60793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::projectPoints 61793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 62793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, const Affine3d& affine, 63793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputArray K, InputArray D, double alpha, OutputArray jacobian) 64793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 65793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler projectPoints(objectPoints, imagePoints, affine.rvec(), affine.translation(), K, D, alpha, jacobian); 66793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 67793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 68793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray _rvec, 69793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputArray _tvec, InputArray _K, InputArray _D, double alpha, OutputArray jacobian) 70793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 71793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // will support only 3-channel data now for points 72793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); 73793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2)); 74793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler size_t n = objectPoints.total(); 75793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 76793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(_rvec.total() * _rvec.channels() == 3 && (_rvec.depth() == CV_32F || _rvec.depth() == CV_64F)); 77793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); 78793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(_tvec.getMat().isContinuous() && _rvec.getMat().isContinuous()); 79793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 80793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d om = _rvec.depth() == CV_32F ? (Vec3d)*_rvec.getMat().ptr<Vec3f>() : *_rvec.getMat().ptr<Vec3d>(); 81793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d T = _tvec.depth() == CV_32F ? (Vec3d)*_tvec.getMat().ptr<Vec3f>() : *_tvec.getMat().ptr<Vec3d>(); 82793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 83793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(_K.size() == Size(3,3) && (_K.type() == CV_32F || _K.type() == CV_64F) && _D.type() == _K.type() && _D.total() == 4); 84793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 85793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d f, c; 86793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (_K.depth() == CV_32F) 87793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 88793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 89793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33f K = _K.getMat(); 90793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2f(K(0, 0), K(1, 1)); 91793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2f(K(0, 2), K(1, 2)); 92793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 93793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 94793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 95793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d K = _K.getMat(); 96793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2d(K(0, 0), K(1, 1)); 97793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2d(K(0, 2), K(1, 2)); 98793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 99793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 100793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d k = _D.depth() == CV_32F ? (Vec4d)*_D.getMat().ptr<Vec4f>(): *_D.getMat().ptr<Vec4d>(); 101793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 102793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JacobianRow *Jn = 0; 103793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (jacobian.needed()) 104793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 105793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int nvars = 2 + 2 + 1 + 4 + 3 + 3; // f, c, alpha, k, om, T, 106793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobian.create(2*(int)n, nvars, CV_64F); 107793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn = jacobian.getMat().ptr<JacobianRow>(0); 108793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 109793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 110793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d R; 111793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx<double, 3, 9> dRdom; 112793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(om, R, dRdom); 113793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Affine3d aff(om, T); 114793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 115793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec3f* Xf = objectPoints.getMat().ptr<Vec3f>(); 116793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec3d* Xd = objectPoints.getMat().ptr<Vec3d>(); 117793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2f *xpf = imagePoints.getMat().ptr<Vec2f>(); 118793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d *xpd = imagePoints.getMat().ptr<Vec2d>(); 119793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 120793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < n; ++i) 121793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 122793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d Xi = objectPoints.depth() == CV_32F ? (Vec3d)Xf[i] : Xd[i]; 123793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d Y = aff*Xi; 124793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 125793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d x(Y[0]/Y[2], Y[1]/Y[2]); 126793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 127793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double r2 = x.dot(x); 128793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double r = std::sqrt(r2); 129793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 130793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // Angle of the incoming ray: 131793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta = atan(r); 132793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 133793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta, 134793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta; 135793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 136793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; 137793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 138793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double inv_r = r > 1e-8 ? 1.0/r : 1; 139793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double cdist = r > 1e-8 ? theta_d * inv_r : 1; 140793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 141793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d xd1 = x * cdist; 142793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); 143793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]); 144793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 145793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (objectPoints.depth() == CV_32F) 146793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler xpf[i] = final_point; 147793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 148793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler xpd[i] = final_point; 149793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 150793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (jacobian.needed()) 151793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 152793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Vec3d Xi = pdepth == CV_32F ? (Vec3d)Xf[i] : Xd[i]; 153793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Vec3d Y = aff*Xi; 154793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double dYdR[] = { Xi[0], Xi[1], Xi[2], 0, 0, 0, 0, 0, 0, 155793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 0, Xi[0], Xi[1], Xi[2], 0, 0, 0, 156793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 0, 0, 0, 0, Xi[0], Xi[1], Xi[2] }; 157793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 158793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d dYdom_data = Matx<double, 3, 9>(dYdR) * dRdom.t(); 159793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec3d *dYdom = (Vec3d*)dYdom_data.val; 160793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 161793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d dYdT_data = Matx33d::eye(); 162793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec3d *dYdT = (Vec3d*)dYdT_data.val; 163793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 164793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Vec2d x(Y[0]/Y[2], Y[1]/Y[2]); 165793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dxdom[2]; 166793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxdom[0] = (1.0/Y[2]) * dYdom[0] - x[0]/Y[2] * dYdom[2]; 167793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxdom[1] = (1.0/Y[2]) * dYdom[1] - x[1]/Y[2] * dYdom[2]; 168793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 169793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dxdT[2]; 170793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxdT[0] = (1.0/Y[2]) * dYdT[0] - x[0]/Y[2] * dYdT[2]; 171793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxdT[1] = (1.0/Y[2]) * dYdT[1] - x[1]/Y[2] * dYdT[2]; 172793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 173793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double r2 = x.dot(x); 174793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dr2dom = 2 * x[0] * dxdom[0] + 2 * x[1] * dxdom[1]; 175793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dr2dT = 2 * x[0] * dxdT[0] + 2 * x[1] * dxdT[1]; 176793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 177793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double r = std::sqrt(r2); 178793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double drdr2 = r > 1e-8 ? 1.0/(2*r) : 1; 179793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d drdom = drdr2 * dr2dom; 180793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d drdT = drdr2 * dr2dT; 181793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 182793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // Angle of the incoming ray: 183793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double theta = atan(r); 184793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double dthetadr = 1.0/(1+r2); 185793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dthetadom = dthetadr * drdom; 186793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dthetadT = dthetadr * drdT; 187793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 188793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; 189793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double dtheta_ddtheta = 1 + 3*k[0]*theta2 + 5*k[1]*theta4 + 7*k[2]*theta6 + 9*k[3]*theta8; 190793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dtheta_ddom = dtheta_ddtheta * dthetadom; 191793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dtheta_ddT = dtheta_ddtheta * dthetadT; 192793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d dtheta_ddk = Vec4d(theta3, theta5, theta7, theta9); 193793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 194793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double inv_r = r > 1e-8 ? 1.0/r : 1; 195793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //double cdist = r > 1e-8 ? theta_d / r : 1; 196793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dcdistdom = inv_r * (dtheta_ddom - cdist*drdom); 197793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dcdistdT = inv_r * (dtheta_ddT - cdist*drdT); 198793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d dcdistdk = inv_r * dtheta_ddk; 199793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 200793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Vec2d xd1 = x * cdist; 201793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d dxd1dk[2]; 202793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dxd1dom[2], dxd1dT[2]; 203793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dom[0] = x[0] * dcdistdom + cdist * dxdom[0]; 204793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dom[1] = x[1] * dcdistdom + cdist * dxdom[1]; 205793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dT[0] = x[0] * dcdistdT + cdist * dxdT[0]; 206793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dT[1] = x[1] * dcdistdT + cdist * dxdT[1]; 207793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dk[0] = x[0] * dcdistdk; 208793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd1dk[1] = x[1] * dcdistdk; 209793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 210793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); 211793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d dxd3dk[2]; 212793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d dxd3dom[2], dxd3dT[2]; 213793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dom[0] = dxd1dom[0] + alpha * dxd1dom[1]; 214793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dom[1] = dxd1dom[1]; 215793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dT[0] = dxd1dT[0] + alpha * dxd1dT[1]; 216793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dT[1] = dxd1dT[1]; 217793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dk[0] = dxd1dk[0] + alpha * dxd1dk[1]; 218793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxd3dk[1] = dxd1dk[1]; 219793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 220793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d dxd3dalpha(xd1[1], 0); 221793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 222793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //final jacobian 223793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].dom = f[0] * dxd3dom[0]; 224793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].dom = f[1] * dxd3dom[1]; 225793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 226793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].dT = f[0] * dxd3dT[0]; 227793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].dT = f[1] * dxd3dT[1]; 228793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 229793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].dk = f[0] * dxd3dk[0]; 230793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].dk = f[1] * dxd3dk[1]; 231793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 232793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].dalpha = f[0] * dxd3dalpha[0]; 233793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].dalpha = 0; //f[1] * dxd3dalpha[1]; 234793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 235793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].df = Vec2d(xd3[0], 0); 236793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].df = Vec2d(0, xd3[1]); 237793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 238793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[0].dc = Vec2d(1, 0); 239793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn[1].dc = Vec2d(0, 1); 240793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 241793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //step to jacobian rows for next point 242793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jn += 2; 243793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 244793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 245793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 246793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 247793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 248793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::distortPoints 249793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 250793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::distortPoints(InputArray undistorted, OutputArray distorted, InputArray K, InputArray D, double alpha) 251793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 252793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // will support only 2-channel data now for points 253793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(undistorted.type() == CV_32FC2 || undistorted.type() == CV_64FC2); 254793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler distorted.create(undistorted.size(), undistorted.type()); 255793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler size_t n = undistorted.total(); 256793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 257793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(K.size() == Size(3,3) && (K.type() == CV_32F || K.type() == CV_64F) && D.total() == 4); 258793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 259793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d f, c; 260793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K.depth() == CV_32F) 261793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 262793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33f camMat = K.getMat(); 263793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2f(camMat(0, 0), camMat(1, 1)); 264793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2f(camMat(0, 2), camMat(1, 2)); 265793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 266793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 267793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 268793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d camMat = K.getMat(); 269793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2d(camMat(0, 0), camMat(1, 1)); 270793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2d(camMat(0 ,2), camMat(1, 2)); 271793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 272793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 273793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>(); 274793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 275793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec2f* Xf = undistorted.getMat().ptr<Vec2f>(); 276793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec2d* Xd = undistorted.getMat().ptr<Vec2d>(); 277793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2f *xpf = distorted.getMat().ptr<Vec2f>(); 278793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d *xpd = distorted.getMat().ptr<Vec2d>(); 279793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 280793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < n; ++i) 281793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 282793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d x = undistorted.depth() == CV_32F ? (Vec2d)Xf[i] : Xd[i]; 283793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 284793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double r2 = x.dot(x); 285793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double r = std::sqrt(r2); 286793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 287793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // Angle of the incoming ray: 288793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta = atan(r); 289793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 290793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta2 = theta*theta, theta3 = theta2*theta, theta4 = theta2*theta2, theta5 = theta4*theta, 291793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler theta6 = theta3*theta3, theta7 = theta6*theta, theta8 = theta4*theta4, theta9 = theta8*theta; 292793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 293793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta_d = theta + k[0]*theta3 + k[1]*theta5 + k[2]*theta7 + k[3]*theta9; 294793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 295793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double inv_r = r > 1e-8 ? 1.0/r : 1; 296793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double cdist = r > 1e-8 ? theta_d * inv_r : 1; 297793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 298793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d xd1 = x * cdist; 299793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d xd3(xd1[0] + alpha*xd1[1], xd1[1]); 300793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d final_point(xd3[0] * f[0] + c[0], xd3[1] * f[1] + c[1]); 301793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 302793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (undistorted.depth() == CV_32F) 303793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler xpf[i] = final_point; 304793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 305793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler xpd[i] = final_point; 306793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 307793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 308793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 309793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 310793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::undistortPoints 311793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 312793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::undistortPoints( InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, InputArray R, InputArray P) 313793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 314793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // will support only 2-channel data now for points 315793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(distorted.type() == CV_32FC2 || distorted.type() == CV_64FC2); 316793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler undistorted.create(distorted.size(), distorted.type()); 317793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 318793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); 319793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); 320793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(D.total() == 4 && K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F)); 321793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 322793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d f, c; 323793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K.depth() == CV_32F) 324793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 325793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33f camMat = K.getMat(); 326793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2f(camMat(0, 0), camMat(1, 1)); 327793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2f(camMat(0, 2), camMat(1, 2)); 328793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 329793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 330793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 331793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d camMat = K.getMat(); 332793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2d(camMat(0, 0), camMat(1, 1)); 333793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2d(camMat(0, 2), camMat(1, 2)); 334793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 335793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 336793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>(); 337793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 338793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d RR = cv::Matx33d::eye(); 339793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!R.empty() && R.total() * R.channels() == 3) 340793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 341793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec3d rvec; 342793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R.getMat().convertTo(rvec, CV_64F); 343793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler RR = cv::Affine3d(rvec).rotation(); 344793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 345793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else if (!R.empty() && R.size() == Size(3, 3)) 346793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R.getMat().convertTo(RR, CV_64F); 347793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 348793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if(!P.empty()) 349793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 350793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d PP; 351793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler P.getMat().colRange(0, 3).convertTo(PP, CV_64F); 352793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler RR = PP * RR; 353793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 354793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 355793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // start undistorting 356793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Vec2f* srcf = distorted.getMat().ptr<cv::Vec2f>(); 357793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Vec2d* srcd = distorted.getMat().ptr<cv::Vec2d>(); 358793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2f* dstf = undistorted.getMat().ptr<cv::Vec2f>(); 359793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d* dstd = undistorted.getMat().ptr<cv::Vec2d>(); 360793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 361793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler size_t n = distorted.total(); 362793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int sdepth = distorted.depth(); 363793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 364793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < n; i++ ) 365793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 366793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d pi = sdepth == CV_32F ? (Vec2d)srcf[i] : srcd[i]; // image point 367793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d pw((pi[0] - c[0])/f[0], (pi[1] - c[1])/f[1]); // world point 368793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 369793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double scale = 1.0; 370793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 371793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta_d = sqrt(pw[0]*pw[0] + pw[1]*pw[1]); 372793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (theta_d > 1e-8) 373793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 374793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // compensate distortion iteratively 375793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta = theta_d; 376793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(int j = 0; j < 10; j++ ) 377793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 378793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta6*theta2; 379793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler theta = theta_d / (1 + k[0] * theta2 + k[1] * theta4 + k[2] * theta6 + k[3] * theta8); 380793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 381793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 382793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler scale = std::tan(theta) / theta_d; 383793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 384793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 385793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d pu = pw * scale; //undistorted point 386793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 387793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // reproject 388793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d pr = RR * Vec3d(pu[0], pu[1], 1.0); // rotated point optionally multiplied by new camera matrix 389793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d fi(pr[0]/pr[2], pr[1]/pr[2]); // final 390793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 391793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( sdepth == CV_32F ) 392793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dstf[i] = fi; 393793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 394793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dstd[i] = fi; 395793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 396793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 397793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 398793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 399793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::undistortPoints 400793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 401793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::initUndistortRectifyMap( InputArray K, InputArray D, InputArray R, InputArray P, 402793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const cv::Size& size, int m1type, OutputArray map1, OutputArray map2 ) 403793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 404793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 ); 405793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler map1.create( size, m1type <= 0 ? CV_16SC2 : m1type ); 406793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F ); 407793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 408793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F)); 409793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((P.depth() == CV_32F || P.depth() == CV_64F) && (R.depth() == CV_32F || R.depth() == CV_64F)); 410793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4)); 411793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); 412793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); 413793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 414793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d f, c; 415793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K.depth() == CV_32F) 416793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 417793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33f camMat = K.getMat(); 418793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2f(camMat(0, 0), camMat(1, 1)); 419793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2f(camMat(0, 2), camMat(1, 2)); 420793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 421793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 422793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 423793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d camMat = K.getMat(); 424793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f = Vec2d(camMat(0, 0), camMat(1, 1)); 425793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler c = Vec2d(camMat(0, 2), camMat(1, 2)); 426793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 427793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 428793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d k = Vec4d::all(0); 429793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!D.empty()) 430793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler k = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr<Vec4f>(): *D.getMat().ptr<Vec4d>(); 431793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 432793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d RR = cv::Matx33d::eye(); 433793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!R.empty() && R.total() * R.channels() == 3) 434793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 435793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec3d rvec; 436793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R.getMat().convertTo(rvec, CV_64F); 437793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler RR = Affine3d(rvec).rotation(); 438793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 439793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else if (!R.empty() && R.size() == Size(3, 3)) 440793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R.getMat().convertTo(RR, CV_64F); 441793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 442793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d PP = cv::Matx33d::eye(); 443793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!P.empty()) 444793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler P.getMat().colRange(0, 3).convertTo(PP, CV_64F); 445793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 446793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d iR = (PP * RR).inv(cv::DECOMP_SVD); 447793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 448793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( int i = 0; i < size.height; ++i) 449793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 450793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler float* m1f = map1.getMat().ptr<float>(i); 451793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler float* m2f = map2.getMat().ptr<float>(i); 452793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler short* m1 = (short*)m1f; 453793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ushort* m2 = (ushort*)m2f; 454793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 455793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double _x = i*iR(0, 1) + iR(0, 2), 456793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _y = i*iR(1, 1) + iR(1, 2), 457793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _w = i*iR(2, 1) + iR(2, 2); 458793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 459793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( int j = 0; j < size.width; ++j) 460793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 461793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double x = _x/_w, y = _y/_w; 462793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 463793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double r = sqrt(x*x + y*y); 464793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta = atan(r); 465793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 466793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta2 = theta*theta, theta4 = theta2*theta2, theta6 = theta4*theta2, theta8 = theta4*theta4; 467793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double theta_d = theta * (1 + k[0]*theta2 + k[1]*theta4 + k[2]*theta6 + k[3]*theta8); 468793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 469793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double scale = (r == 0) ? 1.0 : theta_d / r; 470793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double u = f[0]*x*scale + c[0]; 471793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double v = f[1]*y*scale + c[1]; 472793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 473793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( m1type == CV_16SC2 ) 474793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 475793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int iu = cv::saturate_cast<int>(u*cv::INTER_TAB_SIZE); 476793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int iv = cv::saturate_cast<int>(v*cv::INTER_TAB_SIZE); 477793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m1[j*2+0] = (short)(iu >> cv::INTER_BITS); 478793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m1[j*2+1] = (short)(iv >> cv::INTER_BITS); 479793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1))); 480793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 481793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else if( m1type == CV_32FC1 ) 482793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 483793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m1f[j] = (float)u; 484793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m2f[j] = (float)v; 485793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 486793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 487793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _x += iR(0, 0); 488793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _y += iR(1, 0); 489793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _w += iR(2, 0); 490793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 491793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 492793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 493793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 494793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 495793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::undistortImage 496793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 497793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::undistortImage(InputArray distorted, OutputArray undistorted, 498793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputArray K, InputArray D, InputArray Knew, const Size& new_size) 499793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 500793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Size size = new_size.area() != 0 ? new_size : distorted.size(); 501793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 502793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat map1, map2; 503793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler fisheye::initUndistortRectifyMap(K, D, cv::Matx33d::eye(), Knew, size, CV_16SC2, map1, map2 ); 504793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT); 505793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 506793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 507793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 508793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 509793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::estimateNewCameraMatrixForUndistortRectify 510793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 511793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::estimateNewCameraMatrixForUndistortRectify(InputArray K, InputArray D, const Size &image_size, InputArray R, 512793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler OutputArray P, double balance, const Size& new_size, double fov_scale) 513793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 514793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert( K.size() == Size(3, 3) && (K.depth() == CV_32F || K.depth() == CV_64F)); 515793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((D.empty() || D.total() == 4) && (D.depth() == CV_32F || D.depth() == CV_64F || D.empty())); 516793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 517793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int w = image_size.width, h = image_size.height; 518793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler balance = std::min(std::max(balance, 0.0), 1.0); 519793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 520793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat points(1, 4, CV_64FC2); 521793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d* pptr = points.ptr<Vec2d>(); 522793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[0] = Vec2d(w/2, 0); 523793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[1] = Vec2d(w, h/2); 524793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[2] = Vec2d(w/2, h); 525793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[3] = Vec2d(0, h/2); 526793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 527793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#if 0 528793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int N = 10; 529793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat points(1, N * 4, CV_64FC2); 530793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d* pptr = points.ptr<Vec2d>(); 531793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(int i = 0, k = 0; i < 10; ++i) 532793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 533793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[k++] = Vec2d(w/2, 0) - Vec2d(w/8, 0) + Vec2d(w/4/N*i, 0); 534793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[k++] = Vec2d(w/2, h-1) - Vec2d(w/8, h-1) + Vec2d(w/4/N*i, h-1); 535793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 536793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[k++] = Vec2d(0, h/2) - Vec2d(0, h/8) + Vec2d(0, h/4/N*i); 537793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[k++] = Vec2d(w-1, h/2) - Vec2d(w-1, h/8) + Vec2d(w-1, h/4/N*i); 538793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 539793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#endif 540793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 541793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler fisheye::undistortPoints(points, points, K, D, R); 542793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Scalar center_mass = mean(points); 543793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d cn(center_mass.val); 544793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 545793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double aspect_ratio = (K.depth() == CV_32F) ? K.getMat().at<float >(0,0)/K.getMat().at<float> (1,1) 546793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler : K.getMat().at<double>(0,0)/K.getMat().at<double>(1,1); 547793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 548793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // convert to identity ratio 549793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cn[0] *= aspect_ratio; 550793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < points.total(); ++i) 551793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler pptr[i][1] *= aspect_ratio; 552793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 553793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double minx = DBL_MAX, miny = DBL_MAX, maxx = -DBL_MAX, maxy = -DBL_MAX; 554793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < points.total(); ++i) 555793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 556793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler miny = std::min(miny, pptr[i][1]); 557793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler maxy = std::max(maxy, pptr[i][1]); 558793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler minx = std::min(minx, pptr[i][0]); 559793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler maxx = std::max(maxx, pptr[i][0]); 560793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 561793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 562793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#if 0 563793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double minx = -DBL_MAX, miny = -DBL_MAX, maxx = DBL_MAX, maxy = DBL_MAX; 564793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(size_t i = 0; i < points.total(); ++i) 565793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 566793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (i % 4 == 0) miny = std::max(miny, pptr[i][1]); 567793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (i % 4 == 1) maxy = std::min(maxy, pptr[i][1]); 568793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (i % 4 == 2) minx = std::max(minx, pptr[i][0]); 569793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (i % 4 == 3) maxx = std::min(maxx, pptr[i][0]); 570793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 571793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler#endif 572793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 573793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double f1 = w * 0.5/(cn[0] - minx); 574793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double f2 = w * 0.5/(maxx - cn[0]); 575793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double f3 = h * 0.5 * aspect_ratio/(cn[1] - miny); 576793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double f4 = h * 0.5 * aspect_ratio/(maxy - cn[1]); 577793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 578793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double fmin = std::min(f1, std::min(f2, std::min(f3, f4))); 579793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double fmax = std::max(f1, std::max(f2, std::max(f3, f4))); 580793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 581793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double f = balance * fmin + (1.0 - balance) * fmax; 582793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f *= fov_scale > 0 ? 1.0/fov_scale : 1.0; 583793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 584793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec2d new_f(f, f), new_c = -cn * f + Vec2d(w, h * aspect_ratio) * 0.5; 585793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 586793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // restore aspect ratio 587793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler new_f[1] /= aspect_ratio; 588793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler new_c[1] /= aspect_ratio; 589793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 590793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (new_size.area() > 0) 591793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 592793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double rx = new_size.width /(double)image_size.width; 593793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double ry = new_size.height/(double)image_size.height; 594793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 595793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler new_f[0] *= rx; new_f[1] *= ry; 596793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler new_c[0] *= rx; new_c[1] *= ry; 597793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 598793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 599793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(Matx33d(new_f[0], 0, new_c[0], 600793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, new_f[1], new_c[1], 601793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1)).convertTo(P, P.empty() ? K.type() : P.type()); 602793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 603793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 604793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 605793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 606793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::stereoRectify 607793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 608793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::fisheye::stereoRectify( InputArray K1, InputArray D1, InputArray K2, InputArray D2, const Size& imageSize, 609793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputArray _R, InputArray _tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, 610793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler OutputArray Q, int flags, const Size& newImageSize, double balance, double fov_scale) 611793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 612793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((_R.size() == Size(3, 3) || _R.total() * _R.channels() == 3) && (_R.depth() == CV_32F || _R.depth() == CV_64F)); 613793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(_tvec.total() * _tvec.channels() == 3 && (_tvec.depth() == CV_32F || _tvec.depth() == CV_64F)); 614793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 615793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 616793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat aaa = _tvec.getMat().reshape(3, 1); 617793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 618793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d rvec; // Rodrigues vector 619793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (_R.size() == Size(3, 3)) 620793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 621793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Matx33d rmat; 622793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _R.getMat().convertTo(rmat, CV_64F); 623793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvec = Affine3d(rmat).rvec(); 624793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 625793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else if (_R.total() * _R.channels() == 3) 626793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _R.getMat().convertTo(rvec, CV_64F); 627793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 628793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d tvec; 629793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _tvec.getMat().convertTo(tvec, CV_64F); 630793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 631793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // rectification algorithm 632793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvec *= -0.5; // get average rotation 633793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 634793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d r_r; 635793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(rvec, r_r); // rotate cameras to same orientation by averaging 636793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 637793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d t = r_r * tvec; 638793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d uu(t[0] > 0 ? 1 : -1, 0, 0); 639793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 640793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // calculate global Z rotation 641793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d ww = t.cross(uu); 642793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double nw = norm(ww); 643793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (nw > 0.0) 644793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ww *= acos(fabs(t[0])/cv::norm(t))/nw; 645793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 646793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d wr; 647793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(ww, wr); 648793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 649793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // apply to both views 650793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d ri1 = wr * r_r.t(); 651793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(ri1, false).convertTo(R1, R1.empty() ? CV_64F : R1.type()); 652793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d ri2 = wr * r_r; 653793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(ri2, false).convertTo(R2, R2.empty() ? CV_64F : R2.type()); 654793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec3d tnew = ri2 * tvec; 655793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 656793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // calculate projection/camera matrices. these contain the relevant rectified image internal params (fx, fy=fx, cx, cy) 657793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d newK1, newK2; 658793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler estimateNewCameraMatrixForUndistortRectify(K1, D1, imageSize, R1, newK1, balance, newImageSize, fov_scale); 659793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler estimateNewCameraMatrixForUndistortRectify(K2, D2, imageSize, R2, newK2, balance, newImageSize, fov_scale); 660793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 661793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double fc_new = std::min(newK1(1,1), newK2(1,1)); 662793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Point2d cc_new[2] = { Vec2d(newK1(0, 2), newK1(1, 2)), Vec2d(newK2(0, 2), newK2(1, 2)) }; 663793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 664793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // Vertical focal length must be the same for both images to keep the epipolar constraint use fy for fx also. 665793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // For simplicity, set the principal points for both cameras to be the average 666793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler // of the two principal points (either one of or both x- and y- coordinates) 667793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if( flags & cv::CALIB_ZERO_DISPARITY ) 668793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cc_new[0] = cc_new[1] = (cc_new[0] + cc_new[1]) * 0.5; 669793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 670793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cc_new[0].y = cc_new[1].y = (cc_new[0].y + cc_new[1].y)*0.5; 671793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 672793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(Matx34d(fc_new, 0, cc_new[0].x, 0, 673793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, fc_new, cc_new[0].y, 0, 674793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1, 0), false).convertTo(P1, P1.empty() ? CV_64F : P1.type()); 675793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 676793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(Matx34d(fc_new, 0, cc_new[1].x, tnew[0]*fc_new, // baseline * focal length;, 677793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, fc_new, cc_new[1].y, 0, 678793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1, 0), false).convertTo(P2, P2.empty() ? CV_64F : P2.type()); 679793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 680793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (Q.needed()) 681793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(Matx44d(1, 0, 0, -cc_new[0].x, 682793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 1, 0, -cc_new[0].y, 683793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 0, fc_new, 684793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, -1./tnew[0], (cc_new[0].x - cc_new[1].x)/tnew[0]), false).convertTo(Q, Q.empty() ? CV_64F : Q.depth()); 685793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 686793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 687793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 688793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::calibrate 689793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 690793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerdouble cv::fisheye::calibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, const Size& image_size, 691793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputOutputArray K, InputOutputArray D, OutputArrayOfArrays rvecs, OutputArrayOfArrays tvecs, 692793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int flags , cv::TermCriteria criteria) 693793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 694793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && !imagePoints.empty() && objectPoints.total() == imagePoints.total()); 695793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); 696793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2); 697793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty()); 698793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!D.empty() && D.total() == 4) || D.empty()); 699793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!rvecs.empty() && rvecs.channels() == 3) || rvecs.empty()); 700793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!tvecs.empty() && tvecs.channels() == 3) || tvecs.empty()); 701793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 702793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(((flags & CALIB_USE_INTRINSIC_GUESS) && !K.empty() && !D.empty()) || !(flags & CALIB_USE_INTRINSIC_GUESS)); 703793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 704793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler using namespace cv::internal; 705793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //-------------------------------Initialization 706793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler IntrinsicParams finalParam; 707793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler IntrinsicParams currentParam; 708793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler IntrinsicParams errors; 709793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 710793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[0] = 1; 711793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[1] = 1; 712793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[2] = 1; 713793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[3] = 1; 714793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[4] = flags & CALIB_FIX_SKEW ? 0 : 1; 715793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[5] = flags & CALIB_FIX_K1 ? 0 : 1; 716793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[6] = flags & CALIB_FIX_K2 ? 0 : 1; 717793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[7] = flags & CALIB_FIX_K3 ? 0 : 1; 718793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.isEstimate[8] = flags & CALIB_FIX_K4 ? 0 : 1; 719793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 720793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int recompute_extrinsic = flags & CALIB_RECOMPUTE_EXTRINSIC ? 1: 0; 721793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int check_cond = flags & CALIB_CHECK_COND ? 1 : 0; 722793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 723793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double alpha_smooth = 0.4; 724793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double thresh_cond = 1e6; 725793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double change = 1; 726793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d err_std; 727793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 728793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d _K; 729793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d _D; 730793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (flags & CALIB_USE_INTRINSIC_GUESS) 731793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 732793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler K.getMat().convertTo(_K, CV_64FC1); 733793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler D.getMat().convertTo(_D, CV_64FC1); 734793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.Init(Vec2d(_K(0,0), _K(1, 1)), 735793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d(_K(0,2), _K(1, 2)), 736793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d(flags & CALIB_FIX_K1 ? 0 : _D[0], 737793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler flags & CALIB_FIX_K2 ? 0 : _D[1], 738793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler flags & CALIB_FIX_K3 ? 0 : _D[2], 739793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler flags & CALIB_FIX_K4 ? 0 : _D[3]), 740793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _K(0, 1) / _K(0, 0)); 741793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 742793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 743793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 744793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam.Init(Vec2d(max(image_size.width, image_size.height) / CV_PI, max(image_size.width, image_size.height) / CV_PI), 745793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d(image_size.width / 2.0 - 0.5, image_size.height / 2.0 - 0.5)); 746793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 747793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 748793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errors.isEstimate = finalParam.isEstimate; 749793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 750793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<Vec3d> omc(objectPoints.total()), Tc(objectPoints.total()); 751793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 752793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond, thresh_cond, omc, Tc); 753793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 754793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 755793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //-------------------------------Optimization 756793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(int iter = 0; ; ++iter) 757793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 758793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ((criteria.type == 1 && iter >= criteria.maxCount) || 759793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (criteria.type == 2 && change <= criteria.epsilon) || 760793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount))) 761793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler break; 762793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 763793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, iter + 1.0); 764793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 765793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat JJ2_inv, ex3; 766793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ComputeJacobians(objectPoints, imagePoints, finalParam, omc, Tc, check_cond,thresh_cond, JJ2_inv, ex3); 767793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 768793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat G = alpha_smooth2 * JJ2_inv * ex3; 769793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 770793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler currentParam = finalParam + G; 771793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 772793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler change = norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1]) - 773793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d(finalParam.f[0], finalParam.f[1], finalParam.c[0], finalParam.c[1])) 774793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler / norm(Vec4d(currentParam.f[0], currentParam.f[1], currentParam.c[0], currentParam.c[1])); 775793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 776793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler finalParam = currentParam; 777793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 778793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (recompute_extrinsic) 779793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 780793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CalibrateExtrinsics(objectPoints, imagePoints, finalParam, check_cond, 781793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler thresh_cond, omc, Tc); 782793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 783793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 784793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 785793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //-------------------------------Validation 786793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double rms; 787793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler EstimateUncertainties(objectPoints, imagePoints, finalParam, omc, Tc, errors, err_std, thresh_cond, 788793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler check_cond, rms); 789793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 790793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //------------------------------- 791793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _K = Matx33d(finalParam.f[0], finalParam.f[0] * finalParam.alpha, finalParam.c[0], 792793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, finalParam.f[1], finalParam.c[1], 793793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1); 794793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 795793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K.needed()) cv::Mat(_K).convertTo(K, K.empty() ? CV_64FC1 : K.type()); 796793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (D.needed()) cv::Mat(finalParam.k).convertTo(D, D.empty() ? CV_64FC1 : D.type()); 797793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (rvecs.kind()==_InputArray::STD_VECTOR_MAT) 798793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 799793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int i; 800793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for( i = 0; i < (int)objectPoints.total(); i++ ) 801793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 802793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvecs.getMat(i)=omc[i]; 803793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tvecs.getMat(i)=Tc[i]; 804793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 805793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 806793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 807793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 808793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (rvecs.needed()) cv::Mat(omc).convertTo(rvecs, rvecs.empty() ? CV_64FC3 : rvecs.type()); 809793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (tvecs.needed()) cv::Mat(Tc).convertTo(tvecs, tvecs.empty() ? CV_64FC3 : tvecs.type()); 810793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 811793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 812793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return rms; 813793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 814793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 815793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler////////////////////////////////////////////////////////////////////////////////////////////////////////////// 816793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler/// cv::fisheye::stereoCalibrate 817793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 818793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerdouble cv::fisheye::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, 819793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InputOutputArray K1, InputOutputArray D1, InputOutputArray K2, InputOutputArray D2, Size imageSize, 820793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler OutputArray R, OutputArray T, int flags, TermCriteria criteria) 821793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 822793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && !imagePoints1.empty() && !imagePoints2.empty()); 823793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(objectPoints.total() == imagePoints1.total() || imagePoints1.total() == imagePoints2.total()); 824793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3); 825793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(imagePoints1.type() == CV_32FC2 || imagePoints1.type() == CV_64FC2); 826793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(imagePoints2.type() == CV_32FC2 || imagePoints2.type() == CV_64FC2); 827793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 828793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!K1.empty() && K1.size() == Size(3,3)) || K1.empty()); 829793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!D1.empty() && D1.total() == 4) || D1.empty()); 830793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!K2.empty() && K1.size() == Size(3,3)) || K2.empty()); 831793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert((!D2.empty() && D1.total() == 4) || D2.empty()); 832793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 833793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(((flags & CALIB_FIX_INTRINSIC) && !K1.empty() && !K2.empty() && !D1.empty() && !D2.empty()) || !(flags & CALIB_FIX_INTRINSIC)); 834793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 835793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //-------------------------------Initialization 836793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 837793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int threshold = 50; 838793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double thresh_cond = 1e6; 839793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int check_cond = 1; 840793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 841793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int n_points = (int)objectPoints.getMat(0).total(); 842793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int n_images = (int)objectPoints.total(); 843793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 844793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double change = 1; 845793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 846793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::IntrinsicParams intrinsicLeft; 847793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::IntrinsicParams intrinsicRight; 848793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 849793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::IntrinsicParams intrinsicLeft_errors; 850793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::IntrinsicParams intrinsicRight_errors; 851793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 852793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d _K1, _K2; 853793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d _D1, _D2; 854793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!K1.empty()) K1.getMat().convertTo(_K1, CV_64FC1); 855793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!D1.empty()) D1.getMat().convertTo(_D1, CV_64FC1); 856793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!K2.empty()) K2.getMat().convertTo(_K2, CV_64FC1); 857793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!D2.empty()) D2.getMat().convertTo(_D2, CV_64FC1); 858793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 859793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<Vec3d> rvecs1(n_images), tvecs1(n_images), rvecs2(n_images), tvecs2(n_images); 860793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 861793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (!(flags & CALIB_FIX_INTRINSIC)) 862793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 863793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler calibrate(objectPoints, imagePoints1, imageSize, _K1, _D1, rvecs1, tvecs1, flags, TermCriteria(3, 20, 1e-6)); 864793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler calibrate(objectPoints, imagePoints2, imageSize, _K2, _D2, rvecs2, tvecs2, flags, TermCriteria(3, 20, 1e-6)); 865793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 866793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 867793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.Init(Vec2d(_K1(0,0), _K1(1, 1)), Vec2d(_K1(0,2), _K1(1, 2)), 868793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d(_D1[0], _D1[1], _D1[2], _D1[3]), _K1(0, 1) / _K1(0, 0)); 869793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 870793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.Init(Vec2d(_K2(0,0), _K2(1, 1)), Vec2d(_K2(0,2), _K2(1, 2)), 871793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec4d(_D2[0], _D2[1], _D2[2], _D2[3]), _K2(0, 1) / _K2(0, 0)); 872793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 873793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ((flags & CALIB_FIX_INTRINSIC)) 874793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 875793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::CalibrateExtrinsics(objectPoints, imagePoints1, intrinsicLeft, check_cond, thresh_cond, rvecs1, tvecs1); 876793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::CalibrateExtrinsics(objectPoints, imagePoints2, intrinsicRight, check_cond, thresh_cond, rvecs2, tvecs2); 877793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 878793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 879793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 880793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 881793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 882793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 883793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; 884793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; 885793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; 886793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; 887793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; 888793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 889793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[0] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 890793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[1] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 891793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[2] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 892793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[3] = flags & CALIB_FIX_INTRINSIC ? 0 : 1; 893793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[4] = flags & (CALIB_FIX_SKEW | CALIB_FIX_INTRINSIC) ? 0 : 1; 894793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[5] = flags & (CALIB_FIX_K1 | CALIB_FIX_INTRINSIC) ? 0 : 1; 895793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[6] = flags & (CALIB_FIX_K2 | CALIB_FIX_INTRINSIC) ? 0 : 1; 896793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[7] = flags & (CALIB_FIX_K3 | CALIB_FIX_INTRINSIC) ? 0 : 1; 897793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight.isEstimate[8] = flags & (CALIB_FIX_K4 | CALIB_FIX_INTRINSIC) ? 0 : 1; 898793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 899793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft_errors.isEstimate = intrinsicLeft.isEstimate; 900793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight_errors.isEstimate = intrinsicRight.isEstimate; 901793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 902793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<int> selectedParams; 903793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<int> tmp(6 * (n_images + 1), 1); 904793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler selectedParams.insert(selectedParams.end(), intrinsicLeft.isEstimate.begin(), intrinsicLeft.isEstimate.end()); 905793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler selectedParams.insert(selectedParams.end(), intrinsicRight.isEstimate.begin(), intrinsicRight.isEstimate.end()); 906793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler selectedParams.insert(selectedParams.end(), tmp.begin(), tmp.end()); 907793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 908793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //Init values for rotation and translation between two views 909793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat om_list(1, n_images, CV_64FC3), T_list(1, n_images, CV_64FC3); 910793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat om_ref, R_ref, T_ref, R1, R2; 911793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int image_idx = 0; image_idx < n_images; ++image_idx) 912793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 913793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Rodrigues(rvecs1[image_idx], R1); 914793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Rodrigues(rvecs2[image_idx], R2); 915793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R_ref = R2 * R1.t(); 916793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler T_ref = cv::Mat(tvecs2[image_idx]) - R_ref * cv::Mat(tvecs1[image_idx]); 917793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Rodrigues(R_ref, om_ref); 918793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler om_ref.reshape(3, 1).copyTo(om_list.col(image_idx)); 919793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler T_ref.reshape(3, 1).copyTo(T_list.col(image_idx)); 920793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 921793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec3d omcur = cv::internal::median3d(om_list); 922793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec3d Tcur = cv::internal::median3d(T_list); 923793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 924793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat J = cv::Mat::zeros(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1), 925793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler e = cv::Mat::zeros(4 * n_points * n_images, 1, CV_64FC1), Jkk, ekk; 926793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat J2_inv; 927793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 928793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(int iter = 0; ; ++iter) 929793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 930793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ((criteria.type == 1 && iter >= criteria.maxCount) || 931793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (criteria.type == 2 && change <= criteria.epsilon) || 932793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount))) 933793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler break; 934793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 935793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.create(4 * n_points * n_images, 18 + 6 * (n_images + 1), CV_64FC1); 936793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler e.create(4 * n_points * n_images, 1, CV_64FC1); 937793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jkk.create(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1); 938793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ekk.create(4 * n_points, 1, CV_64FC1); 939793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 940793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT; 941793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 942793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int image_idx = 0; image_idx < n_images; ++image_idx) 943793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 944793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jkk = cv::Mat::zeros(4 * n_points, 18 + 6 * (n_images + 1), CV_64FC1); 945793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 946793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat object = objectPoints.getMat(image_idx).clone(); 947793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat imageLeft = imagePoints1.getMat(image_idx).clone(); 948793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat imageRight = imagePoints2.getMat(image_idx).clone(); 949793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat jacobians, projected; 950793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 951793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //left camera jacobian 952793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat rvec = cv::Mat(rvecs1[image_idx]); 953793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat tvec = cv::Mat(tvecs1[image_idx]); 954793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::projectPoints(object, projected, rvec, tvec, intrinsicLeft, jacobians); 955793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat(cv::Mat((imageLeft - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(0, 2 * n_points)); 956793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(8, 11).copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(0, 2 * n_points)); 957793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(11, 14).copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(0, 2 * n_points)); 958793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(0, 2).copyTo(Jkk.colRange(0, 2).rowRange(0, 2 * n_points)); 959793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(2, 4).copyTo(Jkk.colRange(2, 4).rowRange(0, 2 * n_points)); 960793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(4, 8).copyTo(Jkk.colRange(5, 9).rowRange(0, 2 * n_points)); 961793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.col(14).copyTo(Jkk.col(4).rowRange(0, 2 * n_points)); 962793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 963793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //right camera jacobian 964793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::compose_motion(rvec, tvec, omcur, Tcur, omr, Tr, domrdomckk, domrdTckk, domrdom, domrdT, dTrdomckk, dTrdTckk, dTrdom, dTrdT); 965793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvec = cv::Mat(rvecs2[image_idx]); 966793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tvec = cv::Mat(tvecs2[image_idx]); 967793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 968793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::internal::projectPoints(object, projected, omr, Tr, intrinsicRight, jacobians); 969793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat(cv::Mat((imageRight - projected).t()).reshape(1, 1).t()).copyTo(ekk.rowRange(2 * n_points, 4 * n_points)); 970793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat dxrdom = jacobians.colRange(8, 11) * domrdom + jacobians.colRange(11, 14) * dTrdom; 971793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat dxrdT = jacobians.colRange(8, 11) * domrdT + jacobians.colRange(11, 14)* dTrdT; 972793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat dxrdomckk = jacobians.colRange(8, 11) * domrdomckk + jacobians.colRange(11, 14) * dTrdomckk; 973793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat dxrdTckk = jacobians.colRange(8, 11) * domrdTckk + jacobians.colRange(11, 14) * dTrdTckk; 974793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 975793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxrdom.copyTo(Jkk.colRange(18, 21).rowRange(2 * n_points, 4 * n_points)); 976793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxrdT.copyTo(Jkk.colRange(21, 24).rowRange(2 * n_points, 4 * n_points)); 977793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxrdomckk.copyTo(Jkk.colRange(24 + image_idx * 6, 27 + image_idx * 6).rowRange(2 * n_points, 4 * n_points)); 978793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dxrdTckk.copyTo(Jkk.colRange(27 + image_idx * 6, 30 + image_idx * 6).rowRange(2 * n_points, 4 * n_points)); 979793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(0, 2).copyTo(Jkk.colRange(9 + 0, 9 + 2).rowRange(2 * n_points, 4 * n_points)); 980793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(2, 4).copyTo(Jkk.colRange(9 + 2, 9 + 4).rowRange(2 * n_points, 4 * n_points)); 981793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(4, 8).copyTo(Jkk.colRange(9 + 5, 9 + 9).rowRange(2 * n_points, 4 * n_points)); 982793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.col(14).copyTo(Jkk.col(9 + 4).rowRange(2 * n_points, 4 * n_points)); 983793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 984793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //check goodness of sterepair 985793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double abs_max = 0; 986793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0; i < 4 * n_points; i++) 987793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 988793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (fabs(ekk.at<double>(i)) > abs_max) 989793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 990793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler abs_max = fabs(ekk.at<double>(i)); 991793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 992793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 993793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 994793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(abs_max < threshold); // bad stereo pair 995793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 996793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Jkk.copyTo(J.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points)); 997793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ekk.copyTo(e.rowRange(image_idx * 4 * n_points, (image_idx + 1) * 4 * n_points)); 998793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 999793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1000793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec6d oldTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); 1001793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1002793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //update all parameters 1003793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::subMatrix(J, J, selectedParams, std::vector<int>(J.rows, 1)); 1004793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat J2 = J.t() * J; 1005793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J2_inv = J2.inv(); 1006793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int a = cv::countNonZero(intrinsicLeft.isEstimate); 1007793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int b = cv::countNonZero(intrinsicRight.isEstimate); 1008793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Mat deltas = J2_inv * J.t() * e; 1009793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicLeft = intrinsicLeft + deltas.rowRange(0, a); 1010793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler intrinsicRight = intrinsicRight + deltas.rowRange(a, a + b); 1011793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler omcur = omcur + cv::Vec3d(deltas.rowRange(a + b, a + b + 3)); 1012793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Tcur = Tcur + cv::Vec3d(deltas.rowRange(a + b + 3, a + b + 6)); 1013793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int image_idx = 0; image_idx < n_images; ++image_idx) 1014793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1015793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvecs1[image_idx] = cv::Mat(cv::Mat(rvecs1[image_idx]) + deltas.rowRange(a + b + 6 + image_idx * 6, a + b + 9 + image_idx * 6)); 1016793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tvecs1[image_idx] = cv::Mat(cv::Mat(tvecs1[image_idx]) + deltas.rowRange(a + b + 9 + image_idx * 6, a + b + 12 + image_idx * 6)); 1017793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1018793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1019793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::Vec6d newTom(Tcur[0], Tcur[1], Tcur[2], omcur[0], omcur[1], omcur[2]); 1020793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler change = cv::norm(newTom - oldTom) / cv::norm(newTom); 1021793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1022793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1023793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double rms = 0; 1024793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec2d* ptr_e = e.ptr<Vec2d>(); 1025793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (size_t i = 0; i < e.total() / 2; i++) 1026793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1027793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms += ptr_e[i][0] * ptr_e[i][0] + ptr_e[i][1] * ptr_e[i][1]; 1028793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1029793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1030793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms /= ((double)e.total() / 2.0); 1031793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms = sqrt(rms); 1032793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1033793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _K1 = Matx33d(intrinsicLeft.f[0], intrinsicLeft.f[0] * intrinsicLeft.alpha, intrinsicLeft.c[0], 1034793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, intrinsicLeft.f[1], intrinsicLeft.c[1], 1035793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1); 1036793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1037793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler _K2 = Matx33d(intrinsicRight.f[0], intrinsicRight.f[0] * intrinsicRight.alpha, intrinsicRight.c[0], 1038793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, intrinsicRight.f[1], intrinsicRight.c[1], 1039793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1); 1040793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1041793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat _R; 1042793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(omcur, _R); 1043793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1044793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K1.needed()) cv::Mat(_K1).convertTo(K1, K1.empty() ? CV_64FC1 : K1.type()); 1045793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (K2.needed()) cv::Mat(_K2).convertTo(K2, K2.empty() ? CV_64FC1 : K2.type()); 1046793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (D1.needed()) cv::Mat(intrinsicLeft.k).convertTo(D1, D1.empty() ? CV_64FC1 : D1.type()); 1047793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (D2.needed()) cv::Mat(intrinsicRight.k).convertTo(D2, D2.empty() ? CV_64FC1 : D2.type()); 1048793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (R.needed()) _R.convertTo(R, R.empty() ? CV_64FC1 : R.type()); 1049793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (T.needed()) cv::Mat(Tcur).convertTo(T, T.empty() ? CV_64FC1 : T.type()); 1050793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1051793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return rms; 1052793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1053793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1054793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslernamespace cv{ namespace { 1055793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid subMatrix(const Mat& src, Mat& dst, const std::vector<int>& cols, const std::vector<int>& rows) 1056793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1057793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(src.type() == CV_64FC1); 1058793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1059793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int nonzeros_cols = cv::countNonZero(cols); 1060793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat tmp(src.rows, nonzeros_cols, CV_64FC1); 1061793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1062793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0, j = 0; i < (int)cols.size(); i++) 1063793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1064793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (cols[i]) 1065793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1066793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler src.col(i).copyTo(tmp.col(j++)); 1067793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1068793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1069793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1070793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int nonzeros_rows = cv::countNonZero(rows); 1071793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1); 1072793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0, j = 0; i < (int)rows.size(); i++) 1073793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1074793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (rows[i]) 1075793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1076793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.row(i).copyTo(tmp1.row(j++)); 1077793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1078793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1079793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1080793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dst = tmp1.clone(); 1081793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1082793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1083793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler}} 1084793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1085793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::internal::IntrinsicParams::IntrinsicParams(): 1086793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f(Vec2d::all(0)), c(Vec2d::all(0)), k(Vec4d::all(0)), alpha(0), isEstimate(9,0) 1087793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1088793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1089793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1090793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::internal::IntrinsicParams::IntrinsicParams(Vec2d _f, Vec2d _c, Vec4d _k, double _alpha): 1091793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler f(_f), c(_c), k(_k), alpha(_alpha), isEstimate(9,0) 1092793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1093793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1094793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1095793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::internal::IntrinsicParams cv::internal::IntrinsicParams::operator+(const Mat& a) 1096793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1097793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(a.type() == CV_64FC1); 1098793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler IntrinsicParams tmp; 1099793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double* ptr = a.ptr<double>(); 1100793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1101793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int j = 0; 1102793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.f[0] = this->f[0] + (isEstimate[0] ? ptr[j++] : 0); 1103793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.f[1] = this->f[1] + (isEstimate[1] ? ptr[j++] : 0); 1104793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.c[0] = this->c[0] + (isEstimate[2] ? ptr[j++] : 0); 1105793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.alpha = this->alpha + (isEstimate[4] ? ptr[j++] : 0); 1106793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.c[1] = this->c[1] + (isEstimate[3] ? ptr[j++] : 0); 1107793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.k[0] = this->k[0] + (isEstimate[5] ? ptr[j++] : 0); 1108793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.k[1] = this->k[1] + (isEstimate[6] ? ptr[j++] : 0); 1109793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.k[2] = this->k[2] + (isEstimate[7] ? ptr[j++] : 0); 1110793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.k[3] = this->k[3] + (isEstimate[8] ? ptr[j++] : 0); 1111793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1112793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tmp.isEstimate = isEstimate; 1113793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return tmp; 1114793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1115793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1116793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::internal::IntrinsicParams& cv::internal::IntrinsicParams::operator =(const Mat& a) 1117793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1118793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(a.type() == CV_64FC1); 1119793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double* ptr = a.ptr<double>(); 1120793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1121793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int j = 0; 1122793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1123793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->f[0] = isEstimate[0] ? ptr[j++] : 0; 1124793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->f[1] = isEstimate[1] ? ptr[j++] : 0; 1125793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->c[0] = isEstimate[2] ? ptr[j++] : 0; 1126793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->c[1] = isEstimate[3] ? ptr[j++] : 0; 1127793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->alpha = isEstimate[4] ? ptr[j++] : 0; 1128793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->k[0] = isEstimate[5] ? ptr[j++] : 0; 1129793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->k[1] = isEstimate[6] ? ptr[j++] : 0; 1130793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->k[2] = isEstimate[7] ? ptr[j++] : 0; 1131793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->k[3] = isEstimate[8] ? ptr[j++] : 0; 1132793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1133793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return *this; 1134793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1135793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1136793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::IntrinsicParams::Init(const cv::Vec2d& _f, const cv::Vec2d& _c, const cv::Vec4d& _k, const double& _alpha) 1137793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1138793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->c = _c; 1139793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->f = _f; 1140793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->k = _k; 1141793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler this->alpha = _alpha; 1142793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1143793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1144793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::projectPoints(cv::InputArray objectPoints, cv::OutputArray imagePoints, 1145793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::InputArray _rvec,cv::InputArray _tvec, 1146793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const IntrinsicParams& param, cv::OutputArray jacobian) 1147793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1148793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); 1149793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Matx33d K(param.f[0], param.f[0] * param.alpha, param.c[0], 1150793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, param.f[1], param.c[1], 1151793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1); 1152793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler fisheye::projectPoints(objectPoints, imagePoints, _rvec, _tvec, K, param.k, param.alpha, jacobian); 1153793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1154793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1155793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::ComputeExtrinsicRefine(const Mat& imagePoints, const Mat& objectPoints, Mat& rvec, 1156793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat& tvec, Mat& J, const int MaxIter, 1157793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const IntrinsicParams& param, const double thresh_cond) 1158793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1159793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); 1160793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); 1161793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec6d extrinsics(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2), 1162793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2)); 1163793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double change = 1; 1164793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int iter = 0; 1165793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1166793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler while (change > 1e-10 && iter < MaxIter) 1167793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1168793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<Point2d> x; 1169793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat jacobians; 1170793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler projectPoints(objectPoints, x, rvec, tvec, param, jacobians); 1171793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1172793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat ex = imagePoints - Mat(x).t(); 1173793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ex = ex.reshape(1, 2); 1174793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1175793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J = jacobians.colRange(8, 14).clone(); 1176793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1177793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler SVD svd(J, SVD::NO_UV); 1178793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double condJJ = svd.w.at<double>(0)/svd.w.at<double>(5); 1179793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1180793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (condJJ > thresh_cond) 1181793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler change = 0; 1182793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 1183793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1184793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec6d param_innov; 1185793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler solve(J, ex.reshape(1, (int)ex.total()), param_innov, DECOMP_SVD + DECOMP_NORMAL); 1186793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1187793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec6d param_up = extrinsics + param_innov; 1188793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler change = norm(param_innov)/norm(param_up); 1189793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler extrinsics = param_up; 1190793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler iter = iter + 1; 1191793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1192793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rvec = Mat(Vec3d(extrinsics.val)); 1193793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler tvec = Mat(Vec3d(extrinsics.val+3)); 1194793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1195793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1196793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1197793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1198793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::Mat cv::internal::ComputeHomography(Mat m, Mat M) 1199793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1200793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int Np = m.cols; 1201793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1202793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (m.rows < 3) 1203793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1204793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler vconcat(m, Mat::ones(1, Np, CV_64FC1), m); 1205793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1206793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (M.rows < 3) 1207793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1208793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler vconcat(M, Mat::ones(1, Np, CV_64FC1), M); 1209793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1210793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1211793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler divide(m, Mat::ones(3, 1, CV_64FC1) * m.row(2), m); 1212793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler divide(M, Mat::ones(3, 1, CV_64FC1) * M.row(2), M); 1213793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1214793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat ax = m.row(0).clone(); 1215793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat ay = m.row(1).clone(); 1216793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1217793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double mxx = mean(ax)[0]; 1218793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double myy = mean(ay)[0]; 1219793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1220793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ax = ax - mxx; 1221793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ay = ay - myy; 1222793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1223793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double scxx = mean(abs(ax))[0]; 1224793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double scyy = mean(abs(ay))[0]; 1225793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1226793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat Hnorm (Matx33d( 1/scxx, 0.0, -mxx/scxx, 1227793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0.0, 1/scyy, -myy/scyy, 1228793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0.0, 0.0, 1.0 )); 1229793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1230793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat inv_Hnorm (Matx33d( scxx, 0, mxx, 1231793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, scyy, myy, 1232793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 0, 0, 1 )); 1233793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat mn = Hnorm * m; 1234793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1235793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat L = Mat::zeros(2*Np, 9, CV_64FC1); 1236793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1237793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0; i < Np; ++i) 1238793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1239793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int j = 0; j < 3; j++) 1240793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1241793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler L.at<double>(2 * i, j) = M.at<double>(j, i); 1242793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler L.at<double>(2 * i + 1, j + 3) = M.at<double>(j, i); 1243793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler L.at<double>(2 * i, j + 6) = -mn.at<double>(0,i) * M.at<double>(j, i); 1244793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler L.at<double>(2 * i + 1, j + 6) = -mn.at<double>(1,i) * M.at<double>(j, i); 1245793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1246793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1247793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1248793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (Np > 4) L = L.t() * L; 1249793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler SVD svd(L); 1250793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat hh = svd.vt.row(8) / svd.vt.row(8).at<double>(8); 1251793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat Hrem = hh.reshape(1, 3); 1252793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat H = inv_Hnorm * Hrem; 1253793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1254793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (Np > 4) 1255793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1256793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat hhv = H.reshape(1, 9)(Rect(0, 0, 1, 8)).clone(); 1257793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int iter = 0; iter < 10; iter++) 1258793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1259793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat mrep = H * M; 1260793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat J = Mat::zeros(2 * Np, 8, CV_64FC1); 1261793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat MMM; 1262793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), MMM); 1263793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler divide(mrep, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 2, mrep.cols, 1)), mrep); 1264793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat m_err = m(Rect(0,0, m.cols, 2)) - mrep(Rect(0,0, mrep.cols, 2)); 1265793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler m_err = Mat(m_err.t()).reshape(1, m_err.cols * m_err.rows); 1266793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat MMM2, MMM3; 1267793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 0, mrep.cols, 1)), MMM, MMM2); 1268793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler multiply(Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0, 1, mrep.cols, 1)), MMM, MMM3); 1269793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1270793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0; i < Np; ++i) 1271793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1272793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int j = 0; j < 3; ++j) 1273793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1274793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.at<double>(2 * i, j) = -MMM.at<double>(j, i); 1275793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.at<double>(2 * i + 1, j + 3) = -MMM.at<double>(j, i); 1276793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1277793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1278793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int j = 0; j < 2; ++j) 1279793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1280793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.at<double>(2 * i, j + 6) = MMM2.at<double>(j, i); 1281793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler J.at<double>(2 * i + 1, j + 6) = MMM3.at<double>(j, i); 1282793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1283793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1284793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler divide(M, Mat::ones(3, 1, CV_64FC1) * mrep(Rect(0,2,mrep.cols,1)), MMM); 1285793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat hh_innov = (J.t() * J).inv() * (J.t()) * m_err; 1286793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat hhv_up = hhv - hh_innov; 1287793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat tmp; 1288793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler vconcat(hhv_up, Mat::ones(1,1,CV_64FC1), tmp); 1289793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat H_up = tmp.reshape(1,3); 1290793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler hhv = hhv_up; 1291793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler H = H_up; 1292793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1293793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1294793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return H; 1295793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1296793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1297793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::Mat cv::internal::NormalizePixels(const Mat& imagePoints, const IntrinsicParams& param) 1298793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1299793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); 1300793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1301793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat distorted((int)imagePoints.total(), 1, CV_64FC2), undistorted; 1302793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec2d* ptr = imagePoints.ptr<Vec2d>(0); 1303793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Vec2d* ptr_d = distorted.ptr<Vec2d>(0); 1304793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (size_t i = 0; i < imagePoints.total(); ++i) 1305793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1306793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ptr_d[i] = (ptr[i] - param.c).mul(Vec2d(1.0 / param.f[0], 1.0 / param.f[1])); 1307793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ptr_d[i][0] = ptr_d[i][0] - param.alpha * ptr_d[i][1]; 1308793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1309793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler cv::fisheye::undistortPoints(distorted, undistorted, Matx33d::eye(), param.k); 1310793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return undistorted; 1311793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1312793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1313793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::InitExtrinsics(const Mat& _imagePoints, const Mat& _objectPoints, const IntrinsicParams& param, Mat& omckk, Mat& Tckk) 1314793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1315793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1316793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!_objectPoints.empty() && _objectPoints.type() == CV_64FC3); 1317793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!_imagePoints.empty() && _imagePoints.type() == CV_64FC2); 1318793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1319793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat imagePointsNormalized = NormalizePixels(_imagePoints.t(), param).reshape(1).t(); 1320793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat objectPoints = Mat(_objectPoints.t()).reshape(1).t(); 1321793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat objectPointsMean, covObjectPoints; 1322793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat Rckk; 1323793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int Np = imagePointsNormalized.cols; 1324793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler calcCovarMatrix(objectPoints, covObjectPoints, objectPointsMean, COVAR_NORMAL | COVAR_COLS); 1325793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler SVD svd(covObjectPoints); 1326793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat R(svd.vt); 1327793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (norm(R(Rect(2, 0, 1, 2))) < 1e-6) 1328793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R = Mat::eye(3,3, CV_64FC1); 1329793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (determinant(R) < 0) 1330793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R = -R; 1331793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat T = -R * objectPointsMean; 1332793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat X_new = R * objectPoints + T * Mat::ones(1, Np, CV_64FC1); 1333793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat H = ComputeHomography(imagePointsNormalized, X_new(Rect(0,0,X_new.cols,2))); 1334793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double sc = .5 * (norm(H.col(0)) + norm(H.col(1))); 1335793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler H = H / sc; 1336793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat u1 = H.col(0).clone(); 1337793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler u1 = u1 / norm(u1); 1338793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat u2 = H.col(1).clone() - u1.dot(H.col(1).clone()) * u1; 1339793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler u2 = u2 / norm(u2); 1340793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat u3 = u1.cross(u2); 1341793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat RRR; 1342793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler hconcat(u1, u2, RRR); 1343793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler hconcat(RRR, u3, RRR); 1344793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(RRR, omckk); 1345793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(omckk, Rckk); 1346793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Tckk = H.col(2).clone(); 1347793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Tckk = Tckk + Rckk * T; 1348793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rckk = Rckk * R; 1349793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(Rckk, omckk); 1350793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1351793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1352793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::CalibrateExtrinsics(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, 1353793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const IntrinsicParams& param, const int check_cond, 1354793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const double thresh_cond, InputOutputArray omc, InputOutputArray Tc) 1355793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1356793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); 1357793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); 1358793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(omc.type() == CV_64FC3 || Tc.type() == CV_64FC3); 1359793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1360793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (omc.empty()) omc.create(1, (int)objectPoints.total(), CV_64FC3); 1361793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (Tc.empty()) Tc.create(1, (int)objectPoints.total(), CV_64FC3); 1362793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1363793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int maxIter = 20; 1364793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1365793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for(int image_idx = 0; image_idx < (int)imagePoints.total(); ++image_idx) 1366793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1367793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat omckk, Tckk, JJ_kk; 1368793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat image, object; 1369793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1370793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); 1371793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); 1372793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1373793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler InitExtrinsics(image, object, param, omckk, Tckk); 1374793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1375793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ComputeExtrinsicRefine(image, object, omckk, Tckk, JJ_kk, maxIter, param, thresh_cond); 1376793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (check_cond) 1377793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1378793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler SVD svd(JJ_kk, SVD::NO_UV); 1379793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(svd.w.at<double>(0) / svd.w.at<double>((int)svd.w.total() - 1) < thresh_cond); 1380793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1381793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler omckk.reshape(3,1).copyTo(omc.getMat().col(image_idx)); 1382793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Tckk.reshape(3,1).copyTo(Tc.getMat().col(image_idx)); 1383793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1384793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1385793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1386793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1387793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::ComputeJacobians(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, 1388793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const IntrinsicParams& param, InputArray omc, InputArray Tc, 1389793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const int& check_cond, const double& thresh_cond, Mat& JJ2_inv, Mat& ex3) 1390793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1391793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); 1392793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); 1393793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1394793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!omc.empty() && omc.type() == CV_64FC3); 1395793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3); 1396793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1397793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int n = (int)objectPoints.total(); 1398793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1399793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat JJ3 = Mat::zeros(9 + 6 * n, 9 + 6 * n, CV_64FC1); 1400793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ex3 = Mat::zeros(9 + 6 * n, 1, CV_64FC1 ); 1401793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1402793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int image_idx = 0; image_idx < n; ++image_idx) 1403793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1404793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat image, object; 1405793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); 1406793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); 1407793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1408793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx)); 1409793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1410793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<Point2d> x; 1411793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat jacobians; 1412793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler projectPoints(object, x, om, T, param, jacobians); 1413793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat exkk = image.t() - Mat(x); 1414793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1415793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat A(jacobians.rows, 9, CV_64FC1); 1416793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(0, 4).copyTo(A.colRange(0, 4)); 1417793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.col(14).copyTo(A.col(4)); 1418793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler jacobians.colRange(4, 8).copyTo(A.colRange(5, 9)); 1419793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1420793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler A = A.t(); 1421793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1422793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat B = jacobians.colRange(8, 14).clone(); 1423793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler B = B.t(); 1424793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1425793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JJ3(Rect(0, 0, 9, 9)) = JJ3(Rect(0, 0, 9, 9)) + A * A.t(); 1426793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JJ3(Rect(9 + 6 * image_idx, 9 + 6 * image_idx, 6, 6)) = B * B.t(); 1427793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1428793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat AB = A * B.t(); 1429793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler AB.copyTo(JJ3(Rect(9 + 6 * image_idx, 0, 6, 9))); 1430793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1431793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JJ3(Rect(0, 9 + 6 * image_idx, 9, 6)) = AB.t(); 1432793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ex3(Rect(0,0,1,9)) = ex3(Rect(0,0,1,9)) + A * exkk.reshape(1, 2 * exkk.rows); 1433793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1434793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ex3(Rect(0, 9 + 6 * image_idx, 1, 6)) = B * exkk.reshape(1, 2 * exkk.rows); 1435793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1436793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (check_cond) 1437793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1438793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat JJ_kk = B.t(); 1439793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler SVD svd(JJ_kk, SVD::NO_UV); 1440793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(svd.w.at<double>(0) / svd.w.at<double>(svd.w.rows - 1) < thresh_cond); 1441793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1442793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1443793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1444793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<int> idxs(param.isEstimate); 1445793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler idxs.insert(idxs.end(), 6 * n, 1); 1446793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1447793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler subMatrix(JJ3, JJ3, idxs, idxs); 1448793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler subMatrix(ex3, ex3, std::vector<int>(1, 1), idxs); 1449793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JJ2_inv = JJ3.inv(); 1450793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1451793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1452793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::EstimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, 1453793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const IntrinsicParams& params, InputArray omc, InputArray Tc, 1454793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler IntrinsicParams& errors, Vec2d& std_err, double thresh_cond, int check_cond, double& rms) 1455793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1456793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!objectPoints.empty() && (objectPoints.type() == CV_32FC3 || objectPoints.type() == CV_64FC3)); 1457793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!imagePoints.empty() && (imagePoints.type() == CV_32FC2 || imagePoints.type() == CV_64FC2)); 1458793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1459793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!omc.empty() && omc.type() == CV_64FC3); 1460793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!Tc.empty() && Tc.type() == CV_64FC3); 1461793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1462793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat ex((int)(objectPoints.getMat(0).total() * objectPoints.total()), 1, CV_64FC2); 1463793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1464793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int image_idx = 0; image_idx < (int)objectPoints.total(); ++image_idx) 1465793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1466793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat image, object; 1467793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler objectPoints.getMat(image_idx).convertTo(object, CV_64FC3); 1468793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler imagePoints.getMat (image_idx).convertTo(image, CV_64FC2); 1469793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1470793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat om(omc.getMat().col(image_idx)), T(Tc.getMat().col(image_idx)); 1471793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1472793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std::vector<Point2d> x; 1473793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler projectPoints(object, x, om, T, params, noArray()); 1474793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat ex_ = image.t() - Mat(x); 1475793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ex_.copyTo(ex.rowRange(ex_.rows * image_idx, ex_.rows * (image_idx + 1))); 1476793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1477793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1478793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler meanStdDev(ex, noArray(), std_err); 1479793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler std_err *= sqrt((double)ex.total()/((double)ex.total() - 1.0)); 1480793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1481793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat sigma_x; 1482793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler meanStdDev(ex.reshape(1, 1), noArray(), sigma_x); 1483793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler sigma_x *= sqrt(2.0 * (double)ex.total()/(2.0 * (double)ex.total() - 1.0)); 1484793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1485793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat _JJ2_inv, ex3; 1486793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler ComputeJacobians(objectPoints, imagePoints, params, omc, Tc, check_cond, thresh_cond, _JJ2_inv, ex3); 1487793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1488793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat_<double>& JJ2_inv = (Mat_<double>&)_JJ2_inv; 1489793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1490793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler sqrt(JJ2_inv, JJ2_inv); 1491793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1492793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler double s = sigma_x.at<double>(0); 1493793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat r = 3 * s * JJ2_inv.diag(); 1494793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler errors = r; 1495793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1496793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms = 0; 1497793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler const Vec2d* ptr_ex = ex.ptr<Vec2d>(); 1498793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (size_t i = 0; i < ex.total(); i++) 1499793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1500793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1]; 1501793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1502793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1503793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms /= (double)ex.total(); 1504793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler rms = sqrt(rms); 1505793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1506793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1507793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::dAB(InputArray A, InputArray B, OutputArray dABdA, OutputArray dABdB) 1508793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1509793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(A.getMat().cols == B.getMat().rows); 1510793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(A.type() == CV_64FC1 && B.type() == CV_64FC1); 1511793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1512793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int p = A.getMat().rows; 1513793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int n = A.getMat().cols; 1514793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int q = B.getMat().cols; 1515793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1516793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dABdA.create(p * q, p * n, CV_64FC1); 1517793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dABdB.create(p * q, q * n, CV_64FC1); 1518793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1519793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dABdA.getMat() = Mat::zeros(p * q, p * n, CV_64FC1); 1520793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dABdB.getMat() = Mat::zeros(p * q, q * n, CV_64FC1); 1521793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1522793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0; i < q; ++i) 1523793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1524793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int j = 0; j < p; ++j) 1525793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1526793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int ij = j + i * p; 1527793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int k = 0; k < n; ++k) 1528793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1529793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler int kj = j + k * p; 1530793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dABdA.getMat().at<double>(ij, kj) = B.getMat().at<double>(k, i); 1531793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1532793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1533793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1534793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1535793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler for (int i = 0; i < q; ++i) 1536793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1537793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler A.getMat().copyTo(dABdB.getMat().rowRange(i * p, i * p + p).colRange(i * n, i * n + n)); 1538793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1539793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1540793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1541793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::JRodriguesMatlab(const Mat& src, Mat& dst) 1542793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1543793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat tmp(src.cols, src.rows, src.type()); 1544793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if (src.rows == 9) 1545793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1546793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(0).t()).copyTo(tmp.col(0)); 1547793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(1).t()).copyTo(tmp.col(3)); 1548793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(2).t()).copyTo(tmp.col(6)); 1549793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(3).t()).copyTo(tmp.col(1)); 1550793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(4).t()).copyTo(tmp.col(4)); 1551793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(5).t()).copyTo(tmp.col(7)); 1552793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(6).t()).copyTo(tmp.col(2)); 1553793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(7).t()).copyTo(tmp.col(5)); 1554793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.row(8).t()).copyTo(tmp.col(8)); 1555793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1556793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else 1557793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler { 1558793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(0).t()).copyTo(tmp.row(0)); 1559793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(1).t()).copyTo(tmp.row(3)); 1560793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(2).t()).copyTo(tmp.row(6)); 1561793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(3).t()).copyTo(tmp.row(1)); 1562793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(4).t()).copyTo(tmp.row(4)); 1563793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(5).t()).copyTo(tmp.row(7)); 1564793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(6).t()).copyTo(tmp.row(2)); 1565793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(7).t()).copyTo(tmp.row(5)); 1566793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat(src.col(8).t()).copyTo(tmp.row(8)); 1567793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler } 1568793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dst = tmp.clone(); 1569793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1570793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1571793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslervoid cv::internal::compose_motion(InputArray _om1, InputArray _T1, InputArray _om2, InputArray _T2, 1572793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat& om3, Mat& T3, Mat& dom3dom1, Mat& dom3dT1, Mat& dom3dom2, 1573793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat& dom3dT2, Mat& dT3dom1, Mat& dT3dT1, Mat& dT3dom2, Mat& dT3dT2) 1574793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1575793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat om1 = _om1.getMat(); 1576793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat om2 = _om2.getMat(); 1577793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat T1 = _T1.getMat().reshape(1, 3); 1578793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat T2 = _T2.getMat().reshape(1, 3); 1579793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1580793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //% Rotations: 1581793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat R1, R2, R3, dR1dom1(9, 3, CV_64FC1), dR2dom2; 1582793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(om1, R1, dR1dom1); 1583793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(om2, R2, dR2dom2); 1584793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JRodriguesMatlab(dR1dom1, dR1dom1); 1585793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JRodriguesMatlab(dR2dom2, dR2dom2); 1586793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler R3 = R2 * R1; 1587793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat dR3dR2, dR3dR1; 1588793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dAB(R2, R1, dR3dR2, dR3dR1); 1589793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat dom3dR3; 1590793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Rodrigues(R3, om3, dom3dR3); 1591793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler JRodriguesMatlab(dom3dR3, dom3dR3); 1592793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; 1593793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; 1594793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dom3dT1 = Mat::zeros(3, 3, CV_64FC1); 1595793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dom3dT2 = Mat::zeros(3, 3, CV_64FC1); 1596793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1597793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler //% Translations: 1598793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat T3t = R2 * T1; 1599793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat dT3tdR2, dT3tdT1; 1600793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dAB(R2, T1, dT3tdR2, dT3tdT1); 1601793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat dT3tdom2 = dT3tdR2 * dR2dom2; 1602793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler T3 = T3t + T2; 1603793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dT3dT1 = dT3tdT1; 1604793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dT3dT2 = Mat::eye(3, 3, CV_64FC1); 1605793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dT3dom2 = dT3tdom2; 1606793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler dT3dom1 = Mat::zeros(3, 3, CV_64FC1); 1607793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1608793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1609793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslerdouble cv::internal::median(const Mat& row) 1610793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1611793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(row.type() == CV_64FC1); 1612793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(!row.empty() && row.rows == 1); 1613793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat tmp = row.clone(); 1614793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler sort(tmp, tmp, 0); 1615793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler if ((int)tmp.total() % 2) return tmp.at<double>((int)tmp.total() / 2); 1616793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler else return 0.5 *(tmp.at<double>((int)tmp.total() / 2) + tmp.at<double>((int)tmp.total() / 2 - 1)); 1617793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1618793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler 1619793ee12c6df9cad3806238d32528c49a3ff9331dNoah Preslercv::Vec3d cv::internal::median3d(InputArray m) 1620793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler{ 1621793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler CV_Assert(m.depth() == CV_64F && m.getMat().rows == 1); 1622793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler Mat M = Mat(m.getMat().t()).reshape(1).t(); 1623793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler return Vec3d(median(M.row(0)), median(M.row(1)), median(M.row(2))); 1624793ee12c6df9cad3806238d32528c49a3ff9331dNoah Presler} 1625