merge with devel branch

examples/CMakeLists.txt

@@ -30,6 +30,11 @@ add_executable(fit-model fit-model.cpp)
 target_link_libraries(fit-model eos ${OpenCV_LIBS} ${Boost_LIBRARIES})
 target_link_libraries(fit-model "$<$<CXX_COMPILER_ID:GNU>:-pthread>$<$<CXX_COMPILER_ID:Clang>:-pthreads>")
 
+# Model fitting example that fits orthographic camera, shape, blendshapes, and contours to multiple images:
+add_executable(fit-model-multi fit-model-multi.cpp)
+target_link_libraries(fit-model-multi eos ${OpenCV_LIBS} ${Boost_LIBRARIES})
+target_link_libraries(fit-model-multi "$<$<CXX_COMPILER_ID:GNU>:-pthread>$<$<CXX_COMPILER_ID:Clang>:-pthreads>")
+
 # Generate random samples from the model:
 add_executable(generate-obj generate-obj.cpp)
 target_link_libraries(generate-obj eos ${OpenCV_LIBS} ${Boost_LIBRARIES})
@@ -37,6 +42,7 @@ target_link_libraries(generate-obj eos ${OpenCV_LIBS} ${Boost_LIBRARIES})
 # Install these targets:
 install(TARGETS fit-model-simple DESTINATION bin)
 install(TARGETS fit-model DESTINATION bin)
+install(TARGETS fit-model-multi DESTINATION bin)
 install(TARGETS generate-obj DESTINATION bin)
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/data DESTINATION bin)
 

examples/fit-model-multi.cpp

+/*
+ * eos - A 3D Morphable Model fitting library written in modern C++11/14.
+ *
+ * File: examples/fit-model.cpp
+ *
+ * Copyright 2016 Patrik Huber
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "eos/core/Landmark.hpp"
+#include "eos/core/LandmarkMapper.hpp"
+#include "eos/morphablemodel/MorphableModel.hpp"
+#include "eos/morphablemodel/Blendshape.hpp"
+#include "eos/fitting/fitting.hpp"
+#include "eos/render/utils.hpp"
+#include "eos/render/texture_extraction.hpp"
+
+#include "opencv2/core/core.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+#include "boost/program_options.hpp"
+#include "boost/filesystem.hpp"
+
+#include <vector>
+#include <iostream>
+#include <fstream>
+
+using namespace eos;
+namespace po = boost::program_options;
+namespace fs = boost::filesystem;
+using eos::core::Landmark;
+using eos::core::LandmarkCollection;
+using cv::Mat;
+using cv::Vec2f;
+using cv::Vec3f;
+using cv::Vec4f;
+using std::cout;
+using std::endl;
+using std::vector;
+using std::string;
+
+/**
+ * Reads an ibug .pts landmark file and returns an ordered vector with
+ * the 68 2D landmark coordinates.
+ *
+ * @param[in] filename Path to a .pts file.
+ * @return An ordered vector with the 68 ibug landmarks.
+ */
+LandmarkCollection<cv::Vec2f> read_pts_landmarks(std::string filename)
+{
+	using std::getline;
+	using cv::Vec2f;
+	using std::string;
+	LandmarkCollection<Vec2f> landmarks;
+	landmarks.reserve(68);
+
+	std::ifstream file(filename);
+	if (!file.is_open()) {
+		throw std::runtime_error(string("Could not open landmark file: " + filename));
+	}
+
+	string line;
+	// Skip the first 3 lines, they're header lines:
+	getline(file, line); // 'version: 1'
+	getline(file, line); // 'n_points : 68'
+	getline(file, line); // '{'
+
+	int ibugId = 1;
+	while (getline(file, line))
+	{
+		if (line == "}") { // end of the file
+			break;
+		}
+		std::stringstream lineStream(line);
+
+		Landmark<Vec2f> landmark;
+		landmark.name = std::to_string(ibugId);
+		if (!(lineStream >> landmark.coordinates[0] >> landmark.coordinates[1])) {
+			throw std::runtime_error(string("Landmark format error while parsing the line: " + line));
+		}
+		// From the iBug website:
+		// "Please note that the re-annotated data for this challenge are saved in the Matlab convention of 1 being
+		// the first index, i.e. the coordinates of the top left pixel in an image are x=1, y=1."
+		// ==> So we shift every point by 1:
+		landmark.coordinates[0] -= 1.0f;
+		landmark.coordinates[1] -= 1.0f;
+		landmarks.emplace_back(landmark);
+		++ibugId;
+	}
+	return landmarks;
+};
+
+/**
+ * Draws the given mesh as wireframe into the image.
+ *
+ * It does backface culling, i.e. draws only vertices in CCW order.
+ *
+ * @param[in] image An image to draw into.
+ * @param[in] mesh The mesh to draw.
+ * @param[in] modelview Model-view matrix to draw the mesh.
+ * @param[in] projection Projection matrix to draw the mesh.
+ * @param[in] viewport Viewport to draw the mesh.
+ * @param[in] colour Colour of the mesh to be drawn.
+ */
+void draw_wireframe(cv::Mat image, const core::Mesh& mesh, glm::mat4x4 modelview, glm::mat4x4 projection, glm::vec4 viewport, cv::Scalar colour = cv::Scalar(0, 255, 0, 255))
+{
+	for (const auto& triangle : mesh.tvi)
+	{
+		const auto p1 = glm::project({ mesh.vertices[triangle[0]][0], mesh.vertices[triangle[0]][1], mesh.vertices[triangle[0]][2] }, modelview, projection, viewport);
+		const auto p2 = glm::project({ mesh.vertices[triangle[1]][0], mesh.vertices[triangle[1]][1], mesh.vertices[triangle[1]][2] }, modelview, projection, viewport);
+		const auto p3 = glm::project({ mesh.vertices[triangle[2]][0], mesh.vertices[triangle[2]][1], mesh.vertices[triangle[2]][2] }, modelview, projection, viewport);
+		if (render::detail::are_vertices_ccw_in_screen_space(glm::vec2(p1), glm::vec2(p2), glm::vec2(p3)))
+		{
+			cv::line(image, cv::Point(p1.x, p1.y), cv::Point(p2.x, p2.y), colour);
+			cv::line(image, cv::Point(p2.x, p2.y), cv::Point(p3.x, p3.y), colour);
+			cv::line(image, cv::Point(p3.x, p3.y), cv::Point(p1.x, p1.y), colour);
+		}
+	}
+};
+
+/**
+ * This app demonstrates estimation of the camera and fitting of the shape
+ * model of a 3D Morphable Model from an ibug LFPW image with its landmarks.
+ * In addition to fit-model-simple, this example uses blendshapes, contour-
+ * fitting, and can iterate the fitting.
+ *
+ * 68 ibug landmarks are loaded from the .pts file and converted
+ * to vertex indices using the LandmarkMapper.
+ */
+int main(int argc, char *argv[])
+{
+    fs::path modelfile, isomapfile, mappingsfile, contourfile, edgetopologyfile, blendshapesfile, outputfilebase;
+    vector<fs::path> imagefiles, landmarksfiles;
+	try {
+		po::options_description desc("Allowed options");
+		desc.add_options()
+			("help,h",
+				"display the help message")
+			("model,m", po::value<fs::path>(&modelfile)->required()->default_value("../share/sfm_shape_3448.bin"),
+				"a Morphable Model stored as cereal BinaryArchive")
+            //("image,i", po::value<vector<fs::path>>(&imagefiles)->required()->default_value("data/image_0010.png"),
+            ("image,i", po::value<vector<fs::path>>(&imagefiles)->multitoken(),
+				"an input image")
+            ("landmarks,l", po::value<vector<fs::path>>(&landmarksfiles)->multitoken(),
+				"2D landmarks for the image, in ibug .pts format")
+            ("mapping,p", po::value<fs::path>(&mappingsfile)->required()->default_value("../share/ibug_to_sfm.txt"),
+				"landmark identifier to model vertex number mapping")
+			("model-contour,c", po::value<fs::path>(&contourfile)->required()->default_value("../share/model_contours.json"),
+				"file with model contour indices")
+			("edge-topology,e", po::value<fs::path>(&edgetopologyfile)->required()->default_value("../share/sfm_3448_edge_topology.json"),
+				"file with model's precomputed edge topology")
+			("blendshapes,b", po::value<fs::path>(&blendshapesfile)->required()->default_value("../share/expression_blendshapes_3448.bin"),
+				"file with blendshapes")
+            ("output,o", po::value<fs::path>(&outputfilebase)->required()->default_value("out"),
+				"basename for the output rendering and obj files")
+			;
+		po::variables_map vm;
+		po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
+		if (vm.count("help")) {
+			cout << "Usage: fit-model [options]" << endl;
+			cout << desc;
+			return EXIT_SUCCESS;
+		}
+		po::notify(vm);
+	}
+	catch (const po::error& e) {
+		cout << "Error while parsing command-line arguments: " << e.what() << endl;
+		cout << "Use --help to display a list of options." << endl;
+		return EXIT_SUCCESS;
+	}
+
+    if (landmarksfiles.size() != imagefiles.size()) {
+        cout << "Number of landmarksfiles not equal to number of images given: "<<landmarksfiles.size() <<"!=" <<imagefiles.size()<< endl;
+        return EXIT_SUCCESS;
+    }
+
+    if (landmarksfiles.empty()) {
+        cout << "Please give at least 1 image and landmarkfile" << endl;
+        return EXIT_SUCCESS;
+    }
+	// Load the image, landmarks, LandmarkMapper and the Morphable Model:
+    vector<Mat> images;
+    for (auto& imagefile : imagefiles){
+        images.push_back(cv::imread(imagefile.string()));
+    }
+    vector<LandmarkCollection<cv::Vec2f>> landmarkss;
+	try {
+        for (auto& landmarksfile : landmarksfiles){
+            landmarkss.push_back(read_pts_landmarks(landmarksfile.string()));
+        }
+	}
+	catch (const std::runtime_error& e) {
+		cout << "Error reading the landmarks: " << e.what() << endl;
+		return EXIT_FAILURE;
+	}
+	morphablemodel::MorphableModel morphable_model;
+	try {
+		morphable_model = morphablemodel::load_model(modelfile.string());
+	}
+	catch (const std::runtime_error& e) {
+		cout << "Error loading the Morphable Model: " << e.what() << endl;
+		return EXIT_FAILURE;
+	}
+	// The landmark mapper is used to map ibug landmark identifiers to vertex ids:
+	core::LandmarkMapper landmark_mapper = mappingsfile.empty() ? core::LandmarkMapper() : core::LandmarkMapper(mappingsfile);
+
+	// The expression blendshapes:
+	vector<morphablemodel::Blendshape> blendshapes = morphablemodel::load_blendshapes(blendshapesfile.string());
+
+	// These two are used to fit the front-facing contour to the ibug contour landmarks:
+	fitting::ModelContour model_contour = contourfile.empty() ? fitting::ModelContour() : fitting::ModelContour::load(contourfile.string());
+	fitting::ContourLandmarks ibug_contour = fitting::ContourLandmarks::load(mappingsfile.string());
+
+	// The edge topology is used to speed up computation of the occluding face contour fitting:
+	morphablemodel::EdgeTopology edge_topology = morphablemodel::load_edge_topology(edgetopologyfile.string());
+
+	// Draw the loaded landmarks:
+    vector<Mat> outimgs;
+    for (unsigned i =0; i <images.size(); ++i) {
+        Mat outimg = images[i].clone();
+        for (auto&& lm : landmarkss[i]) {
+            cv::rectangle(outimg, cv::Point2f(lm.coordinates[0] - 2.0f, lm.coordinates[1] - 2.0f), cv::Point2f(lm.coordinates[0] + 2.0f, lm.coordinates[1] + 2.0f), { 255, 0, 0 });
+        }
+        outimgs.push_back(outimg);
+    }
+
+	// Fit the model, get back a mesh and the pose:
+    vector<core::Mesh> meshs;
+    vector<fitting::RenderingParameters> rendering_paramss;
+    vector<int> image_widths;
+    vector<int> image_heights;
+    for (auto& image : images) {
+        image_widths.push_back(image.cols);
+        image_heights.push_back(image.rows);
+    }
+    std::tie(meshs, rendering_paramss) = fitting::fit_shape_and_pose_multi(morphable_model, blendshapes, landmarkss, landmark_mapper, image_widths, image_heights, edge_topology, ibug_contour, model_contour, 50, boost::none, 30.0f);
+
+    for (unsigned i =0; i <images.size(); ++i) {
+
+        // The 3D head pose can be recovered as follows:
+        float yaw_angle = glm::degrees(glm::yaw(rendering_paramss[i].get_rotation()));
+        // and similarly for pitch and roll.
+
+        // Extract the texture from the image using given mesh and camera parameters:
+        Mat affine_from_ortho = fitting::get_3x4_affine_camera_matrix(rendering_paramss[i], images[i].cols, images[i].rows);
+        Mat isomap = render::extract_texture(meshs[i], affine_from_ortho, images[i]);
+
+        // Draw the fitted mesh as wireframe, and save the image:
+        draw_wireframe(outimgs[i], meshs[i], rendering_paramss[i].get_modelview(), rendering_paramss[i].get_projection(), fitting::get_opencv_viewport(images[i].cols, images[i].rows));
+        fs::path outputfile = outputfilebase;
+        outputfile += fs::path(imagefiles[i].stem());
+        outputfile += fs::path(".png");
+        cv::imwrite(outputfile.string(), outimgs[i]);
+
+        // Save the mesh as textured obj:
+        outputfile.replace_extension(".obj");
+        core::write_textured_obj(meshs[i], outputfile.string());
+
+        // And save the isomap:
+        outputfile.replace_extension(".isomap.png");
+        cv::imwrite(outputfile.string(), isomap);
+
+    }
+
+    cout << "Finished fitting and wrote result mesh and isomap to files with basename " << outputfilebase << "." << endl;
+
+	return EXIT_SUCCESS;
+}

include/eos/fitting/fitting.hpp

@@ -190,7 +190,7 @@ inline auto concat(const std::vector<T>& vec_a, const std::vector<T>& vec_b)
 
 /**
  * @brief Fit the pose (camera), shape model, and expression blendshapes to landmarks,
- * in an iterative way.
+ * in an iterative way. Can fit to more than one set of landmarks, thus multiple images.
  *
  * Convenience function that fits pose (camera), the shape model, and expression blendshapes
  * to landmarks, in an iterative (alternating) way. It fits both sides of the face contour as well.
@@ -229,21 +229,27 @@ inline auto concat(const std::vector<T>& vec_a, const std::vector<T>& vec_b)
  * @param[out] fitted_image_points Debug parameter: Returns all the 2D points that have been used for the fitting.
  * @return The fitted model shape instance and the final pose.
  */
-inline std::pair<core::Mesh, fitting::RenderingParameters> fit_shape_and_pose(const morphablemodel::MorphableModel& morphable_model, const std::vector<morphablemodel::Blendshape>& blendshapes, const core::LandmarkCollection<cv::Vec2f>& landmarks, const core::LandmarkMapper& landmark_mapper, int image_width, int image_height, const morphablemodel::EdgeTopology& edge_topology, const fitting::ContourLandmarks& contour_landmarks, const fitting::ModelContour& model_contour, int num_iterations, boost::optional<int> num_shape_coefficients_to_fit, float lambda, boost::optional<fitting::RenderingParameters> initial_rendering_params, std::vector<float>& pca_shape_coefficients, std::vector<float>& blendshape_coefficients, std::vector<cv::Vec2f>& fitted_image_points)
+inline std::pair<std::vector<core::Mesh>, std::vector<fitting::RenderingParameters>> fit_shape_and_pose_multi(const morphablemodel::MorphableModel& morphable_model, const std::vector<morphablemodel::Blendshape>& blendshapes, const std::vector<core::LandmarkCollection<cv::Vec2f>>& landmarks, const core::LandmarkMapper& landmark_mapper, std::vector<int> image_width, std::vector<int> image_height, const morphablemodel::EdgeTopology& edge_topology, const fitting::ContourLandmarks& contour_landmarks, const fitting::ModelContour& model_contour, int num_iterations, boost::optional<int> num_shape_coefficients_to_fit, float lambda, boost::optional<fitting::RenderingParameters> initial_rendering_params, std::vector<float>& pca_shape_coefficients, std::vector<std::vector<float>>& blendshape_coefficients, std::vector<std::vector<cv::Vec2f>>& fitted_image_points)
 {
-	assert(blendshapes.size() > 0);
-	assert(landmarks.size() >= 4);
-	assert(image_width > 0 && image_height > 0);
-	assert(num_iterations > 0); // Can we allow 0, for only the initial pose-fit?
-	assert(pca_shape_coefficients.size() <= morphable_model.get_shape_model().get_num_principal_components());
-	// More asserts I forgot?
+    assert(blendshapes.size() > 0);
+    assert(landmarks.size() > 0 && landmarks.size() == image_width.size() && image_width.size() == image_height.size());
+    assert(num_iterations > 0); // Can we allow 0, for only the initial pose-fit?
+    assert(pca_shape_coefficients.size() <= morphable_model.get_shape_model().get_num_principal_components());
+    int num_images = static_cast<int>(landmarks.size());
+    for (int j = 0; j < num_images; ++j) {
+        assert(landmarks[j].size() >= 4);
+        assert(image_width[j] > 0 && image_height[j] > 0);
+    }
+    // More asserts I forgot?
 
 	using std::vector;
 	using cv::Vec2f;
 	using cv::Vec4f;
+	using cv::Mat;
 	using Eigen::VectorXf;
 	using Eigen::MatrixXf;
 
+
 	if (!num_shape_coefficients_to_fit)
 	{
 		num_shape_coefficients_to_fit = morphable_model.get_shape_model().get_num_principal_components();
@@ -256,112 +262,250 @@ inline std::pair<core::Mesh, fitting::RenderingParameters> fit_shape_and_pose(co
 	// Todo: This leaves the following case open: num_coeffs given is empty or defined, but the
 	// pca_shape_coefficients given is != num_coeffs or the model's max-coeffs. What to do then? Handle & document!
 
-	if (blendshape_coefficients.empty())
-	{
-		blendshape_coefficients.resize(blendshapes.size());
-	}
-
-	MatrixXf blendshapes_as_basis = morphablemodel::to_matrix(blendshapes);
-
-	// Current mesh - either from the given coefficients, or the mean:
-	VectorXf current_pca_shape = morphable_model.get_shape_model().draw_sample(pca_shape_coefficients);
-	VectorXf current_combined_shape = current_pca_shape + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients.data(), blendshape_coefficients.size());
-	auto current_mesh = morphablemodel::sample_to_mesh(current_combined_shape, morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
-
-	// The 2D and 3D point correspondences used for the fitting:
-	vector<Vec4f> model_points; // the points in the 3D shape model
-	vector<int> vertex_indices; // their vertex indices
-	vector<Vec2f> image_points; // the corresponding 2D landmark points
-
-	// Sub-select all the landmarks which we have a mapping for (i.e. that are defined in the 3DMM),
-	// and get the corresponding model points (mean if given no initial coeffs, from the computed shape otherwise):
-	for (int i = 0; i < landmarks.size(); ++i) {
-		auto converted_name = landmark_mapper.convert(landmarks[i].name);
-		if (!converted_name) { // no mapping defined for the current landmark
-			continue;
-		}
-		int vertex_idx = std::stoi(converted_name.get());
-		Vec4f vertex(current_mesh.vertices[vertex_idx].x, current_mesh.vertices[vertex_idx].y, current_mesh.vertices[vertex_idx].z, current_mesh.vertices[vertex_idx].w);
-		model_points.emplace_back(vertex);
-		vertex_indices.emplace_back(vertex_idx);
-		image_points.emplace_back(landmarks[i].coordinates);
-	}
-
-	// Need to do an initial pose fit to do the contour fitting inside the loop.
-	// We'll do an expression fit too, since face shapes vary quite a lot, depending on expressions.
-	fitting::ScaledOrthoProjectionParameters current_pose;
-	current_pose = fitting::estimate_orthographic_projection_linear(image_points, model_points, true, image_height);
-	fitting::RenderingParameters rendering_params(current_pose, image_width, image_height);
-
-	cv::Mat affine_from_ortho = fitting::get_3x4_affine_camera_matrix(rendering_params, image_width, image_height);
-	blendshape_coefficients = fitting::fit_blendshapes_to_landmarks_nnls(blendshapes, current_pca_shape, affine_from_ortho, image_points, vertex_indices);
-
-	// Mesh with same PCA coeffs as before, but new expression fit (this is relevant if no initial blendshape coeffs have been given):
-	current_combined_shape = current_pca_shape + morphablemodel::to_matrix(blendshapes) * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients.data(), blendshape_coefficients.size());
-	current_mesh = morphablemodel::sample_to_mesh(current_combined_shape, morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
-
-	// The static (fixed) landmark correspondences which will stay the same throughout
-	// the fitting (the inner face landmarks):
-	auto fixed_image_points = image_points;
-	auto fixed_vertex_indices = vertex_indices;
-
-	for (int i = 0; i < num_iterations; ++i)
-	{
-		image_points = fixed_image_points;
-		vertex_indices = fixed_vertex_indices;
-		// Given the current pose, find 2D-3D contour correspondences of the front-facing face contour:
-		vector<Vec2f> image_points_contour;
-		vector<int> vertex_indices_contour;
-		auto yaw_angle = glm::degrees(glm::eulerAngles(rendering_params.get_rotation())[1]);
-		// For each 2D contour landmark, get the corresponding 3D vertex point and vertex id:
-		std::tie(image_points_contour, std::ignore, vertex_indices_contour) = fitting::get_contour_correspondences(landmarks, contour_landmarks, model_contour, yaw_angle, current_mesh, rendering_params.get_modelview(), rendering_params.get_projection(), fitting::get_opencv_viewport(image_width, image_height));
-		// Add the contour correspondences to the set of landmarks that we use for the fitting:
-		vertex_indices = fitting::concat(vertex_indices, vertex_indices_contour);
-		image_points = fitting::concat(image_points, image_points_contour);
-
-		// Fit the occluding (away-facing) contour using the detected contour LMs:
-		vector<Eigen::Vector2f> occluding_contour_landmarks;
-		if (yaw_angle >= 0.0f) // positive yaw = subject looking to the left
-		{ // the left contour is the occluding one we want to use ("away-facing")
-			auto contour_landmarks_ = core::filter(landmarks, contour_landmarks.left_contour); // Can do this outside of the loop
-			std::for_each(begin(contour_landmarks_), end(contour_landmarks_), [&occluding_contour_landmarks](auto&& lm) { occluding_contour_landmarks.push_back({ lm.coordinates[0], lm.coordinates[1] }); });
-		}
-		else {
-			auto contour_landmarks_ = core::filter(landmarks, contour_landmarks.right_contour);
-			std::for_each(begin(contour_landmarks_), end(contour_landmarks_), [&occluding_contour_landmarks](auto&& lm) { occluding_contour_landmarks.push_back({ lm.coordinates[0], lm.coordinates[1] }); });
-		}
-		auto edge_correspondences = fitting::find_occluding_edge_correspondences(current_mesh, edge_topology, rendering_params, occluding_contour_landmarks, 180.0f);
-		image_points = fitting::concat(image_points, edge_correspondences.first);
-		vertex_indices = fitting::concat(vertex_indices, edge_correspondences.second);
-
-		// Get the model points of the current mesh, for all correspondences that we've got:
-		model_points.clear();
-		for (const auto& v : vertex_indices)
-		{
-			model_points.push_back({ current_mesh.vertices[v][0], current_mesh.vertices[v][1], current_mesh.vertices[v][2], current_mesh.vertices[v][3] });
-		}
-
-		// Re-estimate the pose, using all correspondences:
-		current_pose = fitting::estimate_orthographic_projection_linear(image_points, model_points, true, image_height);
-		rendering_params = fitting::RenderingParameters(current_pose, image_width, image_height);
-
-		cv::Mat affine_from_ortho = fitting::get_3x4_affine_camera_matrix(rendering_params, image_width, image_height);
+    if (blendshape_coefficients.empty())
+    {
+        for (int j = 0; j < num_images; ++j) {
+            std::vector<float> current_blendshape_coefficients;
+            current_blendshape_coefficients.resize(blendshapes.size());
+            blendshape_coefficients.push_back(current_blendshape_coefficients);
+        }
+    }
+
+    MatrixXf blendshapes_as_basis = morphablemodel::to_matrix(blendshapes);
+
+    // Current mesh - either from the given coefficients, or the mean:
+    VectorXf current_pca_shape = morphable_model.get_shape_model().draw_sample(pca_shape_coefficients);
+    vector<VectorXf> current_combined_shapes;
+    vector<eos::core::Mesh> current_meshs;
+    for (int j = 0; j < num_images; ++j) {
+        VectorXf current_combined_shape = current_pca_shape + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients[j].data(), blendshape_coefficients[j].size());
+        current_combined_shapes.push_back(current_combined_shape);
+
+        eos::core::Mesh current_mesh = morphablemodel::sample_to_mesh(current_combined_shape, morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
+        current_meshs.push_back(current_mesh);
+    }
+
+    // The 2D and 3D point correspondences used for the fitting:
+    vector<vector<Vec4f>> model_points; // the points in the 3D shape model of all frames
+    vector<vector<int>> vertex_indices; // their vertex indices of all frames
+    vector<vector<Vec2f>> image_points; // the corresponding 2D landmark points of all frames
+
+    for (int j = 0; j < num_images; ++j) {
+        vector<Vec4f> current_model_points;
+        vector<int> current_vertex_indices;
+        vector<Vec2f> current_image_points;
+
+        // Sub-select all the landmarks which we have a mapping for (i.e. that are defined in the 3DMM),
+        // and get the corresponding model points (mean if given no initial coeffs, from the computed shape otherwise):
+        for (int i = 0; i < landmarks[j].size(); ++i) {
+            auto converted_name = landmark_mapper.convert(landmarks[j][i].name);
+            if (!converted_name) { // no mapping defined for the current landmark
+                continue;
+            }
+            int vertex_idx = std::stoi(converted_name.get());
+            Vec4f vertex(current_meshs[j].vertices[vertex_idx].x, current_meshs[j].vertices[vertex_idx].y, current_meshs[j].vertices[vertex_idx].z, current_meshs[j].vertices[vertex_idx].w);
+            current_model_points.emplace_back(vertex);
+            current_vertex_indices.emplace_back(vertex_idx);
+            current_image_points.emplace_back(landmarks[j][i].coordinates);
+        }
+
+        model_points.push_back(current_model_points);
+        vertex_indices.push_back(current_vertex_indices);
+        image_points.push_back(current_image_points);
+    }
+
+    // Need to do an initial pose fit to do the contour fitting inside the loop.
+    // We'll do an expression fit too, since face shapes vary quite a lot, depending on expressions.
+    vector<fitting::RenderingParameters> rendering_params;
+    for (int j = 0; j < num_images; ++j) {
+        fitting::ScaledOrthoProjectionParameters current_pose = fitting::estimate_orthographic_projection_linear(image_points[j], model_points[j], true, image_height[j]);
+        fitting::RenderingParameters current_rendering_params(current_pose, image_width[j], image_height[j]);
+        rendering_params.push_back(current_rendering_params);
+
+        cv::Mat affine_from_ortho = fitting::get_3x4_affine_camera_matrix(current_rendering_params, image_width[j], image_height[j]);
+        blendshape_coefficients[j] = fitting::fit_blendshapes_to_landmarks_nnls(blendshapes, current_pca_shape, affine_from_ortho, image_points[j], vertex_indices[j]);
+
+        // Mesh with same PCA coeffs as before, but new expression fit (this is relevant if no initial blendshape coeffs have been given):
+        current_combined_shapes[j] = current_pca_shape + morphablemodel::to_matrix(blendshapes) * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients[j].data(),blendshape_coefficients[j].size());
+        current_meshs[j] = morphablemodel::sample_to_mesh(current_combined_shapes[j], morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
+    }
+
+    // The static (fixed) landmark correspondences which will stay the same throughout
+    // the fitting (the inner face landmarks):
+    vector<vector<int>> fixed_vertex_indices (vertex_indices);
+    vector<vector<Vec2f>> fixed_image_points (image_points);
+
+    for (int i = 0; i < num_iterations; ++i)
+    {
+        std::vector<cv::Mat> affine_from_orthos;
+        std::vector<VectorXf> mean_plus_blendshapes;
+
+        image_points = fixed_image_points;
+        vertex_indices = fixed_vertex_indices;
+
+        for (int j = 0; j < num_images; ++j) {
+
+            // Given the current pose, find 2D-3D contour correspondences of the front-facing face contour:
+            vector<Vec2f> image_points_contour;
+            vector<int> vertex_indices_contour;
+            auto yaw_angle = glm::degrees(glm::eulerAngles(rendering_params[j].get_rotation())[1]);
+            // For each 2D contour landmark, get the corresponding 3D vertex point and vertex id:
+            std::tie(image_points_contour, std::ignore, vertex_indices_contour) = fitting::get_contour_correspondences(landmarks[j], contour_landmarks, model_contour, yaw_angle, current_meshs[j], rendering_params[j].get_modelview(), rendering_params[j].get_projection(), fitting::get_opencv_viewport(image_width[j], image_height[j]));
+            // Add the contour correspondences to the set of landmarks that we use for the fitting:
+            vertex_indices[j] = fitting::concat(vertex_indices[j], vertex_indices_contour);
+            image_points[j] = fitting::concat(image_points[j], image_points_contour);
+
+            // Fit the occluding (away-facing) contour using the detected contour LMs:
+            vector<Eigen::Vector2f> occluding_contour_landmarks;
+            if (yaw_angle >= 0.0f) // positive yaw = subject looking to the left
+            { // the left contour is the occluding one we want to use ("away-facing")
+                auto contour_landmarks_ = core::filter(landmarks[j], contour_landmarks.left_contour); // Can do this outside of the loop
+                std::for_each(begin(contour_landmarks_), end(contour_landmarks_), [&occluding_contour_landmarks](auto&& lm) { occluding_contour_landmarks.push_back({ lm.coordinates[0], lm.coordinates[1] }); });
+            }
+            else {
+                auto contour_landmarks_ = core::filter(landmarks[j], contour_landmarks.right_contour);
+                std::for_each(begin(contour_landmarks_), end(contour_landmarks_), [&occluding_contour_landmarks](auto&& lm) { occluding_contour_landmarks.push_back({ lm.coordinates[0], lm.coordinates[1] }); });
+            }
+            auto edge_correspondences = fitting::find_occluding_edge_correspondences(current_meshs[j], edge_topology, rendering_params[j], occluding_contour_landmarks, 180.0f);
+            image_points[j] = fitting::concat(image_points[j], edge_correspondences.first);
+            vertex_indices[j] = fitting::concat(vertex_indices[j], edge_correspondences.second);
+
+            // Get the model points of the current mesh, for all correspondences that we've got:
+            model_points[j].clear();
+            for (const auto& v : vertex_indices[j])
+            {
+                model_points[j].push_back({ current_meshs[j].vertices[v][0], current_meshs[j].vertices[v][1], current_meshs[j].vertices[v][2], current_meshs[j].vertices[v][3] });
+            }
+
+            // Re-estimate the pose, using all correspondences:
+            fitting::ScaledOrthoProjectionParameters current_pose = fitting::estimate_orthographic_projection_linear(image_points[j], model_points[j], true, image_height[j]);
+            rendering_params[j] = fitting::RenderingParameters(current_pose, image_width[j], image_height[j]);
+
+            cv::Mat affine_from_ortho = fitting::get_3x4_affine_camera_matrix(rendering_params[j], image_width[j], image_height[j]);
+            affine_from_orthos.push_back(affine_from_ortho);
+
+            // Estimate the PCA shape coefficients with the current blendshape coefficients:
+            VectorXf current_mean_plus_blendshapes = morphable_model.get_shape_model().get_mean() + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients[j].data(),blendshape_coefficients[j].size());
+            mean_plus_blendshapes.push_back(current_mean_plus_blendshapes);
+        }
+        pca_shape_coefficients = fitting::fit_shape_to_landmarks_linear_multi(morphable_model, affine_from_orthos, image_points, vertex_indices, mean_plus_blendshapes, lambda, num_shape_coefficients_to_fit);
+        for (auto i: pca_shape_coefficients)
+          std::cout << i << ' ';
+        std::cout << std::endl;
+
+        // Estimate the blendshape coefficients with the current PCA model estimate:
+        current_pca_shape = morphable_model.get_shape_model().draw_sample(pca_shape_coefficients);
+
+        for (int j = 0; j < num_images; ++j) {
+            blendshape_coefficients[j] = fitting::fit_blendshapes_to_landmarks_nnls(blendshapes, current_pca_shape, affine_from_orthos[j], image_points[j], vertex_indices[j]);
+
+            current_combined_shapes[j] = current_pca_shape + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients[j].data(),blendshape_coefficients[j].size());
+            current_meshs[j] = morphablemodel::sample_to_mesh(current_combined_shapes[j], morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
+        }
+    }
+
+    fitted_image_points = image_points;
+    return { current_meshs, rendering_params }; // I think we could also work with a Mat face_instance in this function instead of a Mesh, but it would convolute the code more (i.e. more complicated to access vertices).
+};
 
-		// Estimate the PCA shape coefficients with the current blendshape coefficients:
-		VectorXf mean_plus_blendshapes = morphable_model.get_shape_model().get_mean() + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients.data(), blendshape_coefficients.size());
-		pca_shape_coefficients = fitting::fit_shape_to_landmarks_linear(morphable_model, affine_from_ortho, image_points, vertex_indices, mean_plus_blendshapes, lambda, num_shape_coefficients_to_fit);
+/**
+ * @brief Fit the pose (camera), shape model, and expression blendshapes to landmarks,
+ * in an iterative way.  Can fit to more than one set of landmarks, thus multiple images.
+ *
+ * Convenience function that fits pose (camera), the shape model, and expression blendshapes
+ * to landmarks, in an iterative (alternating) way. It fits both sides of the face contour as well.
+ *
+ * If you want to access the values of shape or blendshape coefficients, or want to set starting
+ * values for them, use the following overload to this function:
+ * std::pair<render::Mesh, fitting::RenderingParameters> fit_shape_and_pose(const morphablemodel::MorphableModel&, const std::vector<morphablemodel::Blendshape>&, const core::LandmarkCollection<cv::Vec2f>&, const core::LandmarkMapper&, int, int, const morphablemodel::EdgeTopology&, const fitting::ContourLandmarks&, const fitting::ModelContour&, int, boost::optional<int>, float, boost::optional<fitting::RenderingParameters>, std::vector<float>&, std::vector<float>&, std::vector<cv::Vec2f>&)
+ *
+ * Todo: Add a convergence criterion.
+ *
+ * \p num_iterations: Results are good for even a single iteration. For single-image fitting and
+ * for full convergence of all parameters, it can take up to 300 iterations. In tracking,
+ * particularly if initialising with the previous frame, it works well with as low as 1 to 5
+ * iterations.
+ * \p edge_topology is used for the occluding-edge face contour fitting.
+ * \p contour_landmarks and \p model_contour are used to fit the front-facing contour.
+ *
+ * @param[in] morphable_model The 3D Morphable Model used for the shape fitting.
+ * @param[in] blendshapes A vector of blendshapes that are being fit to the landmarks in addition to the PCA model.
+ * @param[in] landmarks 2D landmarks from an image to fit the model to.
+ * @param[in] landmark_mapper Mapping info from the 2D landmark points to 3D vertex indices.
+ * @param[in] image_width Width of the input image (needed for the camera model).
+ * @param[in] image_height Height of the input image (needed for the camera model).
+ * @param[in] edge_topology Precomputed edge topology of the 3D model, needed for fast edge-lookup.
+ * @param[in] contour_landmarks 2D image contour ids of left or right side (for example for ibug landmarks).
+ * @param[in] model_contour The model contour indices that should be considered to find the closest corresponding 3D vertex.
+ * @param[in] num_iterations Number of iterations that the different fitting parts will be alternated for.
+ * @param[in] num_shape_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Should be bigger than zero, or boost::none to fit all coefficients.
+ * @param[in] lambda Regularisation parameter of the PCA shape fitting.
+ * @return The fitted model shape instance and the final pose.
+ */
+inline std::pair<std::vector<core::Mesh>, std::vector<fitting::RenderingParameters>> fit_shape_and_pose_multi(const morphablemodel::MorphableModel& morphable_model, const std::vector<morphablemodel::Blendshape>& blendshapes, const std::vector<core::LandmarkCollection<cv::Vec2f>>& landmarks, const core::LandmarkMapper& landmark_mapper, std::vector<int> image_width, std::vector<int> image_height, const morphablemodel::EdgeTopology& edge_topology, const fitting::ContourLandmarks& contour_landmarks, const fitting::ModelContour& model_contour, int num_iterations = 5, boost::optional<int> num_shape_coefficients_to_fit = boost::none, float lambda = 30.0f)
+{
+    std::vector<float> pca_shape_coefficients;
+    std::vector<std::vector<float>> blendshape_coefficients;
+    std::vector<std::vector<cv::Vec2f>> fitted_image_points;
 
-		// Estimate the blendshape coefficients with the current PCA model estimate:
-		current_pca_shape = morphable_model.get_shape_model().draw_sample(pca_shape_coefficients);
-		blendshape_coefficients = fitting::fit_blendshapes_to_landmarks_nnls(blendshapes, current_pca_shape, affine_from_ortho, image_points, vertex_indices);
+    return fit_shape_and_pose_multi(morphable_model, blendshapes, landmarks, landmark_mapper, image_width, image_height, edge_topology, contour_landmarks, model_contour, num_iterations, num_shape_coefficients_to_fit, lambda, boost::none, pca_shape_coefficients, blendshape_coefficients, fitted_image_points);
+};
 
-		current_combined_shape = current_pca_shape + blendshapes_as_basis * Eigen::Map<const Eigen::VectorXf>(blendshape_coefficients.data(), blendshape_coefficients.size());
-		current_mesh = morphablemodel::sample_to_mesh(current_combined_shape, morphable_model.get_color_model().get_mean(), morphable_model.get_shape_model().get_triangle_list(), morphable_model.get_color_model().get_triangle_list(), morphable_model.get_texture_coordinates());
-	}
 
-	fitted_image_points = image_points;
-	return { current_mesh, rendering_params }; // I think we could also work with a Mat face_instance in this function instead of a Mesh, but it would convolute the code more (i.e. more complicated to access vertices).
-};
+/**
+ * @brief Fit the pose (camera), shape model, and expression blendshapes to landmarks,
+ * in an iterative way.
+ *
+ * Convenience function that fits pose (camera), the shape model, and expression blendshapes
+ * to landmarks, in an iterative (alternating) way. It fits both sides of the face contour as well.
+ *
+ * If \p pca_shape_coefficients and/or \p blendshape_coefficients are given, they are used as
+ * starting values in the fitting. When the function returns, they contain the coefficients from
+ * the last iteration.
+ *
+ * Use render::Mesh fit_shape_and_pose(const morphablemodel::MorphableModel&, const std::vector<morphablemodel::Blendshape>&, const core::LandmarkCollection<cv::Vec2f>&, const core::LandmarkMapper&, int, int, const morphablemodel::EdgeTopology&, const fitting::ContourLandmarks&, const fitting::ModelContour&, int, boost::optional<int>, float).
+ * for a simpler overload with reasonable defaults and no optional output.
+ *
+ * \p num_iterations: Results are good for even a single iteration. For single-image fitting and
+ * for full convergence of all parameters, it can take up to 300 iterations. In tracking,
+ * particularly if initialising with the previous frame, it works well with as low as 1 to 5
+ * iterations.
+ * \p edge_topology is used for the occluding-edge face contour fitting.
+ * \p contour_landmarks and \p model_contour are used to fit the front-facing contour.
+ *
+ * Todo: Add a convergence criterion.
+ *
+ * @param[in] morphable_model The 3D Morphable Model used for the shape fitting.
+ * @param[in] blendshapes A vector of blendshapes that are being fit to the landmarks in addition to the PCA model.
+ * @param[in] landmarks 2D landmarks from an image to fit the model to.
+ * @param[in] landmark_mapper Mapping info from the 2D landmark points to 3D vertex indices.
+ * @param[in] image_width Width of the input image (needed for the camera model).
+ * @param[in] image_height Height of the input image (needed for the camera model).
+ * @param[in] edge_topology Precomputed edge topology of the 3D model, needed for fast edge-lookup.
+ * @param[in] contour_landmarks 2D image contour ids of left or right side (for example for ibug landmarks).
+ * @param[in] model_contour The model contour indices that should be considered to find the closest corresponding 3D vertex.
+ * @param[in] num_iterations Number of iterations that the different fitting parts will be alternated for.
+ * @param[in] num_shape_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Should be bigger than zero, or boost::none to fit all coefficients.
+ * @param[in] lambda Regularisation parameter of the PCA shape fitting.
+ * @param[in] initial_rendering_params Currently ignored (not used).
+ * @param[in,out] pca_shape_coefficients If given, will be used as initial PCA shape coefficients to start the fitting. Will contain the final estimated coefficients.
+ * @param[in,out] blendshape_coefficients If given, will be used as initial expression blendshape coefficients to start the fitting. Will contain the final estimated coefficients.
+ * @param[out] fitted_image_points Debug parameter: Returns all the 2D points that have been used for the fitting.
+ * @return The fitted model shape instance and the final pose.
+ */
+inline std::pair<core::Mesh, fitting::RenderingParameters> fit_shape_and_pose(const morphablemodel::MorphableModel& morphable_model, const std::vector<morphablemodel::Blendshape>& blendshapes, const core::LandmarkCollection<cv::Vec2f>& landmarks, const core::LandmarkMapper& landmark_mapper, int image_width, int image_height, const morphablemodel::EdgeTopology& edge_topology, const fitting::ContourLandmarks& contour_landmarks, const fitting::ModelContour& model_contour, int num_iterations, boost::optional<int> num_shape_coefficients_to_fit, float lambda, boost::optional<fitting::RenderingParameters> initial_rendering_params, std::vector<float>& pca_shape_coefficients, std::vector<float>& blendshape_coefficients, std::vector<cv::Vec2f>& fitted_image_points)
+{
+    //we have to create a new vector here if the blendshape_coefficients are empty, as otherwise the new vector is not empty anymore and contains one element
+    std::vector<std::vector<float>> all_blendshape_coefficients;
+    if (!blendshape_coefficients.empty())
+    {
+        all_blendshape_coefficients = {blendshape_coefficients};
+    }
+    std::vector<std::vector<cv::Vec2f>> all_fitted_image_points = {fitted_image_points};
+    std::pair<std::vector<core::Mesh>, std::vector<fitting::RenderingParameters>>  all_meshs_and_params = fit_shape_and_pose_multi( morphable_model, blendshapes, { landmarks }, landmark_mapper, { image_width }, { image_height }, edge_topology, contour_landmarks, model_contour, num_iterations, num_shape_coefficients_to_fit, lambda, initial_rendering_params, pca_shape_coefficients, all_blendshape_coefficients, all_fitted_image_points);
+
+    return {all_meshs_and_params.first[0], all_meshs_and_params.second[0] };
+}
 
 /**
  * @brief Fit the pose (camera), shape model, and expression blendshapes to landmarks,

include/eos/fitting/linear_shape_fitting.hpp

@@ -51,72 +51,108 @@ namespace eos {
  * @param[in] landmarks 2D landmarks from an image to fit the model to.
  * @param[in] vertex_ids The vertex ids in the model that correspond to the 2D points.
  * @param[in] base_face The base or reference face from where the fitting is started. Usually this would be the models mean face, which is what will be used if the parameter is not explicitly specified.
- * @param[in] lambda The regularisation parameter (weight of the prior towards the mean).
+ * @param[in] lambda The regularisation parameter (weight of the prior towards the mean). Gets normalized by the number of images given.
  * @param[in] num_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Should be bigger than zero, or boost::none to fit all coefficients.
  * @param[in] detector_standard_deviation The standard deviation of the 2D landmarks given (e.g. of the detector used), in pixels.
  * @param[in] model_standard_deviation The standard deviation of the 3D vertex points in the 3D model, projected to 2D (so the value is in pixels).
  * @return The estimated shape-coefficients (alphas).
  */
-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>())
+inline std::vector<float> fit_shape_to_landmarks_linear_multi(morphablemodel::MorphableModel morphable_model, std::vector<cv::Mat> affine_camera_matrix, std::vector<std::vector<cv::Vec2f>>& landmarks, std::vector<std::vector<int>>& vertex_ids, std::vector<Eigen::VectorXf> base_face=std::vector<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());
+	assert(affine_camera_matrix.size() == landmarks.size() && landmarks.size() == vertex_ids.size()); // same number of instances (i.e. images/frames) for each of them
 
 	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());
+	int num_images = affine_camera_matrix.size();
 
-	if (base_face.size() == 0)
-	{
-		base_face = morphable_model.get_shape_model().get_mean();
-	}
+    // the regularisation has to be adjusted when more than one image is given
+    lambda *= num_images;
 
+	int total_num_landmarks_dimension = 0;
+	for (auto&& l : landmarks) {
+		total_num_landmarks_dimension += l.size();
+	}
 	// $\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);
-	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));
-		row_index += 4; // replace 3 rows and skip the 4th one, it has all zeros
-	}
+	Mat V_hat_h = Mat::zeros(4 * total_num_landmarks_dimension, num_coeffs_to_fit, CV_32FC1);
+	int V_hat_h_row_index = 0;
 	// 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);
-	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);
-	}
-	// The variances: Add the 2D and 3D standard deviations.
-	// If the user doesn't provide them, we choose the following:
-	// 2D (detector) standard deviation: In pixel, we follow [1] and choose sqrt(3) as the default value.
-	// 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);
-	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
-	}
+	Mat P = Mat::zeros(3 * total_num_landmarks_dimension, 4 * total_num_landmarks_dimension, CV_32FC1);
+	int P_index = 0;
+    Mat Omega = Mat::zeros(3 * total_num_landmarks_dimension, 3 * total_num_landmarks_dimension, CV_32FC1);
+    int Omega_index = 0; // this runs the same as P_index
 	// The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$
-	Mat y = Mat::ones(3 * num_landmarks, 1, CV_32FC1);
-	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)
-	}
+	Mat y = Mat::ones(3 * total_num_landmarks_dimension, 1, CV_32FC1);
+	int y_index = 0; // also runs the same as P_index. Should rename to "running_index"?
 	// 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);
-	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?
-	}
+	Mat v_bar = Mat::ones(4 * total_num_landmarks_dimension, 1, CV_32FC1);
+	int v_bar_index = 0; // also runs the same as P_index. But be careful, if I change it to be only 1 variable, only increment it once! :-)
+						 // Well I think that would make it a bit messy since we need to increment inside the for (landmarks...) loop. Try to refactor some other way.
+
+    for (int k = 0; k < num_images; ++k)
+    {
+        // For each image we have, set up the equations and add it to the matrices:
+        assert(landmarks[k].size() == vertex_ids[k].size()); // has to be valid for each img
+
+        int num_landmarks = static_cast<int>(landmarks[k].size());
+
+        if (base_face[k].size()==0)
+        {
+            base_face[k] = morphable_model.get_shape_model().get_mean();
+        }
+
+        // $\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);
+        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[k][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(V_hat_h_row_index, V_hat_h_row_index + 3));
+            V_hat_h_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);
+        for (int i = 0; i < num_landmarks; ++i) {
+            Mat submatrix_to_replace = P.colRange(4 * P_index, (4 * P_index) + 4).rowRange(3 * P_index, (3 * P_index) + 3);
+            affine_camera_matrix[k].copyTo(submatrix_to_replace);
+            ++P_index;
+        }
+        // The variances: Add the 2D and 3D standard deviations.
+        // If the user doesn't provide them, we choose the following:
+        // 2D (detector) standard deviation: In pixel, we follow [1] and choose sqrt(3) as the default value.
+        // 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 Sigma = Mat::zeros(3 * num_landmarks, 3 * num_landmarks, CV_32FC1);
+        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>(Omega_index, Omega_index) = 1.0f / sigma_squared_2D; // the higher the sigma_squared_2D, the smaller the diagonal entries of Sigma will be
+            ++Omega_index;
+        }
+
+        // The landmarks in matrix notation (in homogeneous coordinates), $3N\times 1$
+        //Mat y = Mat::ones(3 * num_landmarks, 1, CV_32FC1);
+        for (int i = 0; i < num_landmarks; ++i) {
+            y.at<float>(3 * y_index, 0) = landmarks[k][i][0];
+            y.at<float>((3 * y_index) + 1, 0) = landmarks[k][i][1];
+            //y.at<float>((3 * i) + 2, 0) = 1; // already 1, stays (homogeneous coordinate)
+            ++y_index;
+        }
+        // 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);
+        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[k](vertex_ids[k][i] * 3), base_face[k](vertex_ids[k][i] * 3 + 1), base_face[k](vertex_ids[k][i] * 3 + 2), 1.0f);
+            v_bar.at<float>(4 * v_bar_index, 0) = model_mean[0];
+            v_bar.at<float>((4 * v_bar_index) + 1, 0) = model_mean[1];
+            v_bar.at<float>((4 * v_bar_index) + 2, 0) = model_mean[2];
+            //v_bar.at<float>((4 * i) + 3, 0) = 1; // already 1, stays (homogeneous coordinate)
+            ++v_bar_index;
+            // note: now that a Vec4f is returned, we could use copyTo?
+        }
+    }
 
 	// Bring into standard regularised quadratic form with diagonal distance matrix Omega
 	Mat A = P * V_hat_h; // camera matrix times the basis
@@ -145,6 +181,35 @@ inline std::vector<float> fit_shape_to_landmarks_linear(const morphablemodel::Mo
 	return std::vector<float>(c_s);
 };
 
+/**
+ * Fits the shape of a Morphable Model to given 2D landmarks (i.e. estimates the maximum likelihood solution of the shape coefficients) as proposed in [1].
+ * It's a linear, closed-form solution fitting of the shape, with regularisation (prior towards the mean).
+ *
+ * [1] O. Aldrian & W. Smith, Inverse Rendering of Faces with a 3D Morphable Model, PAMI 2013.
+ *
+ * Note: Using less than the maximum number of coefficients to fit is not thoroughly tested yet and may contain an error.
+ * Note: Returns coefficients following standard normal distribution (i.e. all have similar magnitude). Why? Because we fit using the normalised basis?
+ * Note: The standard deviations given should be a vector, i.e. different for each landmark. This is not implemented yet.
+ *
+ * @param[in] morphable_model The Morphable Model whose shape (coefficients) are estimated.
+ * @param[in] affine_camera_matrix A 3x4 affine camera matrix from model to screen-space (should probably be of type CV_32FC1 as all our calculations are done with float).
+ * @param[in] landmarks 2D landmarks from an image to fit the model to.
+ * @param[in] vertex_ids The vertex ids in the model that correspond to the 2D points.
+ * @param[in] base_face The base or reference face from where the fitting is started. Usually this would be the models mean face, which is what will be used if the parameter is not explicitly specified.
+ * @param[in] lambda The regularisation parameter (weight of the prior towards the mean).
+ * @param[in] num_coefficients_to_fit How many shape-coefficients to fit (all others will stay 0). Should be bigger than zero, or boost::none to fit all coefficients.
+ * @param[in] detector_standard_deviation The standard deviation of the 2D landmarks given (e.g. of the detector used), in pixels.
+ * @param[in] model_standard_deviation The standard deviation of the 3D vertex points in the 3D model, projected to 2D (so the value is in pixels).
+ * @return The estimated shape-coefficients (alphas).
+ */
+inline std::vector<float> fit_shape_to_landmarks_linear(const morphablemodel::MorphableModel& morphable_model, cv::Mat affine_camera_matrix, std::vector<cv::Vec2f> landmarks, 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>())
+{
+    std::vector<std::vector<cv::Vec2f>> all_landmarks = {landmarks};
+    std::vector<std::vector<int>> all_vertex_ids = {vertex_ids};
+    return fit_shape_to_landmarks_linear_multi(morphable_model, { affine_camera_matrix }, all_landmarks, all_vertex_ids, { base_face }, lambda, num_coefficients_to_fit, detector_standard_deviation, model_standard_deviation );
+}
+
+
 	} /* namespace fitting */
 } /* namespace eos */