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; }
/* Quickly compute pose of all cameras */ void BundlerApp::BundleAdjustFast() { clock_t start = clock(); /* Compute initial image information */ ComputeGeometricConstraints();//读取constrains.txt里面的信息,构造track信息 #if 0 /* Read keypoints */ printf("[BundleAdjust] Reading key colors...\n"); ReadKeyColors(); #endif /* Set track pointers to -1 */ for (int i = 0; i < (int) m_track_data.size(); i++) { m_track_data[i].m_extra = -1; } /* For now, assume all images form one connected component */ int num_images = GetNumImages(); int *added_order = new int[num_images]; int *added_order_inv = new int[num_images]; /* **** Run bundle adjustment! **** */ camera_params_t *cameras = new camera_params_t[num_images]; int max_pts = (int) m_track_data.size(); // 1243742; /* HACK! */ v3_t *points = new v3_t[max_pts]; v3_t *colors = new v3_t[max_pts]; std::vector<ImageKeyVector> pt_views; /* Initialize the bundle adjustment */ int num_init_cams = 0; InitializeBundleAdjust(num_init_cams, added_order, added_order_inv, cameras, points, colors, pt_views, m_use_constraints); int i_best = -1, j_best = -1, max_matches = 0; double max_score = 0.0; int curr_num_cameras, curr_num_pts; int pt_count; if (num_init_cams == 0) { BundlePickInitialPair(i_best, j_best, true); added_order[0] = i_best; added_order[1] = j_best; printf("[BundleAdjust] Adjusting cameras " "%d and %d (score = %0.3f)\n", i_best, j_best, max_score); /* **** Set up the initial cameras **** */ double init_focal_length_0 = 0.0, init_focal_length_1 = 0.0; pt_count = curr_num_pts = SetupInitialCameraPair(i_best, j_best, init_focal_length_0, init_focal_length_1, cameras, points, colors, pt_views); DumpOutputFile(m_output_directory, "bundle.init.out", num_images, 2, curr_num_pts, added_order, cameras, points, colors, pt_views); /* Run sfm for the first time */ double error0; error0 = RunSFM(curr_num_pts, 2, 0, false, cameras, points, added_order, colors, pt_views); printf(" focal lengths: %0.3f, %0.3f\n", cameras[0].f, cameras[1].f); #if 1 if (m_fix_necker) { /* Swap the cameras and flip the depths to deal with Necker * reversal */ camera_params_t cameras_old[2]; v3_t *points_old; points_old = new v3_t [curr_num_pts]; memcpy(points_old, points, sizeof(v3_t) * curr_num_pts); memcpy(cameras_old, cameras, sizeof(camera_params_t) * 2); camera_params_t tmp = cameras[0]; memcpy(cameras[0].R, cameras[1].R, sizeof(double) * 9); memcpy(cameras[0].t, cameras[1].t, sizeof(double) * 3); cameras[0].f = init_focal_length_0; cameras[0].k[0] = cameras[0].k[1] = 0.0; memcpy(cameras[1].R, tmp.R, sizeof(double) * 9); memcpy(cameras[1].t, tmp.t, sizeof(double) * 3); cameras[1].f = init_focal_length_1; cameras[1].k[0] = cameras[1].k[1] = 0.0; double K1inv[9] = { 1.0 / cameras[0].f, 0.0, 0.0, 0.0, 1.0 / cameras[0].f, 0.0, 0.0, 0.0, 1.0 }; double K2inv[9] = { 1.0 / cameras[1].f, 0.0, 0.0, 0.0, 1.0 / cameras[1].f, 0.0, 0.0, 0.0, 1.0 }; for (int i = 0; i < curr_num_pts; i++) { int k1 = pt_views[i][0].second; int k2 = pt_views[i][1].second; double proj1[3] = { GetKey(added_order[0],k1).m_x, GetKey(added_order[0],k1).m_y, -1.0 }; double proj2[3] = { GetKey(added_order[1],k2).m_x, GetKey(added_order[1],k2).m_y, -1.0 }; double proj1_norm[3], proj2_norm[3]; matrix_product(3, 3, 3, 1, K1inv, proj1, proj1_norm); matrix_product(3, 3, 3, 1, K2inv, proj2, proj2_norm); v2_t p = v2_new(proj1_norm[0] / proj1_norm[2], proj1_norm[1] / proj1_norm[2]); v2_t q = v2_new(proj2_norm[0] / proj2_norm[2], proj2_norm[1] / proj2_norm[2]); double proj_error; double t1[3]; double t2[3]; /* Put the translation in standard form */ matrix_product(3, 3, 3, 1, cameras[0].R, cameras[0].t, t1); matrix_scale(3, 1, t1, -1.0, t1); matrix_product(3, 3, 3, 1, cameras[1].R, cameras[1].t, t2); matrix_scale(3, 1, t2, -1.0, t2); points[i] = triangulate(p, q, cameras[0].R, t1, cameras[1].R, t2, &proj_error); } double error1; error1 = RunSFM(curr_num_pts, 2, 0, false, cameras, points, added_order, colors, pt_views); } #endif DumpPointsToPly(m_output_directory, "points001.ply", curr_num_pts, 2, points, colors, cameras); if (m_bundle_output_base != NULL) { char buf[256]; sprintf(buf, "%s%03d.out", m_bundle_output_base, 1); DumpOutputFile(m_output_directory, buf, num_images, 2, curr_num_pts, added_order, cameras, points, colors, pt_views); } curr_num_cameras = 2; } else { curr_num_cameras = num_init_cams; pt_count = curr_num_pts = (int) m_point_data.size(); } int round = 0; while (curr_num_cameras < num_images) { int parent_idx; int max_cam = FindCameraWithMostMatches(curr_num_cameras, curr_num_pts, added_order, parent_idx, max_matches, pt_views); printf("[BundleAdjust] max_matches = %d\n", max_matches); if (max_matches < m_min_max_matches) break; /* No more connections */ /* Find all images with 75% of the matches of the maximum * (unless overruled by m_num_points_add_camera) */ std::vector<ImagePair> image_set; if (false && max_matches < 48) { /* disabling this */ image_set.push_back(ImagePair(max_cam, parent_idx)); } else { int nMatches = iround(0.75 * max_matches); if (m_num_matches_add_camera > 0) { /* Alternate threshold based on user parameter */ nMatches = std::min(nMatches, m_num_matches_add_camera); } image_set = FindCamerasWithNMatches(nMatches, curr_num_cameras, curr_num_pts, added_order, pt_views); } int num_added_images = (int) image_set.size(); printf("[BundleAdjustFast] Registering %d images\n", num_added_images); for (int i = 0; i < num_added_images; i++) printf("[BundleAdjustFast] Adjusting camera %d\n", image_set[i].first); /* Now, throw the new cameras into the mix */ int image_count = 0; for (int i = 0; i < num_added_images; i++) { int next_idx = image_set[i].first; int parent_idx = image_set[i].second; added_order[curr_num_cameras + image_count] = next_idx; printf("[BundleAdjust[%d]] Adjusting camera %d " "(parent = %d)\n", round, next_idx, (parent_idx == -1 ? -1 : added_order[parent_idx])); /* **** Set up the new camera **** */ bool success = false; camera_params_t camera_new = BundleInitializeImage(m_image_data[next_idx], next_idx, curr_num_cameras + image_count, curr_num_cameras, curr_num_pts, added_order, points, NULL, cameras, pt_views, &success); if (success) { cameras[curr_num_cameras+image_count] = camera_new; image_count++; } else { printf("[BundleAdjust] Couldn't initialize image %d\n", next_idx); m_image_data[next_idx].m_ignore_in_bundle = true; } } /* Compute the distance between the first pair of cameras */ double dist0 = 0.0; printf("[BundleAdjust] Adding new matches\n"); pt_count = curr_num_pts; curr_num_cameras += image_count; if (!m_skip_add_points) { pt_count = BundleAdjustAddAllNewPoints(pt_count, curr_num_cameras, added_order, cameras, points, colors, dist0, pt_views); } curr_num_pts = pt_count; printf("[BundleAdjust] Number of points = %d\n", pt_count); fflush(stdout); if (!m_skip_full_bundle) { /* Run sfm again to update parameters */ RunSFM(curr_num_pts, curr_num_cameras, 0, false, cameras, points, added_order, colors, pt_views); /* Remove bad points and cameras */ RemoveBadPointsAndCameras(curr_num_pts, curr_num_cameras + 1, added_order, cameras, points, colors, pt_views); printf(" focal lengths:\n"); for (int i = 0; i < curr_num_cameras; i++) { if(m_image_data[added_order[i]].m_has_init_focal) { printf(" [%03d] %0.3f (%0.3f) %s %d; %0.3e %0.3e\n", i, cameras[i].f, m_image_data[added_order[i]].m_init_focal, m_image_data[added_order[i]].m_name, added_order[i], cameras[i].k[0], cameras[i].k[1]); } else { printf(" [%03d] %0.3f %s %d; %0.3e %0.3e\n", i, cameras[i].f, m_image_data[added_order[i]].m_name, added_order[i], cameras[i].k[0], cameras[i].k[1]); } // printf(" [%03d] %0.3f\n", i, cameras[i].f); } fflush(stdout); } /* Dump output for this round */ char buf[256]; sprintf(buf, "points%03d.ply", curr_num_cameras); DumpPointsToPly(m_output_directory, buf, curr_num_pts, curr_num_cameras, points, colors, cameras); if (m_bundle_output_base != NULL) { sprintf(buf, "%s%03d.out", m_bundle_output_base, curr_num_cameras); DumpOutputFile(m_output_directory, buf, num_images, curr_num_cameras, curr_num_pts, added_order, cameras, points, colors, pt_views); } round++; }//while clock_t end = clock(); printf("[BundleAdjust] Bundle adjustment took %0.3fs\n", (end - start) / ((double) CLOCKS_PER_SEC)); if (m_estimate_ignored) { EstimateIgnoredCameras(curr_num_cameras, cameras, added_order, curr_num_pts, points, colors, pt_views); } /* Dump output */ if (m_bundle_output_file != NULL) { DumpOutputFile(m_output_directory, m_bundle_output_file, num_images, curr_num_cameras, curr_num_pts, added_order, cameras, points, colors, pt_views); } /* Save the camera parameters and points */ /* Cameras */ for (int i = 0; i < num_images; i++) { m_image_data[i].m_camera.m_adjusted = false; } for (int i = 0; i < curr_num_cameras; i++) { int img = added_order[i]; m_image_data[img].m_camera.m_adjusted = true; memcpy(m_image_data[img].m_camera.m_R, cameras[i].R, 9 * sizeof(double)); matrix_product(3, 3, 3, 1, cameras[i].R, cameras[i].t, m_image_data[img].m_camera.m_t); matrix_scale(3, 1, m_image_data[img].m_camera.m_t, -1.0, m_image_data[img].m_camera.m_t); m_image_data[img].m_camera.m_focal = cameras[i].f; m_image_data[img].m_camera.Finalize(); } /* Points */ for (int i = 0; i < curr_num_pts; i++) { /* Check if the point is visible in any view */ if ((int) pt_views[i].size() == 0) continue; /* Invisible */ PointData pdata; pdata.m_pos[0] = Vx(points[i]); pdata.m_pos[1] = Vy(points[i]); pdata.m_pos[2] = Vz(points[i]); pdata.m_color[0] = (float) Vx(colors[i]); pdata.m_color[1] = (float) Vy(colors[i]); pdata.m_color[2] = (float) Vz(colors[i]); #if 1 for (int j = 0; j < (int) pt_views[i].size(); j++) { int v = pt_views[i][j].first; int vnew = added_order[v]; pdata.m_views.push_back(ImageKey(vnew, pt_views[i][j].second)); } #else pdata.m_views = pt_views[i]; #endif m_point_data.push_back(pdata); } delete [] added_order; delete [] added_order_inv; SetMatchesFromPoints(); bool *image_mask = new bool[num_images]; for (int i = 0; i < num_images; i++) { if (m_image_data[i].m_camera.m_adjusted) image_mask[i] = true; else image_mask[i] = false; } }