47 double x_cx = 0, x_cy = 0;
48 double y_cx = 0, y_cy = 0;
50 for (
int i = 0; i <
zarray_size(correspondences); i++) {
71 for (
int i = 0; i <
zarray_size(correspondences); i++) {
76 double worldx = c[0] - x_cx;
77 double worldy = c[1] - x_cy;
78 double imagex = c[2] - y_cx;
79 double imagey = c[3] - y_cy;
84 double a06 = worldx*imagey;
85 double a07 = worldy*imagey;
113 double a16 = -worldx*imagex;
114 double a17 = -worldy*imagex;
115 double a18 = -imagex;
139 double a20 = -worldx*imagey;
140 double a21 = -worldy*imagey;
141 double a22 = -imagey;
142 double a23 = worldx*imagex;
143 double a24 = worldy*imagex;
170 for (
int i = 0; i < 9; i++)
171 for (
int j = i+1; j < 9; j++)
183 for (
int i = 0; i < 9; i++)
187 for (
int i = 0; i < 3; i++)
188 for (
int j = 0; j < 3; j++)
194 matd_t *b =
matd_create_data(9, 1, (
double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
209 for (
int i = 0; i < 9; i++)
213 for (
int i = 0; i < 3; i++)
214 for (
int j = 0; j < 3; j++)
225 for (
int i = 0; i < 3; i++)
226 for (
int j = 0; j < 3; j++)
287 double R00 = (
MATD_EL(H, 0, 0) - cx*R20) / fx;
288 double R01 = (
MATD_EL(H, 0, 1) - cx*R21) / fx;
289 double TX = (
MATD_EL(H, 0, 2) - cx*TZ) / fx;
290 double R10 = (
MATD_EL(H, 1, 0) - cy*R20) / fy;
291 double R11 = (
MATD_EL(H, 1, 1) - cy*R21) / fy;
292 double TY = (
MATD_EL(H, 1, 2) - cy*TZ) / fy;
296 double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
297 double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
298 double s = 1.0 / sqrtf(length1 * length2);
316 double R02 = R10*R21 - R20*R11;
317 double R12 = R20*R01 - R00*R21;
318 double R22 = R00*R11 - R10*R01;
333 R =
matd_op(
"M*M'", svd.U, svd.V);
369 double R20 = -
MATD_EL(H, 2, 0);
370 double R21 = -
MATD_EL(H, 2, 1);
372 double R00 = (
MATD_EL(H, 0, 0) - A*R20) / F;
373 double R01 = (
MATD_EL(H, 0, 1) - A*R21) / F;
374 double TX = (
MATD_EL(H, 0, 2) - A*TZ) / F;
375 double R10 = (
MATD_EL(H, 1, 0) - B*R20) / G;
376 double R11 = (
MATD_EL(H, 1, 1) - B*R21) / G;
377 double TY = (
MATD_EL(H, 1, 2) - B*TZ) / G;
381 double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
382 double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
383 double s = 1.0 / sqrtf(length1 * length2);
401 double R02 = R10*R21 - R20*R11;
402 double R12 = R20*R01 - R00*R21;
403 double R22 = R00*R11 - R10*R01;
472 double w = q[0], x = q[1], y = q[2], z = q[3];
474 MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z;
475 MATD_EL(M, 0, 1) = 2*x*y - 2*w*z;
476 MATD_EL(M, 0, 2) = 2*x*z + 2*w*y;
478 MATD_EL(M, 1, 0) = 2*x*y + 2*w*z;
479 MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z;
480 MATD_EL(M, 1, 2) = 2*y*z - 2*w*x;
482 MATD_EL(M, 2, 0) = 2*x*z - 2*w*y;
483 MATD_EL(M, 2, 1) = 2*y*z + 2*w*x;
484 MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z;
static void zarray_get_volatile(const zarray_t *za, int idx, void *p)
void quat_to_matrix(const double q[4], matd_t *M)
matd_svd_t matd_svd(matd_t *A)
void matd_destroy(matd_t *m)
#define MATD_SVD_NO_WARNINGS
matd_t * matd_inverse(const matd_t *a)
matd_t * matd_op(const char *expr,...)
matd_t * matd_plu_solve(const matd_plu_t *mlu, const matd_t *b)
static int zarray_size(const zarray_t *za)
matd_t * matd_identity(int dim)
#define MATD_EL(m, row, col)
matd_t * homography_compute(zarray_t *correspondences, int flags)
matd_t * matd_create(int rows, int cols)
void matd_plu_destroy(matd_plu_t *mlu)
#define HOMOGRAPHY_COMPUTE_FLAG_INVERSE
matd_svd_t matd_svd_flags(matd_t *A, int flags)
static double sq(double v)
matd_t * homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy)
matd_plu_t * matd_plu(const matd_t *a)
matd_t * matd_create_data(int rows, int cols, const double *data)
void matd_chol_destroy(matd_chol_t *chol)
matd_t * homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D)
matd_chol_t * matd_chol(matd_t *A)
matd_t * matd_chol_solve(const matd_chol_t *chol, const matd_t *b)