From f5d9a2a9c161953a19a2dffdd50c6b84779d0fc5 Mon Sep 17 00:00:00 2001 From: "woody.chow" Date: Mon, 15 Jan 2018 09:20:35 +0900 Subject: [PATCH] Parallelize initUndistortRectifyMap --- modules/imgproc/src/undistort.avx2.cpp | 6 +- modules/imgproc/src/undistort.cpp | 188 +++++++++++++++++++++++---------- modules/imgproc/src/undistort.hpp | 6 +- 3 files changed, 136 insertions(+), 64 deletions(-) diff --git a/modules/imgproc/src/undistort.avx2.cpp b/modules/imgproc/src/undistort.avx2.cpp index c9ea9b8..69998be 100644 --- a/modules/imgproc/src/undistort.avx2.cpp +++ b/modules/imgproc/src/undistort.avx2.cpp @@ -48,9 +48,9 @@ namespace cv int initUndistortRectifyMapLine_AVX(float* m1f, float* m2f, short* m1, ushort* m2, double* matTilt, const double* ir, double& _x, double& _y, double& _w, int width, int m1type, - double& k1, double& k2, double& k3, double& k4, double& k5, double& k6, - double& p1, double& p2, double& s1, double& s2, double& s3, double& s4, - double& u0, double& v0, double& fx, double& fy) + double k1, double k2, double k3, double k4, double k5, double k6, + double p1, double p2, double s1, double s2, double s3, double s4, + double u0, double v0, double fx, double fy) { int j = 0; diff --git a/modules/imgproc/src/undistort.cpp b/modules/imgproc/src/undistort.cpp index 0ee7997..e8d38d7 100644 --- a/modules/imgproc/src/undistort.cpp +++ b/modules/imgproc/src/undistort.cpp @@ -61,6 +61,133 @@ cv::Mat cv::getDefaultNewCameraMatrix( InputArray _cameraMatrix, Size imgsize, return newCameraMatrix; } +class initUndistortRectifyMapComputer : public cv::ParallelLoopBody +{ +public: + initUndistortRectifyMapComputer( + cv::Size _size, cv::Mat &_map1, cv::Mat &_map2, int _m1type, + const double* _ir, cv::Matx33d &_matTilt, + double _u0, double _v0, double _fx, double _fy, + double _k1, double _k2, double _p1, double _p2, + double _k3, double _k4, double _k5, double _k6, + double _s1, double _s2, double _s3, double _s4) + : size(_size), + map1(_map1), + map2(_map2), + m1type(_m1type), + ir(_ir), + matTilt(_matTilt), + u0(_u0), + v0(_v0), + fx(_fx), + fy(_fy), + k1(_k1), + k2(_k2), + p1(_p1), + p2(_p2), + k3(_k3), + k4(_k4), + k5(_k5), + k6(_k6), + s1(_s1), + s2(_s2), + s3(_s3), + s4(_s4) { +#if CV_TRY_AVX2 + useAVX2 = cv::checkHardwareSupport(CV_CPU_AVX2); +#endif + } + + void operator()( const cv::Range& range ) const + { + const int begin = range.start; + const int end = range.end; + + for( int i = begin; i < end; i++ ) + { + float* m1f = map1.ptr(i); + float* m2f = map2.empty() ? 0 : map2.ptr(i); + short* m1 = (short*)m1f; + ushort* m2 = (ushort*)m2f; + double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; + + int j = 0; + + if (m1type == CV_16SC2) + CV_Assert(m1 != NULL && m2 != NULL); + else if (m1type == CV_32FC1) + CV_Assert(m1f != NULL && m2f != NULL); + else + CV_Assert(m1 != NULL); + + #if CV_TRY_AVX2 + if( useAVX2 ) + j = cv::initUndistortRectifyMapLine_AVX(m1f, m2f, m1, m2, + matTilt.val, ir, _x, _y, _w, size.width, m1type, + k1, k2, k3, k4, k5, k6, p1, p2, s1, s2, s3, s4, u0, v0, fx, fy); + #endif + for( ; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) + { + double w = 1./_w, x = _x*w, y = _y*w; + double x2 = x*x, y2 = y*y; + double r2 = x2 + y2, _2xy = 2*x*y; + double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2); + double xd = (x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2); + double yd = (y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2); + cv::Vec3d vecTilt = matTilt*cv::Vec3d(xd, yd, 1); + double invProj = vecTilt(2) ? 1./vecTilt(2) : 1; + double u = fx*invProj*vecTilt(0) + u0; + double v = fy*invProj*vecTilt(1) + v0; + 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] = (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; + } + else + { + m1f[j*2] = (float)u; + m1f[j*2+1] = (float)v; + } + } + } + } + +private: + cv::Size size; + cv::Mat &map1; + cv::Mat &map2; + int m1type; + const double* ir; + cv::Matx33d &matTilt; + double u0; + double v0; + double fx; + double fy; + double k1; + double k2; + double p1; + double p2; + double k3; + double k4; + double k5; + double k6; + double s1; + double s2; + double s3; + double s4; +#if CV_TRY_AVX2 + bool useAVX2; +#endif +}; + void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _matR, InputArray _newCameraMatrix, Size size, int m1type, OutputArray _map1, OutputArray _map2 ) @@ -137,64 +264,9 @@ void cv::initUndistortRectifyMap( InputArray _cameraMatrix, InputArray _distCoef cv::Matx33d matTilt = cv::Matx33d::eye(); cv::detail::computeTiltProjectionMatrix(tauX, tauY, &matTilt); -#if CV_TRY_AVX2 - bool USE_AVX2 = cv::checkHardwareSupport(CV_CPU_AVX2); -#endif - - for( int i = 0; i < size.height; i++ ) - { - float* m1f = map1.ptr(i); - float* m2f = map2.empty() ? 0 : map2.ptr(i); - short* m1 = (short*)m1f; - ushort* m2 = (ushort*)m2f; - double _x = i*ir[1] + ir[2], _y = i*ir[4] + ir[5], _w = i*ir[7] + ir[8]; - - int j = 0; - - if (m1type == CV_16SC2) - CV_Assert(m1 != NULL && m2 != NULL); - else if (m1type == CV_32FC1) - CV_Assert(m1f != NULL && m2f != NULL); - else - CV_Assert(m1 != NULL); - -#if CV_TRY_AVX2 - if( USE_AVX2 ) - j = cv::initUndistortRectifyMapLine_AVX(m1f, m2f, m1, m2, matTilt.val, ir, _x, _y, _w, size.width, m1type, - k1, k2, k3, k4, k5, k6, p1, p2, s1, s2, s3, s4, u0, v0, fx, fy); -#endif - for( ; j < size.width; j++, _x += ir[0], _y += ir[3], _w += ir[6] ) - { - double w = 1./_w, x = _x*w, y = _y*w; - double x2 = x*x, y2 = y*y; - double r2 = x2 + y2, _2xy = 2*x*y; - double kr = (1 + ((k3*r2 + k2)*r2 + k1)*r2)/(1 + ((k6*r2 + k5)*r2 + k4)*r2); - double xd = (x*kr + p1*_2xy + p2*(r2 + 2*x2) + s1*r2+s2*r2*r2); - double yd = (y*kr + p1*(r2 + 2*y2) + p2*_2xy + s3*r2+s4*r2*r2); - cv::Vec3d vecTilt = matTilt*cv::Vec3d(xd, yd, 1); - double invProj = vecTilt(2) ? 1./vecTilt(2) : 1; - double u = fx*invProj*vecTilt(0) + u0; - double v = fy*invProj*vecTilt(1) + v0; - if( m1type == CV_16SC2 ) - { - int iu = saturate_cast(u*INTER_TAB_SIZE); - int iv = saturate_cast(v*INTER_TAB_SIZE); - m1[j*2] = (short)(iu >> INTER_BITS); - m1[j*2+1] = (short)(iv >> INTER_BITS); - m2[j] = (ushort)((iv & (INTER_TAB_SIZE-1))*INTER_TAB_SIZE + (iu & (INTER_TAB_SIZE-1))); - } - else if( m1type == CV_32FC1 ) - { - m1f[j] = (float)u; - m2f[j] = (float)v; - } - else - { - m1f[j*2] = (float)u; - m1f[j*2+1] = (float)v; - } - } - } + parallel_for_(Range(0, size.height), initUndistortRectifyMapComputer( + size, map1, map2, m1type, ir, matTilt, u0, v0, + fx, fy, k1, k2, p1, p2, k3, k4, k5, k6, s1, s2, s3, s4)); } diff --git a/modules/imgproc/src/undistort.hpp b/modules/imgproc/src/undistort.hpp index c17de04..02c1774 100644 --- a/modules/imgproc/src/undistort.hpp +++ b/modules/imgproc/src/undistort.hpp @@ -48,9 +48,9 @@ namespace cv #if CV_TRY_AVX2 int initUndistortRectifyMapLine_AVX(float* m1f, float* m2f, short* m1, ushort* m2, double* matTilt, const double* ir, double& _x, double& _y, double& _w, int width, int m1type, - double& k1, double& k2, double& k3, double& k4, double& k5, double& k6, - double& p1, double& p2, double& s1, double& s2, double& s3, double& s4, - double& u0, double& v0, double& fx, double& fy); + double k1, double k2, double k3, double k4, double k5, double k6, + double p1, double p2, double s1, double s2, double s3, double s4, + double u0, double v0, double fx, double fy); #endif } -- 2.7.4