mat.cpp

这是个人脸识别程序

			else
				sTag1[iLen] = 0;
			strcpy(sTag, sTag1);
			}
		}
	}
while (c != '{');

if (2 != ::fscanf(pFile, "%d %d\n", &nrows, &ncols))
	SysErr("Can't read matrix size from %s [last valid tag string \"%s\"]", sFile, sTag1);

sprintf(sErr, "Implausible number of rows %d or cols %d in %s (file corrupt?) [last valid tag string \"%s\"]", nrows, ncols, sFile, sTag1);
CheckReasonableDim(nrows, ncols, 0,	sErr, sFile);

if (fVerbose)
	lprintf("%dx%d\n", nrows, ncols);

free();
if (nrows && ncols)	// only read if there is some data
	{
	m = gsl_matrix_alloc(nrows, ncols);
	if (0 != gsl_matrix_fscanf(pFile, m))
		SysErr("Can't read matrix from %s (file truncated?) [last valid tag string \"%s\"]", sFile, sTag1);
	}
c = ' ';
while (c == ' ' || c == '\t' || c == '\n' || c == '\r')	// skip white space
	if (1 != ::fread(&c, 1, 1, pFile))
		SysErr("Can't read matrix from %s (reached EOF before finding '}' footer) [last valid tag string \"%s\"]", sFile, sTag1);

if (c != '}')
	SysErr("%s footer not '}' [last valid tag string \"%s\"]", sFile, sTag1);

if (fCloseFile)
	fclose(pFile);

return NULL;		// success
}

//-----------------------------------------------------------------------------
MatView Mat::view (size_t iStartRow, size_t iStartCol, size_t nrows1, size_t ncols1, int Tda)
{
MatView View(*this, iStartRow, iStartCol, nrows1, ncols1, Tda);
return View;
}

//-----------------------------------------------------------------------------
const MatView Mat::view (size_t iStartRow, size_t iStartCol, size_t nrows1, size_t ncols1, int Tda) const
{
MatView View(*this, iStartRow, iStartCol, nrows1, ncols1, Tda);
return View;
}

//-----------------------------------------------------------------------------
VecView Mat::row (size_t iRow)
{
return VecView(*this, iRow, 0, 1, this->ncols());
}

//-----------------------------------------------------------------------------
const VecView Mat::row (size_t iRow) const
{
return VecView(*this, iRow, 0, 1, this->ncols());
}

//-----------------------------------------------------------------------------
VecView Mat::rowAsCol (size_t iRow)
{
return VecView(*this, iRow, 0, this->ncols(), 1, 1);
}

//-----------------------------------------------------------------------------
const VecView Mat::rowAsCol (size_t iRow) const
{
return VecView(*this, iRow, 0, this->ncols(), 1, 1);
}

//-----------------------------------------------------------------------------
VecView Mat::col (size_t iCol)
{
return VecView(*this, 0, iCol, this->nrows(), 1);
}

//-----------------------------------------------------------------------------
const VecView Mat::col (size_t iCol) const
{
return VecView(*this, 0, iCol, this->nrows(), 1);
}

//-----------------------------------------------------------------------------
VecView Mat::viewDiagAsCol ()
{
return VecView(*this, 0, 0, this->nrows(), 1, ncols()+1);
}

//-----------------------------------------------------------------------------
const VecView Mat::viewDiagAsCol () const
{
return VecView(*this, 0, 0, this->nrows(), 1, ncols()+1);
}

//-----------------------------------------------------------------------------
VecView Mat::viewAsRow ()
{
return VecView(*this, 0, 0, 1, this->nrows() * this->ncols());
}

//-----------------------------------------------------------------------------
const VecView Mat::viewAsRow () const
{
return VecView(*this, 0, 0, 1, this->nrows() * this->ncols());
}

//-----------------------------------------------------------------------------
VecView Mat::viewAsCol ()
{
return VecView(*this, 0, 0, this->nrows() * this->ncols(), 1, 1);
}

//-----------------------------------------------------------------------------
const VecView Mat::viewAsCol () const
{
return VecView(*this, 0, 0, this->nrows() * this->ncols(), 1, 1);
}

//-----------------------------------------------------------------------------
MatView Mat::viewAsSquare ()
{
int ncols = int(sqrt(this->nelems()));
DASSERT(ncols * ncols == this->nelems());	// can't square properly (assertion is not essential but protects the user)
return MatView(*this, 0, 0, ncols, ncols, ncols);
}

//-----------------------------------------------------------------------------
const MatView Mat::viewAsSquare () const
{
int ncols = int(sqrt(this->nelems()));
DASSERT(ncols * ncols == this->nelems());	// can't square properly (assertion is not essential but protects the user)
return MatView(*this, 0, 0, ncols, ncols, ncols);
}

//-----------------------------------------------------------------------------
// Multiply a 3x3 matrix by a 1x2 vector. Used for homogenous transforms.
//
// TODO Not yet tested in test.cpp (but tested elsewhere and it works)
// TODO This could be made more efficient by using arrays internally instead of matrices

Vec Mat::mat33TimesVec2 (const Vec &v2) const 	// returns row Vec(2)
{
#if GSL_RANGE_CHECK
CheckDim(v2,    1, 2, "mat33TimesVec2 vector");
CheckDim(*this, 3, 3, "mat33TimesVec2 transform matrix");
#endif

static Vec Vec3(3, ROWVEC);	// static for efficiency
Vec3(VX) = getElem(0, VX) * v2(VX)  +  getElem(0, VY) * v2(VY)  +  getElem(0, VZ);
Vec3(VY) = getElem(1, VX) * v2(VX)  +  getElem(1, VY) * v2(VY)  +  getElem(1, VZ);
Vec3(VZ) = getElem(2, VX) * v2(VX)  +  getElem(2, VY) * v2(VY)  +  getElem(2, VZ);

DASSERT(!fEqual(Vec3(0, VZ), 0)); 	// check for div by zero

static Vec Vec2(2, ROWVEC);
Vec2(VX) = Vec3(0, VX) / Vec3(0, VZ);
Vec2(VY) = Vec3(0, VY) / Vec3(0, VZ);

return Vec2;
}

//-----------------------------------------------------------------------------
// Returns a row vector whose elements are the averages of the columns

Vec Mat::colAverages () const
{
Vec Centroid(ncols(), ROWVEC);	// initialized to 0
int iRow = nrows();
while (iRow--)
	Centroid += row(iRow);
return Centroid / nrows();
}

//-----------------------------------------------------------------------------
// Returns a row vector whose elements are the sums of the columns

Vec Mat::colSums () const
{
Vec Cols(ncols(), ROWVEC);	// initialized to 0
int iRow = nrows();
while (iRow--)
	Cols += row(iRow);
return Cols;
}

//-----------------------------------------------------------------------------
// Returns sum of the given column

double Mat::colSum (int iCol) const
{
double Sum = 0;
int iRow = nrows();
while (iRow--)
	Sum += getElem(iRow, iCol);
return Sum;
}

//-----------------------------------------------------------------------------
// Returns sum of the given row

double Mat::rowSum (int iRow) const
{
double Sum = 0;
int iCol = ncols();
while (iCol--)
	Sum += getElem(iRow, iCol);
return Sum;
}

//-----------------------------------------------------------------------------
// Set elements in the given range to Val
// See also fill(const double &d) in mat.hpp

void Mat::fill(double Val, int iStartRow, int iStartCol, int nEndRow, int nEndCol)
{
for (int iRow = iStartRow; iRow < nEndRow; iRow++)
	for (int iCol = iStartCol; iCol < nEndCol; iCol++)
		gsl_matrix_set(m, iRow, iCol, Val);
}

//-----------------------------------------------------------------------------
// Type convert a one element matrix to a double.
// It's probably possible to do this type conversion automatically but making
// the programmer do it explicitly helps catch errors.

double OneElemToDouble (const Mat &A)
{
DASSERT(A.nrows() == 1 && A.ncols() == 1);
return A(0, 0);
}

//-----------------------------------------------------------------------------
bool fSameDim (const Mat &A, const Mat &B)
{
return A.nrows() == B.nrows() && A.ncols() == B.ncols();
}

//-----------------------------------------------------------------------------
void CheckReasonableDim (size_t nrows, size_t ncols, int nMin, const char *sMsg, const char *sFile)
{
if ((int)nrows < nMin || nrows > CONF_nMaxMatDim || (int)ncols < nMin || ncols > CONF_nMaxMatDim)
	SysErr(sMsg, nrows, ncols, sFile);
}

//-----------------------------------------------------------------------------
void CheckSameDim (const Mat &A, const Mat &B, const char *sMsg)
{
if (!fSameDim(A, B))
	SysErr("%s matrices have different sizes %dx%d %dx%d", sMsg, A.nrows(), A.ncols(), B.nrows(), B.ncols());
}

//-----------------------------------------------------------------------------
void CheckSameNbrRows (const Mat &A, const Mat &B, const char *sMsg)
{
if ( A.nrows() != B.nrows())
	SysErr("%s matrices have different number of rows %dx%d %dx%d", sMsg, A.nrows(), A.ncols(), B.nrows(), B.ncols());
}

//-----------------------------------------------------------------------------
void CheckSameNbrCols (const Mat &A, const Mat &B, const char *sMsg)
{
if ( A.ncols() != B.ncols())
	SysErr("%s matrices have different number of columns %dx%d %dx%d", sMsg, A.nrows(), A.ncols(), B.nrows(), B.ncols());
}

//-----------------------------------------------------------------------------
void CheckDim (const Mat &A, size_t nrows, size_t ncols, const char *sMsg)
{
if (A.nrows() != nrows || A.ncols() != ncols)
	SysErr("%s wrong size matrix %dx%d expected %dx%d", sMsg, A.nrows(), A.ncols(), nrows, ncols);
}

//-----------------------------------------------------------------------------
void Check2Cols (const Mat &A, const char *sMsg)
{
if (A.ncols() != 2)
	SysErr("%s expected two columns in matrix %dx%d", sMsg, A.nrows(), A.ncols());
}

//-----------------------------------------------------------------------------
void CheckIsVector (const Mat &A, const char *sMsg)
{
if (!A.fVector())
	SysErr("%s %dx%d isn't a vector", sMsg, A.nrows(), A.ncols());
}

//-----------------------------------------------------------------------------
void CheckIsSquare (const Mat &A, const char *sMsg)
{
if (!A.fSquare())
	SysErr("%s matrix %dx%d isn't square", sMsg, A.nrows(), A.ncols());
}

//-----------------------------------------------------------------------------
bool fEqualMat (const Mat &A, const Mat &B, double MaxDiff)
{
#if GSL_RANGE_CHECK
CheckSameDim(A, B, "Mat:fEqualMat");		// SysErr if not same size
#endif

for (int iRow = 0; iRow < A.nrows(); iRow++)
	for (int iCol = 0; iCol < A.ncols(); iCol++)
		if (!fEqual(A(iRow, iCol), B(iRow, iCol), MaxDiff))
			{
			lprintf("A(%d,%d) %g != B(%d,%d) %g\n", iRow, iCol, A(iRow, iCol), iRow, iCol, B(iRow, iCol));
			return false;
			}

return true;
}

//-----------------------------------------------------------------------------
// Returns a col vector of elements in A that are in the given range.
// Note that if A is multidimensional matrix,  we still return a row vector,
// formed by row-wise concatenation of in-range elements of A.
//
// If fInRange is clear than return elements OUT of the given range.
//
// Note: we use "<=" for in-range and "<" for out-of-range checks.

Vec GetElemsInRange(const Mat &A, double GreaterThan, double LessThan, bool fInRange)
{
int iRow, iCol, iElem = 0;

// figure out the length of the new vector

for (iRow = 0; iRow < A.nrows(); iRow++)
	for (iCol = 0; iCol < A.ncols(); iCol++)
		{
		double Elem = A(iRow, iCol);
		if ((fInRange && Elem >= GreaterThan && Elem <= LessThan)
			|| (!fInRange && (Elem < GreaterThan || Elem > LessThan)))
			{
			iElem++;
			}
		}
// copy the matching elements into results

Vec Result(iElem);
iElem = 0;
for (iRow = 0; iRow < A.nrows(); iRow++)
	for (iCol = 0; iCol < A.ncols(); iCol++)
		{
		double Elem = A(iRow, iCol);
		if ((fInRange && Elem >= GreaterThan && Elem <= LessThan)
			|| (!fInRange && (Elem < GreaterThan || Elem > LessThan)))
			{
			gsl_matrix_set(Result.m, iElem++, 0, Elem);
			}
		}
return Result;
}

//-----------------------------------------------------------------------------
// Same as GetElemsInRange above but does component-wise in-range checks against
// range vectors (in the above function the ranges are scalars).

Vec	GetElemsInRange (const Mat &A, const Mat &GreaterThan, const Mat &LessThan, bool fInRange)
{
int iRow, iCol, iElem = 0;

#if GSL_RANGE_CHECK
CheckSameDim(A, GreaterThan, "GetElemsInRange");		// SysErr if not same size
CheckSameDim(A, LessThan,    "GetElemsInRange");
#endif

// figure out the length of the new vector

for (iRow = 0; iRow < A.nrows(); iRow++)
	for (iCol = 0; iCol < A.ncols(); iCol++)
		{
		double Elem = A(iRow, iCol);
		if ((fInRange && Elem >= GreaterThan(iRow, iCol) && Elem <= LessThan(iRow, iCol))
			|| (!fInRange && (Elem < GreaterThan(iRow, iCol) || Elem > LessThan(iRow, iCol))))
			{
			iElem++;
			}
		}
// copy the matching elements into results

Vec Result(iElem);
iElem = 0;
for (iRow = 0; iRow < A.nrows(); iRow++)
	for (iCol = 0; iCol < A.ncols(); iCol++)
		{
		double Elem = A(iRow, iCol);
		if ((fInRange && Elem >= GreaterThan(iRow, iCol) && Elem <= LessThan(iRow, iCol))
			|| (!fInRange && (Elem < GreaterThan(iRow, iCol) || Elem > LessThan(iRow, iCol))))
			{
			gsl_matrix_set(Result.m, iElem++, 0, Elem);
			}
		}
return Result;
}

//-----------------------------------------------------------------------------
// Solves Ax=b by LU decomposition.  Returns col vec x and LU factorization in LU.
// A doesn't get changed.
// This allocates and inits ppPerm, caller must free pPerm.
//
// If pSmallestPivot is not null then the smallest pivot is stored in it.
// (If the smallest pivot is 0 then A is singular and can't solve using this function.)
//
// If matrix is singular this will fail (it doesn't do least squares or anything like that)

Vec SolveWithLU (const Mat &A, Vec &b, double *pSmallestPivot, Mat &LU, gsl_permutation **ppPerm)
{
#if GSL_RANGE_CHECK
CheckSameDim(A, LU, "SolveWithLU");
CheckIsVector(b, "SolveWithLU b");
if (A.nrows() != b.nelems())
	SysErr("LUSolve b %dx%d doesn't conform to A %dx%d",
		b.nrows(), b.ncols(), A.nrows(), A.ncols());
#endif

int	Sign, iRet, nrows = A.nrows();

gsl_permutation *pPerm = gsl_permutation_alloc(nrows);
gsl_vector 		*xVec  = gsl_vector_alloc(nrows);
gsl_vector_view  bVec  = gsl_vector_view_array(b.m->data, nrows);

LU = A;	// use copy not view because LU_solve destroys A

if (0 != (iRet = gsl_linalg_LU_decomp(LU.m, pPerm, &Sign)))
	SysErr("LUSolve gsl_linalg_LU_decomp returned %d", iRet);

if (pSmallestPivot)		// find and store the smallest pivot?
	{
	*pSmallestPivot = GSL_POSINF;
	for (int i = LU.nrows()-1; i >= 0; i--)	// smallest last so reduce nbr assignments
		if (fabs(LU(i,i)) < *pSmallestPivot)