/* Estimate relative pose from a given set of point matches */
int EstimatePose5Point(const std::vector<Keypoint> &k1,
const std::vector<Keypoint> &k2,
std::vector<KeypointMatch> matches,
int num_trials, double threshold,
double *K1, double *K2,
double *R, double *t)
{
int num_pts = (int) matches.size();
v2_t *k1_pts = new v2_t[num_pts];
v2_t *k2_pts = new v2_t[num_pts];
for (int i = 0; i < num_pts; i++) {
int idx1 = matches[i].m_idx1;
int idx2 = matches[i].m_idx2;
k1_pts[i] = v2_new(k1[idx1].m_x, k1[idx1].m_y);
k2_pts[i] = v2_new(k2[idx2].m_x, k2[idx2].m_y);
}
int num_inliers = compute_pose_ransac(num_pts, k1_pts, k2_pts,
K1, K2, threshold, num_trials, R, t);
delete [] k1_pts;
delete [] k2_pts;
return num_inliers;
}
/* Compute the mean of a set of vectors */
v2_t v2_mean(int n, v2_t *v) {
int i;
v2_t mean = v2_new(0.0, 0.0);
for (i = 0; i < n; i++) {
mean = v2_add(mean, v[i]);
}
return v2_scale(1.0 / n, mean);
}
/* Compute the centroid of an array of 2D vectors */
v2_t v2_compute_centroid(v2_t *pts, int num_pts) {
int i;
v2_t centroid = v2_new(0.0, 0.0);
for (i = 0; i < num_pts; i++) {
Vx(centroid) += Vx(pts[i]);
Vy(centroid) += Vy(pts[i]);
}
return v2_scale(1.0 / ((double) num_pts), centroid);
}
std::vector<v2_t> GetPointProjections(const CameraInfo &cam,
const std::vector<PointData> &points,
const std::vector<int> &indices,
bool inside_only,
int &num_inside)
{
int num_points = (int) indices.size();
std::vector<v2_t> projs;
BoundingBox bbox = cam.GetBoundingBox();
num_inside = 0;
for (int i = 0; i < num_points; i++) {
int pidx = indices[i];
const PointData &p = points[pidx];
double proj[2];
#if 1
bool in_front = cam.Project(p.m_pos, proj);
if (!in_front)
continue;
bool inside = bbox.Contains(proj[0], proj[1]);
#else
bool inside = cam.Project(p.m_pos, proj);
#endif
if (inside)
num_inside++;
if (inside_only && inside)
projs.push_back(v2_new(proj[0], proj[1]));
else if (!inside_only)
projs.push_back(v2_new(proj[0], proj[1]));
}
return projs;
}
iv2_t iv2_compute_centroid(iv2_t *pts, int num_pts) {
int i;
v2_t centroid = v2_new(0.0, 0.0);
iv2_t i_centroid;
for (i = 0; i < num_pts; i++) {
Vx(centroid) += (double) Vx(pts[i]);
Vy(centroid) += (double) Vy(pts[i]);
}
#define ROUND(x) (((x) < 0.0) ? (int) ((x) - 0.5) : (int) ((x) + 0.5))
centroid = v2_scale(1.0 / ((double) num_pts), centroid);
i_centroid = iv2_new((int16_t) ROUND(Vx(centroid)),
(int16_t) ROUND(Vy(centroid)));
return i_centroid;
}
/* Add new points to the bundle adjustment */
int
BundlerApp::BundleAdjustAddAllNewPoints(int num_points, int num_cameras,
int *added_order,
camera_params_t *cameras,
v3_t *points, v3_t *colors,
double reference_baseline,
std::vector<ImageKeyVector> &pt_views,
double max_reprojection_error,
int min_views)
{
std::vector<int> track_idxs;
std::vector<ImageKeyVector> new_tracks;
int num_tracks_total = (int) m_track_data.size();
int *tracks_seen = new int[num_tracks_total];
for (int i = 0; i < num_tracks_total; i++) {
tracks_seen[i] = -1;
}
/* Gather up the projections of all the new tracks */
for (int i = 0; i < num_cameras; i++) {
int image_idx1 = added_order[i];
int num_keys = GetNumKeys(image_idx1);
for (int j = 0; j < num_keys; j++) {
Keypoint &key = GetKey(image_idx1, j);
if (key.m_track == -1)
continue; /* Key belongs to no track */
if (key.m_extra != -1)
continue; /* Key is outlier or has already been added */
int track_idx = key.m_track;
/* Check if this track is already associated with a point */
if (m_track_data[track_idx].m_extra != -1)
continue;
/* Check if we've seen this track */
int seen = tracks_seen[track_idx];
if (seen == -1) {
/* We haven't yet seen this track, create a new track */
tracks_seen[track_idx] = (int) new_tracks.size();
ImageKeyVector track;
track.push_back(ImageKey(i, j));
new_tracks.push_back(track);
track_idxs.push_back(track_idx);
} else {
new_tracks[seen].push_back(ImageKey(i, j));
}
}
}
delete [] tracks_seen;
/* Now for each (sub) track, triangulate to see if the track is
* consistent */
int pt_count = num_points;
int num_ill_conditioned = 0;
int num_high_reprojection = 0;
int num_cheirality_failed = 0;
int num_added = 0;
int num_tracks = (int) new_tracks.size();
for (int i = 0; i < num_tracks; i++) {
int num_views = (int) new_tracks[i].size();
if (num_views < min_views) continue; /* Not enough views */
#if 0
printf("Triangulating track ");
PrintTrack(new_tracks[i]);
printf("\n");
#endif
/* Check if at least two cameras fix the position of the point */
bool conditioned = false;
bool good_distance = false;
double max_angle = 0.0;
for (int j = 0; j < num_views; j++) {
for (int k = j+1; k < num_views; k++) {
int camera_idx1 = new_tracks[i][j].first;
int image_idx1 = added_order[camera_idx1];
int key_idx1 = new_tracks[i][j].second;
int camera_idx2 = new_tracks[i][k].first;
int image_idx2 = added_order[camera_idx2];
int key_idx2 = new_tracks[i][k].second;
Keypoint &key1 = GetKey(image_idx1, key_idx1);
Keypoint &key2 = GetKey(image_idx2, key_idx2);
v2_t p = v2_new(key1.m_x, key1.m_y);
v2_t q = v2_new(key2.m_x, key2.m_y);
if (m_optimize_for_fisheye) {
double p_x = Vx(p), p_y = Vy(p);
double q_x = Vx(q), q_y = Vy(q);
m_image_data[image_idx1].
UndistortPoint(p_x, p_y, Vx(p), Vy(p));
m_image_data[image_idx2].
UndistortPoint(q_x, q_y, Vx(q), Vy(q));
}
double angle = ComputeRayAngle(p, q,
cameras[camera_idx1],
cameras[camera_idx2]);
if (angle > max_angle)
max_angle = angle;
/* Check that the angle between the rays is large
* enough */
if (RAD2DEG(angle) >= m_ray_angle_threshold) {
conditioned = true;
}
#if 0
double dist_jk =
GetCameraDistance(cameras + j, cameras + k,
m_explicit_camera_centers);
if (dist_jk > m_min_camera_distance_ratio * reference_baseline)
good_distance = true;
#else
good_distance = true;
#endif
}
}
if (!conditioned || !good_distance) {
num_ill_conditioned++;
#if 0
printf(">> Track is ill-conditioned [max_angle = %0.3f]\n",
RAD2DEG(max_angle));
fflush(stdout);
#endif
continue;
}
double error;
v3_t pt;
if (!m_panorama_mode) {
pt = TriangulateNViews(new_tracks[i], added_order, cameras,
error, true);
} else {
pt = GeneratePointAtInfinity(new_tracks[i], added_order, cameras,
error, true);
}
// Changed by Wan, Yi
if (::isnan(error) || error > max_reprojection_error) {
num_high_reprojection++;
#if 0
printf(">> Reprojection error [%0.3f] is too large\n", error);
fflush(stdout);
#endif
continue;
}
bool all_in_front = true;
for (int j = 0; j < num_views; j++) {
int camera_idx = new_tracks[i][j].first;
bool in_front = CheckCheirality(pt, cameras[camera_idx]);
if (!in_front) {
all_in_front = false;
break;
}
}
if (!all_in_front) {
num_cheirality_failed++;
#if 0
printf(">> Cheirality check failed\n");
fflush(stdout);
#endif
continue;
}
/* All tests succeeded, so let's add the point */
#if 0
printf("Triangulating track ");
PrintTrack(new_tracks[i]);
printf("\n");
printf(">> All tests succeeded [%0.3f, %0.3f] for point [%d]\n",
RAD2DEG(max_angle), error, pt_count);
#endif
fflush(stdout);
points[pt_count] = pt;
int camera_idx = new_tracks[i][0].first;
int image_idx = added_order[camera_idx];
int key_idx = new_tracks[i][0].second;
unsigned char r = GetKey(image_idx, key_idx).m_r;
unsigned char g = GetKey(image_idx, key_idx).m_g;
unsigned char b = GetKey(image_idx, key_idx).m_b;
colors[pt_count] = v3_new((double) r, (double) g, (double) b);
pt_views.push_back(new_tracks[i]);
/* Set the point index on the keys */
for (int j = 0; j < num_views; j++) {
int camera_idx = new_tracks[i][j].first;
int image_idx = added_order[camera_idx];
int key_idx = new_tracks[i][j].second;
GetKey(image_idx, key_idx).m_extra = pt_count;
}
int track_idx = track_idxs[i];
m_track_data[track_idx].m_extra = pt_count;
pt_count++;
num_added++;
}
printf("[AddAllNewPoints] Added %d new points\n", num_added);
printf("[AddAllNewPoints] Ill-conditioned tracks: %d\n",
num_ill_conditioned);
printf("[AddAllNewPoints] Bad reprojections: %d\n", num_high_reprojection);
printf("[AddAllNewPoints] Failed cheirality checks: %d\n",
num_cheirality_failed);
return pt_count;
}
v2* v2_copy(v2* v) {
v2* ret = v2_new(0,0);
memcpy(ret->v, v->v, 2 * sizeof(float));
return ret;
}
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;
}
/* Compute the pair-wise minimum / maximum of two vectors */
v2_t v2_minimum(v2_t u, v2_t v) {
return v2_new(MIN(Vx(u), Vx(v)),
MIN(Vy(u), Vy(v)));
}
v2_t v2_scale(double s, v2_t v) {
return v2_new(s * Vx(v), s * Vy(v));
}
/* Add new points to the bundle adjustment */
int BundlerApp::BundleAdjustAddNewPoints(int camera_idx,
int num_points, int num_cameras,
int *added_order,
camera_params_t *cameras,
v3_t *points, v3_t *colors,
double reference_baseline,
std::vector<ImageKeyVector> &pt_views)
{
int pt_count = num_points;
int image_idx = added_order[camera_idx];
/* Recompute the locations of the new points given the initial
* pose estimate */
for (int i = 0; i < num_cameras; i++) {
int other = added_order[i];
if (other == image_idx)
continue;
int first = MIN(image_idx, other);
int second = MAX(image_idx, other);
MatchIndex idx = GetMatchIndex(first, second);
SetMatchesFromTracks(first, second);
printf(" Matches[%d,%d] = %d\n", image_idx, other,
(int) m_matches.GetNumMatches(idx));
// (int) m_match_lists[idx].size());
double disti = GetCameraDistance(cameras + i, cameras + camera_idx);
printf(" dist0, disti = %0.3e, %0.3e\n", reference_baseline, disti);
if (disti < m_min_camera_distance_ratio * reference_baseline) {
printf(" Distance too low (possible panorama?)\n");
// m_match_lists[idx].clear();
m_matches.ClearMatch(idx);
continue;
}
std::vector<KeypointMatch> &list = m_matches.GetMatchList(idx);
for (int j = 0; j < (int) list.size(); j++) {
int idx1 = list[j].m_idx1;
int idx2 = list[j].m_idx2;
int this_idx, other_idx;
if (image_idx == first) {
this_idx = idx1;
other_idx = idx2;
} else {
other_idx = idx1;
this_idx = idx2;
}
if (GetKey(other,other_idx).m_extra == -2) {
/* The other key was already marked as an outlier */
continue;
} else if (GetKey(image_idx,this_idx).m_extra == -2) {
/* This key was already marked as an outlier */
continue;
}
if (GetKey(other,other_idx).m_extra == -1 &&
GetKey(image_idx,this_idx).m_extra >= 0) {
/**** Connecting an existing point *** */
/* Connect up the other point to this one */
int pt_idx = GetKey(image_idx,this_idx).m_extra;
/* Check reprojection error */
v2_t pr = sfm_project_final(cameras + i, points[pt_idx],
true, m_estimate_distortion);
double dx = GetKey(other,other_idx).m_x - Vx(pr);
double dy = GetKey(other,other_idx).m_y - Vy(pr);
double proj_error = sqrt(dx * dx + dy * dy);
if (proj_error >= 32.0) {
printf(" Would have connected existing match "
"%d ==> %d [%d] (cam: %d), \n"
" but reprojection error (%0.3f) "
"is too high.\n",
this_idx, other_idx, pt_idx, other, proj_error);
} else {
printf(" Connecting existing match "
"%d ==> %d [%d] (cam: %d) [%0.3f]\n",
this_idx, other_idx, pt_idx, other, proj_error);
GetKey(other,other_idx).m_extra = pt_idx;
pt_views[pt_idx].push_back(ImageKey(i, other_idx));
}
} else if (GetKey(other,other_idx).m_extra == -1) {
if (GetKey(image_idx,this_idx).m_extra != -1) {
printf("Error! Key (%d,%d) shouldn't be seen yet!\n",
image_idx, this_idx);
printf("Point index is %d\n",
GetKey(image_idx,this_idx).m_extra);
}
/* This is a new point */
GetKey(other,other_idx).m_extra = pt_count;
GetKey(image_idx,this_idx).m_extra = pt_count;
/* Set up the 3D point */
v2_t p = v2_new(GetKey(other,other_idx).m_x,
GetKey(other,other_idx).m_y);
v2_t q = v2_new(GetKey(image_idx,this_idx).m_x,
GetKey(image_idx,this_idx).m_y);
if (m_optimize_for_fisheye) {
double p_x = Vx(p), p_y = Vy(p);
double q_x = Vx(q), q_y = Vy(q);
m_image_data[other].
UndistortPoint(p_x, p_y, Vx(p), Vy(p));
m_image_data[image_idx].
UndistortPoint(q_x, q_y, Vx(q), Vy(q));
}
double proj_error = 0.0;
bool in_front = false;
double angle = 0.0;
points[pt_count] =
Triangulate(p, q, cameras[i], cameras[camera_idx],
proj_error, in_front, angle, true);
/* Check that the angle between the rays is large
* enough */
if (RAD2DEG(angle) < m_ray_angle_threshold) {
printf(" Ray angle %d => %d is too small (%0.3f)\n",
this_idx, other_idx, RAD2DEG(angle));
/* Remove point */
GetKey(other,other_idx).m_extra = -1;
GetKey(image_idx,this_idx).m_extra = -1;
continue;
}
/* Check the reprojection error */
if (proj_error >= ADD_REPROJECTION_ERROR) {
printf(" Projection error for %d => %d is %0.3e, "
"removing\n",
this_idx, other_idx, proj_error);
/* Remove point */
GetKey(other,other_idx).m_extra = -2;
GetKey(image_idx,this_idx).m_extra = -2;
continue;
}
/* Check cheirality */
if (!in_front) {
printf(" Cheirality violated!\n");
/* Remove point */
GetKey(other,other_idx).m_extra = -2;
GetKey(image_idx,this_idx).m_extra = -2;
continue;
}
printf(" Adding match %d ==> %d [%d] (cam: %d ==> %d) "
"[%0.3f, %0.3f]\n",
other_idx, this_idx, pt_count, image_idx, other,
RAD2DEG(angle), proj_error);
/* Finally, add the point */
unsigned char r = GetKey(other,other_idx).m_r;
unsigned char g = GetKey(other,other_idx).m_g;
unsigned char b = GetKey(other,other_idx).m_b;
colors[pt_count] = v3_new((double) r,
(double) g,
(double) b);
ImageKeyVector views;
views.push_back(ImageKey(i, other_idx));
views.push_back(ImageKey(camera_idx, this_idx));
pt_views.push_back(views);
pt_count++;
} else if (GetKey(other,other_idx).m_extra >= 0 &&
GetKey(image_idx,this_idx).m_extra == -1) {
/* We didn't connect this point originally --
* check if it's now a good idea to add it in */
/* Connect up the other point to this one */
int pt_idx = GetKey(other,other_idx).m_extra;
/* Check reprojection error */
v2_t pr = sfm_project_final(cameras + camera_idx,
points[pt_idx],
true, m_estimate_distortion);
double dx = GetKey(image_idx,this_idx).m_x - Vx(pr);
double dy = GetKey(image_idx,this_idx).m_y - Vy(pr);
double proj_error = sqrt(dx * dx + dy * dy);
if (proj_error <= INIT_REPROJECTION_ERROR) {
printf(" Reconnecting point [%d] (%d) (error: %0.3f)\n",
pt_idx, this_idx, proj_error);
GetKey(image_idx,this_idx).m_extra = pt_idx;
pt_views[pt_idx].push_back(ImageKey(camera_idx,this_idx));
} else {
/* Throw out this point as an outlier */
GetKey(image_idx,this_idx).m_extra = -2;
}
}
}
// m_match_lists[idx].clear();
m_matches.ClearMatch(idx);
}
return pt_count;
}
v2_t v2_maximum(v2_t u, v2_t v) {
return v2_new(MAX(Vx(u), Vx(v)),
MAX(Vy(u), Vy(v)));
}
/* Create a new image by applying transformation T to img and
* resampling. Resize the image so that the whole thing fits when
* transformed. */
img_t *img_resample_bbox(img_t *img, trans2D_t *T) {
int w = img->w, h = img->h;
int x, y;
trans2D_t *Tinv = transform_invert(T);
int w_new, h_new, i;
// double x_min = DBL_MAX, x_max = -DBL_MAX, y_min = DBL_MAX, y_max = -DBL_MAX;
v2_t min = v2_new(DBL_MAX, DBL_MAX);
v2_t max = v2_new(-DBL_MAX, -DBL_MAX);
v2_t origin;
img_t *Timg;
/* Find the new dimensions of the window */
v2_t crs[4]; /* Four corners of the original image */
crs[0] = v2_new(0, 0);
crs[1] = v2_new(0, h - 1);
crs[2] = v2_new(w - 1, 0);
crs[3] = v2_new(w - 1, h - 1);
for (i = 0; i < 4; i++) {
crs[i] = v2_add(crs[i], img->origin);
crs[i] = transform_vector(T, crs[i]);
min = v2_minimum(min, crs[i]);
max = v2_maximum(max, crs[i]);
}
Vx(min) = floor(Vx(min));
Vy(min) = floor(Vy(min));
w_new = iround(floor(Vx(max) - Vx(min) + 1));
h_new = iround(floor(Vy(max) - Vy(min) + 1));
origin = min;
Timg = img_new(w_new, h_new);
Timg->origin = origin;
for (y = 0; y < h_new; y++) {
for (x = 0; x < w_new; x++) {
double Tp[2];
fcolor_t c;
/* Invert the point (x, y) - trans */
transform_point(Tinv, x + Vx(origin), y + Vy(origin), &Tp[0], &Tp[1]);
#if 1
/* Check if the result is in range */
if (Tp[0] < Vx(img->origin) || Tp[1] < Vy(img->origin) ||
Tp[0] > Vx(img->origin) + w - 1 || Tp[1] > Vy(img->origin) + h - 1) {
/* pass */
} else {
/* Check if the result is valid */
int x_f = (int) (Tp[0] - Vx(img->origin));
int x_c = x_f + 1;
int y_f = (int) (Tp[1] - Vy(img->origin));
int y_c = y_f + 1;
if (img_pixel_is_valid(img, x_f, y_f) ||
img_pixel_is_valid(img, x_c, y_f) ||
img_pixel_is_valid(img, x_f, y_c) ||
img_pixel_is_valid(img, x_c, y_c)) {
/* Apply bilinear interpolation */
c = pixel_lerp(img, Tp[0] - Vx(img->origin), Tp[1] - Vy(img->origin));
img_set_pixel(Timg, x, y, iround(c.r), iround(c.g), iround(c.b));
} else {
// img_nullify_pixel(Timg, x, y);
}
}
#else
if (Tp[0] < 0.0) Tp[0] = 0.0;
else if (Tp[0] > w - 1) Tp[0] = w - 1;
if (Tp[1] < 0.0) Tp[1] = 0.0;
else if (Tp[1] > h - 1) Tp[1] = h - 1;
c = pixel_lerp(img, Tp[0], Tp[1]);
img_set_pixel(Timg, x, y, c.r, c.g, c.b);
#endif
}
}
/* Add the old origin to the image */
// Timg->origin = v2_add(origin, img->origin);
transform_free(Tinv);
return Timg;
}
/* Fit a plane to the points at the given indices */
std::vector<int> FitPlaneToPoints(const std::vector<PointData> &points,
const std::vector<int> &indices,
double *plane,
int ransac_rounds,
double ransac_threshold,
bool par_to_up, bool perp_to_up,
double *up)
{
if (par_to_up && perp_to_up) {
printf("[FitPlaneToPoints] Error: cannot be both "
"parallel and perpendicular to the up vector!\n");
perp_to_up = false;
}
std::vector<int> inliers;
if (!par_to_up) {
/* Marshall the points */
int num_points = (int) indices.size();
v3_t *pts = new v3_t[num_points];
for (int i = 0; i < num_points; i++) {
int pt_idx = indices[i];
const PointData &pt = points[pt_idx];
pts[i] = v3_new(pt.m_pos[0], pt.m_pos[1], pt.m_pos[2]);
}
/* Fit the plane */
int num_inliers = 0;
double error = fit_3D_plane_ortreg_ransac(num_points, pts,
ransac_rounds,
ransac_threshold,
&num_inliers, plane);
printf("error = %0.3f\n", error);
/* Gather the inliers */
for (int i = 0; i < num_points; i++) {
double dist = plane_point_distance(plane, pts[i]);
if (dist < ransac_threshold) {
inliers.push_back(indices[i]);
}
}
if (perp_to_up) {
/* Compute the mean of the inliers */
int num_inliers = (int) inliers.size();
v3_t *pts_inlier = new v3_t[num_inliers];
for (int i = 0; i < num_inliers; i++) {
int pt_idx = inliers[i];
const PointData &pt = points[pt_idx];
pts_inlier[i] = v3_new(pt.m_pos[0], pt.m_pos[1], pt.m_pos[2]);
}
v3_t mean = v3_mean(num_inliers, pts_inlier);
double dot;
matrix_product(1, 3, 3, 1, up, mean.p, &dot);
plane[0] = up[0];
plane[1] = up[1];
plane[2] = up[2];
plane[3] = -dot;
delete [] pts_inlier;
}
delete [] pts;
} else {
assert(fabs(up[1] - 1.0) < 1.0e-5);
/* Marshall the points */
int num_points = (int) indices.size(); // points.size();
v2_t *pts = new v2_t[num_points];
for (int i = 0; i < num_points; i++) {
int pt_idx = indices[i];
const PointData &pt = points[pt_idx];
pts[i] = v2_new(pt.m_pos[0], pt.m_pos[2]);
}
/* Fit the plane */
double line[3];
int num_inliers = 0;
double error = fit_2D_line_ortreg_ransac(num_points, pts,
ransac_rounds,
ransac_threshold,
&num_inliers, line);
plane[0] = line[0];
plane[1] = 0.0;
plane[2] = line[1];
plane[3] = line[2];
printf("error = %0.3f\n", error);
printf("num_inliers = %d\n", num_inliers);
/* Gather the inliers */
for (int i = 0; i < num_points; i++) {
int pt_idx = indices[i];
const PointData &p = points[pt_idx];
v3_t pt = v3_new(p.m_pos[0], p.m_pos[1], p.m_pos[2]);
double dist = plane_point_distance(plane, pt);
if (dist < ransac_threshold) {
inliers.push_back(indices[i]);
}
}
}
return inliers;
}
/* Triangulate two points */
v3_t Triangulate(v2_t p, v2_t q,
camera_params_t c1, camera_params_t c2,
double &proj_error, bool &in_front, double &angle,
bool explicit_camera_centers)
{
double K1[9], K2[9];
double K1inv[9], K2inv[9];
GetIntrinsics(c1, K1);
GetIntrinsics(c2, K2);
matrix_invert(3, K1, K1inv);
matrix_invert(3, K2, K2inv);
/* Set up the 3D point */
// EDIT!!!
double proj1[3] = { Vx(p), Vy(p), -1.0 };
double proj2[3] = { Vx(q), Vy(q), -1.0 };
double proj1_norm[3], proj2_norm[3];
matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm);
matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm);
v2_t p_norm = v2_new(proj1_norm[0] / proj1_norm[2],
proj1_norm[1] / proj1_norm[2]);
v2_t q_norm = v2_new(proj2_norm[0] / proj2_norm[2],
proj2_norm[1] / proj2_norm[2]);
/* Undo radial distortion */
p_norm = UndistortNormalizedPoint(p_norm, c1);
q_norm = UndistortNormalizedPoint(q_norm, c2);
/* Compute the angle between the rays */
angle = ComputeRayAngle(p, q, c1, c2);
/* Triangulate the point */
v3_t pt;
if (!explicit_camera_centers) {
pt = triangulate(p_norm, q_norm, c1.R, c1.t, c2.R, c2.t, &proj_error);
} else {
double t1[3];
double t2[3];
/* Put the translation in standard form */
matrix_product(3, 3, 3, 1, c1.R, c1.t, t1);
matrix_scale(3, 1, t1, -1.0, t1);
matrix_product(3, 3, 3, 1, c2.R, c2.t, t2);
matrix_scale(3, 1, t2, -1.0, t2);
pt = triangulate(p_norm, q_norm, c1.R, t1, c2.R, t2, &proj_error);
}
proj_error = (c1.f + c2.f) * 0.5 * sqrt(proj_error * 0.5);
/* Check cheirality */
bool cc1 = CheckCheirality(pt, c1);
bool cc2 = CheckCheirality(pt, c2);
in_front = (cc1 && cc2);
return pt;
}
/* Triangulate a subtrack */
v3_t BundlerApp::TriangulateNViews(const ImageKeyVector &views,
int *added_order,
camera_params_t *cameras,
double &error,
bool explicit_camera_centers)
{
int num_views = (int) views.size();
v2_t *pv = new v2_t[num_views];
double *Rs = new double[9 * num_views];
double *ts = new double[3 * num_views];
for (int i = 0; i < num_views; i++) {
camera_params_t *cam = NULL;
int camera_idx = views[i].first;
int image_idx = added_order[camera_idx];
int key_idx = views[i].second;
Keypoint &key = GetKey(image_idx, key_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 p_n[3];
matrix_product(3, 3, 3, 1, Kinv, p3, p_n);
// EDIT!!!
pv[i] = v2_new(-p_n[0], -p_n[1]);
pv[i] = UndistortNormalizedPoint(pv[i], cameras[camera_idx]);
cam = cameras + camera_idx;
memcpy(Rs + 9 * i, cam->R, 9 * sizeof(double));
if (!explicit_camera_centers) {
memcpy(ts + 3 * i, cam->t, 3 * sizeof(double));
} else {
matrix_product(3, 3, 3, 1, cam->R, cam->t, ts + 3 * i);
matrix_scale(3, 1, ts + 3 * i, -1.0, ts + 3 * i);
}
}
v3_t pt = triangulate_n(num_views, pv, Rs, ts, &error);
error = 0.0;
for (int i = 0; i < num_views; i++) {
int camera_idx = views[i].first;
int image_idx = added_order[camera_idx];
int key_idx = views[i].second;
Keypoint &key = GetKey(image_idx, key_idx);
v2_t pr = sfm_project_final(cameras + camera_idx, pt,
explicit_camera_centers ? 1 : 0,
m_estimate_distortion ? 1 : 0);
if (m_optimize_for_fisheye) {
double x = Vx(pr), y = Vy(pr);
m_image_data[image_idx].DistortPoint(x, y, Vx(pr), Vy(pr));
}
double dx = Vx(pr) - key.m_x;
double dy = Vy(pr) - key.m_y;
error += dx * dx + dy * dy;
}
error = sqrt(error / num_views);
delete [] pv;
delete [] Rs;
delete [] ts;
return pt;
}
v2_t v2_add(v2_t u, v2_t v) {
return v2_new(Vx(u) + Vx(v),
Vy(u) + Vy(v));
}
v2_t v2_sub(v2_t u, v2_t v) {
return v2_new(Vx(u) - Vx(v),
Vy(u) - Vy(v));
}
/* Quickly compute pose of all cameras */
void BundlerApp::BundleAdjustFast()
{
clock_t start = clock();
/* Compute initial image information */
ComputeGeometricConstraints();//读取constrains.txt里面的信息，构造track信息
#if 0
/* Read keypoints */
printf("[BundleAdjust] Reading key colors...\n");
ReadKeyColors();
#endif
/* Set track pointers to -1 */
for (int i = 0; i < (int) m_track_data.size(); i++)
{
m_track_data[i].m_extra = -1;
}
/* For now, assume all images form one connected component */
int num_images = GetNumImages();
int *added_order = new int[num_images];
int *added_order_inv = new int[num_images];
/* **** Run bundle adjustment! **** */
camera_params_t *cameras = new camera_params_t[num_images];
int max_pts = (int) m_track_data.size(); // 1243742; /* HACK! */
v3_t *points = new v3_t[max_pts];
v3_t *colors = new v3_t[max_pts];
std::vector<ImageKeyVector> pt_views;
/* Initialize the bundle adjustment */
int num_init_cams = 0;
InitializeBundleAdjust(num_init_cams, added_order, added_order_inv,
cameras, points, colors, pt_views,
m_use_constraints);
int i_best = -1, j_best = -1, max_matches = 0;
double max_score = 0.0;
int curr_num_cameras, curr_num_pts;
int pt_count;
if (num_init_cams == 0)
{
BundlePickInitialPair(i_best, j_best, true);
added_order[0] = i_best;
added_order[1] = j_best;
printf("[BundleAdjust] Adjusting cameras "
"%d and %d (score = %0.3f)\n",
i_best, j_best, max_score);
/* **** Set up the initial cameras **** */
double init_focal_length_0 = 0.0, init_focal_length_1 = 0.0;
pt_count = curr_num_pts =
SetupInitialCameraPair(i_best, j_best,
init_focal_length_0, init_focal_length_1,
cameras, points, colors, pt_views);
DumpOutputFile(m_output_directory, "bundle.init.out",
num_images, 2, curr_num_pts,
added_order, cameras, points, colors, pt_views);
/* Run sfm for the first time */
double error0;
error0 = RunSFM(curr_num_pts, 2, 0, false,
cameras, points, added_order, colors, pt_views);
printf(" focal lengths: %0.3f, %0.3f\n", cameras[0].f, cameras[1].f);
#if 1
if (m_fix_necker) {
/* Swap the cameras and flip the depths to deal with Necker
* reversal */
camera_params_t cameras_old[2];
v3_t *points_old;
points_old = new v3_t [curr_num_pts];
memcpy(points_old, points, sizeof(v3_t) * curr_num_pts);
memcpy(cameras_old, cameras, sizeof(camera_params_t) * 2);
camera_params_t tmp = cameras[0];
memcpy(cameras[0].R, cameras[1].R, sizeof(double) * 9);
memcpy(cameras[0].t, cameras[1].t, sizeof(double) * 3);
cameras[0].f = init_focal_length_0;
cameras[0].k[0] = cameras[0].k[1] = 0.0;
memcpy(cameras[1].R, tmp.R, sizeof(double) * 9);
memcpy(cameras[1].t, tmp.t, sizeof(double) * 3);
cameras[1].f = init_focal_length_1;
cameras[1].k[0] = cameras[1].k[1] = 0.0;
double K1inv[9] =
{ 1.0 / cameras[0].f, 0.0, 0.0,
0.0, 1.0 / cameras[0].f, 0.0,
0.0, 0.0, 1.0 };
double K2inv[9] =
{ 1.0 / cameras[1].f, 0.0, 0.0,
0.0, 1.0 / cameras[1].f, 0.0,
0.0, 0.0, 1.0 };
for (int i = 0; i < curr_num_pts; i++) {
int k1 = pt_views[i][0].second;
int k2 = pt_views[i][1].second;
double proj1[3] = { GetKey(added_order[0],k1).m_x,
GetKey(added_order[0],k1).m_y,
-1.0 };
double proj2[3] = { GetKey(added_order[1],k2).m_x,
GetKey(added_order[1],k2).m_y,
-1.0 };
double proj1_norm[3], proj2_norm[3];
matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm);
matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm);
v2_t p = v2_new(proj1_norm[0] / proj1_norm[2],
proj1_norm[1] / proj1_norm[2]);
v2_t q = v2_new(proj2_norm[0] / proj2_norm[2],
proj2_norm[1] / proj2_norm[2]);
double proj_error;
double t1[3];
double t2[3];
/* Put the translation in standard form */
matrix_product(3, 3, 3, 1, cameras[0].R, cameras[0].t, t1);
matrix_scale(3, 1, t1, -1.0, t1);
matrix_product(3, 3, 3, 1, cameras[1].R, cameras[1].t, t2);
matrix_scale(3, 1, t2, -1.0, t2);
points[i] = triangulate(p, q,
cameras[0].R, t1, cameras[1].R, t2,
&proj_error);
}
double error1;
error1 = RunSFM(curr_num_pts, 2, 0, false,
cameras, points, added_order, colors, pt_views);
}
#endif
DumpPointsToPly(m_output_directory, "points001.ply",
curr_num_pts, 2, points, colors, cameras);
if (m_bundle_output_base != NULL) {
char buf[256];
sprintf(buf, "%s%03d.out", m_bundle_output_base, 1);
DumpOutputFile(m_output_directory, buf, num_images, 2, curr_num_pts,
added_order, cameras, points, colors, pt_views);
}
curr_num_cameras = 2;
}
else {
curr_num_cameras = num_init_cams;
pt_count = curr_num_pts = (int) m_point_data.size();
}
int round = 0;
while (curr_num_cameras < num_images) {
int parent_idx;
int max_cam =
FindCameraWithMostMatches(curr_num_cameras, curr_num_pts,
added_order, parent_idx,
max_matches, pt_views);
printf("[BundleAdjust] max_matches = %d\n", max_matches);
if (max_matches < m_min_max_matches)
break; /* No more connections */
/* Find all images with 75% of the matches of the maximum
* (unless overruled by m_num_points_add_camera) */
std::vector<ImagePair> image_set;
if (false && max_matches < 48) { /* disabling this */
image_set.push_back(ImagePair(max_cam, parent_idx));
} else {
int nMatches = iround(0.75 * max_matches);
if (m_num_matches_add_camera > 0) {
/* Alternate threshold based on user parameter */
nMatches = std::min(nMatches, m_num_matches_add_camera);
}
image_set =
FindCamerasWithNMatches(nMatches,
curr_num_cameras, curr_num_pts,
added_order, pt_views);
}
int num_added_images = (int) image_set.size();
printf("[BundleAdjustFast] Registering %d images\n",
num_added_images);
for (int i = 0; i < num_added_images; i++)
printf("[BundleAdjustFast] Adjusting camera %d\n",
image_set[i].first);
/* Now, throw the new cameras into the mix */
int image_count = 0;
for (int i = 0; i < num_added_images; i++) {
int next_idx = image_set[i].first;
int parent_idx = image_set[i].second;
added_order[curr_num_cameras + image_count] = next_idx;
printf("[BundleAdjust[%d]] Adjusting camera %d "
"(parent = %d)\n",
round, next_idx,
(parent_idx == -1 ? -1 : added_order[parent_idx]));
/* **** Set up the new camera **** */
bool success = false;
camera_params_t camera_new =
BundleInitializeImage(m_image_data[next_idx],
next_idx,
curr_num_cameras + image_count,
curr_num_cameras, curr_num_pts,
added_order, points,
NULL, cameras,
pt_views, &success);
if (success) {
cameras[curr_num_cameras+image_count] = camera_new;
image_count++;
} else {
printf("[BundleAdjust] Couldn't initialize image %d\n",
next_idx);
m_image_data[next_idx].m_ignore_in_bundle = true;
}
}
/* Compute the distance between the first pair of cameras */
double dist0 = 0.0;
printf("[BundleAdjust] Adding new matches\n");
pt_count = curr_num_pts;
curr_num_cameras += image_count;
if (!m_skip_add_points) {
pt_count =
BundleAdjustAddAllNewPoints(pt_count, curr_num_cameras,
added_order, cameras,
points, colors,
dist0, pt_views);
}
curr_num_pts = pt_count;
printf("[BundleAdjust] Number of points = %d\n", pt_count);
fflush(stdout);
if (!m_skip_full_bundle) {
/* Run sfm again to update parameters */
RunSFM(curr_num_pts, curr_num_cameras, 0, false,
cameras, points, added_order, colors, pt_views);
/* Remove bad points and cameras */
RemoveBadPointsAndCameras(curr_num_pts, curr_num_cameras + 1,
added_order, cameras, points, colors,
pt_views);
printf(" focal lengths:\n");
for (int i = 0; i < curr_num_cameras; i++) {
if(m_image_data[added_order[i]].m_has_init_focal) {
printf(" [%03d] %0.3f (%0.3f) %s %d; %0.3e %0.3e\n",
i, cameras[i].f,
m_image_data[added_order[i]].m_init_focal,
m_image_data[added_order[i]].m_name,
added_order[i], cameras[i].k[0], cameras[i].k[1]);
} else {
printf(" [%03d] %0.3f %s %d; %0.3e %0.3e\n",
i, cameras[i].f,
m_image_data[added_order[i]].m_name,
added_order[i], cameras[i].k[0], cameras[i].k[1]);
}
// printf(" [%03d] %0.3f\n", i, cameras[i].f);
}
fflush(stdout);
}
/* Dump output for this round */
char buf[256];
sprintf(buf, "points%03d.ply", curr_num_cameras);
DumpPointsToPly(m_output_directory, buf,
curr_num_pts, curr_num_cameras,
points, colors, cameras);
if (m_bundle_output_base != NULL) {
sprintf(buf, "%s%03d.out", m_bundle_output_base,
curr_num_cameras);
DumpOutputFile(m_output_directory, buf,
num_images, curr_num_cameras, curr_num_pts,
added_order, cameras, points, colors, pt_views);
}
round++;
}//while
clock_t end = clock();
printf("[BundleAdjust] Bundle adjustment took %0.3fs\n",
(end - start) / ((double) CLOCKS_PER_SEC));
if (m_estimate_ignored) {
EstimateIgnoredCameras(curr_num_cameras,
cameras, added_order,
curr_num_pts, points, colors, pt_views);
}
/* Dump output */
if (m_bundle_output_file != NULL) {
DumpOutputFile(m_output_directory, m_bundle_output_file,
num_images, curr_num_cameras, curr_num_pts,
added_order, cameras, points, colors, pt_views);
}
/* Save the camera parameters and points */
/* Cameras */
for (int i = 0; i < num_images; i++) {
m_image_data[i].m_camera.m_adjusted = false;
}
for (int i = 0; i < curr_num_cameras; i++) {
int img = added_order[i];
m_image_data[img].m_camera.m_adjusted = true;
memcpy(m_image_data[img].m_camera.m_R, cameras[i].R,
9 * sizeof(double));
matrix_product(3, 3, 3, 1,
cameras[i].R, cameras[i].t,
m_image_data[img].m_camera.m_t);
matrix_scale(3, 1,
m_image_data[img].m_camera.m_t, -1.0,
m_image_data[img].m_camera.m_t);
m_image_data[img].m_camera.m_focal = cameras[i].f;
m_image_data[img].m_camera.Finalize();
}
/* Points */
for (int i = 0; i < curr_num_pts; i++) {
/* Check if the point is visible in any view */
if ((int) pt_views[i].size() == 0)
continue; /* Invisible */
PointData pdata;
pdata.m_pos[0] = Vx(points[i]);
pdata.m_pos[1] = Vy(points[i]);
pdata.m_pos[2] = Vz(points[i]);
pdata.m_color[0] = (float) Vx(colors[i]);
pdata.m_color[1] = (float) Vy(colors[i]);
pdata.m_color[2] = (float) Vz(colors[i]);
#if 1
for (int j = 0; j < (int) pt_views[i].size(); j++) {
int v = pt_views[i][j].first;
int vnew = added_order[v];
pdata.m_views.push_back(ImageKey(vnew, pt_views[i][j].second));
}
#else
pdata.m_views = pt_views[i];
#endif
m_point_data.push_back(pdata);
}
delete [] added_order;
delete [] added_order_inv;
SetMatchesFromPoints();
bool *image_mask = new bool[num_images];
for (int i = 0; i < num_images; i++) {
if (m_image_data[i].m_camera.m_adjusted)
image_mask[i] = true;
else
image_mask[i] = false;
}
}
