y2022/vision/blob_detector.cc

#include "y2022/vision/blob_detector.h"

#include <cmath>
#include <optional>
#include <string>

#include "aos/network/team_number.h"
#include "aos/time/time.h"
#include "opencv2/features2d.hpp"
#include "opencv2/imgproc.hpp"

DEFINE_uint64(red_delta, 100,
              "Required difference between green pixels vs. red");
DEFINE_uint64(blue_delta, 50,
              "Required difference between green pixels vs. blue");

DEFINE_bool(use_outdoors, false,
            "If true, change thresholds to handle outdoor illumination");
DEFINE_uint64(outdoors_red_delta, 100,
              "Difference between green pixels vs. red, when outdoors");
DEFINE_uint64(outdoors_blue_delta, 1,
              "Difference between green pixels vs. blue, when outdoors");

namespace y2022 {
namespace vision {

cv::Mat BlobDetector::ThresholdImage(cv::Mat bgr_image) {
  size_t red_delta = FLAGS_red_delta;
  size_t blue_delta = FLAGS_blue_delta;

  if (FLAGS_use_outdoors) {
    red_delta = FLAGS_outdoors_red_delta;
    blue_delta = FLAGS_outdoors_blue_delta;
  }

  cv::Mat binarized_image(cv::Size(bgr_image.cols, bgr_image.rows), CV_8UC1);
  for (int row = 0; row < bgr_image.rows; row++) {
    for (int col = 0; col < bgr_image.cols; col++) {
      cv::Vec3b pixel = bgr_image.at<cv::Vec3b>(row, col);
      uint8_t blue = pixel.val[0];
      uint8_t green = pixel.val[1];
      uint8_t red = pixel.val[2];
      // Simple filter that looks for green pixels sufficiently brigher than
      // red and blue
      if ((green > blue + blue_delta) && (green > red + red_delta)) {
        binarized_image.at<uint8_t>(row, col) = 255;
      } else {
        binarized_image.at<uint8_t>(row, col) = 0;
      }
    }
  }

  return binarized_image;
}

std::vector<std::vector<cv::Point>> BlobDetector::FindBlobs(
    cv::Mat binarized_image) {
  // find the contours (blob outlines)
  std::vector<std::vector<cv::Point>> contours;
  std::vector<cv::Vec4i> hierarchy;
  cv::findContours(binarized_image, contours, hierarchy, cv::RETR_CCOMP,
                   cv::CHAIN_APPROX_SIMPLE);

  return contours;
}

std::vector<BlobDetector::BlobStats> BlobDetector::ComputeStats(
    const std::vector<std::vector<cv::Point>> &blobs) {
  std::vector<BlobDetector::BlobStats> blob_stats;
  for (auto blob : blobs) {
    auto blob_size = cv::boundingRect(blob).size();
    cv::Moments moments = cv::moments(blob);

    const auto centroid =
        cv::Point(moments.m10 / moments.m00, moments.m01 / moments.m00);
    const double aspect_ratio =
        static_cast<double>(blob_size.width) / blob_size.height;
    const double area = moments.m00;
    const size_t num_points = blob.size();

    blob_stats.emplace_back(
        BlobStats{centroid, aspect_ratio, area, num_points});
  }
  return blob_stats;
}

namespace {

// Linear equation in the form ax + by = c
struct Line {
 public:
  double a, b, c;

  std::optional<cv::Point2d> Intersection(const Line &l) const {
    // Use Cramer's rule to solve for the intersection
    const double denominator = Determinant(a, b, l.a, l.b);
    const double numerator_x = Determinant(c, b, l.c, l.b);
    const double numerator_y = Determinant(a, c, l.a, l.c);

    std::optional<cv::Point2d> intersection = std::nullopt;
    // Return nullopt if the denominator is 0, meaning the same slopes
    if (denominator != 0) {
      intersection =
          cv::Point2d(numerator_x / denominator, numerator_y / denominator);
    }

    return intersection;
  }

 private:  // Determinant of [[a, b], [c, d]]
  static double Determinant(double a, double b, double c, double d) {
    return (a * d) - (b * c);
  }
};

struct Circle {
 public:
  cv::Point2d center;
  double radius;

  static std::optional<Circle> Fit(std::vector<cv::Point2d> centroids) {
    CHECK_EQ(centroids.size(), 3ul);
    // For the 3 points, we have 3 equations in the form
    // (x - h)^2 + (y - k)^2 = r^2
    // Manipulate them to solve for the center and radius
    // (x1 - h)^2 + (y1 - k)^2 = r^2 ->
    // x1^2 + h^2 - 2x1h + y1^2 + k^2 - 2y1k = r^2
    // Also, (x2 - h)^2 + (y2 - k)^2 = r^2
    // Subtracting these two, we get
    // x1^2 - x2^2 - 2h(x1 - x2) + y1^2 - y2^2 - 2k(y1 - y2) = 0 ->
    // h(x1 - x2) + k(y1 - y2) = (-x1^2 + x2^2 - y1^2 + y2^2) / -2
    // Doing the same with equations 1 and 3, we get the second linear equation
    // h(x1 - x3) + k(y1 - y3) = (-x1^2 + x3^2 - y1^2 + y3^2) / -2
    // Now, we can solve for their intersection and find the center
    const auto l =
        Line{centroids[0].x - centroids[1].x, centroids[0].y - centroids[1].y,
             (-std::pow(centroids[0].x, 2) + std::pow(centroids[1].x, 2) -
              std::pow(centroids[0].y, 2) + std::pow(centroids[1].y, 2)) /
                 -2.0};
    const auto m =
        Line{centroids[0].x - centroids[2].x, centroids[0].y - centroids[2].y,
             (-std::pow(centroids[0].x, 2) + std::pow(centroids[2].x, 2) -
              std::pow(centroids[0].y, 2) + std::pow(centroids[2].y, 2)) /
                 -2.0};
    const auto center = l.Intersection(m);

    std::optional<Circle> circle = std::nullopt;
    if (center) {
      // Now find the radius
      const double radius = cv::norm(centroids[0] - *center);
      circle = Circle{*center, radius};
    }
    return circle;
  }

  double DistanceTo(cv::Point2d p) const {
    const auto p_prime = TranslateToOrigin(p);
    // Now, the distance is simply the difference between distance from the
    // origin to p' and the radius.
    return std::abs(cv::norm(p_prime) - radius);
  }

  double AngleOf(cv::Point2d p) const {
    auto p_prime = TranslateToOrigin(p);
    // Flip the y because y values go downwards.
    p_prime.y *= -1;
    return std::atan2(p_prime.y, p_prime.x);
  }

  // TODO(milind): handle wrapping around 2pi
  bool InAngleRange(cv::Point2d p, double theta_min, double theta_max) const {
    const double theta = AngleOf(p);
    return (theta >= theta_min && theta <= theta_max);
  }

 private:
  // Translate the point on the circle
  // as if the circle's center is the origin (0,0)
  cv::Point2d TranslateToOrigin(cv::Point2d p) const {
    return cv::Point2d(p.x - center.x, p.y - center.y);
  }
};

}  // namespace

void BlobDetector::FilterBlobs(BlobResult *blob_result) {
  std::vector<std::vector<cv::Point>> filtered_blobs;
  std::vector<BlobStats> filtered_stats;

  auto blob_it = blob_result->unfiltered_blobs.begin();
  auto stats_it = blob_result->blob_stats.begin();
  while (blob_it < blob_result->unfiltered_blobs.end() &&
         stats_it < blob_result->blob_stats.end()) {
    constexpr double kTapeAspectRatio = 5.0 / 2.0;
    constexpr double kAspectRatioThreshold = 1.6;
    constexpr double kMinArea = 10;
    constexpr size_t kMinNumPoints = 6;

    // Remove all blobs that are at the bottom of the image, have a different
    // aspect ratio than the tape, or have too little area or points.
    if ((std::abs(1.0 - kTapeAspectRatio / stats_it->aspect_ratio) <
         kAspectRatioThreshold) &&
        (stats_it->area >= kMinArea) &&
        (stats_it->num_points >= kMinNumPoints)) {
      filtered_blobs.push_back(*blob_it);
      filtered_stats.push_back(*stats_it);
    }
    blob_it++;
    stats_it++;
  }

  // Threshold for mean distance from a blob centroid to a circle.
  constexpr double kCircleDistanceThreshold = 10.0;
  // We should only expect to see blobs between these angles on a circle.
  constexpr double kDegToRad = M_PI / 180.0;
  constexpr double kMinBlobAngle = 50.0 * kDegToRad;
  constexpr double kMaxBlobAngle = M_PI - kMinBlobAngle;
  std::vector<std::vector<cv::Point>> blob_circle;
  Circle circle;
  std::vector<cv::Point2d> centroids;

  // If we see more than this number of blobs after filtering based on
  // color/size, the circle fit may detect noise so just return no blobs.
  constexpr size_t kMinFilteredBlobs = 3;
  constexpr size_t kMaxFilteredBlobs = 50;
  if (filtered_blobs.size() >= kMinFilteredBlobs &&
      filtered_blobs.size() <= kMaxFilteredBlobs) {
    constexpr size_t kRansacIterations = 15;
    for (size_t i = 0; i < kRansacIterations; i++) {
      // Pick 3 random blobs and see how many fit on their circle
      const size_t j = std::rand() % filtered_blobs.size();
      const size_t k = std::rand() % filtered_blobs.size();
      const size_t l = std::rand() % filtered_blobs.size();

      // Restart if the random indices clash
      if ((j == k) || (j == l) || (k == l)) {
        i--;
        continue;
      }

      std::vector<std::vector<cv::Point>> current_blobs{
          filtered_blobs[j], filtered_blobs[k], filtered_blobs[l]};
      std::vector<cv::Point2d> current_centroids{filtered_stats[j].centroid,
                                                 filtered_stats[k].centroid,
                                                 filtered_stats[l].centroid};
      const std::optional<Circle> current_circle =
          Circle::Fit(current_centroids);

      // Make sure that a circle could be created from the points
      if (!current_circle) {
        continue;
      }

      // Only try to fit points to this circle if all of these are between
      // certain angles.
      if (current_circle->InAngleRange(current_centroids[0], kMinBlobAngle,
                                       kMaxBlobAngle) &&
          current_circle->InAngleRange(current_centroids[1], kMinBlobAngle,
                                       kMaxBlobAngle) &&
          current_circle->InAngleRange(current_centroids[2], kMinBlobAngle,
                                       kMaxBlobAngle)) {
        for (size_t m = 0; m < filtered_blobs.size(); m++) {
          // Add this blob to the list if it is close to the circle, is on the
          // top half,  and isn't one of the other blobs
          if ((m != j) && (m != k) && (m != l) &&
              current_circle->InAngleRange(filtered_stats[m].centroid,
                                           kMinBlobAngle, kMaxBlobAngle) &&
              (current_circle->DistanceTo(filtered_stats[m].centroid) <
               kCircleDistanceThreshold)) {
            current_blobs.emplace_back(filtered_blobs[m]);
            current_centroids.emplace_back(filtered_stats[m].centroid);
          }
        }

        if (current_blobs.size() > blob_circle.size()) {
          blob_circle = current_blobs;
          circle = *current_circle;
          centroids = current_centroids;
        }
      }
    }
  }

  cv::Point avg_centroid(-1, -1);
  if (centroids.size() > 0) {
    for (auto centroid : centroids) {
      avg_centroid.x += centroid.x;
      avg_centroid.y += centroid.y;
    }
    avg_centroid.x /= centroids.size();
    avg_centroid.y /= centroids.size();

    for (auto centroid : centroids) {
      blob_result->filtered_centroids.emplace_back(
          static_cast<int>(centroid.x), static_cast<int>(centroid.y));
    }

    // Sort the filtered centroids to make them go from left to right
    std::sort(blob_result->filtered_centroids.begin(),
              blob_result->filtered_centroids.end(),
              [&circle](cv::Point p, cv::Point q) {
                // If the angle is greater, it is more left and should be
                // considered "less" for sorting
                return circle.AngleOf(p) > circle.AngleOf(q);
              });
  }

  blob_result->filtered_blobs = blob_circle;
  blob_result->centroid = avg_centroid;
}

void BlobDetector::DrawBlobs(const BlobResult &blob_result,
                             cv::Mat view_image) {
  CHECK_GT(view_image.cols, 0);
  if (blob_result.unfiltered_blobs.size() > 0) {
    // Draw blobs unfilled, with red color border
    cv::drawContours(view_image, blob_result.unfiltered_blobs, -1,
                     cv::Scalar(0, 0, 255), 0);
  }

  cv::drawContours(view_image, blob_result.filtered_blobs, -1,
                   cv::Scalar(0, 100, 0), cv::FILLED);

  static constexpr double kCircleRadius = 2.0;
  // Draw blob centroids
  for (auto stats : blob_result.blob_stats) {
    cv::circle(view_image, stats.centroid, kCircleRadius,
               cv::Scalar(0, 215, 255), cv::FILLED);
  }
  for (auto centroid : blob_result.filtered_centroids) {
    cv::circle(view_image, centroid, kCircleRadius, cv::Scalar(0, 255, 0),
               cv::FILLED);
  }

  // Draw average centroid
  cv::circle(view_image, blob_result.centroid, kCircleRadius,
             cv::Scalar(255, 255, 0), cv::FILLED);
}

void BlobDetector::ExtractBlobs(cv::Mat bgr_image,
                                BlobDetector::BlobResult *blob_result) {
  auto start = aos::monotonic_clock::now();
  blob_result->binarized_image = ThresholdImage(bgr_image);
  blob_result->unfiltered_blobs = FindBlobs(blob_result->binarized_image);
  blob_result->blob_stats = ComputeStats(blob_result->unfiltered_blobs);
  FilterBlobs(blob_result);
  auto end = aos::monotonic_clock::now();
  VLOG(2) << "Blob detection elapsed time: "
          << std::chrono::duration<double, std::milli>(end - start).count()
          << " ms";
}

}  // namespace vision
}  // namespace y2022