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/camera_reader.cc

diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 2fd581a..92d3727 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -90,8 +90,12 @@
 }
 }  // namespace
 
-void CameraReader::ProcessImage(cv::Mat image_mat,
+void CameraReader::ProcessImage(cv::Mat image_mat_distorted,
                                 int64_t image_monotonic_timestamp_ns) {
+  cv::Mat image_mat;
+  cv::undistort(image_mat_distorted, image_mat, CameraIntrinsics(),
+                CameraDistCoeffs());
+
   BlobDetector::BlobResult blob_result;
   BlobDetector::ExtractBlobs(image_mat, &blob_result);
   auto builder = target_estimate_sender_.MakeBuilder();

y2022/vision/camera_reader.h

diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index a777bcc..6a00351 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -63,7 +63,8 @@
   const calibration::CameraCalibration *FindCameraCalibration() const;
 
   // Processes an image (including sending the results).
-  void ProcessImage(cv::Mat image, int64_t image_monotonic_timestamp_ns);
+  void ProcessImage(cv::Mat image_mat_distorted,
+                    int64_t image_monotonic_timestamp_ns);
 
   // Reads an image, and then performs all of our processing on it.
   void ReadImage();
@@ -96,6 +97,7 @@
     const cv::Mat result(5, 1, CV_32F,
                          const_cast<void *>(static_cast<const void *>(
                              camera_calibration_->dist_coeffs()->data())));
+    result.convertTo(result, CV_64F);
     CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
     return result;
   }

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);