private int calculateMovement(CXYVector xdX)
{
int size = _ASMResult.size;
int detectedEdgeCnt = 0;
for (int i = 0; i < size; i++)
{
xdX[i] = CVector2.vNormal(_ASMResult[(i + size - 1) % size],
_ASMResult[i],
_ASMResult[(i + 1) % size]);
double dx = xdX[i][0];
double dy = xdX[i][1];
scanForEdge(_ASMResult[i][0], _ASMResult[i][1],
ref dx, ref dy);
if (dx != 0 && dy != 0)
{
detectedEdgeCnt++;
}
xdX[i][0] = dx;
xdX[i][1] = dy;
xdX[i].add(_ASMResult[i]);
}
return(detectedEdgeCnt);
}
public CXYVector generateNewVariation(int n, double [] b)
{
CXYVector ret = new CXYVector(_mean);
int i, j, m = _numPoints * 2;
double buff;
double[] pb = new double[m];
bool isEven = true;
if (n <= 0)
{
n = _degreeOfFreedom;
}
for (i = 0; i < m; i++)
{
buff = 0.0;
for (j = 0; j < n; j++)
{
buff += _eigen.vectors[i, j] * b[j];
}
if (isEven)
{
ret[i / 2][0] += buff;
}
else
{
ret[i / 2][1] += buff;
}
isEven = !isEven;
}
return(ret);
}
public bool initializeTraining(double modelSize)
{
if (_numSamples <= 0 || _numPoints <= 0)
{
return(false);
}
// Copy the label
_trainingData = new CXYVector[_numSamples];
int i, k, l, s;
for (k = 0; k < _numSamples; k++)
{
_trainingData[k] = new CXYVector(_numPoints, ((CSample)_sample[k]).points);
}
// create a normal shape
/* Create A Circle Shape
* Use This To Transform The "Center" Point Of The Mean Sample To Origin
* The Mean Sample Orientation Will Be Fix Using Manual Orientation
*/
double radius = modelSize;
_normalShape = new CXYVector(_numPoints);
for (i = 1; i < 2 * _normalShape.size; i += 2)
{
double angle = (i * System.Math.PI) / _normalShape.size;
_normalShape[i / 2] = new CVector2(radius * System.Math.Sin(angle), radius * System.Math.Cos(angle));
}
// Make First Sample As The Mean Sample
_mean = new CXYVector(_trainingData[0]);
// Create the weight diagonal matrix
// the matrix is reduced to vector
_weight = new double[_numPoints];
for (k = 0; k < _numPoints; k++)
{
double sumVar = 0.0;
for (l = 0; l < _numPoints; l++)
{
double sigX = 0.0;
double sigX2 = 0.0;
for (s = 0; s < _numSamples; s++)
{
CVector2 tv = _trainingData[s][k].substractCopy(_trainingData[s][l]);
double delta = tv.length2();
sigX += Math.Sqrt(delta);
sigX2 += delta;
}
sumVar += (sigX2 - ((sigX * sigX) / _numSamples)) / _numSamples;
}
_weight[k] = 1.0 / sumVar;
}
_energy = 100000;
return(true);
}
private void postrendering(ref Graphics g)
{
if (g != null && _detectionMethod != TDetectionMethod.NONE)
{
// get zoomRatio
_zoomRatioX = (float)_inputSource.cameraWindow.Width / _asmEngine.PDMData.clippingSize.Width;
_zoomRatioY = (float)_inputSource.cameraWindow.Height / _asmEngine.PDMData.clippingSize.Height;
// draw optimized shape
CXYVector optimizedShape = _asmEngine.ASMResult;
int i, j;
g.DrawRectangle(Pens.White, (float)_asmEngine.cx * _zoomRatioX - 1, (float)_asmEngine.cy * _zoomRatioY - 1, 3, 3);
for (i = 0; i < optimizedShape.size; i++)
{
/* CVector2 movement = CVector2.vNormal(_asmEngine.PDMData.mean[(i + optimizedShape.size - 1) % optimizedShape.size],
* _asmEngine.PDMData.mean[i],
* _asmEngine.PDMData.mean[(i + 1) % optimizedShape.size]);
* g.DrawLine(
* Pens.Yellow,
* (float)((_asmEngine.PDMData.mean[i][0]+_asmEngine.cx)*_zoomRatioX),
* (float)((_asmEngine.PDMData.mean[i][1]+_asmEngine.cy)*_zoomRatioY),
* (float)((_asmEngine.PDMData.mean[i][0]+_asmEngine.cx+movement[0]*10)*_zoomRatioX),
* (float)((_asmEngine.PDMData.mean[i][1]+_asmEngine.cy+movement[1]*10)*_zoomRatioY));*/
for (j = 0; j < i; j++)
{
if (_asmEngine.isConnected(i, j))
{
/* g.DrawLine(
* Pens.White,
* (float)((_asmEngine.lastResult[i][0])*_zoomRatioX),(float)((_asmEngine.lastResult[i][1])*_zoomRatioY),
* (float)((_asmEngine.lastResult[j][0])*_zoomRatioX),(float)((_asmEngine.lastResult[j][1])*_zoomRatioY));
* g.DrawLine(
* Pens.Red,
* (float)((_asmEngine.lastResult[i][0]-_asmEngine.movement[i][0])*_zoomRatioX),(float)((_asmEngine.lastResult[i][1]-_asmEngine.movement[i][1])*_zoomRatioY),
* (float)((_asmEngine.lastResult[i][0]+_asmEngine.movement[i][0])*_zoomRatioX),(float)((_asmEngine.lastResult[i][1]+_asmEngine.movement[i][1])*_zoomRatioY));
* g.DrawLine(
* Pens.Yellow,
* (float)((_asmEngine.detectedEdge[i][0])*_zoomRatioX),(float)((_asmEngine.detectedEdge[i][1])*_zoomRatioY),
* (float)((_asmEngine.detectedEdge[j][0])*_zoomRatioX),(float)((_asmEngine.detectedEdge[j][1])*_zoomRatioY));
* g.DrawLine(
* Pens.Green,
* (float)((_asmEngine.newModelInImageParameter[i][0])*_zoomRatioX),(float)((_asmEngine.newModelInImageParameter[i][1])*_zoomRatioY),
* (float)((_asmEngine.newModelInImageParameter[j][0])*_zoomRatioX),(float)((_asmEngine.newModelInImageParameter[j][1])*_zoomRatioY));
* g.DrawLine(
* Pens.Pink,
* (float)((_asmEngine.alignedModel[i][0])*_zoomRatioX),(float)((_asmEngine.alignedModel[i][1])*_zoomRatioY),
* (float)((_asmEngine.alignedModel[j][0])*_zoomRatioX),(float)((_asmEngine.alignedModel[j][1])*_zoomRatioY));
*/ g.DrawLine(
Pens.Blue,
(float)((optimizedShape[i][0]) * _zoomRatioX), (float)((optimizedShape[i][1]) * _zoomRatioY),
(float)((optimizedShape[j][0]) * _zoomRatioX), (float)((optimizedShape[j][1]) * _zoomRatioY));
}
}
}
}
}
public CPointDistributionModel(Size clippingSize, CXYVector mean, CEigen eigen, int freedom, bool [,] connectivity)
{
_clippingSize = new Size(clippingSize.Width, clippingSize.Height);
_numPoints = mean.size;
_mean = mean;
_eigen = eigen;
_sqrtEigenValues = new double[_numPoints * 2];
for (int i = 0; i < _numPoints * 2; i++)
{
_sqrtEigenValues[i] = Math.Sign(eigen.values[i]) * Math.Sqrt(Math.Abs(eigen.values[i]));
}
_degreeOfFreedom = freedom;
_connectivity = (bool[, ])connectivity.Clone();
}
public long realignAll()
{
int i;
long start = CSettings.currentTimeMillis();
CXYVector.realign(_trainingData[0], _mean, _weight);
_newMean = new CXYVector(_trainingData[0]);
for (i = 1; i < _numSamples; i++)
{
CXYVector.realign(_trainingData[i], _mean, _weight);
_newMean.add(_trainingData[i]);
}
_newMean.divide(_numSamples);
return(CSettings.currentTimeMillis() - start);
}
public void doSearch(Bitmap image)
{
// resize image to sample resolution
resetSearch(image);
// optimize
bool res = false;
CXYVector _prevASMResult = new CXYVector(_ASMResult);
while (_stepCounter < _maxStep && !isConverge())
{
res = doStep() || res;
}
if (!res)
{
_ASMResult = new CXYVector(_prevASMResult);
_energy = 0;
}
}
public void normalizeMean()
{
CXYVector.realign(_newMean, _normalShape, _weight);
_energy = _mean.distance(_newMean);
_mean = new CXYVector(_newMean);
}
public CSample(string newImageFilename)
{
_pictureFilename = newImageFilename;
_numPoints = 0;
_points = new CXYVector(CSettings.MAXPOINT);
}
public bool doStep()
{
if (_stepCounter >= maxStep)
{
return(false);
}
_stepCounter++;
if (isConverge())
{
return(false);
}
int i, j;
// Calculate Movement
int detectedEdgeCnt = calculateMovement(_XdX);
// Compute Change
CXYVector currentShape = new CXYVector(_PDMData.mean);
CMatrix3x3 pose = CXYVector.realign(currentShape, _XdX, _weight);
// Get Residual Adjustment
CXYVector XdX = new CXYVector(_XdX);
XdX.transform(pose.inverse());
double [] dx = XdX.substractDouble(_PDMData.mean);
// Translate Into Model Parameter
double [] db = new double[_PDMData.freedom];
double [,] vectorsPtr = _PDMData.eigen.vectors;
double dimension = _size * 2;
for (i = 0; i < _PDMData.freedom; i++)
{
db[i] = 0;
for (j = 0; j < dimension; j++)
{
db[i] += vectorsPtr[j, i] * dx[j];
}
}
// Find Out If Parameter Is Out Of Limit
double dm = 0.0;
for (i = 0; i < _PDMData.freedom; i++)
{
dm += (db[i] * db[i]) / _PDMData.eigen.values[i];
}
// If It Does, Apply Limit
dm = Math.Sqrt(dm);
if (dm > 3.0)
{
for (i = 0; i < _PDMData.freedom; i++)
{
db[i] *= 3.0 / dm;
}
}
// Update Parameter
currentShape = PDMData.generateNewVariation(-1, db);
CXYVector.realign(currentShape, _XdX, _weight);
//currentShape.transform(pose);
_energy = _ASMResult.distance(currentShape);
_ASMResult = currentShape;
return(detectedEdgeCnt > (_ASMResult.size / 10));
}
public void resetModel()
{
_ASMResult = new CXYVector(_PDMData.mean);
_ASMResult.add(new CVector2(_cx, _cy));
}
public static CPointDistributionModel open(string path)
{
if (!File.Exists(path))
{
return(null);
}
try
{
StreamReader tsFile = File.OpenText(path);
bool isStartMean = false, isStartEVal = false, isStartEVec = false, isStartConnect = false;
int ctr = 0, j;
Size buffClippingSize = new Size();
int buffNumPoints = 0, buffFreedom = 0;
CXYVector buffMean = null;
double [,] buffVec = null;
double [] buffVal = null;
bool [,] buffConectivity = null;
while (!tsFile.EndOfStream)
{
string curLine = tsFile.ReadLine().Trim();
if (curLine == "" || curLine[0] == '#')
{
continue;
}
string[] word = curLine.Split(' ');
if (isStartMean)
{
if (word[0] == "_EndMean")
{
isStartMean = false;
ctr = 0;
continue;
}
buffMean[ctr][0] = double.Parse(word[0]);
buffMean[ctr][1] = double.Parse(word[1]);
ctr++;
}
else if (isStartEVec)
{
if (word[0] == "_EndEigenVectors")
{
isStartEVec = false;
ctr = 0;
continue;
}
for (j = 0; j < word.Length; j++)
{
buffVec[ctr, j] = double.Parse(word[j]);
}
ctr++;
}
else if (isStartEVal)
{
if (word[0] == "_EndEigenValues")
{
isStartEVal = false;
ctr = 0;
continue;
}
buffVal[ctr] = double.Parse(word[0]);
ctr++;
}
else if (isStartConnect)
{
if (word[0] == "_EndConnectivity")
{
isStartConnect = false;
ctr = 0;
continue;
}
for (j = 0; j < word.Length; j++)
{
if (word[j] == "1")
{
buffConectivity[ctr, j] = buffConectivity[j, ctr] = true;
}
}
ctr++;
}
else
{
switch (word[0])
{
case "clippingSize":
buffClippingSize = new Size(int.Parse(word[1]), int.Parse(word[2]));
break;
case "numpoint":
buffNumPoints = int.Parse(word[1]);
buffMean = new CXYVector(buffNumPoints);
buffVec = new double[buffNumPoints * 2 + 1, buffNumPoints *2 + 1];
buffVal = new double[buffNumPoints * 2 + 1];
buffConectivity = new bool[buffNumPoints * 2, buffNumPoints *2];
break;
case "degOfFreedom":
buffFreedom = int.Parse(word[1]);
break;
case "_StartMean":
isStartMean = true;
ctr = 0;
break;
case "_StartEigenVectors":
isStartEVec = true;
ctr = 0;
break;
case "_StartEigenValues":
isStartEVal = true;
ctr = 0;
break;
case "_StartConnectivity":
isStartConnect = true;
ctr = 1;
break;
}
}
}
tsFile.Close();
tsFile.Dispose();
tsFile = null;
return(new CPointDistributionModel(
buffClippingSize,
buffMean,
new CEigen(
buffNumPoints * 2,
buffVec,
buffVal),
buffFreedom,
buffConectivity));
}catch (Exception) {
return(null);
}
}
