Estimate target location from blobs
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib9bfe0dc494b1581236d1f3f57bc65c6687981e5
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 737363d..b76ffc5 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -1,6 +1,11 @@
#include "y2022/vision/blob_detector.h"
+#include <cmath>
+#include <string>
+
#include "aos/network/team_number.h"
+#include "opencv2/features2d.hpp"
+#include "opencv2/imgproc.hpp"
DEFINE_uint64(green_delta, 50,
"Required difference between green pixels vs. red and blue");
@@ -20,7 +25,8 @@
uint8_t red = pixel.val[2];
// Simple filter that looks for green pixels sufficiently brigher than
// red and blue
- if ((green > blue + 30) && (green > red + 50)) {
+ if ((green > blue + FLAGS_green_delta) &&
+ (green > red + FLAGS_green_delta)) {
binarized_image.at<uint8_t>(row, col) = 255;
} else {
binarized_image.at<uint8_t>(row, col) = 0;
@@ -69,16 +75,28 @@
std::vector<std::vector<cv::Point>> blobs,
std::vector<BlobDetector::BlobStats> blob_stats) {
std::vector<std::vector<cv::Point>> filtered_blobs;
+
auto blob_it = blobs.begin();
auto stats_it = blob_stats.begin();
while (blob_it < blobs.end() && stats_it < blob_stats.end()) {
+ // To estimate the maximum y, we can figure out the y value of the blobs
+ // when the camera is the farthest from the target, at the field corner.
+ // We can solve for the pitch of the blob:
+ // blob_pitch = atan((height_tape - height_camera) / depth) + camera_pitch
+ // The triangle with the height of the tape above the camera and the camera
+ // depth is similar to the one with the focal length in y pixels and the y
+ // coordinate offset from the center of the image.
+ // Therefore y_offset = focal_length_y * tan(blob_pitch), and
+ // y = -(y_offset - offset_y)
constexpr int kMaxY = 400;
constexpr double kTapeAspectRatio = 5.0 / 2.0;
constexpr double kAspectRatioThreshold = 1.5;
constexpr double kMinArea = 10;
- constexpr size_t kMinPoints = 2;
+ constexpr size_t kMinPoints = 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
+ // TODO(milind): modify to take into account that blobs will be on the side.
if ((stats_it->centroid.y <= kMaxY) &&
(std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
kAspectRatioThreshold) &&
@@ -88,6 +106,7 @@
blob_it++;
stats_it++;
}
+
return filtered_blobs;
}
@@ -99,12 +118,14 @@
CHECK_GT(view_image.cols, 0);
if (unfiltered_blobs.size() > 0) {
// Draw blobs unfilled, with red color border
- drawContours(view_image, unfiltered_blobs, -1, cv::Scalar(0, 0, 255), 0);
+ cv::drawContours(view_image, unfiltered_blobs, -1, cv::Scalar(0, 0, 255),
+ 0);
}
- drawContours(view_image, filtered_blobs, -1, cv::Scalar(0, 255, 0),
- cv::FILLED);
+ cv::drawContours(view_image, filtered_blobs, -1, cv::Scalar(0, 255, 0),
+ cv::FILLED);
+ // Draw blob centroids
for (auto stats : blob_stats) {
cv::circle(view_image, stats.centroid, 2, cv::Scalar(255, 0, 0),
cv::FILLED);