Welcome to mirror list, hosted at ThFree Co, Russian Federation.

git.blender.org/blender.git - Unnamed repository; edit this file 'description' to name the repository.
summaryrefslogtreecommitdiff
path: root/intern/libmv/libmv/multiview/euclidean_resection.cc
diff options
context:
space:
mode:
Diffstat (limited to 'intern/libmv/libmv/multiview/euclidean_resection.cc')
-rw-r--r--intern/libmv/libmv/multiview/euclidean_resection.cc198
1 files changed, 103 insertions, 95 deletions
diff --git a/intern/libmv/libmv/multiview/euclidean_resection.cc b/intern/libmv/libmv/multiview/euclidean_resection.cc
index 16a1a0caafa..249d7ebef3d 100644
--- a/intern/libmv/libmv/multiview/euclidean_resection.cc
+++ b/intern/libmv/libmv/multiview/euclidean_resection.cc
@@ -23,8 +23,8 @@
#include <cmath>
#include <limits>
-#include <Eigen/SVD>
#include <Eigen/Geometry>
+#include <Eigen/SVD>
#include "libmv/base/vector.h"
#include "libmv/logging/logging.h"
@@ -35,9 +35,10 @@ namespace euclidean_resection {
typedef unsigned int uint;
-bool EuclideanResection(const Mat2X &x_camera,
- const Mat3X &X_world,
- Mat3 *R, Vec3 *t,
+bool EuclideanResection(const Mat2X& x_camera,
+ const Mat3X& X_world,
+ Mat3* R,
+ Vec3* t,
ResectionMethod method) {
switch (method) {
case RESECTION_ANSAR_DANIILIDIS:
@@ -49,20 +50,20 @@ bool EuclideanResection(const Mat2X &x_camera,
case RESECTION_PPNP:
return EuclideanResectionPPnP(x_camera, X_world, R, t);
break;
- default:
- LOG(FATAL) << "Unknown resection method.";
+ 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,
+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();
+ << "Invalid size for x_image: " << x_image.rows() << "x"
+ << x_image.cols();
Mat2X x_camera;
if (x_image.rows() == 2) {
@@ -73,18 +74,15 @@ bool EuclideanResection(const Mat &x_image,
return EuclideanResection(x_camera, X_world, R, t, method);
}
-void AbsoluteOrientation(const Mat3X &X,
- const Mat3X &Xp,
- Mat3 *R,
- Vec3 *t) {
+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 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;
+ Xn.col(i) = X.col(i) - C;
Xpn.col(i) = Xp.col(i) - Cp;
}
@@ -100,10 +98,12 @@ void AbsoluteOrientation(const Mat3X &X,
double Szy = Xn.row(2).dot(Xpn.row(1));
Mat4 N;
+ // clang-format off
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;
+ // clang-format on
// Find the unit quaternion q that maximizes qNq. It is the eigenvector
// corresponding to the lagest eigenvalue.
@@ -118,6 +118,7 @@ void AbsoluteOrientation(const Mat3X &X,
double q1q3 = q(1) * q(3);
double q2q3 = q(2) * q(3);
+ // clang-format off
(*R) << qq(0) + qq(1) - qq(2) - qq(3),
2 * (q1q2 - q0q3),
2 * (q1q3 + q0q2),
@@ -127,6 +128,7 @@ void AbsoluteOrientation(const Mat3X &X,
2 * (q1q3 - q0q2),
2 * (q2q3 + q0q1),
qq(0) - qq(1) - qq(2) + qq(3);
+ // clang-format on
// Fix the handedness of the R matrix.
if (R->determinant() < 0) {
@@ -176,9 +178,7 @@ static int Sign(double value) {
// 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) {
+static Vec MatrixToConstraint(const Mat& A, int num_k_columns, int num_lambda) {
Vec C(num_k_columns);
C.setZero();
int idx = 0;
@@ -195,17 +195,17 @@ static Vec MatrixToConstraint(const Mat &A,
}
// Normalizes the columns of vectors.
-static void NormalizeColumnVectors(Mat3X *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) {
+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);
@@ -229,14 +229,14 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
// 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) {
+ 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;
+ -center_to_point_distance * center_to_point_distance;
ij_index(row, 0) = i;
ij_index(row, 1) = j;
++row;
@@ -246,17 +246,17 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
}
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);
+ 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_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();
@@ -275,8 +275,8 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
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),
+ MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2) -
+ V.row(idx3).transpose() * V.row(idx4),
num_k_columns,
num_lambda);
++counter_k_row;
@@ -296,8 +296,8 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
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),
+ MatrixToConstraint(V.row(idx1).transpose() * V.row(idx2) -
+ V.row(idx3).transpose() * V.row(idx4),
num_k_columns,
num_lambda);
++counter_k_row;
@@ -317,14 +317,12 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
}
}
// 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)));
+ 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)));
+ 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) {
@@ -353,9 +351,9 @@ void EuclideanResectionAnsarDaniilidis(const Mat2X &x_camera,
}
// Selects 4 virtual control points using mean and PCA.
-static void SelectControlPoints(const Mat3X &X_world,
- Mat *X_centered,
- Mat34 *X_control_points) {
+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.
@@ -379,13 +377,13 @@ static void SelectControlPoints(const Mat3X &X_world,
}
// Computes the barycentric coordinates for all real points
-static void ComputeBarycentricCoordinates(const Mat3X &X_world_centered,
- const Mat34 &X_control_points,
- Mat4X *alphas) {
+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);
+ C2.col(c - 1) = X_control_points.col(c) - X_control_points.col(0);
}
Mat3 C2inv = C2.inverse();
@@ -401,14 +399,15 @@ static void ComputeBarycentricCoordinates(const Mat3X &X_world_centered,
// 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) {
+ 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();
+ 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();
@@ -436,9 +435,10 @@ static void ComputePointsCoordinatesInCameraFrame(
}
}
-bool EuclideanResectionEPnP(const Mat2X &x_camera,
- const Mat3X &X_world,
- Mat3 *R, Vec3 *t) {
+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();
@@ -462,6 +462,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
double a3 = alphas(3, c);
double ui = x_camera(0, c);
double vi = x_camera(1, c);
+ // clang-format off
M.block(2*c, 0, 2, 12) << a0, 0,
a0*(-ui), a1, 0,
a1*(-ui), a2, 0,
@@ -471,10 +472,11 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
a1, a1*(-vi), 0,
a2, a2*(-vi), 0,
a3, a3*(-vi);
+ // clang-format on
}
// TODO(julien): Avoid the transpose by rewriting the u2.block() calls.
- Eigen::JacobiSVD<Mat> MtMsvd(M.transpose()*M, Eigen::ComputeFullU);
+ Eigen::JacobiSVD<Mat> MtMsvd(M.transpose() * M, Eigen::ComputeFullU);
Eigen::Matrix<double, 12, 12> u2 = MtMsvd.matrixU().transpose();
// Estimate the L matrix.
@@ -495,21 +497,22 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
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);
+ 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++) {
+ // clang-format off
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)),
@@ -520,19 +523,23 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
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));
+ // clang-format on
}
Vec6 rho;
+ // clang-format off
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();
+ // clang-format on
// 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();
+ Mat3 K;
+ K.setIdentity();
vector<Mat3> Rs(3);
vector<Vec3> ts(3);
Vec rmse(3);
@@ -546,7 +553,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
//
// 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();
+ 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]
@@ -563,7 +570,7 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
if (b4(0) < 0) {
b4 = -b4;
}
- b4(0) = std::sqrt(b4(0));
+ 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]);
@@ -669,12 +676,12 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
// 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]
@@ -708,33 +715,34 @@ bool EuclideanResectionEPnP(const Mat2X &x_camera,
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) {
+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
@@ -748,20 +756,21 @@ bool EuclideanResectionPPnP(const Mat2X &x_camera,
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());
+ 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;
+ E = Y - Z * PR;
error = (E - E_old).norm();
LG << "PPnP error(" << (iteration++) << "): " << error;
E_old = E;
}
- *t = -*R*c;
+ *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
@@ -769,6 +778,5 @@ bool EuclideanResectionPPnP(const Mat2X &x_camera,
return true;
}
-
-} // namespace resection
+} // namespace euclidean_resection
} // namespace libmv
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-15 20:53:11 +0300