mirror of https://github.com/opencv/opencv.git
Open Source Computer Vision Library
https://opencv.org/
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
329 lines
13 KiB
329 lines
13 KiB
/*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) 2000-2008, Intel Corporation, all rights reserved. |
|
// Copyright (C) 2009, Willow Garage Inc., all rights reserved. |
|
// Third party copyrights are property of their respective owners. |
|
// |
|
// 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*/ |
|
|
|
#include "precomp.hpp" |
|
#include "opencv2/core/hal/intrin.hpp" |
|
|
|
namespace cv { |
|
CV_CPU_OPTIMIZATION_NAMESPACE_BEGIN |
|
// forward declarations |
|
Ptr<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type, |
|
const double* _ir, 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); |
|
|
|
|
|
#ifndef CV_CPU_OPTIMIZATION_DECLARATIONS_ONLY |
|
namespace |
|
{ |
|
class initUndistortRectifyMapComputer : public ParallelLoopBody |
|
{ |
|
public: |
|
initUndistortRectifyMapComputer( |
|
Size _size, Mat &_map1, Mat &_map2, int _m1type, |
|
const double* _ir, 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_SIMD_64F |
|
for (int i = 0; i < 2 * v_float64::nlanes; ++i) |
|
{ |
|
s_x[i] = ir[0] * i; |
|
s_y[i] = ir[3] * i; |
|
s_w[i] = ir[6] * i; |
|
} |
|
#endif |
|
} |
|
|
|
void operator()( const cv::Range& range ) const CV_OVERRIDE |
|
{ |
|
CV_INSTRUMENT_REGION(); |
|
|
|
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_SIMD_64F |
|
const v_float64 v_one = vx_setall_f64(1.0); |
|
for (; j <= size.width - 2*v_float64::nlanes; j += 2*v_float64::nlanes, _x += 2*v_float64::nlanes * ir[0], _y += 2*v_float64::nlanes * ir[3], _w += 2*v_float64::nlanes * ir[6]) |
|
{ |
|
v_float64 m_0, m_1, m_2, m_3; |
|
m_2 = v_one / (vx_setall_f64(_w) + vx_load(s_w)); |
|
m_3 = v_one / (vx_setall_f64(_w) + vx_load(s_w + v_float64::nlanes)); |
|
m_0 = vx_setall_f64(_x); m_1 = vx_setall_f64(_y); |
|
v_float64 x_0 = (m_0 + vx_load(s_x)) * m_2; |
|
v_float64 x_1 = (m_0 + vx_load(s_x + v_float64::nlanes)) * m_3; |
|
v_float64 y_0 = (m_1 + vx_load(s_y)) * m_2; |
|
v_float64 y_1 = (m_1 + vx_load(s_y + v_float64::nlanes)) * m_3; |
|
|
|
v_float64 xd_0 = x_0 * x_0; |
|
v_float64 yd_0 = y_0 * y_0; |
|
v_float64 xd_1 = x_1 * x_1; |
|
v_float64 yd_1 = y_1 * y_1; |
|
|
|
v_float64 r2_0 = xd_0 + yd_0; |
|
v_float64 r2_1 = xd_1 + yd_1; |
|
|
|
m_1 = vx_setall_f64(k3); |
|
m_2 = vx_setall_f64(k2); |
|
m_3 = vx_setall_f64(k1); |
|
m_0 = v_muladd(v_muladd(v_muladd(m_1, r2_0, m_2), r2_0, m_3), r2_0, v_one); |
|
m_1 = v_muladd(v_muladd(v_muladd(m_1, r2_1, m_2), r2_1, m_3), r2_1, v_one); |
|
m_3 = vx_setall_f64(k6); |
|
m_2 = vx_setall_f64(k5); |
|
m_0 /= v_muladd(v_muladd(v_muladd(m_3, r2_0, m_2), r2_0, vx_setall_f64(k4)), r2_0, v_one); |
|
m_1 /= v_muladd(v_muladd(v_muladd(m_3, r2_1, m_2), r2_1, vx_setall_f64(k4)), r2_1, v_one); |
|
|
|
m_3 = vx_setall_f64(2.0); |
|
xd_0 = v_muladd(m_3, xd_0, r2_0); |
|
yd_0 = v_muladd(m_3, yd_0, r2_0); |
|
xd_1 = v_muladd(m_3, xd_1, r2_1); |
|
yd_1 = v_muladd(m_3, yd_1, r2_1); |
|
m_2 = x_0 * y_0 * m_3; |
|
m_3 = x_1 * y_1 * m_3; |
|
|
|
x_0 *= m_0; y_0 *= m_0; x_1 *= m_1; y_1 *= m_1; |
|
|
|
m_0 = vx_setall_f64(p1); |
|
m_1 = vx_setall_f64(p2); |
|
xd_0 = v_muladd(xd_0, m_1, x_0); |
|
yd_0 = v_muladd(yd_0, m_0, y_0); |
|
xd_1 = v_muladd(xd_1, m_1, x_1); |
|
yd_1 = v_muladd(yd_1, m_0, y_1); |
|
|
|
xd_0 = v_muladd(m_0, m_2, xd_0); |
|
yd_0 = v_muladd(m_1, m_2, yd_0); |
|
xd_1 = v_muladd(m_0, m_3, xd_1); |
|
yd_1 = v_muladd(m_1, m_3, yd_1); |
|
|
|
m_0 = r2_0 * r2_0; |
|
m_1 = r2_1 * r2_1; |
|
m_2 = vx_setall_f64(s2); |
|
m_3 = vx_setall_f64(s1); |
|
xd_0 = v_muladd(m_3, r2_0, v_muladd(m_2, m_0, xd_0)); |
|
xd_1 = v_muladd(m_3, r2_1, v_muladd(m_2, m_1, xd_1)); |
|
m_2 = vx_setall_f64(s4); |
|
m_3 = vx_setall_f64(s3); |
|
yd_0 = v_muladd(m_3, r2_0, v_muladd(m_2, m_0, yd_0)); |
|
yd_1 = v_muladd(m_3, r2_1, v_muladd(m_2, m_1, yd_1)); |
|
|
|
m_0 = vx_setall_f64(matTilt.val[0]); |
|
m_1 = vx_setall_f64(matTilt.val[1]); |
|
m_2 = vx_setall_f64(matTilt.val[2]); |
|
x_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2)); |
|
x_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2)); |
|
m_0 = vx_setall_f64(matTilt.val[3]); |
|
m_1 = vx_setall_f64(matTilt.val[4]); |
|
m_2 = vx_setall_f64(matTilt.val[5]); |
|
y_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2)); |
|
y_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2)); |
|
m_0 = vx_setall_f64(matTilt.val[6]); |
|
m_1 = vx_setall_f64(matTilt.val[7]); |
|
m_2 = vx_setall_f64(matTilt.val[8]); |
|
r2_0 = v_muladd(m_0, xd_0, v_muladd(m_1, yd_0, m_2)); |
|
r2_1 = v_muladd(m_0, xd_1, v_muladd(m_1, yd_1, m_2)); |
|
m_0 = vx_setzero_f64(); |
|
r2_0 = v_select(r2_0 == m_0, v_one, v_one / r2_0); |
|
r2_1 = v_select(r2_1 == m_0, v_one, v_one / r2_1); |
|
|
|
m_0 = vx_setall_f64(fx); |
|
m_1 = vx_setall_f64(u0); |
|
m_2 = vx_setall_f64(fy); |
|
m_3 = vx_setall_f64(v0); |
|
x_0 = v_muladd(m_0 * r2_0, x_0, m_1); |
|
y_0 = v_muladd(m_2 * r2_0, y_0, m_3); |
|
x_1 = v_muladd(m_0 * r2_1, x_1, m_1); |
|
y_1 = v_muladd(m_2 * r2_1, y_1, m_3); |
|
|
|
if (m1type == CV_32FC1) |
|
{ |
|
v_store(&m1f[j], v_cvt_f32(x_0, x_1)); |
|
v_store(&m2f[j], v_cvt_f32(y_0, y_1)); |
|
} |
|
else if (m1type == CV_32FC2) |
|
{ |
|
v_float32 mf0, mf1; |
|
v_zip(v_cvt_f32(x_0, x_1), v_cvt_f32(y_0, y_1), mf0, mf1); |
|
v_store(&m1f[j * 2], mf0); |
|
v_store(&m1f[j * 2 + v_float32::nlanes], mf1); |
|
} |
|
else // m1type == CV_16SC2 |
|
{ |
|
m_0 = vx_setall_f64(INTER_TAB_SIZE); |
|
x_0 *= m_0; x_1 *= m_0; y_0 *= m_0; y_1 *= m_0; |
|
|
|
v_int32 mask = vx_setall_s32(INTER_TAB_SIZE - 1); |
|
v_int32 iu = v_round(x_0, x_1); |
|
v_int32 iv = v_round(y_0, y_1); |
|
|
|
v_pack_u_store(&m2[j], (iu & mask) + (iv & mask) * vx_setall_s32(INTER_TAB_SIZE)); |
|
v_int32 out0, out1; |
|
v_zip(iu >> INTER_BITS, iv >> INTER_BITS, out0, out1); |
|
v_store(&m1[j * 2], v_pack(out0, out1)); |
|
} |
|
} |
|
|
|
vx_cleanup(); |
|
#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); |
|
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; |
|
} |
|
} |
|
} |
|
} |
|
|
|
private: |
|
Size size; |
|
Mat &map1; |
|
Mat &map2; |
|
int m1type; |
|
const double* ir; |
|
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_SIMD_64F |
|
double s_x[2*v_float64::nlanes]; |
|
double s_y[2*v_float64::nlanes]; |
|
double s_w[2*v_float64::nlanes]; |
|
#endif |
|
}; |
|
} |
|
|
|
Ptr<ParallelLoopBody> getInitUndistortRectifyMapComputer(Size _size, Mat &_map1, Mat &_map2, int _m1type, |
|
const double* _ir, 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) |
|
{ |
|
CV_INSTRUMENT_REGION(); |
|
|
|
return Ptr<initUndistortRectifyMapComputer>(new initUndistortRectifyMapComputer(_size, _map1, _map2, _m1type, _ir, _matTilt, _u0, _v0, _fx, _fy, |
|
_k1, _k2, _p1, _p2, _k3, _k4, _k5, _k6, _s1, _s2, _s3, _s4)); |
|
} |
|
|
|
#endif |
|
CV_CPU_OPTIMIZATION_NAMESPACE_END |
|
} |
|
/* End of file */
|
|
|