static int find_translation(int np, double *pts1, double *pts2, double *mat) { int i; double sx, sy, dx, dy; double sumx, sumy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); sumx = 0; sumy = 0; for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); sumx += dx - sx; sumy += dy - sy; } mat[0] = sumx / np; mat[1] = sumy / np; denormalize_translation_reorder(mat, T1, T2); return 0; }
static int find_affine(int np, double *pts1, double *pts2, double *mat) { const int np2 = np * 2; double *a = (double *)aom_malloc(sizeof(*a) * np2 * 13); double *b = a + np2 * 6; double *temp = b + np2; int i; double sx, sy, dx, dy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); a[i * 2 * 6 + 0] = sx; a[i * 2 * 6 + 1] = sy; a[i * 2 * 6 + 2] = 0; a[i * 2 * 6 + 3] = 0; a[i * 2 * 6 + 4] = 1; a[i * 2 * 6 + 5] = 0; a[(i * 2 + 1) * 6 + 0] = 0; a[(i * 2 + 1) * 6 + 1] = 0; a[(i * 2 + 1) * 6 + 2] = sx; a[(i * 2 + 1) * 6 + 3] = sy; a[(i * 2 + 1) * 6 + 4] = 0; a[(i * 2 + 1) * 6 + 5] = 1; b[2 * i] = dx; b[2 * i + 1] = dy; } if (pseudo_inverse(temp, a, np2, 6)) { aom_free(a); return 1; } multiply_mat(temp, b, mat, 6, np2, 1); denormalize_affine_reorder(mat, T1, T2); aom_free(a); return 0; }
static int find_affine(int np, double *pts1, double *pts2, double *mat) { const int np2 = np * 2; double *a = (double *)aom_malloc(sizeof(*a) * (np2 * 7 + 42)); double *b = a + np2 * 6; double *temp = b + np2; int i; double sx, sy, dx, dy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); a[i * 2 * 6 + 0] = sx; a[i * 2 * 6 + 1] = sy; a[i * 2 * 6 + 2] = 0; a[i * 2 * 6 + 3] = 0; a[i * 2 * 6 + 4] = 1; a[i * 2 * 6 + 5] = 0; a[(i * 2 + 1) * 6 + 0] = 0; a[(i * 2 + 1) * 6 + 1] = 0; a[(i * 2 + 1) * 6 + 2] = sx; a[(i * 2 + 1) * 6 + 3] = sy; a[(i * 2 + 1) * 6 + 4] = 0; a[(i * 2 + 1) * 6 + 5] = 1; b[2 * i] = dx; b[2 * i + 1] = dy; } if (!least_squares(6, a, np2, 6, b, temp, mat)) { aom_free(a); return 1; } denormalize_affine_reorder(mat, T1, T2); aom_free(a); return 0; }
static int find_homography(int np, double *pts1, double *pts2, double *mat) { // Implemented from Peter Kovesi's normalized implementation const int np3 = np * 3; double *a = (double *)aom_malloc(sizeof(*a) * np3 * 18); double *U = a + np3 * 9; double S[9], V[9 * 9], H[9]; int i, mini; double sx, sy, dx, dy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); a[i * 3 * 9 + 0] = a[i * 3 * 9 + 1] = a[i * 3 * 9 + 2] = 0; a[i * 3 * 9 + 3] = -sx; a[i * 3 * 9 + 4] = -sy; a[i * 3 * 9 + 5] = -1; a[i * 3 * 9 + 6] = dy * sx; a[i * 3 * 9 + 7] = dy * sy; a[i * 3 * 9 + 8] = dy; a[(i * 3 + 1) * 9 + 0] = sx; a[(i * 3 + 1) * 9 + 1] = sy; a[(i * 3 + 1) * 9 + 2] = 1; a[(i * 3 + 1) * 9 + 3] = a[(i * 3 + 1) * 9 + 4] = a[(i * 3 + 1) * 9 + 5] = 0; a[(i * 3 + 1) * 9 + 6] = -dx * sx; a[(i * 3 + 1) * 9 + 7] = -dx * sy; a[(i * 3 + 1) * 9 + 8] = -dx; a[(i * 3 + 2) * 9 + 0] = -dy * sx; a[(i * 3 + 2) * 9 + 1] = -dy * sy; a[(i * 3 + 2) * 9 + 2] = -dy; a[(i * 3 + 2) * 9 + 3] = dx * sx; a[(i * 3 + 2) * 9 + 4] = dx * sy; a[(i * 3 + 2) * 9 + 5] = dx; a[(i * 3 + 2) * 9 + 6] = a[(i * 3 + 2) * 9 + 7] = a[(i * 3 + 2) * 9 + 8] = 0; } if (SVD(U, S, V, a, np3, 9)) { aom_free(a); return 1; } else { double minS = 1e12; mini = -1; for (i = 0; i < 9; ++i) { if (S[i] < minS) { minS = S[i]; mini = i; } } } for (i = 0; i < 9; i++) H[i] = V[i * 9 + mini]; denormalize_homography_reorder(H, T1, T2); aom_free(a); if (H[8] == 0.0) { return 1; } else { // normalize double f = 1.0 / H[8]; for (i = 0; i < 8; i++) mat[i] = f * H[i]; } return 0; }
static int find_hortrapezoid(int np, double *pts1, double *pts2, double *mat) { const int np3 = np * 3; double *a = (double *)aom_malloc(sizeof(*a) * np3 * 14); double *U = a + np3 * 7; double S[7], V[7 * 7], H[9]; int i, mini; double sx, sy, dx, dy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); a[i * 3 * 7 + 0] = a[i * 3 * 7 + 1] = a[i * 3 * 7 + 2] = 0; a[i * 3 * 7 + 3] = -sy; a[i * 3 * 7 + 4] = -1; a[i * 3 * 7 + 5] = dy * sy; a[i * 3 * 7 + 6] = dy; a[(i * 3 + 1) * 7 + 0] = sx; a[(i * 3 + 1) * 7 + 1] = sy; a[(i * 3 + 1) * 7 + 2] = 1; a[(i * 3 + 1) * 7 + 3] = a[(i * 3 + 1) * 7 + 4] = 0; a[(i * 3 + 1) * 7 + 5] = -dx * sy; a[(i * 3 + 1) * 7 + 6] = -dx; a[(i * 3 + 2) * 7 + 0] = -dy * sx; a[(i * 3 + 2) * 7 + 1] = -dy * sy; a[(i * 3 + 2) * 7 + 2] = -dy; a[(i * 3 + 2) * 7 + 3] = dx * sy; a[(i * 3 + 2) * 7 + 4] = dx; a[(i * 3 + 2) * 7 + 5] = a[(i * 3 + 2) * 7 + 6] = 0; } if (SVD(U, S, V, a, np3, 7)) { aom_free(a); return 1; } else { double minS = 1e12; mini = -1; for (i = 0; i < 7; ++i) { if (S[i] < minS) { minS = S[i]; mini = i; } } } H[3] = H[6] = 0; for (i = 0; i < 3; i++) H[i] = V[i * 7 + mini]; for (; i < 5; i++) H[i + 1] = V[i * 7 + mini]; for (; i < 7; i++) H[i + 2] = V[i * 7 + mini]; denormalize_homography_reorder(H, T1, T2); aom_free(a); if (H[8] == 0.0) { return 1; } else { // normalize double f = 1.0 / H[8]; for (i = 0; i < 8; i++) mat[i] = f * H[i]; } return 0; }