bool BundlerApp::EstimateRelativePose2(int i1, int i2,
camera_params_t &camera1,
camera_params_t &camera2)
{
// int num_images = GetNumImages();
MatchIndex list_idx;
if (i1 < i2)
list_idx = GetMatchIndex(i1, i2); // i1 * num_images + i2;
else
list_idx = GetMatchIndex(i2, i1); // i2 * num_images + i1;
std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx);
// int num_matches = (int) m_match_lists[list_idx].size();
int num_matches = (int) matches.size();
// double f1 = m_image_data[i1].m_init_focal;
// double f2 = m_image_data[i2].m_init_focal;
double K1[9], K2[9];
GetIntrinsics(camera1, K1);
GetIntrinsics(camera2, K2);
double R0[9], t0[3];
int num_inliers = 0;
if (!m_optimize_for_fisheye) {
num_inliers =
EstimatePose5Point(m_image_data[i1].m_keys,
m_image_data[i2].m_keys,
matches,
512, /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.003, // 0.004 /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
} else {
std::vector<Keypoint> k1 = m_image_data[i1].UndistortKeysCopy();
std::vector<Keypoint> k2 = m_image_data[i2].UndistortKeysCopy();
num_inliers =
EstimatePose5Point(k1, k2, matches,
1024, /*512*/ /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.004, /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
}
if (num_inliers == 0)
return false;
printf(" Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches,
100.0 * num_inliers / num_matches);
bool initialized = false;
if (!initialized) {
memcpy(camera2.R, R0, sizeof(double) * 9);
matrix_transpose_product(3, 3, 3, 1, R0, t0, camera2.t);
matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
}
return true;
}
void PlaneData::Transform(const double *M)
{
double p[4] = { m_normal[0], m_normal[1], m_normal[2], m_dist };
double Minv[16];
matrix_invert(4, (double *)M, Minv);
double pNew[4];
matrix_transpose_product(4, 4, 4, 1, Minv, p, pNew);
double len = matrix_norm(3, 1, pNew);
m_normal[0] = pNew[0] / len;
m_normal[1] = pNew[1] / len;
m_normal[2] = pNew[2] / len;
m_dist = pNew[3] / len;
#if 0
double origin[4] = { m_origin[0], m_origin[1], m_origin[2], 1.0 };
double Morigin[4];
matrix_product(4, 4, 4, 1, (double *) M, origin, Morigin);
double dot0, dot1;
matrix_product(1, 4, 4, 1, p, origin, &dot0);
matrix_product(1, 4, 4, 1, pNew, Morigin, &dot1);
printf("dot0 = %0.3f\n", dot0);
printf("dot1 = %0.3f\n", dot1);
#endif
}
v3_t BundlerApp::GeneratePointAtInfinity(const ImageKeyVector &views,
int *added_order,
camera_params_t *cameras,
double &error,
bool explicit_camera_centers)
{
camera_params_t *cam = NULL;
int camera_idx = views[0].first;
int image_idx = added_order[camera_idx];
int key_idx = views[0].second;
Keypoint &key = GetKey(image_idx, key_idx);
cam = cameras + camera_idx;
double p3[3] = { key.m_x, key.m_y, 1.0 };
if (m_optimize_for_fisheye) {
/* Undistort the point */
double x = p3[0], y = p3[1];
m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]);
}
double K[9], Kinv[9];
GetIntrinsics(cameras[camera_idx], K);
matrix_invert(3, K, Kinv);
double ray[3];
matrix_product(3, 3, 3, 1, Kinv, p3, ray);
/* We now have a ray, put it at infinity */
double ray_world[3];
matrix_transpose_product(3, 3, 3, 1, cam->R, ray, ray_world);
double pos[3] = { 0.0, 0.0, 0.0 };
double pt_inf[3] = { 0.0, 0.0, 0.0 };
if (!explicit_camera_centers) {
} else {
memcpy(pos, cam->t, 3 * sizeof(double));
double ray_extend[3];
matrix_scale(3, 1, ray, 100.0, ray_extend);
matrix_sum(3, 1, 3, 1, pos, ray, pt_inf);
}
return v3_new(pt_inf[0], pt_inf[1], pt_inf[2]);
}
/* Decompose a square matrix into an orthogonal matrix and a symmetric
* positive semidefinite matrix */
void matrix_polar_decomposition(int n, double *A, double *Q, double *S)
{
double *U, *diag, *VT;
double *diag_full, *tmp;
int i;
U = (double *) malloc(sizeof(double) * n * n);
diag = (double *) malloc(sizeof(double) * n);
VT = (double *) malloc(sizeof(double) * n * n);
/* Compute SVD */
dgesvd_driver(n, n, A, U, diag, VT);
/* Compute Q */
matrix_product(n, n, n, n, U, VT, Q);
/* Compute S */
diag_full = (double *) malloc(sizeof(double) * n * n);
for (i = 0; i < n * n; i++) {
diag_full[i] = 0.0;
}
for (i = 0; i < n; i++) {
diag_full[i * n + i] = diag[i];
}
tmp = (double *) malloc(sizeof(double) * n * n);
matrix_transpose_product(n, n, n, n, VT, diag_full, tmp);
matrix_product(n, n, n, n, tmp, VT, S);
free(U);
free(diag);
free(VT);
free(diag_full);
free(tmp);
}
void ThresholdTwists(int num_images, ModelMap &models,
std::vector<ImageData> &image_data, bool panos_only)
{
int *num_large_twists = new int[num_images];
int *degree = new int[num_images];
for (int i = 0; i < num_images; i++) {
num_large_twists[i] = 0;
degree[i] = 0;
}
for (int i = 0; i < num_images; i++) {
ModelTable::iterator iter;
for (iter = models.Begin(i); iter != models.End(i); iter++) {
unsigned int j = iter->first; // iter->m_index;
if (i >= j)
continue;
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
TwoFrameModel &m = models.GetModel(idx);
/* Compute the twist */
double Rp_i[9], Rp_j[9];
matrix_transpose(3, 3, m.m_camera0.R, Rp_i);
matrix_transpose(3, 3, m.m_camera1.R, Rp_j);
double Rp_ij[9];
matrix_transpose_product(3, 3, 3, 3, Rp_i, Rp_j, Rp_ij);
double twist_angle = GetTwist(Rp_ij);
if (fabs(RAD2DEG(twist_angle)) >= 12.0) {
num_large_twists[i]++;
num_large_twists[j]++;
}
degree[i]++;
degree[j]++;
}
}
}
for (int i = 0; i < num_images; i++) {
if (degree[i] == 0)
continue;
double perc_large_twists = (double) num_large_twists[i] / degree[i];
int w = image_data[i].GetWidth();
int h = image_data[i].GetHeight();
double ratio = (double) w / h;
if ((panos_only || perc_large_twists < 0.4) &&
ratio > 0.4 && ratio < 2.5) {
continue;
}
printf("[ThresholdTwists] Removing image %d with score %0.3f, %0.3f\n",
i, perc_large_twists, ratio);
std::list<unsigned int> nbrs = models.GetNeighbors(i);
std::list<unsigned int>::iterator iter;
for (iter = nbrs.begin(); iter != nbrs.end(); iter++) {
unsigned int j = *iter; // iter->m_index;
if (i < j) {
MatchIndex idx = GetMatchIndex(i, j);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
} else {
MatchIndex idx = GetMatchIndex(j, i);
if (models.Contains(idx)) {
models.RemoveModel(idx);
}
}
}
}
}
bool BundlerApp::EstimateRelativePose(int i1, int i2,
camera_params_t &camera1,
camera_params_t &camera2)
{
MatchIndex list_idx;
if (i1 < i2)
list_idx = GetMatchIndex(i1, i2);
else
list_idx = GetMatchIndex(i2, i1);
std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx);
int num_matches = (int) matches.size();
double f1 = m_image_data[i1].m_init_focal;
double f2 = m_image_data[i2].m_init_focal;
double E[9], F[9];
std::vector<int> inliers;
if (!m_optimize_for_fisheye) {
inliers =
EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys,
matches,
4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds,
m_fmatrix_threshold * m_fmatrix_threshold,
f1, f2, E, F);
} else {
/* FIXME */
inliers =
EstimateEMatrix(m_image_data[i1].m_keys, m_image_data[i2].m_keys,
matches,
4 * m_fmatrix_rounds, // 8 * m_fmatrix_rounds,
m_fmatrix_threshold * m_fmatrix_threshold,
f1, f2, E, F);
}
if ((int) inliers.size() == 0)
return false;
int num_inliers = (int) inliers.size();
printf(" Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches,
100.0 * num_inliers / num_matches);
/* Estimate a homography with the inliers */
std::vector<KeypointMatch> match_inliers;
for (int i = 0; i < num_inliers; i++) {
match_inliers.push_back(matches[inliers[i]]);
}
int num_match_inliers = (int) match_inliers.size();
double H[9];
std::vector<int> Hinliers =
EstimateTransform(m_image_data[i1].m_keys, m_image_data[i2].m_keys,
match_inliers, MotionHomography,
128 /*m_homography_rounds*/,
6.0 /*m_homography_threshold*/, H);
printf(" Found %d / %d homography inliers (%0.3f%%)\n",
(int) Hinliers.size(), num_inliers,
100.0 * Hinliers.size() / num_inliers);
bool initialized = false;
if ((int) Hinliers.size() > 0) {
matrix_print(3, 3, H);
printf("\n");
if ((double) Hinliers.size() / num_inliers >= 0.75 /*0.85*/) {
KeypointMatch &match0 = matches[Hinliers[0]];
v2_t p10 = v2_new(m_image_data[i1].m_keys[match0.m_idx1].m_x,
m_image_data[i1].m_keys[match0.m_idx1].m_y);
v2_t p20 = v2_new(m_image_data[i2].m_keys[match0.m_idx2].m_x,
m_image_data[i2].m_keys[match0.m_idx2].m_y);
double R1[9], t1[3], R2[9], t2[3];
bool success =
DecomposeHomography(H, f1, f2, R1, t1, R2, t2, p10, p20);
if (success) {
printf("[BundleTwoFrame] Using homography "
"for initialization\n");
/* Decide which solution to use */
double F1h[9], F2h[9];
ComputeFundamentalMatrix(f1, f2, R1, t1, F1h);
ComputeFundamentalMatrix(f1, f2, R2, t2, F2h);
double F1hT[9], F2hT[9];
matrix_transpose(3, 3, F1h, F1hT);
matrix_transpose(3, 3, F2h, F2hT);
int num_inliers1 = 0, num_inliers2 = 0;
for (int i = 0; i < num_match_inliers; i++) {
const KeypointMatch &match = match_inliers[i];
const Keypoint &k1 = m_image_data[i1].m_keys[match.m_idx1];
const Keypoint &k2 = m_image_data[i2].m_keys[match.m_idx2];
v3_t rt = v3_new(k1.m_x, k1.m_y, 1.0);
v3_t lft = v3_new(k2.m_x, k2.m_y, 1.0);
double r1a = fmatrix_compute_residual(F1h, lft, rt);
double r1b = fmatrix_compute_residual(F1hT, rt, lft);
double r2a = fmatrix_compute_residual(F2h, lft, rt);
double r2b = fmatrix_compute_residual(F2hT, rt, lft);
if (r1a < m_fmatrix_threshold && r1b < m_fmatrix_threshold)
num_inliers1++;
if (r2a < m_fmatrix_threshold && r2b < m_fmatrix_threshold)
num_inliers2++;
}
initialized = true;
double *R, *t;
printf(" H1: %d inliers, H2: %d inliers\n",
num_inliers1, num_inliers2);
if (num_inliers1 > num_inliers2) {
R = R1;
t = t1;
} else {
R = R2;
t = t2;
}
memcpy(camera2.R, R, sizeof(double) * 9);
matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t);
matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
}
}
}
if (!initialized) {
KeypointMatch &match = matches[inliers[0]];
v2_t p1 = v2_new(m_image_data[i1].m_keys[match.m_idx1].m_x / f1,
m_image_data[i1].m_keys[match.m_idx1].m_y / f1);
v2_t p2 = v2_new(m_image_data[i2].m_keys[match.m_idx2].m_x / f2,
m_image_data[i2].m_keys[match.m_idx2].m_y / f2);
double R[9], t[3];
int success = find_extrinsics_essential(E, p1, p2, R, t);
if (!success) {
return false;
}
memcpy(camera2.R, R, sizeof(double) * 9);
matrix_transpose_product(3, 3, 3, 1, R, t, camera2.t);
matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
}
return true;
}
void lmdif_driver3(void *fcn, int m, int n, double *xvec, double tol,
int maxfev, double *H) {
int info;
double *fvec;
double gtol = 0, epsfcn = 0;
// int maxfev = 200 * (n + 1);
double *diag;
int mode = 1;
double factor = 100;
int nprint = 1;
int nfev;
double *fjac;
int ldfjac = m;
int *ipvt;
double *qtf;
double *wa1, *wa2, *wa3, *wa4;
if (maxfev == -1)
maxfev = 200 * (n + 1);
if (n > m) {
printf("Error: lmdif called with n > m\n");
return;
}
fvec = (double *)malloc(sizeof(double) * m);
diag = (double *)malloc(sizeof(double) * n);
fjac = (double *)malloc(sizeof(double) * m * n);
ipvt = (int *)malloc(sizeof(int) * n);
qtf = (double *)malloc(sizeof(double) * n);
wa1 = (double *)malloc(sizeof(double) * n);
wa2 = (double *)malloc(sizeof(double) * n);
wa3 = (double *)malloc(sizeof(double) * n);
wa4 = (double *)malloc(sizeof(double) * m);
//#ifdef WIN32
// LMDIF(fcn, &m, &n, xvec, fvec, &tol, &tol, >ol, &maxfev,
// &epsfcn, diag, &mode, &factor, &nprint, &info, &nfev,
// fjac, &ldfjac, ipvt, qtf, wa1, wa2, wa3, wa4);
//#else
lmdif_(fcn, &m, &n, xvec, fvec, &tol, &tol, >ol, &maxfev,
&epsfcn, diag, &mode, &factor, &nprint, &info, &nfev,
fjac, &ldfjac, ipvt, qtf, wa1, wa2, wa3, wa4);
//#endif
#if 1
switch (info) {
case 0:
printf("Improper input parameters\n");
break;
case 1:
printf("Sum of squares tolerance reached\n");
break;
case 2:
printf("x is within tolerance\n");
break;
case 3:
printf("Sum of squares and x are within tolerance\n");
break;
case 4:
printf("fvec orthogonal\n");
break;
case 5:
printf("max function calls made\n");
break;
case 6:
printf("tolerance is too small (squares)\n");
break;
case 7:
printf("tolerance is too small (x)\n");
break;
default:
printf("???\n");
}
#endif
/* Copy the Hessian in fjac to H */
if (H != NULL) {
int i, j;
double *P, *tmp;
double *R;
clock_t start, end;
R = malloc(sizeof(double) * n * n);
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
if (j < i) {
R[i * n + j] = 0.0;
} else {
int idx0 = j * m + i;
int idx1 = i * n + j;
R[idx1] = fjac[idx0];
}
}
}
start = clock();
matrix_transpose_product(n, n, n, n, R, R, H);
// cblas_dgemm_driver_transpose(n, n, n, R, R, H);
/* Unpermute the entries of H */
P = malloc(sizeof(double) * n * n);
for (i = 0; i < n; i++) {
int p = ipvt[i] - 1;
if (p < 0)
printf("[lmdif_driver3] Error: p < 0\n");
for (j = 0; j < n; j++) {
if (p == j)
P[i * n + j] = 1.0;
else
P[i * n + j] = 0.0;
}
}
tmp = malloc(sizeof(double) * n * n);
matrix_transpose_product(n, n, n, n, P, H, tmp);
matrix_product(n, n, n, n, tmp, P, H);
// cblas_dgemm_driver_transpose(n, n, n, P, H, tmp);
// cblas_dgemm_driver(n, n, n, tmp, P, H);
end = clock();
printf("[lmdif_driver3] post-process took %0.3fs\n",
(double) (end - start) / CLOCKS_PER_SEC);
free(P);
free(tmp);
}
/* Clean up */
free(fvec);
free(diag);
free(fjac);
free(ipvt);
free(qtf);
free(wa1);
free(wa2);
free(wa3);
free(wa4);
}