diff options
| -rw-r--r-- | extern/libmv/libmv/simple_pipeline/bundle.cc | 102 |
1 files changed, 100 insertions, 2 deletions
diff --git a/extern/libmv/libmv/simple_pipeline/bundle.cc b/extern/libmv/libmv/simple_pipeline/bundle.cc index 9835248e7d5..09523340ed7 100644 --- a/extern/libmv/libmv/simple_pipeline/bundle.cc +++ b/extern/libmv/libmv/simple_pipeline/bundle.cc @@ -321,6 +321,78 @@ void EuclideanBundlerPerformEvaluation(const Tracks &tracks, } } +// This is an utility function to only bundle 3D position of +// given markers list. +// +// Main purpose of this function is to adjust positions of tracks +// which does have constant zero weight and so far only were using +// algebraic intersection to obtain their 3D positions. +// +// At this point we only need to bundle points positions, cameras +// are to be totally still here. +void EuclideanBundlePointsOnly(const vector<Marker> &markers, + vector<Vec6> &all_cameras_R_t, + double ceres_intrinsics[8], + EuclideanReconstruction *reconstruction) { + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); + int num_residuals = 0; + for (int i = 0; i < markers.size(); ++i) { + const Marker &marker = markers[i]; + EuclideanCamera *camera = reconstruction->CameraForImage(marker.image); + EuclideanPoint *point = reconstruction->PointForTrack(marker.track); + if (camera == NULL || point == NULL) { + continue; + } + + // Rotation of camera denoted in angle axis followed with + // camera translaiton. + double *current_camera_R_t = &all_cameras_R_t[camera->image](0); + + problem.AddResidualBlock(new ceres::AutoDiffCostFunction< + OpenCVReprojectionError, 2, 8, 6, 3>( + new OpenCVReprojectionError( + marker.x, + marker.y, + 1.0)), + NULL, + ceres_intrinsics, + current_camera_R_t, + &point->X(0)); + + problem.SetParameterBlockConstant(current_camera_R_t); + num_residuals++; + } + + LG << "Number of residuals: " << num_residuals; + if (!num_residuals) { + LG << "Skipping running minimizer with zero residuals"; + return; + } + + problem.SetParameterBlockConstant(ceres_intrinsics); + + // Configure the solver. + ceres::Solver::Options options; + options.use_nonmonotonic_steps = true; + options.preconditioner_type = ceres::SCHUR_JACOBI; + options.linear_solver_type = ceres::ITERATIVE_SCHUR; + options.use_inner_iterations = true; + options.max_num_iterations = 100; + +#ifdef _OPENMP + options.num_threads = omp_get_max_threads(); + options.num_linear_solver_threads = omp_get_max_threads(); +#endif + + // Solve! + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + LG << "Final report:\n" << summary.FullReport(); + +} + } // namespace void EuclideanBundle(const Tracks &tracks, @@ -343,8 +415,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, LG << "Original intrinsics: " << *intrinsics; vector<Marker> markers = tracks.AllMarkers(); - ceres::Problem::Options problem_options; - ceres::Problem problem(problem_options); + // N-th element denotes whether track N is a constant zero-weigthed track. + vector<bool> zero_weight_tracks_flags(tracks.MaxTrack(), true); // Residual blocks with 10 parameters are unwieldly with Ceres, so pack the // intrinsics into a single block and rely on local parameterizations to @@ -375,6 +447,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, } // Add residual blocks to the problem. + ceres::Problem::Options problem_options; + ceres::Problem problem(problem_options); int num_residuals = 0; bool have_locked_camera = false; for (int i = 0; i < markers.size(); ++i) { @@ -414,6 +488,8 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, problem.SetParameterization(current_camera_R_t, constant_translation_parameterization); } + + zero_weight_tracks_flags[marker.track] = false; } num_residuals++; @@ -492,6 +568,28 @@ void EuclideanBundleCommonIntrinsics(const Tracks &tracks, EuclideanBundlerPerformEvaluation(tracks, reconstruction, &all_cameras_R_t, &problem, evaluation); } + + // Separate step to adjust positions of tracks which are + // constant zero-weighted. + vector<Marker> zero_weight_markers; + for (int track = 0; track < tracks.MaxTrack(); ++track) { + if (zero_weight_tracks_flags[track]) { + vector<Marker> current_markers = tracks.MarkersForTrack(track); + zero_weight_markers.reserve(zero_weight_markers.size() + + current_markers.size()); + for (int i = 0; i < current_markers.size(); ++i) { + zero_weight_markers.push_back(current_markers[i]); + } + } + } + + if (zero_weight_markers.size()) { + LG << "Refining position of constant zero-weighted tracks"; + EuclideanBundlePointsOnly(zero_weight_markers, + all_cameras_R_t, + ceres_intrinsics, + reconstruction); + } } void ProjectiveBundle(const Tracks & /*tracks*/, |