Changed linear blendshape fitting to use Eigen

Mostly we're using the _nnls function for blendshape fitting, but just for completeness, this function is now changed to use only Eigen as well.

include/eos/fitting/blendshape_fitting.hpp

@@ -25,6 +25,7 @@
 #include "eos/morphablemodel/Blendshape.hpp"
 
 #include "Eigen/Core"
+#include "Eigen/QR"
 #include "nnls.h"
 
 #include "opencv2/core/core.hpp"
@@ -60,62 +61,56 @@ inline std::vector<float> fit_blendshapes_to_landmarks_linear(const std::vector<
 	assert(landmarks.size() == vertex_ids.size());
 
 	using cv::Mat;
+	using Eigen::VectorXf;
 	using Eigen::MatrixXf;
 
 	const int num_blendshapes = blendshapes.size();
 	const int num_landmarks = static_cast<int>(landmarks.size());
 
 	// Copy all blendshapes into a "basis" matrix with each blendshape being a column:
-	Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> blendshapes_as_basis = morphablemodel::to_matrix(blendshapes);
-	// 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 blendshapes_basis_as_mat = Mat(blendshapes_as_basis.rows(), blendshapes_as_basis.cols(), CV_32FC1, blendshapes_as_basis.data());
+	MatrixXf blendshapes_as_basis = morphablemodel::to_matrix(blendshapes);
 
 	// $\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_blendshapes, CV_32FC1);
+	MatrixXf V_hat_h = MatrixXf::Zero(4 * num_landmarks, num_blendshapes);
 	int row_index = 0;
 	for (int i = 0; i < num_landmarks; ++i) {
-		Mat basis_rows = blendshapes_basis_as_mat.rowRange(vertex_ids[i] * 3, (vertex_ids[i] * 3) + 3);
-		basis_rows.copyTo(V_hat_h.rowRange(row_index, row_index + 3));
+		V_hat_h.block(row_index, 0, 3, V_hat_h.cols()) = blendshapes_as_basis.block(vertex_ids[i] * 3, 0, 3, blendshapes_as_basis.cols());
 		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 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(face_instance(vertex_ids[i]*3), face_instance(vertex_ids[i]*3 + 1), face_instance(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?
+		v_bar(4 * i) = face_instance(vertex_ids[i] * 3);
+		v_bar((4 * i) + 1) = face_instance(vertex_ids[i] * 3 + 1);
+		v_bar((4 * i) + 2) = face_instance(vertex_ids[i] * 3 + 2);
+		//v_bar((4 * i) + 3) = 1; // already 1, stays (homogeneous coordinate)
 	}
 
 	// Bring into standard regularised quadratic form:
-	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.
+	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
 	
-	Mat AtAReg = A.t() * A + lambda * Mat::eye(num_blendshapes, num_blendshapes, CV_32FC1);
-	// Solve using OpenCV:
-	Mat c_s;
-	bool non_singular = cv::solve(AtAReg, -A.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>.
+	const MatrixXf AtAReg = A.transpose() * A + lambda * Eigen::MatrixXf::Identity(num_blendshapes, num_blendshapes);
+	const MatrixXf rhs = -A.transpose() * b;
 
-	return std::vector<float>(c_s);
+	const VectorXf coefficients = AtAReg.colPivHouseholderQr().solve(rhs);
+
+	return std::vector<float>(coefficients.data(), coefficients.data() + coefficients.size());
 };
 
 /**