eos  0.7.1
nonlinear_camera_estimation.hpp
1 /*
2  * Eos - A 3D Morphable Model fitting library written in modern C++11/14.
3  *
4  * File: include/eos/fitting/nonlinear_camera_estimation.hpp
5  *
6  * Copyright 2015 Patrik Huber
7  *
8  * Licensed under the Apache License, Version 2.0 (the "License");
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
12  * http://www.apache.org/licenses/LICENSE-2.0
13  *
14  * Unless required by applicable law or agreed to in writing, software
15  * distributed under the License is distributed on an "AS IS" BASIS,
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20 #pragma once
21 
22 #ifndef NONLINEARCAMERAESTIMATION_HPP_
23 #define NONLINEARCAMERAESTIMATION_HPP_
24 
25 #include "eos/fitting/detail/nonlinear_camera_estimation_detail.hpp"
26 
27 #include "glm/gtc/matrix_transform.hpp"
28 
29 #include "eos/fitting/detail/optional_cerealisation.hpp"
30 #include "cereal/cereal.hpp"
31 #include "cereal/archives/json.hpp"
32 
33 #include "Eigen/Geometry"
34 #include "unsupported/Eigen/NonLinearOptimization"
35 
36 #include "opencv2/core/core.hpp"
37 
38 #include "boost/optional.hpp"
39 
40 #include <vector>
41 #include <cassert>
42 
43 namespace eos {
44  namespace fitting {
45 
50 struct Frustum
51 {
52  float l, r, b, t;
53  // optional<float> n, f;
54  boost::optional<float> n;
55  boost::optional<float> f;
56 
57  friend class cereal::access;
63  template<class Archive>
64  void serialize(Archive& archive)
65  {
66  archive(CEREAL_NVP(l), CEREAL_NVP(r), CEREAL_NVP(b), CEREAL_NVP(t), CEREAL_NVP(n), CEREAL_NVP(f));
67  };
68 };
69 
75 enum class CameraType
76 {
77  Orthographic,
78  Perspective
79 };
80 
103 {
104  CameraType camera_type; // what's the default?
105  Frustum frustum;
106 
107  float r_x; // Pitch.
108  float r_y; // Yaw. Positive means subject is looking left (we see her right cheek).
109  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).
110  float t_x; // Todo: define whether it's the camera translation/rotation or the model's.
111  float t_y;
112 
113  int screen_width;
114  int screen_height;
115 
116  boost::optional<float> focal_length; // only for certain camera types
117 
118  friend class cereal::access;
124  template<class Archive>
125  void serialize(Archive& archive)
126  {
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));
128  };
129 };
130 
138 void save_rendering_parameters(RenderingParameters rendering_parameters, std::string filename)
139 {
140  std::ofstream file(filename);
141  if (file.fail()) {
142  throw std::runtime_error("Error opening file for writing: " + filename);
143  }
144  cereal::JSONOutputArchive output_archive(file);
145  output_archive(cereal::make_nvp("rendering_parameters", rendering_parameters));
146 };
147 
153 cv::Mat to_mat(const glm::mat4x4& glm_matrix)
154 {
155  // glm stores its matrices in col-major order in memory, OpenCV in row-major order.
156  // Hence we transpose the glm matrix to flip the memory layout, and then point OpenCV
157  // to that location.
158  auto glm_matrix_t = glm::transpose(glm_matrix);
159  cv::Mat opencv_mat(4, 4, CV_32FC1, &glm_matrix_t[0]);
160  // we need to clone because the underlying data of the original goes out of scope
161  return opencv_mat.clone();
162 };
163 
183 {
184  // rotation order: RPY * P
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;
190  return modelview;
191 };
192 
200 cv::Mat get_3x4_affine_camera_matrix(fitting::RenderingParameters params, int width, int height)
201 {
202  auto view_model = to_mat(get_4x4_modelview_matrix(params));
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;
205 
206  glm::vec4 viewport(0, height, width, -height); // flips y, origin top-left, like in OpenCV
207  // equivalent to what glm::project's viewport does, but we don't change z and w:
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);
212 
213  cv::Mat full_projection_4x4 = viewport_mat * mvp;
214  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]
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;
219 
220  return full_projection_3x4;
221 };
222 
229 glm::vec4 get_opencv_viewport(int width, int height)
230 {
231  return glm::vec4(0, height, width, -height);
232 };
233 
259 RenderingParameters estimate_orthographic_camera(std::vector<cv::Vec2f> image_points, std::vector<cv::Vec4f> model_points, int width, int height)
260 {
261  using cv::Mat;
262  assert(image_points.size() == model_points.size());
263  assert(image_points.size() >= 6); // Number of correspondence points given needs to be equal to or larger than 6
264 
265  const float aspect = static_cast<float>(width) / height;
266 
267  // Set up the initial parameter vector and the cost function:
268  int num_params = 6;
269  Eigen::VectorXd parameters; // [rot_x_pitch, rot_y_yaw, rot_z_roll, t_x, t_y, frustum_scale]
270  parameters.setConstant(num_params, 0.0); // Set all 6 values to zero (except frustum_scale, see next line)
271  parameters[5] = 110.0; // This is just a rough hand-chosen scaling estimate - we could do a lot better. But it works.
272  detail::OrthographicParameterProjection cost_function(image_points, model_points, width, height);
273 
274  // Note: we have analytical derivatives, so we should use them!
275  Eigen::NumericalDiff<detail::OrthographicParameterProjection> cost_function_with_derivative(cost_function, 0.0001);
276  // 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.
277 
278  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<detail::OrthographicParameterProjection>> lm(cost_function_with_derivative);
279  auto info = lm.minimize(parameters); // we could or should use the return value
280  // 'parameters' contains the solution now.
281 
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 };
284 };
285 
286  } /* namespace fitting */
287 } /* namespace eos */
288 
289 #endif /* NONLINEARCAMERAESTIMATION_HPP_ */
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&#39;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