diff --git a/modules/ccalib/include/opencv2/omnidir.hpp b/modules/ccalib/include/opencv2/omnidir.hpp new file mode 100644 index 000000000..a5a886f41 --- /dev/null +++ b/modules/ccalib/include/opencv2/omnidir.hpp @@ -0,0 +1,179 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University, +// all rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ +#ifndef __OPENCV_OMNIDIR_HPP__ +#define __OPENCV_OMNIDIR_HPP__ +#ifdef __cplusplus + +#include +#include +#include +#include +#include +namespace cv +{ + +/* @defgroup calib3d_omnidir Omnidirectional camera model + +*/ +/** @brief The methods in this namespace is to calibrate omnidirectional cameras. + This module was accepted as a GSoC 2015 project for OpenCV, authored by + Baisheng Lai, mentored by Bo Li. + @ingroup calib3d_omnidir +*/ +namespace omnidir +{ + //! @addtogroup calib3d_omnidir + //! @{ + enum { + CALIB_USE_GUESS = 1, + CALIB_FIX_SKEW = 2, + CALIB_FIX_K1 = 4, + CALIB_FIX_K2 = 8, + CALIB_FIX_P1 = 16, + CALIB_FIX_P2 = 32, + CALIB_FIX_XI = 64, + CALIB_FIX_GAMMA = 128, + CALIB_FIX_CENTER = 256 + }; + + /** @brief Projects points for omnidirectional camera using CMei's model + + @param objectPoints Object points in world coordiante, 1xN/Nx1 3-channel of type CV_64F and N + is the number of points. + @param imagePoints Output array of image points, 1xN/Nx1 2-channel of type CV_64F + @param rvec vector of rotation between world coordinate and camera coordinate, i.e., om + @param tvec vector of translation between pattern coordinate and camera coordinate + @param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. + @param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$. + @param xi The parameter xi for CMei's model + @param jacobian Optional output 2Nx16 of type CV_64F jacobian matrix, constains the derivatives of + image pixel points wrt parametes including \f$om, T, f_x, f_y, s, c_x, c_y, xi, k_1, k_2, p_1, p_2\f$. + This matrix will be used in calibration by optimization. + + The function projects object 3D points of world coordiante to image pixels, parametered by intrinsic + and extrinsic parameters. Also, it optionaly compute a by-product: the jacobian matrix containing + onstains the derivatives of image pixel points wrt intrinsic and extrinsic parametes. + */ + CV_EXPORTS_W void projectPoints(InputArray objectPoints, OutputArray imagePoints, InputArray rvec, InputArray tvec, + InputArray K, double xi, InputArray D, OutputArray jacobian = noArray()); + + /** @brief Undistort 2D image points for omnidirectional camera using CMei's model + + @param distorted Array of distorted image points, 1xN/Nx1 2-channel of tyep CV_64F + @param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. + @param D Distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$. + @param xi The parameter xi for CMei's model + @param R Rotation trainsform between the original and object space : 3x3 1-channel, or vector: 3x1/1x3 + 1-channel or 1x1 3-channel + @param undistorted array of normalized object points, 1xN/Nx1 2-channel of type CV_64F + */ + + CV_EXPORTS_W void undistortPoints(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, double xi, InputArray R); + + /** @brief Computes undistortion and rectification maps for omnidirectional camera image transform by cv::remap(). + If D is empty zero distortion is used, if R or P is empty identity matrixes are used. + + @param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. + @param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$. + @param xi The parameter xi for CMei's model + @param R Rotation trainsform between the original and object space : 3x3 1-channel, or vector: 3x1/1x3 + @param P New camera matrix (3x3) or new projection matrix (3x4) + @param size Undistorted image size. + @param mltype Type of the first output map that can be CV_32FC1 or CV_16SC2 . See convertMaps() + for details. + @param map1 The first output map. + @param map2 The second output map. + */ + CV_EXPORTS_W void initUndistortRectifyMap(InputArray K, InputArray D, double xi, InputArray R, InputArray P, const cv::Size& size, + int mltype, OutputArray map1, OutputArray map2); + + /** @brief Undistort omnidirectional images to perspective images + + @param distorted omnidirectional image with very large distortion + @param undistorted The output undistorted image + @param K Camera matrix \f$K = \vecthreethree{f_x}{s}{c_x}{0}{f_y}{c_y}{0}{0}{_1}\f$. + @param D Input vector of distortion coefficients \f$(k_1, k_2, p_1, p_2)\f$. + @param xi The parameter xi for CMei's model + @param Knew Camera matrix of the distorted image. By default, it is just K. + @param new_size The new image size. By default, it is the size of distorted. + */ + CV_EXPORTS_W void undistortImage(InputArray distorted, OutputArray undistorted, InputArray K, InputArray D, double xi, + InputArray Knew = cv::noArray(), const Size& new_size = Size()); + + /** @brief Perform omnidirectional camera calibration + + @param patternPoints Vector of vector of pattern points in world (pattern) coordiante, 1xN/Nx1 3-channel + @param imagePoints Vector of vector of correspoinding image points of objectPoints + @param size Image size of calibration images. + @param K Output calibrated camera matrix. If you want to initialize K by yourself, input a non-empty K. + @param xi Ouput parameter xi for CMei's model + @param D Output distortion parameters \f$(k_1, k_2, p_1, p_2)\f$ + @param omAll Output rotations for each calibration images + @param tAll Output translation for each calibration images + @param flags The flags that control calibrate + @param criteria Termination criteria for optimization + */ + CV_EXPORTS_W double calibrate(InputOutputArrayOfArrays patternPoints, InputOutputArrayOfArrays imagePoints, Size size, + InputOutputArray K, InputOutputArray xi, InputOutputArray D, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, + int flags, TermCriteria criteria); + + +//! @} calib3d_omnidir +namespace internal +{ + void initializeCalibration(InputOutputArrayOfArrays objectPoints, InputOutputArrayOfArrays imagePoints, Size size, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray K, double& xi); + void computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags); + void encodeParameters(InputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, InputArray distoaration, double xi, int n, OutputArray parameters); + void decodeParameters(InputArray paramsters, OutputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray distoration, double& xi); + void estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, Mat& errors, Vec2d& std_error, double& rms, int flags); + double computeMeanReproerr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints); + void checkFixed(Mat &G, int flags, int n); + void subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows); + void flags2idx(int flags, std::vector& idx, int n); + void fillFixed(Mat&G, int flags, int n); +} // internal + + +} // omnidir + +} //cv +#endif +#endif \ No newline at end of file diff --git a/modules/ccalib/src/omnidir.cpp b/modules/ccalib/src/omnidir.cpp new file mode 100644 index 000000000..2a065aa8f --- /dev/null +++ b/modules/ccalib/src/omnidir.cpp @@ -0,0 +1,1022 @@ +/*M/////////////////////////////////////////////////////////////////////////////////////// +// +// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. +// +// By downloading, copying, installing or using the software you agree to this license. +// If you do not agree to this license, do not download, install, +// copy or use the software. +// +// +// License Agreement +// For Open Source Computer Vision Library +// +// Copyright (C) 2015, Baisheng Lai (laibaisheng@gmail.com), Zhejiang University, +// all rights reserved. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. +// +//M*/ + +/** + * This module was accepted as a GSoC 2015 project for OpenCV, authored by + * Baisheng Lai, mentored by Bo Li. + * + * The omnidirectional camera in this module is denoted by the catadioptric + * model. Please refer to Mei's paper for details of the camera model: + * + * C. Mei and P. Rives, “Single view point omnidirectional camera + * calibration from planar grids,â€?in ICRA 2007. + * + * The implementation of the calibration part is based on Li's calibration + * toolbox: + * + * B. Li, L. Heng, K. Köser and M. Pollefeys, "A Multiple-Camera System + * Calibration Toolbox Using A Feature Descriptor-Based Calibration + * Pattern", in IROS 2013. + */ +#ifndef __OPENCV_OMNIDIR_CPP__ +#define __OPENCV_OMNIDIR_CPP__ +#ifdef __cplusplus +#include "precomp.hpp" +#include "opencv2/omnidir.hpp" + +namespace cv { namespace +{ + struct JacobianRow + { + Matx13d dom,dT; + Matx12d df; + double ds; + Matx12d dc; + double dxi; + Matx14d dkp; // distortion k1,k2,p1,p2 + }; +}} + +///////////////////////////////////////////////////////////////////////////// +//////// projectPoints +void cv::omnidir::projectPoints(InputArray objectPoints, OutputArray imagePoints, + InputArray rvec, InputArray tvec, InputArray K, double xi, InputArray D, OutputArray jacobian) +{ + + CV_Assert(objectPoints.type() == CV_64FC3); + CV_Assert(rvec.type() == CV_64F && rvec.total() == 3); + CV_Assert(tvec.type() == CV_64F && tvec.total() == 3); + CV_Assert(K.type() == CV_64F && K.size() == Size(3,3)); + CV_Assert(D.type() == CV_64F && D.total() == 4); + // each row is an image point + imagePoints.create(objectPoints.size(), CV_MAKETYPE(objectPoints.depth(), 2)); + int n = (int)objectPoints.total(); + Vec3d om = *rvec.getMat().ptr(); + Vec3d T = *tvec.getMat().ptr(); + Matx33d Kc = K.getMat(); + Vec kp= (Vec)*D.getMat().ptr >(); + + Vec2d f,c; + + f = Vec2d(Kc(0,0),Kc(1,1)); + c = Vec2d(Kc(0,2),Kc(1,2)); + double s = Kc(0,1); + const Vec3d* Xw_all = objectPoints.getMat().ptr(); + Vec2d* xpd = imagePoints.getMat().ptr(); + + Matx33d R; + Matx dRdom; + Rodrigues(om, R, dRdom); + + JacobianRow *Jn = 0; + if (jacobian.needed()) + { + int nvars = 2+2+1+4+3+3+1; // f,c,s,kp,om,T,xi + jacobian.create(2*int(n), nvars, CV_64F); + Jn = jacobian.getMat().ptr(0); + } + + double k1=kp[0],k2=kp[1]; + double p1 = kp[2], p2 = kp[3]; + + for (int i = 0; i < n; i++) + { + // convert to camera coordinate + Vec3d Xw = (Vec3d)Xw_all[i]; + + Vec3d Xc = (Vec3d)(R*Xw + T); + + // convert to unit sphere + Vec3d Xs = Xc/cv::norm(Xc); + + // convert to normalized image plane + Vec2d xu = Vec2d(Xs[0]/(Xs[2]+xi), Xs[1]/(Xs[2]+xi)); + + // add distortion + Vec2d xd; + double r2 = xu[0]*xu[0]+xu[1]*xu[1]; + double r4 = r2*r2; + + xd[0] = xu[0]*(1+k1*r2+k2*r4) + 2*p1*xu[0]*xu[1] + p2*(r2+2*xu[0]*xu[0]); + xd[1] = xu[1]*(1+k1*r2+k2*r4) + p1*(r2+2*xu[1]*xu[1]) + 2*p2*xu[0]*xu[1]; + + // convert to pixel coordinate + xpd[i][0] = f[0]*xd[0]+s*xd[1]+c[0]; + xpd[i][1] = f[1]*xd[1]+c[1]; + + if (jacobian.needed()) + { + double dXcdR_a[] = {Xw[0],Xw[1],Xw[2],0,0,0,0,0,0, + 0,0,0,Xw[0],Xw[1],Xw[2],0,0,0, + 0,0,0,0,0,0,Xw[0],Xw[1],Xw[2]}; + Matx dXcdR(dXcdR_a); + Matx33d dXcdom = dXcdR * dRdom.t(); + double r_1 = 1.0/norm(Xc); + double r_3 = pow(r_1,3); + Matx33d dXsdXc(r_1-Xc[0]*Xc[0]*r_3, -(Xc[0]*Xc[1])*r_3, -(Xc[0]*Xc[2])*r_3, + -(Xc[0]*Xc[1])*r_3, r_1-Xc[1]*Xc[1]*r_3, -(Xc[1]*Xc[2])*r_3, + -(Xc[0]*Xc[2])*r_3, -(Xc[1]*Xc[2])*r_3, r_1-Xc[2]*Xc[2]*r_3); + Matx23d dxudXs(1/(Xs[2]+xi), 0, -Xs[0]/(Xs[2]+xi)/(Xs[2]+xi), + 0, 1/(Xs[2]+xi), -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi)); + // pre-compute some reusable things + double temp1 = 2*k1*xu[0] + 4*k2*xu[0]*r2; + double temp2 = 2*k1*xu[1] + 4*k2*xu[1]*r2; + Matx22d dxddxu(k2*r4+6*p2*xu[0]+2*p1*xu[1]+xu[0]*temp1+k1*r2+1, 2*p1*xu[0]+2*p2*xu[1]+xu[0]*temp2, + 2*p1*xu[0]+2*p2*xu[1]+xu[1]*temp1, k2*r4+2*p2*xu[0]+6*p1*xu[1]+xu[1]*temp2+k1*r2+1); + Matx22d dxpddxd(f[0], s, + 0, f[1]); + Matx23d dxpddXc = dxpddxd * dxddxu * dxudXs * dXsdXc; + + // derivative of xpd respect to om + Matx23d dxpddom = dxpddXc * dXcdom; + Matx33d dXcdT(1.0,0.0,0.0, + 0.0,1.0,0.0, + 0.0,0.0,1.0); + // derivative of xpd respect to T + + Matx23d dxpddT = dxpddXc * dXcdT; + Matx21d dxudxi(-Xs[0]/(Xs[2]+xi)/(Xs[2]+xi), + -Xs[1]/(Xs[2]+xi)/(Xs[2]+xi)); + + // derivative of xpd respect to xi + Matx21d dxpddxi = dxpddxd * dxddxu * dxudxi; + Matx dxddkp(xu[0]*r2, xu[0]*r4, 2*xu[0]*xu[1], r2+2*xu[0]*xu[0], + xu[1]*r2, xu[1]*r4, r2+2*xu[1]*xu[1], 2*xu[0]*xu[1]); + + // derivative of xpd respect to kp + Matx dxpddkp = dxpddxd * dxddkp; + + // derivative of xpd respect to f + Matx22d dxpddf(xd[0], 0, + 0, xd[1]); + + // derivative of xpd respect to c + Matx22d dxpddc(1, 0, + 0, 1); + + Jn[0].dom = dxpddom.row(0); + Jn[1].dom = dxpddom.row(1); + Jn[0].dT = dxpddT.row(0); + Jn[1].dT = dxpddT.row(1); + Jn[0].dkp = dxpddkp.row(0); + Jn[1].dkp = dxpddkp.row(1); + Jn[0].dxi = dxpddxi(0,0); + Jn[1].dxi = dxpddxi(1,0); + Jn[0].df = dxpddf.row(0); + Jn[1].df = dxpddf.row(1); + Jn[0].dc = dxpddc.row(0); + Jn[1].dc = dxpddc.row(1); + Jn[0].ds = xd[1]; + Jn[1].ds = 0; + Jn += 2; + } + } +} + +///////////////////////////////////////////////////////////////////////////// +//////// undistortPoints +void cv::omnidir::undistortPoints( InputArray distorted, OutputArray undistorted, + InputArray K, InputArray D, double xi, InputArray R) +{ + CV_Assert(distorted.type() == CV_64FC2); + undistorted.create(distorted.size(), distorted.type()); + + CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); + CV_Assert(D.type() == CV_64F && D.total() == 4 && K.size() == Size(3, 3) && K.depth() == CV_64F); + + cv::Vec2d f, c; + Matx33d camMat = K.getMat(); + f = cv::Vec2d(camMat(0,0), camMat(1,1)); + c = cv::Vec2d(camMat(0,2), camMat(1,2)); + double s = camMat(0,1); + Vec4d kp = (Vec4d)*D.getMat().ptr(); + Vec2d k = Vec2d(kp[0], kp[1]); + Vec2d p = Vec2d(kp[2], kp[3]); + + cv::Matx33d RR = cv::Matx33d::eye(); + // R is om + if(!R.empty() && R.total()*R.channels() == 3) + { + cv::Vec3d rvec; + R.getMat().convertTo(rvec, CV_64F); + cv::Rodrigues(rvec, RR); + } + else if (!R.empty() && R.size() == Size(3,3)) + { + R.getMat().convertTo(RR, CV_64F); + } + + const cv::Vec2d *srcd = distorted.getMat().ptr(); + cv::Vec2d *dstd = undistorted.getMat().ptr(); + + int n = (int)distorted.total(); + for (int i = 0; i < n; i++) + { + Vec2d pi = (Vec2d)srcd[i]; // image point + Vec2d pp((pi[0]*f[1]-c[0]*f[1]-s*(pi[1]-c[1]))/(f[0]*f[1]), (pi[1]-c[1])/f[1]); //plane + Vec2d pu = pp; // points without distortion + + // remove distortion iteratively + for (int j = 0; j < 20; j++) + { + double r2 = pu[0]*pu[0] + pu[1]*pu[1]; + double r4 = r2*r2; + pu[0] = (pp[0] - 2*p[0]*pu[0]*pu[1] - p[1]*(r2+2*pu[0]*pu[0])) / (1 + k[0]*r2 + k[1]*r4); + pu[1] = (pp[1] - 2*p[1]*pu[0]*pu[1] - p[0]*(r2+2*pu[1]*pu[1])) / (1 + k[0]*r2 + k[1]*r4); + } + + // project to unit sphere + double r2 = pu[0]*pu[0] + pu[1]*pu[1]; + double a = (r2 + 1); + double b = 2*xi*r2; + double cc = r2*xi*xi-1; + double Zs = (-b + sqrt(b*b - 4*a*cc))/(2*a); + Vec3d Xw = Vec3d(pu[0]*(Zs + xi), pu[1]*(Zs +xi), Zs); + + // rotate + Xw = RR * Xw; + + // project back to sphere + Vec3d Xs = Xw / cv::norm(Xw); + + // reproject to camera plane + Vec3d ppu = Vec3d(Xs[0]/(Xs[2]+xi), Xs[1]/(Xs[2]+xi), 1.0); + + dstd[i] = Vec2d(ppu[0], ppu[1]); + } +} + + +///////////////////////////////////////////////////////////////////////////// +//////// cv::omnidir::initUndistortRectifyMap +void cv::omnidir::initUndistortRectifyMap(InputArray K, InputArray D, double xi, InputArray R, InputArray P, + const cv::Size& size, int m1type, OutputArray map1, OutputArray map2) +{ + CV_Assert( m1type == CV_16SC2 || m1type == CV_32F || m1type <=0 ); + map1.create( size, m1type <= 0 ? CV_16SC2 : m1type ); + map2.create( size, map1.type() == CV_16SC2 ? CV_16UC1 : CV_32F ); + + CV_Assert((K.depth() == CV_32F || K.depth() == CV_64F) && (D.depth() == CV_32F || D.depth() == CV_64F)); + CV_Assert(K.size() == Size(3, 3) && (D.empty() || D.total() == 4)); + CV_Assert(P.empty()|| (P.depth() == CV_32F || P.depth() == CV_64F)); + CV_Assert(P.empty() || P.size() == Size(3, 3) || P.size() == Size(4, 3)); + CV_Assert(R.empty() || (R.depth() == CV_32F || R.depth() == CV_64F)); + CV_Assert(R.empty() || R.size() == Size(3, 3) || R.total() * R.channels() == 3); + + cv::Vec2d f, c; + double s; + if (K.depth() == CV_32F) + { + Matx33f camMat = K.getMat(); + f = Vec2f(camMat(0, 0), camMat(1, 1)); + c = Vec2f(camMat(0, 2), camMat(1, 2)); + s = camMat(0,1); + } + else + { + Matx33d camMat = K.getMat(); + f = Vec2d(camMat(0, 0), camMat(1, 1)); + c = Vec2d(camMat(0, 2), camMat(1, 2)); + s = camMat(0,1); + } + + Vec4d kp = Vec4d::all(0); + if (!D.empty()) + kp = D.depth() == CV_32F ? (Vec4d)*D.getMat().ptr(): *D.getMat().ptr(); + Vec2d k = Vec2d(kp[0], kp[1]); + Vec2d p = Vec2d(kp[2], kp[3]); + cv::Matx33d RR = cv::Matx33d::eye(); + if (!R.empty() && R.total() * R.channels() == 3) + { + cv::Vec3d rvec; + R.getMat().convertTo(rvec, CV_64F); + cv::Rodrigues(rvec, RR); + } + else if (!R.empty() && R.size() == Size(3, 3)) + R.getMat().convertTo(RR, CV_64F); + + cv::Matx33d PP = cv::Matx33d::eye(); + if (!P.empty()) + P.getMat().colRange(0, 3).convertTo(PP, CV_64F); + else + PP = K.getMat(); + + cv::Matx33d iR = (PP*RR).inv(cv::DECOMP_SVD); + + // so far it is undistorted to perspective image + for (int i = 0; i < size.height; ++i) + { + float* m1f = map1.getMat().ptr(i); + float* m2f = map2.getMat().ptr(i); + short* m1 = (short*)m1f; + ushort* m2 = (ushort*)m2f; + + double _x = i*iR(0, 1) + iR(0, 2), + _y = i*iR(1, 1) + iR(1, 2), + _w = i*iR(2, 1) + iR(2, 2); + for(int j = 0; j < size.width; ++j, _x+=iR(0,0), _y+=iR(1,0), _w+=iR(2,0)) + { + // project back to unit sphere + double r = sqrt(_x*_x + _y*_y + _w*_w); + double Xs = _x / r; + double Ys = _y / r; + double Zs = _w / r; + // project to image plane + double xu = Xs / (Zs + xi), + yu = Ys / (Zs + xi); + // add distortion + double r2 = xu*xu + yu*yu; + double r4 = r2*r2; + double xd = (1+k[0]*r2+k[1]*r4)*xu + 2*p[0]*xu*yu + p[1]*(r2+2*xu*xu); + double yd = (1+k[0]*r2+k[1]*r4)*yu + p[0]*(r2+2*yu*yu) + 2*p[1]*xu*yu; + // to image pixel + double u = f[0]*xd + s*yd + c[0]; + double v = f[1]*yd + c[1]; + + if( m1type == CV_16SC2 ) + { + int iu = cv::saturate_cast(u*cv::INTER_TAB_SIZE); + int iv = cv::saturate_cast(v*cv::INTER_TAB_SIZE); + m1[j*2+0] = (short)(iu >> cv::INTER_BITS); + m1[j*2+1] = (short)(iv >> cv::INTER_BITS); + m2[j] = (ushort)((iv & (cv::INTER_TAB_SIZE-1))*cv::INTER_TAB_SIZE + (iu & (cv::INTER_TAB_SIZE-1))); + } + else if( m1type == CV_32FC1 ) + { + m1f[j] = (float)u; + m2f[j] = (float)v; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// cv::omnidir::undistortImage + +void cv::omnidir::undistortImage(InputArray distorted, OutputArray undistorted, + InputArray K, InputArray D, double xi, InputArray Knew, const Size& new_size) +{ + Size size = new_size.area() != 0 ? new_size : distorted.size(); + + cv::Mat map1, map2; + omnidir::initUndistortRectifyMap(K, D, xi, cv::Matx33d::eye(), Knew, size, CV_16SC2, map1, map2 ); + cv::remap(distorted, undistorted, map1, map2, INTER_LINEAR, BORDER_CONSTANT); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// cv::omnidir::internal::initializeCalibration + +void cv::omnidir::internal::initializeCalibration(InputOutputArrayOfArrays patternPoints, InputOutputArrayOfArrays imagePoints, Size size, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray K, double& xi) +{ + // For details please refer to Section III from Li's IROS 2013 paper + + double u0 = size.width / 2; + double v0 = size.height / 2; + + int n_img = (int)imagePoints.total(); + + std::vector v_omAll(n_img), v_tAll(n_img); + + std::vector gammaAll(n_img), reProjErrorAll(n_img); + + std::vector patternPointsFilter, imagePointsFilter; + K.create(3, 3, CV_64F); + Mat _K; + for (int image_idx = 0; image_idx < n_img; ++image_idx) + { + cv::Mat objPoints = patternPoints.getMat(image_idx); + cv::Mat imgPoints = imagePoints.getMat(image_idx); + // objectPoints should be 3-channel data, imagePoints should be 2-channel data + CV_Assert(objPoints.type() == CV_64FC3 && imgPoints.type() == CV_64FC2); + + std::vector xy, uv; + cv::split(objPoints, xy); + cv::split(imgPoints, uv); + + int n_point = imgPoints.rows * imgPoints.cols; + cv::Mat x = xy[0].reshape(1, n_point), y = xy[1].reshape(1, n_point), + u = uv[0].reshape(1, n_point) - u0, v = uv[1].reshape(1, n_point) - v0; + + cv::Mat sqrRho = u.mul(u) + v.mul(v); + // compute extrinsic parameters + cv::Mat M(n_point, 6, CV_64F); + Mat(-v.mul(x)).copyTo(M.col(0)); + Mat(-v.mul(y)).copyTo(M.col(1)); + Mat(u.mul(x)).copyTo(M.col(2)); + Mat(u.mul(y)).copyTo(M.col(3)); + Mat(-v).copyTo(M.col(4)); + Mat(u).copyTo(M.col(5)); + + Mat W,U,V; + cv::SVD::compute(M, W, U, V,SVD::FULL_UV); + V = V.t(); + + double miniReprojectError = 1e5; + // the signs of r1, r2, r3 are unkown, so they can be flipped. + for (int coef = 1; coef >= -1; coef-=2) + { + double r11 = V.at(0, 5) * coef; + double r12 = V.at(1, 5) * coef; + double r21 = V.at(2, 5) * coef; + double r22 = V.at(3, 5) * coef; + double t1 = V.at(4, 5) * coef; + double t2 = V.at(5, 5) * coef; + + Mat roots; + double r31s; + solvePoly(Matx13d(-(r11*r12+r21*r22)*(r11*r12+r21*r22), r11*r11+r21*r21-r12*r12-r22*r22, 1), roots); + + if (roots.at(0)[0] > 0) + r31s = sqrt(roots.at(0)[0]); + else + r31s = sqrt(roots.at(1)[0]); + + for (int coef2 = 1; coef2 >= -1; coef2-=2) + { + double r31 = r31s * coef2; + double r32 = -(r11*r12 + r21*r22) / r31; + + cv::Vec3d r1(r11, r21, r31); + cv::Vec3d r2(r12, r22, r32); + cv::Vec3d t(t1, t2, 0); + double scale = 1 / cv::norm(r1); + r1 = r1 * scale; + r2 = r2 * scale; + t = t * scale; + + // compute intrisic parameters + // Form equations in Scaramuzza's paper + // A Toolbox for Easily Calibrating Omnidirectional Cameras + Mat A(n_point*2, 3, CV_64F); + Mat((r1[1]*x + r2[1]*y + t[1])/2).copyTo(A.rowRange(0, n_point).col(0)); + Mat((r1[0]*x + r2[0]*y + t[0])/2).copyTo(A.rowRange(n_point, 2*n_point).col(0)); + Mat(-A.col(0).rowRange(0, n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(0, n_point)); + Mat(-A.col(0).rowRange(n_point, 2*n_point).mul(sqrRho)).copyTo(A.col(1).rowRange(n_point, 2*n_point)); + Mat(-v).copyTo(A.rowRange(0, n_point).col(2)); + Mat(-u).copyTo(A.rowRange(n_point, 2*n_point).col(2)); + + // Operation to avoid bad numerical-condition of A + Vec3d maxA, minA; + for (int j = 0; j < A.cols; j++) + { + cv::minMaxLoc(cv::abs(A.col(j)), &minA[j], &maxA[j]); + A.col(j) = A.col(j) / maxA[j]; + } + + Mat B(n_point*2 , 1, CV_64F); + Mat(v.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(0, n_point)); + Mat(u.mul(r1[2]*x + r2[2]*y)).copyTo(B.rowRange(n_point, 2*n_point)); + + Mat res = A.inv(DECOMP_SVD) * B; + res = res.mul(1/Mat(maxA)); + + double gamma = sqrt(res.at(0) / res.at(1)); + t[2] = res.at(2); + + cv::Vec3d r3 = r1.cross(r2); + + Matx33d R(r1[0], r2[0], r3[0], + r1[1], r2[1], r3[1], + r1[2], r2[2], r3[2]); + Vec3d om; + Rodrigues(R, om); + + // project pattern points to images + Mat projedImgPoints; + Matx33d Kc(gamma, 0, u0, 0, gamma, v0, 0, 0, 1); + + // reproj error + cv::omnidir::projectPoints(objPoints, projedImgPoints, om, t, Kc, 1, Matx14d(0, 0, 0, 0), cv::noArray()); + double reprojectError = omnidir::internal::computeMeanReproerr(imgPoints, projedImgPoints); + + // if this reproject error is smaller + if (reprojectError < miniReprojectError) + { + miniReprojectError = reprojectError; + reProjErrorAll[image_idx] = miniReprojectError; + v_omAll[image_idx] = om; + v_tAll[image_idx] = t; + gammaAll[image_idx] = gamma; + } + } + } + } + + // filter initial results whose reproject errors are too large + std::vector reProjErrorFilter,v_gammaFilter; + std::vector omFilter, tFilter; + double gammaFinal = 0; + + // choose median value + size_t n = gammaAll.size() / 2; + std::nth_element(gammaAll.begin(), gammaAll.begin()+n, gammaAll.end()); + gammaFinal = gammaAll[n]; + + _K = Mat(Matx33d(gammaFinal, 0, u0, 0, gammaFinal, v0, 0, 0, 1)); + _K.convertTo(K, CV_64F); + + // recompute reproject error using the final gamma + for (int i = 0; i< n_img; i++) + { + Mat _projected; + cv::omnidir::projectPoints(patternPoints.getMat(i), _projected, v_omAll[i], v_tAll[i], _K, 1, Matx14d(0, 0, 0, 0), cv::noArray()); + double _error = omnidir::internal::computeMeanReproerr(imagePoints.getMat(i), _projected); + if(_error < 20) + { + reProjErrorFilter.push_back(_error); + omFilter.push_back(v_omAll[i]); + tFilter.push_back(v_tAll[i]); + patternPointsFilter.push_back(patternPoints.getMat(i).clone()); + imagePointsFilter.push_back(imagePoints.getMat(i).clone()); + } + } + + cv::Mat(omFilter).convertTo(omAll, CV_64FC3); + cv::Mat(tFilter).convertTo(tAll, CV_64FC3); + + int depth1 = patternPoints.depth(), depth2 = imagePoints.depth(); + patternPoints.create(Size(1, (int)patternPointsFilter.size()), CV_MAKETYPE(depth1,3)); + imagePoints.create(Size(1, (int)patternPointsFilter.size()), CV_MAKETYPE(depth2,2)); + + for(int i = 0; i < (int)patternPointsFilter.size(); ++i) + { + patternPointsFilter[i].copyTo(patternPoints.getMat(i)); + imagePointsFilter[i].copyTo(imagePoints.getMat(i)); + } + + xi = 1; +} +////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/// cv::omnidir::internal::computeJacobian + +void cv::omnidir::internal::computeJacobian(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, + InputArray parameters, Mat& JTJ_inv, Mat& JTE, int flags) +{ + CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); + CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); + + int n = (int)objectPoints.total(); + + Mat JTJ = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F); + JTJ_inv = Mat::zeros(10 + 6*n, 10 + 6*n, CV_64F); + JTE = Mat::zeros(10 + 6*n, 1, CV_64F); + //Mat J = Mat::zeros(2*n*objectPoints.getMat(0).total(), 10+6*n, CV_64F); + //Mat exAll = Mat::zeros(2*n*objectPoints.getMat(0).total(), 1, CV_64F); + double *para = parameters.getMat().ptr(); + Matx33d K(para[6*n], para[6*n+2], para[6*n+3], + 0, para[6*n+1], para[6*n+4], + 0, 0, 1); + Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + double xi = para[6*n+5]; + for (int i = 0; i < n; i++) + { + Mat objPoints, imgPoints, om, T; + objPoints = objectPoints.getMat(i); + imgPoints = imagePoints.getMat(i); + + om = parameters.getMat().colRange(i*6, i*6+3); + T = parameters.getMat().colRange(i*6+3, (i+1)*6); + Mat imgProj, jacobian; + omnidir::projectPoints(objPoints, imgProj, om, T, K, xi, D, jacobian); + Mat projError = imgPoints - imgProj; + + // The intrinsic part of Jacobian + Mat JIn(jacobian.rows, 10, CV_64F); + Mat JEx(jacobian.rows, 6, CV_64F); + + jacobian.colRange(6, 16).copyTo(JIn); + jacobian.colRange(0, 6).copyTo(JEx); + + JTJ(Rect(6*n, 6*n, 10, 10)) = JTJ(Rect(6*n, 6*n, 10, 10)) + JIn.t()*JIn; + + JTJ(Rect(i*6, i*6, 6, 6)) = JEx.t() * JEx; + + Mat JExTIn = JEx.t() * JIn; + + JExTIn.copyTo(JTJ(Rect(6*n, i*6, 10, 6))); + + Mat(JIn.t()*JEx).copyTo(JTJ(Rect(i*6, 6*n, 6, 10))); + + JTE(Rect(0, 6*n, 1, 10)) = JTE(Rect(0, 6*n,1, 10)) + JIn.t() * projError.reshape(1, 2*projError.rows); + JTE(Rect(0, i*6, 1, 6)) = JEx.t() * projError.reshape(1, 2*projError.rows); + + } + + std::vector _idx(6*n+10, 1); + flags2idx(flags, _idx, n); + + subMatrix(JTJ, JTJ, _idx, _idx); + subMatrix(JTE, JTE, std::vector(1, 1), _idx); + // in case JTJ is singular + double epsilon = 1e-9; + JTJ_inv = Mat(JTJ+epsilon).inv(); +} + +double cv::omnidir::calibrate(InputOutputArrayOfArrays patternPoints, InputOutputArrayOfArrays imagePoints, Size size, + InputOutputArray K, InputOutputArray xi, InputOutputArray D, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, + int flags, TermCriteria criteria) +{ + CV_Assert(!patternPoints.empty() && !imagePoints.empty() && patternPoints.total() == imagePoints.total()); + CV_Assert(patternPoints.type() == CV_64FC3 && imagePoints.type() == CV_64FC2); + CV_Assert((!K.empty() && K.size() == Size(3,3)) || K.empty()); + CV_Assert((!D.empty() && D.total() == 4) || D.empty()); + CV_Assert((!xi.empty() && xi.total() == 1) || xi.empty()); + + double _xi; + // initialization + cv::omnidir::internal::initializeCalibration(patternPoints, imagePoints, size, omAll, tAll, K, _xi); + int n = (int)patternPoints.total(); + Mat finalParam(1, 10 + 6*n, CV_64F); + Mat currentParam(1, 10 + 6*n, CV_64F); + cv::omnidir::internal::encodeParameters(K, omAll, tAll, Mat::zeros(1,4,CV_64F), _xi, n, currentParam); + + // optimization + const double alpha_smooth = 0.1; + //const double thresh_cond = 1e6; + double change = 1; + for(int iter = 0; ; ++iter) + { + if ((criteria.type == 1 && iter >= criteria.maxCount) || + (criteria.type == 2 && change <= criteria.epsilon) || + (criteria.type == 3 && (change <= criteria.epsilon || iter >= criteria.maxCount))) + break; + double alpha_smooth2 = 1 - std::pow(1 - alpha_smooth, (double)iter/10 + 1.0); + Mat JTJ_inv, JTError; + + cv::omnidir::internal::computeJacobian(patternPoints, imagePoints, currentParam, JTJ_inv, JTError, flags); + + // Gauss¨CNewton + Mat G = alpha_smooth2*JTJ_inv * JTError; + + omnidir::internal::fillFixed(G, flags, n); + + finalParam = currentParam + G.t(); + + change = norm(G) / norm(currentParam); + + currentParam = finalParam.clone(); + + } + cv::omnidir::internal::decodeParameters(currentParam, K, omAll, tAll, D, _xi); + + std::vector proImagePoints; + + for (int i = 0; i < n; ++i) + { + Mat imgPointsi; + cv::omnidir::projectPoints(patternPoints.getMat(i), imgPointsi, omAll.getMat().at(i), tAll.getMat().at(i), K, _xi, D, noArray()); + proImagePoints.push_back(imgPointsi); + } + //double meanRepr = omnidir::internal::computeMeanReproerr(imagePoints, proImagePoints); + xi.create(1, 1, CV_64F); + Mat xi_m = Mat(1, 1, CV_64F); + xi_m.at(0) = _xi; + xi_m.copyTo(xi.getMat()); + Vec2d std_error; + double rms; + Mat errors; + cv::omnidir::internal::estimateUncertainties(patternPoints, imagePoints, finalParam, errors, std_error, rms, flags); + return rms; +} + +void cv::omnidir::internal::encodeParameters(InputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, InputArray distoaration, double xi, int n, OutputArray parameters) +{ + CV_Assert(K.type() == CV_64F && K.size() == Size(3,3)); + CV_Assert(distoaration.total() == 4 && distoaration.type() == CV_64F); + + Mat _omAll = omAll.getMat(), _tAll = tAll.getMat(); + Mat tmp = Mat(_omAll.at(0)).reshape(1,3).clone(); + Matx33d _K = K.getMat(); + Vec4d _D = (Vec4d)distoaration.getMat(); + parameters.create(1, 10+6*n,CV_64F); + Mat _params = parameters.getMat(); + for (int i = 0; i < n; i++) + { + Mat(_omAll.at(i)).reshape(1, 1).copyTo(_params.colRange(i*6, i*6+3)); + Mat(_tAll.at(i)).reshape(1, 1).copyTo(_params.colRange(i*6+3, (i+1)*6)); + } + + _params.at(0, 6*n) = _K(0,0); + _params.at(0, 6*n+1) = _K(1,1); + _params.at(0, 6*n+2) = _K(0,1); + _params.at(0, 6*n+3) = _K(0,2); + _params.at(0, 6*n+4) = _K(1,2); + _params.at(0, 6*n+5) = xi; + _params.at(0, 6*n+6) = _D[0]; + _params.at(0, 6*n+7) = _D[1]; + _params.at(0, 6*n+8) = _D[2]; + _params.at(0, 6*n+9) = _D[3]; +} + + void cv::omnidir::internal::decodeParameters(InputArray paramsters, OutputArray K, OutputArrayOfArrays omAll, OutputArrayOfArrays tAll, OutputArray distoration, double& xi) + { + if(K.empty()) + K.create(3,3,CV_64F); + Matx33d _K; + int n = (int)(paramsters.total()-10)/6; + if(omAll.empty()) + omAll.create(1, n, CV_64FC3); + if(tAll.empty()) + tAll.create(1, n, CV_64FC3); + if(distoration.empty()) + distoration.create(1, 4, CV_64F); + Matx14d _D = distoration.getMat(); + Mat param = paramsters.getMat(); + double *para = param.ptr(); + _K = Matx33d(para[6*n], para[6*n+2], para[6*n+3], + 0, para[6*n+1], para[6*n+4], + 0, 0, 1); + _D = Matx14d(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + xi = para[6*n+5]; + + for (int i = 0; i < n; i++) + { + param.colRange(i*6, i*6+3).reshape(3, 1).copyTo(omAll.getMat(i)); + param.colRange(i*6+3, i*6+6).reshape(3, 1).copyTo(tAll.getMat(i)); + } + Mat(_D).convertTo(distoration, CV_64F); + Mat(_K).convertTo(K, CV_64F); + + } + +void cv::omnidir::internal::estimateUncertainties(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints, InputArray parameters, + Mat& errors, Vec2d& std_error, double& rms, int flags) +{ + CV_Assert(!objectPoints.empty() && objectPoints.type() == CV_64FC3); + CV_Assert(!imagePoints.empty() && imagePoints.type() == CV_64FC2); + CV_Assert(!parameters.empty() && parameters.type() == CV_64F); + + int n = (int) objectPoints.total(); + // assume every image has the same number of objectpoints + int nPointsImage = (int)objectPoints.getMat(0).total(); + + Mat reprojError = Mat(n*nPointsImage, 1, CV_64FC2); + + double* para = parameters.getMat().ptr(); + Matx33d K(para[6*n], para[6*n+2], para[6*n+3], + 0, para[6*n+1], para[6*n+4], + 0, 0, 1); + Matx14d D(para[6*n+6], para[6*n+7], para[6*n+8], para[6*n+9]); + double xi = para[6*n+5]; + + for(int i=0; i < n; ++i) + { + Mat imgPoints = imagePoints.getMat(i); + Mat objPoints = objectPoints.getMat(i); + + Mat om = parameters.getMat().colRange(i*6, i*6+3); + Mat T = parameters.getMat().colRange(i*6+3, (i+1)*6); + + Mat x; + omnidir::projectPoints(objPoints, x, om, T, K, xi, D, cv::noArray()); + + Mat errorx = (imgPoints - x); + + //reprojError.rowRange(errorx.rows*i, errorx.rows*(i+1)) = errorx.clone(); + errorx.copyTo(reprojError.rowRange(errorx.rows*i, errorx.rows*(i+1))); + } + + meanStdDev(reprojError, noArray(), std_error); + std_error *= sqrt((double)reprojError.total()/((double)reprojError.total() - 1.0)); + + Mat simga_x; + meanStdDev(reprojError.reshape(1,1), noArray(), simga_x); + simga_x *= sqrt(2.0*(double)reprojError.total()/(2.0*(double)reprojError.total() - 1.0)); + double s = simga_x.at(0); + + Mat _JTJ_inv, _JTE; + computeJacobian(objectPoints, imagePoints, parameters, _JTJ_inv, _JTE, flags); + sqrt(_JTJ_inv, _JTJ_inv); + + errors = 3 * s * _JTJ_inv.diag(); + + checkFixed(errors, flags, n); + + rms = 0; + const Vec2d* ptr_ex = reprojError.ptr(); + for (int i = 0; i < (int)reprojError.total(); i++) + { + rms += ptr_ex[i][0] * ptr_ex[i][0] + ptr_ex[i][1] * ptr_ex[i][1]; + } + + rms /= (double)reprojError.total(); + rms = sqrt(rms); +} + +// +double cv::omnidir::internal::computeMeanReproerr(InputArrayOfArrays imagePoints, InputArrayOfArrays proImagePoints) +{ + CV_Assert(!imagePoints.empty() && imagePoints.type()==CV_64FC2); + CV_Assert(!proImagePoints.empty() && proImagePoints.type() == CV_64FC2); + CV_Assert(imagePoints.total() == proImagePoints.total()); + + int n = (int)imagePoints.total(); + double reprojError = 0; + int totalPoints = 0; + for (int i = 0; i < n; i++) + { + Mat errorI = imagePoints.getMat(i) - proImagePoints.getMat(i); + totalPoints += (int)errorI.total(); + Vec2d* ptr_err = errorI.ptr(); + for (int j = 0; j < (int)errorI.total(); j++) + { + reprojError += sqrt(ptr_err[j][0]*ptr_err[j][0] + ptr_err[j][1]*ptr_err[j][1]); + } + } + double meanReprojError = reprojError / totalPoints; + return meanReprojError; +} + +void cv::omnidir::internal::checkFixed(Mat& G, int flags, int n) +{ + int _flags = flags; + if(_flags >= omnidir::CALIB_FIX_CENTER) + { + G.at(6*n+3) = 0; + G.at(6*n+4) = 0; + _flags -= omnidir::CALIB_FIX_CENTER; + } + if(_flags >= omnidir::CALIB_FIX_GAMMA) + { + G.at(6*n) = 0; + G.at(6*n+1) = 0; + _flags -= omnidir::CALIB_FIX_GAMMA; + } + if(_flags >= omnidir::CALIB_FIX_XI) + { + G.at(6*n + 5) = 0; + _flags -= omnidir::CALIB_FIX_XI; + } + if(_flags >= omnidir::CALIB_FIX_P2) + { + G.at(6*n + 9) = 0; + _flags -= omnidir::CALIB_FIX_P2; + } + if(_flags >= omnidir::CALIB_FIX_P1) + { + G.at(6*n + 8) = 0; + _flags -= omnidir::CALIB_FIX_P1; + } + if(_flags >= omnidir::CALIB_FIX_K2) + { + G.at(6*n + 7) = 0; + _flags -= omnidir::CALIB_FIX_K2; + } + if(_flags >= omnidir::CALIB_FIX_K1) + { + G.at(6*n + 6) = 0; + _flags -= omnidir::CALIB_FIX_K1; + } + if(_flags >= omnidir::CALIB_FIX_SKEW) + { + G.at(6*n + 2) = 0; + } +} + +// This function is from fisheye.cpp +void cv::omnidir::internal::subMatrix(const Mat& src, Mat& dst, const std::vector& cols, const std::vector& rows) +{ + CV_Assert(src.type() == CV_64FC1); + + int nonzeros_cols = cv::countNonZero(cols); + Mat tmp(src.rows, nonzeros_cols, CV_64FC1); + + for (int i = 0, j = 0; i < (int)cols.size(); i++) + { + if (cols[i]) + { + src.col(i).copyTo(tmp.col(j++)); + } + } + + int nonzeros_rows = cv::countNonZero(rows); + Mat tmp1(nonzeros_rows, nonzeros_cols, CV_64FC1); + for (int i = 0, j = 0; i < (int)rows.size(); i++) + { + if (rows[i]) + { + tmp.row(i).copyTo(tmp1.row(j++)); + } + } + + dst = tmp1.clone(); +} + +void cv::omnidir::internal::flags2idx(int flags, std::vector& idx, int n) +{ + int _flags = flags; + if(_flags >= omnidir::CALIB_FIX_CENTER) + { + idx[6*n+3] = 0; + idx[6*n+4] = 0; + _flags -= omnidir::CALIB_FIX_CENTER; + } + if(_flags >= omnidir::CALIB_FIX_GAMMA) + { + idx[6*n] = 0; + idx[6*n+1] = 0; + _flags -= omnidir::CALIB_FIX_GAMMA; + } + if(_flags >= omnidir::CALIB_FIX_XI) + { + idx[6*n + 5] = 0; + _flags -= omnidir::CALIB_FIX_XI; + } + if(_flags >= omnidir::CALIB_FIX_P2) + { + idx[6*n + 9] = 0; + _flags -= omnidir::CALIB_FIX_P2; + } + if(_flags >= omnidir::CALIB_FIX_P1) + { + idx[6*n + 8] = 0; + _flags -= omnidir::CALIB_FIX_P1; + } + if(_flags >= omnidir::CALIB_FIX_K2) + { + idx[6*n + 7] = 0; + _flags -= omnidir::CALIB_FIX_K2; + } + if(_flags >= omnidir::CALIB_FIX_K1) + { + idx[6*n + 6] = 0; + _flags -= omnidir::CALIB_FIX_K1; + } + if(_flags >= omnidir::CALIB_FIX_SKEW) + { + idx[6*n + 2] = 0; + } +} + +void cv::omnidir::internal::fillFixed(Mat&G, int flags, int n) +{ + Mat tmp = G.clone(); + std::vector idx(6*n + 10, 1); + flags2idx(flags, idx, n); + G.release(); + G.create(6*n +10, 1, CV_64F); + G = cv::Mat::zeros(6*n +10, 1, CV_64F); + for (int i = 0,j=0; i < (int)idx.size(); i++) + { + if (idx[i]) + { + G.at(i) = tmp.at(j++); + } + } +} + +//double cv::omnidir::stereoCalibrate(InputArrayOfArrays objectPoints, InputArrayOfArrays imagePoints1, InputArrayOfArrays imagePoints2, +// Size imageSize, InputOutputArray K1, double& xi1, InputOutputArray D1, InputOutputArray K2, double& xi2, +// InputOutputArray D2, OutputArray R, OutputArray T, int flags, TermCriteria criteria) +//{ +// return 1; +//} + + +//void cv::omnidir::stereoRectify(InputArray K1, InputArray D1, double xi1, InputArray K2, InputArray D2, double xi2, const Size imageSize, +// InputArray R, InputArray tvec, OutputArray R1, OutputArray R2, OutputArray P1, OutputArray P2, OutputArray Q, int flags, +// const Size& newImageSize) +//{ +// +//} + +#endif // __OPENCV_OMNIDIR_CPP__ +#endif // cplusplus \ No newline at end of file