gaze  0.1.0
Perform gaze tracking with common webcams.
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Groups Pages
head_pose_estimation.cpp
1 #include "gaze/pipeline_steps/head_pose_estimation.h"
2 
3 #include <cstdlib>
4 #include <string>
5 #include <vector>
6 
7 #include "dlib/gui_widgets.h"
8 #include "dlib/image_processing.h"
9 #include "opencv2/opencv.hpp"
10 #include "yaml-cpp/yaml.h"
11 
12 #include "gaze/util/config.h"
13 #include "gaze/util/data.h"
14 
15 
16 namespace gaze {
17 
18 namespace pipeline {
19 
20 HeadPoseEstimation::HeadPoseEstimation() {
21  YAML::Node config = util::get_config(this->number);
22  this->name = config["name"] ?
23  config["name"].as<std::string>() : "HeadPoseEstimation";
24 
25  if (config["landmark_indices"]) {
26  this->index_list = config["landmark_indices"].as<std::vector<int>>();
27  } else {
28  this->index_list = {30, 8, 36, 45, 48, 54};
29  }
30 
31  if (config["model"]) {
32  this->model_points = config["model"].as<std::vector<cv::Point3d>>();
33  } else {
34  this->model_points = {
35  {0.0, 0.0, 0.0},
36  {0.0, -0.0636, -0.0125},
37  {-0.0433, 0.0327, -0.026},
38  {0.0433, 0.0327, -0.026},
39  {-0.0289, -0.0289, -0.0241},
40  {0.0289, -0.0289, -0.0241}
41  };
42  }
43 
44  if (config["model_scale"]) {
45  this->model_scale = config["model_scale"].as<decltype(this->model_scale)>();
46  } else {
47  this->model_scale = 1;
48  }
49 
50  this->coord_base_overlay.push_back({{-1, -1}, {-1, -1},
51  dlib::rgb_pixel(255, 0, 0)});
52  this->coord_base_overlay.push_back({{-1, -1}, {-1, -1},
53  dlib::rgb_pixel(0, 255, 0)});
54  this->coord_base_overlay.push_back({{-1, -1}, {-1, -1},
55  dlib::rgb_pixel(0, 0, 255)});
56 }
57 
58 void HeadPoseEstimation::update_overlay(const util::Data& data) {
59  this->widget->clear_overlay();
60  if (data.landmarks.num_parts() <= 0) {
61  return;
62  }
63 
64  // Project reference points to image.
65  std::vector<cv::Point3d> base_vecs =
66  {{0, 0, 0},
67  {0.1 * this->model_scale, 0, 0},
68  {0, 0.1 * this->model_scale, 0},
69  {0, 0, 0.1 * this->model_scale}};
70  std::vector<cv::Point2d> image_points;
71  cv::projectPoints(base_vecs, data.head_rotation, data.head_translation,
72  this->read_or_set_camera_matrix(data),
73  this->get_and_maybe_read_distortions(data), image_points);
74 
75  // coordinate system
76  for (auto i = decltype(this->coord_base_overlay.size()){0};
77  i < this->coord_base_overlay.size(); ++i) {
78  this->coord_base_overlay[i] = dlib::image_display::overlay_line(
79  dlib::point(image_points[0].x, image_points[0].y),
80  dlib::point(image_points[i + 1].x, image_points[i + 1].y),
81  this->coord_base_overlay[i].color);
82  }
83  this->widget->add_overlay(this->coord_base_overlay);
84 
85  // detected vs. projected points in overlay
86  std::vector<cv::Point2d> projected_points;
87  cv::projectPoints(this->model_points, data.head_rotation,
88  data.head_translation, this->read_or_set_camera_matrix(data),
89  this->get_and_maybe_read_distortions(data), projected_points);
90 
91  auto color_detection = dlib::rgb_pixel(255, 0, 0);
92  auto color_projection = dlib::rgb_pixel(0, 0, 255);
95  for (auto i = decltype(projected_points.size()){0};
96  i < projected_points.size(); ++i) {
97  detections.push_back(dlib::image_display::overlay_circle(
98  data.landmarks.part(this->index_list[i]), 2, color_detection,
99  "d" + std::to_string(this->index_list[i])));
100  projections.push_back(dlib::image_display::overlay_circle(
101  dlib::point(projected_points[i].x, projected_points[i].y),
102  2, color_projection, "p" + std::to_string(this->index_list[i])));
103  }
104  this->widget->add_overlay(detections);
105  this->widget->add_overlay(projections);
106 }
107 
109  if (data.landmarks.num_parts() <= 0) {
110  return;
111  }
112 
113  // Convert dlib::points to cv::Point for solvePNP
114  std::vector<cv::Point2d> im_points;
115  for (auto i : this->index_list) {
116  im_points.push_back(cv::Point2d(data.landmarks.part(i).x(),
117  data.landmarks.part(i).y()));
118  }
119 
120  cv::solvePnP(this->model_points, im_points,
121  this->read_or_set_camera_matrix(data),
122  this->get_and_maybe_read_distortions(data),
124 }
125 
127  this->widget->set_image(data.image);
128  this->update_overlay(data);
129 }
130 
132  const util::Data& data) {
133  if (this->has_camera_matrix) {
134  return this->camera_matrix;
135  }
136  YAML::Node camera_config = util::get_config()["meta"]["camera"];
137  if (camera_config["camera_matrix"]) {
138  this->camera_matrix = camera_config["camera_matrix"].as<cv::Mat>();
139  } else {
140  this->camera_matrix <<
141  data.source_image.cols, 0, data.source_image.cols / 2,
142  0, data.source_image.cols, data.source_image.rows / 2,
143  0, 0, 1;
144  }
145  this->has_camera_matrix = true;
146  return this->camera_matrix;
147 }
148 
150  if (this->has_distortions) {
151  return this->distortions;
152  }
153  YAML::Node camera_config = util::get_config()["meta"]["camera"];
154  if (camera_config["distortion_coefficients"]) {
155  this->distortions = camera_config["distortion_coefficients"].as<cv::Mat>();
156  } else {
157  this->distortions = cv::Mat::zeros(4, 1, CV_64F);
158  }
159  this->has_distortions = true;
160  return this->distortions;
161 }
162 
163 } // namespace pipeline
164 
165 } // namespace gaze
std::shared_ptr< widget_type > widget
void process(util::Data &data) override
static MatExpr zeros(int rows, int cols, int type)
dlib::array2d< dlib::bgr_pixel > image
Definition: data.h:52
cv::Mat get_and_maybe_read_distortions(const util::Data &data)
cv::Mat head_rotation
Definition: data.h:82
#define CV_64F
void push_back(const value_type &__x)
dlib::full_object_detection landmarks
Definition: data.h:64
cv::Matx33d read_or_set_camera_matrix(const util::Data &data)
SOLVEPNP_ITERATIVE
cv::Mat head_translation
Definition: data.h:87
std::string name
Definition: pipeline_step.h:31
bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=SOLVEPNP_ITERATIVE)
Wraps the data acquired per frame into a single instance.
Definition: data.h:27
size_type size() const noexcept
void projectPoints(InputArray objectPoints, InputArray rvec, InputArray tvec, InputArray cameraMatrix, InputArray distCoeffs, OutputArray imagePoints, OutputArray jacobian=noArray(), double aspectRatio=0)
void visualize(util::Data &data) override
cv::Mat source_image
Definition: data.h:47
YAML::Node get_config()
Definition: config.in.cpp:12