// 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