Added preliminary version of perspective camera estimation. It doesn't really work quite yet.

include/eos/fitting/detail/nonlinear_camera_estimation_detail.hpp

@@ -119,6 +119,72 @@ private:
 	int height;
 };
 
+
+// Next: PerspectiveCamEsti test
+// ret: 3rd entry is the z
+// radians
+// expects the landmark points to be in opencv convention, i.e. origin TL
+glm::vec3 project_perspective(glm::vec3 point, float rot_x_pitch, float rot_y_yaw, float rot_z_roll, float tx, float ty, float tz, float fovy, /* fixed params now: */ int width, int height)
+{
+	// We could use quaternions in here, to be independent of the RPY... etc convention.
+	// Then, the user can decompose the quaternion as he wishes to. But then we'd have to estimate 4 parameters?
+	// This can of course be optimised, but we keep it this way while we're debugging and as long as it's not a performance issue.
+	auto rot_mtx_x = glm::rotate(glm::mat4(1.0f), rot_x_pitch, glm::vec3{ 1.0f, 0.0f, 0.0f });
+	auto rot_mtx_y = glm::rotate(glm::mat4(1.0f), rot_y_yaw, glm::vec3{ 0.0f, 1.0f, 0.0f });
+	auto rot_mtx_z = glm::rotate(glm::mat4(1.0f), rot_z_roll, glm::vec3{ 0.0f, 0.0f, 1.0f });
+	auto t_mtx = glm::translate(glm::mat4(1.0f), glm::vec3{ tx, ty, tz }); // glm: Col-major memory layout. [] gives the column
+
+	// Note/Todo: Is this the full ortho? n/f missing? or do we need to multiply it with Proj...? See Shirley CG!
+	// glm::frustum()?
+	const float aspect = static_cast<float>(width) / height;
+
+	//auto ortho_mtx = glm::ortho(-1.0f * aspect * frustum_scale, 1.0f * aspect * frustum_scale, -1.0f * frustum_scale, 1.0f * frustum_scale);
+	auto persp_mtx = glm::perspective(fovy, aspect, 1.0f, 2000.0f);
+
+	glm::vec4 viewport(0, height, width, -height); // flips y, origin top-left, like in OpenCV
+	// P = RPY * P
+	glm::vec3 res = glm::project(point, t_mtx * rot_mtx_z * rot_mtx_x * rot_mtx_y, persp_mtx, viewport);
+	return res;
+};
+
+
+/**
+ * @brief LevenbergMarquardt cost function for the orthographic camera estimation.
+ */
+struct PerspectiveParameterProjection : Functor<double>
+{
+public:
+	// Creates a new PerspectiveParameterProjection object with given data.
+	PerspectiveParameterProjection(std::vector<cv::Vec2f> image_points, std::vector<cv::Vec4f> model_points, int width, int height) : Functor<double>(7, image_points.size()), image_points(image_points), model_points(model_points), width(width), height(height) {};
+
+	// x = current params, fvec = the errors/differences of the proj with current params and the GT (image_points)
+	int operator()(const Eigen::VectorXd& x, Eigen::VectorXd& fvec) const
+	{
+		const float aspect = static_cast<float>(width) / height;
+		for (int i = 0; i < values(); i++)
+		{
+			// opencv to glm:
+			glm::vec3 point_3d(model_points[i][0], model_points[i][1], model_points[i][2]);
+			// projection given current params x:
+			glm::vec3 proj_with_current_param_esti = project_perspective(point_3d, x[0], x[1], x[2], x[3], x[4], x[5], x[6], width, height);
+			cv::Vec2f proj_point_2d(proj_with_current_param_esti.x, proj_with_current_param_esti.y);
+			// diff of current proj to ground truth, our error
+			auto diff = cv::norm(proj_point_2d, image_points[i]);
+			// fvec should contain the differences
+			// don't square it.
+			fvec[i] = diff;
+		}
+		return 0;
+	};
+
+private:
+	std::vector<cv::Vec2f> image_points;
+	std::vector<cv::Vec4f> model_points;
+	int width;
+	int height;
+};
+
+
 		} /* namespace detail */
 	} /* namespace fitting */
 } /* namespace eos */

include/eos/fitting/nonlinear_camera_estimation.hpp

@@ -74,6 +74,17 @@ struct OrthographicRenderingParameters
 	Frustum frustum;
 };
 
+struct PerspectiveRenderingParameters
+{
+	float r_x; // Pitch.
+	float r_y; // Yaw. Positive means subject is looking left (we see her right cheek).
+	float r_z; // Roll. Positive means the subject's right eye is further down than the other one (he tilts his head to the right).
+	float t_x;
+	float t_y;
+	float t_z;
+	float fovy;
+};
+
 /**
  * @brief Converts a glm::mat4x4 to a cv::Mat.
  *
@@ -212,6 +223,34 @@ OrthographicRenderingParameters estimate_orthographic_camera(std::vector<cv::Vec
 	return OrthographicRenderingParameters{ static_cast<float>(parameters[0]), static_cast<float>(parameters[1]), static_cast<float>(parameters[2]), static_cast<float>(parameters[3]), static_cast<float>(parameters[4]), camera_frustum };
 };
 
+PerspectiveRenderingParameters estimate_perspective_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());
+	assert(image_points.size() >= 7); // Number of correspondence points given needs to be equal to or larger than 7 (7 now, correct?)
+
+	const float aspect = static_cast<float>(width) / height;
+
+	// Set up the initial parameter vector and the cost function:
+	int num_params = 7;
+	Eigen::VectorXd parameters; // [rot_x_pitch, rot_y_yaw, rot_z_roll, t_x, t_y, t_z, fovy]
+	parameters.setConstant(num_params, 0.0); // Set all 6 values to zero (except frustum_scale, see next line)
+	parameters[5] = -1000.0; // tz. This is just a rough hand-chosen tz estimate - we could do a lot better. //But it works.
+	parameters[6] = 0.55; // 45°. This is just a rough hand-chosen fovy estimate - we could do a lot better. //But it works.
+	detail::PerspectiveParameterProjection cost_function(image_points, model_points, width, height);
+
+	// Note: we have analytical derivatives, so we should use them!
+	Eigen::NumericalDiff<detail::PerspectiveParameterProjection> cost_function_with_derivative(cost_function, 0.0001);
+	// I had to change the default value of epsfcn, it works well around 0.0001. It couldn't produce the derivative with the default, I guess the changes in the gradient were too small.
+
+	Eigen::LevenbergMarquardt<Eigen::NumericalDiff<detail::PerspectiveParameterProjection>> lm(cost_function_with_derivative);
+	auto info = lm.minimize(parameters); // we could or should use the return value
+										 // 'parameters' contains the solution now.
+
+	//Frustum camera_frustum{ -1.0f * aspect * static_cast<float>(parameters[5]), 1.0f * aspect * static_cast<float>(parameters[5]), -1.0f * static_cast<float>(parameters[5]), 1.0f * static_cast<float>(parameters[5]) };
+	return PerspectiveRenderingParameters{ static_cast<float>(parameters[0]), static_cast<float>(parameters[1]), static_cast<float>(parameters[2]), static_cast<float>(parameters[3]), static_cast<float>(parameters[4]), static_cast<float>(parameters[5]), static_cast<float>(parameters[6]) };
+};
+
 	} /* namespace fitting */
 } /* namespace eos */