static void PrintTrack(const ImageKeyVector &track)
{
int num_views = (int) track.size();
printf("[");
for (int i = 0; i < num_views; i++) {
printf(" (%d %d)", track[i].first, track[i].second);
if (i < num_views - 1)
printf(",");
}
printf(" ]");
}
/* 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;
}
/* 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;
}
/* 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;
}
/* Dump an output file containing information about the current
* state of the world */
void BaseApp::DumpOutputFile(const char *output_dir, const char *filename,
int num_images, int num_cameras, int num_points,
int *added_order,
camera_params_t *cameras,
v3_t *points, v3_t *colors,
std::vector<ImageKeyVector> &pt_views
/*bool output_radial_distortion*/)
{
clock_t start = clock();
int num_visible_points = 0;
for (int i = 0; i < num_points; i++) {
if (pt_views[i].size() > 0)
num_visible_points++;
}
char buf[256];
sprintf(buf, "%s/%s", output_dir, filename);
FILE *f = fopen(buf, "w");
if (f == NULL) {
printf("Error opening file %s for writing\n", buf);
return;
}
// if (output_radial_distortion) {
/* Print version number */
// fprintf(f, "# Bundle file v0.4\n");
fprintf(f, "# Bundle file v0.3\n");
// }
fprintf(f, "%d %d\n", num_images, num_visible_points);
/* Dump cameras */
for (int i = 0; i < num_images; i++) {
#if 0
/* Print the name of the file */
fprintf(f, "%s %d %d\n",
m_image_data[i].m_name,
m_image_data[i].GetWidth(), m_image_data[i].GetHeight());
#endif
int idx = -1;
for (int j = 0; j < num_cameras; j++) {
if (added_order[j] == i) {
idx = j;
break;
}
}
if (idx == -1) {
// if (!output_radial_distortion)
// fprintf(f, "0\n");
// else
fprintf(f, "0 0 0\n");
fprintf(f, "0 0 0\n0 0 0\n0 0 0\n0 0 0\n");
} else {
// if (!output_radial_distortion)
// fprintf(f, "%0.10e\n", cameras[idx].f);
// else
fprintf(f, "%0.10e %0.10e %0.10e\n",
cameras[idx].f, cameras[idx].k[0], cameras[idx].k[1]);
fprintf(f, "%0.10e %0.10e %0.10e\n",
cameras[idx].R[0],
cameras[idx].R[1],
cameras[idx].R[2]);
fprintf(f, "%0.10e %0.10e %0.10e\n",
cameras[idx].R[3],
cameras[idx].R[4],
cameras[idx].R[5]);
fprintf(f, "%0.10e %0.10e %0.10e\n",
cameras[idx].R[6],
cameras[idx].R[7],
cameras[idx].R[8]);
double t[3];
matrix_product(3, 3, 3, 1, cameras[idx].R, cameras[idx].t, t);
matrix_scale(3, 1, t, -1.0, t);
fprintf(f, "%0.10e %0.10e %0.10e\n", t[0], t[1], t[2]);
}
}
/* Dump points */
for (int i = 0; i < num_points; i++) {
int num_visible = (int) pt_views[i].size();
if (num_visible > 0) {
/* Position */
fprintf(f, "%0.10e %0.10e %0.10e\n",
Vx(points[i]), Vy(points[i]), Vz(points[i]));
// Vx(points[idx]), Vy(points[idx]), Vz(points[idx]));
/* Color */
fprintf(f, "%d %d %d\n",
iround(Vx(colors[i])),
iround(Vy(colors[i])),
iround(Vz(colors[i])));
int num_visible = (int) pt_views[i].size();
fprintf(f, "%d", num_visible);
for (int j = 0; j < num_visible; j++) {
int img = added_order[pt_views[i][j].first];
int key = pt_views[i][j].second;
double x = m_image_data[img].m_keys[key].m_x;
double y = m_image_data[img].m_keys[key].m_y;
fprintf(f, " %d %d %0.4f %0.4f", img, key, x, y);
}
fprintf(f, "\n");
}
}
#if 0
/* Finally, dump all outliers */
ImageKeyVector outliers;
for (int i = 0; i < num_images; i++) {
/* Find the index of this camera in the ordering */
int idx = -1;
for (int j = 0; j < num_cameras; j++) {
if (added_order[j] == i) {
idx = j;
break;
}
}
if (idx == -1) continue;
int num_keys = GetNumKeys(i);
for (int j = 0; j < num_keys; j++) {
if (GetKey(i,j).m_extra == -2) {
outliers.push_back(ImageKey(i,j));
}
}
}
int num_outliers = (int) outliers.size();
fprintf(f, "%d\n", num_outliers);
for (int i = 0; i < num_outliers; i++) {
fprintf(f, "%d %d\n", outliers[i].first, outliers[i].second);
}
#endif
fclose(f);
clock_t end = clock();
printf("[DumpOutputFile] Wrote file in %0.3fs\n",
(double) (end - start) / (double) CLOCKS_PER_SEC);
}
