Skip to content

E
eos
Commit 5047d78f authored by Patrik Huber's avatar Patrik Huber
Browse files
Options

Changed linear shape fitting to use Eigen 

All matrix operations and solving the linear system is now done with Eigen matrices.
We use colPivHouseholderQr for now to solve the linear system, which doesn't have any requirements on the matrix.
parent 4dea2703
Hide whitespace changes
Inline Side-by-side
Showing with 32 additions and 50 deletions
include/eos/fitting/linear_shape_fitting.hpp
View file @ 5047d78f
......@@ -24,7 +24,7 @@
#include "eos/morphablemodel/MorphableModel.hpp"
//#include "Eigen/LU"
#include "Eigen/QR"
#include "opencv2/core/core.hpp"
......@@ -59,9 +59,11 @@ namespace eos {
*/
inline std::vector<float> fit_shape_to_landmarks_linear(const morphablemodel::MorphableModel& morphable_model, cv::Mat affine_camera_matrix, const std::vector<cv::Vec2f>& landmarks, const std::vector<int>& vertex_ids, Eigen::VectorXf base_face=Eigen::VectorXf(), float lambda=3.0f, boost::optional<int> num_coefficients_to_fit=boost::optional<int>(), boost::optional<float> detector_standard_deviation=boost::optional<float>(), boost::optional<float> model_standard_deviation=boost::optional<float>())
{
using cv::Mat;
assert(landmarks.size() == vertex_ids.size());
using Eigen::VectorXf;
using Eigen::MatrixXf;
int num_coeffs_to_fit = num_coefficients_to_fit.get_value_or(morphable_model.get_shape_model().get_num_principal_components());
int num_landmarks = static_cast<int>(landmarks.size());
......@@ -72,21 +74,18 @@ inline std::vector<float> fit_shape_to_landmarks_linear(const morphablemodel::Mo
// $\hat{V} \in R^{3N\times m-1}$, subselect the rows of the eigenvector matrix $V$ associated with the $N$ feature points
// And we insert a row of zeros after every third row, resulting in matrix $\hat{V}_h \in R^{4N\times m-1}$:
Mat V_hat_h = Mat::zeros(4 * num_landmarks, num_coeffs_to_fit, CV_32FC1);
MatrixXf V_hat_h = MatrixXf::Zero(4 * num_landmarks, num_coeffs_to_fit);
int row_index = 0;
for (int i = 0; i < num_landmarks; ++i) {
Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> basis_rows_ = morphable_model.get_shape_model().get_rescaled_pca_basis_at_point(vertex_ids[i]); // In the paper, the orthonormal basis might be used? I'm not sure, check it. It's even a mess in the paper. PH 26.5.2014: I think the rescaled basis is fine/better.
// Above converts to a RowMajor matrix on return - for now, since the core algorithm still uses cv::Mat (and OpenCV stores data in row-major memory order).
Mat basis_rows = Mat(basis_rows_.rows(), basis_rows_.cols(), CV_32FC1, basis_rows_.data());
//basisRows.copyTo(V_hat_h.rowRange(rowIndex, rowIndex + 3));
basis_rows.colRange(0, num_coeffs_to_fit).copyTo(V_hat_h.rowRange(row_index, row_index + 3));
MatrixXf basis_rows_ = morphable_model.get_shape_model().get_rescaled_pca_basis_at_point(vertex_ids[i]); // In the paper, the orthonormal basis might be used? I'm not sure, check it. It's even a mess in the paper. PH 26.5.2014: I think the rescaled basis is fine/better.
V_hat_h.block(row_index, 0, 3, V_hat_h.cols()) = basis_rows_.block(0, 0, basis_rows_.rows(), num_coeffs_to_fit);
row_index += 4; // replace 3 rows and skip the 4th one, it has all zeros
}
// Form a block diagonal matrix $P \in R^{3N\times 4N}$ in which the camera matrix C (P_Affine, affine_camera_matrix) is placed on the diagonal:
Mat P = Mat::zeros(3 * num_landmarks, 4 * num_landmarks, CV_32FC1);
MatrixXf P = MatrixXf::Zero(3 * num_landmarks, 4 * num_landmarks);
for (int i = 0; i < num_landmarks; ++i) {
Mat submatrix_to_replace = P.colRange(4 * i, (4 * i) + 4).rowRange(3 * i, (3 * i) + 3);
affine_camera_matrix.copyTo(submatrix_to_replace);
using RowMajorMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
P.block(3 * i, 4 * i, 3, 4) = Eigen::Map<RowMajorMatrixXf>(affine_camera_matrix.ptr<float>(), affine_camera_matrix.rows, affine_camera_matrix.cols);
}
// The variances: Add the 2D and 3D standard deviations.
// If the user doesn't provide them, we choose the following:
......@@ -94,55 +93,38 @@ inline std::vector<float> fit_shape_to_landmarks_linear(const morphablemodel::Mo
// 3D (model) variance: 0.0f. It only makes sense to set it to something when we have a different variance for different vertices.
// The 3D variance has to be projected to 2D (for details, see paper [1]) so the units do match up.
float sigma_squared_2D = std::pow(detector_standard_deviation.get_value_or(std::sqrt(3.0f)), 2) + std::pow(model_standard_deviation.get_value_or(0.0f), 2);
Mat Omega = Mat::zeros(3 * num_landmarks, 3 * num_landmarks, CV_32FC1);
MatrixXf Omega = MatrixXf::Zero(3 * num_landmarks, 3 * num_landmarks);
for (int i = 0; i < 3 * num_landmarks; ++i) {
// Sigma(i, i) = sqrt(sigma_squared_2D), but then Omega is Sigma.t() * Sigma (squares the diagonal) - so we just assign 1/sigma_squared_2D to Omega here:
Omega.at<float>(i, i) = 1.0f / sigma_squared_2D; // the higher the sigma_squared_2D, the smaller the diagonal entries of Sigma will be
Omega(i, i) = 1.0f / sigma_squared_2D;
}
// The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$
Mat y = Mat::ones(3 * num_landmarks, 1, CV_32FC1);
VectorXf y = VectorXf::Ones(3 * num_landmarks);
for (int i = 0; i < num_landmarks; ++i) {
y.at<float>(3 * i, 0) = landmarks[i][0];
y.at<float>((3 * i) + 1, 0) = landmarks[i][1];
//y.at<float>((3 * i) + 2, 0) = 1; // already 1, stays (homogeneous coordinate)
y(3 * i) = landmarks[i][0];
y((3 * i) + 1) = landmarks[i][1];
//y((3 * i) + 2) = 1; // already 1, stays (homogeneous coordinate)
}
// The mean, with an added homogeneous coordinate (x_1, y_1, z_1, 1, x_2, ...)^t
Mat v_bar = Mat::ones(4 * num_landmarks, 1, CV_32FC1);
VectorXf v_bar = VectorXf::Ones(4 * num_landmarks);
for (int i = 0; i < num_landmarks; ++i) {
//cv::Vec4f model_mean = morphable_model.get_shape_model().get_mean_at_point(vertex_ids[i]);
cv::Vec4f model_mean(base_face(vertex_ids[i] * 3), base_face(vertex_ids[i] * 3 + 1), base_face(vertex_ids[i] * 3 + 2), 1.0f);
v_bar.at<float>(4 * i, 0) = model_mean[0];
v_bar.at<float>((4 * i) + 1, 0) = model_mean[1];
v_bar.at<float>((4 * i) + 2, 0) = model_mean[2];
//v_bar.at<float>((4 * i) + 3, 0) = 1; // already 1, stays (homogeneous coordinate)
// note: now that a Vec4f is returned, we could use copyTo?
const cv::Vec4f model_mean(base_face(vertex_ids[i] * 3), base_face(vertex_ids[i] * 3 + 1), base_face(vertex_ids[i] * 3 + 2), 1.0f);
v_bar(4 * i) = model_mean[0];
v_bar((4 * i) + 1) = model_mean[1];
v_bar((4 * i) + 2) = model_mean[2];
//v_bar((4 * i) + 3) = 1; // already 1, stays (homogeneous coordinate)
}
// Bring into standard regularised quadratic form with diagonal distance matrix Omega
Mat A = P * V_hat_h; // camera matrix times the basis
Mat b = P * v_bar - y; // camera matrix times the mean, minus the landmarks.
//Mat c_s; // The x, we solve for this! (the variance-normalised shape parameter vector, $c_s = [a_1/sigma_{s,1} , ..., a_m-1/sigma_{s,m-1}]^t$
//int numShapePc = morphableModel.getShapeModel().getNumberOfPrincipalComponents();
const int num_shape_pc = num_coeffs_to_fit;
Mat AtOmegaA = A.t() * Omega * A;
Mat AtOmegaAReg = AtOmegaA + lambda * Mat::eye(num_shape_pc, num_shape_pc, CV_32FC1);
// Invert (and perform some sanity checks) using Eigen:
/* using RowMajorMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
Eigen::Map<RowMajorMatrixXf> AtOmegaAReg_Eigen(AtOmegaAReg.ptr<float>(), AtOmegaAReg.rows, AtOmegaAReg.cols);
Eigen::FullPivLU<RowMajorMatrixXf> luOfAtOmegaAReg(AtOmegaAReg_Eigen); // Calculate the full-pivoting LU decomposition of the regularized AtA. Note: We could also try FullPivHouseholderQR if our system is non-minimal (i.e. there are more constraints than unknowns).
auto rankOfAtOmegaAReg = luOfAtOmegaAReg.rank();
bool isAtOmegaARegInvertible = luOfAtOmegaAReg.isInvertible();
float threshold = std::abs(luOfAtOmegaAReg.maxPivot()) * luOfAtOmegaAReg.threshold(); // originaly "2 * ..." but I commented it out
RowMajorMatrixXf AtARegInv_EigenFullLU = luOfAtOmegaAReg.inverse(); // Note: We should use ::solve() instead
Mat AtOmegaARegInvFullLU(AtARegInv_EigenFullLU.rows(), AtARegInv_EigenFullLU.cols(), CV_32FC1, AtARegInv_EigenFullLU.data()); // create an OpenCV Mat header for the Eigen data
*/
// Solve using OpenCV:
Mat c_s; // Note/Todo: We get coefficients ~ N(0, sigma) I think. They are not multiplied with the eigenvalues.
bool non_singular = cv::solve(AtOmegaAReg, -A.t() * Omega.t() * b, c_s, cv::DECOMP_SVD); // DECOMP_SVD calculates the pseudo-inverse if the matrix is not invertible.
// Because we're using SVD, non_singular will always be true. If we were to use e.g. Cholesky, we could return an expected<T>.
return std::vector<float>(c_s);
// Bring into standard regularised quadratic form with diagonal distance matrix Omega:
const MatrixXf A = P * V_hat_h; // camera matrix times the basis
const MatrixXf b = P * v_bar - y; // camera matrix times the mean, minus the landmarks
const MatrixXf AtOmegaAReg = A.transpose() * Omega * A + lambda * Eigen::MatrixXf::Identity(num_coeffs_to_fit, num_coeffs_to_fit);
const MatrixXf rhs = -A.transpose() * Omega.transpose() * b;
// c_s: The 'x' that we solve for. (The variance-normalised shape parameter vector, $c_s = [a_1/sigma_{s,1} , ..., a_m-1/sigma_{s,m-1}]^t$.)
// We get coefficients ~ N(0, 1), because we're fitting with the rescaled basis. The coefficients are not multiplied with their eigenvalues.
const VectorXf c_s = AtOmegaAReg.colPivHouseholderQr().solve(rhs);
return std::vector<float>(c_s.data(), c_s.data() + c_s.size());
};
} /* namespace fitting */
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment