y2022/vision/viewer.cc

#include <map>
#include <opencv2/calib3d.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <random>

#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/time/time.h"
#include "frc971/vision/vision_generated.h"
#include "y2022/vision/blob_detector.h"
#include "y2022/vision/target_estimate_generated.h"

DEFINE_string(capture, "",
              "If set, capture a single image and save it to this filename.");
DEFINE_string(channel, "/camera", "Channel name for the image.");
DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
DEFINE_string(png_dir, "", "Path to a set of images to display.");
DEFINE_bool(show_features, true, "Show the blobs.");

namespace y2022 {
namespace vision {
namespace {

using namespace frc971::vision;

std::map<int64_t, BlobDetector::BlobResult> target_est_map;
aos::Fetcher<frc971::vision::CameraImage> image_fetcher;
aos::Fetcher<y2022::vision::TargetEstimate> target_estimate_fetcher;

std::vector<std::vector<cv::Point>> FbsToCvBlobs(
    const flatbuffers::Vector<flatbuffers::Offset<Blob>> &blobs_fbs) {
  std::vector<std::vector<cv::Point>> blobs;
  for (const auto blob : blobs_fbs) {
    std::vector<cv::Point> points;
    for (const Point *point : *blob->points()) {
      points.emplace_back(cv::Point{point->x(), point->y()});
    }
    blobs.emplace_back(points);
  }
  return blobs;
}

std::vector<BlobDetector::BlobStats> FbsToBlobStats(
    const flatbuffers::Vector<flatbuffers::Offset<BlobStatsFbs>>
        &blob_stats_fbs) {
  std::vector<BlobDetector::BlobStats> blob_stats;
  for (const auto stats_fbs : blob_stats_fbs) {
    cv::Point centroid{stats_fbs->centroid()->x(), stats_fbs->centroid()->y()};
    blob_stats.emplace_back(BlobDetector::BlobStats{
        centroid, stats_fbs->aspect_ratio(), stats_fbs->area(),
        static_cast<size_t>(stats_fbs->num_points())});
  }
  return blob_stats;
}

bool DisplayLoop() {
  int64_t target_timestamp = 0;
  if (target_estimate_fetcher.Fetch()) {
    const TargetEstimate *target_est = target_estimate_fetcher.get();
    CHECK(target_est != nullptr)
        << "Got null when trying to fetch target estimate";

    target_timestamp = target_est->image_monotonic_timestamp_ns();
    if (target_est->blob_result()->filtered_blobs()->size() > 0) {
      VLOG(2) << "Got blobs for timestamp " << target_est << "\n";
    }
    // Store the TargetEstimate data so we can match timestamp with image
    target_est_map[target_timestamp] = BlobDetector::BlobResult(
        {cv::Mat(), 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()}});
    // Only keep last 10 matches
    while (target_est_map.size() > 10u) {
      target_est_map.erase(target_est_map.begin());
    }
  }
  int64_t image_timestamp = 0;
  if (!image_fetcher.Fetch()) {
    VLOG(2) << "Couldn't fetch image";
    return true;
  }
  const CameraImage *image = image_fetcher.get();
  CHECK(image != nullptr) << "Couldn't read image";
  image_timestamp = image->monotonic_timestamp_ns();
  VLOG(2) << "Got image at timestamp: " << image_timestamp;

  // Create color image:
  cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
                          (void *)image->data()->data());
  cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
  cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);

  if (!FLAGS_capture.empty()) {
    cv::imwrite(FLAGS_capture, bgr_image);
    return false;
  }

  auto target_est_it = target_est_map.find(image_timestamp);
  if (target_est_it != target_est_map.end()) {
    LOG(INFO) << image->monotonic_timestamp_ns() << ": # unfiltered blobs: "
              << target_est_it->second.unfiltered_blobs.size()
              << "; # filtered blobs: "
              << target_est_it->second.filtered_blobs.size();

    cv::Mat ret_image =
        cv::Mat::zeros(cv::Size(image->cols(), image->rows()), CV_8UC3);
    BlobDetector::DrawBlobs(ret_image, target_est_it->second.filtered_blobs,
                            target_est_it->second.unfiltered_blobs,
                            target_est_it->second.blob_stats,
                            target_est_it->second.centroid);
    cv::imshow("blobs", ret_image);
  }

  cv::imshow("image", bgr_image);

  int keystroke = cv::waitKey(1);
  if ((keystroke & 0xFF) == static_cast<int>('c')) {
    // Convert again, to get clean image
    cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
    std::stringstream name;
    name << "capture-" << aos::realtime_clock::now() << ".png";
    cv::imwrite(name.str(), bgr_image);
    LOG(INFO) << "Saved image file: " << name.str();
  } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
    return false;
  }
  return true;
}

void ViewerMain() {
  aos::FlatbufferDetachedBuffer<aos::Configuration> config =
      aos::configuration::ReadConfig(FLAGS_config);

  aos::ShmEventLoop event_loop(&config.message());

  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) {
        if (!DisplayLoop()) {
          LOG(INFO) << "Calling event_loop Exit";
          event_loop.Exit();
        };
      },
      ::std::chrono::milliseconds(100));

  event_loop.Run();
}
}  // namespace
}  // namespace vision
}  // namespace y2022

// Quick and lightweight viewer for images
int main(int argc, char **argv) {
  aos::InitGoogle(&argc, &argv);
  y2022::vision::ViewerMain();
}