22 #ifndef AFFINECAMERAESTIMATION_HPP_ 23 #define AFFINECAMERAESTIMATION_HPP_ 25 #include "eos/render/utils.hpp" 27 #include "opencv2/core/core.hpp" 28 #include "opencv2/core/core_c.h" 56 assert(image_points.size() == model_points.size());
57 assert(image_points.size() >= 4);
59 const int num_correspondences =
static_cast<int>(image_points.size());
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);
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);
79 cv::reduce(matImagePoints, imagePointsMean, 0, CV_REDUCE_AVG);
80 imagePointsMean = cv::repeat(imagePointsMean, matImagePoints.rows, 1);
81 matImagePoints = matImagePoints - imagePointsMean;
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));
88 average_norm /= matImagePoints.rows;
90 float scaleFactor =
static_cast<float>(std::sqrt(2)) / average_norm;
91 matImagePoints *= scaleFactor;
94 Mat T = Mat::zeros(3, 3, CV_32FC1);
95 T.at<
float>(0, 0) = scaleFactor;
96 T.at<
float>(1, 1) = scaleFactor;
97 T.at<
float>(0, 2) = -imagePointsMean.at<
float>(0, 0) * scaleFactor;
98 T.at<
float>(1, 2) = -imagePointsMean.at<
float>(0, 1) * scaleFactor;
99 T.at<
float>(2, 2) = 1;
102 Mat tmpOrigMdlPoints = matModelPoints.clone();
105 cv::reduce(matModelPoints, modelPointsMean, 0, CV_REDUCE_AVG);
106 modelPointsMean = cv::repeat(modelPointsMean, matModelPoints.rows, 1);
107 matModelPoints = matModelPoints - modelPointsMean;
111 for (
int row = 0; row < matModelPoints.rows; ++row) {
112 average_norm +=
static_cast<float>(cv::norm(matModelPoints.row(row), cv::NORM_L2));
114 average_norm /= matModelPoints.rows;
116 scaleFactor =
static_cast<float>(std::sqrt(3)) / average_norm;
117 matModelPoints *= scaleFactor;
120 Mat U = Mat::zeros(4, 4, CV_32FC1);
121 U.at<
float>(0, 0) = scaleFactor;
122 U.at<
float>(1, 1) = scaleFactor;
123 U.at<
float>(2, 2) = scaleFactor;
124 U.at<
float>(0, 3) = -modelPointsMean.at<
float>(0, 0) * scaleFactor;
125 U.at<
float>(1, 3) = -modelPointsMean.at<
float>(0, 1) * scaleFactor;
126 U.at<
float>(2, 3) = -modelPointsMean.at<
float>(0, 2) * scaleFactor;
127 U.at<
float>(3, 3) = 1;
133 Mat A_8 = Mat::zeros(num_correspondences * 2, 8, CV_32FC1);
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);
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);
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();
151 C_tilde.row(1) = p_8.rowRange(4, 8).t();
152 C_tilde.at<
float>(2, 3) = 1;
154 Mat P_Affine = T.inv() * C_tilde * U;
177 inline cv::Vec2f
project_affine(cv::Vec4f vertex, cv::Mat affine_camera_matrix,
int screen_width,
int screen_height)
180 cv::Mat clip_coords = affine_camera_matrix * cv::Mat(vertex);
183 return screen_coords;
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'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