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;
}
