diff options
Diffstat (limited to 'intern/libmv/libmv/multiview')
31 files changed, 5326 insertions, 0 deletions
diff --git a/intern/libmv/libmv/multiview/conditioning.cc b/intern/libmv/libmv/multiview/conditioning.cc new file mode 100644 index 00000000000..0afbf119ea3 --- /dev/null +++ b/intern/libmv/libmv/multiview/conditioning.cc @@ -0,0 +1,99 @@ +// Copyright (c) 2010 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/conditioning.h" +#include "libmv/multiview/projection.h" + +namespace libmv { + +// HZ 4.4.4 pag.109: Point conditioning (non isotropic) +void PreconditionerFromPoints(const Mat &points, Mat3 *T) { + Vec mean, variance; + MeanAndVarianceAlongRows(points, &mean, &variance); + + double xfactor = sqrt(2.0 / variance(0)); + double yfactor = sqrt(2.0 / variance(1)); + + // If variance is equal to 0.0 set scaling factor to identity. + // -> Else it will provide nan value (because division by 0). + if (variance(0) < 1e-8) + xfactor = mean(0) = 1.0; + if (variance(1) < 1e-8) + yfactor = mean(1) = 1.0; + + *T << xfactor, 0, -xfactor * mean(0), + 0, yfactor, -yfactor * mean(1), + 0, 0, 1; +} +// HZ 4.4.4 pag.107: Point conditioning (isotropic) +void IsotropicPreconditionerFromPoints(const Mat &points, Mat3 *T) { + Vec mean, variance; + MeanAndVarianceAlongRows(points, &mean, &variance); + + double var_norm = variance.norm(); + double factor = sqrt(2.0 / var_norm); + + // If variance is equal to 0.0 set scaling factor to identity. + // -> Else it will provide nan value (because division by 0). + if (var_norm < 1e-8) { + factor = 1.0; + mean.setOnes(); + } + + *T << factor, 0, -factor * mean(0), + 0, factor, -factor * mean(1), + 0, 0, 1; +} + +void ApplyTransformationToPoints(const Mat &points, + const Mat3 &T, + Mat *transformed_points) { + int n = points.cols(); + transformed_points->resize(2, n); + Mat3X p(3, n); + EuclideanToHomogeneous(points, &p); + p = T * p; + HomogeneousToEuclidean(p, transformed_points); +} + +void NormalizePoints(const Mat &points, + Mat *normalized_points, + Mat3 *T) { + PreconditionerFromPoints(points, T); + ApplyTransformationToPoints(points, *T, normalized_points); +} + +void NormalizeIsotropicPoints(const Mat &points, + Mat *normalized_points, + Mat3 *T) { + IsotropicPreconditionerFromPoints(points, T); + ApplyTransformationToPoints(points, *T, normalized_points); +} + +// Denormalize the results. See HZ page 109. +void UnnormalizerT::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) { + *H = T2.transpose() * (*H) * T1; +} + +void UnnormalizerI::Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H) { + *H = T2.inverse() * (*H) * T1; +} + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/conditioning.h b/intern/libmv/libmv/multiview/conditioning.h new file mode 100644 index 00000000000..8f3e3a76070 --- /dev/null +++ b/intern/libmv/libmv/multiview/conditioning.h @@ -0,0 +1,59 @@ +// Copyright (c) 2010 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_MULTIVIEW_CONDITIONNING_H_ +#define LIBMV_MULTIVIEW_CONDITIONNING_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +// Point conditioning (non isotropic) +void PreconditionerFromPoints(const Mat &points, Mat3 *T); +// Point conditioning (isotropic) +void IsotropicPreconditionerFromPoints(const Mat &points, Mat3 *T); + +void ApplyTransformationToPoints(const Mat &points, + const Mat3 &T, + Mat *transformed_points); + +void NormalizePoints(const Mat &points, + Mat *normalized_points, + Mat3 *T); + +void NormalizeIsotropicPoints(const Mat &points, + Mat *normalized_points, + Mat3 *T); + +/// Use inverse for unnormalize +struct UnnormalizerI { + // Denormalize the results. See HZ page 109. + static void Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H); +}; + +/// Use transpose for unnormalize +struct UnnormalizerT { + // Denormalize the results. See HZ page 109. + static void Unnormalize(const Mat3 &T1, const Mat3 &T2, Mat3 *H); +}; + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_CONDITIONNING_H_ diff --git a/intern/libmv/libmv/multiview/euclidean_resection.cc b/intern/libmv/libmv/multiview/euclidean_resection.cc new file mode 100644 index 00000000000..245b027fb7c --- /dev/null +++ b/intern/libmv/libmv/multiview/euclidean_resection.cc @@ -0,0 +1,774 @@ +// Copyright (c) 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/euclidean_resection.h" + +#include <cmath> +#include <limits> + +#include <Eigen/SVD> +#include <Eigen/Geometry> + +#include "libmv/base/vector.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/projection.h" + +namespace libmv { +namespace euclidean_resection { + +typedef unsigned int uint; + +bool EuclideanResection(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t, + ResectionMethod method) { + switch (method) { + case RESECTION_ANSAR_DANIILIDIS: + EuclideanResectionAnsarDaniilidis(x_camera, X_world, R, t); + break; + case RESECTION_EPNP: + return EuclideanResectionEPnP(x_camera, X_world, R, t); + break; + case RESECTION_PPNP: + return EuclideanResectionPPnP(x_camera, X_world, R, t); + break; + default: + LOG(FATAL) << "Unknown resection method."; + } + return false; +} + +bool EuclideanResection(const Mat &x_image, + const Mat3X &X_world, + const Mat3 &K, + Mat3 *R, Vec3 *t, + ResectionMethod method) { + CHECK(x_image.rows() == 2 || x_image.rows() == 3) + << "Invalid size for x_image: " + << x_image.rows() << "x" << x_image.cols(); + + Mat2X x_camera; + if (x_image.rows() == 2) { + EuclideanToNormalizedCamera(x_image, K, &x_camera); + } else if (x_image.rows() == 3) { + HomogeneousToNormalizedCamera(x_image, K, &x_camera); + } + return EuclideanResection(x_camera, X_world, R, t, method); +} + +void AbsoluteOrientation(const Mat3X &X, + const Mat3X &Xp, + Mat3 *R, + Vec3 *t) { + int num_points = X.cols(); + Vec3 C = X.rowwise().sum() / num_points; // Centroid of X. + Vec3 Cp = Xp.rowwise().sum() / num_points; // Centroid of Xp. + + // Normalize the two point sets. + Mat3X Xn(3, num_points), Xpn(3, num_points); + for (int i = 0; i < num_points; ++i) { + Xn.col(i) = X.col(i) - C; + Xpn.col(i) = Xp.col(i) - Cp; + } + + // Construct the N matrix (pg. 635). + double Sxx = Xn.row(0).dot(Xpn.row(0)); + double Syy = Xn.row(1).dot(Xpn.row(1)); + double Szz = Xn.row(2).dot(Xpn.row(2)); + double Sxy = Xn.row(0).dot(Xpn.row(1)); + double Syx = Xn.row(1).dot(Xpn.row(0)); + double Sxz = Xn.row(0).dot(Xpn.row(2)); + double Szx = Xn.row(2).dot(Xpn.row(0)); + double Syz = Xn.row(1).dot(Xpn.row(2)); + double Szy = Xn.row(2).dot(Xpn.row(1)); + + Mat4 N; + N << Sxx + Syy + Szz, Syz - Szy, Szx - Sxz, Sxy - Syx, + Syz - Szy, Sxx - Syy - Szz, Sxy + Syx, Szx + Sxz, + Szx - Sxz, Sxy + Syx, -Sxx + Syy - Szz, Syz + Szy, + Sxy - Syx, Szx + Sxz, Syz + Szy, -Sxx - Syy + Szz; + + // Find the unit quaternion q that maximizes qNq. It is the eigenvector + // corresponding to the lagest eigenvalue. + Vec4 q = N.jacobiSvd(Eigen::ComputeFullU).matrixU().col(0); + + // Retrieve the 3x3 rotation matrix. + Vec4 qq = q.array() * q.array(); + double q0q1 = q(0) * q(1); + double q0q2 = q(0) * q(2); + double q0q3 = q(0) * q(3); + double q1q2 = q(1) * q(2); + double q1q3 = q(1) * q(3); + double q2q3 = q(2) * q(3); + + (*R) << qq(0) + qq(1) - qq(2) - qq(3), + 2 * (q1q2 - q0q3), + 2 * (q1q3 + q0q2), + 2 * (q1q2+ q0q3), + qq(0) - qq(1) + qq(2) - qq(3), + 2 * (q2q3 - q0q1), + 2 * (q1q3 - q0q2), + 2 * (q2q3 + q0q1), + qq(0) - qq(1) - qq(2) + qq(3); + + // Fix the handedness of the R matrix. + if (R->determinant() < 0) { + R->row(2) = -R->row(2); + } + // Compute the final translation. + *t = Cp - *R * C; +} + +// Convert i and j indices of the original variables into their quadratic +// permutation single index. It follows that t_ij = t_ji. +static int IJToPointIndex(int i, int j, int num_points) { + // Always make sure that j is bigger than i. This handles t_ij = t_ji. + if (j < i) { + std::swap(i, j); + } + int idx; + int num_permutation_rows = num_points * (num_points - 1) / 2; + + // All t_ii's are located at the end of the t vector after all t_ij's. + if (j == i) { + idx = num_permutation_rows + i; + } else { + int offset = (num_points - i - 1) * (num_points - i) / 2; + idx = (num_permutation_rows - offset + j - i - 1); + } + return idx; +}; + +// Convert i and j indexes of the solution for lambda to their linear indexes. +static int IJToIndex(int i, int j, int num_lambda) { + if (j < i) { + std::swap(i, j); + } + int A = num_lambda * (num_lambda + 1) / 2; + int B = num_lambda - i; + int C = B * (B + 1) / 2; + int idx = A - C + j - i; + return idx; +}; + +static int Sign(double value) { + return (value < 0) ? -1 : 1; +}; + +// Organizes a square matrix into a single row constraint on the elements of +// Lambda to create the constraints in equation (5) in "Linear Pose Estimation +// from Points or Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no. +// 5. +static Vec MatrixToConstraint(const Mat &A, + int num_k_columns, + int num_lambda) { + Vec C(num_k_columns); + C.setZero(); + int idx = 0; + for (int i = 0; i < num_lambda; ++i) { + for (int j = i; j < num_lambda; ++j) { + C(idx) = A(i, j); + if (i != j) { + C(idx) += A(j, i); + } + ++idx; + } + } + return C; +} + +// Normalizes the columns of vectors. +static void NormalizeColumnVectors(Mat3X *vectors) { + int num_columns = vectors->cols(); + for (int i = 0; i < num_columns; ++i) { + vectors->col(i).normalize(); + } +} + +void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, + Vec3 *t) { + CHECK(x_camera.cols() == X_world.cols()); + CHECK(x_camera.cols() > 3); + + int num_points = x_camera.cols(); + + // Copy the normalized camera coords into 3 vectors and normalize them so + // that they are unit vectors from the camera center. + Mat3X x_camera_unit(3, num_points); + x_camera_unit.block(0, 0, 2, num_points) = x_camera; + x_camera_unit.row(2).setOnes(); + NormalizeColumnVectors(&x_camera_unit); + + int num_m_rows = num_points * (num_points - 1) / 2; + int num_tt_variables = num_points * (num_points + 1) / 2; + int num_m_columns = num_tt_variables + 1; + Mat M(num_m_columns, num_m_columns); + M.setZero(); + Matu ij_index(num_tt_variables, 2); + + // Create the constraint equations for the t_ij variables (7) and arrange + // them into the M matrix (8). Also store the initial (i, j) indices. + int row = 0; + for (int i = 0; i < num_points; ++i) { + for (int j = i+1; j < num_points; ++j) { + M(row, row) = -2 * x_camera_unit.col(i).dot(x_camera_unit.col(j)); + M(row, num_m_rows + i) = x_camera_unit.col(i).dot(x_camera_unit.col(i)); + M(row, num_m_rows + j) = x_camera_unit.col(j).dot(x_camera_unit.col(j)); + Vec3 Xdiff = X_world.col(i) - X_world.col(j); + double center_to_point_distance = Xdiff.norm(); + M(row, num_m_columns - 1) = + - center_to_point_distance * center_to_point_distance; + ij_index(row, 0) = i; + ij_index(row, 1) = j; + ++row; + } + ij_index(i + num_m_rows, 0) = i; + ij_index(i + num_m_rows, 1) = i; + } + + int num_lambda = num_points + 1; // Dimension of the null space of M. + Mat V = M.jacobiSvd(Eigen::ComputeFullV).matrixV().block(0, + num_m_rows, + num_m_columns, + num_lambda); + + // TODO(vess): The number of constraint equations in K (num_k_rows) must be + // (num_points + 1) * (num_points + 2)/2. This creates a performance issue + // for more than 4 points. It is fine for 4 points at the moment with 18 + // instead of 15 equations. + int num_k_rows = num_m_rows + num_points * + (num_points*(num_points-1)/2 - num_points+1); + int num_k_columns = num_lambda * (num_lambda + 1) / 2; + Mat K(num_k_rows, num_k_columns); + K.setZero(); + + // Construct the first part of the K matrix corresponding to (t_ii, t_jk) for + // i != j. + int counter_k_row = 0; + for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) { + for (int idx2 = 0; idx2 < num_m_rows; ++idx2) { + unsigned int i = ij_index(idx1, 0); + unsigned int j = ij_index(idx2, 0); + unsigned int k = ij_index(idx2, 1); + + if (i != j && i != k) { + int idx3 = IJToPointIndex(i, j, num_points); + int idx4 = IJToPointIndex(i, k, num_points); + + K.row(counter_k_row) = + MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)- + V.row(idx3).transpose() * V.row(idx4), + num_k_columns, + num_lambda); + ++counter_k_row; + } + } + } + + // Construct the second part of the K matrix corresponding to (t_ii,t_jk) for + // j==k. + for (int idx1 = num_m_rows; idx1 < num_tt_variables; ++idx1) { + for (int idx2 = idx1 + 1; idx2 < num_tt_variables; ++idx2) { + unsigned int i = ij_index(idx1, 0); + unsigned int j = ij_index(idx2, 0); + unsigned int k = ij_index(idx2, 1); + + int idx3 = IJToPointIndex(i, j, num_points); + int idx4 = IJToPointIndex(i, k, num_points); + + K.row(counter_k_row) = + MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2)- + V.row(idx3).transpose() * V.row(idx4), + num_k_columns, + num_lambda); + ++counter_k_row; + } + } + Vec L_sq = K.jacobiSvd(Eigen::ComputeFullV).matrixV().col(num_k_columns - 1); + + // Pivot on the largest element for numerical stability. Afterwards recover + // the sign of the lambda solution. + double max_L_sq_value = fabs(L_sq(IJToIndex(0, 0, num_lambda))); + int max_L_sq_index = 1; + for (int i = 1; i < num_lambda; ++i) { + double abs_sq_value = fabs(L_sq(IJToIndex(i, i, num_lambda))); + if (max_L_sq_value < abs_sq_value) { + max_L_sq_value = abs_sq_value; + max_L_sq_index = i; + } + } + // Ensure positiveness of the largest value corresponding to lambda_ii. + L_sq = L_sq * Sign(L_sq(IJToIndex(max_L_sq_index, + max_L_sq_index, + num_lambda))); + + Vec L(num_lambda); + L(max_L_sq_index) = sqrt(L_sq(IJToIndex(max_L_sq_index, + max_L_sq_index, + num_lambda))); + + for (int i = 0; i < num_lambda; ++i) { + if (i != max_L_sq_index) { + L(i) = L_sq(IJToIndex(max_L_sq_index, i, num_lambda)) / L(max_L_sq_index); + } + } + + // Correct the scale using the fact that the last constraint is equal to 1. + L = L / (V.row(num_m_columns - 1).dot(L)); + Vec X = V * L; + + // Recover the distances from the camera center to the 3D points Q. + Vec d(num_points); + d.setZero(); + for (int c_point = num_m_rows; c_point < num_tt_variables; ++c_point) { + d(c_point - num_m_rows) = sqrt(X(c_point)); + } + + // Create the 3D points in the camera system. + Mat X_cam(3, num_points); + for (int c_point = 0; c_point < num_points; ++c_point) { + X_cam.col(c_point) = d(c_point) * x_camera_unit.col(c_point); + } + // Recover the camera translation and rotation. + AbsoluteOrientation(X_world, X_cam, R, t); +} + +// Selects 4 virtual control points using mean and PCA. +static void SelectControlPoints(const Mat3X &X_world, + Mat *X_centered, + Mat34 *X_control_points) { + size_t num_points = X_world.cols(); + + // The first virtual control point, C0, is the centroid. + Vec mean, variance; + MeanAndVarianceAlongRows(X_world, &mean, &variance); + X_control_points->col(0) = mean; + + // Computes PCA + X_centered->resize(3, num_points); + for (size_t c = 0; c < num_points; c++) { + X_centered->col(c) = X_world.col(c) - mean; + } + Mat3 X_centered_sq = (*X_centered) * X_centered->transpose(); + Eigen::JacobiSVD<Mat3> X_centered_sq_svd(X_centered_sq, Eigen::ComputeFullU); + Vec3 w = X_centered_sq_svd.singularValues(); + Mat3 u = X_centered_sq_svd.matrixU(); + for (size_t c = 0; c < 3; c++) { + double k = sqrt(w(c) / num_points); + X_control_points->col(c + 1) = mean + k * u.col(c); + } +} + +// Computes the barycentric coordinates for all real points +static void ComputeBarycentricCoordinates(const Mat3X &X_world_centered, + const Mat34 &X_control_points, + Mat4X *alphas) { + size_t num_points = X_world_centered.cols(); + Mat3 C2; + for (size_t c = 1; c < 4; c++) { + C2.col(c-1) = X_control_points.col(c) - X_control_points.col(0); + } + + Mat3 C2inv = C2.inverse(); + Mat3X a = C2inv * X_world_centered; + + alphas->resize(4, num_points); + alphas->setZero(); + alphas->block(1, 0, 3, num_points) = a; + for (size_t c = 0; c < num_points; c++) { + (*alphas)(0, c) = 1.0 - alphas->col(c).sum(); + } +} + +// Estimates the coordinates of all real points in the camera coordinate frame +static void ComputePointsCoordinatesInCameraFrame( + const Mat4X &alphas, + const Vec4 &betas, + const Eigen::Matrix<double, 12, 12> &U, + Mat3X *X_camera) { + size_t num_points = alphas.cols(); + + // Estimates the control points in the camera reference frame. + Mat34 C2b; C2b.setZero(); + for (size_t cu = 0; cu < 4; cu++) { + for (size_t c = 0; c < 4; c++) { + C2b.col(c) += betas(cu) * U.block(11 - cu, c * 3, 1, 3).transpose(); + } + } + + // Estimates the 3D points in the camera reference frame + X_camera->resize(3, num_points); + for (size_t c = 0; c < num_points; c++) { + X_camera->col(c) = C2b * alphas.col(c); + } + + // Check the sign of the z coordinate of the points (should be positive) + uint num_z_neg = 0; + for (size_t i = 0; i < X_camera->cols(); ++i) { + if ((*X_camera)(2, i) < 0) { + num_z_neg++; + } + } + + // If more than 50% of z are negative, we change the signs + if (num_z_neg > 0.5 * X_camera->cols()) { + C2b = -C2b; + *X_camera = -(*X_camera); + } +} + +bool EuclideanResectionEPnP(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t) { + CHECK(x_camera.cols() == X_world.cols()); + CHECK(x_camera.cols() > 3); + size_t num_points = X_world.cols(); + + // Select the control points. + Mat34 X_control_points; + Mat X_centered; + SelectControlPoints(X_world, &X_centered, &X_control_points); + + // Compute the barycentric coordinates. + Mat4X alphas(4, num_points); + ComputeBarycentricCoordinates(X_centered, X_control_points, &alphas); + + // Estimates the M matrix with the barycentric coordinates + Mat M(2 * num_points, 12); + Eigen::Matrix<double, 2, 12> sub_M; + for (size_t c = 0; c < num_points; c++) { + double a0 = alphas(0, c); + double a1 = alphas(1, c); + double a2 = alphas(2, c); + double a3 = alphas(3, c); + double ui = x_camera(0, c); + double vi = x_camera(1, c); + M.block(2*c, 0, 2, 12) << a0, 0, + a0*(-ui), a1, 0, + a1*(-ui), a2, 0, + a2*(-ui), a3, 0, + a3*(-ui), 0, + a0, a0*(-vi), 0, + a1, a1*(-vi), 0, + a2, a2*(-vi), 0, + a3, a3*(-vi); + } + + // TODO(julien): Avoid the transpose by rewriting the u2.block() calls. + Eigen::JacobiSVD<Mat> MtMsvd(M.transpose()*M, Eigen::ComputeFullU); + Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose(); + + // Estimate the L matrix. + Eigen::Matrix<double, 6, 3> dv1; + Eigen::Matrix<double, 6, 3> dv2; + Eigen::Matrix<double, 6, 3> dv3; + Eigen::Matrix<double, 6, 3> dv4; + + dv1.row(0) = u2.block(11, 0, 1, 3) - u2.block(11, 3, 1, 3); + dv1.row(1) = u2.block(11, 0, 1, 3) - u2.block(11, 6, 1, 3); + dv1.row(2) = u2.block(11, 0, 1, 3) - u2.block(11, 9, 1, 3); + dv1.row(3) = u2.block(11, 3, 1, 3) - u2.block(11, 6, 1, 3); + dv1.row(4) = u2.block(11, 3, 1, 3) - u2.block(11, 9, 1, 3); + dv1.row(5) = u2.block(11, 6, 1, 3) - u2.block(11, 9, 1, 3); + dv2.row(0) = u2.block(10, 0, 1, 3) - u2.block(10, 3, 1, 3); + dv2.row(1) = u2.block(10, 0, 1, 3) - u2.block(10, 6, 1, 3); + dv2.row(2) = u2.block(10, 0, 1, 3) - u2.block(10, 9, 1, 3); + dv2.row(3) = u2.block(10, 3, 1, 3) - u2.block(10, 6, 1, 3); + dv2.row(4) = u2.block(10, 3, 1, 3) - u2.block(10, 9, 1, 3); + dv2.row(5) = u2.block(10, 6, 1, 3) - u2.block(10, 9, 1, 3); + dv3.row(0) = u2.block(9, 0, 1, 3) - u2.block(9, 3, 1, 3); + dv3.row(1) = u2.block(9, 0, 1, 3) - u2.block(9, 6, 1, 3); + dv3.row(2) = u2.block(9, 0, 1, 3) - u2.block(9, 9, 1, 3); + dv3.row(3) = u2.block(9, 3, 1, 3) - u2.block(9, 6, 1, 3); + dv3.row(4) = u2.block(9, 3, 1, 3) - u2.block(9, 9, 1, 3); + dv3.row(5) = u2.block(9, 6, 1, 3) - u2.block(9, 9, 1, 3); + dv4.row(0) = u2.block(8, 0, 1, 3) - u2.block(8, 3, 1, 3); + dv4.row(1) = u2.block(8, 0, 1, 3) - u2.block(8, 6, 1, 3); + dv4.row(2) = u2.block(8, 0, 1, 3) - u2.block(8, 9, 1, 3); + dv4.row(3) = u2.block(8, 3, 1, 3) - u2.block(8, 6, 1, 3); + dv4.row(4) = u2.block(8, 3, 1, 3) - u2.block(8, 9, 1, 3); + dv4.row(5) = u2.block(8, 6, 1, 3) - u2.block(8, 9, 1, 3); + + Eigen::Matrix<double, 6, 10> L; + for (size_t r = 0; r < 6; r++) { + L.row(r) << dv1.row(r).dot(dv1.row(r)), + 2.0 * dv1.row(r).dot(dv2.row(r)), + dv2.row(r).dot(dv2.row(r)), + 2.0 * dv1.row(r).dot(dv3.row(r)), + 2.0 * dv2.row(r).dot(dv3.row(r)), + dv3.row(r).dot(dv3.row(r)), + 2.0 * dv1.row(r).dot(dv4.row(r)), + 2.0 * dv2.row(r).dot(dv4.row(r)), + 2.0 * dv3.row(r).dot(dv4.row(r)), + dv4.row(r).dot(dv4.row(r)); + } + Vec6 rho; + rho << (X_control_points.col(0) - X_control_points.col(1)).squaredNorm(), + (X_control_points.col(0) - X_control_points.col(2)).squaredNorm(), + (X_control_points.col(0) - X_control_points.col(3)).squaredNorm(), + (X_control_points.col(1) - X_control_points.col(2)).squaredNorm(), + (X_control_points.col(1) - X_control_points.col(3)).squaredNorm(), + (X_control_points.col(2) - X_control_points.col(3)).squaredNorm(); + + // There are three possible solutions based on the three approximations of L + // (betas). Below, each one is solved for then the best one is chosen. + Mat3X X_camera; + Mat3 K; K.setIdentity(); + vector<Mat3> Rs(3); + vector<Vec3> ts(3); + Vec rmse(3); + + // At one point this threshold was 1e-3, and caused no end of problems in + // Blender by causing frames to not resect when they would have worked fine. + // When the resect failed, the projective followup is run leading to worse + // results, and often the dreaded "flipping" where objects get flipped + // between frames. Instead, disable the check for now, always succeeding. The + // ultimate check is always reprojection error anyway. + // + // TODO(keir): Decide if setting this to infinity, effectively disabling the + // check, is the right approach. So far this seems the case. + double kSuccessThreshold = std::numeric_limits<double>::max(); + + // Find the first possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_1 = [b00 b01 b02 b03] + Vec4 betas = Vec4::Zero(); + Eigen::Matrix<double, 6, 4> l_6x4; + for (size_t r = 0; r < 6; r++) { + l_6x4.row(r) << L(r, 0), L(r, 1), L(r, 3), L(r, 6); + } + Eigen::JacobiSVD<Mat> svd_of_l4(l_6x4, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec4 b4 = svd_of_l4.solve(rho); + if ((l_6x4 * b4).isApprox(rho, kSuccessThreshold)) { + if (b4(0) < 0) { + b4 = -b4; + } + b4(0) = std::sqrt(b4(0)); + betas << b4(0), b4(1) / b4(0), b4(2) / b4(0), b4(3) / b4(0); + ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); + AbsoluteOrientation(X_world, X_camera, &Rs[0], &ts[0]); + rmse(0) = RootMeanSquareError(x_camera, X_world, K, Rs[0], ts[0]); + } else { + LOG(ERROR) << "First approximation of beta not good enough."; + ts[0].setZero(); + rmse(0) = std::numeric_limits<double>::max(); + } + + // Find the second possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_2 = [b00 b01 b11] + betas.setZero(); + Eigen::Matrix<double, 6, 3> l_6x3; + l_6x3 = L.block(0, 0, 6, 3); + Eigen::JacobiSVD<Mat> svdOfL3(l_6x3, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec3 b3 = svdOfL3.solve(rho); + VLOG(2) << " rho = " << rho; + VLOG(2) << " l_6x3 * b3 = " << l_6x3 * b3; + if ((l_6x3 * b3).isApprox(rho, kSuccessThreshold)) { + if (b3(0) < 0) { + betas(0) = std::sqrt(-b3(0)); + betas(1) = (b3(2) < 0) ? std::sqrt(-b3(2)) : 0; + } else { + betas(0) = std::sqrt(b3(0)); + betas(1) = (b3(2) > 0) ? std::sqrt(b3(2)) : 0; + } + if (b3(1) < 0) { + betas(0) = -betas(0); + } + betas(2) = 0; + betas(3) = 0; + ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); + AbsoluteOrientation(X_world, X_camera, &Rs[1], &ts[1]); + rmse(1) = RootMeanSquareError(x_camera, X_world, K, Rs[1], ts[1]); + } else { + LOG(ERROR) << "Second approximation of beta not good enough."; + ts[1].setZero(); + rmse(1) = std::numeric_limits<double>::max(); + } + + // Find the third possible solution for R, t corresponding to: + // Betas = [b00 b01 b11 b02 b12 b22 b03 b13 b23 b33] + // Betas_approx_3 = [b00 b01 b11 b02 b12] + betas.setZero(); + Eigen::Matrix<double, 6, 5> l_6x5; + l_6x5 = L.block(0, 0, 6, 5); + Eigen::JacobiSVD<Mat> svdOfL5(l_6x5, + Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec5 b5 = svdOfL5.solve(rho); + if ((l_6x5 * b5).isApprox(rho, kSuccessThreshold)) { + if (b5(0) < 0) { + betas(0) = std::sqrt(-b5(0)); + if (b5(2) < 0) { + betas(1) = std::sqrt(-b5(2)); + } else { + b5(2) = 0; + } + } else { + betas(0) = std::sqrt(b5(0)); + if (b5(2) > 0) { + betas(1) = std::sqrt(b5(2)); + } else { + b5(2) = 0; + } + } + if (b5(1) < 0) { + betas(0) = -betas(0); + } + betas(2) = b5(3) / betas(0); + betas(3) = 0; + ComputePointsCoordinatesInCameraFrame(alphas, betas, u2, &X_camera); + AbsoluteOrientation(X_world, X_camera, &Rs[2], &ts[2]); + rmse(2) = RootMeanSquareError(x_camera, X_world, K, Rs[2], ts[2]); + } else { + LOG(ERROR) << "Third approximation of beta not good enough."; + ts[2].setZero(); + rmse(2) = std::numeric_limits<double>::max(); + } + + // Finally, with all three solutions, select the (R, t) with the best RMSE. + VLOG(2) << "RMSE for solution 0: " << rmse(0); + VLOG(2) << "RMSE for solution 1: " << rmse(1); + VLOG(2) << "RMSE for solution 2: " << rmse(2); + size_t n = 0; + if (rmse(1) < rmse(0)) { + n = 1; + } + if (rmse(2) < rmse(n)) { + n = 2; + } + if (rmse(n) == std::numeric_limits<double>::max()) { + LOG(ERROR) << "All three possibilities failed. Reporting failure."; + return false; + } + + VLOG(1) << "RMSE for best solution #" << n << ": " << rmse(n); + *R = Rs[n]; + *t = ts[n]; + + // TODO(julien): Improve the solutions with non-linear refinement. + return true; +} + +/* + + Straight from the paper: + http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf + + function [R T] = ppnp(P,S,tol) + % input + % P : matrix (nx3) image coordinates in camera reference [u v 1] + % S : matrix (nx3) coordinates in world reference [X Y Z] + % tol: exit threshold + % + % output + % R : matrix (3x3) rotation (world-to-camera) + % T : vector (3x1) translation (world-to-camera) + % + n = size(P,1); + Z = zeros(n); + e = ones(n,1); + A = eye(n)-((e*e’)./n); + II = e./n; + err = +Inf; + E_old = 1000*ones(n,3); + while err>tol + [U,˜,V] = svd(P’*Z*A*S); + VT = V’; + R=U*[1 0 0; 0 1 0; 0 0 det(U*VT)]*VT; + PR = P*R; + c = (S-Z*PR)’*II; + Y = S-e*c’; + Zmindiag = diag(PR*Y’)./(sum(P.*P,2)); + Zmindiag(Zmindiag<0)=0; + Z = diag(Zmindiag); + E = Y-Z*PR; + err = norm(E-E_old,’fro’); + E_old = E; + end + T = -R*c; + end + + */ +// TODO(keir): Re-do all the variable names and add comments matching the paper. +// This implementation has too much of the terseness of the original. On the +// other hand, it did work on the first try. +bool EuclideanResectionPPnP(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t) { + int n = x_camera.cols(); + Mat Z = Mat::Zero(n, n); + Vec e = Vec::Ones(n); + Mat A = Mat::Identity(n, n) - (e * e.transpose() / n); + Vec II = e / n; + + Mat P(n, 3); + P.col(0) = x_camera.row(0); + P.col(1) = x_camera.row(1); + P.col(2).setConstant(1.0); + + Mat S = X_world.transpose(); + + double error = std::numeric_limits<double>::infinity(); + Mat E_old = 1000 * Mat::Ones(n, 3); + + Vec3 c; + Mat E(n, 3); + + int iteration = 0; + double tolerance = 1e-5; + // TODO(keir): The limit of 100 can probably be reduced, but this will require + // some investigation. + while (error > tolerance && iteration < 100) { + Mat3 tmp = P.transpose() * Z * A * S; + Eigen::JacobiSVD<Mat3> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV); + Mat3 U = svd.matrixU(); + Mat3 VT = svd.matrixV().transpose(); + Vec3 s; + s << 1, 1, (U * VT).determinant(); + *R = U * s.asDiagonal() * VT; + Mat PR = P * *R; // n x 3 + c = (S - Z*PR).transpose() * II; + Mat Y = S - e*c.transpose(); // n x 3 + Vec Zmindiag = (PR * Y.transpose()).diagonal() + .cwiseQuotient(P.rowwise().squaredNorm()); + for (int i = 0; i < n; ++i) { + Zmindiag[i] = std::max(Zmindiag[i], 0.0); + } + Z = Zmindiag.asDiagonal(); + E = Y - Z*PR; + error = (E - E_old).norm(); + LOG(INFO) << "PPnP error(" << (iteration++) << "): " << error; + E_old = E; + } + *t = -*R*c; + + // TODO(keir): Figure out what the failure cases are. Is it too many + // iterations? Spend some time going through the math figuring out if there + // is some way to detect that the algorithm is going crazy, and return false. + return true; +} + + +} // namespace resection +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/euclidean_resection.h b/intern/libmv/libmv/multiview/euclidean_resection.h new file mode 100644 index 00000000000..28eae92611c --- /dev/null +++ b/intern/libmv/libmv/multiview/euclidean_resection.h @@ -0,0 +1,148 @@ +// Copyright (c) 2010 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_MULTIVIEW_EUCLIDEAN_RESECTION_H_ +#define LIBMV_MULTIVIEW_EUCLIDEAN_RESECTION_H_ + +#include "libmv/numeric/numeric.h" +#include "libmv/multiview/projection.h" + +namespace libmv { +namespace euclidean_resection { + +enum ResectionMethod { + RESECTION_ANSAR_DANIILIDIS, + + // The "EPnP" algorithm by Lepetit et al. + // http://cvlab.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf + RESECTION_EPNP, + + // The Procrustes PNP algorithm ("PPnP") + // http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf + RESECTION_PPNP +}; + +/** + * Computes the extrinsic parameters, R and t for a calibrated camera + * from 4 or more 3D points and their normalized images. + * + * \param x_camera Image points in normalized camera coordinates e.g. x_camera + * = inv(K) * x_image. + * \param X_world 3D points in the world coordinate system + * \param R Solution for the camera rotation matrix + * \param t Solution for the camera translation vector + * \param method The resection method to use. + */ +bool EuclideanResection(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t, + ResectionMethod method = RESECTION_EPNP); + +/** + * Computes the extrinsic parameters, R and t for a calibrated camera + * from 4 or more 3D points and their images. + * + * \param x_image Image points in non-normalized image coordinates. The + * coordates are laid out one per row. The matrix can be Nx2 + * or Nx3 for euclidean or homogenous 2D coordinates. + * \param X_world 3D points in the world coordinate system + * \param K Intrinsic parameters camera matrix + * \param R Solution for the camera rotation matrix + * \param t Solution for the camera translation vector + * \param method Resection method + */ +bool EuclideanResection(const Mat &x_image, + const Mat3X &X_world, + const Mat3 &K, + Mat3 *R, Vec3 *t, + ResectionMethod method = RESECTION_EPNP); + +/** + * The absolute orientation algorithm recovers the transformation between a set + * of 3D points, X and Xp such that: + * + * Xp = R*X + t + * + * The recovery of the absolute orientation is implemented after this article: + * Horn, Hilden, "Closed-form solution of absolute orientation using + * orthonormal matrices" + */ +void AbsoluteOrientation(const Mat3X &X, + const Mat3X &Xp, + Mat3 *R, + Vec3 *t); + +/** + * Computes the extrinsic parameters, R and t for a calibrated camera from 4 or + * more 3D points and their images. + * + * \param x_camera Image points in normalized camera coordinates, e.g. + * x_camera=inv(K)*x_image + * \param X_world 3D points in the world coordinate system + * \param R Solution for the camera rotation matrix + * \param t Solution for the camera translation vector + * + * This is the algorithm described in: "Linear Pose Estimation from Points or + * Lines", by Ansar, A. and Daniilidis, PAMI 2003. vol. 25, no. 5. + */ +void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t); +/** + * Computes the extrinsic parameters, R and t for a calibrated camera from 4 or + * more 3D points and their images. + * + * \param x_camera Image points in normalized camera coordinates, + * e.g. x_camera = inv(K) * x_image + * \param X_world 3D points in the world coordinate system + * \param R Solution for the camera rotation matrix + * \param t Solution for the camera translation vector + * + * This is the algorithm described in: + * "{EP$n$P: An Accurate $O(n)$ Solution to the P$n$P Problem", by V. Lepetit + * and F. Moreno-Noguer and P. Fua, IJCV 2009. vol. 81, no. 2 + * \note: the non-linear optimization is not implemented here. + */ +bool EuclideanResectionEPnP(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t); + +/** + * Computes the extrinsic parameters, R and t for a calibrated camera from 4 or + * more 3D points and their images. + * + * \param x_camera Image points in normalized camera coordinates, + * e.g. x_camera = inv(K) * x_image + * \param X_world 3D points in the world coordinate system + * \param R Solution for the camera rotation matrix + * \param t Solution for the camera translation vector + * + * Straight from the paper: + * http://www.diegm.uniud.it/fusiello/papers/3dimpvt12-b.pdf + */ +bool EuclideanResectionPPnP(const Mat2X &x_camera, + const Mat3X &X_world, + Mat3 *R, Vec3 *t); + +} // namespace euclidean_resection +} // namespace libmv + + +#endif /* LIBMV_MULTIVIEW_EUCLIDEAN_RESECTION_H_ */ diff --git a/intern/libmv/libmv/multiview/euclidean_resection_test.cc b/intern/libmv/libmv/multiview/euclidean_resection_test.cc new file mode 100644 index 00000000000..378837d3d2d --- /dev/null +++ b/intern/libmv/libmv/multiview/euclidean_resection_test.cc @@ -0,0 +1,237 @@ +// Copyright (c) 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/euclidean_resection.h" +#include "libmv/numeric/numeric.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/projection.h" +#include "testing/testing.h" + +using namespace libmv::euclidean_resection; +using namespace libmv; + +// Generates all necessary inputs and expected outputs for EuclideanResection. +static void CreateCameraSystem(const Mat3& KK, + const Mat3X& x_image, + const Vec& X_distances, + const Mat3& R_input, + const Vec3& T_input, + Mat2X *x_camera, + Mat3X *X_world, + Mat3 *R_expected, + Vec3 *T_expected) { + int num_points = x_image.cols(); + + Mat3X x_unit_cam(3, num_points); + x_unit_cam = KK.inverse() * x_image; + + // Create normalized camera coordinates to be used as an input to the PnP + // function, instead of using NormalizeColumnVectors(&x_unit_cam). + *x_camera = x_unit_cam.block(0, 0, 2, num_points); + for (int i = 0; i < num_points; ++i) { + x_unit_cam.col(i).normalize(); + } + + // Create the 3D points in the camera system. + Mat X_camera(3, num_points); + for (int i = 0; i < num_points; ++i) { + X_camera.col(i) = X_distances(i) * x_unit_cam.col(i); + } + + // Apply the transformation to the camera 3D points + Mat translation_matrix(3, num_points); + translation_matrix.row(0).setConstant(T_input(0)); + translation_matrix.row(1).setConstant(T_input(1)); + translation_matrix.row(2).setConstant(T_input(2)); + + *X_world = R_input * X_camera + translation_matrix; + + // Create the expected result for comparison. + *R_expected = R_input.transpose(); + *T_expected = *R_expected * (-T_input); +}; + +TEST(AbsoluteOrientation, QuaternionSolution) { + int num_points = 4; + Mat X; + Mat Xp; + X = 100 * Mat::Random(3, num_points); + + // Create a random translation and rotation. + Mat3 R_input; + R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()); + + Vec3 t_input; + t_input.setRandom(); + t_input = 100 * t_input; + + Mat translation_matrix(3, num_points); + translation_matrix.row(0).setConstant(t_input(0)); + translation_matrix.row(1).setConstant(t_input(1)); + translation_matrix.row(2).setConstant(t_input(2)); + + // Create the transformed 3D points Xp as Xp = R * X + t. + Xp = R_input * X + translation_matrix; + + // Output variables. + Mat3 R; + Vec3 t; + + AbsoluteOrientation(X, Xp, &R, &t); + + EXPECT_MATRIX_NEAR(t, t_input, 1e-6); + EXPECT_MATRIX_NEAR(R, R_input, 1e-8); +} + +TEST(EuclideanResection, Points4KnownImagePointsRandomTranslationRotation) { + // In this test only the translation and rotation are random. The image + // points are selected from a real case and are well conditioned. + Vec2i image_dimensions; + image_dimensions << 1600, 1200; + + Mat3 KK; + KK << 2796, 0, 804, + 0 , 2796, 641, + 0, 0, 1; + + // The real image points. + int num_points = 4; + Mat3X x_image(3, num_points); + x_image << 1164.06, 734.948, 749.599, 430.727, + 681.386, 844.59, 496.315, 580.775, + 1, 1, 1, 1; + + + // A vector of the 4 distances to the 3D points. + Vec X_distances = 100 * Vec::Random(num_points).array().abs(); + + // Create the random camera motion R and t that resection should recover. + Mat3 R_input; + R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()); + + Vec3 T_input; + T_input.setRandom(); + T_input = 100 * T_input; + + // Create the camera system, also getting the expected result of the + // transformation. + Mat3 R_expected; + Vec3 T_expected; + Mat3X X_world; + Mat2X x_camera; + CreateCameraSystem(KK, x_image, X_distances, R_input, T_input, + &x_camera, &X_world, &R_expected, &T_expected); + + // Finally, run the code under test. + Mat3 R_output; + Vec3 T_output; + EuclideanResection(x_camera, X_world, + &R_output, &T_output, + RESECTION_ANSAR_DANIILIDIS); + + EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5); + EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7); + + // For now, the EPnP doesn't have a non-linear optimization step and so is + // not precise enough with only 4 points. + // + // TODO(jmichot): Reenable this test when there is nonlinear refinement. +#if 0 + R_output.setIdentity(); + T_output.setZero(); + + EuclideanResection(x_camera, X_world, + &R_output, &T_output, + RESECTION_EPNP); + + EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5); + EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7);*/ +#endif +} + +// TODO(jmichot): Reduce the code duplication here with the code above. +TEST(EuclideanResection, Points6AllRandomInput) { + Mat3 KK; + KK << 2796, 0, 804, + 0 , 2796, 641, + 0, 0, 1; + + // Create random image points for a 1600x1200 image. + int w = 1600; + int h = 1200; + int num_points = 6; + Mat3X x_image(3, num_points); + x_image.row(0) = w * Vec::Random(num_points).array().abs(); + x_image.row(1) = h * Vec::Random(num_points).array().abs(); + x_image.row(2).setOnes(); + + // Normalized camera coordinates to be used as an input to the PnP function. + Mat2X x_camera; + Vec X_distances = 100 * Vec::Random(num_points).array().abs(); + + // Create the random camera motion R and t that resection should recover. + Mat3 R_input; + R_input = Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitY()) + * Eigen::AngleAxisd(rand(), Eigen::Vector3d::UnitZ()); + + Vec3 T_input; + T_input.setRandom(); + T_input = 100 * T_input; + + // Create the camera system. + Mat3 R_expected; + Vec3 T_expected; + Mat3X X_world; + CreateCameraSystem(KK, x_image, X_distances, R_input, T_input, + &x_camera, &X_world, &R_expected, &T_expected); + + // Test each of the resection methods. + { + Mat3 R_output; + Vec3 T_output; + EuclideanResection(x_camera, X_world, + &R_output, &T_output, + RESECTION_ANSAR_DANIILIDIS); + EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5); + EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7); + } + { + Mat3 R_output; + Vec3 T_output; + EuclideanResection(x_camera, X_world, + &R_output, &T_output, + RESECTION_EPNP); + EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5); + EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7); + } + { + Mat3 R_output; + Vec3 T_output; + EuclideanResection(x_image, X_world, KK, + &R_output, &T_output); + EXPECT_MATRIX_NEAR(T_output, T_expected, 1e-5); + EXPECT_MATRIX_NEAR(R_output, R_expected, 1e-7); + } +} diff --git a/intern/libmv/libmv/multiview/fundamental.cc b/intern/libmv/libmv/multiview/fundamental.cc new file mode 100644 index 00000000000..ea8594c8cc0 --- /dev/null +++ b/intern/libmv/libmv/multiview/fundamental.cc @@ -0,0 +1,544 @@ +// Copyright (c) 2007, 2008 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/fundamental.h" + +#include "ceres/ceres.h" +#include "libmv/logging/logging.h" +#include "libmv/numeric/numeric.h" +#include "libmv/numeric/poly.h" +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/triangulation.h" + +namespace libmv { + +static void EliminateRow(const Mat34 &P, int row, Mat *X) { + X->resize(2, 4); + + int first_row = (row + 1) % 3; + int second_row = (row + 2) % 3; + + for (int i = 0; i < 4; ++i) { + (*X)(0, i) = P(first_row, i); + (*X)(1, i) = P(second_row, i); + } +} + +void ProjectionsFromFundamental(const Mat3 &F, Mat34 *P1, Mat34 *P2) { + *P1 << Mat3::Identity(), Vec3::Zero(); + Vec3 e2; + Mat3 Ft = F.transpose(); + Nullspace(&Ft, &e2); + *P2 << CrossProductMatrix(e2) * F, e2; +} + +// Addapted from vgg_F_from_P. +void FundamentalFromProjections(const Mat34 &P1, const Mat34 &P2, Mat3 *F) { + Mat X[3]; + Mat Y[3]; + Mat XY; + + for (int i = 0; i < 3; ++i) { + EliminateRow(P1, i, X + i); + EliminateRow(P2, i, Y + i); + } + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + VerticalStack(X[j], Y[i], &XY); + (*F)(i, j) = XY.determinant(); + } + } +} + +// HZ 11.1 pag.279 (x1 = x, x2 = x') +// http://www.cs.unc.edu/~marc/tutorial/node54.html +static double EightPointSolver(const Mat &x1, const Mat &x2, Mat3 *F) { + DCHECK_EQ(x1.rows(), 2); + DCHECK_GE(x1.cols(), 8); + DCHECK_EQ(x1.rows(), x2.rows()); + DCHECK_EQ(x1.cols(), x2.cols()); + + int n = x1.cols(); + Mat A(n, 9); + for (int i = 0; i < n; ++i) { + A(i, 0) = x2(0, i) * x1(0, i); + A(i, 1) = x2(0, i) * x1(1, i); + A(i, 2) = x2(0, i); + A(i, 3) = x2(1, i) * x1(0, i); + A(i, 4) = x2(1, i) * x1(1, i); + A(i, 5) = x2(1, i); + A(i, 6) = x1(0, i); + A(i, 7) = x1(1, i); + A(i, 8) = 1; + } + + Vec9 f; + double smaller_singular_value = Nullspace(&A, &f); + *F = Map<RMat3>(f.data()); + return smaller_singular_value; +} + +// HZ 11.1.1 pag.280 +void EnforceFundamentalRank2Constraint(Mat3 *F) { + Eigen::JacobiSVD<Mat3> USV(*F, Eigen::ComputeFullU | Eigen::ComputeFullV); + Vec3 d = USV.singularValues(); + d(2) = 0.0; + *F = USV.matrixU() * d.asDiagonal() * USV.matrixV().transpose(); +} + +// HZ 11.2 pag.281 (x1 = x, x2 = x') +double NormalizedEightPointSolver(const Mat &x1, + const Mat &x2, + Mat3 *F) { + DCHECK_EQ(x1.rows(), 2); + DCHECK_GE(x1.cols(), 8); + DCHECK_EQ(x1.rows(), x2.rows()); + DCHECK_EQ(x1.cols(), x2.cols()); + + // Normalize the data. + Mat3 T1, T2; + PreconditionerFromPoints(x1, &T1); + PreconditionerFromPoints(x2, &T2); + Mat x1_normalized, x2_normalized; + ApplyTransformationToPoints(x1, T1, &x1_normalized); + ApplyTransformationToPoints(x2, T2, &x2_normalized); + + // Estimate the fundamental matrix. + double smaller_singular_value = + EightPointSolver(x1_normalized, x2_normalized, F); + EnforceFundamentalRank2Constraint(F); + + // Denormalize the fundamental matrix. + *F = T2.transpose() * (*F) * T1; + + return smaller_singular_value; +} + +// Seven-point algorithm. +// http://www.cs.unc.edu/~marc/tutorial/node55.html +double FundamentalFrom7CorrespondencesLinear(const Mat &x1, + const Mat &x2, + std::vector<Mat3> *F) { + DCHECK_EQ(x1.rows(), 2); + DCHECK_EQ(x1.cols(), 7); + DCHECK_EQ(x1.rows(), x2.rows()); + DCHECK_EQ(x2.cols(), x2.cols()); + + // Build a 9 x n matrix from point matches, where each row is equivalent to + // the equation x'T*F*x = 0 for a single correspondence pair (x', x). The + // domain of the matrix is a 9 element vector corresponding to F. The + // nullspace should be rank two; the two dimensions correspond to the set of + // F matrices satisfying the epipolar geometry. + Matrix<double, 7, 9> A; + for (int ii = 0; ii < 7; ++ii) { + A(ii, 0) = x1(0, ii) * x2(0, ii); // 0 represents x coords, + A(ii, 1) = x1(1, ii) * x2(0, ii); // 1 represents y coords. + A(ii, 2) = x2(0, ii); + A(ii, 3) = x1(0, ii) * x2(1, ii); + A(ii, 4) = x1(1, ii) * x2(1, ii); + A(ii, 5) = x2(1, ii); + A(ii, 6) = x1(0, ii); + A(ii, 7) = x1(1, ii); + A(ii, 8) = 1.0; + } + + // Find the two F matrices in the nullspace of A. + Vec9 f1, f2; + double s = Nullspace2(&A, &f1, &f2); + Mat3 F1 = Map<RMat3>(f1.data()); + Mat3 F2 = Map<RMat3>(f2.data()); + + // Then, use the condition det(F) = 0 to determine F. In other words, solve + // det(F1 + a*F2) = 0 for a. + double a = F1(0, 0), j = F2(0, 0), + b = F1(0, 1), k = F2(0, 1), + c = F1(0, 2), l = F2(0, 2), + d = F1(1, 0), m = F2(1, 0), + e = F1(1, 1), n = F2(1, 1), + f = F1(1, 2), o = F2(1, 2), + g = F1(2, 0), p = F2(2, 0), + h = F1(2, 1), q = F2(2, 1), + i = F1(2, 2), r = F2(2, 2); + + // Run fundamental_7point_coeffs.py to get the below coefficients. + // The coefficients are in ascending powers of alpha, i.e. P[N]*x^N. + double P[4] = { + a*e*i + b*f*g + c*d*h - a*f*h - b*d*i - c*e*g, + a*e*r + a*i*n + b*f*p + b*g*o + c*d*q + c*h*m + d*h*l + e*i*j + f*g*k - + a*f*q - a*h*o - b*d*r - b*i*m - c*e*p - c*g*n - d*i*k - e*g*l - f*h*j, + a*n*r + b*o*p + c*m*q + d*l*q + e*j*r + f*k*p + g*k*o + h*l*m + i*j*n - + a*o*q - b*m*r - c*n*p - d*k*r - e*l*p - f*j*q - g*l*n - h*j*o - i*k*m, + j*n*r + k*o*p + l*m*q - j*o*q - k*m*r - l*n*p, + }; + + // Solve for the roots of P[3]*x^3 + P[2]*x^2 + P[1]*x + P[0] = 0. + double roots[3]; + int num_roots = SolveCubicPolynomial(P, roots); + + // Build the fundamental matrix for each solution. + for (int kk = 0; kk < num_roots; ++kk) { + F->push_back(F1 + roots[kk] * F2); + } + return s; +} + +double FundamentalFromCorrespondences7Point(const Mat &x1, + const Mat &x2, + std::vector<Mat3> *F) { + DCHECK_EQ(x1.rows(), 2); + DCHECK_GE(x1.cols(), 7); + DCHECK_EQ(x1.rows(), x2.rows()); + DCHECK_EQ(x1.cols(), x2.cols()); + + // Normalize the data. + Mat3 T1, T2; + PreconditionerFromPoints(x1, &T1); + PreconditionerFromPoints(x2, &T2); + Mat x1_normalized, x2_normalized; + ApplyTransformationToPoints(x1, T1, &x1_normalized); + ApplyTransformationToPoints(x2, T2, &x2_normalized); + + // Estimate the fundamental matrix. + double smaller_singular_value = + FundamentalFrom7CorrespondencesLinear(x1_normalized, x2_normalized, &(*F)); + + for (int k = 0; k < F->size(); ++k) { + Mat3 & Fmat = (*F)[k]; + // Denormalize the fundamental matrix. + Fmat = T2.transpose() * Fmat * T1; + } + return smaller_singular_value; +} + +void NormalizeFundamental(const Mat3 &F, Mat3 *F_normalized) { + *F_normalized = F / FrobeniusNorm(F); + if ((*F_normalized)(2, 2) < 0) { + *F_normalized *= -1; + } +} + +double SampsonDistance(const Mat &F, const Vec2 &x1, const Vec2 &x2) { + Vec3 x(x1(0), x1(1), 1.0); + Vec3 y(x2(0), x2(1), 1.0); + + Vec3 F_x = F * x; + Vec3 Ft_y = F.transpose() * y; + double y_F_x = y.dot(F_x); + + return Square(y_F_x) / ( F_x.head<2>().squaredNorm() + + Ft_y.head<2>().squaredNorm()); +} + +double SymmetricEpipolarDistance(const Mat &F, const Vec2 &x1, const Vec2 &x2) { + Vec3 x(x1(0), x1(1), 1.0); + Vec3 y(x2(0), x2(1), 1.0); + + Vec3 F_x = F * x; + Vec3 Ft_y = F.transpose() * y; + double y_F_x = y.dot(F_x); + + return Square(y_F_x) * ( 1 / F_x.head<2>().squaredNorm() + + 1 / Ft_y.head<2>().squaredNorm()); +} + +// HZ 9.6 pag 257 (formula 9.12) +void EssentialFromFundamental(const Mat3 &F, + const Mat3 &K1, + const Mat3 &K2, + Mat3 *E) { + *E = K2.transpose() * F * K1; +} + +// HZ 9.6 pag 257 (formula 9.12) +// Or http://ai.stanford.edu/~birch/projective/node20.html +void FundamentalFromEssential(const Mat3 &E, + const Mat3 &K1, + const Mat3 &K2, + Mat3 *F) { + *F = K2.inverse().transpose() * E * K1.inverse(); +} + +void RelativeCameraMotion(const Mat3 &R1, + const Vec3 &t1, + const Mat3 &R2, + const Vec3 &t2, + Mat3 *R, + Vec3 *t) { + *R = R2 * R1.transpose(); + *t = t2 - (*R) * t1; +} + +// HZ 9.6 pag 257 +void EssentialFromRt(const Mat3 &R1, + const Vec3 &t1, + const Mat3 &R2, + const Vec3 &t2, + Mat3 *E) { + Mat3 R; + Vec3 t; + RelativeCameraMotion(R1, t1, R2, t2, &R, &t); + Mat3 Tx = CrossProductMatrix(t); + *E = Tx * R; +} + +// HZ 9.6 pag 259 (Result 9.19) +void MotionFromEssential(const Mat3 &E, + std::vector<Mat3> *Rs, + std::vector<Vec3> *ts) { + Eigen::JacobiSVD<Mat3> USV(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + Mat3 U = USV.matrixU(); + Mat3 Vt = USV.matrixV().transpose(); + + // Last column of U is undetermined since d = (a a 0). + if (U.determinant() < 0) { + U.col(2) *= -1; + } + // Last row of Vt is undetermined since d = (a a 0). + if (Vt.determinant() < 0) { + Vt.row(2) *= -1; + } + + Mat3 W; + W << 0, -1, 0, + 1, 0, 0, + 0, 0, 1; + + Mat3 U_W_Vt = U * W * Vt; + Mat3 U_Wt_Vt = U * W.transpose() * Vt; + + Rs->resize(4); + (*Rs)[0] = U_W_Vt; + (*Rs)[1] = U_W_Vt; + (*Rs)[2] = U_Wt_Vt; + (*Rs)[3] = U_Wt_Vt; + + ts->resize(4); + (*ts)[0] = U.col(2); + (*ts)[1] = -U.col(2); + (*ts)[2] = U.col(2); + (*ts)[3] = -U.col(2); +} + +int MotionFromEssentialChooseSolution(const std::vector<Mat3> &Rs, + const std::vector<Vec3> &ts, + const Mat3 &K1, + const Vec2 &x1, + const Mat3 &K2, + const Vec2 &x2) { + DCHECK_EQ(4, Rs.size()); + DCHECK_EQ(4, ts.size()); + + Mat34 P1, P2; + Mat3 R1; + Vec3 t1; + R1.setIdentity(); + t1.setZero(); + P_From_KRt(K1, R1, t1, &P1); + for (int i = 0; i < 4; ++i) { + const Mat3 &R2 = Rs[i]; + const Vec3 &t2 = ts[i]; + P_From_KRt(K2, R2, t2, &P2); + Vec3 X; + TriangulateDLT(P1, x1, P2, x2, &X); + double d1 = Depth(R1, t1, X); + double d2 = Depth(R2, t2, X); + // Test if point is front to the two cameras. + if (d1 > 0 && d2 > 0) { + return i; + } + } + return -1; +} + +bool MotionFromEssentialAndCorrespondence(const Mat3 &E, + const Mat3 &K1, + const Vec2 &x1, + const Mat3 &K2, + const Vec2 &x2, + Mat3 *R, + Vec3 *t) { + std::vector<Mat3> Rs; + std::vector<Vec3> ts; + MotionFromEssential(E, &Rs, &ts); + int solution = MotionFromEssentialChooseSolution(Rs, ts, K1, x1, K2, x2); + if (solution >= 0) { + *R = Rs[solution]; + *t = ts[solution]; + return true; + } else { + return false; + } +} + +void FundamentalToEssential(const Mat3 &F, Mat3 *E) { + Eigen::JacobiSVD<Mat3> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + + // See Hartley & Zisserman page 294, result 11.1, which shows how to get the + // closest essential matrix to a matrix that is "almost" an essential matrix. + double a = svd.singularValues()(0); + double b = svd.singularValues()(1); + double s = (a + b) / 2.0; + + LG << "Initial reconstruction's rotation is non-euclidean by " + << (((a - b) / std::max(a, b)) * 100) << "%; singular values:" + << svd.singularValues().transpose(); + + Vec3 diag; + diag << s, s, 0; + + *E = svd.matrixU() * diag.asDiagonal() * svd.matrixV().transpose(); +} + +// Default settings for fundamental estimation which should be suitable +// for a wide range of use cases. +EstimateFundamentalOptions::EstimateFundamentalOptions(void) : + max_num_iterations(50), + expected_average_symmetric_distance(1e-16) { +} + +namespace { +// Cost functor which computes symmetric epipolar distance +// used for fundamental matrix refinement. +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_; +}; + +// Termination checking callback used for fundamental estimation. +// It finished the minimization as soon as actual average of +// symmetric epipolar distance is less or equal to the expected +// average value. +class TerminationCheckingCallback : public ceres::IterationCallback { + public: + TerminationCheckingCallback(const Mat &x1, const Mat &x2, + const EstimateFundamentalOptions &options, + Mat3 *F) + : options_(options), x1_(x1), x2_(x2), F_(F) {} + + 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 epipolar distance. + double average_distance = 0.0; + for (int i = 0; i < x1_.cols(); i++) { + average_distance = SymmetricEpipolarDistance(*F_, + 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 EstimateFundamentalOptions &options_; + const Mat &x1_; + const Mat &x2_; + Mat3 *F_; +}; +} // namespace + +/* Fundamental transformation estimation. */ +bool EstimateFundamentalFromCorrespondences( + const Mat &x1, + const Mat &x2, + const EstimateFundamentalOptions &options, + Mat3 *F) { + // Step 1: Algebraic fundamental estimation. + + // Assume algebraic estiation always succeeds, + NormalizedEightPointSolver(x1, x2, F); + + LG << "Estimated matrix after algebraic estimation:\n" << *F; + + // 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; + + // Terminate if the average symmetric distance is good enough. + TerminationCheckingCallback callback(x1, x2, options, F); + 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" << *F; + + return summary.IsSolutionUsable(); +} + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/fundamental.h b/intern/libmv/libmv/multiview/fundamental.h new file mode 100644 index 00000000000..a6c7a6802fe --- /dev/null +++ b/intern/libmv/libmv/multiview/fundamental.h @@ -0,0 +1,180 @@ +// Copyright (c) 2007, 2008, 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_MULTIVIEW_FUNDAMENTAL_H_ +#define LIBMV_MULTIVIEW_FUNDAMENTAL_H_ + +#include <vector> + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +void ProjectionsFromFundamental(const Mat3 &F, Mat34 *P1, Mat34 *P2); +void FundamentalFromProjections(const Mat34 &P1, const Mat34 &P2, Mat3 *F); + +/** + * 7 points (minimal case, points coordinates must be normalized before): + */ +double FundamentalFrom7CorrespondencesLinear(const Mat &x1, + const Mat &x2, + std::vector<Mat3> *F); + +/** + * 7 points (points coordinates must be in image space): + */ +double FundamentalFromCorrespondences7Point(const Mat &x1, + const Mat &x2, + std::vector<Mat3> *F); + +/** + * 8 points (points coordinates must be in image space): + */ +double NormalizedEightPointSolver(const Mat &x1, + const Mat &x2, + Mat3 *F); + +/** + * Fundamental matrix utility function: + */ +void EnforceFundamentalRank2Constraint(Mat3 *F); + +void NormalizeFundamental(const Mat3 &F, Mat3 *F_normalized); + +/** + * Approximate squared reprojection errror. + * + * See page 287 of HZ equation 11.9. This avoids triangulating the point, + * relying only on the entries in F. + */ +double SampsonDistance(const Mat &F, const Vec2 &x1, const Vec2 &x2); + +/** + * Calculates the sum of the distances from the points to the epipolar lines. + * + * See page 288 of HZ equation 11.10. + */ +double SymmetricEpipolarDistance(const Mat &F, const Vec2 &x1, const Vec2 &x2); + +/** + * Compute the relative camera motion between two cameras. + * + * Given the motion parameters of two cameras, computes the motion parameters + * of the second one assuming the first one to be at the origin. + * If T1 and T2 are the camera motions, the computed relative motion is + * T = T2 T1^{-1} + */ +void RelativeCameraMotion(const Mat3 &R1, + const Vec3 &t1, + const Mat3 &R2, + const Vec3 &t2, + Mat3 *R, + Vec3 *t); + +void EssentialFromFundamental(const Mat3 &F, + const Mat3 &K1, + const Mat3 &K2, + Mat3 *E); + +void FundamentalFromEssential(const Mat3 &E, + const Mat3 &K1, + const Mat3 &K2, + Mat3 *F); + +void EssentialFromRt(const Mat3 &R1, + const Vec3 &t1, + const Mat3 &R2, + const Vec3 &t2, + Mat3 *E); + +void MotionFromEssential(const Mat3 &E, + std::vector<Mat3> *Rs, + std::vector<Vec3> *ts); + +/** + * Choose one of the four possible motion solutions from an essential matrix. + * + * Decides the right solution by checking that the triangulation of a match + * x1--x2 lies in front of the cameras. See HZ 9.6 pag 259 (9.6.3 Geometrical + * interpretation of the 4 solutions) + * + * \return index of the right solution or -1 if no solution. + */ +int MotionFromEssentialChooseSolution(const std::vector<Mat3> &Rs, + const std::vector<Vec3> &ts, + const Mat3 &K1, + const Vec2 &x1, + const Mat3 &K2, + const Vec2 &x2); + +bool MotionFromEssentialAndCorrespondence(const Mat3 &E, + const Mat3 &K1, + const Vec2 &x1, + const Mat3 &K2, + const Vec2 &x2, + Mat3 *R, + Vec3 *t); + +/** + * Find closest essential matrix E to fundamental F + */ +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 EstimateFundamentalOptions { + // Default constructor which sets up a options for generic usage. + EstimateFundamentalOptions(void); + + // Maximal number of iterations for refinement step. + int max_num_iterations; + + // Expected average of symmetric epipolar distance between + // actual destination points and original ones transformed by + // estimated fundamental matrix. + // + // Refinement will finish as soon as average of symmetric + // epipolar distance is less or equal to this value. + // + // This distance is measured in the same units as input points are. + double expected_average_symmetric_distance; +}; + +/** + * 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 EstimateFundamentalFromCorrespondences( + const Mat &x1, + const Mat &x2, + const EstimateFundamentalOptions &options, + Mat3 *F); + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_FUNDAMENTAL_H_ diff --git a/intern/libmv/libmv/multiview/fundamental_test.cc b/intern/libmv/libmv/multiview/fundamental_test.cc new file mode 100644 index 00000000000..da0eb449b8f --- /dev/null +++ b/intern/libmv/libmv/multiview/fundamental_test.cc @@ -0,0 +1,162 @@ +// Copyright (c) 2007, 2008 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 <iostream> + +#include "libmv/logging/logging.h" +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/test_data_sets.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" + +namespace { + +using namespace libmv; + +TEST(Fundamental, FundamentalFromProjections) { + Mat34 P1_gt, P2_gt; + P1_gt << 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0; + P2_gt << 1, 1, 1, 3, + 0, 2, 0, 3, + 0, 1, 1, 0; + Mat3 F_gt; + FundamentalFromProjections(P1_gt, P2_gt, &F_gt); + + Mat34 P1, P2; + ProjectionsFromFundamental(F_gt, &P1, &P2); + + Mat3 F; + FundamentalFromProjections(P1, P2, &F); + + EXPECT_MATRIX_PROP(F_gt, F, 1e-6); +} + +TEST(Fundamental, PreconditionerFromPoints) { + int n = 4; + Mat points(2, n); + points << 0, 0, 1, 1, + 0, 2, 1, 3; + + Mat3 T; + PreconditionerFromPoints(points, &T); + + Mat normalized_points; + ApplyTransformationToPoints(points, T, &normalized_points); + + Vec mean, variance; + MeanAndVarianceAlongRows(normalized_points, &mean, &variance); + + EXPECT_NEAR(0, mean(0), 1e-8); + EXPECT_NEAR(0, mean(1), 1e-8); + EXPECT_NEAR(2, variance(0), 1e-8); + EXPECT_NEAR(2, variance(1), 1e-8); +} + +TEST(Fundamental, EssentialFromFundamental) { + TwoViewDataSet d = TwoRealisticCameras(); + + Mat3 E_from_Rt; + EssentialFromRt(d.R1, d.t1, d.R2, d.t2, &E_from_Rt); + + Mat3 E_from_F; + EssentialFromFundamental(d.F, d.K1, d.K2, &E_from_F); + + EXPECT_MATRIX_PROP(E_from_Rt, E_from_F, 1e-6); +} + +TEST(Fundamental, MotionFromEssential) { + TwoViewDataSet d = TwoRealisticCameras(); + + Mat3 E; + EssentialFromRt(d.R1, d.t1, d.R2, d.t2, &E); + + Mat3 R; + Vec3 t; + RelativeCameraMotion(d.R1, d.t1, d.R2, d.t2, &R, &t); + NormalizeL2(&t); + + std::vector<Mat3> Rs; + std::vector<Vec3> ts; + MotionFromEssential(E, &Rs, &ts); + bool one_solution_is_correct = false; + for (size_t i = 0; i < Rs.size(); ++i) { + if (FrobeniusDistance(Rs[i], R) < 1e-8 && DistanceL2(ts[i], t) < 1e-8) { + one_solution_is_correct = true; + break; + } + } + EXPECT_TRUE(one_solution_is_correct); +} + +TEST(Fundamental, MotionFromEssentialChooseSolution) { + TwoViewDataSet d = TwoRealisticCameras(); + + Mat3 E; + EssentialFromRt(d.R1, d.t1, d.R2, d.t2, &E); + + Mat3 R; + Vec3 t; + RelativeCameraMotion(d.R1, d.t1, d.R2, d.t2, &R, &t); + NormalizeL2(&t); + + std::vector<Mat3> Rs; + std::vector<Vec3> ts; + MotionFromEssential(E, &Rs, &ts); + + Vec2 x1, x2; + MatrixColumn(d.x1, 0, &x1); + MatrixColumn(d.x2, 0, &x2); + int solution = MotionFromEssentialChooseSolution(Rs, ts, d.K1, x1, d.K2, x2); + + EXPECT_LE(0, solution); + EXPECT_LE(solution, 3); + EXPECT_LE(FrobeniusDistance(Rs[solution], R), 1e-8); + EXPECT_LE(DistanceL2(ts[solution], t), 1e-8); +} + +TEST(Fundamental, MotionFromEssentialAndCorrespondence) { + TwoViewDataSet d = TwoRealisticCameras(); + + Mat3 E; + EssentialFromRt(d.R1, d.t1, d.R2, d.t2, &E); + + Mat3 R; + Vec3 t; + RelativeCameraMotion(d.R1, d.t1, d.R2, d.t2, &R, &t); + NormalizeL2(&t); + + Vec2 x1, x2; + MatrixColumn(d.x1, 0, &x1); + MatrixColumn(d.x2, 0, &x2); + + Mat3 R_estimated; + Vec3 t_estimated; + MotionFromEssentialAndCorrespondence(E, d.K1, x1, d.K2, x2, + &R_estimated, &t_estimated); + + EXPECT_LE(FrobeniusDistance(R_estimated, R), 1e-8); + EXPECT_LE(DistanceL2(t_estimated, t), 1e-8); +} + +} // namespace diff --git a/intern/libmv/libmv/multiview/homography.cc b/intern/libmv/libmv/multiview/homography.cc new file mode 100644 index 00000000000..ce533a3ead2 --- /dev/null +++ b/intern/libmv/libmv/multiview/homography.cc @@ -0,0 +1,465 @@ +// 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) + : 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_; +}; + +// 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 diff --git a/intern/libmv/libmv/multiview/homography.h b/intern/libmv/libmv/multiview/homography.h new file mode 100644 index 00000000000..6d810c845ed --- /dev/null +++ b/intern/libmv/libmv/multiview/homography.h @@ -0,0 +1,145 @@ +// Copyright (c) 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_MULTIVIEW_HOMOGRAPHY_H_ +#define LIBMV_MULTIVIEW_HOMOGRAPHY_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +/** + * 2D homography transformation estimation. + * + * This function estimates the homography transformation from a list of 2D + * correspondences which represents either: + * + * - 3D points on a plane, with a general moving camera. + * - 3D points with a rotating camera (pure rotation). + * - 3D points + different planar projections + * + * \param x1 The first 2xN or 3xN matrix of euclidean or homogeneous points. + * \param x2 The second 2xN or 3xN matrix of euclidean or homogeneous points. + * \param H The 3x3 homography transformation matrix (8 dof) such that + * x2 = H * x1 with |a b c| + * H = |d e f| + * |g h 1| + * \param expected_precision The expected precision in order for instance + * to accept almost homography matrices. + * + * \return True if the transformation estimation has succeeded. + * \note There must be at least 4 non-colinear points. + */ +bool Homography2DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat3 *H, + double expected_precision = + 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 EstimateHomographyOptions { + // Default constructor which sets up a options for generic usage. + EstimateHomographyOptions(void); + + // Normalize correspondencies before estimating the homography + // in order to increase estimation stability. + // + // Normaliztion will make it so centroid od correspondences + // is the coordinate origin and their average distance from + // the origin is sqrt(2). + // + // See: + // - R. Hartley and A. Zisserman. Multiple View Geometry in Computer + // Vision. Cambridge University Press, second edition, 2003. + // - https://www.cs.ubc.ca/grads/resources/thesis/May09/Dubrofsky_Elan.pdf + bool use_normalization; + + // Maximal number of iterations for the refinement step. + int max_num_iterations; + + // Expected average of symmetric geometric distance between + // actual destination points and original ones transformed by + // estimated homography matrix. + // + // Refinement will finish as soon as average of symmetric + // geometric distance is less or equal to this value. + // + // This distance is measured in the same units as input points are. + double expected_average_symmetric_distance; +}; + +/** + * 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 EstimateHomography2DFromCorrespondences( + const Mat &x1, + const Mat &x2, + const EstimateHomographyOptions &options, + Mat3 *H); + +/** + * 3D Homography transformation estimation. + * + * This function can be used in order to estimate the homography transformation + * from a list of 3D correspondences. + * + * \param[in] x1 The first 4xN matrix of homogeneous points + * \param[in] x2 The second 4xN matrix of homogeneous points + * \param[out] H The 4x4 homography transformation matrix (15 dof) such that + * x2 = H * x1 with |a b c d| + * H = |e f g h| + * |i j k l| + * |m n o 1| + * \param[in] expected_precision The expected precision in order for instance + * to accept almost homography matrices. + * + * \return true if the transformation estimation has succeeded + * + * \note Need at least 5 non coplanar points + * \note Points coordinates must be in homogeneous coordinates + */ +bool Homography3DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat4 *H, + double expected_precision = + EigenDouble::dummy_precision()); + +/** + * Calculate symmetric geometric cost: + * + * D(H * x1, x2)^2 + D(H^-1 * x2, x1) + */ +double SymmetricGeometricDistance(const Mat3 &H, + const Vec2 &x1, + const Vec2 &x2); + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_HOMOGRAPHY_H_ diff --git a/intern/libmv/libmv/multiview/homography_error.h b/intern/libmv/libmv/multiview/homography_error.h new file mode 100644 index 00000000000..f8b9d45e73c --- /dev/null +++ b/intern/libmv/libmv/multiview/homography_error.h @@ -0,0 +1,248 @@ +// Copyright (c) 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_MULTIVIEW_HOMOGRAPHY_ERRORS_H_ +#define LIBMV_MULTIVIEW_HOMOGRAPHY_ERRORS_H_ + +#include "libmv/multiview/projection.h" + +namespace libmv { +namespace homography { +namespace homography2D { + + /** + * Structure for estimating the asymmetric error between a vector x2 and the + * transformed x1 such that + * Error = ||x2 - Psi(H * x1)||^2 + * where Psi is the function that transforms homogeneous to euclidean coords. + * \note It should be distributed as Chi-squared with k = 2. + */ +struct AsymmetricError { + /** + * Computes the asymmetric residuals between a set of 2D points x2 and the + * transformed 2D point set x1 such that + * Residuals_i = x2_i - Psi(H * x1_i) + * where Psi is the function that transforms homogeneous to euclidean coords. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[in] x2 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[out] dx A 2xN matrix of column vectors of residuals errors + */ + static void Residuals(const Mat &H, const Mat &x1, + const Mat &x2, Mat2X *dx) { + dx->resize(2, x1.cols()); + Mat3X x2h_est; + if (x1.rows() == 2) + x2h_est = H * EuclideanToHomogeneous(static_cast<Mat2X>(x1)); + else + x2h_est = H * x1; + dx->row(0) = x2h_est.row(0).array() / x2h_est.row(2).array(); + dx->row(1) = x2h_est.row(1).array() / x2h_est.row(2).array(); + if (x2.rows() == 2) + *dx = x2 - *dx; + else + *dx = HomogeneousToEuclidean(static_cast<Mat3X>(x2)) - *dx; + } + /** + * Computes the asymmetric residuals between a 2D point x2 and the transformed + * 2D point x1 such that + * Residuals = x2 - Psi(H * x1) + * where Psi is the function that transforms homogeneous to euclidean coords. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[in] x2 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[out] dx A vector of size 2 of the residual error + */ + static void Residuals(const Mat &H, const Vec &x1, + const Vec &x2, Vec2 *dx) { + Vec3 x2h_est; + if (x1.rows() == 2) + x2h_est = H * EuclideanToHomogeneous(static_cast<Vec2>(x1)); + else + x2h_est = H * x1; + if (x2.rows() == 2) + *dx = x2 - x2h_est.head<2>() / x2h_est[2]; + else + *dx = HomogeneousToEuclidean(static_cast<Vec3>(x2)) - + x2h_est.head<2>() / x2h_est[2]; + } + /** + * Computes the squared norm of the residuals between a set of 2D points x2 + * and the transformed 2D point set x1 such that + * Error = || x2 - Psi(H * x1) ||^2 + * where Psi is the function that transforms homogeneous to euclidean coords. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[in] x2 A set of 2D points (2xN or 3xN matrix of column vectors). + * \return The squared norm of the asymmetric residuals errors + */ + static double Error(const Mat &H, const Mat &x1, const Mat &x2) { + Mat2X dx; + Residuals(H, x1, x2, &dx); + return dx.squaredNorm(); + } + /** + * Computes the squared norm of the residuals between a 2D point x2 and the + * transformed 2D point x1 such that rms = || x2 - Psi(H * x1) ||^2 + * where Psi is the function that transforms homogeneous to euclidean coords. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[in] x2 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \return The squared norm of the asymmetric residual error + */ + static double Error(const Mat &H, const Vec &x1, const Vec &x2) { + Vec2 dx; + Residuals(H, x1, x2, &dx); + return dx.squaredNorm(); + } +}; + + /** + * Structure for estimating the symmetric error + * between a vector x2 and the transformed x1 such that + * Error = ||x2 - Psi(H * x1)||^2 + ||x1 - Psi(H^-1 * x2)||^2 + * where Psi is the function that transforms homogeneous to euclidean coords. + * \note It should be distributed as Chi-squared with k = 4. + */ +struct SymmetricError { + /** + * Computes the squared norm of the residuals between x2 and the + * transformed x1 such that + * Error = ||x2 - Psi(H * x1)||^2 + ||x1 - Psi(H^-1 * x2)||^2 + * where Psi is the function that transforms homogeneous to euclidean coords. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[in] x2 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \return The squared norm of the symmetric residuals errors + */ + static double Error(const Mat &H, const Vec &x1, const Vec &x2) { + // TODO(keir): This is awesomely inefficient because it does a 3x3 + // inversion for each evaluation. + Mat3 Hinv = H.inverse(); + return AsymmetricError::Error(H, x1, x2) + + AsymmetricError::Error(Hinv, x2, x1); + } + // TODO(julien) Add residuals function \see AsymmetricError +}; + /** + * Structure for estimating the algebraic error (cross product) + * between a vector x2 and the transformed x1 such that + * Error = ||[x2] * H * x1||^^2 + * where [x2] is the skew matrix of x2. + */ +struct AlgebraicError { + // TODO(julien) Make an AlgebraicError2Rows and AlgebraicError3Rows + + /** + * Computes the algebraic residuals (cross product) between a set of 2D + * points x2 and the transformed 2D point set x1 such that + * [x2] * H * x1 where [x2] is the skew matrix of x2. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[in] x2 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[out] dx A 3xN matrix of column vectors of residuals errors + */ + static void Residuals(const Mat &H, const Mat &x1, + const Mat &x2, Mat3X *dx) { + dx->resize(3, x1.cols()); + Vec3 col; + for (int i = 0; i < x1.cols(); ++i) { + Residuals(H, x1.col(i), x2.col(i), &col); + dx->col(i) = col; + } + } + /** + * Computes the algebraic residuals (cross product) between a 2D point x2 + * and the transformed 2D point x1 such that + * [x2] * H * x1 where [x2] is the skew matrix of x2. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[in] x2 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[out] dx A vector of size 3 of the residual error + */ + static void Residuals(const Mat &H, const Vec &x1, + const Vec &x2, Vec3 *dx) { + Vec3 x2h_est; + if (x1.rows() == 2) + x2h_est = H * EuclideanToHomogeneous(static_cast<Vec2>(x1)); + else + x2h_est = H * x1; + if (x2.rows() == 2) + *dx = SkewMat(EuclideanToHomogeneous(static_cast<Vec2>(x2))) * x2h_est; + else + *dx = SkewMat(x2) * x2h_est; + // TODO(julien) This is inefficient since it creates an + // identical 3x3 skew matrix for each evaluation. + } + /** + * Computes the squared norm of the algebraic residuals between a set of 2D + * points x2 and the transformed 2D point set x1 such that + * [x2] * H * x1 where [x2] is the skew matrix of x2. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A set of 2D points (2xN or 3xN matrix of column vectors). + * \param[in] x2 A set of 2D points (2xN or 3xN matrix of column vectors). + * \return The squared norm of the asymmetric residuals errors + */ + static double Error(const Mat &H, const Mat &x1, const Mat &x2) { + Mat3X dx; + Residuals(H, x1, x2, &dx); + return dx.squaredNorm(); + } + /** + * Computes the squared norm of the algebraic residuals between a 2D point x2 + * and the transformed 2D point x1 such that + * [x2] * H * x1 where [x2] is the skew matrix of x2. + * + * \param[in] H The 3x3 homography matrix. + * The estimated homography should approximatelly hold the condition y = H x. + * \param[in] x1 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \param[in] x2 A 2D point (vector of size 2 or 3 (euclidean/homogeneous)) + * \return The squared norm of the asymmetric residual error + */ + static double Error(const Mat &H, const Vec &x1, const Vec &x2) { + Vec3 dx; + Residuals(H, x1, x2, &dx); + return dx.squaredNorm(); + } +}; +// TODO(keir): Add error based on ideal points. + +} // namespace homography2D +// TODO(julien) add homography3D errors +} // namespace homography +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_HOMOGRAPHY_ERRORS_H_ diff --git a/intern/libmv/libmv/multiview/homography_parameterization.h b/intern/libmv/libmv/multiview/homography_parameterization.h new file mode 100644 index 00000000000..ca8fbd8066e --- /dev/null +++ b/intern/libmv/libmv/multiview/homography_parameterization.h @@ -0,0 +1,91 @@ +// Copyright (c) 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_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ +#define LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +/** A parameterization of the 2D homography matrix that uses 8 parameters so + * that the matrix is normalized (H(2,2) == 1). + * The homography matrix H is built from a list of 8 parameters (a, b,...g, h) + * as follows + * |a b c| + * H = |d e f| + * |g h 1| + */ +template<typename T = double> +class Homography2DNormalizedParameterization { + public: + typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h + typedef Eigen::Matrix<T, 3, 3> Parameterized; // H + + /// Convert from the 8 parameters to a H matrix. + static void To(const Parameters &p, Parameterized *h) { + *h << p(0), p(1), p(2), + p(3), p(4), p(5), + p(6), p(7), 1.0; + } + + /// Convert from a H matrix to the 8 parameters. + static void From(const Parameterized &h, Parameters *p) { + *p << h(0, 0), h(0, 1), h(0, 2), + h(1, 0), h(1, 1), h(1, 2), + h(2, 0), h(2, 1); + } +}; + +/** A parameterization of the 2D homography matrix that uses 15 parameters so + * that the matrix is normalized (H(3,3) == 1). + * The homography matrix H is built from a list of 15 parameters (a, b,...n, o) + * as follows + * |a b c d| + * H = |e f g h| + * |i j k l| + * |m n o 1| + */ +template<typename T = double> +class Homography3DNormalizedParameterization { + public: + typedef Eigen::Matrix<T, 15, 1> Parameters; // a, b, ... n, o + typedef Eigen::Matrix<T, 4, 4> Parameterized; // H + + /// Convert from the 15 parameters to a H matrix. + static void To(const Parameters &p, Parameterized *h) { + *h << p(0), p(1), p(2), p(3), + p(4), p(5), p(6), p(7), + p(8), p(9), p(10), p(11), + p(12), p(13), p(14), 1.0; + } + + /// Convert from a H matrix to the 15 parameters. + static void From(const Parameterized &h, Parameters *p) { + *p << h(0, 0), h(0, 1), h(0, 2), h(0, 3), + h(1, 0), h(1, 1), h(1, 2), h(1, 3), + h(2, 0), h(2, 1), h(2, 2), h(2, 3), + h(3, 0), h(3, 1), h(3, 2); + } +}; + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ diff --git a/intern/libmv/libmv/multiview/homography_test.cc b/intern/libmv/libmv/multiview/homography_test.cc new file mode 100644 index 00000000000..8d7266e3d11 --- /dev/null +++ b/intern/libmv/libmv/multiview/homography_test.cc @@ -0,0 +1,261 @@ +// Copyright (c) 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. + +#include "testing/testing.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/homography.h" + +namespace { +using namespace libmv; + +namespace { + +// Check whether homography transform M actually transforms +// given vectors x1 to x2. Used to check validness of a reconstructed +// homography matrix. +// TODO(sergey): Consider using this in all tests since possible homography +// matrix is not fixed to a single value and different-looking matrices +// might actually crrespond to the same exact transform. +void CheckHomography2DTransform(const Mat3 &H, + const Mat &x1, + const Mat &x2) { + for (int i = 0; i < x2.cols(); ++i) { + Vec3 x2_expected = x2.col(i); + Vec3 x2_observed = H * x1.col(i); + x2_observed /= x2_observed(2); + EXPECT_MATRIX_NEAR(x2_expected, x2_observed, 1e-8); + } +} + +} // namespace + +TEST(Homography2DTest, Rotation45AndTranslationXY) { + Mat x1(3, 4); + x1 << 0, 1, 0, 5, + 0, 0, 2, 3, + 1, 1, 1, 1; + + double angle = 45.0; + Mat3 m; + m << cos(angle), -sin(angle), -2, + sin(angle), cos(angle), 5, + 0, 0, 1; + + Mat x2 = x1; + // Transform point from ground truth matrix + for (int i = 0; i < x2.cols(); ++i) + x2.col(i) = m * x1.col(i); + + Mat3 homography_mat; + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(x1, x2, &homography_mat)); + VLOG(1) << "Mat Homography2D "; + VLOG(1) << homography_mat; + VLOG(1) << "Mat GT "; + VLOG(1) << m; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); +} + +TEST(Homography2DTest, AffineGeneral4) { + // TODO(julien) find why it doesn't work with 4 points!!! + Mat x1(3, 4); + x1 << 0, 1, 0, 2, + 0, 0, 1, 2, + 1, 1, 1, 1; + Mat3 m; + m << 3, -1, 4, + 6, -2, -3, + 0, 0, 1; + + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) { + x2.col(i) = m * x1.col(i); + } + + Mat3 homography_mat; + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(x1, x2, &homography_mat)); + VLOG(1) << "Mat Homography2D"; + VLOG(1) << homography_mat; + CheckHomography2DTransform(homography_mat, x1, x2); + + // Test with euclidean coordinates + Mat eX1, eX2; + HomogeneousToEuclidean(x1, &eX1); + HomogeneousToEuclidean(x2, &eX2); + homography_mat.setIdentity(); + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(eX1, eX2, &homography_mat)); + + VLOG(1) << "Mat Homography2D "; + VLOG(1) << homography_mat; + CheckHomography2DTransform(homography_mat, x1, x2); +} + +TEST(Homography2DTest, AffineGeneral5) { + Mat x1(3, 5); + x1 << 0, 1, 0, 2, 5, + 0, 0, 1, 2, 2, + 1, 1, 1, 1, 1; + Mat3 m; + m << 3, -1, 4, + 6, -2, -3, + 0, 0, 1; + + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) + x2.col(i) = m * x1.col(i); + + Mat3 homography_mat; + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(x1, x2, &homography_mat)); + + VLOG(1) << "Mat Homography2D "; + VLOG(1) << homography_mat; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); + + // Test with euclidean coordinates + Mat eX1, eX2; + HomogeneousToEuclidean(x1, &eX1); + HomogeneousToEuclidean(x2, &eX2); + homography_mat.setIdentity(); + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(eX1, eX2, &homography_mat)); + + VLOG(1) << "Mat Homography2D "; + VLOG(1) << homography_mat; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); +} + +TEST(Homography2DTest, HomographyGeneral) { + Mat x1(3, 4); + x1 << 0, 1, 0, 5, + 0, 0, 2, 3, + 1, 1, 1, 1; + Mat3 m; + m << 3, -1, 4, + 6, -2, -3, + 1, -3, 1; + + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) + x2.col(i) = m * x1.col(i); + + Mat3 homography_mat; + EXPECT_TRUE(Homography2DFromCorrespondencesLinear(x1, x2, &homography_mat)); + + VLOG(1) << "Mat Homography2D "; + VLOG(1) << homography_mat; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); +} + +TEST(Homography3DTest, RotationAndTranslationXYZ) { + Mat x1(4, 5); + x1 << 0, 0, 1, 5, 2, + 0, 1, 2, 3, 5, + 0, 2, 0, 1, 5, + 1, 1, 1, 1, 1; + Mat4 M; + M.setIdentity(); + /* + M = AngleAxisd(45.0, Vector3f::UnitZ()) + * AngleAxisd(25.0, Vector3f::UnitX()) + * AngleAxisd(5.0, Vector3f::UnitZ());*/ + + // Rotation on x + translation + double angle = 45.0; + Mat4 rot; + rot << 1, 0, 0, 1, + 0, cos(angle), -sin(angle), 3, + 0, sin(angle), cos(angle), -2, + 0, 0, 0, 1; + M *= rot; + // Rotation on y + angle = 25.0; + rot << cos(angle), 0, sin(angle), 0, + 0, 1, 0, 0, + -sin(angle), 0, cos(angle), 0, + 0, 0, 0, 1; + M *= rot; + // Rotation on z + angle = 5.0; + rot << cos(angle), -sin(angle), 0, 0, + sin(angle), cos(angle), 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + M *= rot; + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) { + x2.col(i) = M * x1.col(i); + } + + Mat4 homography_mat; + EXPECT_TRUE(Homography3DFromCorrespondencesLinear(x1, x2, &homography_mat)); + + VLOG(1) << "Mat Homography3D " << homography_mat; + VLOG(1) << "Mat GT " << M; + EXPECT_MATRIX_NEAR(homography_mat, M, 1e-8); +} + +TEST(Homography3DTest, AffineGeneral) { + Mat x1(4, 5); + x1 << 0, 0, 1, 5, 2, + 0, 1, 2, 3, 5, + 0, 2, 0, 1, 5, + 1, 1, 1, 1, 1; + Mat4 m; + m << 3, -1, 4, 1, + 6, -2, -3, -6, + 1, 0, 1, 2, + 0, 0, 0, 1; + + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) { + x2.col(i) = m * x1.col(i); + } + + Mat4 homography_mat; + EXPECT_TRUE(Homography3DFromCorrespondencesLinear(x1, x2, &homography_mat)); + VLOG(1) << "Mat Homography3D "; + VLOG(1) << homography_mat; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); +} + +TEST(Homography3DTest, HomographyGeneral) { + Mat x1(4, 5); + x1 << 0, 0, 1, 5, 2, + 0, 1, 2, 3, 5, + 0, 2, 0, 1, 5, + 1, 1, 1, 1, 1; + Mat4 m; + m << 3, -1, 4, 1, + 6, -2, -3, -6, + 1, 0, 1, 2, + -3, 1, 0, 1; + + Mat x2 = x1; + for (int i = 0; i < x2.cols(); ++i) { + x2.col(i) = m * x1.col(i); + } + + Mat4 homography_mat; + EXPECT_TRUE(Homography3DFromCorrespondencesLinear(x1, x2, &homography_mat)); + VLOG(1) << "Mat Homography3D"; + VLOG(1) << homography_mat; + EXPECT_MATRIX_NEAR(homography_mat, m, 1e-8); +} + +} // namespace diff --git a/intern/libmv/libmv/multiview/nviewtriangulation.h b/intern/libmv/libmv/multiview/nviewtriangulation.h new file mode 100644 index 00000000000..f4614ab1a5c --- /dev/null +++ b/intern/libmv/libmv/multiview/nviewtriangulation.h @@ -0,0 +1,80 @@ +// Copyright (c) 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. +// +// Compute a 3D position of a point from several images of it. In particular, +// compute the projective point X in R^4 such that x = PX. +// +// Algorithm is the standard DLT; for derivation see appendix of Keir's thesis. + +#ifndef LIBMV_MULTIVIEW_NVIEWTRIANGULATION_H +#define LIBMV_MULTIVIEW_NVIEWTRIANGULATION_H + +#include "libmv/base/vector.h" +#include "libmv/logging/logging.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +// x's are 2D coordinates (x,y,1) in each image; Ps are projective cameras. The +// output, X, is a homogeneous four vectors. +template<typename T> +void NViewTriangulate(const Matrix<T, 2, Dynamic> &x, + const vector<Matrix<T, 3, 4> > &Ps, + Matrix<T, 4, 1> *X) { + int nviews = x.cols(); + assert(nviews == Ps.size()); + + Matrix<T, Dynamic, Dynamic> design(3*nviews, 4 + nviews); + design.setConstant(0.0); + for (int i = 0; i < nviews; i++) { + design.template block<3, 4>(3*i, 0) = -Ps[i]; + design(3*i + 0, 4 + i) = x(0, i); + design(3*i + 1, 4 + i) = x(1, i); + design(3*i + 2, 4 + i) = 1.0; + } + Matrix<T, Dynamic, 1> X_and_alphas; + Nullspace(&design, &X_and_alphas); + X->resize(4); + *X = X_and_alphas.head(4); +} + +// x's are 2D coordinates (x,y,1) in each image; Ps are projective cameras. The +// output, X, is a homogeneous four vectors. +// This method uses the algebraic distance approximation. +// Note that this method works better when the 2D points are normalized +// with an isotopic normalization. +template<typename T> +void NViewTriangulateAlgebraic(const Matrix<T, 2, Dynamic> &x, + const vector<Matrix<T, 3, 4> > &Ps, + Matrix<T, 4, 1> *X) { + int nviews = x.cols(); + assert(nviews == Ps.size()); + + Matrix<T, Dynamic, 4> design(2*nviews, 4); + for (int i = 0; i < nviews; i++) { + design.template block<2, 4>(2*i, 0) = SkewMatMinimal(x.col(i)) * Ps[i]; + } + X->resize(4); + Nullspace(&design, X); +} + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_RESECTION_H diff --git a/intern/libmv/libmv/multiview/nviewtriangulation_test.cc b/intern/libmv/libmv/multiview/nviewtriangulation_test.cc new file mode 100644 index 00000000000..5a4d8499753 --- /dev/null +++ b/intern/libmv/libmv/multiview/nviewtriangulation_test.cc @@ -0,0 +1,94 @@ +// Copyright (c) 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 <iostream> + +#include "libmv/base/vector.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/nviewtriangulation.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/test_data_sets.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" + +namespace { + +using namespace libmv; + +TEST(NViewTriangulate, FiveViews) { + int nviews = 5; + int npoints = 6; + NViewDataSet d = NRealisticCamerasFull(nviews, npoints); + + // Collect P matrices together. + vector<Mat34> Ps(nviews); + for (int j = 0; j < nviews; ++j) { + Ps[j] = d.P(j); + } + + for (int i = 0; i < npoints; ++i) { + // Collect the image of point i in each frame. + Mat2X xs(2, nviews); + for (int j = 0; j < nviews; ++j) { + xs.col(j) = d.x[j].col(i); + } + Vec4 X; + NViewTriangulate(xs, Ps, &X); + + // Check reprojection error. Should be nearly zero. + for (int j = 0; j < nviews; ++j) { + Vec3 x_reprojected = Ps[j]*X; + x_reprojected /= x_reprojected(2); + double error = (x_reprojected.head(2) - xs.col(j)).norm(); + EXPECT_NEAR(error, 0.0, 1e-9); + } + } +} + +TEST(NViewTriangulateAlgebraic, FiveViews) { + int nviews = 5; + int npoints = 6; + NViewDataSet d = NRealisticCamerasFull(nviews, npoints); + + // Collect P matrices together. + vector<Mat34> Ps(nviews); + for (int j = 0; j < nviews; ++j) { + Ps[j] = d.P(j); + } + + for (int i = 0; i < npoints; ++i) { + // Collect the image of point i in each frame. + Mat2X xs(2, nviews); + for (int j = 0; j < nviews; ++j) { + xs.col(j) = d.x[j].col(i); + } + Vec4 X; + NViewTriangulate(xs, Ps, &X); + + // Check reprojection error. Should be nearly zero. + for (int j = 0; j < nviews; ++j) { + Vec3 x_reprojected = Ps[j]*X; + x_reprojected /= x_reprojected(2); + double error = (x_reprojected.head<2>() - xs.col(j)).norm(); + EXPECT_NEAR(error, 0.0, 1e-9); + } + } +} +} // namespace diff --git a/intern/libmv/libmv/multiview/panography.cc b/intern/libmv/libmv/multiview/panography.cc new file mode 100644 index 00000000000..b62802948c4 --- /dev/null +++ b/intern/libmv/libmv/multiview/panography.cc @@ -0,0 +1,125 @@ +// Copyright (c) 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/panography.h" + +namespace libmv { + +static bool Build_Minimal2Point_PolynomialFactor( + const Mat & x1, const Mat & x2, + double * P) { // P must be a double[4] + assert(2 == x1.rows()); + assert(2 == x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Setup the variable of the input problem: + Vec xx1 = (x1.col(0)).transpose(); + Vec yx1 = (x1.col(1)).transpose(); + + double a12 = xx1.dot(yx1); + Vec xx2 = (x2.col(0)).transpose(); + Vec yx2 = (x2.col(1)).transpose(); + double b12 = xx2.dot(yx2); + + double a1 = xx1.squaredNorm(); + double a2 = yx1.squaredNorm(); + + double b1 = xx2.squaredNorm(); + double b2 = yx2.squaredNorm(); + + // Build the 3rd degre polynomial in F^2. + // + // f^6 * p + f^4 * q + f^2* r + s = 0; + // + // Coefficients in ascending powers of alpha, i.e. P[N]*x^N. + // Run panography_coeffs.py to get the below coefficients. + P[0] = b1*b2*a12*a12-a1*a2*b12*b12; + P[1] = -2*a1*a2*b12+2*a12*b1*b2+b1*a12*a12+b2*a12*a12-a1*b12*b12-a2*b12*b12; + P[2] = b1*b2-a1*a2-2*a1*b12-2*a2*b12+2*a12*b1+2*a12*b2+a12*a12-b12*b12; + P[3] = b1+b2-2*b12-a1-a2+2*a12; + + // If P[3] equal to 0 we get ill conditionned data + return (P[3] != 0.0); +} + +// This implements a minimal solution (2 points) for panoramic stitching: +// +// http://www.cs.ubc.ca/~mbrown/minimal/minimal.html +// +// [1] M. Brown and R. Hartley and D. Nister. Minimal Solutions for Panoramic +// Stitching. CVPR07. +void F_FromCorrespondance_2points(const Mat &x1, const Mat &x2, + vector<double> *fs) { + // Build Polynomial factor to get squared focal value. + double P[4]; + Build_Minimal2Point_PolynomialFactor(x1, x2, &P[0]); + + // Solve it by using F = f^2 and a Cubic polynomial solver + // + // F^3 * p + F^2 * q + F^1 * r + s = 0 + // + double roots[3]; + int num_roots = SolveCubicPolynomial(P, roots); + for (int i = 0; i < num_roots; ++i) { + if (roots[i] > 0.0) { + fs->push_back(sqrt(roots[i])); + } + } +} + +// Compute the 3x3 rotation matrix that fits two 3D point clouds in the least +// square sense. The method is from: +// +// K. Arun,T. Huand and D. Blostein. Least-squares fitting of 2 3-D point +// sets. IEEE Transactions on Pattern Analysis and Machine Intelligence, +// 9:698-700, 1987. +void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2, + const double focal, + Mat3 *R) { + assert(3 == x1.rows()); + assert(2 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Build simplified K matrix + Mat3 K(Mat3::Identity() * 1.0/focal); + K(2, 2)= 1.0; + + // Build the correlation matrix; equation (22) in [1]. + Mat3 C = Mat3::Zero(); + for (int i = 0; i < x1.cols(); ++i) { + Mat r1i = (K * x1.col(i)).normalized(); + Mat r2i = (K * x2.col(i)).normalized(); + C += r2i * r1i.transpose(); + } + + // Solve for rotation. Equations (24) and (25) in [1]. + Eigen::JacobiSVD<Mat> svd(C, Eigen::ComputeThinU | Eigen::ComputeThinV); + Mat3 scale = Mat3::Identity(); + scale(2, 2) = ((svd.matrixU() * svd.matrixV().transpose()).determinant() > 0.0) + ? 1.0 + : -1.0; + + (*R) = svd.matrixU() * scale * svd.matrixV().transpose(); +} + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/panography.h b/intern/libmv/libmv/multiview/panography.h new file mode 100644 index 00000000000..6e87bd71304 --- /dev/null +++ b/intern/libmv/libmv/multiview/panography.h @@ -0,0 +1,99 @@ +// Copyright (c) 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. +// + +#ifndef LIBMV_MULTIVIEW_PANOGRAPHY_H +#define LIBMV_MULTIVIEW_PANOGRAPHY_H + +#include "libmv/numeric/numeric.h" +#include "libmv/numeric/poly.h" +#include "libmv/base/vector.h" + +namespace libmv { + +// This implements a minimal solution (2 points) for panoramic stitching: +// +// http://www.cs.ubc.ca/~mbrown/minimal/minimal.html +// +// [1] M. Brown and R. Hartley and D. Nister. Minimal Solutions for Panoramic +// Stitching. CVPR07. +// +// The 2-point algorithm solves for the rotation of the camera with a single +// focal length (4 degrees of freedom). +// +// Compute from 1 to 3 possible focal lenght for 2 point correspondences. +// Suppose that the cameras share the same optical center and focal lengths: +// +// Image 1 => H*x = x' => Image 2 +// x (u1j) x' (u2j) +// a (u11) a' (u21) +// b (u12) b' (u22) +// +// The return values are 1 to 3 possible values for the focal lengths such +// that: +// +// [f 0 0] +// K = [0 f 0] +// [0 0 1] +// +void F_FromCorrespondance_2points(const Mat &x1, const Mat &x2, + vector<double> *fs); + +// Compute the 3x3 rotation matrix that fits two 3D point clouds in the least +// square sense. The method is from: +// +// K. Arun,T. Huand and D. Blostein. Least-squares fitting of 2 3-D point +// sets. IEEE Transactions on Pattern Analysis and Machine Intelligence, +// 9:698-700, 1987. +// +// Given the calibration matrices K1, K2 solve for the rotation from +// corresponding image rays. +// +// R = min || X2 - R * x1 ||. +// +// In case of panography, which is for a camera that shares the same camera +// center, +// +// H = K2 * R * K1.inverse(); +// +// For the full explanation, see Section 8, Solving for Rotation from [1]. +// +// Parameters: +// +// x1 : Point cloud A (3D coords) +// x2 : Point cloud B (3D coords) +// +// [f 0 0] +// K1 = [0 f 0] +// [0 0 1] +// +// K2 (the same form as K1, but may have different f) +// +// Returns: A rotation matrix that minimizes +// +// R = arg min || X2 - R * x1 || +// +void GetR_FixedCameraCenter(const Mat &x1, const Mat &x2, + const double focal, + Mat3 *R); + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_PANOGRAPHY_H diff --git a/intern/libmv/libmv/multiview/panography_kernel.cc b/intern/libmv/libmv/multiview/panography_kernel.cc new file mode 100644 index 00000000000..8fdc9e79aed --- /dev/null +++ b/intern/libmv/libmv/multiview/panography_kernel.cc @@ -0,0 +1,51 @@ +// 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/panography_kernel.h" +#include "libmv/multiview/panography.h" + +namespace libmv { +namespace panography { +namespace kernel { + +void TwoPointSolver::Solve(const Mat &x1, const Mat &x2, vector<Mat3> *Hs) { + // Solve for the focal lengths. + vector<double> fs; + F_FromCorrespondance_2points(x1, x2, &fs); + + // Then solve for the rotations and homographies. + Mat x1h, x2h; + EuclideanToHomogeneous(x1, &x1h); + EuclideanToHomogeneous(x2, &x2h); + for (int i = 0; i < fs.size(); ++i) { + Mat3 K1 = Mat3::Identity() * fs[i]; + K1(2, 2) = 1.0; + + Mat3 R; + GetR_FixedCameraCenter(x1h, x2h, fs[i], &R); + R /= R(2, 2); + + (*Hs).push_back(K1 * R * K1.inverse()); + } +} + +} // namespace kernel +} // namespace panography +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/panography_kernel.h b/intern/libmv/libmv/multiview/panography_kernel.h new file mode 100644 index 00000000000..a6adbd54b20 --- /dev/null +++ b/intern/libmv/libmv/multiview/panography_kernel.h @@ -0,0 +1,54 @@ +// Copyright (c) 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. + +#ifndef LIBMV_MULTIVIEW_PANOGRAPHY_KERNEL_H +#define LIBMV_MULTIVIEW_PANOGRAPHY_KERNEL_H + +#include "libmv/base/vector.h" +#include "libmv/multiview/conditioning.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/two_view_kernel.h" +#include "libmv/multiview/homography_error.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { +namespace panography { +namespace kernel { + +struct TwoPointSolver { + enum { MINIMUM_SAMPLES = 2 }; + static void Solve(const Mat &x1, const Mat &x2, vector<Mat3> *Hs); +}; + +typedef two_view::kernel::Kernel< + TwoPointSolver, homography::homography2D::AsymmetricError, Mat3> + UnnormalizedKernel; + +typedef two_view::kernel::Kernel< + two_view::kernel::NormalizedSolver<TwoPointSolver, UnnormalizerI>, + homography::homography2D::AsymmetricError, + Mat3> + Kernel; + +} // namespace kernel +} // namespace panography +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_PANOGRAPHY_KERNEL_H diff --git a/intern/libmv/libmv/multiview/panography_test.cc b/intern/libmv/libmv/multiview/panography_test.cc new file mode 100644 index 00000000000..f6faf0f6022 --- /dev/null +++ b/intern/libmv/libmv/multiview/panography_test.cc @@ -0,0 +1,144 @@ +// Copyright (c) 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/logging/logging.h" +#include "libmv/multiview/panography.h" +#include "libmv/multiview/panography_kernel.h" +#include "libmv/multiview/projection.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" + +namespace libmv { +namespace { + +TEST(Panography, PrintSomeSharedFocalEstimationValues) { + Mat x1(2, 2), x2(2, 2); + x1<< 158, 78, + 124, 113; + x2<< 300, 214, + 125, 114; + + // Normalize data (set principal point 0,0 and image border to 1.0). + x1.block<1, 2>(0, 0) /= 320; + x1.block<1, 2>(1, 0) /= 240; + x2.block<1, 2>(0, 0) /= 320; + x2.block<1, 2>(1, 0) /= 240; + x1+=Mat2::Constant(0.5); + x2+=Mat2::Constant(0.5); + + vector<double> fs; + F_FromCorrespondance_2points(x1, x2, &fs); + + // Assert we found a valid solution. + EXPECT_EQ(1, fs.size()); + EXPECT_NEAR(1.01667, fs[1], 1e-3); +} + +TEST(Panography, GetR_FixedCameraCenterWithIdentity) { + Mat x1(3, 3); + x1 << 0.5, 0.6, 0.7, + 0.5, 0.5, 0.4, + 10.0, 10.0, 10.0; + + Mat3 R; + GetR_FixedCameraCenter(x1, x1, 1.0, &R); + R /= R(2, 2); + EXPECT_MATRIX_NEAR(Mat3::Identity(), R, 1e-8); + LOG(INFO) << "R \n" << R; +} + +TEST(Panography, Homography_GetR_Test_PitchY30) { + int n = 3; + + Mat x1(3, n); + x1 << 0.5, 0.6, 0.7, + 0.5, 0.5, 0.4, + 10, 10, 10; + + Mat x2 = x1; + const double alpha = 30.0 * M_PI / 180.0; + Mat3 rotY; + rotY << cos(alpha), 0, -sin(alpha), + 0, 1, 0, + sin(alpha), 0, cos(alpha); + + for (int i = 0; i < n; ++i) { + x2.block<3, 1>(0, i) = rotY * x1.col(i); + } + + Mat3 R; + GetR_FixedCameraCenter(x1, x2, 1.0, &R); + + // Assert that residuals are small enough + for (int i = 0; i < n; ++i) { + Vec residuals = (R * x1.col(i)) - x2.col(i); + EXPECT_NEAR(0, residuals.norm(), 1e-6); + } + + // Check that the rotation angle along Y is the expected one. + // Use the euler approximation to recover the angle. + double pitch_y = asin(R(2, 0)) * 180.0 / M_PI; + EXPECT_NEAR(30, pitch_y, 1e-4); +} + +TEST(MinimalPanoramic, Real_Case_Kernel) { + const int n = 2; + Mat x1(2, n); // From image 0.jpg + x1<< 158, 78, + 124, 113; + + Mat x2(2, n); // From image 3.jpg + x2<< 300, 214, + 125, 114; + + Mat3 Ground_TruthHomography; + Ground_TruthHomography<< 1, 0.02, 129.83, + -0.02, 1.012, 0.07823, + 0, 0, 1; + + vector<Mat3> Hs; + + libmv::panography::kernel::TwoPointSolver::Solve(x1, x2, &Hs); + + LOG(INFO) << "Got " << Hs.size() << " solutions."; + for (int j = 0; j < Hs.size(); ++j) { + Mat3 H = Hs[j]; + + EXPECT_MATRIX_NEAR(H, Ground_TruthHomography, 1e-1); + + Mat x1h, x2h; + EuclideanToHomogeneous(x1, &x1h); + EuclideanToHomogeneous(x2, &x2h); + + // Assert that residuals are small enough + for (int i = 0; i < n; ++i) { + Vec x1p = H * x1h.col(i); + Vec residuals = x1p/x1p(2) - x2h.col(i); + EXPECT_MATRIX_NEAR_ZERO(residuals, 1e-5); + } + } +} + +} // namespace +} // namespace libmv + +// TODO(pmoulon): Add a real test case based on images. +// TODO(pmoulon): Add a check for the actual f value for the real images. +// TODO(pmoulon): Add a test that has some inliers and outliers. diff --git a/intern/libmv/libmv/multiview/projection.cc b/intern/libmv/libmv/multiview/projection.cc new file mode 100644 index 00000000000..f8bece3de68 --- /dev/null +++ b/intern/libmv/libmv/multiview/projection.cc @@ -0,0 +1,224 @@ +// Copyright (c) 2007, 2008 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/projection.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P) { + P->block<3, 3>(0, 0) = R; + P->col(3) = t; + (*P) = K * (*P); +} + +void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) { + // Decompose using the RQ decomposition HZ A4.1.1 pag.579. + Mat3 K = P.block(0, 0, 3, 3); + + Mat3 Q; + Q.setIdentity(); + + // Set K(2,1) to zero. + if (K(2, 1) != 0) { + double c = -K(2, 2); + double s = K(2, 1); + double l = sqrt(c * c + s * s); + c /= l; + s /= l; + Mat3 Qx; + Qx << 1, 0, 0, + 0, c, -s, + 0, s, c; + K = K * Qx; + Q = Qx.transpose() * Q; + } + // Set K(2,0) to zero. + if (K(2, 0) != 0) { + double c = K(2, 2); + double s = K(2, 0); + double l = sqrt(c * c + s * s); + c /= l; + s /= l; + Mat3 Qy; + Qy << c, 0, s, + 0, 1, 0, + -s, 0, c; + K = K * Qy; + Q = Qy.transpose() * Q; + } + // Set K(1,0) to zero. + if (K(1, 0) != 0) { + double c = -K(1, 1); + double s = K(1, 0); + double l = sqrt(c * c + s * s); + c /= l; + s /= l; + Mat3 Qz; + Qz << c, -s, 0, + s, c, 0, + 0, 0, 1; + K = K * Qz; + Q = Qz.transpose() * Q; + } + + Mat3 R = Q; + + // Ensure that the diagonal is positive. + // TODO(pau) Change this to ensure that: + // - K(0,0) > 0 + // - K(2,2) = 1 + // - det(R) = 1 + if (K(2, 2) < 0) { + K = -K; + R = -R; + } + if (K(1, 1) < 0) { + Mat3 S; + S << 1, 0, 0, + 0, -1, 0, + 0, 0, 1; + K = K * S; + R = S * R; + } + if (K(0, 0) < 0) { + Mat3 S; + S << -1, 0, 0, + 0, 1, 0, + 0, 0, 1; + K = K * S; + R = S * R; + } + + // Compute translation. + Vec p(3); + p << P(0, 3), P(1, 3), P(2, 3); + // TODO(pau) This sould be done by a SolveLinearSystem(A, b, &x) call. + // TODO(keir) use the eigen LU solver syntax... + Vec3 t = K.inverse() * p; + + // scale K so that K(2,2) = 1 + K = K / K(2, 2); + + *Kp = K; + *Rp = R; + *tp = t; +} + +void ProjectionShiftPrincipalPoint(const Mat34 &P, + const Vec2 &principal_point, + const Vec2 &principal_point_new, + Mat34 *P_new) { + Mat3 T; + T << 1, 0, principal_point_new(0) - principal_point(0), + 0, 1, principal_point_new(1) - principal_point(1), + 0, 0, 1; + *P_new = T * P; +} + +void ProjectionChangeAspectRatio(const Mat34 &P, + const Vec2 &principal_point, + double aspect_ratio, + double aspect_ratio_new, + Mat34 *P_new) { + Mat3 T; + T << 1, 0, 0, + 0, aspect_ratio_new / aspect_ratio, 0, + 0, 0, 1; + Mat34 P_temp; + + ProjectionShiftPrincipalPoint(P, principal_point, Vec2(0, 0), &P_temp); + P_temp = T * P_temp; + ProjectionShiftPrincipalPoint(P_temp, Vec2(0, 0), principal_point, P_new); +} + +void HomogeneousToEuclidean(const Mat &H, Mat *X) { + int d = H.rows() - 1; + int n = H.cols(); + X->resize(d, n); + for (size_t i = 0; i < n; ++i) { + double h = H(d, i); + for (int j = 0; j < d; ++j) { + (*X)(j, i) = H(j, i) / h; + } + } +} + +void HomogeneousToEuclidean(const Mat3X &h, Mat2X *e) { + e->resize(2, h.cols()); + e->row(0) = h.row(0).array() / h.row(2).array(); + e->row(1) = h.row(1).array() / h.row(2).array(); +} +void HomogeneousToEuclidean(const Mat4X &h, Mat3X *e) { + e->resize(3, h.cols()); + e->row(0) = h.row(0).array() / h.row(3).array(); + e->row(1) = h.row(1).array() / h.row(3).array(); + e->row(2) = h.row(2).array() / h.row(3).array(); +} + +void HomogeneousToEuclidean(const Vec3 &H, Vec2 *X) { + double w = H(2); + *X << H(0) / w, H(1) / w; +} + +void HomogeneousToEuclidean(const Vec4 &H, Vec3 *X) { + double w = H(3); + *X << H(0) / w, H(1) / w, H(2) / w; +} + +void EuclideanToHomogeneous(const Mat &X, Mat *H) { + int d = X.rows(); + int n = X.cols(); + H->resize(d + 1, n); + H->block(0, 0, d, n) = X; + H->row(d).setOnes(); +} + +void EuclideanToHomogeneous(const Vec2 &X, Vec3 *H) { + *H << X(0), X(1), 1; +} + +void EuclideanToHomogeneous(const Vec3 &X, Vec4 *H) { + *H << X(0), X(1), X(2), 1; +} + +// TODO(julien) Call conditioning.h/ApplyTransformationToPoints ? +void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n) { + Mat3X x_image_h; + EuclideanToHomogeneous(x, &x_image_h); + Mat3X x_camera_h = K.inverse() * x_image_h; + HomogeneousToEuclidean(x_camera_h, n); +} + +void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n) { + Mat3X x_camera_h = K.inverse() * x; + HomogeneousToEuclidean(x_camera_h, n); +} + +double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X) { + return (R*X)(2) + t(2); +} + +double Depth(const Mat3 &R, const Vec3 &t, const Vec4 &X) { + Vec3 Xe = X.head<3>() / X(3); + return Depth(R, t, Xe); +} + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/projection.h b/intern/libmv/libmv/multiview/projection.h new file mode 100644 index 00000000000..3220bc2dbbc --- /dev/null +++ b/intern/libmv/libmv/multiview/projection.h @@ -0,0 +1,231 @@ +// Copyright (c) 2007, 2008 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_MULTIVIEW_PROJECTION_H_ +#define LIBMV_MULTIVIEW_PROJECTION_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +void P_From_KRt(const Mat3 &K, const Mat3 &R, const Vec3 &t, Mat34 *P); +void KRt_From_P(const Mat34 &P, Mat3 *K, Mat3 *R, Vec3 *t); + +// Applies a change of basis to the image coordinates of the projection matrix +// so that the principal point becomes principal_point_new. +void ProjectionShiftPrincipalPoint(const Mat34 &P, + const Vec2 &principal_point, + const Vec2 &principal_point_new, + Mat34 *P_new); + +// Applies a change of basis to the image coordinates of the projection matrix +// so that the aspect ratio becomes aspect_ratio_new. This is done by +// stretching the y axis. The aspect ratio is defined as the quotient between +// the focal length of the y and the x axis. +void ProjectionChangeAspectRatio(const Mat34 &P, + const Vec2 &principal_point, + double aspect_ratio, + double aspect_ratio_new, + Mat34 *P_new); + +void HomogeneousToEuclidean(const Mat &H, Mat *X); +void HomogeneousToEuclidean(const Mat3X &h, Mat2X *e); +void HomogeneousToEuclidean(const Mat4X &h, Mat3X *e); +void HomogeneousToEuclidean(const Vec3 &H, Vec2 *X); +void HomogeneousToEuclidean(const Vec4 &H, Vec3 *X); +inline Vec2 HomogeneousToEuclidean(const Vec3 &h) { + return h.head<2>() / h(2); +} +inline Vec3 HomogeneousToEuclidean(const Vec4 &h) { + return h.head<3>() / h(3); +} +inline Mat2X HomogeneousToEuclidean(const Mat3X &h) { + Mat2X e(2, h.cols()); + e.row(0) = h.row(0).array() / h.row(2).array(); + e.row(1) = h.row(1).array() / h.row(2).array(); + return e; +} + +void EuclideanToHomogeneous(const Mat &X, Mat *H); +inline Mat3X EuclideanToHomogeneous(const Mat2X &x) { + Mat3X h(3, x.cols()); + h.block(0, 0, 2, x.cols()) = x; + h.row(2).setOnes(); + return h; +} +inline void EuclideanToHomogeneous(const Mat2X &x, Mat3X *h) { + h->resize(3, x.cols()); + h->block(0, 0, 2, x.cols()) = x; + h->row(2).setOnes(); +} +inline Mat4X EuclideanToHomogeneous(const Mat3X &x) { + Mat4X h(4, x.cols()); + h.block(0, 0, 3, x.cols()) = x; + h.row(3).setOnes(); + return h; +} +inline void EuclideanToHomogeneous(const Mat3X &x, Mat4X *h) { + h->resize(4, x.cols()); + h->block(0, 0, 3, x.cols()) = x; + h->row(3).setOnes(); +} +void EuclideanToHomogeneous(const Vec2 &X, Vec3 *H); +void EuclideanToHomogeneous(const Vec3 &X, Vec4 *H); +inline Vec3 EuclideanToHomogeneous(const Vec2 &x) { + return Vec3(x(0), x(1), 1); +} +inline Vec4 EuclideanToHomogeneous(const Vec3 &x) { + return Vec4(x(0), x(1), x(2), 1); +} +// Conversion from image coordinates to normalized camera coordinates +void EuclideanToNormalizedCamera(const Mat2X &x, const Mat3 &K, Mat2X *n); +void HomogeneousToNormalizedCamera(const Mat3X &x, const Mat3 &K, Mat2X *n); + +inline Vec2 Project(const Mat34 &P, const Vec3 &X) { + Vec4 HX; + HX << X, 1.0; + Vec3 hx = P * HX; + return hx.head<2>() / hx(2); +} + +inline void Project(const Mat34 &P, const Vec4 &X, Vec3 *x) { + *x = P * X; +} + +inline void Project(const Mat34 &P, const Vec4 &X, Vec2 *x) { + Vec3 hx = P * X; + *x = hx.head<2>() / hx(2); +} + +inline void Project(const Mat34 &P, const Vec3 &X, Vec3 *x) { + Vec4 HX; + HX << X, 1.0; + Project(P, HX, x); +} + +inline void Project(const Mat34 &P, const Vec3 &X, Vec2 *x) { + Vec3 hx; + Project(P, X, x); + *x = hx.head<2>() / hx(2); +} + +inline void Project(const Mat34 &P, const Mat4X &X, Mat2X *x) { + x->resize(2, X.cols()); + for (int c = 0; c < X.cols(); ++c) { + Vec3 hx = P * X.col(c); + x->col(c) = hx.head<2>() / hx(2); + } +} + +inline Mat2X Project(const Mat34 &P, const Mat4X &X) { + Mat2X x; + Project(P, X, &x); + return x; +} + +inline void Project(const Mat34 &P, const Mat3X &X, Mat2X *x) { + x->resize(2, X.cols()); + for (int c = 0; c < X.cols(); ++c) { + Vec4 HX; + HX << X.col(c), 1.0; + Vec3 hx = P * HX; + x->col(c) = hx.head<2>() / hx(2); + } +} + +inline void Project(const Mat34 &P, const Mat3X &X, const Vecu &ids, Mat2X *x) { + x->resize(2, ids.size()); + Vec4 HX; + Vec3 hx; + for (int c = 0; c < ids.size(); ++c) { + HX << X.col(ids[c]), 1.0; + hx = P * HX; + x->col(c) = hx.head<2>() / hx(2); + } +} + +inline Mat2X Project(const Mat34 &P, const Mat3X &X) { + Mat2X x(2, X.cols()); + Project(P, X, &x); + return x; +} + +inline Mat2X Project(const Mat34 &P, const Mat3X &X, const Vecu &ids) { + Mat2X x(2, ids.size()); + Project(P, X, ids, &x); + return x; +} + +double Depth(const Mat3 &R, const Vec3 &t, const Vec3 &X); +double Depth(const Mat3 &R, const Vec3 &t, const Vec4 &X); + +/** +* Returns true if the homogenious 3D point X is in front of +* the camera P. +*/ +inline bool isInFrontOfCamera(const Mat34 &P, const Vec4 &X) { + double condition_1 = P.row(2).dot(X) * X[3]; + double condition_2 = X[2] * X[3]; + if (condition_1 > 0 && condition_2 > 0) + return true; + else + return false; +} + +inline bool isInFrontOfCamera(const Mat34 &P, const Vec3 &X) { + Vec4 X_homo; + X_homo.segment<3>(0) = X; + X_homo(3) = 1; + return isInFrontOfCamera( P, X_homo); +} + +/** +* Transforms a 2D point from pixel image coordinates to a 2D point in +* normalized image coordinates. +*/ +inline Vec2 ImageToNormImageCoordinates(Mat3 &Kinverse, Vec2 &x) { + Vec3 x_h = Kinverse*EuclideanToHomogeneous(x); + return HomogeneousToEuclidean( x_h ); +} + +/// Estimates the root mean square error (2D) +inline double RootMeanSquareError(const Mat2X &x_image, + const Mat4X &X_world, + const Mat34 &P) { + size_t num_points = x_image.cols(); + Mat2X dx = Project(P, X_world) - x_image; + return dx.norm() / num_points; +} + +/// Estimates the root mean square error (2D) +inline double RootMeanSquareError(const Mat2X &x_image, + const Mat3X &X_world, + const Mat3 &K, + const Mat3 &R, + const Vec3 &t) { + Mat34 P; + P_From_KRt(K, R, t, &P); + size_t num_points = x_image.cols(); + Mat2X dx = Project(P, X_world) - x_image; + return dx.norm() / num_points; +} +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_PROJECTION_H_ diff --git a/intern/libmv/libmv/multiview/projection_test.cc b/intern/libmv/libmv/multiview/projection_test.cc new file mode 100644 index 00000000000..460a186e7c4 --- /dev/null +++ b/intern/libmv/libmv/multiview/projection_test.cc @@ -0,0 +1,115 @@ +// Copyright (c) 2007, 2008 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 <iostream> + +#include "libmv/multiview/projection.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" + +namespace { +using namespace libmv; + +TEST(Projection, P_From_KRt) { + Mat3 K, Kp; + K << 10, 1, 30, + 0, 20, 40, + 0, 0, 1; + + Mat3 R, Rp; + R << 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + + Vec3 t, tp; + t << 1, 2, 3; + + Mat34 P; + P_From_KRt(K, R, t, &P); + KRt_From_P(P, &Kp, &Rp, &tp); + + EXPECT_MATRIX_NEAR(K, Kp, 1e-8); + EXPECT_MATRIX_NEAR(R, Rp, 1e-8); + EXPECT_MATRIX_NEAR(t, tp, 1e-8); + + // TODO(keir): Change the code to ensure det(R) == 1, which is not currently + // the case. Also add a test for that here. +} + +Vec4 GetRandomPoint() { + Vec4 X; + X.setRandom(); + X(3) = 1; + return X; +} + +TEST(Projection, isInFrontOfCamera) { + Mat34 P; + P << 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0; + + Vec4 X_front = GetRandomPoint(); + Vec4 X_back = GetRandomPoint(); + X_front(2) = 10; // Any point in the positive Z direction + // where Z > 1 is infront of the camera. + X_back(2) = -10; // Any point int he negative Z dirstaion + // is behind the camera. + + bool res_front = isInFrontOfCamera(P, X_front); + bool res_back = isInFrontOfCamera(P, X_back); + + EXPECT_TRUE(res_front); + EXPECT_FALSE(res_back); +} + +TEST(AutoCalibration, ProjectionShiftPrincipalPoint) { + Mat34 P1, P2; + P1 << 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0; + P2 << 1, 0, 3, 0, + 0, 1, 4, 0, + 0, 0, 1, 0; + Mat34 P1_computed, P2_computed; + ProjectionShiftPrincipalPoint(P1, Vec2(0, 0), Vec2(3, 4), &P2_computed); + ProjectionShiftPrincipalPoint(P2, Vec2(3, 4), Vec2(0, 0), &P1_computed); + + EXPECT_MATRIX_EQ(P1, P1_computed); + EXPECT_MATRIX_EQ(P2, P2_computed); +} + +TEST(AutoCalibration, ProjectionChangeAspectRatio) { + Mat34 P1, P2; + P1 << 1, 0, 3, 0, + 0, 1, 4, 0, + 0, 0, 1, 0; + P2 << 1, 0, 3, 0, + 0, 2, 4, 0, + 0, 0, 1, 0; + Mat34 P1_computed, P2_computed; + ProjectionChangeAspectRatio(P1, Vec2(3, 4), 1, 2, &P2_computed); + ProjectionChangeAspectRatio(P2, Vec2(3, 4), 2, 1, &P1_computed); + + EXPECT_MATRIX_EQ(P1, P1_computed); + EXPECT_MATRIX_EQ(P2, P2_computed); +} + +} // namespace diff --git a/intern/libmv/libmv/multiview/resection.h b/intern/libmv/libmv/multiview/resection.h new file mode 100644 index 00000000000..c142d6deeb2 --- /dev/null +++ b/intern/libmv/libmv/multiview/resection.h @@ -0,0 +1,62 @@ +// Copyright (c) 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. +// +// Compute the projection matrix from a set of 3D points X and their +// projections x = PX in 2D. This is useful if a point cloud is reconstructed. +// +// Algorithm is the standard DLT as described in Hartley & Zisserman, page 179. + +#ifndef LIBMV_MULTIVIEW_RESECTION_H +#define LIBMV_MULTIVIEW_RESECTION_H + +#include "libmv/logging/logging.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { +namespace resection { + +// x's are 2D image coordinates, (x,y,1), and X's are homogeneous four vectors. +template<typename T> +void Resection(const Matrix<T, 2, Dynamic> &x, + const Matrix<T, 4, Dynamic> &X, + Matrix<T, 3, 4> *P) { + int N = x.cols(); + assert(X.cols() == N); + + Matrix<T, Dynamic, 12> design(2*N, 12); + design.setZero(); + for (int i = 0; i < N; i++) { + T xi = x(0, i); + T yi = x(1, i); + // See equation (7.2) on page 179 of H&Z. + design.template block<1, 4>(2*i, 4) = -X.col(i).transpose(); + design.template block<1, 4>(2*i, 8) = yi*X.col(i).transpose(); + design.template block<1, 4>(2*i + 1, 0) = X.col(i).transpose(); + design.template block<1, 4>(2*i + 1, 8) = -xi*X.col(i).transpose(); + } + Matrix<T, 12, 1> p; + Nullspace(&design, &p); + reshape(p, 3, 4, P); +} + +} // namespace resection +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_RESECTION_H diff --git a/intern/libmv/libmv/multiview/resection_test.cc b/intern/libmv/libmv/multiview/resection_test.cc new file mode 100644 index 00000000000..368e2281cfa --- /dev/null +++ b/intern/libmv/libmv/multiview/resection_test.cc @@ -0,0 +1,61 @@ +// Copyright (c) 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 <iostream> + +#include "libmv/multiview/projection.h" +#include "libmv/multiview/resection.h" +#include "libmv/multiview/test_data_sets.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" +#include "libmv/logging/logging.h" + +namespace { + +using namespace libmv; +using namespace libmv::resection; + +TEST(Resection, ThreeViews) { + int nviews = 5; + int npoints = 6; + NViewDataSet d = NRealisticCamerasFull(nviews, npoints); + for (int i = 0; i < nviews; ++i) { + Mat4X X(4, npoints); + X.block(0, 0, 3, npoints) = d.X; + X.row(3).setOnes(); + const Mat2X &x = d.x[i]; + Mat34 P; + Resection(x, X, &P); + Mat34 P_expected = d.P(i); + + // Because the P matrices are homogeneous, it is necessary to be tricky + // about the scale factor to make them match. + P_expected *= 1/P_expected.array().abs().sum(); + P *= 1/P.array().abs().sum(); + if (!((P(0, 0) > 0 && P_expected(0, 0) > 0) || + (P(0, 0) < 0 && P_expected(0, 0) < 0))) { + P *= -1; + } + + EXPECT_MATRIX_NEAR(P_expected, P, 1e-9); + } +} + +} // namespace diff --git a/intern/libmv/libmv/multiview/test_data_sets.cc b/intern/libmv/libmv/multiview/test_data_sets.cc new file mode 100644 index 00000000000..110bde6f762 --- /dev/null +++ b/intern/libmv/libmv/multiview/test_data_sets.cc @@ -0,0 +1,196 @@ +// Copyright (c) 2007, 2008 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/test_data_sets.h" + +#include <cmath> + +#include "libmv/numeric/numeric.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/fundamental.h" + +namespace libmv { + +TwoViewDataSet TwoRealisticCameras(bool same_K) { + TwoViewDataSet d; + + d.K1 << 320, 0, 160, + 0, 320, 120, + 0, 0, 1; + if (same_K) { + d.K2 = d.K1; + } else { + d.K2 << 360, 0, 170, + 0, 360, 110, + 0, 0, 1; + } + d.R1 = RotationAroundZ(-0.1); + d.R2 = RotationAroundX(-0.1); + d.t1 << 1, 1, 10; + d.t2 << -2, -1, 10; + P_From_KRt(d.K1, d.R1, d.t1, &d.P1); + P_From_KRt(d.K2, d.R2, d.t2, &d.P2); + + FundamentalFromProjections(d.P1, d.P2, &d.F); + + d.X.resize(3, 30); + d.X.setRandom(); + + Project(d.P1, d.X, &d.x1); + Project(d.P2, d.X, &d.x2); + + return d; +} + +nViewDatasetConfigator::nViewDatasetConfigator(int fx , int fy, + int cx, int cy, + double distance, + double jitter_amount) { + _fx = fx; + _fy = fy; + _cx = cx; + _cy = cy; + _dist = distance; + _jitter_amount = jitter_amount; +} + +NViewDataSet NRealisticCamerasFull(int nviews, int npoints, + const nViewDatasetConfigator config) { + NViewDataSet d; + d.n = nviews; + d.K.resize(nviews); + d.R.resize(nviews); + d.t.resize(nviews); + d.C.resize(nviews); + d.x.resize(nviews); + d.x_ids.resize(nviews); + + d.X.resize(3, npoints); + d.X.setRandom(); + d.X *= 0.6; + + Vecu all_point_ids(npoints); + for (size_t j = 0; j < npoints; ++j) + all_point_ids[j] = j; + + for (size_t i = 0; i < nviews; ++i) { + Vec3 camera_center, t, jitter, lookdir; + + double theta = i * 2 * M_PI / nviews; + camera_center << sin(theta), 0.0, cos(theta); + camera_center *= config._dist; + d.C[i] = camera_center; + + jitter.setRandom(); + jitter *= config._jitter_amount / camera_center.norm(); + lookdir = -camera_center + jitter; + + d.K[i] << config._fx, 0, config._cx, + 0, config._fy, config._cy, + 0, 0, 1; + d.R[i] = LookAt(lookdir); + d.t[i] = -d.R[i] * camera_center; + d.x[i] = Project(d.P(i), d.X); + d.x_ids[i] = all_point_ids; + } + return d; +} + + +NViewDataSet NRealisticCamerasSparse(int nviews, int npoints, + float view_ratio, unsigned min_projections, + const nViewDatasetConfigator config) { + assert(view_ratio <= 1.0); + assert(view_ratio > 0.0); + assert(min_projections <= npoints); + NViewDataSet d; + d.n = nviews; + d.K.resize(nviews); + d.R.resize(nviews); + d.t.resize(nviews); + d.C.resize(nviews); + d.x.resize(nviews); + d.x_ids.resize(nviews); + + d.X.resize(3, npoints); + d.X.setRandom(); + d.X *= 0.6; + + Mat visibility(nviews, npoints); + visibility.setZero(); + Mat randoms(nviews, npoints); + randoms.setRandom(); + randoms = (randoms.array() + 1)/2.0; + unsigned num_visibles = 0; + for (size_t i = 0; i < nviews; ++i) { + num_visibles = 0; + for (size_t j = 0; j < npoints; j++) { + if (randoms(i, j) <= view_ratio) { + visibility(i, j) = true; + num_visibles++; + } + } + if (num_visibles < min_projections) { + unsigned num_projections_to_add = min_projections - num_visibles; + for (size_t j = 0; j < npoints && num_projections_to_add > 0; ++j) { + if (!visibility(i, j)) { + num_projections_to_add--; + } + } + num_visibles += num_projections_to_add; + } + d.x_ids[i].resize(num_visibles); + d.x[i].resize(2, num_visibles); + } + + size_t j_visible = 0; + Vec3 X; + for (size_t i = 0; i < nviews; ++i) { + Vec3 camera_center, t, jitter, lookdir; + + double theta = i * 2 * M_PI / nviews; + camera_center << sin(theta), 0.0, cos(theta); + camera_center *= config._dist; + d.C[i] = camera_center; + + jitter.setRandom(); + jitter *= config._jitter_amount / camera_center.norm(); + lookdir = -camera_center + jitter; + + d.K[i] << config._fx, 0, config._cx, + 0, config._fy, config._cy, + 0, 0, 1; + d.R[i] = LookAt(lookdir); + d.t[i] = -d.R[i] * camera_center; + j_visible = 0; + for (size_t j = 0; j < npoints; j++) { + if (visibility(i, j)) { + X = d.X.col(j); + d.x[i].col(j_visible) = Project(d.P(i), X); + d.x_ids[i][j_visible] = j; + j_visible++; + } + } + } + return d; +} + + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/test_data_sets.h b/intern/libmv/libmv/multiview/test_data_sets.h new file mode 100644 index 00000000000..cf01663ca02 --- /dev/null +++ b/intern/libmv/libmv/multiview/test_data_sets.h @@ -0,0 +1,105 @@ +// Copyright (c) 2007, 2008 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_MULTIVIEW_TEST_DATA_SETS_H_ +#define LIBMV_MULTIVIEW_TEST_DATA_SETS_H_ + +#include "libmv/base/vector.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/multiview/projection.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +struct TwoViewDataSet { + Mat3 K1, K2; // Internal parameters. + Mat3 R1, R2; // Rotation. + Vec3 t1, t2; // Translation. + Mat34 P1, P2; // Projection matrix, P = K(R|t) + Mat3 F; // Fundamental matrix. + Mat3X X; // 3D points. + Mat2X x1, x2; // Projected points. +}; + +// Two cameras at (-1,-1,-10) and (2,1,-10) looking approximately towards z+. +TwoViewDataSet TwoRealisticCameras(bool same_K = false); + +// An N-view metric dataset . An important difference between this +// and the other reconstruction data types is that all points are seen by all +// cameras. +struct NViewDataSet { + vector<Mat3> K; // Internal parameters (fx, fy, etc). + vector<Mat3> R; // Rotation. + vector<Vec3> t; // Translation. + vector<Vec3> C; // Camera centers. + Mat3X X; // 3D points. + vector<Mat2X> x; // Projected points; may have noise added. + vector<Vecu> x_ids; // Indexes of points corresponding to the projections + + int n; // Actual number of cameras. + + Mat34 P(int i) { + assert(i < n); + return K[i] * HStack(R[i], t[i]); + } + Mat3 F(int i, int j) { + Mat3 F_; + FundamentalFromProjections(P(i), P(j), &F_); + return F_; + } + void Reproject() { + for (int i = 0; i < n; ++i) { + x[i] = Project(P(i), X); + } + } + // TODO(keir): Add gaussian jitter functions. +}; + +struct nViewDatasetConfigator { + /// Internal camera parameters + int _fx; + int _fy; + int _cx; + int _cy; + + /// Camera random position parameters + double _dist; + double _jitter_amount; + + nViewDatasetConfigator(int fx = 1000, int fy = 1000, + int cx = 500, int cy = 500, + double distance = 1.5, + double jitter_amount = 0.01); +}; + +NViewDataSet NRealisticCamerasFull(int nviews, int npoints, + const nViewDatasetConfigator + config = nViewDatasetConfigator()); + +// Generates sparse projections (not all points are projected) +NViewDataSet NRealisticCamerasSparse(int nviews, int npoints, + float view_ratio = 0.6, + unsigned min_projections = 3, + const nViewDatasetConfigator + config = nViewDatasetConfigator()); + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_TEST_DATA_SETS_H_ diff --git a/intern/libmv/libmv/multiview/triangulation.cc b/intern/libmv/libmv/multiview/triangulation.cc new file mode 100644 index 00000000000..4d146c8f21b --- /dev/null +++ b/intern/libmv/libmv/multiview/triangulation.cc @@ -0,0 +1,50 @@ +// Copyright (c) 2007, 2008 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/triangulation.h" + +#include "libmv/numeric/numeric.h" +#include "libmv/multiview/projection.h" + +namespace libmv { + +// HZ 12.2 pag.312 +void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, + const Mat34 &P2, const Vec2 &x2, + Vec4 *X_homogeneous) { + Mat4 design; + for (int i = 0; i < 4; ++i) { + design(0, i) = x1(0) * P1(2, i) - P1(0, i); + design(1, i) = x1(1) * P1(2, i) - P1(1, i); + design(2, i) = x2(0) * P2(2, i) - P2(0, i); + design(3, i) = x2(1) * P2(2, i) - P2(1, i); + } + Nullspace(&design, X_homogeneous); +} + +void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, + const Mat34 &P2, const Vec2 &x2, + Vec3 *X_euclidean) { + Vec4 X_homogeneous; + TriangulateDLT(P1, x1, P2, x2, &X_homogeneous); + HomogeneousToEuclidean(X_homogeneous, X_euclidean); +} + +} // namespace libmv diff --git a/intern/libmv/libmv/multiview/triangulation.h b/intern/libmv/libmv/multiview/triangulation.h new file mode 100644 index 00000000000..be878890242 --- /dev/null +++ b/intern/libmv/libmv/multiview/triangulation.h @@ -0,0 +1,38 @@ +// Copyright (c) 2007, 2008 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_MULTIVIEW_TRIANGULATION_H_ +#define LIBMV_MULTIVIEW_TRIANGULATION_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, + const Mat34 &P2, const Vec2 &x2, + Vec4 *X_homogeneous); + +void TriangulateDLT(const Mat34 &P1, const Vec2 &x1, + const Mat34 &P2, const Vec2 &x2, + Vec3 *X_euclidean); + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_TRIANGULATION_H_ diff --git a/intern/libmv/libmv/multiview/triangulation_test.cc b/intern/libmv/libmv/multiview/triangulation_test.cc new file mode 100644 index 00000000000..66d1ee25a62 --- /dev/null +++ b/intern/libmv/libmv/multiview/triangulation_test.cc @@ -0,0 +1,47 @@ +// Copyright (c) 2007, 2008 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 <iostream> + +#include "libmv/multiview/triangulation.h" +#include "libmv/multiview/fundamental.h" +#include "libmv/multiview/projection.h" +#include "libmv/multiview/test_data_sets.h" +#include "libmv/numeric/numeric.h" +#include "testing/testing.h" + +namespace { +using namespace libmv; + +TEST(Triangulation, TriangulateDLT) { + TwoViewDataSet d = TwoRealisticCameras(); + + for (int i = 0; i < d.X.cols(); ++i) { + Vec2 x1, x2; + MatrixColumn(d.x1, i, &x1); + MatrixColumn(d.x2, i, &x2); + Vec3 X_estimated, X_gt; + MatrixColumn(d.X, i, &X_gt); + TriangulateDLT(d.P1, x1, d.P2, x2, &X_estimated); + EXPECT_NEAR(0, DistanceLInfinity(X_estimated, X_gt), 1e-8); + } +} + +} // namespace diff --git a/intern/libmv/libmv/multiview/two_view_kernel.h b/intern/libmv/libmv/multiview/two_view_kernel.h new file mode 100644 index 00000000000..7af0ed5ddab --- /dev/null +++ b/intern/libmv/libmv/multiview/two_view_kernel.h @@ -0,0 +1,137 @@ +// Copyright (c) 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. + +#ifndef LIBMV_MULTIVIEW_TWO_VIEW_KERNEL_H_ +#define LIBMV_MULTIVIEW_TWO_VIEW_KERNEL_H_ + +#include "libmv/base/vector.h" +#include "libmv/logging/logging.h" +#include "libmv/multiview/conditioning.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { +namespace two_view { +namespace kernel { + +template<typename Solver, typename Unnormalizer> +struct NormalizedSolver { + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + static void Solve(const Mat &x1, const Mat &x2, vector<Mat3> *models) { + assert(2 == x1.rows()); + assert(MINIMUM_SAMPLES <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Normalize the data. + Mat3 T1, T2; + Mat x1_normalized, x2_normalized; + NormalizePoints(x1, &x1_normalized, &T1); + NormalizePoints(x2, &x2_normalized, &T2); + + Solver::Solve(x1_normalized, x2_normalized, models); + + for (int i = 0; i < models->size(); ++i) { + Unnormalizer::Unnormalize(T1, T2, &(*models)[i]); + } + } +}; + +template<typename Solver, typename Unnormalizer> +struct IsotropicNormalizedSolver { + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + static void Solve(const Mat &x1, const Mat &x2, vector<Mat3> *models) { + assert(2 == x1.rows()); + assert(MINIMUM_SAMPLES <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + // Normalize the data. + Mat3 T1, T2; + Mat x1_normalized, x2_normalized; + NormalizeIsotropicPoints(x1, &x1_normalized, &T1); + NormalizeIsotropicPoints(x2, &x2_normalized, &T2); + + Solver::Solve(x1_normalized, x2_normalized, models); + + for (int i = 0; i < models->size(); ++i) { + Unnormalizer::Unnormalize(T1, T2, &(*models)[i]); + } + } +}; +// This is one example (targeted at solvers that operate on correspondences +// between two views) that shows the "kernel" part of a robust fitting +// problem: +// +// 1. The model; Mat3 in the case of the F or H matrix. +// 2. The minimum number of samples needed to fit; 7 or 8 (or 4). +// 3. A way to convert samples to a model. +// 4. A way to convert a sample and a model to an error. +// +// Of particular note is that the kernel does not expose what the samples are. +// All the robust fitting algorithm sees is that there is some number of +// samples; it is able to fit subsets of them (via the kernel) and check their +// error, but can never access the samples themselves. +// +// The Kernel objects must follow the following concept so that the robust +// fitting alogrithm can fit this type of relation: +// +// 1. Kernel::Model +// 2. Kernel::MINIMUM_SAMPLES +// 3. Kernel::Fit(vector<int>, vector<Kernel::Model> *) +// 4. Kernel::Error(int, Model) -> error +// +// The fit routine must not clear existing entries in the vector of models; it +// should append new solutions to the end. +template<typename SolverArg, + typename ErrorArg, + typename ModelArg = Mat3> +class Kernel { + public: + Kernel(const Mat &x1, const Mat &x2) : x1_(x1), x2_(x2) {} + typedef SolverArg Solver; + typedef ModelArg Model; + enum { MINIMUM_SAMPLES = Solver::MINIMUM_SAMPLES }; + void Fit(const vector<int> &samples, vector<Model> *models) const { + Mat x1 = ExtractColumns(x1_, samples); + Mat x2 = ExtractColumns(x2_, samples); + Solver::Solve(x1, x2, models); + } + double Error(int sample, const Model &model) const { + return ErrorArg::Error(model, + static_cast<Vec>(x1_.col(sample)), + static_cast<Vec>(x2_.col(sample))); + } + int NumSamples() const { + return x1_.cols(); + } + static void Solve(const Mat &x1, const Mat &x2, vector<Model> *models) { + // By offering this, Kernel types can be passed to templates. + Solver::Solve(x1, x2, models); + } + protected: + const Mat &x1_; + const Mat &x2_; +}; + +} // namespace kernel +} // namespace two_view +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_TWO_VIEW_KERNEL_H_ |