eos  0.7.1
affine_camera_estimation.hpp
1 /*
2  * Eos - A 3D Morphable Model fitting library written in modern C++11/14.
3  *
4  * File: include/eos/fitting/affine_camera_estimation.hpp
5  *
6  * Copyright 2014, 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 AFFINECAMERAESTIMATION_HPP_
23 #define AFFINECAMERAESTIMATION_HPP_
24 
25 #include "eos/render/utils.hpp"
26 
27 #include "opencv2/core/core.hpp"
28 #include "opencv2/core/core_c.h" // for CV_REDUCE_AVG
29 
30 #include <vector>
31 #include <cassert>
32 
33 namespace eos {
34  namespace fitting {
35 
53 cv::Mat estimate_affine_camera(std::vector<cv::Vec2f> image_points, std::vector<cv::Vec4f> model_points)
54 {
55  using cv::Mat;
56  assert(image_points.size() == model_points.size());
57  assert(image_points.size() >= 4); // Number of correspondence points given needs to be equal to or larger than 4
58 
59  const int num_correspondences = static_cast<int>(image_points.size());
60 
61  Mat matImagePoints; // will be numCorrespondences x 2, CV_32FC1
62  Mat matModelPoints; // will be numCorrespondences x 3, CV_32FC1
63 
64  for (int i = 0; i < image_points.size(); ++i) {
65  Mat imgPoint(1, 2, CV_32FC1);
66  imgPoint.at<float>(0, 0) = image_points[i][0];
67  imgPoint.at<float>(0, 1) = image_points[i][1];
68  matImagePoints.push_back(imgPoint);
69 
70  Mat mdlPoint(1, 3, CV_32FC1);
71  mdlPoint.at<float>(0, 0) = model_points[i][0];
72  mdlPoint.at<float>(0, 1) = model_points[i][1];
73  mdlPoint.at<float>(0, 2) = model_points[i][2];
74  matModelPoints.push_back(mdlPoint);
75  }
76 
77  // translate the centroid of the image points to the origin:
78  Mat imagePointsMean; // use non-homogeneous coords for the next few steps? (less submatrices etc overhead)
79  cv::reduce(matImagePoints, imagePointsMean, 0, CV_REDUCE_AVG);
80  imagePointsMean = cv::repeat(imagePointsMean, matImagePoints.rows, 1); // get T_13 and T_23 from imagePointsMean
81  matImagePoints = matImagePoints - imagePointsMean;
82  // scale the image points such that the RMS distance from the origin is sqrt(2):
83  // 1) calculate the average norm (root mean squared distance) of all vectors
84  float average_norm = 0.0f;
85  for (int row = 0; row < matImagePoints.rows; ++row) {
86  average_norm += static_cast<float>(cv::norm(matImagePoints.row(row), cv::NORM_L2));
87  }
88  average_norm /= matImagePoints.rows;
89  // 2) multiply every vectors coordinate by sqrt(2)/average_norm
90  float scaleFactor = static_cast<float>(std::sqrt(2)) / average_norm;
91  matImagePoints *= scaleFactor; // add unit homogeneous component here
92  // The points in matImagePoints now have a RMS distance from the origin of sqrt(2).
93  // The normalisation matrix so that the 2D points are mean-free and their norm is as described above.
94  Mat T = Mat::zeros(3, 3, CV_32FC1);
95  T.at<float>(0, 0) = scaleFactor; // s_x
96  T.at<float>(1, 1) = scaleFactor; // s_y
97  T.at<float>(0, 2) = -imagePointsMean.at<float>(0, 0) * scaleFactor; // t_x
98  T.at<float>(1, 2) = -imagePointsMean.at<float>(0, 1) * scaleFactor; // t_y
99  T.at<float>(2, 2) = 1;
100 
101  // center the model points to the origin:
102  Mat tmpOrigMdlPoints = matModelPoints.clone(); // Temp for testing: Save the original coordinates.
103  // translate the centroid of the model points to the origin:
104  Mat modelPointsMean; // use non-homogeneous coords for the next few steps? (less submatrices etc overhead)
105  cv::reduce(matModelPoints, modelPointsMean, 0, CV_REDUCE_AVG);
106  modelPointsMean = cv::repeat(modelPointsMean, matModelPoints.rows, 1);
107  matModelPoints = matModelPoints - modelPointsMean;
108  // scale the model points such that the RMS distance from the origin is sqrt(3):
109  // 1) calculate the average norm (root mean squared distance) of all vectors
110  average_norm = 0.0f;
111  for (int row = 0; row < matModelPoints.rows; ++row) {
112  average_norm += static_cast<float>(cv::norm(matModelPoints.row(row), cv::NORM_L2));
113  }
114  average_norm /= matModelPoints.rows;
115  // 2) multiply every vectors coordinate by sqrt(3)/avgnorm
116  scaleFactor = static_cast<float>(std::sqrt(3)) / average_norm;
117  matModelPoints *= scaleFactor; // add unit homogeneous component here
118  // The points in matModelPoints now have a RMS distance from the origin of sqrt(3).
119  // The normalisation matrix so that the 3D points are mean-free and their norm is as described above.
120  Mat U = Mat::zeros(4, 4, CV_32FC1);
121  U.at<float>(0, 0) = scaleFactor; // s_x
122  U.at<float>(1, 1) = scaleFactor; // s_y
123  U.at<float>(2, 2) = scaleFactor; // s_z
124  U.at<float>(0, 3) = -modelPointsMean.at<float>(0, 0) * scaleFactor; // t_x
125  U.at<float>(1, 3) = -modelPointsMean.at<float>(0, 1) * scaleFactor; // t_y
126  U.at<float>(2, 3) = -modelPointsMean.at<float>(0, 2) * scaleFactor; // t_z
127  U.at<float>(3, 3) = 1;
128 
129  // Estimate the normalised camera matrix (C tilde).
130  // We are solving the system $A_8 * p_8 = b$
131  // The solution is obtained by the pseudo-inverse of A_8:
132  // $p_8 = A_8^+ * b$
133  Mat A_8 = Mat::zeros(num_correspondences * 2, 8, CV_32FC1);
134  //Mat p_8(); // p_8 is 8 x 1. We are solving for it.
135  Mat b(num_correspondences * 2, 1, CV_32FC1);
136  for (int i = 0; i < num_correspondences; ++i) {
137  A_8.at<float>(2 * i, 0) = matModelPoints.at<float>(i, 0); // could maybe made faster by assigning the whole row/col-range if possible?
138  A_8.at<float>(2 * i, 1) = matModelPoints.at<float>(i, 1);
139  A_8.at<float>(2 * i, 2) = matModelPoints.at<float>(i, 2);
140  A_8.at<float>(2 * i, 3) = 1;
141  A_8.at<float>((2 * i) + 1, 4) = matModelPoints.at<float>(i, 0);
142  A_8.at<float>((2 * i) + 1, 5) = matModelPoints.at<float>(i, 1);
143  A_8.at<float>((2 * i) + 1, 6) = matModelPoints.at<float>(i, 2);
144  A_8.at<float>((2 * i) + 1, 7) = 1;
145  b.at<float>(2 * i, 0) = matImagePoints.at<float>(i, 0);
146  b.at<float>((2 * i) + 1, 0) = matImagePoints.at<float>(i, 1);
147  }
148  Mat p_8 = A_8.inv(cv::DECOMP_SVD) * b;
149  Mat C_tilde = Mat::zeros(3, 4, CV_32FC1);
150  C_tilde.row(0) = p_8.rowRange(0, 4).t(); // The first row of C_tilde consists of the first 4 entries of the col-vector p_8
151  C_tilde.row(1) = p_8.rowRange(4, 8).t(); // Second row = last 4 entries
152  C_tilde.at<float>(2, 3) = 1; // the last row is [0 0 0 1]
153 
154  Mat P_Affine = T.inv() * C_tilde * U;
155  return P_Affine;
156 };
157 
177 inline cv::Vec2f project_affine(cv::Vec4f vertex, cv::Mat affine_camera_matrix, int screen_width, int screen_height)
178 {
179  // Transform to clip space:
180  cv::Mat clip_coords = affine_camera_matrix * cv::Mat(vertex);
181  // Take the x and y coordinates in clip space and apply the window transform:
182  cv::Vec2f screen_coords = render::clip_to_screen_space(cv::Vec2f(clip_coords.rowRange(0, 2)), screen_width, screen_height);
183  return screen_coords;
184 };
185 
186  } /* namespace fitting */
187 } /* namespace eos */
188 
189 #endif /* AFFINECAMERAESTIMATION_HPP_ */
cv::Mat estimate_affine_camera(std::vector< cv::Vec2f > image_points, std::vector< cv::Vec4f > model_points)
Definition: affine_camera_estimation.hpp:53
cv::Vec2f project_affine(cv::Vec4f vertex, cv::Mat affine_camera_matrix, int screen_width, int screen_height)
Definition: affine_camera_estimation.hpp:177
Namespace containing all of eos&#39;s 3D model fitting functionality.
cv::Vec2f clip_to_screen_space(const cv::Vec2f &clip_coordinates, int screen_width, int screen_height)
Definition: utils.hpp:50