int main(int argc, char *argv[])
{
midi_file_t *file;
unsigned int i;
if (argc < 2)
{
printf("Usage: %s <filename>\n", argv[0]);
exit(1);
}
file = MIDI_LoadFile(argv[1]);
if (file == NULL)
{
fprintf(stderr, "Failed to open %s\n", argv[1]);
exit(1);
}
for (i=0; i<file->num_tracks; ++i)
{
printf("\n== Track %i ==\n\n", i);
PrintTrack(&file->tracks[i]);
}
return 0;
}
/* 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;
}
void PrintTracks()
{
for (int i=0;i<esdEv->GetNumberOfTracks();i++) PrintTrack(i);
printf("NCosm: %d\n",esdEv->GetNumberOfCosmicTracks());
}
