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(" ]");
}
/* 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;
}
/* 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);
}
