matrix.cpp

opencv实现的人体运动跟踪源码

    fclose( file );
#endif
    //return CV_NO_ERR;
	return true;
}

BOOL CMatrix::SetIdentity(double *Matr, int width, int height)
{
    int i, j;
    double *dst = Matr;
    int Step = width;

    if( !Matr )
        return false;
    if( (width <= 0) || (height <= 0) )
        return false;
    for( j = 0; j < height; j++ )
    {
        for( i = 0; i < width; i++ )
        {
            dst[i] = (i == j) ? 1.0 : 0.0;
        }
        dst += Step;
    }

    return true;
}

BOOL CMatrix::ScaleVector(double *src, double *dest, int length, double value)
{
    int i;
    double val = value;

    if( src == NULL )
        return false;
    if( dest == NULL )
        return false;
    if( length <= 0 )
        return false;

    for( i = 0; i < length; i++ )
    {
        dest[i] = src[i] * val;
    }

    return true;
}

BOOL CMatrix::SubVector(double *src1, double *src2, double *dest, int length)
{
    int i;

    /* check bad arguments */
    if( src1 == NULL )
        return false;
    if( src2 == NULL )
        return false;
    if( dest == NULL )
        return false;
    if( length <= 0 )
        return false;

    for( i = 0; i < length; i++ )
    {
        dest[i] = src1[i] - src2[i];
    }

    return true;
}

BOOL CMatrix::MulTransposed(double * src, int srcHeight, int srcWidth, double * dst, int dstHeight, int dstWidth, int order )
{
    if( order )
    {
        if( (dstHeight != srcWidth) || (dstWidth != srcWidth) )
            return false;

        MulTransMatrixR(src, srcWidth, srcHeight, dst);
    }
    else
    {
        if( (dstHeight != srcHeight) || (dstWidth != srcHeight) )
            return false;

        MulTransMatrixL(src, srcWidth, srcHeight, dst);
    }

	return true;
}

BOOL CMatrix::MulTransMatrixR(double *srcMatr, int matrWidth,int matrHeight, double *destMatr)
{
    int stepm = matrWidth;
    int i, j;
    double *C = destMatr, *col_i;

    if( srcMatr == NULL || C == NULL || srcMatr == C )
        return false;
    if( matrHeight <= 0 || matrWidth <= 0 )
        return false;

    //col_i = (double *) icvAlloc( sizeof( double ) * matrHeight );
	col_i = new double[matrHeight];

    for( i = 0; i < matrWidth; i++, C += stepm )
    {
        for( j = 0; j < matrHeight; j++ )
            col_i[j] = srcMatr[j * stepm + i];
        for( j = i; j < matrWidth; j++ )
        {
            int st = stepm, k = 0;
            double s = 0.0f, *A = srcMatr;

            for( ; k < matrHeight; k++, A += st )
                s += col_i[k] * A[j];
            C[j] = s;
        }
    }
    //icvFree( (void **) &col_i );
	delete [] col_i;

    for( i = 1; i < matrWidth; i++ )
        for( j = 0; j < i; j++ )
            destMatr[j + i * stepm] = destMatr[i + j * stepm];

    return true;
}

BOOL CMatrix::MulTransMatrixL(double *srcMatr,int matrWidth, int matrHeight, double *destMatr)
{
    int i, j, k;
    double Result;

    for( j = 0; j < matrHeight; j++ )
        for( i = j; i < matrHeight; i++ )
        {
            Result = 0.;
            for( k = 0; k < matrWidth; k++ )
                Result += srcMatr[i * matrWidth + k] * srcMatr[j * matrWidth + k];
            destMatr[j * matrHeight + i] = Result;
        }
    for( j = 0; j < matrHeight; j++ )
        for( i = j; i < matrHeight; i++ )
            destMatr[i * matrHeight + j] = destMatr[j * matrHeight + i];

    return true;
}

double CMatrix::Trace(double *matr, int width, int height)
{
    int t;
    int size;
    double tr;

    size = min( width, height );
    tr = 0;
    for( t = 0; t < size; t++ )
    {
        tr += matr[t * width + t];
    }
    
    return tr;
}

double CMatrix::DotProduct(double *src1, double *src2, int length)
{
    double sum = 0.0;
   
    for(int i = 0; i < length; i++ )
    {
        sum += src1[i] * src2[i];
    }

    return sum;
}

double CMatrix::Det(double *matr, int matrSize)
{
    //CvMatr64d matrWork;
    //CvVect64d tmpLine;
	double *matrWork;
	double *tmpLine;

	double det;

    int i, j, t;
    int change_sign;
    double coef;
    double mul;
    double norm = 0, eps, eps0 = 1.0e-40;

    //if( matr == NULL || det == NULL ) return CV_NULLPTR_ERR;
    //if( matrSize < 1 ) return CV_BADFACTOR_ERR;

    if( matrSize == 1 )
    {
        //*det = *matr;
        //return CV_NO_ERR;
		return *matr;
    }

    if( matrSize == 2 )
    {
        //*det = matr[0]*matr[3] - matr[1]*matr[2];
        //return CV_NO_ERR;
		return matr[0]*matr[3] - matr[1]*matr[2];
    }

    if( matrSize == 3 )
    {
/*        *det = matr[0] * ( matr[4]*matr[8] - matr[5]*matr[7] )
             + matr[3] * ( matr[2]*matr[7] - matr[1]*matr[8] )
             + matr[6] * ( matr[1]*matr[5] - matr[2]*matr[4] );
        return CV_NO_ERR;
*/
		return  matr[0] * ( matr[4]*matr[8] - matr[5]*matr[7] )
             + matr[3] * ( matr[2]*matr[7] - matr[1]*matr[8] )
             + matr[6] * ( matr[1]*matr[5] - matr[2]*matr[4] );
    }

    if( matrSize == 4 )
    {
/*        *det = matr[0] * (matr[5] * (matr[10]*matr[15] - matr[11]*matr[14])
                       +  matr[6] * (matr[11]*matr[13] - matr[9] *matr[15])
                       +  matr[7] * (matr[9] *matr[14] - matr[10]*matr[13]) );

        *det+= matr[1] * (matr[6] * (matr[8] *matr[15] - matr[11]*matr[12])
                       + matr[7]  * (matr[10]*matr[12] - matr[8] *matr[14])
                       + matr[4]  * (matr[11]*matr[14] - matr[10]*matr[15]) );

        *det+= matr[2] * (matr[7] * (matr[8] *matr[13] - matr[9] *matr[12])
                       + matr[4]  * (matr[9] *matr[15] - matr[11]*matr[13])
                       + matr[5]  * (matr[11]*matr[12] - matr[8] *matr[15]) );

        *det+= matr[3] * (matr[4] * (matr[10]*matr[13] - matr[9] *matr[14])
                       + matr[5]  * (matr[8] *matr[14] - matr[10]*matr[12])
                       + matr[6]  * (matr[9] *matr[12] - matr[8] *matr[13]) );
        return CV_NO_ERR;
*/
		det = matr[0] * (matr[5] * (matr[10]*matr[15] - matr[11]*matr[14])
                       +  matr[6] * (matr[11]*matr[13] - matr[9] *matr[15])
                       +  matr[7] * (matr[9] *matr[14] - matr[10]*matr[13]) );

        det+= matr[1] * (matr[6] * (matr[8] *matr[15] - matr[11]*matr[12])
                       + matr[7]  * (matr[10]*matr[12] - matr[8] *matr[14])
                       + matr[4]  * (matr[11]*matr[14] - matr[10]*matr[15]) );

        det+= matr[2] * (matr[7] * (matr[8] *matr[13] - matr[9] *matr[12])
                       + matr[4]  * (matr[9] *matr[15] - matr[11]*matr[13])
                       + matr[5]  * (matr[11]*matr[12] - matr[8] *matr[15]) );

        det+= matr[3] * (matr[4] * (matr[10]*matr[13] - matr[9] *matr[14])
                       + matr[5]  * (matr[8] *matr[14] - matr[10]*matr[12])
                       + matr[6]  * (matr[9] *matr[12] - matr[8] *matr[13]) );
        return det;

    }

    change_sign = 0;

    for( i=0; i<matrSize*matrSize; i++ )
        norm += fabs( matr[i] );

    norm /= matrSize*matrSize;
    eps = eps0 * norm;

    //matrWork = icvCreateMatrix_64d( matrSize, matrSize );
    //tmpLine  = icvCreateVector_64d( matrSize );
	matrWork = new double[matrSize * matrSize];
	tmpLine = new double[matrSize];

    //icvCopyMatrix_64d( matr, matrSize, matrSize, matrWork );
	memcpy(matrWork, matr, matrSize * matrSize * sizeof(double));

    /* Make matrix is up triangular */
    for( i = 0; i < matrSize - 1; i++ )
    {
        int     in = i * matrSize;
        double  mx = fabs( matrWork[in+i] );
        int change = i;

        /* find maximal element */

        for( t = i+1; t < matrSize; t++ )
        {
            double w = fabs( matrWork[t*matrSize + i] );
            if( w > mx )
            {
                mx     = w;
                change = t;
            }
        }

        if( mx < eps ) /* Element != 0 not found;  det = 0 */
        {
/*            *det = 0.0;
            icvDeleteMatrix( matrWork );
            icvDeleteVector( tmpLine );
            return CV_NO_ERR;
*/
			delete [] matrWork;
			delete [] tmpLine;
			return 0.0;

        }

        if( change != i ) /* change lines */
        {
            int nchange = change * matrSize;

            //icvCopyVector_64d( matrWork + nchange, matrSize,  tmpLine );
            //icvCopyVector_64d( matrWork + in,      matrSize,  matrWork + nchange );
            //icvCopyVector_64d( tmpLine,            matrSize,  matrWork + in );
			memcpy(tmpLine, matrWork + nchange, matrSize * sizeof(double));
			memcpy(matrWork + nchange, matrWork + in, matrSize * sizeof(double));
			memcpy(matrWork + in, tmpLine, matrSize * sizeof(double));

            change_sign++;
        }

        /* Virtual set elements to zero */

        coef = 1.0e0 / matrWork[in+i];

        for( j = i+1; j < matrSize; j++ )
        {
            int jn = j * matrSize;

            if( matrWork[jn + i] )
            {
                mul = coef * matrWork[jn + i];

                //icvScaleVector_64d( matrWork+in+i+1, tmpLine, matrSize-i-1, mul );
                //icvSubVector_64d  ( matrWork+jn+i+1, tmpLine, matrWork+jn+i+1, matrSize-i-1 );
				ScaleVector(matrWork + in + i + 1, tmpLine, matrSize - i - 1, mul);
				SubVector(matrWork + jn + i + 1, tmpLine, matrWork + jn + i + 1, matrSize - i - 1);

            }
        }

    }  /* i */

    det = ( (change_sign & 1) == 1 ) ? -1.0e0 : 1.0e0;

    for( t=0, j=0; t < matrSize; t++, j += matrSize+1 )
        det *= matrWork[j];

    //icvDeleteMatrix( matrWork );
	//icvDeleteVector( tmpLine );
	delete [] matrWork;
	delete [] tmpLine;

    return det;
}      

BOOL CMatrix::EigenVV(double *src, int srcHeight, double * evects, int evectsHeight, 
			  double * evals, int evalsNum, double eps )
{

    if( (srcHeight != evectsHeight) || (evalsNum != srcHeight) )
        return false;

    if (JacobiEigens( src, evects, evals, srcHeight, eps))
		return true;

	return false;
}

BOOL CMatrix::SVDNew(double *A, int height, int width, double *w, double *v)
{
	//require height >= width

	if (height < width) return false;

	double *a = new double[height * width];
	double *vT = new double[width * width];
	double *u = new double[height * height];

	memcpy(a, A, height * width * sizeof(double));
	double eps = 0.0000001;
	
	int ka = height > width ? height : width;
	
	if (!bmuav(a, height, width, u, vT, eps, ka)) {
		delete [] a;
		delete [] vT;
		delete [] u;
		return false;
	}

	double *p1;
	double *p2;

	for (int i = 0; i < height; i++) {
		p1 = A + i * width;
		p2 = u + i * height;
		for ( int j = 0; j < width; j++) {
			*p1++ = *p2++;
		}
	}

	memset(w, 0, sizeof(double) * width * width);

	for ( i = 0; i < width; i++) {
		p1 = w + i * width + i;
		p2 = a + i * width + i;
		*p1 = *p2;
	}

	Transpose(vT, width, width, v);
	
	delete [] a;
	delete [] vT;
	delete [] u;
	return true;

}

// A = U*E*VT   A m*n   U m*m   V n*n
// a = A
// when returned :
// a = E
// u = U
// v = VT

BOOL CMatrix::bmuav(double *a, int m, int n, double *u, double *v, double eps, int ka)
{ 
	int i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks;
	double d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];
    double *s,*e,*w;
    s = new double[ka];
    e = new double[ka];
    w = new double[ka];

    it = 60; k = n;
    if (m-1<n) k=m-1;
    l=m;
    if (n-2<m) l=n-2;
    if (l<0) l=0;
    ll=k;
    if (l>k) ll=l;
    if (ll>=1)
      { for (kk=1; kk<=ll; kk++)
          { if (kk<=k)
              { d=0.0;
                for (i=kk; i<=m; i++)
                  { ix=(i-1)*n+kk-1; d=d+a[ix]*a[ix];}
                s[kk-1]=sqrt(d);
                if (s[kk-1]!=0.0)
                  { ix=(kk-1)*n+kk-1;
                    if (a[ix]!=0.0)
                      { s[kk-1]=fabs(s[kk-1]);
                        if (a[ix]<0.0) s[kk-1]=-s[kk-1];
                      }
                    for (i=kk; i<=m; i++)
                      { iy=(i-1)*n+kk-1;
                        a[iy]=a[iy]/s[kk-1];
                      }
                    a[ix]=1.0+a[ix];
                  }