/* Read in information about the world */
void BaseApp::ReadBundleFile(const char *filename)
{
printf("[ReadBundleFile] Reading file...\n");
FILE *f = fopen(filename, "r");
if (f == NULL) {
printf("Error opening file %s for reading\n", filename);
return;
}
int num_images, num_points;
char first_line[256];
fgets(first_line, 256, f);
if (first_line[0] == '#') {
double version;
sscanf(first_line, "# Bundle file v%lf", &version);
m_bundle_version = version;
printf("[ReadBundleFile] Bundle version: %0.3f\n", version);
fscanf(f, "%d %d\n", &num_images, &num_points);
} else if (first_line[0] == 'v') {
double version;
sscanf(first_line, "v%lf", &version);
m_bundle_version = version;
printf("[ReadBundleFile] Bundle version: %0.3f\n", version);
fscanf(f, "%d %d\n", &num_images, &num_points);
} else {
m_bundle_version = 0.1;
sscanf(first_line, "%d %d\n", &num_images, &num_points);
}
printf("[ReadBundleFile] Reading %d images and %d points...\n",
num_images, num_points);
if (num_images != GetNumImages()) {
printf("Error: number of images doesn't match file!\n");
return;
}
/* Read cameras */
for (int i = 0; i < num_images; i++) {
double focal_length;
double R[9];
double t[3];
double k[2] = { 0.0, 0.0 };
if (m_bundle_version >= 0.4) {
char name[512];
int w, h;
fscanf(f, "%s %d %d\n", name, &w, &h);
}
/* Focal length */
if (m_bundle_version > 0.1) {
fscanf(f, "%lf %lf %lf\n", &focal_length, k+0, k+1);
} else {
fscanf(f, "%lf\n", &focal_length);
}
/* Rotation */
fscanf(f, "%lf %lf %lf\n%lf %lf %lf\n%lf %lf %lf\n",
R+0, R+1, R+2, R+3, R+4, R+5, R+6, R+7, R+8);
/* Translation */
fscanf(f, "%lf %lf %lf\n", t+0, t+1, t+2);
#if 0
if (m_bundle_version < 0.3) {
R[2] = -R[2];
R[5] = -R[5];
R[6] = -R[6];
R[7] = -R[7];
t[2] = -t[2];
}
#endif
if (focal_length <= 100.0 || m_image_data[i].m_ignore_in_bundle) {
/* No (or bad) information about this camera */
m_image_data[i].m_camera.m_adjusted = false;
} else {
CameraInfo cd;
cd.m_adjusted = true;
cd.m_width = m_image_data[i].GetWidth();
cd.m_height = m_image_data[i].GetHeight();
cd.m_focal = focal_length;
cd.m_k[0] = k[0];
cd.m_k[1] = k[1];
memcpy(cd.m_R, R, sizeof(double) * 9);
memcpy(cd.m_t, t, sizeof(double) * 3);
cd.Finalize();
m_image_data[i].m_camera = cd;
}
}
/* Read points */
m_point_data.clear();
m_point_data.resize(num_points);
int num_min_views_points = 0;
for (int i = 0; i < num_points; i++) {
PointData &pt = m_point_data[i];
/* Position */
fscanf(f, "%lf %lf %lf\n",
pt.m_pos + 0, pt.m_pos + 1, pt.m_pos + 2);
// if (m_bundle_version < 0.3)
// pt.m_pos[2] = -pt.m_pos[2];
/* Color */
fscanf(f, "%f %f %f\n",
pt.m_color + 0, pt.m_color + 1, pt.m_color + 2);
int num_visible;
fscanf(f, "%d", &num_visible);
pt.m_num_vis=num_visible;
if (num_visible >=3)
num_min_views_points++;
// pt.m_views.resize(num_visible);
for (int j = 0; j < num_visible; j++) {
int view, key;
fscanf(f, "%d %d", &view, &key);
if (!m_image_data[view].m_camera.m_adjusted) {
// printf("[ReadBundleFile] "
// "Removing view %d from point %d\n", view, i);
} else {
/* Check cheirality */
bool val = (m_bundle_version >= 0.3);
double proj_test[2];
if (m_image_data[view].m_camera.
Project(pt.m_pos, proj_test) == val) {
pt.m_views.push_back(ImageKey(view, key));
} else {
printf("[ReadBundleFile] "
"Removing view %d from point %d [cheirality]\n",
view, i);
// pt.m_views.push_back(ImageKey(view, key));
}
}
// pt.m_views.push_back(ImageKey(view, key));
if (m_bundle_version >= 0.3) {
double x, y;
fscanf(f, "%lf %lf", &x, &y);
}
}
// #define CROP_POINT_CLOUD
#ifdef CROP_POINT_CLOUD
const double x_min = 1.327;
const double x_max = 3.556;
const double y_min = -1.414;
const double y_max = 1.074;
const double z_min = -5.502;
const double z_max = -3.288;
if (pt.m_pos[0] < x_min || pt.m_pos[0] > x_max ||
pt.m_pos[1] < y_min || pt.m_pos[1] > y_max ||
pt.m_pos[2] < z_min || pt.m_pos[2] > z_max) {
pt.m_views.clear();
}
#endif /* CROP_POINT_CLOUD */
}
#if 0
/* Read outliers */
int num_outliers;
fscanf(f, "%d", &num_outliers);
for (int i = 0; i < num_outliers; i++) {
ImageKey ik;
fscanf(f, "%d %d", &(ik.first), &(ik.second));
m_outliers.push_back(ik);
}
#endif
fclose(f);
printf("[ReadBundleFile] %d / %d points visible to more than 2 cameras!\n",
num_min_views_points, num_points);
}
/*
Read in information about the "world" from the specified file.
*/
void BaseApp::ReadBundleFile(char *filename)
{
#ifdef _DEBUG_
printf("[BaseApp::ReadBundleFile] Reading file...\n");
#endif
//--[1] See if file will open ...
FILE *f = fopen(filename, "r");
if (f == NULL)
{
printf("Error opening file %s for reading\n", filename);
return;
}
//--[2] If so, then process the header. Get #images and #points.
int num_images, num_points;
char first_line[256];
fgets(first_line, 256, f);
if (first_line[0] == '#')
{
double version;
sscanf(first_line, "# Bundle file v%lf", &version);
m_bundle_version = version;
fscanf(f, "%d %d\n", &num_images, &num_points);
#ifdef _DEBUG_
printf("[ReadBundleFile] Bundle version: %0.3f\n", version);
#endif
}
else if (first_line[0] == 'v')
{
double version;
sscanf(first_line, "v%lf", &version);
m_bundle_version = version;
fscanf(f, "%d %d\n", &num_images, &num_points);
#ifdef _DEBUG_
printf("[ReadBundleFile] Bundle version: %0.3f\n", version);
#endif
}
else
{
m_bundle_version = 0.1;
sscanf(first_line, "%d %d\n", &num_images, &num_points);
}
#ifdef _DEBUG_
printf("[BaseApp::ReadBundleFile] Reading %d images and %d points...\n",
num_images, num_points);
#endif
if (num_images != GetNumImages()) // Compare to known info (sanity check).
{
printf("Error: number of images doesn't match file!\n");
return;
}
//--[3] Read in camera information. (One camera per image, yeah?)
// Camera has focal length, two nonlinear parms, and extrinsic parms.
for (int i = 0; i < num_images; i++)
{
double focal_length;
double R[9];
double t[3];
double k[2] = { 0.0, 0.0 };
if (m_bundle_version >= 0.4)
{
char name[512];
int w, h;
fscanf(f, "%s %d %d\n", name, &w, &h);
}
//--[3.1] Focal length
if (m_bundle_version > 0.1)
fscanf(f, "%lf %lf %lf\n", &focal_length, k+0, k+1);
else
fscanf(f, "%lf\n", &focal_length);
//--[3.2] Rotation
fscanf(f, "%lf %lf %lf\n%lf %lf %lf\n%lf %lf %lf\n",
R+0, R+1, R+2, R+3, R+4, R+5, R+6, R+7, R+8);
//--[3.3] Translation
fscanf(f, "%lf %lf %lf\n", t+0, t+1, t+2);
//--[3.4] If data is "sane" then aggregate data into camera info object.
if (focal_length <= 100.0 || m_image_data[i].m_ignore_in_bundle)
{
/* No (or bad) information about this camera */
m_image_data[i].m_camera.m_adjusted = false;
}
else
{
CameraInfo cd;
cd.m_adjusted = true;
cd.m_width = m_image_data[i].GetWidth();
cd.m_height = m_image_data[i].GetHeight();
cd.m_focal = focal_length;
cd.m_k[0] = k[0];
cd.m_k[1] = k[1];
memcpy(cd.m_R, R, sizeof(double) * 9);
memcpy(cd.m_t, t, sizeof(double) * 3);
cd.Finalize();
m_image_data[i].m_camera = cd;
}
}
//--[4] Read in the points of the bundle/world point cloud.
m_point_data.clear();
m_point_data.resize(num_points);
int num_min_views_points = 0;
for (int i = 0; i < num_points; i++)
{
PointData &pt = m_point_data[i];
//--[4.1] Position
fscanf(f, "%lf %lf %lf\n", pt.m_pos + 0, pt.m_pos + 1, pt.m_pos + 2);
//--[4.2] Color
fscanf(f, "%f %f %f\n", pt.m_color + 0, pt.m_color + 1, pt.m_color + 2);
//--[4.3] Visibility info (frame, keypoint index, coord location).
int num_visible;
fscanf(f, "%d", &num_visible); // How many images have this point?
pt.m_num_vis=num_visible;
if (num_visible >=3) // If more than three, add as good point.
num_min_views_points++;
for (int j = 0; j < num_visible; j++)
{
int view, key;
fscanf(f, "%d %d", &view, &key);
if (m_image_data[view].m_camera.m_adjusted)
{
/* Check chirality */
bool val = (m_bundle_version >= 0.3);
double proj_test[2];
if (m_image_data[view].m_camera.Project(pt.m_pos, proj_test) == val)
{
pt.m_views.push_back(ImageKey(view, key));
}
else
{
printf("[BaseApp::ReadBundleFile] "
"Excluding view %d from point %d [chirality]\n", view, i);
}
}
if (m_bundle_version >= 0.3)
{
double x, y;
fscanf(f, "%lf %lf", &x, &y);
}
}
// #define CROP_POINT_CLOUD
#ifdef CROP_POINT_CLOUD
const double x_min = 1.327;
const double x_max = 3.556;
const double y_min = -1.414;
const double y_max = 1.074;
const double z_min = -5.502;
const double z_max = -3.288;
if (pt.m_pos[0] < x_min || pt.m_pos[0] > x_max ||
pt.m_pos[1] < y_min || pt.m_pos[1] > y_max ||
pt.m_pos[2] < z_min || pt.m_pos[2] > z_max)
pt.m_views.clear();
#endif /* CROP_POINT_CLOUD */
}
fclose(f);
#ifdef _DEBUG_
printf("[BaseApp::ReadBundleFile] %d / %d points visible to over 2 cameras!\n",
num_min_views_points, num_points);
#endif
}
