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<float>(i);
+ float* m2f = map2.empty() ? 0 : map2.ptr<float>(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<int>(u*cv::INTER_TAB_SIZE);
+ int iv = cv::saturate_cast<int>(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 )
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<float>(i);
- float* m2f = map2.empty() ? 0 : map2.ptr<float>(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<int>(u*INTER_TAB_SIZE);
- int iv = saturate_cast<int>(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));
}