diff options
Diffstat (limited to 'extern')
| -rw-r--r-- | extern/libmv/CMakeLists.txt | 2 | ||||
| -rw-r--r-- | extern/libmv/files.txt | 2 | ||||
| -rw-r--r-- | extern/libmv/libmv-capi.cc | 103 | ||||
| -rw-r--r-- | extern/libmv/libmv-capi.h | 4 | ||||
| -rw-r--r-- | extern/libmv/libmv/simple_pipeline/keyframe_selection.cc | 579 | ||||
| -rw-r--r-- | extern/libmv/libmv/simple_pipeline/keyframe_selection.h | 52 |
6 files changed, 736 insertions, 6 deletions
diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt index a18011fc599..4c88a4b7cc2 100644 --- a/extern/libmv/CMakeLists.txt +++ b/extern/libmv/CMakeLists.txt @@ -67,6 +67,7 @@ if(WITH_LIBMV) libmv/simple_pipeline/detect.cc libmv/simple_pipeline/initialize_reconstruction.cc libmv/simple_pipeline/intersect.cc + libmv/simple_pipeline/keyframe_selection.cc libmv/simple_pipeline/modal_solver.cc libmv/simple_pipeline/pipeline.cc libmv/simple_pipeline/reconstruction.cc @@ -125,6 +126,7 @@ if(WITH_LIBMV) libmv/simple_pipeline/detect.h libmv/simple_pipeline/initialize_reconstruction.h libmv/simple_pipeline/intersect.h + libmv/simple_pipeline/keyframe_selection.h libmv/simple_pipeline/modal_solver.h libmv/simple_pipeline/pipeline.h libmv/simple_pipeline/reconstruction.h diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt index 426b6db41fc..bfa22bfe898 100644 --- a/extern/libmv/files.txt +++ b/extern/libmv/files.txt @@ -46,6 +46,8 @@ libmv/simple_pipeline/initialize_reconstruction.cc libmv/simple_pipeline/initialize_reconstruction.h libmv/simple_pipeline/intersect.cc libmv/simple_pipeline/intersect.h +libmv/simple_pipeline/keyframe_selection.cc +libmv/simple_pipeline/keyframe_selection.h libmv/simple_pipeline/modal_solver.cc libmv/simple_pipeline/modal_solver.h libmv/simple_pipeline/pipeline.cc diff --git a/extern/libmv/libmv-capi.cc b/extern/libmv/libmv-capi.cc index 0e25650db79..563919e1d7b 100644 --- a/extern/libmv/libmv-capi.cc +++ b/extern/libmv/libmv-capi.cc @@ -56,6 +56,7 @@ #include "libmv/simple_pipeline/camera_intrinsics.h" #include "libmv/simple_pipeline/modal_solver.h" #include "libmv/simple_pipeline/reconstruction_scale.h" +#include "libmv/simple_pipeline/keyframe_selection.h" #ifdef _MSC_VER # define snprintf _snprintf @@ -497,9 +498,82 @@ static void finishReconstruction(const libmv::Tracks &tracks, const libmv::Camer libmv_reconstruction->error = libmv::EuclideanReprojectionError(tracks, reconstruction, camera_intrinsics); } +static bool selectTwoKeyframesBasedOnGRICAndVariance( + libmv::Tracks &tracks, + libmv::Tracks &normalized_tracks, + libmv::CameraIntrinsics &camera_intrinsics, + libmv::ReconstructionOptions &reconstruction_options, + int &keyframe1, + int &keyframe2) +{ + libmv::vector<int> keyframes; + + /* Get list of all keyframe candidates first. */ + SelectkeyframesBasedOnGRICAndVariance(normalized_tracks, + camera_intrinsics, + keyframes); + + if (keyframes.size() < 2) { + LG << "Not enough keyframes detected by GRIC"; + return false; + } + else if (keyframes.size() == 2) { + keyframe1 = keyframes[0]; + keyframe2 = keyframes[1]; + return true; + } + + /* Now choose two keyframes with minimal reprojection error after initial + * reconstruction choose keyframes with the least reprojection error after + * solving from two candidate keyframes. + * + * In fact, currently libmv returns single pair only, so this code will + * not actually run. But in the future this could change, so let's stay + * prepared. + */ + int previous_keyframe = keyframes[0]; + double best_error = std::numeric_limits<double>::max(); + for (int i = 1; i < keyframes.size(); i++) { + libmv::EuclideanReconstruction reconstruction; + int current_keyframe = keyframes[i]; + + libmv::vector<libmv::Marker> keyframe_markers = + normalized_tracks.MarkersForTracksInBothImages(previous_keyframe, + current_keyframe); + + libmv::Tracks keyframe_tracks(keyframe_markers); + + /* get a solution from two keyframes only */ + libmv::EuclideanReconstructTwoFrames(keyframe_markers, &reconstruction); + libmv::EuclideanBundle(keyframe_tracks, &reconstruction); + libmv::EuclideanCompleteReconstruction(reconstruction_options, + keyframe_tracks, + &reconstruction, NULL); + + double current_error = + libmv::EuclideanReprojectionError(tracks, + reconstruction, + camera_intrinsics); + + LG << "Error between " << previous_keyframe + << " and " << current_keyframe + << ": " << current_error; + + if (current_error < best_error) { + best_error = current_error; + keyframe1 = previous_keyframe; + keyframe2 = current_keyframe; + } + + previous_keyframe = current_keyframe; + } + + return true; +} + libmv_Reconstruction *libmv_solveReconstruction(const libmv_Tracks *libmv_tracks, const libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options, - const libmv_reconstructionOptions *libmv_reconstruction_options, + libmv_reconstructionOptions *libmv_reconstruction_options, reconstruct_progress_update_cb progress_update_callback, void *callback_customdata) { @@ -512,19 +586,38 @@ libmv_Reconstruction *libmv_solveReconstruction(const libmv_Tracks *libmv_tracks ReconstructUpdateCallback update_callback = ReconstructUpdateCallback(progress_update_callback, callback_customdata); + /* Retrieve reconstruction options from C-API to libmv API */ cameraIntrinsicsFromOptions(libmv_camera_intrinsics_options, &camera_intrinsics); - /* Invert the camera intrinsics */ - libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics); - - /* actual reconstruction */ libmv::ReconstructionOptions reconstruction_options; reconstruction_options.success_threshold = libmv_reconstruction_options->success_threshold; reconstruction_options.use_fallback_reconstruction = libmv_reconstruction_options->use_fallback_reconstruction; + /* Invert the camera intrinsics */ + libmv::Tracks normalized_tracks = getNormalizedTracks(tracks, camera_intrinsics); + + /* keyframe selection */ int keyframe1 = libmv_reconstruction_options->keyframe1, keyframe2 = libmv_reconstruction_options->keyframe2; + if (libmv_reconstruction_options->select_keyframes) { + LG << "Using automatic keyframe selection"; + + update_callback.invoke(0, "Selecting keyframes"); + + selectTwoKeyframesBasedOnGRICAndVariance(tracks, + normalized_tracks, + camera_intrinsics, + reconstruction_options, + keyframe1, + keyframe2); + + /* so keyframes in the interface would be updated */ + libmv_reconstruction_options->keyframe1 = keyframe1; + libmv_reconstruction_options->keyframe2 = keyframe2; + } + + /* actual reconstruction */ LG << "frames to init from: " << keyframe1 << " " << keyframe2; libmv::vector<libmv::Marker> keyframe_markers = diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h index 5fab666e5a8..beac3e85468 100644 --- a/extern/libmv/libmv-capi.h +++ b/extern/libmv/libmv-capi.h @@ -92,7 +92,9 @@ typedef struct libmv_cameraIntrinsicsOptions { } libmv_cameraIntrinsicsOptions; typedef struct libmv_reconstructionOptions { + int select_keyframes; int keyframe1, keyframe2; + int refine_intrinsics; double success_threshold; @@ -103,7 +105,7 @@ typedef void (*reconstruct_progress_update_cb) (void *customdata, double progres struct libmv_Reconstruction *libmv_solveReconstruction(const struct libmv_Tracks *libmv_tracks, const libmv_cameraIntrinsicsOptions *libmv_camera_intrinsics_options, - const libmv_reconstructionOptions *libmv_reconstruction_options, + libmv_reconstructionOptions *libmv_reconstruction_options, reconstruct_progress_update_cb progress_update_callback, void *callback_customdata); struct libmv_Reconstruction *libmv_solveModal(const struct libmv_Tracks *libmv_tracks, diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc new file mode 100644 index 00000000000..256b056015a --- /dev/null +++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.cc @@ -0,0 +1,579 @@ +// Copyright (c) 2012 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/simple_pipeline/keyframe_selection.h" + +#include "libmv/numeric/numeric.h" +#include "ceres/ceres.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/homography.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/simple_pipeline/intersect.h" +#include "libmv/simple_pipeline/bundle.h" + +#include <Eigen/Eigenvalues> + +namespace libmv { +namespace { + +Vec2 NorrmalizedToPixelSpace(Vec2 vec, const CameraIntrinsics &intrinsics) { + Vec2 result; + + double focal_length_x = intrinsics.focal_length_x(); + double focal_length_y = intrinsics.focal_length_y(); + + double principal_point_x = intrinsics.principal_point_x(); + double principal_point_y = intrinsics.principal_point_y(); + + result(0) = vec(0) * focal_length_x + principal_point_x; + result(1) = vec(1) * focal_length_y + principal_point_y; + + return result; +} + +Mat3 IntrinsicsNormalizationMatrix(const CameraIntrinsics &intrinsics) { + Mat3 T = Mat3::Identity(), S = Mat3::Identity(); + + T(0, 2) = -intrinsics.principal_point_x(); + T(1, 2) = -intrinsics.principal_point_y(); + + S(0, 0) /= intrinsics.focal_length_x(); + S(1, 1) /= intrinsics.focal_length_y(); + + return S * T; +} + +class HomographySymmetricGeometricCostFunctor { + public: + HomographySymmetricGeometricCostFunctor(Vec2 x, Vec2 y) + : x_(x), y_(y) { } + + template<typename T> + bool operator()(const T *homography_parameters, T *residuals) const { + typedef Eigen::Matrix<T, 3, 3> Mat3; + typedef Eigen::Matrix<T, 3, 1> Vec3; + + Mat3 H(homography_parameters); + + Vec3 x(T(x_(0)), T(x_(1)), T(1.0)); + Vec3 y(T(y_(0)), T(y_(1)), T(1.0)); + + Vec3 H_x = H * x; + Vec3 Hinv_y = H.inverse() * y; + + H_x /= H_x(2); + Hinv_y /= Hinv_y(2); + + residuals[0] = H_x(0) - T(y_(0)); + residuals[1] = H_x(1) - T(y_(1)); + + residuals[2] = Hinv_y(0) - T(x_(0)); + residuals[3] = Hinv_y(1) - T(x_(1)); + + return true; + } + + const Vec2 x_; + const Vec2 y_; +}; + +void ComputeHomographyFromCorrespondences(const Mat &x1, const Mat &x2, + CameraIntrinsics &intrinsics, + Mat3 *H) { + // Algebraic homography estimation, happens with normalized coordinates + Homography2DFromCorrespondencesLinear(x1, x2, H, 1e-12); + + // Refine matrix using Ceres minimizer + + // TODO(sergey): look into refinement in pixel space. + ceres::Problem problem; + + for (int i = 0; i < x1.cols(); i++) { + HomographySymmetricGeometricCostFunctor + *homography_symmetric_geometric_cost_function = + new HomographySymmetricGeometricCostFunctor(x1.col(i), + x2.col(i)); + + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction< + HomographySymmetricGeometricCostFunctor, + 4, /* num_residuals */ + 9>(homography_symmetric_geometric_cost_function), + NULL, + H->data()); + } + + // Configure the solve. + ceres::Solver::Options solver_options; + solver_options.linear_solver_type = ceres::DENSE_QR; + solver_options.max_num_iterations = 50; + solver_options.update_state_every_iteration = true; + solver_options.parameter_tolerance = 1e-16; + solver_options.function_tolerance = 1e-16; + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + VLOG(1) << "Summary:\n" << summary.FullReport(); + + // Convert homography to original pixel space + Mat3 N = IntrinsicsNormalizationMatrix(intrinsics); + *H = N.inverse() * (*H) * N; +} + +class FundamentalSymmetricEpipolarCostFunctor { + public: + FundamentalSymmetricEpipolarCostFunctor(Vec2 x, Vec2 y) + : x_(x), y_(y) {} + + template<typename T> + bool operator()(const T *fundamental_parameters, T *residuals) const { + typedef Eigen::Matrix<T, 3, 3> Mat3; + typedef Eigen::Matrix<T, 3, 1> Vec3; + + Mat3 F(fundamental_parameters); + + Vec3 x(T(x_(0)), T(x_(1)), T(1.0)); + Vec3 y(T(y_(0)), T(y_(1)), T(1.0)); + + Vec3 F_x = F * x; + Vec3 Ft_y = F.transpose() * y; + T y_F_x = y.dot(F_x); + + residuals[0] = y_F_x * T(1) / F_x.head(2).norm(); + residuals[1] = y_F_x * T(1) / Ft_y.head(2).norm(); + + return true; + } + + const Mat x_; + const Mat y_; +}; + +void ComputeFundamentalFromCorrespondences(const Mat &x1, const Mat &x2, + CameraIntrinsics &intrinsics, + Mat3 *F) { + // Algebraic fundamental estimation, happens with normalized coordinates + NormalizedEightPointSolver(x1, x2, F); + + // Refine matrix using Ceres minimizer + + // TODO(sergey): look into refinement in pixel space. + ceres::Problem problem; + + for (int i = 0; i < x1.cols(); i++) { + FundamentalSymmetricEpipolarCostFunctor + *fundamental_symmetric_epipolar_cost_function = + new FundamentalSymmetricEpipolarCostFunctor(x1.col(i), + x2.col(i)); + + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction< + FundamentalSymmetricEpipolarCostFunctor, + 2, /* num_residuals */ + 9>(fundamental_symmetric_epipolar_cost_function), + NULL, + F->data()); + } + + // Configure the solve. + ceres::Solver::Options solver_options; + solver_options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY; + solver_options.max_num_iterations = 50; + solver_options.update_state_every_iteration = true; + solver_options.parameter_tolerance = 1e-16; + solver_options.function_tolerance = 1e-16; + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + VLOG(1) << "Summary:\n" << summary.FullReport(); + + // Convert fundamental to original pixel space + Mat3 N = IntrinsicsNormalizationMatrix(intrinsics); + *F = N.inverse() * (*F) * N; +} + +// P.H.S. Torr +// Geometric Motion Segmentation and Model Selection +// +// http://reference.kfupm.edu.sa/content/g/e/geometric_motion_segmentation_and_model__126445.pdf +// +// d is the number of dimensions modeled +// (d = 3 for a fundamental matrix or 2 for a homography) +// k is the number of degrees of freedom in the model +// (k = 7 for a fundamental matrix or 8 for a homography) +// r is the dimension of the data +// (r = 4 for 2D correspondences between two frames) +double GRIC(const Vec &e, int d, int k, int r) { + int n = e.rows(); + double lambda1 = log(static_cast<double>(r)); + double lambda2 = log(static_cast<double>(r * n)); + + // lambda3 limits the residual error, and this paper + // http://elvera.nue.tu-berlin.de/files/0990Knorr2006.pdf + // suggests using lambda3 of 2 + // same value is used in Torr's Problem of degeneracy in structure + // and motion recovery from uncalibrated image sequences + // http://www.robots.ox.ac.uk/~vgg/publications/papers/torr99.ps.gz + double lambda3 = 2.0; + + // measurement error of tracker + double sigma2 = 0.01; + + // Actual GRIC computation + double gric_result = 0.0; + + for (int i = 0; i < n; i++) { + double rho = std::min(e(i) * e(i) / sigma2, lambda3 * (r - d)); + gric_result += rho; + } + + gric_result += lambda1 * d * n; + gric_result += lambda2 * k; + + return gric_result; +} + +// Compute a generalized inverse using eigen value decomposition. +// It'll actually also zero 7 last eigen values to deal with +// gauges, since this function is used to compute variance of +// reconstructed 3D points. +// +// TODO(sergey): Could be generalized by making it so number +// of values to be zeroed is passed by an argument +// and moved to numeric module. +Mat pseudoInverse(const Mat &matrix) { + Eigen::EigenSolver<Mat> eigenSolver(matrix); + Mat D = eigenSolver.pseudoEigenvalueMatrix(); + Mat V = eigenSolver.pseudoEigenvectors(); + + double epsilon = std::numeric_limits<double>::epsilon(); + + for (int i = 0; i < D.cols(); ++i) { + if (D(i, i) > epsilon) + D(i, i) = 1.0 / D(i, i); + else + D(i, i) = 0.0; + } + + // Zero last 7 (which corresponds to smallest eigen values). + // 7 equals to the number of gauge freedoms. + for (int i = D.cols() - 7; i < D.cols(); ++i) + D(i, i) = 0.0; + + return V * D * V.inverse(); +} +} // namespace + +void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks, + CameraIntrinsics &intrinsics, + vector<int> &keyframes) { + // Mirza Tahir Ahmed, Matthew N. Dailey + // Robust key frame extraction for 3D reconstruction from video streams + // + // http://www.cs.ait.ac.th/~mdailey/papers/Tahir-KeyFrame.pdf + + int max_image = tracks.MaxImage(); + int next_keyframe = 1; + int number_keyframes = 0; + + // Limit correspondence ratio from both sides. + // On the one hand if number of correspondent features is too low, + // triangulation will suffer. + // On the other hand high correspondence likely means short baseline. + // which also will affect om accuracy + const double Tmin = 0.8; + const double Tmax = 1.0; + + Mat3 N = IntrinsicsNormalizationMatrix(intrinsics); + Mat3 N_inverse = N.inverse(); + + double Sc_best = std::numeric_limits<double>::max(); + double success_intersects_factor_best = 0.0f; + + while (next_keyframe != -1) { + int current_keyframe = next_keyframe; + double Sc_best_candidate = std::numeric_limits<double>::max(); + + LG << "Found keyframe " << next_keyframe; + + number_keyframes++; + next_keyframe = -1; + + for (int candidate_image = current_keyframe + 1; + candidate_image <= max_image; + candidate_image++) { + // Conjunction of all markers from both keyframes + vector<Marker> all_markers = + tracks.MarkersInBothImages(current_keyframe, candidate_image); + + // Match keypoints between frames current_keyframe and candidate_image + vector<Marker> tracked_markers = + tracks.MarkersForTracksInBothImages(current_keyframe, candidate_image); + + // Correspondences in normalized space + Mat x1, x2; + CoordinatesForMarkersInImage(tracked_markers, current_keyframe, &x1); + CoordinatesForMarkersInImage(tracked_markers, candidate_image, &x2); + + LG << "Found " << x1.cols() + << " correspondences between " << current_keyframe + << " and " << candidate_image; + + // Not enough points to construct fundamental matrix + if (x1.cols() < 8 || x2.cols() < 8) + continue; + + // STEP 1: Correspondence ratio constraint + int Tc = tracked_markers.size(); + int Tf = all_markers.size(); + double Rc = static_cast<double>(Tc) / Tf; + + LG << "Correspondence between " << current_keyframe + << " and " << candidate_image + << ": " << Rc; + + if (Rc < Tmin || Rc > Tmax) + continue; + + Mat3 H, F; + ComputeHomographyFromCorrespondences(x1, x2, intrinsics, &H); + ComputeFundamentalFromCorrespondences(x1, x2, intrinsics, &F); + + // TODO(sergey): STEP 2: Discard outlier matches + + // STEP 3: Geometric Robust Information Criteria + + // Compute error values for homography and fundamental matrices + Vec H_e, F_e; + H_e.resize(x1.cols()); + F_e.resize(x1.cols()); + for (int i = 0; i < x1.cols(); i++) { + Vec2 current_x1 = + NorrmalizedToPixelSpace(Vec2(x1(0, i), x1(1, i)), intrinsics); + Vec2 current_x2 = + NorrmalizedToPixelSpace(Vec2(x2(0, i), x2(1, i)), intrinsics); + + H_e(i) = SymmetricGeometricDistance(H, current_x1, current_x2); + F_e(i) = SymmetricEpipolarDistance(F, current_x1, current_x2); + } + + LG << "H_e: " << H_e.transpose(); + LG << "F_e: " << F_e.transpose(); + + // Degeneracy constraint + double GRIC_H = GRIC(H_e, 2, 8, 4); + double GRIC_F = GRIC(F_e, 3, 7, 4); + + LG << "GRIC values for frames " << current_keyframe + << " and " << candidate_image + << ", H-GRIC: " << GRIC_H + << ", F-GRIC: " << GRIC_F; + + if (GRIC_H <= GRIC_F) + continue; + + // TODO(sergey): STEP 4: PELC criterion + + // STEP 5: Estimation of reconstruction error + // + // Uses paper Keyframe Selection for Camera Motion and Structure + // Estimation from Multiple Views + // Uses ftp://ftp.tnt.uni-hannover.de/pub/papers/2004/ECCV2004-TTHBAW.pdf + // Basically, equation (15) + // + // TODO(sergey): separate all the constraints into functions, + // this one is getting to much cluttered already + + // Definitions in equation (15): + // - I is the number of 3D feature points + // - A is the number of essential parameters of one camera + + EuclideanReconstruction reconstruction; + + // The F matrix should be an E matrix, but squash it just to be sure + + // Reconstruction should happen using normalized fundamental matrix + Mat3 F_normal = N * F * N_inverse; + + Mat3 E; + FundamentalToEssential(F_normal, &E); + + // Recover motion between the two images. Since this function assumes a + // calibrated camera, use the identity for K + Mat3 R; + Vec3 t; + Mat3 K = Mat3::Identity(); + + if (!MotionFromEssentialAndCorrespondence(E, + K, x1.col(0), + K, x2.col(0), + &R, &t)) { + LG << "Failed to compute R and t from E and K"; + continue; + } + + LG << "Camera transform between frames " << current_keyframe + << " and " << candidate_image + << ":\nR:\n" << R + << "\nt:" << t.transpose(); + + // First camera is identity, second one is relative to it + reconstruction.InsertCamera(current_keyframe, + Mat3::Identity(), + Vec3::Zero()); + reconstruction.InsertCamera(candidate_image, R, t); + + // Reconstruct 3D points + int intersects_total = 0, intersects_success = 0; + for (int i = 0; i < tracked_markers.size(); i++) { + if (!reconstruction.PointForTrack(tracked_markers[i].track)) { + vector<Marker> reconstructed_markers; + + int track = tracked_markers[i].track; + + reconstructed_markers.push_back(tracked_markers[i]); + + // We know there're always only two markers for a track + // Also, we're using brute-force search because we don't + // actually know about markers layout in a list, but + // at this moment this cycle will run just once, which + // is not so big deal + + for (int j = i + 1; j < tracked_markers.size(); j++) { + if (tracked_markers[j].track == track) { + reconstructed_markers.push_back(tracked_markers[j]); + break; + } + } + + intersects_total++; + + if (EuclideanIntersect(reconstructed_markers, &reconstruction)) { + LG << "Ran Intersect() for track " << track; + intersects_success++; + } else { + LG << "Filed to intersect track " << track; + } + } + } + + double success_intersects_factor = + (double) intersects_success / intersects_total; + + if (success_intersects_factor < success_intersects_factor_best) { + LG << "Skip keyframe candidate because of " + "lower successful intersections ratio"; + + continue; + } + + success_intersects_factor_best = success_intersects_factor; + + Tracks two_frames_tracks(tracked_markers); + CameraIntrinsics empty_intrinsics; + BundleEvaluation evaluation; + evaluation.evaluate_jacobian = true; + + EuclideanBundleCommonIntrinsics(two_frames_tracks, + BUNDLE_NO_INTRINSICS, + BUNDLE_NO_CONSTRAINTS, + &reconstruction, + &empty_intrinsics, + &evaluation); + + Mat &jacobian = evaluation.jacobian; + + Mat JT_J = jacobian.transpose() * jacobian; + Mat JT_J_inv = pseudoInverse(JT_J); + + Mat temp_derived = JT_J * JT_J_inv * JT_J; + bool is_inversed = (temp_derived - JT_J).cwiseAbs2().sum() < + 1e-4 * std::min(temp_derived.cwiseAbs2().sum(), + JT_J.cwiseAbs2().sum()); + + LG << "Check on inversed: " << (is_inversed ? "true" : "false" ) + << ", det(JT_J): " << JT_J.determinant(); + + if (!is_inversed) { + LG << "Ignoring candidature due to poor jacobian stability"; + continue; + } + + Mat Sigma_P; + Sigma_P = JT_J_inv.bottomRightCorner(evaluation.num_points * 3, + evaluation.num_points * 3); + + int I = evaluation.num_points; + int A = 12; + + double Sc = static_cast<double>(I + A) / Square(3 * I) * Sigma_P.trace(); + + LG << "Expected estimation error between " + << current_keyframe << " and " + << candidate_image << ": " << Sc; + + // Pairing with a lower Sc indicates a better choice + if (Sc > Sc_best_candidate) + continue; + + Sc_best_candidate = Sc; + + next_keyframe = candidate_image; + } + + // This is a bit arbitrary and main reason of having this is to deal + // better with situations when there's no keyframes were found for + // current keyframe this could happen when there's no so much parallax + // in the beginning of image sequence and then most of features are + // getting occluded. In this case there could be good keyframe pair in + // the middle of the sequence + // + // However, it's just quick hack and smarter way to do this would be nice + if (next_keyframe == -1) { + next_keyframe = current_keyframe + 10; + number_keyframes = 0; + + if (next_keyframe >= max_image) + break; + + LG << "Starting searching for keyframes starting from " << next_keyframe; + } else { + // New pair's expected reconstruction error is lower + // than existing pair's one. + // + // For now let's store just one candidate, easy to + // store more candidates but needs some thoughts + // how to choose best one automatically from them + // (or allow user to choose pair manually). + if (Sc_best > Sc_best_candidate) { + keyframes.clear(); + keyframes.push_back(current_keyframe); + keyframes.push_back(next_keyframe); + Sc_best = Sc_best_candidate; + } + } + } +} + +} // namespace libmv diff --git a/extern/libmv/libmv/simple_pipeline/keyframe_selection.h b/extern/libmv/libmv/simple_pipeline/keyframe_selection.h new file mode 100644 index 00000000000..16d1e33a6fe --- /dev/null +++ b/extern/libmv/libmv/simple_pipeline/keyframe_selection.h @@ -0,0 +1,52 @@ +// Copyright (c) 2010, 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_ +#define LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_ + +#include "libmv/base/vector.h" +#include "libmv/simple_pipeline/tracks.h" +#include "libmv/simple_pipeline/camera_intrinsics.h" + +namespace libmv { + +// Get list of all images which are good enough to be as keyframes from +// camera reconstruction. Based on GRIC criteria and uses Pollefeys' +// approach for correspondence ratio constraint. +// +// As an additional, additional criteria based on reconstruction +// variance is used. This means if correspondence and GRIC criteria +// are passed, two-frames reconstruction using candidate keyframes +// happens. After reconstruction variance of 3D points is calculating +// and if expected error estimation is too large, keyframe candidate +// is rejecting. +// +// \param tracks contains all tracked correspondences between frames +// expected to be undistorted and normalized +// \param intrinsics is camera intrinsics +// \param keyframes will contain all images number which are considered +// good to be used for reconstruction +void SelectkeyframesBasedOnGRICAndVariance(const Tracks &tracks, + CameraIntrinsics &intrinsics, + vector<int> &keyframes); + +} // namespace libmv + +#endif // LIBMV_SIMPLE_PIPELINE_KEYFRAME_SELECTION_H_ |