commit 46a783840e74c51f06a41757959e2f4ffadcfe0a
author Jim Ostrowski <yimmy13@gmail.com> Sun Feb 06 14:05:58 2022 -0800
committer Jim Ostrowski <yimmy13@gmail.com> Sun Feb 06 22:16:30 2022 -0800
tree a827e6140e1cbc330e51b47972f55085104039a0
parent 7704756f30436a31a2e7f6f2377f61403f5ac532

A couple fixes to viewer

Changed to use Fetch() instead of FetchNext(), which was crashing
Properly initialize the target_estimate_fetcher
Only call DrawBlobs and display blob image if we have estimate
Send proper size image into DrawBlobs

Change-Id: Id0646848a80a659092c28773ba1154a109e7d835
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>

y2022/vision/viewer.cc

diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 7c13d1b..6ee6144 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -68,8 +68,10 @@
 
   // TODO(Milind) Store the target estimates and match them by timestamp to make
   // sure we're getting the right one.
-  CHECK(target_estimate_fetcher.FetchNext());
-  const TargetEstimate *target = target_estimate_fetcher.get();
+  const TargetEstimate *target_est = nullptr;
+  if (target_estimate_fetcher.Fetch()) {
+    target_est = target_estimate_fetcher.get();
+  }
 
   // Create color image:
   cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
@@ -82,19 +84,23 @@
     return false;
   }
 
-  LOG(INFO) << image->monotonic_timestamp_ns()
-            << ": # blobs: " << target->blob_result()->filtered_blobs()->size();
+  LOG(INFO) << image->monotonic_timestamp_ns() << ": # unfiltered blobs: "
+            << target_est->blob_result()->unfiltered_blobs()->size()
+            << "; # filtered blobs: "
+            << target_est->blob_result()->filtered_blobs()->size();
 
-  cv::Mat ret_image;
-  BlobDetector::DrawBlobs(
-      ret_image, FbsToCvBlobs(*target->blob_result()->filtered_blobs()),
-      FbsToCvBlobs(*target->blob_result()->unfiltered_blobs()),
-      FbsToBlobStats(*target->blob_result()->blob_stats()),
-      cv::Point{target->blob_result()->centroid()->x(),
-                target->blob_result()->centroid()->y()});
+  cv::Mat ret_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+  if (target_est != nullptr) {
+    BlobDetector::DrawBlobs(
+        ret_image, FbsToCvBlobs(*target_est->blob_result()->filtered_blobs()),
+        FbsToCvBlobs(*target_est->blob_result()->unfiltered_blobs()),
+        FbsToBlobStats(*target_est->blob_result()->blob_stats()),
+        cv::Point{target_est->blob_result()->centroid()->x(),
+                  target_est->blob_result()->centroid()->y()});
+    cv::imshow("blobs", ret_image);
+  }
 
   cv::imshow("image", rgb_image);
-  cv::imshow("blobs", ret_image);
 
   int keystroke = cv::waitKey(1);
   if ((keystroke & 0xFF) == static_cast<int>('c')) {
@@ -119,6 +125,9 @@
   image_fetcher =
       event_loop.MakeFetcher<frc971::vision::CameraImage>(FLAGS_channel);
 
+  target_estimate_fetcher =
+      event_loop.MakeFetcher<y2022::vision::TargetEstimate>(FLAGS_channel);
+
   // Run the display loop
   event_loop.AddPhasedLoop(
       [&event_loop](int) {