commit 3c1a5c06b8de6fe6861f1ad416c7ecbe21e178d1
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 27 16:27:19 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Mon Mar 28 09:23:30 2022 -0700
tree 5b165df6111413274bbf1735007571ba4005e6ef
parent f67f67d8a3904f63aa26b35d56f55e1675d8ea11

Undistort image in camera reader

This may be the source of bad detection at the side of the image.

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: Ic121ffe57ebd7d350cf486436267bc7f24150ef3

y2022/vision/viewer.cc

diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 7c35ce9..38f3d71 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -224,9 +224,10 @@
                      << FLAGS_calibration_node << "\" with team number "
                      << FLAGS_calibration_team_number;
 
-  auto intrinsics_float = cv::Mat(3, 3, CV_32F,
-                                  const_cast<void *>(static_cast<const void *>(
-                                      calibration->intrinsics()->data())));
+  const auto intrinsics_float = cv::Mat(
+      3, 3, CV_32F,
+      const_cast<void *>(
+          static_cast<const void *>(calibration->intrinsics()->data())));
   cv::Mat intrinsics;
   intrinsics_float.convertTo(intrinsics, CV_64F);
 
@@ -240,11 +241,21 @@
   cv::Mat extrinsics;
   extrinsics_float.convertTo(extrinsics, CV_64F);
 
+  const auto dist_coeffs_float = cv::Mat(
+      5, 1, CV_32F,
+      const_cast<void *>(
+          static_cast<const void *>(calibration->dist_coeffs()->data())));
+  cv::Mat dist_coeffs;
+  dist_coeffs_float.convertTo(dist_coeffs, CV_64F);
+
   TargetEstimator estimator(intrinsics, extrinsics);
 
   for (auto it = file_list.begin() + FLAGS_skip; it < file_list.end(); it++) {
     LOG(INFO) << "Reading file " << (it - file_list.begin()) << ": " << *it;
-    cv::Mat image_mat = cv::imread(it->c_str());
+    cv::Mat image_mat_distorted = cv::imread(it->c_str());
+    cv::Mat image_mat;
+    cv::undistort(image_mat_distorted, image_mat, intrinsics, dist_coeffs);
+
     BlobDetector::BlobResult blob_result;
     blob_result.binarized_image =
         cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC1);