/* 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); }