generic-library/opencv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

improved accuracy of the matrix determinant and matrix inversion functions (trac #431)

Browse Source
This commit is contained in:
Vadim Pisarevsky
2010-07-13 14:17:49 +00:00
parent fa91788222
commit da02688429
2 changed files with 248 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

444
modules/core/src/lapack.cpp
View File

@@ -229,79 +229,86 @@ double determinant( const Mat& mat )
size_t step = mat.step; size_t step = mat.step;
const uchar* m = mat.data; const uchar* m = mat.data;
CV_Assert( mat.rows == mat.cols ); CV_Assert( mat.rows == mat.cols && (type == CV_32F || type == CV_64F));
#define Mf(y, x) ((float*)(m + y*step))[x] #define Mf(y, x) ((float*)(m + y*step))[x]
#define Md(y, x) ((double*)(m + y*step))[x] #define Md(y, x) ((double*)(m + y*step))[x]
if( type == CV_32F ) if( rows <= 10 )
{ {
if( rows == 2 ) if( type == CV_32F )
result = det2(Mf);
else if( rows == 3 )
result = det3(Mf);
else if( rows == 1 )
result = Mf(0,0);
else
{ {
integer i, n = rows, *ipiv, info=0; if( rows == 2 )
int bufSize = n*n*sizeof(float) + (n+1)*sizeof(ipiv[0]), sign=0; result = det2(Mf);
AutoBuffer<uchar> buffer(bufSize); else if( rows == 3 )
result = det3(Mf);
Mat a(n, n, CV_32F, (uchar*)buffer); else if( rows == 1 )
mat.copyTo(a); result = Mf(0,0);
else
ipiv = (integer*)cvAlignPtr(a.data + a.step*a.rows, sizeof(integer));
sgetrf_(&n, &n, (float*)a.data, &n, ipiv, &info);
assert(info >= 0);
if( info == 0 )
{ {
result = 1; size_t bufSize = rows*rows*sizeof(float);
for( i = 0; i < n; i++ ) AutoBuffer<uchar> buffer(bufSize);
Mat a(rows, rows, CV_32F, (uchar*)buffer);
mat.copyTo(a);
result = LU((float*)a.data, rows, 0, 0);
if( result )
{ {
result *= ((float*)a.data)[i*(n+1)]; for( int i = 0; i < rows; i++ )
sign ^= ipiv[i] != i+1; result *= ((const float*)(a.data + a.step*i))[i];
result = 1./result;
} }
result *= sign ? -1 : 1;
} }
} }
}
else if( type == CV_64F )
{
if( rows == 2 )
result = det2(Md);
else if( rows == 3 )
result = det3(Md);
else if( rows == 1 )
result = Md(0,0);
else else
{ {
integer i, n = rows, *ipiv, info=0; if( rows == 2 )
int bufSize = n*n*sizeof(double) + (n+1)*sizeof(ipiv[0]), sign=0; result = det2(Md);
AutoBuffer<uchar> buffer(bufSize); else if( rows == 3 )
result = det3(Md);
Mat a(n, n, CV_64F, (uchar*)buffer); else if( rows == 1 )
mat.copyTo(a); result = Md(0,0);
ipiv = (integer*)cvAlignPtr(a.data + a.step*a.rows, sizeof(integer)); else
dgetrf_(&n, &n, (double*)a.data, &n, ipiv, &info);
assert(info >= 0);
if( info == 0 )
{ {
result = 1; size_t bufSize = rows*rows*sizeof(double);
for( i = 0; i < n; i++ ) AutoBuffer<uchar> buffer(bufSize);
Mat a(rows, rows, CV_64F, (uchar*)buffer);
mat.copyTo(a);
result = LU((double*)a.data, rows, 0, 0);
if( result )
{ {
result *= ((double*)a.data)[i*(n+1)]; for( int i = 0; i < rows; i++ )
sign ^= ipiv[i] != i+1; result *= ((const double*)(a.data + a.step*i))[i];
result = 1./result;
} }
result *= sign ? -1 : 1;
} }
} }
} }
else else
CV_Error( CV_StsUnsupportedFormat, "" ); {
integer i, n = rows, *ipiv, info=0, sign = 0;
size_t bufSize = n*n*sizeof(double) + (n+1)*sizeof(ipiv[0]);
AutoBuffer<uchar> buffer(bufSize);
Mat a(n, n, CV_64F, (uchar*)buffer);
mat.convertTo(a, CV_64F);
ipiv = (integer*)cvAlignPtr(a.data + a.step*a.rows, sizeof(integer));
dgetrf_(&n, &n, (double*)a.data, &n, ipiv, &info);
assert(info >= 0);
if( info == 0 )
{
result = 1;
for( i = 0; i < n; i++ )
{
result *= ((double*)a.data)[i*(n+1)];
sign ^= ipiv[i] != i+1;
}
result *= sign ? -1 : 1;
}
}
#undef Mf #undef Mf
#undef Md #undef Md
@@ -341,193 +348,216 @@ double invert( const Mat& src, Mat& dst, int method )
CV_Assert( src.rows == src.cols && (type == CV_32F || type == CV_64F)); CV_Assert( src.rows == src.cols && (type == CV_32F || type == CV_64F));
dst.create( src.rows, src.cols, type ); dst.create( src.rows, src.cols, type );
if( method == DECOMP_LU || method == DECOMP_CHOLESKY ) if( src.rows <= 3 )
{ {
if( src.rows <= 3 ) uchar* srcdata = src.data;
uchar* dstdata = dst.data;
size_t srcstep = src.step;
size_t dststep = dst.step;
if( src.rows == 2 )
{ {
uchar* srcdata = src.data; if( type == CV_32FC1 )
uchar* dstdata = dst.data;
size_t srcstep = src.step;
size_t dststep = dst.step;
if( src.rows == 2 )
{ {
if( type == CV_32FC1 ) double d = det2(Sf);
if( d != 0. )
{ {
double d = det2(Sf); double t0, t1;
if( d != 0. ) result = d;
{ d = 1./d;
double t0, t1; t0 = Sf(0,0)*d;
result = d; t1 = Sf(1,1)*d;
d = 1./d; Df(1,1) = (float)t0;
t0 = Sf(0,0)*d; Df(0,0) = (float)t1;
t1 = Sf(1,1)*d; t0 = -Sf(0,1)*d;
Df(1,1) = (float)t0; t1 = -Sf(1,0)*d;
Df(0,0) = (float)t1; Df(0,1) = (float)t0;
t0 = -Sf(0,1)*d; Df(1,0) = (float)t1;
t1 = -Sf(1,0)*d;
Df(0,1) = (float)t0;
Df(1,0) = (float)t1;
}
}
else
{
double d = det2(Sd);
if( d != 0. )
{
double t0, t1;
result = d;
d = 1./d;
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( src.rows == 3 )
{
if( type == CV_32FC1 )
{
double d = det3(Sf);
if( d != 0. )
{
float t[9];
result = d;
d = 1./d;
t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d);
t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d);
t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d);
t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d);
t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d);
t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d);
t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d);
t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d);
t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d);
Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2];
Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5];
Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8];
}
}
else
{
double d = det3(Sd);
if( d != 0. )
{
double t[9];
result = d;
d = 1./d;
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 else
{ {
assert( src.rows == 1 ); double d = det2(Sd);
if( d != 0. )
if( type == CV_32FC1 )
{ {
double d = Sf(0,0); double t0, t1;
if( d != 0. ) result = d;
{ d = 1./d;
result = d; t0 = Sd(0,0)*d;
Df(0,0) = (float)(1./d); t1 = Sd(1,1)*d;
} Dd(1,1) = t0;
} Dd(0,0) = t1;
else t0 = -Sd(0,1)*d;
{ t1 = -Sd(1,0)*d;
double d = Sd(0,0); Dd(0,1) = t0;
if( d != 0. ) Dd(1,0) = t1;
{
result = d;
Dd(0,0) = 1./d;
}
} }
} }
return result;
} }
else if( src.rows == 3 )
{
if( type == CV_32FC1 )
{
double d = det3(Sf);
if( d != 0. )
{
float t[9];
result = d;
d = 1./d;
src.copyTo(dst); t[0] = (float)((Sf(1,1) * Sf(2,2) - Sf(1,2) * Sf(2,1)) * d);
integer n = dst.cols, lwork=-1, elem_size = CV_ELEM_SIZE(type), t[1] = (float)((Sf(0,2) * Sf(2,1) - Sf(0,1) * Sf(2,2)) * d);
lda = (int)(dst.step/elem_size), piv1=0, info=0; t[2] = (float)((Sf(0,1) * Sf(1,2) - Sf(0,2) * Sf(1,1)) * d);
t[3] = (float)((Sf(1,2) * Sf(2,0) - Sf(1,0) * Sf(2,2)) * d);
t[4] = (float)((Sf(0,0) * Sf(2,2) - Sf(0,2) * Sf(2,0)) * d);
t[5] = (float)((Sf(0,2) * Sf(1,0) - Sf(0,0) * Sf(1,2)) * d);
t[6] = (float)((Sf(1,0) * Sf(2,1) - Sf(1,1) * Sf(2,0)) * d);
t[7] = (float)((Sf(0,1) * Sf(2,0) - Sf(0,0) * Sf(2,1)) * d);
t[8] = (float)((Sf(0,0) * Sf(1,1) - Sf(0,1) * Sf(1,0)) * d);
Df(0,0) = t[0]; Df(0,1) = t[1]; Df(0,2) = t[2];
Df(1,0) = t[3]; Df(1,1) = t[4]; Df(1,2) = t[5];
Df(2,0) = t[6]; Df(2,1) = t[7]; Df(2,2) = t[8];
}
}
else
{
double d = det3(Sd);
if( d != 0. )
{
double t[9];
result = d;
d = 1./d;
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( src.rows == 1 );
if( type == CV_32FC1 )
{
double d = Sf(0,0);
if( d != 0. )
{
result = d;
Df(0,0) = (float)(1./d);
}
}
else
{
double d = Sd(0,0);
if( d != 0. )
{
result = d;
Dd(0,0) = 1./d;
}
}
}
return result;
}
if( dst.cols <= 10 )
{
int n = dst.cols, elem_size = CV_ELEM_SIZE(type);
AutoBuffer<uchar> buf(n*n*2*elem_size);
Mat src1(n, n, type, (uchar*)buf);
Mat dst1(n, n, type, dst.isContinuous() ? dst.data : src1.data + n*n*elem_size);
src.copyTo(src1);
setIdentity(dst1);
if( method == DECOMP_LU && type == CV_32F )
result = LU((float*)src1.data, n, (float*)dst1.data, n);
else if( method == DECOMP_LU && type == CV_64F )
result = LU((double*)src1.data, n, (double*)dst1.data, n);
else if( method == DECOMP_LU && type == CV_32F )
result = Cholesky((float*)src1.data, n, (float*)dst1.data, n);
else
result = Cholesky((double*)src1.data, n, (double*)dst1.data, n);
dst1.copyTo(dst);
result = std::abs(result);
}
else
{
integer n = dst.cols, lwork=-1, lda = n, piv1=0, info=0;
int t_size = type == CV_32F ? n*n*sizeof(double) : 0;
int buf_size = t_size;
AutoBuffer<uchar> buf;
if( method == DECOMP_LU ) if( method == DECOMP_LU )
{ {
int buf_size = (int)(n*sizeof(integer)); double work1 = 0;
AutoBuffer<uchar> buf; dgetri_(&n, (double*)dst.data, &lda, &piv1, &work1, &lwork, &info);
uchar* buffer; lwork = cvRound(work1);
if( type == CV_32F ) buf_size += (int)(n*sizeof(integer) + (lwork + 1)*sizeof(double));
{
real work1 = 0;
sgetri_(&n, (float*)dst.data, &lda, &piv1, &work1, &lwork, &info);
lwork = cvRound(work1);
}
else
{
double work1 = 0;
dgetri_(&n, (double*)dst.data, &lda, &piv1, &work1, &lwork, &info);
lwork = cvRound(work1);
}
buf_size += (int)((lwork + 1)*elem_size);
buf.allocate(buf_size); buf.allocate(buf_size);
buffer = (uchar*)buf; uchar* buffer = (uchar*)buf;
Mat arr = dst;
if( type == CV_32F ) if( type == CV_32F )
{ {
sgetrf_(&n, &n, (float*)dst.data, &lda, (integer*)buffer, &info); arr = Mat(n, n, CV_64F, buffer);
if(info==0) src.convertTo(arr, CV_64F);
sgetri_(&n, (float*)dst.data, &lda, (integer*)buffer, buffer += t_size;
(float*)(buffer + n*sizeof(integer)), &lwork, &info);
} }
else else
{ {
dgetrf_(&n, &n, (double*)dst.data, &lda, (integer*)buffer, &info); src.copyTo(arr);
if(info==0) lda = arr.step/sizeof(double);
dgetri_(&n, (double*)dst.data, &lda, (integer*)buffer,
(double*)cvAlignPtr(buffer + n*sizeof(integer), elem_size), &lwork, &info);
} }
dgetrf_(&n, &n, (double*)arr.data, &lda, (integer*)buffer, &info);
if(info==0)
dgetri_(&n, (double*)arr.data, &lda, (integer*)buffer,
(double*)cvAlignPtr(buffer + n*sizeof(integer), sizeof(double)),
&lwork, &info);
if(info==0 && arr.data != dst.data)
arr.convertTo(dst, dst.type());
} }
else if( method == CV_CHOLESKY ) else if( method == DECOMP_CHOLESKY )
{ {
char L[] = {'L', '\0'}; Mat arr = dst;
if( type == CV_32F ) if( type == CV_32F )
{ {
spotrf_(L, &n, (float*)dst.data, &lda, &info); buf.allocate(buf_size);
if(info==0) arr = Mat(n, n, CV_64F, (uchar*)buf);
spotri_(L, &n, (float*)dst.data, &lda, &info); src.convertTo(arr, CV_64F);
} }
else else
{ {
dpotrf_(L, &n, (double*)dst.data, &lda, &info); src.copyTo(arr);
if(info==0) lda = arr.step/sizeof(double);
dpotri_(L, &n, (double*)dst.data, &lda, &info); }
char L[] = {'L', '\0'};
dpotrf_(L, &n, (double*)arr.data, &lda, &info);
if(info==0)
dpotri_(L, &n, (double*)arr.data, &lda, &info);
if(info==0)
{
completeSymm(arr);
if( arr.data != dst.data )
arr.convertTo(dst, dst.type());
} }
completeSymm(dst);
} }
result = info == 0; result = info == 0;
} }

8
tests/cxcore/src/amath.cpp
View File

@@ -2420,7 +2420,7 @@ void CxCore_DetTest::prepare_to_validation( int )
*((CvScalar*)(test_mat[REF_OUTPUT][0].data.db)) = cvRealScalar(cvTsLU(&test_mat[TEMP][0], 0, 0)); *((CvScalar*)(test_mat[REF_OUTPUT][0].data.db)) = cvRealScalar(cvTsLU(&test_mat[TEMP][0], 0, 0));
} }
//CxCore_DetTest det_test; CxCore_DetTest det_test;
@@ -2475,8 +2475,8 @@ void CxCore_InvertTest::get_test_array_types_and_sizes( int test_case_idx, CvSiz
if( bits & 4 ) if( bits & 4 )
{ {
sizes[INPUT][0] = cvSize(min_size, min_size); sizes[INPUT][0] = cvSize(min_size, min_size);
if( bits & 8 ) if( bits & 16 )
method = CV_SVD_SYM; method = CV_CHOLESKY;
} }
} }
else else
@@ -2536,7 +2536,7 @@ int CxCore_InvertTest::prepare_test_case( int test_case_idx )
{ {
cvTsFloodWithZeros( &test_mat[INPUT][0], ts->get_rng() ); cvTsFloodWithZeros( &test_mat[INPUT][0], ts->get_rng() );
if( method == CV_SVD_SYM ) if( method == CV_CHOLESKY )
{ {
cvTsGEMM( &test_mat[INPUT][0], &test_mat[INPUT][0], 1., cvTsGEMM( &test_mat[INPUT][0], &test_mat[INPUT][0], 1.,
0, 0., &test_mat[TEMP][0], CV_GEMM_B_T ); 0, 0., &test_mat[TEMP][0], CV_GEMM_B_T );