/* Cross three 4x1 vectors */
void matrix_cross4(const double *u, const double *v, const double *w, double *x)
{
double sub1[9] =
{ u[1], u[2], u[3],
v[1], v[2], v[3],
w[1], w[2], w[3] };
double sub2[9] =
{ u[0], u[2], u[3],
v[0], v[2], v[3],
w[0], w[2], w[3] };
double sub3[9] =
{ u[0], u[1], u[3],
v[0], v[1], v[3],
w[0], w[1], w[3] };
double sub4[9] =
{ u[0], u[1], u[2],
v[0], v[1], v[2],
w[0], w[1], w[2] };
double det1 = matrix_determinant3(sub1);
double det2 = matrix_determinant3(sub2);
double det3 = matrix_determinant3(sub3);
double det4 = matrix_determinant3(sub4);
x[0] = det1;
x[1] = det2;
x[2] = det3;
x[3] = det4;
}
/* Align two sets of points with a 3D rotation */
double align_3D_rotation(int n, v3_t *r_pts, v3_t *l_pts, double *R)
{
double A[9];
double U[9], S[3], V[9], VT[9], RT[9];
int i;
double error;
#if 0
if (n > 3) {
printf("A:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(r_pts[i]), Vy(r_pts[i]), Vz(r_pts[i]));
}
printf("B:\n");
for (i = 0; i < n; i++) {
printf("%0.6f %0.6f %0.6f\n",
Vx(l_pts[i]), Vy(l_pts[i]), Vz(l_pts[i]));
}
}
#endif
for (i = 0; i < 9; i++)
A[i] = 0.0;
for (i = 0; i < n; i++) {
double *a = l_pts[i].p, *b = r_pts[i].p;
// matrix_product(3, 1, 1, 3, l_pts[i].p, r_pts[i].p, tensor);
A[0] += a[0] * b[0];
A[1] += a[0] * b[1];
A[2] += a[0] * b[2];
A[3] += a[1] * b[0];
A[4] += a[1] * b[1];
A[5] += a[1] * b[2];
A[6] += a[2] * b[0];
A[7] += a[2] * b[1];
A[8] += a[2] * b[2];
}
// dgesvd_driver(3, 3, A, U, S, VT);
// printf("svd:\n");
// matrix_print(3, 3, A);
svd(3, 3, 1, 1, 1.0e-12, 1.0e-12, A, S, U, V, VT);
// printf("U:\n");
// matrix_print(3, 3, U);
// printf("VT:\n");
// matrix_print(3, 3, VT);
// printf("S:\n");
// matrix_print(3, 3, S);
matrix_product33(U, VT, RT);
matrix_transpose(3, 3, RT, R);
// printf("R:\n");
// matrix_print(3, 3, R);
if (matrix_determinant3(R) < 0.0) {
/* We're dealing with a reflection */
double tmp[9];
double reflectZ[9] = { 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, -1.0 };
matrix_product33(U, reflectZ, tmp);
matrix_product33(tmp, VT, RT);
matrix_transpose(3, 3, RT, R);
}
/* Compute error */
error = 0.0;
for (i = 0; i < n; i++) {
double rot[3];
double diff[3];
double dist;
matrix_product331(R, l_pts[i].p, rot);
matrix_diff(3, 1, 3, 1, rot, r_pts[i].p, diff);
dist = matrix_norm(3, 1, diff);
// printf("d[%d] = %0.6f\n", i, dist);
error += dist;
}
return error / n;
}
/* Computes the closed-form least-squares solution to a rigid
* body alignment.
*
* n: the number of points
* right_pts: Target set of n points
* left_pts: Source set of n points */
double align_horn_3D(int n, v3_t *right_pts, v3_t *left_pts, int scale_xform,
double *Tout) {
int i;
v3_t right_centroid = v3_new(0.0, 0.0, 0.0);
v3_t left_centroid = v3_new(0.0, 0.0, 0.0);
double M[3][3] = { { 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, },
{ 0.0, 0.0, 0.0, } };
double MT[3][3];
double MTM[3][3];
double eval[3], sqrteval_inv[3];
double evec[3][3], evec_tmp[3][3];
double Sinv[3][3], U[3][3];
double Tcenter[4][4] = { { 1.0, 0.0, 0.0, 0.0 },
{ 0.0, 1.0, 0.0, 0.0 },
{ 0.0, 0.0, 1.0, 0.0 },
{ 0.0, 0.0, 0.0, 1.0 } };
double Ttmp[4][4];
double T[16], R[16];
double sum_num, sum_den, scale, RMS_sum;
int perm[3];
/* Compute the centroid of both point sets */
right_centroid = v3_mean(n, right_pts);
left_centroid = v3_mean(n, left_pts);
/* Compute the scale */
sum_num = sum_den = 0.0;
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
sum_num += v3_magsq(r);
sum_den += v3_magsq(l);
}
scale = sqrt(sum_num / sum_den);
/* Fill in the matrix M */
for (i = 0; i < n; i++) {
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
M[0][0] += Vx(r) * Vx(l);
M[0][1] += Vx(r) * Vy(l);
M[0][2] += Vx(r) * Vz(l);
M[1][0] += Vy(r) * Vx(l);
M[1][1] += Vy(r) * Vy(l);
M[1][2] += Vy(r) * Vz(l);
M[2][0] += Vz(r) * Vx(l);
M[2][1] += Vz(r) * Vy(l);
M[2][2] += Vz(r) * Vz(l);
}
/* Compute MTM */
matrix_transpose(3, 3, (double *)M, (double *)MT);
matrix_product(3, 3, 3, 3, (double *)MT, (double *)M, (double *)MTM);
/* Calculate Sinv, the inverse of the square root of MTM */
dgeev_driver(3, (double *)MTM, (double *)evec, eval);
/* Sort the eigenvalues */
qsort_descending();
qsort_perm(3, eval, perm);
memcpy(evec_tmp[0], evec[perm[0]], sizeof(double) * 3);
memcpy(evec_tmp[1], evec[perm[1]], sizeof(double) * 3);
memcpy(evec_tmp[2], evec[perm[2]], sizeof(double) * 3);
memcpy(evec, evec_tmp, sizeof(double) * 9);
sqrteval_inv[0] = 1.0 / sqrt(eval[0]);
sqrteval_inv[1] = 1.0 / sqrt(eval[1]);
if (eval[2] < 1.0e-8 * eval[0]) {
sqrteval_inv[2] = 0.0;
} else {
sqrteval_inv[2] = 1.0 / sqrt(eval[2]);
}
Sinv[0][0] =
sqrteval_inv[0] * evec[0][0] * evec[0][0] +
sqrteval_inv[1] * evec[1][0] * evec[1][0] +
sqrteval_inv[2] * evec[2][0] * evec[2][0];
Sinv[0][1] =
sqrteval_inv[0] * evec[0][0] * evec[0][1] +
sqrteval_inv[1] * evec[1][0] * evec[1][1] +
sqrteval_inv[2] * evec[2][0] * evec[2][1];
Sinv[0][2] =
sqrteval_inv[0] * evec[0][0] * evec[0][2] +
sqrteval_inv[1] * evec[1][0] * evec[1][2] +
sqrteval_inv[2] * evec[2][0] * evec[2][2];
Sinv[1][0] =
sqrteval_inv[0] * evec[0][1] * evec[0][0] +
sqrteval_inv[1] * evec[1][1] * evec[1][0] +
sqrteval_inv[2] * evec[2][1] * evec[2][0];
Sinv[1][1] =
sqrteval_inv[0] * evec[0][1] * evec[0][1] +
sqrteval_inv[1] * evec[1][1] * evec[1][1] +
sqrteval_inv[2] * evec[2][1] * evec[2][1];
Sinv[1][2] =
sqrteval_inv[0] * evec[0][1] * evec[0][2] +
sqrteval_inv[1] * evec[1][1] * evec[1][2] +
sqrteval_inv[2] * evec[2][1] * evec[2][2];
Sinv[2][0] =
sqrteval_inv[0] * evec[0][2] * evec[0][0] +
sqrteval_inv[1] * evec[1][2] * evec[1][0] +
sqrteval_inv[2] * evec[2][2] * evec[2][0];
Sinv[2][1] =
sqrteval_inv[0] * evec[0][2] * evec[0][1] +
sqrteval_inv[1] * evec[1][2] * evec[1][1] +
sqrteval_inv[2] * evec[2][2] * evec[2][1];
Sinv[2][2] =
sqrteval_inv[0] * evec[0][2] * evec[0][2] +
sqrteval_inv[1] * evec[1][2] * evec[1][2] +
sqrteval_inv[2] * evec[2][2] * evec[2][2];
/* U = M * Sinv */
matrix_product(3, 3, 3, 3, (double *)M, (double *)Sinv, (double *)U);
if (eval[2] < 1.0e-8 * eval[0]) {
double u3u3[9], Utmp[9];
matrix_transpose_product2(3, 1, 3, 1, evec[2], evec[2], u3u3);
matrix_sum(3, 3, 3, 3, (double *) U, u3u3, Utmp);
if (matrix_determinant3(Utmp) < 0.0) {
printf("[align_horn_3D] Recomputing matrix...\n");
matrix_diff(3, 3, 3, 3, (double *) U, u3u3, Utmp);
}
memcpy(U, Utmp, 9 * sizeof(double));
}
/* Fill in the rotation matrix */
R[0] = U[0][0]; R[1] = U[0][1]; R[2] = U[0][2]; R[3] = 0.0;
R[4] = U[1][0]; R[5] = U[1][1]; R[6] = U[1][2]; R[7] = 0.0;
R[8] = U[2][0]; R[9] = U[2][1]; R[10] = U[2][2]; R[11] = 0.0;
R[12] = 0.0; R[13] = 0.0; R[14] = 0.0; R[15] = 1.0;
/* Fill in the translation matrix */
matrix_ident(4, T);
T[3] = Vx(right_centroid);
T[7] = Vy(right_centroid);
T[11] = Vz(right_centroid);
if (scale_xform == 0)
scale = 1.0;
Tcenter[0][0] = scale;
Tcenter[1][1] = scale;
Tcenter[2][2] = scale;
Tcenter[0][3] = -scale * Vx(left_centroid);
Tcenter[1][3] = -scale * Vy(left_centroid);
Tcenter[2][3] = -scale * Vz(left_centroid);
matrix_product(4, 4, 4, 4, T, R, (double *) Ttmp);
matrix_product(4, 4, 4, 4, (double *)Ttmp, (double *)Tcenter, Tout);
#if 0
T[2] = Vx(v3_sub(right_centroid, left_centroid));
T[5] = Vy(v3_sub(right_centroid, left_centroid));
T[8] = Vz(v3_sub(right_centroid, left_centroid));
#endif
/* Now compute the RMS error between the points */
RMS_sum = 0.0;
for (i = 0; i < n; i++) {
double left[4] = { Vx(left_pts[i]),
Vy(left_pts[i]),
Vz(left_pts[i]), 1.0 };
double left_prime[3];
double dx, dy, dz;
matrix_product(4, 4, 4, 1, Tout, left, left_prime);
dx = left_prime[0] - Vx(right_pts[i]);
dy = left_prime[1] - Vy(right_pts[i]);
dz = left_prime[2] - Vz(right_pts[i]);
RMS_sum += dx * dx + dy * dy + dz * dz;
#if 0
v3_t r = v3_sub(right_centroid, right_pts[i]);
v3_t l = v3_sub(left_centroid, left_pts[i]);
v3_t resid;
/* Rotate, scale l */
v3_t Rl, SRl;
Vx(Rl) = R[0] * Vx(l) + R[1] * Vy(l) + R[2] * Vz(l);
Vy(Rl) = R[3] * Vx(l) + R[4] * Vy(l) + R[5] * Vz(l);
Vz(Rl) = R[6] * Vx(l) + R[7] * Vy(l) + R[8] * Vz(l);
SRl = v3_scale(scale, Rl);
resid = v3_sub(r, SRl);
RMS_sum += v3_magsq(resid);
#endif
}
return sqrt(RMS_sum / n);
}
