diff options
Diffstat (limited to 'extern/libmv/libmv/multiview')
-rw-r--r-- | extern/libmv/libmv/multiview/fundamental.cc | 91 | ||||
-rw-r--r-- | extern/libmv/libmv/multiview/fundamental.h | 37 | ||||
-rw-r--r-- | extern/libmv/libmv/multiview/homography.cc | 111 | ||||
-rw-r--r-- | extern/libmv/libmv/multiview/homography.h | 39 |
4 files changed, 278 insertions, 0 deletions
diff --git a/extern/libmv/libmv/multiview/fundamental.cc b/extern/libmv/libmv/multiview/fundamental.cc index a05ef7907a2..ec6c70d81cf 100644 --- a/extern/libmv/libmv/multiview/fundamental.cc +++ b/extern/libmv/libmv/multiview/fundamental.cc @@ -20,6 +20,7 @@ #include "libmv/multiview/fundamental.h" +#include "ceres/ceres.h" #include "libmv/logging/logging.h" #include "libmv/numeric/numeric.h" #include "libmv/numeric/poly.h" @@ -407,4 +408,94 @@ void FundamentalToEssential(const Mat3 &F, Mat3 *E) { *E = svd.matrixU() * diag.asDiagonal() * svd.matrixV().transpose(); } +FundamentalEstimationOptions::FundamentalEstimationOptions(void) : + use_refine_if_algebraic_fails(true), + max_num_iterations(50), + parameter_tolerance(1e-16), + function_tolerance(1e-16) { +} + +class FundamentalSymmetricEpipolarCostFunctor { + public: + FundamentalSymmetricEpipolarCostFunctor(const Vec2 &x, + const 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_; +}; + +/* Fundamental transformation estimation. */ +bool FundamentalFromCorrespondencesEuc( + const Mat &x1, + const Mat &x2, + const FundamentalEstimationOptions &options, + Mat3 *F) { + // Step 1: Algebraic fundamental estimation. + bool algebraic_success = NormalizedEightPointSolver(x1, x2, F); + + LG << "Algebraic result " << algebraic_success + << ", estimated matrix:\n" << *F; + + if (!algebraic_success && !options.use_refine_if_algebraic_fails) { + return false; + } + + // Step 2: Refine matrix using Ceres minimizer. + 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_QR; + solver_options.max_num_iterations = options.max_num_iterations; + solver_options.update_state_every_iteration = true; + solver_options.parameter_tolerance = options.parameter_tolerance; + solver_options.function_tolerance = options.function_tolerance; + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + VLOG(1) << "Summary:\n" << summary.FullReport(); + + LG << "Final refined matrix:\n" << *F; + + return !(summary.termination_type == ceres::DID_NOT_RUN || + summary.termination_type == ceres::NUMERICAL_FAILURE); +} + } // namespace libmv diff --git a/extern/libmv/libmv/multiview/fundamental.h b/extern/libmv/libmv/multiview/fundamental.h index 1ad184d8314..2961a46cdc4 100644 --- a/extern/libmv/libmv/multiview/fundamental.h +++ b/extern/libmv/libmv/multiview/fundamental.h @@ -144,6 +144,43 @@ bool MotionFromEssentialAndCorrespondence(const Mat3 &E, */ void FundamentalToEssential(const Mat3 &F, Mat3 *E); +/** + * This structure contains options that controls how the fundamental + * estimation operates. + * + * Defaults should be suitable for a wide range of use cases, but + * better performance and accuracy might require tweaking/ + */ +struct FundamentalEstimationOptions { + // Default constructor which sets up a options for generic usage. + FundamentalEstimationOptions(void); + + // Refine fundamental matrix even if algebraic estimation reported failure. + bool use_refine_if_algebraic_fails; + + // Maximal number of iterations for refinement step. + int max_num_iterations; + + // Paramaneter tolerance used by minimizer termination criteria. + float parameter_tolerance; + + // Function tolerance used by minimizer termination criteria. + float function_tolerance; +}; + +/** + * Fundamental transformation estimation. + * + * This function estimates the fundamental transformation from a list of 2D + * correspondences by doing algebraic estimation first followed with result + * refinement. + */ +bool FundamentalFromCorrespondencesEuc( + const Mat &x1, + const Mat &x2, + const FundamentalEstimationOptions &options, + Mat3 *F); + } // namespace libmv #endif // LIBMV_MULTIVIEW_FUNDAMENTAL_H_ diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc index 317eb3f85c6..6041849d9fe 100644 --- a/extern/libmv/libmv/multiview/homography.cc +++ b/extern/libmv/libmv/multiview/homography.cc @@ -20,6 +20,7 @@ #include "libmv/multiview/homography.h" +#include "ceres/ceres.h" #include "libmv/logging/logging.h" #include "libmv/multiview/homography_parameterization.h" @@ -153,6 +154,116 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1, return false; } } + +HomographyEstimationOptions::HomographyEstimationOptions(void) : + expected_algebraic_precision(EigenDouble::dummy_precision()), + use_refine_if_algebraic_fails(true), + max_num_iterations(50), + parameter_tolerance(1e-16), + function_tolerance(1e-16) { +} + +class HomographySymmetricGeometricCostFunctor { + public: + HomographySymmetricGeometricCostFunctor(const Vec2 &x, + const 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); + + // This is a forward error. + residuals[0] = H_x(0) - T(y_(0)); + residuals[1] = H_x(1) - T(y_(1)); + + // This is a backward error. + residuals[2] = Hinv_y(0) - T(x_(0)); + residuals[3] = Hinv_y(1) - T(x_(1)); + + return true; + } + + const Vec2 x_; + const Vec2 y_; +}; + +/** 2D Homography transformation estimation in the case that points are in + * euclidean coordinates. + */ +bool Homography2DFromCorrespondencesEuc( + const Mat &x1, + const Mat &x2, + const HomographyEstimationOptions &options, + Mat3 *H) { + // TODO(sergey): Support homogenous coordinates, not just euclidean. + + assert(2 == x1.rows()); + assert(4 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Step 1: Algebraic homography estimation. + bool algebraic_success = + Homography2DFromCorrespondencesLinear(x1, x2, H, + options.expected_algebraic_precision); + + LG << "Algebraic result " << algebraic_success + << ", estimated matrix:\n" << *H; + + if (!algebraic_success && !options.use_refine_if_algebraic_fails) { + return false; + } + + // Step 2: Refine matrix using Ceres minimizer. + 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 = options.max_num_iterations; + solver_options.update_state_every_iteration = true; + solver_options.parameter_tolerance = options.parameter_tolerance; + solver_options.function_tolerance = options.function_tolerance; + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + VLOG(1) << "Summary:\n" << summary.FullReport(); + + LG << "Final refined matrix:\n" << *H; + + return !(summary.termination_type == ceres::DID_NOT_RUN || + summary.termination_type == ceres::NUMERICAL_FAILURE); +} + /** * x2 ~ A * x1 * x2^t * Hi * A *x1 = 0 diff --git a/extern/libmv/libmv/multiview/homography.h b/extern/libmv/libmv/multiview/homography.h index 8d2dff930eb..1928e39dffb 100644 --- a/extern/libmv/libmv/multiview/homography.h +++ b/extern/libmv/libmv/multiview/homography.h @@ -54,6 +54,45 @@ bool Homography2DFromCorrespondencesLinear(const Mat &x1, EigenDouble::dummy_precision()); /** + * This structure contains options that controls how the homography + * estimation operates. + * + * Defaults should be suitable for a wide range of use cases, but + * better performance and accuracy might require tweaking/ + */ +struct HomographyEstimationOptions { + // Default constructor which sets up a options for generic usage. + HomographyEstimationOptions(void); + + // Expected precision of algebraic estimation. + double expected_algebraic_precision; + + // Refine homography even if algebraic estimation reported failure. + bool use_refine_if_algebraic_fails; + + // Maximal number of iterations for refinement step. + int max_num_iterations; + + // Paramaneter tolerance used by minimizer termination criteria. + float parameter_tolerance; + + // Function tolerance used by minimizer termination criteria. + float function_tolerance; +}; + +/** + * 2D homography transformation estimation. + * + * This function estimates the homography transformation from a list of 2D + * correspondences by doing algebraic estimation first followed with result + * refinement. + */ +bool Homography2DFromCorrespondencesEuc(const Mat &x1, + const Mat &x2, + const HomographyEstimationOptions &options, + Mat3 *H); + +/** * 3D Homography transformation estimation. * * This function can be used in order to estimate the homography transformation |