// $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 itstatic 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 shapesdouble 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 shapedouble 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 Shape2SHAPE 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 efficiencyint 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=0double  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 buggyMat 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 1double  VecData1[] = { SAxx, SAyx, SAx };VecView Vec1(VecData1, 3);Vec	Soln1(SolveWithLU(A, Vec1));		// a b tx// equation 2double  VecData2[] = { SAxy, SAyy, SAy };VecView Vec2(VecData2, 3);Vec	Soln2(SolveWithLU(A, Vec2));		// c y ty// combine the solutionsdouble 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 shapesdouble 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 searchxMin = 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 shapedouble 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 shapeint 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,0free(afUnused);}//-----------------------------------------------------------------------------// Centralize shape so its centroid is in the middle of the imagevoid CentralizeShape (SHAPE &Shape) // io{bool *afUnused;int nrows = Shape.nrows();SaveUnusedPoints(&afUnused, Shape, nrows); 		// remember which points are 0,0int 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,0free(afUnused);}//-----------------------------------------------------------------------------// Return horizontal extent of the shape i.e. furthest distance in// x direction across the shapedouble 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);}