diff options
Diffstat (limited to 'extern/libmv/libmv/multiview/homography.cc')
| -rw-r--r-- | extern/libmv/libmv/multiview/homography.cc | 470 |
1 files changed, 0 insertions, 470 deletions
diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc deleted file mode 100644 index 346acb3afd9..00000000000 --- a/extern/libmv/libmv/multiview/homography.cc +++ /dev/null @@ -1,470 +0,0 @@ -// Copyright (c) 2008, 2009 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/multiview/homography.h" - -#include "ceres/ceres.h" -#include "libmv/logging/logging.h" -#include "libmv/multiview/conditioning.h" -#include "libmv/multiview/homography_parameterization.h" - -namespace libmv { -/** 2D Homography transformation estimation in the case that points are in - * euclidean coordinates. - * - * x = H y - * x and y vector must have the same direction, we could write - * crossproduct(|x|, * H * |y| ) = |0| - * - * | 0 -1 x2| |a b c| |y1| |0| - * | 1 0 -x1| * |d e f| * |y2| = |0| - * |-x2 x1 0| |g h 1| |1 | |0| - * - * That gives : - * - * (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0| - * (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0| - * (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0| - */ -static bool Homography2DFromCorrespondencesLinearEuc( - const Mat &x1, - const Mat &x2, - Mat3 *H, - double expected_precision) { - assert(2 == x1.rows()); - assert(4 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - int n = x1.cols(); - MatX8 L = Mat::Zero(n * 3, 8); - Mat b = Mat::Zero(n * 3, 1); - for (int i = 0; i < n; ++i) { - int j = 3 * i; - L(j, 0) = x1(0, i); // a - L(j, 1) = x1(1, i); // b - L(j, 2) = 1.0; // c - L(j, 6) = -x2(0, i) * x1(0, i); // g - L(j, 7) = -x2(0, i) * x1(1, i); // h - b(j, 0) = x2(0, i); // i - - ++j; - L(j, 3) = x1(0, i); // d - L(j, 4) = x1(1, i); // e - L(j, 5) = 1.0; // f - L(j, 6) = -x2(1, i) * x1(0, i); // g - L(j, 7) = -x2(1, i) * x1(1, i); // h - b(j, 0) = x2(1, i); // i - - // This ensures better stability - // TODO(julien) make a lite version without this 3rd set - ++j; - L(j, 0) = x2(1, i) * x1(0, i); // a - L(j, 1) = x2(1, i) * x1(1, i); // b - L(j, 2) = x2(1, i); // c - L(j, 3) = -x2(0, i) * x1(0, i); // d - L(j, 4) = -x2(0, i) * x1(1, i); // e - L(j, 5) = -x2(0, i); // f - } - // Solve Lx=B - Vec h = L.fullPivLu().solve(b); - Homography2DNormalizedParameterization<double>::To(h, H); - if ((L * h).isApprox(b, expected_precision)) { - return true; - } else { - return false; - } -} - -/** 2D Homography transformation estimation in the case that points are in - * homogeneous coordinates. - * - * | 0 -x3 x2| |a b c| |y1| -x3*d+x2*g -x3*e+x2*h -x3*f+x2*1 |y1| (-x3*d+x2*g)*y1 (-x3*e+x2*h)*y2 (-x3*f+x2*1)*y3 |0| - * | x3 0 -x1| * |d e f| * |y2| = x3*a-x1*g x3*b-x1*h x3*c-x1*1 * |y2| = (x3*a-x1*g)*y1 (x3*b-x1*h)*y2 (x3*c-x1*1)*y3 = |0| - * |-x2 x1 0| |g h 1| |y3| -x2*a+x1*d -x2*b+x1*e -x2*c+x1*f |y3| (-x2*a+x1*d)*y1 (-x2*b+x1*e)*y2 (-x2*c+x1*f)*y3 |0| - * X = |a b c d e f g h|^t - */ -bool Homography2DFromCorrespondencesLinear(const Mat &x1, - const Mat &x2, - Mat3 *H, - double expected_precision) { - if (x1.rows() == 2) { - return Homography2DFromCorrespondencesLinearEuc(x1, x2, H, - expected_precision); - } - assert(3 == x1.rows()); - assert(4 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - - const int x = 0; - const int y = 1; - const int w = 2; - int n = x1.cols(); - MatX8 L = Mat::Zero(n * 3, 8); - Mat b = Mat::Zero(n * 3, 1); - for (int i = 0; i < n; ++i) { - int j = 3 * i; - L(j, 0) = x2(w, i) * x1(x, i); // a - L(j, 1) = x2(w, i) * x1(y, i); // b - L(j, 2) = x2(w, i) * x1(w, i); // c - L(j, 6) = -x2(x, i) * x1(x, i); // g - L(j, 7) = -x2(x, i) * x1(y, i); // h - b(j, 0) = x2(x, i) * x1(w, i); - - ++j; - L(j, 3) = x2(w, i) * x1(x, i); // d - L(j, 4) = x2(w, i) * x1(y, i); // e - L(j, 5) = x2(w, i) * x1(w, i); // f - L(j, 6) = -x2(y, i) * x1(x, i); // g - L(j, 7) = -x2(y, i) * x1(y, i); // h - b(j, 0) = x2(y, i) * x1(w, i); - - // This ensures better stability - ++j; - L(j, 0) = x2(y, i) * x1(x, i); // a - L(j, 1) = x2(y, i) * x1(y, i); // b - L(j, 2) = x2(y, i) * x1(w, i); // c - L(j, 3) = -x2(x, i) * x1(x, i); // d - L(j, 4) = -x2(x, i) * x1(y, i); // e - L(j, 5) = -x2(x, i) * x1(w, i); // f - } - // Solve Lx=B - Vec h = L.fullPivLu().solve(b); - if ((L * h).isApprox(b, expected_precision)) { - Homography2DNormalizedParameterization<double>::To(h, H); - return true; - } else { - return false; - } -} - -// Default settings for homography estimation which should be suitable -// for a wide range of use cases. -EstimateHomographyOptions::EstimateHomographyOptions(void) : - use_normalization(true), - max_num_iterations(50), - expected_average_symmetric_distance(1e-16) { -} - -namespace { -void GetNormalizedPoints(const Mat &original_points, - Mat *normalized_points, - Mat3 *normalization_matrix) { - IsotropicPreconditionerFromPoints(original_points, normalization_matrix); - ApplyTransformationToPoints(original_points, - *normalization_matrix, - normalized_points); -} - -// Cost functor which computes symmetric geometric distance -// used for homography matrix refinement. -class HomographySymmetricGeometricCostFunctor { - public: - HomographySymmetricGeometricCostFunctor(const Vec2 &x, - const Vec2 &y) { - xx_ = x(0); - xy_ = x(1); - yx_ = y(0); - yy_ = y(1); - } - - 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(xx_), T(xy_), T(1.0)); - Vec3 y(T(yx_), T(yy_), 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(yx_); - residuals[1] = H_x(1) - T(yy_); - - // This is a backward error. - residuals[2] = Hinv_y(0) - T(xx_); - residuals[3] = Hinv_y(1) - T(xy_); - - return true; - } - - // TODO(sergey): Think of better naming. - double xx_, xy_; - double yx_, yy_; -}; - -// Termination checking callback used for homography estimation. -// It finished the minimization as soon as actual average of -// symmetric geometric distance is less or equal to the expected -// average value. -class TerminationCheckingCallback : public ceres::IterationCallback { - public: - TerminationCheckingCallback(const Mat &x1, const Mat &x2, - const EstimateHomographyOptions &options, - Mat3 *H) - : options_(options), x1_(x1), x2_(x2), H_(H) {} - - virtual ceres::CallbackReturnType operator()( - const ceres::IterationSummary& summary) { - // If the step wasn't successful, there's nothing to do. - if (!summary.step_is_successful) { - return ceres::SOLVER_CONTINUE; - } - - // Calculate average of symmetric geometric distance. - double average_distance = 0.0; - for (int i = 0; i < x1_.cols(); i++) { - average_distance = SymmetricGeometricDistance(*H_, - x1_.col(i), - x2_.col(i)); - } - average_distance /= x1_.cols(); - - if (average_distance <= options_.expected_average_symmetric_distance) { - return ceres::SOLVER_TERMINATE_SUCCESSFULLY; - } - - return ceres::SOLVER_CONTINUE; - } - - private: - const EstimateHomographyOptions &options_; - const Mat &x1_; - const Mat &x2_; - Mat3 *H_; -}; -} // namespace - -/** 2D Homography transformation estimation in the case that points are in - * euclidean coordinates. - */ -bool EstimateHomography2DFromCorrespondences( - const Mat &x1, - const Mat &x2, - const EstimateHomographyOptions &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()); - - Mat3 T1 = Mat3::Identity(), - T2 = Mat3::Identity(); - - // Step 1: Algebraic homography estimation. - Mat x1_normalized, x2_normalized; - - if (options.use_normalization) { - LG << "Estimating homography using normalization."; - GetNormalizedPoints(x1, &x1_normalized, &T1); - GetNormalizedPoints(x2, &x2_normalized, &T2); - } else { - x1_normalized = x1; - x2_normalized = x2; - } - - // Assume algebraic estiation always suceeds, - Homography2DFromCorrespondencesLinear(x1_normalized, x2_normalized, H); - - // Denormalize the homography matrix. - if (options.use_normalization) { - *H = T2.inverse() * (*H) * T1; - } - - LG << "Estimated matrix after algebraic estimation:\n" << *H; - - // 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; - - // Terminate if the average symmetric distance is good enough. - TerminationCheckingCallback callback(x1, x2, options, H); - solver_options.callbacks.push_back(&callback); - - // 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.IsSolutionUsable(); -} - -/** - * x2 ~ A * x1 - * x2^t * Hi * A *x1 = 0 - * H1 = H2 = H3 = - * | 0 0 0 1| |-x2w| |0 0 0 0| | 0 | | 0 0 1 0| |-x2z| - * | 0 0 0 0| -> | 0 | |0 0 1 0| -> |-x2z| | 0 0 0 0| -> | 0 | - * | 0 0 0 0| | 0 | |0-1 0 0| | x2y| |-1 0 0 0| | x2x| - * |-1 0 0 0| | x2x| |0 0 0 0| | 0 | | 0 0 0 0| | 0 | - * H4 = H5 = H6 = - * |0 0 0 0| | 0 | | 0 1 0 0| |-x2y| |0 0 0 0| | 0 | - * |0 0 0 1| -> |-x2w| |-1 0 0 0| -> | x2x| |0 0 0 0| -> | 0 | - * |0 0 0 0| | 0 | | 0 0 0 0| | 0 | |0 0 0 1| |-x2w| - * |0-1 0 0| | x2y| | 0 0 0 0| | 0 | |0 0-1 0| | x2z| - * |a b c d| - * A = |e f g h| - * |i j k l| - * |m n o 1| - * - * x2^t * H1 * A *x1 = (-x2w*a +x2x*m )*x1x + (-x2w*b +x2x*n )*x1y + (-x2w*c +x2x*o )*x1z + (-x2w*d +x2x*1 )*x1w = 0 - * x2^t * H2 * A *x1 = (-x2z*e +x2y*i )*x1x + (-x2z*f +x2y*j )*x1y + (-x2z*g +x2y*k )*x1z + (-x2z*h +x2y*l )*x1w = 0 - * x2^t * H3 * A *x1 = (-x2z*a +x2x*i )*x1x + (-x2z*b +x2x*j )*x1y + (-x2z*c +x2x*k )*x1z + (-x2z*d +x2x*l )*x1w = 0 - * x2^t * H4 * A *x1 = (-x2w*e +x2y*m )*x1x + (-x2w*f +x2y*n )*x1y + (-x2w*g +x2y*o )*x1z + (-x2w*h +x2y*1 )*x1w = 0 - * x2^t * H5 * A *x1 = (-x2y*a +x2x*e )*x1x + (-x2y*b +x2x*f )*x1y + (-x2y*c +x2x*g )*x1z + (-x2y*d +x2x*h )*x1w = 0 - * x2^t * H6 * A *x1 = (-x2w*i +x2z*m )*x1x + (-x2w*j +x2z*n )*x1y + (-x2w*k +x2z*o )*x1z + (-x2w*l +x2z*1 )*x1w = 0 - * - * X = |a b c d e f g h i j k l m n o|^t -*/ -bool Homography3DFromCorrespondencesLinear(const Mat &x1, - const Mat &x2, - Mat4 *H, - double expected_precision) { - assert(4 == x1.rows()); - assert(5 <= x1.cols()); - assert(x1.rows() == x2.rows()); - assert(x1.cols() == x2.cols()); - const int x = 0; - const int y = 1; - const int z = 2; - const int w = 3; - int n = x1.cols(); - MatX15 L = Mat::Zero(n * 6, 15); - Mat b = Mat::Zero(n * 6, 1); - for (int i = 0; i < n; ++i) { - int j = 6 * i; - L(j, 0) = -x2(w, i) * x1(x, i); // a - L(j, 1) = -x2(w, i) * x1(y, i); // b - L(j, 2) = -x2(w, i) * x1(z, i); // c - L(j, 3) = -x2(w, i) * x1(w, i); // d - L(j, 12) = x2(x, i) * x1(x, i); // m - L(j, 13) = x2(x, i) * x1(y, i); // n - L(j, 14) = x2(x, i) * x1(z, i); // o - b(j, 0) = -x2(x, i) * x1(w, i); - - ++j; - L(j, 4) = -x2(z, i) * x1(x, i); // e - L(j, 5) = -x2(z, i) * x1(y, i); // f - L(j, 6) = -x2(z, i) * x1(z, i); // g - L(j, 7) = -x2(z, i) * x1(w, i); // h - L(j, 8) = x2(y, i) * x1(x, i); // i - L(j, 9) = x2(y, i) * x1(y, i); // j - L(j, 10) = x2(y, i) * x1(z, i); // k - L(j, 11) = x2(y, i) * x1(w, i); // l - - ++j; - L(j, 0) = -x2(z, i) * x1(x, i); // a - L(j, 1) = -x2(z, i) * x1(y, i); // b - L(j, 2) = -x2(z, i) * x1(z, i); // c - L(j, 3) = -x2(z, i) * x1(w, i); // d - L(j, 8) = x2(x, i) * x1(x, i); // i - L(j, 9) = x2(x, i) * x1(y, i); // j - L(j, 10) = x2(x, i) * x1(z, i); // k - L(j, 11) = x2(x, i) * x1(w, i); // l - - ++j; - L(j, 4) = -x2(w, i) * x1(x, i); // e - L(j, 5) = -x2(w, i) * x1(y, i); // f - L(j, 6) = -x2(w, i) * x1(z, i); // g - L(j, 7) = -x2(w, i) * x1(w, i); // h - L(j, 12) = x2(y, i) * x1(x, i); // m - L(j, 13) = x2(y, i) * x1(y, i); // n - L(j, 14) = x2(y, i) * x1(z, i); // o - b(j, 0) = -x2(y, i) * x1(w, i); - - ++j; - L(j, 0) = -x2(y, i) * x1(x, i); // a - L(j, 1) = -x2(y, i) * x1(y, i); // b - L(j, 2) = -x2(y, i) * x1(z, i); // c - L(j, 3) = -x2(y, i) * x1(w, i); // d - L(j, 4) = x2(x, i) * x1(x, i); // e - L(j, 5) = x2(x, i) * x1(y, i); // f - L(j, 6) = x2(x, i) * x1(z, i); // g - L(j, 7) = x2(x, i) * x1(w, i); // h - - ++j; - L(j, 8) = -x2(w, i) * x1(x, i); // i - L(j, 9) = -x2(w, i) * x1(y, i); // j - L(j, 10) = -x2(w, i) * x1(z, i); // k - L(j, 11) = -x2(w, i) * x1(w, i); // l - L(j, 12) = x2(z, i) * x1(x, i); // m - L(j, 13) = x2(z, i) * x1(y, i); // n - L(j, 14) = x2(z, i) * x1(z, i); // o - b(j, 0) = -x2(z, i) * x1(w, i); - } - // Solve Lx=B - Vec h = L.fullPivLu().solve(b); - if ((L * h).isApprox(b, expected_precision)) { - Homography3DNormalizedParameterization<double>::To(h, H); - return true; - } else { - return false; - } -} - -double SymmetricGeometricDistance(const Mat3 &H, - const Vec2 &x1, - const Vec2 &x2) { - Vec3 x(x1(0), x1(1), 1.0); - Vec3 y(x2(0), x2(1), 1.0); - - Vec3 H_x = H * x; - Vec3 Hinv_y = H.inverse() * y; - - H_x /= H_x(2); - Hinv_y /= Hinv_y(2); - - return (H_x.head<2>() - y.head<2>()).squaredNorm() + - (Hinv_y.head<2>() - x.head<2>()).squaredNorm(); -} - -} // namespace libmv |