asm.cpp

这是个人脸识别程序

// $masm\asm.cpp 1.5 milbo$ utilities for active contour models
// Warning: this is raw research code -- expect it to be quite messy.
// milbo durban dec05
//-----------------------------------------------------------------------------
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// A copy of the GNU General Public License is available at
// http://www.r-project.org/Licenses/
//-----------------------------------------------------------------------------

#include "all.hpp"

//-----------------------------------------------------------------------------
bool fPointUsed (const SHAPE &Shape, int iPoint)
{
return Shape(iPoint, VX) != 0 || Shape(iPoint, VY) != 0;
}

//-----------------------------------------------------------------------------
int nGetNbrUsedPoints (const SHAPE &Shape, int nPoints)
{
int nUsed = 0;

for (int iPoint = 0; iPoint < nPoints; iPoint++)
	if (fPointUsed(Shape, iPoint))
		nUsed++;

return nUsed;
}

//-----------------------------------------------------------------------------
// This allocates *pfUnused, and the caller must free it

static void SaveUnusedPoints (bool **ppfUnused,						// out
								const SHAPE &Shape, int nrows)		// in
{
bool *pfUnused = (bool *)malloc(nrows * sizeof(bool *));
*ppfUnused = pfUnused;
for (int iRow = 0; iRow < nrows; iRow++)
	*pfUnused++ = !fPointUsed(Shape, iRow);
}

//-----------------------------------------------------------------------------
static void RestoreUnusedPoints (SHAPE &Shape, 							// out
									const bool afUnused[], int nrows)	// in
{
for (int iRow = 0; iRow < nrows; iRow++)
	if (afUnused[iRow])
		{
		Shape(iRow, VX) = 0;
		Shape(iRow, VY) = 0;
		}
}

//-----------------------------------------------------------------------------
// This returns the Euclidean distance between the same point in two different shapes

double PointDist (const SHAPE &Shape1, const SHAPE &Shape2, const int iPoint)
{
return sqrt(SQR(Shape1(iTranslatedPoint(iPoint), VX) - Shape2(iTranslatedPoint(iPoint), VX)) +
            SQR(Shape1(iTranslatedPoint(iPoint), VY) - Shape2(iTranslatedPoint(iPoint), VY)));
}

//-----------------------------------------------------------------------------
// This returns the Euclidean distance between two points in a shape

double PointDist (const SHAPE &Shape, const int iPoint1, const int iPoint2)
{
return sqrt(SQR(Shape(iTranslatedPoint(iPoint1), VX) - Shape(iTranslatedPoint(iPoint2), VX)) +
            SQR(Shape(iTranslatedPoint(iPoint1), VY) - Shape(iTranslatedPoint(iPoint2), VY)));
}

//-----------------------------------------------------------------------------
// This returns a vector of the distances between corresponding points in Shape1 and Shape2

SHAPE ShapeDist (const SHAPE &Shape1, const SHAPE &Shape2)
{
CheckSameDim(Shape1, Shape2, "ShapeDist");

Vec OutShape(Shape1.nrows());

for (int iRow = 0; iRow < Shape1.nrows(); iRow++)
	OutShape(iRow) = PointDist(Shape1, Shape2, iRow);

return OutShape;
}

//-----------------------------------------------------------------------------
// Multiply a Nx2 matrix (the InShape) by a 3x3 homogenous TransformMat
// This knows about unused landmarks, and leaves them untouched
// Returned result is the transformed InShape.

SHAPE TransformShape (const SHAPE &InShape, const Mat &TransformMat)
{
DASSERT(InShape.ncols() == 2);
DASSERT(TransformMat.ncols() == 3 && TransformMat.nrows() == 3);

static SHAPE OutShape; OutShape.assign(InShape);	// static for efficiency
int iRow = InShape.nrows();
while (iRow--)
	if (fPointUsed(InShape, iRow))
		{
		MatView Row(OutShape.row(iRow));
		Row = TransformMat.mat33TimesVec2(Row);
		}

return OutShape;
}

//-----------------------------------------------------------------------------
// Align Shape to to AnchorShape, overwrite Shape
// This knows about unused landmarks in AnchorShape and Shape, and leaves them untouched
// See also algorithm C.3 in Appendix C of CootesTaylor 2004 www.isbe.man.ac.uk/~bim/Models/app_models.pdf
//
// Returns the transformation matrix i.e. the pose, but also updates Shape
//
// This is a similarity transform so the transform matrix has the form
//		a -b  Tx
//		b  a  Ty
//		0  0  1
//
// Make pWeights equal to NULL if you don't want to weight the landmarks.  NULL is the default.

Mat AlignShape (SHAPE &Shape, 									// io
  				const SHAPE &AnchorShape, const Vec *pWeights)	// in
{
CheckSameNbrRows(Shape, AnchorShape, "AlignShape");
if (pWeights)
	ASSERT(Shape.nrows() == pWeights->nelems());

double X1 = 0, Y1 = 0, X2 = 0, Y2 = 0, W = 0, Z = 0, C1 = 0, C2 = 0;

int iRow = Shape.nrows();

while (iRow--)
	{
	double x1 = AnchorShape(iRow, VX);
	double y1 = AnchorShape(iRow, VY);
	double x2 = Shape(iRow, VX);
	double y2 = Shape(iRow, VY);

	if (x1 == 0 && y1 == 0)			// is anchor landmark unused?
		;							// get here if AnchorShape is BioId/AR but Shape is not, used when aligning AvShape to BioId/Ar
	else if (x2 == 0 && y2 == 0)	// is landmark unused?
		;							// get here if AnchorShape is not BioId/AR but Shape is BioId
	else
		{
		double w = (pWeights? (*pWeights)(iRow): 1.0);

		W  += w;
		Z  += w * (x2 * x2 + y2 * y2);
		X1 += w * x1;
		Y1 += w * y1;
		X2 += w * x2;
		Y2 += w * y2;
		C1 += w * (x1 * x2 + y1 * y2);
		C2 += w * (y1 * x2 - x1 * y2);
		}
	}
double SolnData[] = { X2, -Y2,   W,   0,
				      Y2,  X2,   0,   W,
				 	   Z,	0,	X2,  Y2,
				 	   0,	Z, -Y2,  X2 };

MatView Mat4(SolnData, 4, 4, 0);			// 4x4, tda=0

double  VecData[] = { X1, Y1, C1, C2 };
VecView Vec4(VecData, 4);

Vec	Soln(SolveWithLU(Mat4, Vec4));

double TransformData[] = { Soln(0), -Soln(1), Soln(2),
					 	   Soln(1),  Soln(0), Soln(3),
    				       0, 	     0,       1 };

Mat Transform(TransformData, 3, 3);

Shape = TransformShape(Shape, Transform);

return Transform;
}

//-----------------------------------------------------------------------------
// Align Shape to to AnchorShape, overwrite Shape.
// Based on algorithm C.4 in Appendix C of CootesTaylor 2004 www.isbe.man.ac.uk/~bim/Models/app_models.pdf
//
// Returns the transformation matrix i.e. the pose, but also updates Shape
//
// This is a similarity transform so the transform matrix has the form
//		a  c  Tx
//		b  d  Ty
//		0  0  1
//
// Weighted alignment not yet supported
// Unused point code is buggy

Mat AlignShape_Affine (SHAPE &Shape, 					// io
			  				const SHAPE &AnchorShape)	// in
{
CheckSameNbrRows(Shape, AnchorShape, "AlignShape_Affine");

double Sx = 0, Sy = 0, Sxx = 0, Syy = 0, Sxy = 0;
double SAx = 0, SAy = 0, SAxx = 0, SAyy = 0, SAxy = 0, SAyx = 0;

for (int i = 0; i < Shape.nrows(); i++)
	{
	double x1 = AnchorShape(i, VX);
	double y1 = AnchorShape(i, VY);
	double x2 = Shape(i, VX);
	double y2 = Shape(i, VY);

	if (x1 == 0 && y1 == 0)			// is anchor landmark unused?
		;							// get here if AnchorShape is BioId/AR but Shape is not, used when aligning AvShape to BioId/Ar
	else if (x2 == 0 && y2 == 0)	// is landmark unused?
		;							// get here if AnchorShape is not BioId/AR but Shape is BioId
	else
		{
		Sx += x2;
		Sy += y2;
		Sxx += x2 * x2;
		Syy += y2 * y2;
		Sxy += x2 * y2;

		SAx += x1;
		SAy += y1;
		SAxx += x2 * x1;
		SAyy += y2 * y1;
		SAxy += x2 * y1;
		SAyx += y2 * x1;
		}
	}
double AData[] = { Sxx, Sxy, Sx,
	   		 	   Sxy, Syy, Sy,
	   		 	   Sx,  Sy,  Shape.nrows() };

MatView A(AData, 3, 3, 0);	// 3x3, tda=0

// equation 1

double  VecData1[] = { SAxx, SAyx, SAx };
VecView Vec1(VecData1, 3);
Vec	Soln1(SolveWithLU(A, Vec1));		// a b tx

// equation 2

double  VecData2[] = { SAxy, SAyy, SAy };
VecView Vec2(VecData2, 3);

Vec	Soln2(SolveWithLU(A, Vec2));		// c y ty

// combine the solutions

double TransformData[] = { Soln1(0), Soln1(1), Soln1(2),		// a b tx
						   Soln2(0), Soln2(1), Soln2(2),		// c d ty
    				       0, 	     0,        1 };

Mat Transform(TransformData, 3, 3);

Shape = TransformShape(Shape, Transform);

return Transform;
}

//-----------------------------------------------------------------------------
// Like AlignShape but stretches the shape horizontally looking for a best fit.
// Also, this is much slower than AlignShape (it calls AlignShape internally)
//
// Returns the transformation matrix i.e. the pose, but also updates Shape
//
// This is a (constrained) affine transform so the transform matrix has the form
//		a  b  Tx
//		c  d  Ty
//		0  0  1
//
// TODO This routine could be improved I think, this version is just a first cut
// There is probably an analytical way of doing this (which would be much faster)

Mat AlignShape_Iteration (SHAPE &Shape, 									// io
		  					const SHAPE &AnchorShape, const Vec *pWeights)	// in
{
Check2Cols(Shape, "AlignShape_Iteration Shape");
Check2Cols(AnchorShape, "AlignShape_Iteration AnchorShape");
CheckSameNbrRows(Shape, AnchorShape, "AlignShape_Iteration");

SHAPE OrgShape(Shape);
double xBestStretch;

// Two pass algorithm: 1st pass does a coarse grained search.  We use two passes for speed.
// This is not identical to doing one big search (because there may be more than one
// minima?).  But tests show that the search results using the two pass algorithm
// are very close to a big one pass algorithm.

double xMin = 0.50;		// empirically determined to be a big enough range for AR, BioId, XM2VTS shapes
double xMax = 2.00;
int nIters = 10;
double xStretchDelta = (xMax - xMin) / (nIters - 1);

double MinDist = FLT_MAX;
for (int iter = 0; iter < nIters; iter++)
	{
	double xStretch = xMin + iter * xStretchDelta;
	Shape = OrgShape;
	Shape.col(VX) *= xStretch;
	Mat Transform = AlignShape(Shape, AnchorShape, pWeights);
	double Dist = ShapeDist(Shape, AnchorShape).sum();
	if (Dist < MinDist)
		{
		MinDist = Dist;
		xBestStretch = xStretch;
		}
	}
// 2nd pass does a fine grained search

xMin = xBestStretch - xStretchDelta;
xMax = xBestStretch + xStretchDelta;
nIters *= 2;
xStretchDelta = (xMax - xMin) / (nIters - 1);

SHAPE BestShape;
Mat BestTransform;

MinDist = FLT_MAX;
for (iter = 0; iter < nIters; iter++)
	{
	double xStretch = xMin + iter * xStretchDelta;
	Shape = OrgShape;
	Shape.col(VX) *= xStretch;
	Mat Transform = AlignShape(Shape, AnchorShape, pWeights);
	double Dist = ShapeDist(Shape, AnchorShape).sum();
	if (Dist < MinDist)
		{
		MinDist = Dist;
		BestShape = Shape;
		BestTransform = Transform;
		BestTransform(0, 0) *= xStretch;
		xBestStretch = xStretch;
		}
	}
if (xBestStretch == xMin || xBestStretch == xMax)
	Warn("At limit in AlignShape_Iteration %g", xBestStretch);

Shape = BestShape;

return BestTransform;
}

//-----------------------------------------------------------------------------
// Rotate Shape so it is horizontally/vertically aligned, then centralize it.
// Don't change its size.

void StraightenAndCentralizeShape (SHAPE &Shape) // io
{
// TODO This routine has a bug -- it gets confused if not all points are used.
// So make sure we don't hit this bug by issuing a sys err if need be.

if (nGetNbrUsedPoints(Shape, Shape.nrows() !=  Shape.nrows()))
	SysErr("StraightenAndCentralizeShape doesn't work if not all points used");

bool *afUnused;
int nrows = Shape.nrows();
SaveUnusedPoints(&afUnused, Shape, nrows);  		// remember which points are 0,0

// rotate shape

double Theta = 0;
double xAv = Shape.col(VX).sum();
double yAv = Shape.col(VY).sum();
Theta = M_PI/2 + atan2(yAv, xAv);

double TransformData[] = { cos(Theta), -sin(Theta), 0,
						   sin(Theta), cos(Theta), 	0,
						   0, 	       0,           1 };

Mat Transform(TransformData, 3, 3);
Shape = TransformShape(Shape, Transform);
RestoreUnusedPoints(Shape, afUnused, nrows);		// restore original 0,0 points back to 0,0 TODO shouldn't be needed?

// centralize shape

int nUsed = nGetNbrUsedPoints(Shape, nrows);
xAv = Shape.col(VX).sum() / nUsed;
yAv = Shape.col(VY).sum() / nUsed;
Shape.col(VX) -= xAv;
Shape.col(VY) -= yAv;
RestoreUnusedPoints(Shape, afUnused, nrows);		// restore original 0,0 points back to 0,0
free(afUnused);
}

//-----------------------------------------------------------------------------
// Centralize shape so its centroid is in the middle of the image

void CentralizeShape (SHAPE &Shape) // io
{
bool *afUnused;
int nrows = Shape.nrows();
SaveUnusedPoints(&afUnused, Shape, nrows); 		// remember which points are 0,0

int nUsed = nGetNbrUsedPoints(Shape, nrows);
double xAv = Shape.col(VX).sum() / nUsed;
double yAv = Shape.col(VY).sum() / nUsed;
Shape.col(VX) -= xAv;
Shape.col(VY) -= yAv;

RestoreUnusedPoints(Shape, afUnused, nrows);	// restore original 0,0 points back to 0,0
free(afUnused);
}

//-----------------------------------------------------------------------------
// Return horizontal extent of the shape i.e. furthest distance in
// x direction across the shape

double xShapeExtent (const SHAPE &Shape)
{
return fabs(Shape.col(VX).maxElem() - Shape.col(VX).minElem());
}

//-----------------------------------------------------------------------------
double yShapeExtent (const SHAPE &Shape)
{
return fabs(Shape.col(VY).maxElem() - Shape.col(VY).minElem());
}

//-----------------------------------------------------------------------------
double GetPyrScale (int iLev, double PyrRatio)
{
return pow(PyrRatio, iLev);
}

//-----------------------------------------------------------------------------
double GetX (double X, int iOffset, int iOrthOffset, double DeltaX, double DeltaY)
{
return X + (iOffset * DeltaX) + (iOrthOffset * DeltaY);
}

//-----------------------------------------------------------------------------
double GetY (double Y, int iOffset, int iOrthOffset, double DeltaX, double DeltaY)
{
return Y + (iOffset * DeltaY) - (iOrthOffset * DeltaX);
}