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.

include/eos/fitting/linear_shape_fitting.hpp

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