Fixed last commit, added helper functions for glm/Mat and...

Fixed last commit, added helper functions for glm/Mat and get_4x4_modelview_matrix(fitting::OrthographicRenderingParameters

include/eos/fitting/nonlinear_camera_estimation.hpp

@@ -74,6 +74,92 @@ struct OrthographicRenderingParameters
 	Frustum frustum;
 };
 
+/**
+ * @brief Converts a glm::mat4x4 to a cv::Mat.
+ *
+ * Note: move to render namespace
+ */
+cv::Mat to_mat(const glm::mat4x4& glm_matrix)
+{
+	// glm stores its matrices in col-major order in memory, OpenCV in row-major order.
+	// Hence we transpose the glm matrix to flip the memory layout, and then point OpenCV
+	// to that location.
+	auto glm_matrix_t = glm::transpose(glm_matrix);
+	cv::Mat opencv_mat(4, 4, CV_32FC1, &glm_matrix_t[0]);
+	// we need to clone because the underlying data of the original goes out of scope
+	return opencv_mat.clone();
+};
+
+/**
+ * @brief Creates a 4x4 model-view matrix from given fitting parameters.
+ *
+ * Together with the Frustum information, this describes the full
+ * orthographic rendering parameters of the OpenGL pipeline.
+ * Example:
+ *
+ * @code
+ * fitting::OrthographicRenderingParameters rendering_params = ...;
+ * glm::mat4x4 view_model = get_4x4_modelview_matrix(rendering_params);
+ * glm::mat4x4 ortho_projection = glm::ortho(rendering_params.frustum.l, rendering_params.frustum.r, rendering_params.frustum.b, rendering_params.frustum.t);
+ * glm::vec4 viewport(0, image.rows, image.cols, -image.rows); // flips y, origin top-left, like in OpenCV
+ *
+ * // project a point from 3D to 2D:
+ * glm::vec3 point_3d = ...; // from a mesh for example
+ * glm::vec3 point_2d = glm::project(point_3d, view_model, ortho_projection, viewport);
+ * @endcode
+ */
+glm::mat4x4 get_4x4_modelview_matrix(fitting::OrthographicRenderingParameters params)
+{
+	// rotation order: RPY * P
+	auto rot_mtx_x = glm::rotate(glm::mat4(1.0f), params.r_x, glm::vec3{ 1.0f, 0.0f, 0.0f });
+	auto rot_mtx_y = glm::rotate(glm::mat4(1.0f), params.r_y, glm::vec3{ 0.0f, 1.0f, 0.0f });
+	auto rot_mtx_z = glm::rotate(glm::mat4(1.0f), params.r_z, glm::vec3{ 0.0f, 0.0f, 1.0f });
+	auto t_mtx = glm::translate(glm::mat4(1.0f), glm::vec3{ params.t_x, params.t_y, 0.0f });
+	auto modelview = t_mtx * rot_mtx_z * rot_mtx_x * rot_mtx_y;
+	return modelview;
+};
+
+/**
+ * @brief Creates a 3x4 affine camera matrix from given fitting parameters. The
+ * matrix transforms points directly from model-space to screen-space.
+ *
+ * This function is mainly used since the linear shape fitting fitting::fit_shape_to_landmarks_linear
+ * expects one of these 3x4 affine camera matrices, as well as render::extract_texture.
+ */
+cv::Mat get_3x4_affine_camera_matrix(fitting::OrthographicRenderingParameters params, int width, int height)
+{
+	auto view_model = to_mat(get_4x4_modelview_matrix(params));
+	auto ortho_projection = to_mat(glm::ortho(params.frustum.l, params.frustum.r, params.frustum.b, params.frustum.t));
+	cv::Mat mvp = ortho_projection * view_model;
+
+	glm::vec4 viewport(0, height, width, -height); // flips y, origin top-left, like in OpenCV
+	// equivalent to what glm::project's viewport does, but we don't change z and w:
+	cv::Mat viewport_mat = (cv::Mat_<float>(4, 4) << viewport[2] / 2.0f, 0.0f,       0.0f, viewport[2] / 2.0f + viewport[0],
+												     0.0f,               viewport[3] / 2.0f, 0.0f, viewport[3] / 2.0f + viewport[1],
+													 0.0f,               0.0f,               1.0f, 0.0f,
+													 0.0f,               0.0f,               0.0f, 1.0f);
+
+	cv::Mat full_projection_4x4 = viewport_mat * mvp;
+	cv::Mat full_projection_3x4 = full_projection_4x4.rowRange(0, 3); // we take the first 3 rows, but then set the last one to [0 0 0 1]
+	full_projection_3x4.at<float>(2, 0) = 0.0f;
+	full_projection_3x4.at<float>(2, 1) = 0.0f;
+	full_projection_3x4.at<float>(2, 2) = 0.0f;
+	full_projection_3x4.at<float>(2, 3) = 1.0f;
+
+	return full_projection_3x4;
+};
+
+/**
+ * @brief Returns a glm/OpenGL compatible viewport vector that flips y and
+ * has the origin on the top-left, like in OpenCV.
+ *
+ * Note: Move to detail namespace / not used at the moment.
+ */
+glm::vec4 get_opencv_viewport(int width, int height)
+{
+	return glm::vec4(0, height, width, -height);
+};
+
 /**
  * @brief This algorithm estimates the rotation angles and translation of the model, as
  * well as the viewing frustum of the camera, given a set of corresponding 2D-3D points.
@@ -99,7 +185,7 @@ struct OrthographicRenderingParameters
  * @param[in] height Height of the image (or viewport).
  * @return The estimated model and camera parameters.
  */
-RenderingParameters estimate_orthographic_camera(std::vector<cv::Vec2f> image_points, std::vector<cv::Vec4f> model_points, int width, int height)
+OrthographicRenderingParameters estimate_orthographic_camera(std::vector<cv::Vec2f> image_points, std::vector<cv::Vec4f> model_points, int width, int height)
 {
 	using cv::Mat;
 	assert(image_points.size() == model_points.size());