// compute optimal pose and scale using a procedure described by Umeyame,
// Least-squares estimation of transformation parameters between two point patterns (IEEE Pami 1991)
double computeScalingFactor(MeshType* mesh1, MeshType* mesh2)
{
if (mesh1->GetNumberOfPoints() != mesh2->GetNumberOfPoints()) {
throw std::runtime_error("meshes should be in correspondence when computing specificity");
}
PointListType points1;
PointListType points2;
for (unsigned i = 0; i < mesh1->GetNumberOfPoints(); i++) {
MeshType::PointType pt1;
mesh1->GetPoint(i, &pt1);
points1.push_back(pt1);
MeshType::PointType pt2;
mesh2->GetPoint(i, &pt2);
points2.push_back(pt2);
}
vnlMatrixType X = createMatrixFromPoints(points1);
vnlMatrixType Y = createMatrixFromPoints(points2);
vnlVectorType mu_x = calcMean(X);
vnlVectorType mu_y = calcMean(Y);
ElementType sigma2_x = calcVar(X, mu_x);
ElementType sigma2_y = calcVar(Y, mu_y);
vnlMatrixType Sigma_xy = calcCov(X, Y, mu_x, mu_y);
vnl_svd<ElementType> SVD(Sigma_xy);
unsigned m = X.rows();
vnlMatrixType S(m,m);
S.set_identity();
ElementType detU = vnl_qr<ElementType>(SVD.U()).determinant();
ElementType detV = vnl_qr<ElementType>(SVD.V()).determinant();
if ( detU * detV == -1) {
S[m-1][m-1] = -1;
}
// the procedure actually computes the optimal rotation, translation and scale. We only
// use the scaling factor
vnlMatrixType R = SVD.U() * S * SVD.V().transpose();
ElementType c = 1/sigma2_x * vnl_trace(SVD.W()*S);
vnlVectorType t = mu_y - c * R * mu_x;
ElementType epsilon2 = sigma2_y - pow(vnl_trace(SVD.W()*S), 2) / sigma2_x;
return c;
}
//Find out which regions has the least variance
int KuwaharaRegions::minVar() {
calcVar();
int i = 0;
double var = varianzas[0];
if (var > varianzas[1]) {var = varianzas[1]; i = 1;}
if (var > varianzas[2]) {var = varianzas[2]; i = 2;}
if (var > varianzas[3]) {var = varianzas[3]; i = 3;}
return i;
}
