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()); }