GCC Code Coverage Report

Directory: ./
File: libs/geometry/vision/helper.cpp
Date: 2024-04-19 18:48:28
Exec Total Coverage
Lines: 96 154 62.3%
Branches: 51 238 21.4%
Line Branch Exec Source
1 /************************************************************************
2 *
3 * Copyright (C) 2017-2023 IRCAD France
4 * Copyright (C) 2017-2021 IHU Strasbourg
5 *
6 * This file is part of Sight.
7 *
8 * Sight is free software: you can redistribute it and/or modify it under
9 * the terms of the GNU Lesser General Public License as published by
10 * the Free Software Foundation, either version 3 of the License, or
11 * (at your option) any later version.
12 *
13 * Sight is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public
19 * License along with Sight. If not, see <https://www.gnu.org/licenses/>.
20 *
21 ***********************************************************************/
22
23 #include "helper.hpp"
24
25 #include "geometry/vision/detail/reprojection_error.hpp"
26
27 #include <core/spy_log.hpp>
28
29 #include <geometry/eigen/helper.hpp>
30
31 #include <cmath>
32 #include <thread>
33
34 namespace sight::geometry::vision::helper
35 {
36
37 //-----------------------------------------------------------------------------
38
39 3 error_and_points_t compute_reprojection_error(
40 const std::vector<cv::Point3f>& _object_points,
41 const std::vector<cv::Point2f>& _image_points,
42 const cv::Mat& _rvecs,
43 const cv::Mat& _tvecs,
44 const cv::Mat& _camera_matrix,
45 const cv::Mat& _dist_coeffs
46 )
47 {
48
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 error_and_points_t error_and_projected_points;
49 3 std::vector<cv::Point2f> image_points2;
50 3 int total_points = 0;
51 3 double total_err = 0;
52 3 double err = NAN;
53
54 //projection
55
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 6 cv::projectPoints(
56
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 6 cv::Mat(_object_points),
57 _rvecs,
58 _tvecs,
59 _camera_matrix,
60 _dist_coeffs,
61 image_points2
62 );
63
64
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 error_and_projected_points.second = image_points2;
65
66 //difference
67
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 3 err = cv::norm(cv::Mat(_image_points), cv::Mat(image_points2), cv::NORM_L2);
68
69 3 int n = static_cast<int>(_object_points.size());
70 3 total_err += err * err;
71 3 total_points += n;
72
73 3 error_and_projected_points.first = std::sqrt(total_err / total_points);
74
75 3 return error_and_projected_points;
76 3 }
77
78 //-----------------------------------------------------------------------------
79
80 ✗ cv::Matx44f camera_pose_monocular(
81 const std::vector<cv::Point3f>& _object_points,
82 const std::vector<cv::Point2f>& _image_points,
83 const cv::Mat _camera_matrix,
84 const cv::Mat& _dist_coeffs,
85 const int _flag
86 )
87 {
88 SIGHT_ASSERT(
89 "There should be the same number of 3d points than 2d points",
90 _object_points.size() == _image_points.size()
91 ✗ );
92
93 ✗ cv::Mat rvec;
94 ✗ cv::Mat tvec;
95 ✗ cv::Mat r;
96 ✗ cv::Mat t;
97 ✗ t = cv::Mat::eye(4, 4, CV_64F);
98
99 //solvePnP
100 ✗ cv::solvePnP(_object_points, _image_points, _camera_matrix, _dist_coeffs, rvec, tvec, false, _flag);
101
102 // to matrix
103 ✗ cv::Rodrigues(rvec, r); // R is 3x3
104
105 ✗ t(cv::Range(0, 3), cv::Range(0, 3)) = r * 1; // copies R into T
106 ✗ t(cv::Range(0, 3), cv::Range(3, 4)) = tvec * 1; // copies tvec into T
107
108 ✗ return cv::Matx44f(t);
109 }
110
111 //-----------------------------------------------------------------------------
112
113 ✗ cv::Matx44f camera_pose_stereo(
114 const std::vector<cv::Point3f>& _object_points,
115 const cv::Mat& _camera_matrix1,
116 const cv::Mat& _dist_coeffs1,
117 const cv::Mat& _camera_matrix2,
118 const cv::Mat& _dist_coeffs2,
119 const std::vector<cv::Point2f>& _img_points1,
120 const std::vector<cv::Point2f>& _img_points2,
121 const cv::Mat& _r,
122 const cv::Mat& _t
123 )
124 {
125 //1. initialize solution with solvePnP
126 ✗ cv::Mat rvec;
127 ✗ cv::Mat tvec;
128 ✗ cv::Mat r;
129 ✗ cv::Mat t;
130 ✗ t = cv::Mat::eye(4, 4, CV_64F);
131
132 ✗ cv::Mat extrinsic = cv::Mat::eye(4, 4, CV_64F);
133
134 ✗ extrinsic(cv::Range(0, 3), cv::Range(0, 3)) = _r * 1;
135 ✗ extrinsic(cv::Range(0, 3), cv::Range(3, 4)) = _t * 1;
136
137 ✗ cv::solvePnP(
138 _object_points,
139 _img_points1,
140 _camera_matrix1,
141 _dist_coeffs1,
142 rvec,
143 tvec,
144 false,
145 cv::SOLVEPNP_ITERATIVE
146 );
147
148 ✗ std::vector<double> optim_vector = {{
149 ✗ rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2),
150 ✗ tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2)
151 }
152 ✗ };
153
154 //2. Creation of ceres problem
155 //Minimization of sum of reprojection error for each points in each images
156
157 ✗ ::ceres::Problem problem;
158
159 //Cost function for image 1
160 ✗ for(std::size_t i = 0 ; i < _img_points1.size() ; ++i)
161 {
162 ✗ ::ceres::CostFunction* cost_function = detail::reprojection_error::create(
163 _camera_matrix1,
164 _dist_coeffs1,
165 ✗ _img_points1[i],
166 ✗ _object_points[i],
167 ✗ cv::Mat::eye(4, 4, CV_64F)
168 );
169 ✗ problem.AddResidualBlock(
170 cost_function,
171 nullptr,
172 optim_vector.data()
173 );
174 }
175
176 //image 2
177 ✗ for(std::size_t i = 0 ; i < _img_points2.size() ; ++i)
178 {
179 ✗ ::ceres::CostFunction* cost_function = detail::reprojection_error::create(
180 _camera_matrix2,
181 _dist_coeffs2,
182 ✗ _img_points2[i],
183 ✗ _object_points[i],
184 extrinsic
185 );
186 ✗ problem.AddResidualBlock(
187 cost_function,
188 nullptr,
189 optim_vector.data()
190 );
191 }
192
193 ✗ ::ceres::Solver::Options options;
194 ✗ options.linear_solver_type = ::ceres::SPARSE_NORMAL_CHOLESKY;
195 ✗ options.trust_region_strategy_type = ::ceres::LEVENBERG_MARQUARDT;
196 ✗ options.minimizer_progress_to_stdout = false;
197 ✗ options.gradient_tolerance = 1e-8;
198 ✗ options.function_tolerance = 1e-8;
199 ✗ options.max_num_iterations = 100;
200
201 ✗ int numthreads = static_cast<int>(std::thread::hardware_concurrency() / 2);
202
203 ✗ options.num_threads = numthreads;
204
205 ✗ ::ceres::Solver::Summary summary;
206 ✗ ::ceres::Solve(options, &problem, &summary);
207
208 ✗ SIGHT_DEBUG("Ceres report : " + summary.FullReport());
209
210 ✗ cv::Mat final_r_vec = (cv::Mat_<double>(3, 1) << optim_vector[0], optim_vector[1], optim_vector[2]);
211 ✗ cv::Mat final_t_vec = (cv::Mat_<double>(3, 1) << optim_vector[3], optim_vector[4], optim_vector[5]);
212
213 ✗ cv::Rodrigues(final_r_vec, r); //Rotation vec. to matrix
214
215 ✗ t(cv::Range(0, 3), cv::Range(0, 3)) = r * 1; // copies R into T
216 ✗ t(cv::Range(0, 3), cv::Range(3, 4)) = final_t_vec * 1; // copies tvec into T
217
218 ✗ return cv::Matx44f(t);
219 }
220
221 //-----------------------------------------------------------------------------
222
223 2 void calibrate_pointing_tool(
224 const data::vector::csptr _matrices_vector,
225 data::matrix4::sptr _calibration_matrix,
226 data::matrix4::sptr _center_matrix
227 )
228 {
229
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 if(_matrices_vector->size() < 4)
230 {
231 ✗ SIGHT_WARN("Number of points when computing the tool calibration should be more than 5.");
232 ✗ return;
233 }
234
235 2 geometry::eigen::helper::EigenMatrix matrix_sum;
236 2 matrix_sum.fill(0.);
237 2 Eigen::Vector4d vector_sum;
238 2 vector_sum.fill(0);
239
240
2/2
✓ Branch 0 taken 80 times.
✓ Branch 1 taken 2 times.
 82 for(const auto& i : *_matrices_vector)
241 {
242 80 data::matrix4::csptr m1 = std::dynamic_pointer_cast<data::matrix4>(i);
243 80 SIGHT_ASSERT("This element of the vector is not a data::matrix4", m1);
244 80 geometry::eigen::helper::EigenMatrix xyz1;
245 80 xyz1.fill(0.);
246 80 xyz1(0, 0) = (*m1)(0, 3);
247 80 xyz1(0, 1) = (*m1)(1, 3);
248 80 xyz1(0, 2) = (*m1)(2, 3);
249 80 xyz1(0, 3) = 1.0;
250
251 80 matrix_sum = matrix_sum + xyz1.transpose() * xyz1;
252
2/6
✓ Branch 0 taken 80 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 80 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 80 vector_sum = vector_sum + xyz1.squaredNorm() * Eigen::Vector4d(xyz1(0, 0), xyz1(0, 1), xyz1(0, 2), xyz1(0, 3));
253 80 }
254
255 2 geometry::eigen::helper::EigenMatrix temp_matrix;
256 2 temp_matrix.fill(0.);
257 2 temp_matrix(0, 0) = vector_sum[0];
258 2 temp_matrix(0, 1) = vector_sum[1];
259 2 temp_matrix(0, 2) = vector_sum[2];
260 2 temp_matrix(0, 3) = vector_sum[3];
261 2 temp_matrix = -temp_matrix* matrix_sum.inverse();
262
263 2 const double a = -1. * temp_matrix(0, 0) / 2.;
264 2 const double b = -1. * temp_matrix(0, 1) / 2.;
265 2 const double c = -1. * temp_matrix(0, 2) / 2.;
266
267 2 Eigen::Vector3d translation;
268 2 translation.fill(0);
269
2/2
✓ Branch 0 taken 80 times.
✓ Branch 1 taken 2 times.
 82 for(const auto& i : *_matrices_vector)
270 {
271
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 data::matrix4::csptr m1 = std::dynamic_pointer_cast<data::matrix4>(i);
272 80 SIGHT_ASSERT("This element of the vector is not a data::matrix4", m1);
273
2/4
✓ Branch 0 taken 80 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 80 times.
✗ Branch 4 not taken.
 160 const geometry::eigen::helper::EigenMatrix point_matrix = geometry::eigen::helper::to_eigen(m1);
274
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 geometry::eigen::helper::EigenMatrix center_matrix(point_matrix);
275
1/2
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
 80 const geometry::eigen::helper::EigenMatrix point_matrix_inverse = point_matrix.inverse();
276
277 80 center_matrix(0, 3) = a;
278 80 center_matrix(1, 3) = b;
279 80 center_matrix(2, 3) = c;
280
281 80 const geometry::eigen::helper::EigenMatrix calibration_matrix = point_matrix_inverse * center_matrix;
282
1/2
✓ Branch 0 taken 80 times.
✗ Branch 1 not taken.
 80 translation(0) += calibration_matrix(0, 3);
283 80 translation(1) += calibration_matrix(1, 3);
284
1/2
✓ Branch 0 taken 80 times.
✗ Branch 1 not taken.
 80 translation(2) += calibration_matrix(2, 3);
285 80 }
286
287 2 translation /= static_cast<double>(_matrices_vector->size());
288
289 2 (*_calibration_matrix)(0, 3) = translation(0);
290 2 (*_calibration_matrix)(1, 3) = translation(1);
291 2 (*_calibration_matrix)(2, 3) = translation(2);
292
293 2 (*_center_matrix)(0, 3) = a;
294 2 (*_center_matrix)(1, 3) = b;
295 2 (*_center_matrix)(2, 3) = c;
296 }
297
298 //-----------------------------------------------------------------------------
299
300 391 data::point_list::sptr detect_chessboard(
301 const cv::Mat& _img,
302 std::size_t _x_dim,
303 std::size_t _y_dim,
304 float _scale
305 )
306 {
307 391 data::point_list::sptr pointlist;
308
309 391 SIGHT_ASSERT("Expected 8bit pixel components, this image has: " << 8 * _img.elemSize1(), _img.elemSize1() == 1);
310
311 // Ensure that we have a true depth-less 2D image.
312
2/6
✗ Branch 0 not taken.
✓ Branch 1 taken 391 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 6 taken 391 times.
✗ Branch 7 not taken.
 391 const cv::Mat img2d = _img.dims == 3 ? _img.reshape(0, 2, _img.size + 1) : _img;
313
314 391 cv::Mat gray_img;
315
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 389 times.
 391 if(_img.channels() == 1)
316 {
317
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 gray_img = img2d;
318 }
319 else
320 {
321
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 389 times.
 389 const auto cvt_method = _img.channels() == 3 ? cv::COLOR_RGB2GRAY : cv::COLOR_RGBA2GRAY;
322
323
1/2
✓ Branch 1 taken 389 times.
✗ Branch 2 not taken.
 389 cv::cvtColor(img2d, gray_img, cvt_method);
324 }
325
326 391 const cv::Size board_size(static_cast<int>(_x_dim) - 1, static_cast<int>(_y_dim) - 1);
327 391 std::vector<cv::Point2f> corners;
328
329 391 const int flags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FILTER_QUADS
330 | cv::CALIB_CB_FAST_CHECK;
331
332 391 cv::Mat detection_image;
333
334
2/2
✓ Branch 0 taken 123 times.
✓ Branch 1 taken 268 times.
 391 if(_scale < 1.F)
335 {
336
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 cv::resize(gray_img, detection_image, cv::Size(), _scale, _scale);
337 }
338 else
339 {
340
1/2
✓ Branch 1 taken 268 times.
✗ Branch 2 not taken.
 268 detection_image = gray_img;
341 }
342
343
3/4
✓ Branch 1 taken 391 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 123 times.
✓ Branch 4 taken 268 times.
 391 const bool pattern_was_found = cv::findChessboardCorners(detection_image, board_size, corners, flags);
344
345
2/2
✓ Branch 0 taken 123 times.
✓ Branch 1 taken 268 times.
 391 if(pattern_was_found)
346 {
347 // Rescale points to get their coordinates in the full scale image.
348 7634 const auto rescale = [_scale](cv::Point2f& _pt){_pt = _pt / _scale;};
349 123 std::ranges::for_each(corners, rescale);
350
351 // Refine points coordinates in the full scale image.
352
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 cv::TermCriteria term(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 30, 0.1);
353
2/4
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 123 times.
✗ Branch 5 not taken.
 123 cv::cornerSubPix(gray_img, corners, cv::Size(5, 5), cv::Size(-1, -1), term);
354
355
2/4
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 123 times.
 246 pointlist = std::make_shared<data::point_list>();
356
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 data::point_list::container_t& points = pointlist->get_points();
357
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 points.reserve(corners.size());
358
359
1/2
✓ Branch 2 taken 7634 times.
✗ Branch 3 not taken.
 7634 const auto cv2_sight_pt = [](const cv::Point2f& _p){return std::make_shared<data::point>(_p.x, _p.y);};
360
1/2
✓ Branch 1 taken 123 times.
✗ Branch 2 not taken.
 123 std::ranges::transform(corners, std::back_inserter(points), cv2_sight_pt);
361 }
362
363 391 return pointlist;
364 391 }
365
366 // ----------------------------------------------------------------------------
367
368 } // namespace sight::geometry::vision::helper
369