用opencv和C++程序实现广义普鲁克分析（Generalized Procrustes analysis）

[keyuding_1993]

广义普鲁克分析法的算法流程如下图所示：
现在已经有了一些不是很完整的代码，代码头文件如下：[code=c++]struct RotateAndScaleCoefs
{
RotateAndScaleCoefs()
{
s = 1;
theta = 0;
}

RotateAndScaleCoefs(double _s, double _theta) : s(_s), theta(_theta) {}

double s;
double theta;
};

struct TranslationCoefs
{
TranslationCoefs()
{
xt = 0.0;
yt = 0.0;
}

TranslationCoefs(double _xt, double _yt) : xt(_xt), yt(_yt) {}

double xt;
double yt;
};

struct TransformationCoefs
{
TransformationCoefs()
{
a = 1;
b = 0;
}

TransformationCoefs(double _a, double _b) : a(_a), b(_b) {}

double a;
double b;
};

class Procrustes2D
{
public:
static void procrustesAnalysis(QVector<Vector> &vectors, bool scale = true,
double eps = 0, int maxIterations = INT_MAX);

static double getOptimalRotation(Vector &from, Vector &to);

static RotateAndScaleCoefs align(Vector &from, Vector &to);

static void rotateAndScale(Vector &vector, RotateAndScaleCoefs &coefs);

static void transformate(Vector &vector, TransformationCoefs &coefs);

static void translate(Vector &vector, TranslationCoefs &coefs);

static TranslationCoefs centralizedTranslation(Vector &vector);

static void centralize(Vector &vector);

static void centralize(QVector<Vector> &vectors);

static Vector getMeanShape(QVector<Vector> &vectors);
};
[/code]

主代码如下：[code=c++]#include "procrustes.h"

void Procrustes2D::procrustesAnalysis(QVector<Vector> &vectors, bool scale, double eps, int maxIterations)
{
int count = vectors.count();
assert(count > 0);

int len = vectors[0].rows;
assert(len > 0);
assert(len % 2 == 0);

// Translate each vector to the center of origin
centralize(vectors);

// Choose one example as an initial estimate of the mean shape and scale so that |x| = 1.
Vector mean = scale ? vectors[0].normalized() : vectors[0];
Vector oldMean(mean);
double oldDelta = 1e300;

int iteration = 1;
bool end = false;
while (!end)
{
// Align all the shapes with the current estimate of the mean shape.
for (int i = 0; i < count; i++)
{
if (scale)
{
RotateAndScaleCoefs c = align(vectors[i], mean);
rotateAndScale(vectors[i], c);
}
else
{
double theta = getOptimalRotation(vectors[i], mean);
RotateAndScaleCoefs c; c.theta = theta;
rotateAndScale(vectors[i], c);
}
}

// Re-estimate mean from aligned shapes
mean = getMeanShape(vectors);

// Apply constraints on the current estimate of the mean
if (scale)
mean.normalize();

// If not converged, iterate
Vector diff = mean-oldMean;
double delta = diff.sqrMagnitude();

if (delta <= eps || iteration > maxIterations)
end = true;

oldMean = Vector(mean);
oldDelta = delta;

if (iteration % 100 == 0)
qDebug() << " iteration:" << iteration << "; delta:" << delta;

iteration += 1;
}
}

void Procrustes2D::translate(Vector &vector, TranslationCoefs &coefs)
{
int n = vector.rows/2;
for (int i = 0; i < n; i++)
{
vector(i, 0) += coefs.xt;
vector(i+n, 0) += coefs.yt;
}
}

TranslationCoefs Procrustes2D::centralizedTranslation(Vector &vector)
{
double meanx = 0.0;
double meany = 0.0;
int n = vector.rows/2;

for (int i = 0; i < n; i++)
{
meanx += vector(i, 0);
meany += vector(i+n, 0);
}
meanx /= n;
meany /= n;

TranslationCoefs c;
c.xt = -meanx;
c.yt = -meany;
return c;
}

void Procrustes2D::centralize(Vector &vector)
{
TranslationCoefs c = centralizedTranslation(vector);
translate(vector, c);
}

void Procrustes2D::centralize(QVector<Vector> &vectors)
{
int n = vectors.count();
for (int i = 0; i < n; i++)
centralize(vectors[i]);
}

void Procrustes2D::rotateAndScale(Vector &vector, RotateAndScaleCoefs &coefs)
{
double sint = coefs.s * sin(coefs.theta);
double cost = coefs.s * cos(coefs.theta);

int n = vector.rows/2;
for (int i = 0; i < n; i++)
{
double oldx = vector(i, 0);
double oldy = vector(i+n, 0);
double x = cost*oldx - sint*oldy;
double y = sint*oldx + cost*oldy;

vector(i, 0) = x;
vector(i+n, 0) = y;
}
}

void Procrustes2D::transformate(Vector &vector, TransformationCoefs &coefs)
{
int n = vector.rows/2;
for (int i = 0; i < n; i++)
{
double oldx = vector(i, 0);
double oldy = vector(i+n, 0);
double x = coefs.a * oldx + -coefs.b * oldy;
double y = coefs.b * oldx + coefs.a * oldy;

vector(i, 0) = x;
vector(i+n, 0) = y;
}
}

double Procrustes2D::getOptimalRotation(Vector &from, Vector &to)
{
int n = from.rows/2;
double numerator = 0.0;
double denumerator = 0.0;
for (int i = 0; i < n; i++)
{
numerator += (from(i) * to(n+i) - from(n+i) * to(i));
denumerator += (from(i) * to(i) + from(n+i) * to(n+i));
}

return atan(numerator/denumerator);
}

RotateAndScaleCoefs Procrustes2D::align(Vector &from, Vector &to)
{
Vector reference(to);
double referenceScale = 1.0/reference.magnitude();
reference.mul(referenceScale);

Vector vector(from);
double vectorScale = 1.0/vector.magnitude();
vector.mul(vectorScale);

double s = vectorScale/referenceScale;
double theta = getOptimalRotation(vector, reference);
RotateAndScaleCoefs c(s, theta);

return c;
}

Vector Procrustes2D::getMeanShape(QVector<Vector> &vectors)
{
int n = vectors.count();
Vector mean(vectors[0].rows);
for (int i = 0; i < n; i++)
{
mean += vectors[0];
}
mean = mean / ((double)n);
return mean;
}
[/code]
哪位大神能帮忙调试一下程序？

[赵4老师]

代码功能归根结底不是别人帮自己看或讲解或注释出来的；而是被自己静下心来花足够长的时间和精力亲自动手单步或设断点或对执行到某步获得的中间结果显示或写到日志文件中一步一步分析出来的。 提醒：再牛×的老师也无法代替学生自己领悟和上厕所！ 单步调试和设断点调试（VS IDE中编译连接通过以后，按F10或F11键单步执行，按Shift+F11退出当前函数；在某行按F9设断点后按F5执行停在该断点处。）是程序员必须掌握的技能之一。