in modules/core/src/lapack.cpp [792:1042]
double cv::invert( InputArray _src, OutputArray _dst, int method )
{
bool result = false;
Mat src = _src.getMat();
int type = src.type();
CV_Assert(type == CV_32F || type == CV_64F);
size_t esz = CV_ELEM_SIZE(type);
int m = src.rows, n = src.cols;
if( method == DECOMP_SVD )
{
int nm = std::min(m, n);
AutoBuffer<uchar> _buf((m*nm + nm + nm*n)*esz + sizeof(double));
uchar* buf = alignPtr((uchar*)_buf, (int)esz);
Mat u(m, nm, type, buf);
Mat w(nm, 1, type, u.ptr() + m*nm*esz);
Mat vt(nm, n, type, w.ptr() + nm*esz);
SVD::compute(src, w, u, vt);
SVD::backSubst(w, u, vt, Mat(), _dst);
return type == CV_32F ?
(w.ptr<float>()[0] >= FLT_EPSILON ?
w.ptr<float>()[n-1]/w.ptr<float>()[0] : 0) :
(w.ptr<double>()[0] >= DBL_EPSILON ?
w.ptr<double>()[n-1]/w.ptr<double>()[0] : 0);
}
CV_Assert( m == n );
if( method == DECOMP_EIG )
{
AutoBuffer<uchar> _buf((n*n*2 + n)*esz + sizeof(double));
uchar* buf = alignPtr((uchar*)_buf, (int)esz);
Mat u(n, n, type, buf);
Mat w(n, 1, type, u.ptr() + n*n*esz);
Mat vt(n, n, type, w.ptr() + n*esz);
eigen(src, w, vt);
transpose(vt, u);
SVD::backSubst(w, u, vt, Mat(), _dst);
return type == CV_32F ?
(w.ptr<float>()[0] >= FLT_EPSILON ?
w.ptr<float>()[n-1]/w.ptr<float>()[0] : 0) :
(w.ptr<double>()[0] >= DBL_EPSILON ?
w.ptr<double>()[n-1]/w.ptr<double>()[0] : 0);
}
CV_Assert( method == DECOMP_LU || method == DECOMP_CHOLESKY );
_dst.create( n, n, type );
Mat dst = _dst.getMat();
if( n <= 3 )
{
const uchar* srcdata = src.ptr();
uchar* dstdata = dst.ptr();
size_t srcstep = src.step;
size_t dststep = dst.step;
if( n == 2 )
{
if( type == CV_32FC1 )
{
double d = det2(Sf);
if( d != 0. )
{
result = true;
d = 1./d;
#if CV_SSE2
if(USE_SSE2)
{
__m128 zero = _mm_setzero_ps();
__m128 t0 = _mm_loadl_pi(zero, (const __m64*)srcdata); //t0 = sf(0,0) sf(0,1)
__m128 t1 = _mm_loadh_pi(zero, (const __m64*)(srcdata+srcstep)); //t1 = sf(1,0) sf(1,1)
__m128 s0 = _mm_or_ps(t0, t1);
__m128 det =_mm_set1_ps((float)d);
s0 = _mm_mul_ps(s0, det);
static const uchar CV_DECL_ALIGNED(16) inv[16] = {0,0,0,0,0,0,0,0x80,0,0,0,0x80,0,0,0,0};
__m128 pattern = _mm_load_ps((const float*)inv);
s0 = _mm_xor_ps(s0, pattern);//==-1*s0
s0 = _mm_shuffle_ps(s0, s0, _MM_SHUFFLE(0,2,1,3));
_mm_storel_pi((__m64*)dstdata, s0);
_mm_storeh_pi((__m64*)((float*)(dstdata+dststep)), s0);
}
else
#endif
{
double t0, t1;
t0 = Sf(0,0)*d;
t1 = Sf(1,1)*d;
Df(1,1) = (float)t0;
Df(0,0) = (float)t1;
t0 = -Sf(0,1)*d;
t1 = -Sf(1,0)*d;
Df(0,1) = (float)t0;
Df(1,0) = (float)t1;
}
}
}
else
{
double d = det2(Sd);
if( d != 0. )
{
result = true;
d = 1./d;
#if CV_SSE2
if(USE_SSE2)
{
__m128d s0 = _mm_loadu_pd((const double*)srcdata); //s0 = sf(0,0) sf(0,1)
__m128d s1 = _mm_loadu_pd ((const double*)(srcdata+srcstep));//s1 = sf(1,0) sf(1,1)
__m128d sm = _mm_unpacklo_pd(s0, _mm_load_sd((const double*)(srcdata+srcstep)+1)); //sm = sf(0,0) sf(1,1) - main diagonal
__m128d ss = _mm_shuffle_pd(s0, s1, _MM_SHUFFLE2(0,1)); //ss = sf(0,1) sf(1,0) - secondary diagonal
__m128d det = _mm_load1_pd((const double*)&d);
sm = _mm_mul_pd(sm, det);
static const uchar CV_DECL_ALIGNED(16) inv[8] = {0,0,0,0,0,0,0,0x80};
__m128d pattern = _mm_load1_pd((double*)inv);
ss = _mm_mul_pd(ss, det);
ss = _mm_xor_pd(ss, pattern);//==-1*ss
s0 = _mm_shuffle_pd(sm, ss, _MM_SHUFFLE2(0,1));
s1 = _mm_shuffle_pd(ss, sm, _MM_SHUFFLE2(0,1));
_mm_storeu_pd((double*)dstdata, s0);
_mm_storeu_pd((double*)(dstdata+dststep), s1);
}
else
#endif
{
double t0, t1;
t0 = Sd(0,0)*d;
t1 = Sd(1,1)*d;
Dd(1,1) = t0;
Dd(0,0) = t1;
t0 = -Sd(0,1)*d;
t1 = -Sd(1,0)*d;
Dd(0,1) = t0;
Dd(1,0) = t1;
}
}
}
}
else if( n == 3 )
{
if( type == CV_32FC1 )
{
double d = det3(Sf);
if( d != 0. )
{
double t[12];
result = true;
d = 1./d;
t[0] = (((double)Sf(1,1) * Sf(2,2) - (double)Sf(1,2) * Sf(2,1)) * d);
t[1] = (((double)Sf(0,2) * Sf(2,1) - (double)Sf(0,1) * Sf(2,2)) * d);
t[2] = (((double)Sf(0,1) * Sf(1,2) - (double)Sf(0,2) * Sf(1,1)) * d);
t[3] = (((double)Sf(1,2) * Sf(2,0) - (double)Sf(1,0) * Sf(2,2)) * d);
t[4] = (((double)Sf(0,0) * Sf(2,2) - (double)Sf(0,2) * Sf(2,0)) * d);
t[5] = (((double)Sf(0,2) * Sf(1,0) - (double)Sf(0,0) * Sf(1,2)) * d);
t[6] = (((double)Sf(1,0) * Sf(2,1) - (double)Sf(1,1) * Sf(2,0)) * d);
t[7] = (((double)Sf(0,1) * Sf(2,0) - (double)Sf(0,0) * Sf(2,1)) * d);
t[8] = (((double)Sf(0,0) * Sf(1,1) - (double)Sf(0,1) * Sf(1,0)) * d);
Df(0,0) = (float)t[0]; Df(0,1) = (float)t[1]; Df(0,2) = (float)t[2];
Df(1,0) = (float)t[3]; Df(1,1) = (float)t[4]; Df(1,2) = (float)t[5];
Df(2,0) = (float)t[6]; Df(2,1) = (float)t[7]; Df(2,2) = (float)t[8];
}
}
else
{
double d = det3(Sd);
if( d != 0. )
{
result = true;
d = 1./d;
double t[9];
t[0] = (Sd(1,1) * Sd(2,2) - Sd(1,2) * Sd(2,1)) * d;
t[1] = (Sd(0,2) * Sd(2,1) - Sd(0,1) * Sd(2,2)) * d;
t[2] = (Sd(0,1) * Sd(1,2) - Sd(0,2) * Sd(1,1)) * d;
t[3] = (Sd(1,2) * Sd(2,0) - Sd(1,0) * Sd(2,2)) * d;
t[4] = (Sd(0,0) * Sd(2,2) - Sd(0,2) * Sd(2,0)) * d;
t[5] = (Sd(0,2) * Sd(1,0) - Sd(0,0) * Sd(1,2)) * d;
t[6] = (Sd(1,0) * Sd(2,1) - Sd(1,1) * Sd(2,0)) * d;
t[7] = (Sd(0,1) * Sd(2,0) - Sd(0,0) * Sd(2,1)) * d;
t[8] = (Sd(0,0) * Sd(1,1) - Sd(0,1) * Sd(1,0)) * d;
Dd(0,0) = t[0]; Dd(0,1) = t[1]; Dd(0,2) = t[2];
Dd(1,0) = t[3]; Dd(1,1) = t[4]; Dd(1,2) = t[5];
Dd(2,0) = t[6]; Dd(2,1) = t[7]; Dd(2,2) = t[8];
}
}
}
else
{
assert( n == 1 );
if( type == CV_32FC1 )
{
double d = Sf(0,0);
if( d != 0. )
{
result = true;
Df(0,0) = (float)(1./d);
}
}
else
{
double d = Sd(0,0);
if( d != 0. )
{
result = true;
Dd(0,0) = 1./d;
}
}
}
if( !result )
dst = Scalar(0);
return result;
}
int elem_size = CV_ELEM_SIZE(type);
AutoBuffer<uchar> buf(n*n*elem_size);
Mat src1(n, n, type, (uchar*)buf);
src.copyTo(src1);
setIdentity(dst);
if( method == DECOMP_LU && type == CV_32F )
result = hal::LU(src1.ptr<float>(), src1.step, n, dst.ptr<float>(), dst.step, n) != 0;
else if( method == DECOMP_LU && type == CV_64F )
result = hal::LU(src1.ptr<double>(), src1.step, n, dst.ptr<double>(), dst.step, n) != 0;
else if( method == DECOMP_CHOLESKY && type == CV_32F )
result = hal::Cholesky(src1.ptr<float>(), src1.step, n, dst.ptr<float>(), dst.step, n);
else
result = hal::Cholesky(src1.ptr<double>(), src1.step, n, dst.ptr<double>(), dst.step, n);
if( !result )
dst = Scalar(0);
return result;
}