bool BundlerApp::OnInit()
{
printf("[OnInit] Running program %s\n", argv[0]);
char *imageList;
bool load_file = false;
if (argc >= 2) {
printf("Loading images from file '%s'\n", argv[1]);
imageList = argv[1];
load_file = true;
} else {
PrintUsage();
exit(0);
}
printf("[BundlerApp::OnInit] Processing options...\n");
ProcessOptions(argc - 1, argv + 1);
if (m_use_intrinsics && m_estimate_distortion) {
printf("Error: --intrinsics and --estimate_distortion "
"are incompatible\n");
exit(1);
}
if (m_fixed_focal_length && m_estimate_distortion) {
printf("Error: --fixed_focal_length and --estimate_distortion "
"are currently incompatible\n");
exit(1);
}
printf("[BundlerApp::OnInit] Loading frame...\n");
printf("[BundlerApp::OnInit] Loading images...\n");
fflush(stdout);
if (load_file) {
FILE *f = fopen(imageList, "r");
if (f == NULL) {
printf("[BundlerApp::OnInit] Error opening file %s for reading\n",
imageList);
exit(1);
}
LoadImageNamesFromFile(f);
int num_images = GetNumImages();
if (m_fisheye) {
double fCx = 0.0, fCy = 0.0, fRad = 0.0, fAngle = 0.0, fFocal = 0.0;
ReadFisheyeParameters(m_fisheye_params,
fCx, fCy, fRad, fAngle, fFocal);
for (int i = 0; i < num_images; i++) {
if (m_image_data[i].m_fisheye) {
m_image_data[i].m_fCx = fCx;
m_image_data[i].m_fCy = fCy;
m_image_data[i].m_fRad = fRad;
m_image_data[i].m_fAngle = fAngle;
m_image_data[i].m_fFocal = fFocal;
}
}
}
fclose(f);
}
if (m_rerun_bundle) {
// ReadCameraConstraints();
assert(m_bundle_provided);
printf("[BundlerApp::OnInit] Reading bundle file...\n");
ReadBundleFile(m_bundle_file);
if (m_bundle_version < 0.3) {
printf("[BundlerApp::OnInit] Reflecting scene...\n");
FixReflectionBug();
}
ReRunSFM();
exit(0);
}
if (m_use_constraints) {
ReadCameraConstraints();
}
if (m_ignore_file != NULL) {
printf("[BundlerApp::OnInit] Reading ignore file...\n");
ReadIgnoreFile();
}
if (m_ignore_file != NULL) {
printf("[BundlerApp::OnInit] Reading ignore file...\n");
ReadIgnoreFile();
}
/* Do bundle adjustment (or read from file if provided) */
// ParseCommand("UndistortAll", NULL);
if (m_bundle_provided) {
printf("[BundlerApp::OnInit] Reading bundle file...\n");
ReadBundleFile(m_bundle_file);
if (m_bundle_version < 0.3) {
printf("[BundlerApp::OnInit] Reflecting scene...\n");
FixReflectionBug();
}
if (m_compress_list) {
OutputCompressed();
return 0;
}
if (m_reposition_scene) {
double center[3], R[9], scale;
RepositionScene(center, R, scale);
OutputCompressed("reposition");
return 0;
}
if (m_prune_bad_points) {
SetupImagePoints(3);
RemoveBadImages(24);
PruneBadPoints();
OutputCompressed("pruned");
return 0;
}
if (m_scale_focal != 1.0) {
ScaleFocalLengths(m_scale_focal);
return 0;
}
if (m_scale_focal_file != NULL) {
ScaleFocalLengths(m_scale_focal_file);
return 0;
}
if (m_rotate_cameras_file != NULL) {
RotateCameras(m_rotate_cameras_file);
}
if (m_track_file != NULL) {
CreateTracksFromPoints();
WriteTracks(m_track_file);
}
if (m_zero_distortion_params) {
ZeroDistortionParams();
OutputCompressed("nord");
return 0;
}
if (m_output_relposes) {
double center[3], R[9], scale;
RepositionScene(center, R, scale);
RepositionScene(center, R, scale);
// OutputRelativePoses2D(m_output_relposes_file);
OutputRelativePoses3D(m_output_relposes_file);
return 0;
}
if (m_compute_covariance) {
ComputeCameraCovariance();
return 0;
}
#define MIN_POINT_VIEWS 3 // 0 // 2
if (!m_run_bundle) {
SetMatchesFromPoints(MIN_POINT_VIEWS);
// WriteMatchTableDrew(".final");
printf("[BundlerApp::OnInit] "
"Setting up image points and lines...\n");
SetupImagePoints(/*2*/ MIN_POINT_VIEWS);
RemoveBadImages(6);
if (m_point_constraint_file != NULL) {
printf("[BundlerApp::OnInit] Reading point constraints...\n");
m_use_point_constraints = true;
ReadPointConstraints();
}
printf("[BundlerApp::OnInit] Scaling world...\n");
printf("[BundlerApp::OnInit] Computing camera orientations...\n");
ComputeImageRotations();
double center[3], R[9], scale;
RepositionScene(center, R, scale);
if (m_rerun_bundle) {
ReRunSFM();
}
}
if (m_add_image_file != NULL) {
printf("[BundlerApp::OnInit] Adding additional images...\n");
FILE *f = fopen(m_add_image_file, "r");
if (f == NULL) {
printf("[BundlerApp::OnInit] Error opening file %s for "
"reading\n",
m_add_image_file);
} else {
BundleImagesFromFile(f);
/* Write the output */
OutputCompressed("added");
if (m_bundle_version < 0.3)
FixReflectionBug();
// RunSFMWithNewImages(4);
fclose(f);
}
}
}
if (m_run_bundle) {
if (!m_fast_bundle)
BundleAdjust();
else
BundleAdjustFast();
if (m_bundle_version < 0.3)
FixReflectionBug();
exit(0);
}
return true;
}
void BaseApp::ReloadBundleFile(const char *filename)
{
#ifndef __DEMO__
/* Count the old number of cameras */
int num_images = GetNumImages();
int old_num_cameras = 0;
for (int i = 0; i < num_images; i++) {
if (m_image_data[i].m_camera.m_adjusted)
old_num_cameras++;
}
/* Save the previous model */
std::vector<PointData> old_points = m_point_data;
std::vector<ImageData> old_images = m_image_data;
/* Load the new model */
ClearModel();
ReadBundleFile(filename);
if (m_bundle_version < 0.3)
FixReflectionBug();
/* Count the new number of cameras */
int num_cameras = 0;
for (int i = 0; i < num_images; i++) {
if (m_image_data[i].m_camera.m_adjusted)
num_cameras++;
}
int old_num_points = old_points.size();
int new_num_points = m_point_data.size();
std::vector<v3_t> points_old_csp, points_new_csp;
/* Find point correspondences */
for (int i = 0; i < old_num_points; i++) {
for (int j = i - 5; j < i + 5; j++) {
if (j < 0 || j >= new_num_points) continue;
float *old_col = old_points[i].m_color;
float *col = m_point_data[j].m_color;
if (old_col[0] == col[0] &&
old_col[1] == col[1] &&
old_col[2] == col[2]) {
double *old_pos = old_points[i].m_pos;
double *pos = m_point_data[i].m_pos;
points_old_csp.push_back(v3_new(old_pos[0],
old_pos[1],
old_pos[2]));
points_new_csp.push_back(v3_new(pos[0], pos[1], pos[2]));
goto Next;
}
}
Next: ;
}
int num_csp_points = points_old_csp.size();
int num_points = old_num_cameras + num_csp_points;
v3_t *left_points = new v3_t[num_points];
v3_t *right_points = new v3_t[num_points];
int count = 0;
for (int i = 0; i < num_images; i++) {
if (old_images[i].m_camera.m_adjusted) {
double left_pos[3], right_pos[3];
m_image_data[i].m_camera.GetPosition(left_pos);
old_images[i].m_camera.GetPosition(right_pos);
left_points[count] =
v3_new(left_pos[0], left_pos[1], left_pos[2]);
right_points[count] =
v3_new(right_pos[0], right_pos[1], right_pos[2]);
count++;
}
}
for (int i = 0; i < num_csp_points; i++) {
left_points[count] = points_new_csp[i];
right_points[count] = points_old_csp[i];
count++;
}
/* Do the registration */
double T[16];
align_horn_3D(num_points, right_points, left_points, 1, T);
/* Transform the world */
memcpy(m_xform, T, 16 * sizeof(double));
TransformWorldReal();
// TransformWorld();
delete [] left_points;
delete [] right_points;
#endif /* __DEMO__ */
}
