22 #ifndef NONLINEARCAMERAESTIMATION_HPP_ 23 #define NONLINEARCAMERAESTIMATION_HPP_ 25 #include "eos/fitting/detail/nonlinear_camera_estimation_detail.hpp" 27 #include "glm/gtc/matrix_transform.hpp" 29 #include "eos/fitting/detail/optional_cerealisation.hpp" 30 #include "cereal/cereal.hpp" 31 #include "cereal/archives/json.hpp" 33 #include "Eigen/Geometry" 34 #include "unsupported/Eigen/NonLinearOptimization" 36 #include "opencv2/core/core.hpp" 38 #include "boost/optional.hpp" 54 boost::optional<float> n;
55 boost::optional<float> f;
57 friend class cereal::access;
63 template<
class Archive>
66 archive(CEREAL_NVP(l), CEREAL_NVP(r), CEREAL_NVP(b), CEREAL_NVP(t), CEREAL_NVP(n), CEREAL_NVP(f));
116 boost::optional<float> focal_length;
118 friend class cereal::access;
124 template<
class Archive>
127 archive(CEREAL_NVP(camera_type), CEREAL_NVP(frustum), CEREAL_NVP(r_x), CEREAL_NVP(r_y), CEREAL_NVP(r_z), CEREAL_NVP(t_x), CEREAL_NVP(t_y), CEREAL_NVP(screen_width), CEREAL_NVP(screen_height), CEREAL_NVP(focal_length));
140 std::ofstream file(filename);
142 throw std::runtime_error(
"Error opening file for writing: " + filename);
144 cereal::JSONOutputArchive output_archive(file);
145 output_archive(cereal::make_nvp(
"rendering_parameters", rendering_parameters));
153 cv::Mat
to_mat(
const glm::mat4x4& glm_matrix)
158 auto glm_matrix_t = glm::transpose(glm_matrix);
159 cv::Mat opencv_mat(4, 4, CV_32FC1, &glm_matrix_t[0]);
161 return opencv_mat.clone();
185 auto rot_mtx_x = glm::rotate(glm::mat4(1.0f), params.r_x, glm::vec3{ 1.0f, 0.0f, 0.0f });
186 auto rot_mtx_y = glm::rotate(glm::mat4(1.0f), params.r_y, glm::vec3{ 0.0f, 1.0f, 0.0f });
187 auto rot_mtx_z = glm::rotate(glm::mat4(1.0f), params.r_z, glm::vec3{ 0.0f, 0.0f, 1.0f });
188 auto t_mtx = glm::translate(glm::mat4(1.0f), glm::vec3{ params.t_x, params.t_y, 0.0f });
189 auto modelview = t_mtx * rot_mtx_z * rot_mtx_x * rot_mtx_y;
203 auto ortho_projection =
to_mat(glm::ortho(params.frustum.l, params.frustum.r, params.frustum.b, params.frustum.t));
204 cv::Mat mvp = ortho_projection * view_model;
206 glm::vec4 viewport(0, height, width, -height);
208 cv::Mat viewport_mat = (cv::Mat_<float>(4, 4) << viewport[2] / 2.0f, 0.0f, 0.0f, viewport[2] / 2.0f + viewport[0],
209 0.0f, viewport[3] / 2.0f, 0.0f, viewport[3] / 2.0f + viewport[1],
210 0.0f, 0.0f, 1.0f, 0.0f,
211 0.0f, 0.0f, 0.0f, 1.0f);
213 cv::Mat full_projection_4x4 = viewport_mat * mvp;
214 cv::Mat full_projection_3x4 = full_projection_4x4.rowRange(0, 3);
215 full_projection_3x4.at<
float>(2, 0) = 0.0f;
216 full_projection_3x4.at<
float>(2, 1) = 0.0f;
217 full_projection_3x4.at<
float>(2, 2) = 0.0f;
218 full_projection_3x4.at<
float>(2, 3) = 1.0f;
220 return full_projection_3x4;
231 return glm::vec4(0, height, width, -height);
262 assert(image_points.size() == model_points.size());
263 assert(image_points.size() >= 6);
265 const float aspect =
static_cast<float>(width) / height;
269 Eigen::VectorXd parameters;
270 parameters.setConstant(num_params, 0.0);
271 parameters[5] = 110.0;
272 detail::OrthographicParameterProjection cost_function(image_points, model_points, width, height);
275 Eigen::NumericalDiff<detail::OrthographicParameterProjection> cost_function_with_derivative(cost_function, 0.0001);
278 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<detail::OrthographicParameterProjection>> lm(cost_function_with_derivative);
279 auto info = lm.minimize(parameters);
282 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]) };
283 return RenderingParameters{ CameraType::Orthographic, camera_frustum,
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]), width, height };
cv::Mat to_mat(const glm::mat4x4 &glm_matrix)
Converts a glm::mat4x4 to a cv::Mat.
Definition: nonlinear_camera_estimation.hpp:153
RenderingParameters estimate_orthographic_camera(std::vector< cv::Vec2f > image_points, std::vector< cv::Vec4f > model_points, int width, int height)
This algorithm estimates the rotation angles and translation of the model, as well as the viewing fru...
Definition: nonlinear_camera_estimation.hpp:259
void save_rendering_parameters(RenderingParameters rendering_parameters, std::string filename)
Definition: nonlinear_camera_estimation.hpp:138
glm::vec4 get_opencv_viewport(int width, int height)
Returns a glm/OpenGL compatible viewport vector that flips y and has the origin on the top-left...
Definition: nonlinear_camera_estimation.hpp:229
cv::Mat get_3x4_affine_camera_matrix(fitting::RenderingParameters params, int width, int height)
Creates a 3x4 affine camera matrix from given fitting parameters. The matrix transforms points direct...
Definition: nonlinear_camera_estimation.hpp:200
void serialize(Archive &archive)
Definition: nonlinear_camera_estimation.hpp:64
Namespace containing all of eos's 3D model fitting functionality.
Represents a set of estimated model parameters (rotation, translation) and camera parameters (viewing...
Definition: nonlinear_camera_estimation.hpp:102
CameraType
Type of a camera (projection).
Definition: nonlinear_camera_estimation.hpp:75
glm::mat4x4 get_4x4_modelview_matrix(fitting::RenderingParameters params)
Creates a 4x4 model-view matrix from given fitting parameters.
Definition: nonlinear_camera_estimation.hpp:182
void serialize(Archive &archive)
Definition: nonlinear_camera_estimation.hpp:125
A class representing a camera viewing frustum. At the moment used as orthographic camera only...
Definition: nonlinear_camera_estimation.hpp:50