Look for blobs that fit within certain angles
Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I2bc2cf2123e2c0e163944c91b37f0f4d614fce3e
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index a732762..76b16ca 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -17,11 +17,11 @@
namespace y2022 {
namespace vision {
-cv::Mat BlobDetector::ThresholdImage(cv::Mat rgb_image) {
- cv::Mat binarized_image(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
- for (int row = 0; row < rgb_image.rows; row++) {
- for (int col = 0; col < rgb_image.cols; col++) {
- cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(row, col);
+cv::Mat BlobDetector::ThresholdImage(cv::Mat bgr_image) {
+ 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];
@@ -143,15 +143,26 @@
}
double DistanceTo(cv::Point2d p) const {
- // Translate the point so that the circle orgin can be (0, 0)
- const auto p_prime = cv::Point2d(p.y - center.y, p.x - center.x);
+ 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);
}
- // Inverted because y-coordinates go backwards
- bool OnTopHalf(cv::Point2d p) const { return p.y <= center.y; }
+ bool InAngleRange(cv::Point2d p, double theta_min, double theta_max) const {
+ auto p_prime = TranslateToOrigin(p);
+ // Flip the y because y values go downwards.
+ p_prime.y *= -1;
+ const double theta = std::atan2(p_prime.y, p_prime.x);
+ 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
@@ -176,17 +187,17 @@
// y = -(y_offset - offset_y)
constexpr int kMaxY = 400;
constexpr double kTapeAspectRatio = 5.0 / 2.0;
- constexpr double kAspectRatioThreshold = 1.5;
+ constexpr double kAspectRatioThreshold = 1.6;
constexpr double kMinArea = 10;
- constexpr size_t kMinPoints = 6;
+ 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
- // TODO(milind): modify to take into account that blobs will be on the side.
+ // aspect ratio than the tape, or have too little area or points.
if ((stats_it->centroid.y <= kMaxY) &&
(std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
kAspectRatioThreshold) &&
- (stats_it->area >= kMinArea) && (stats_it->num_points >= kMinPoints)) {
+ (stats_it->area >= kMinArea) &&
+ (stats_it->num_points >= kMinNumPoints)) {
filtered_blobs.push_back(*blob_it);
filtered_stats.push_back(*stats_it);
}
@@ -196,6 +207,9 @@
// Threshold for mean distance from a blob centroid to a circle.
constexpr double kCircleDistanceThreshold = 5.0;
+ // We should only expect to see blobs between these angles on a circle.
+ constexpr double kMinBlobAngle = M_PI / 3;
+ constexpr double kMaxBlobAngle = M_PI - kMinBlobAngle;
std::vector<std::vector<cv::Point>> blob_circle;
std::vector<cv::Point2d> centroids;
@@ -230,16 +244,20 @@
continue;
}
- // Only try to fit points to this circle if all of these are on the top
- // half, like how the blobs should be
- if (circle->OnTopHalf(current_centroids[0]) &&
- circle->OnTopHalf(current_centroids[1]) &&
- circle->OnTopHalf(current_centroids[2])) {
+ // Only try to fit points to this circle if all of these are between
+ // certain angles.
+ if (circle->InAngleRange(current_centroids[0], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ circle->InAngleRange(current_centroids[1], kMinBlobAngle,
+ kMaxBlobAngle) &&
+ 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 != i) && (m != j) && (m != k) &&
- circle->OnTopHalf(filtered_stats[m].centroid) &&
+ circle->InAngleRange(filtered_stats[m].centroid, kMinBlobAngle,
+ kMaxBlobAngle) &&
(circle->DistanceTo(filtered_stats[m].centroid) <
kCircleDistanceThreshold)) {
current_blobs.emplace_back(filtered_blobs[m]);
@@ -293,10 +311,10 @@
cv::circle(view_image, centroid, 3, cv::Scalar(255, 255, 0), cv::FILLED);
}
-void BlobDetector::ExtractBlobs(cv::Mat rgb_image,
+void BlobDetector::ExtractBlobs(cv::Mat bgr_image,
BlobDetector::BlobResult *blob_result) {
auto start = aos::monotonic_clock::now();
- blob_result->binarized_image = ThresholdImage(rgb_image);
+ 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);
auto filtered_pair =
diff --git a/y2022/vision/blob_detector.h b/y2022/vision/blob_detector.h
index 4077e2c..d263d32 100644
--- a/y2022/vision/blob_detector.h
+++ b/y2022/vision/blob_detector.h
@@ -28,7 +28,7 @@
// Given an image, threshold it to find "green" pixels
// Input: Color image
// Output: Grayscale (binarized) image with green pixels set to 255
- static cv::Mat ThresholdImage(cv::Mat rgb_image);
+ static cv::Mat ThresholdImage(cv::Mat bgr_image);
// Given binary image, extract blobs
static std::vector<std::vector<cv::Point>> FindBlobs(cv::Mat threshold_image);
@@ -52,7 +52,7 @@
const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
const std::vector<BlobStats> &blob_stats, cv::Point centroid);
- static void ExtractBlobs(cv::Mat rgb_image, BlobResult *blob_result);
+ static void ExtractBlobs(cv::Mat bgr_image, BlobResult *blob_result);
};
} // namespace vision
} // namespace y2022
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index db87cb0..abce18c 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -124,8 +124,8 @@
// that can be sent in a second.
std::this_thread::sleep_for(std::chrono::milliseconds(50));
LOG(INFO) << "Reading file " << file;
- cv::Mat rgb_image = cv::imread(file.c_str());
- ProcessImage(rgb_image);
+ cv::Mat bgr_image = cv::imread(file.c_str());
+ ProcessImage(bgr_image);
}
event_loop_->Exit();
return;
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 6ee6144..c9c2aff 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -76,11 +76,11 @@
// Create color image:
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
- cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
- cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2RGB_YUYV);
if (!FLAGS_capture.empty()) {
- cv::imwrite(FLAGS_capture, rgb_image);
+ cv::imwrite(FLAGS_capture, bgr_image);
return false;
}
@@ -100,15 +100,15 @@
cv::imshow("blobs", ret_image);
}
- cv::imshow("image", rgb_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, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ 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(), rgb_image);
+ cv::imwrite(name.str(), bgr_image);
LOG(INFO) << "Saved image file: " << name.str();
} else if ((keystroke & 0xFF) == static_cast<int>('q')) {
return false;