Merge "Make the vision solver use a less strict problem."
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index 69a2cc5..2e77079 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -37,13 +37,31 @@
cc_library(
name = "threshold",
- hdrs = ["threshold.h"],
+ srcs = [
+ "threshold.cc",
+ ],
+ hdrs = [
+ "threshold.h",
+ ],
deps = [
":range_image",
"//aos/vision/image:image_types",
],
)
+cc_test(
+ name = "threshold_test",
+ srcs = [
+ "threshold_test.cc",
+ ],
+ deps = [
+ ":range_image",
+ ":threshold",
+ "//aos/testing:googletest",
+ "//aos/vision/image:image_types",
+ ],
+)
+
cc_library(
name = "hierarchical_contour_merge",
srcs = ["hierarchical_contour_merge.cc"],
diff --git a/aos/vision/blob/range_image.cc b/aos/vision/blob/range_image.cc
index c01a919..81a8c48 100644
--- a/aos/vision/blob/range_image.cc
+++ b/aos/vision/blob/range_image.cc
@@ -5,18 +5,10 @@
namespace aos {
namespace vision {
+namespace {
-void DrawRangeImage(const RangeImage &rimg, ImagePtr outbuf, PixelRef color) {
- for (int i = 0; i < (int)rimg.ranges().size(); ++i) {
- int y = rimg.min_y() + i;
- for (ImageRange rng : rimg.ranges()[i]) {
- for (int x = rng.st; x < rng.ed; ++x) {
- outbuf.get_px(x, y) = color;
- }
- }
- }
-}
-
+// Merge sort of multiple range images into a single range image.
+// They must not overlap.
RangeImage MergeRangeImage(const BlobList &blobl) {
if (blobl.size() == 1) return blobl[0];
@@ -48,6 +40,8 @@
}
}
+} // namespace
+
std::string ShortDebugPrint(const BlobList &blobl) {
RangeImage rimg = MergeRangeImage(blobl);
std::string out;
@@ -88,6 +82,10 @@
}
}
+void PrintTo(const ImageRange &range, std::ostream *os) {
+ *os << "{" << range.st << ", " << range.ed << "}";
+}
+
void RangeImage::Flip(int image_width, int image_height) {
std::reverse(ranges_.begin(), ranges_.end());
for (std::vector<ImageRange> &range : ranges_) {
@@ -102,6 +100,31 @@
min_y_ = image_height - static_cast<int>(ranges_.size()) - min_y_;
}
+void PrintTo(const RangeImage &range, std::ostream *os) {
+ *os << "{min_y=" << range.min_y()
+ << ", ranges={";
+ bool first_row = true;
+ for (const auto &row : range) {
+ if (first_row) {
+ first_row = false;
+ } else {
+ *os << ", ";
+ }
+ *os << "{";
+ bool first_value = true;
+ for (const auto &value : row) {
+ if (first_value) {
+ first_value = false;
+ } else {
+ *os << ", ";
+ }
+ *os << "{" << value.st << ", " << value.ed << "}";
+ }
+ *os << "}";
+ }
+ *os << "}}";
+}
+
int RangeImage::npixels() {
if (npixelsc_ > 0) {
return npixelsc_;
diff --git a/aos/vision/blob/range_image.h b/aos/vision/blob/range_image.h
index 3647890..a735442 100644
--- a/aos/vision/blob/range_image.h
+++ b/aos/vision/blob/range_image.h
@@ -21,8 +21,14 @@
int calc_width() const { return ed - st; }
bool operator<(const ImageRange &o) const { return st < o.st; }
+ bool operator==(const ImageRange &other) const {
+ return st == other.st && ed == other.ed;
+ }
+ bool operator!=(const ImageRange &other) const { return !(*this == other); }
};
+void PrintTo(const ImageRange &range, std::ostream *os);
+
// Image in pre-thresholded run-length encoded format.
class RangeImage {
public:
@@ -31,9 +37,22 @@
explicit RangeImage(int l) { ranges_.reserve(l); }
RangeImage() {}
+ bool operator==(const RangeImage &other) const {
+ if (min_y_ != other.min_y_) { return false; }
+ if (ranges_ != other.ranges_) { return false; }
+ return true;
+ }
+ bool operator!=(const RangeImage &other) const { return !(*this == other); }
+
int size() const { return ranges_.size(); }
+ // Returns the total number of included pixels.
int npixels();
+ // Calculates the total number of included pixels.
+ //
+ // TODO(Brian): Present a nicer API than the current duality between this and
+ // npixels(), which is annoying because npixels() has to modify the cached
+ // data so it can't be const.
int calc_area() const;
void Flip(ImageFormat fmt) { Flip(fmt.w, fmt.h); }
@@ -59,22 +78,19 @@
// minimum index in y where the blob starts
int min_y_ = 0;
- // ranges are always sorted in y then x order
+ // Each vector<ImageRange> represents all the matched ranges in a given row.
+ // Each ImageRange within that row represents a run of pixels which matches.
std::vector<std::vector<ImageRange>> ranges_;
// Cached pixel count.
int npixelsc_ = -1;
};
+void PrintTo(const RangeImage &range, std::ostream *os);
+
typedef std::vector<RangeImage> BlobList;
typedef std::vector<const RangeImage *> BlobLRef;
-void DrawRangeImage(const RangeImage &rimg, ImagePtr outbuf, PixelRef color);
-
-// Merge sort of multiple range images into a single range image.
-// They must not overlap.
-RangeImage MergeRangeImage(const BlobList &blobl);
-
// Debug print range image as ranges.
std::string ShortDebugPrint(const BlobList &blobl);
// Debug print range image as ### for the ranges.
diff --git a/aos/vision/blob/threshold.cc b/aos/vision/blob/threshold.cc
new file mode 100644
index 0000000..4fc58eb
--- /dev/null
+++ b/aos/vision/blob/threshold.cc
@@ -0,0 +1,213 @@
+#include "aos/vision/blob/threshold.h"
+
+namespace aos {
+namespace vision {
+
+#define MASH(v0, v1, v2, v3, v4) \
+ ((uint8_t(v0) << 4) | (uint8_t(v1) << 3) | (uint8_t(v2) << 2) | \
+ (uint8_t(v3) << 1) | (uint8_t(v4)))
+
+RangeImage FastYuyvYThreshold(ImageFormat fmt, const char *data,
+ uint8_t value) {
+ std::vector<std::vector<ImageRange>> ranges;
+ ranges.reserve(fmt.h);
+ for (int y = 0; y < fmt.h; ++y) {
+ const char *row = fmt.w * y * 2 + data;
+ bool p_score = false;
+ int pstart = -1;
+ std::vector<ImageRange> rngs;
+ for (int x = 0; x < fmt.w / 4; ++x) {
+ uint8_t v[8];
+ memcpy(&v[0], row + x * 4 * 2, 8);
+ uint8_t pattern =
+ MASH(p_score, v[0] > value, v[2] > value, v[4] > value, v[6] > value);
+ switch (pattern) {
+ /*
+# Ruby code to generate the below code:
+32.times do |v|
+ puts "case MASH(#{[v[4], v[3], v[2], v[1], v[0]].join(", ")}):"
+ p_score = v[4]
+ pstart = "pstart"
+ 4.times do |i|
+ if v[3 - i] != p_score
+ if (p_score == 1)
+ puts " rngs.emplace_back(ImageRange(#{pstart},
+x * 4 + #{i}));"
+ else
+ pstart = "x * 4 + #{i}"
+ end
+ p_score = v[3 - i]
+ end
+ end
+ if (pstart != "pstart")
+ puts " pstart = #{pstart};"
+ end
+ if (p_score != v[4])
+ puts " p_score = #{["false", "true"][v[0]]};"
+ end
+ puts " break;"
+end
+*/
+ case MASH(0, 0, 0, 0, 0):
+ break;
+ case MASH(0, 0, 0, 0, 1):
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 0, 0, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(0, 0, 0, 1, 1):
+ pstart = x * 4 + 2;
+ p_score = true;
+ break;
+ case MASH(0, 0, 1, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(0, 0, 1, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 0, 1, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(0, 0, 1, 1, 1):
+ pstart = x * 4 + 1;
+ p_score = true;
+ break;
+ case MASH(0, 1, 0, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 0, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 1, 0, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(0, 1, 0, 1, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
+ pstart = x * 4 + 2;
+ p_score = true;
+ break;
+ case MASH(0, 1, 1, 0, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 1, 0, 1):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
+ pstart = x * 4 + 3;
+ p_score = true;
+ break;
+ case MASH(0, 1, 1, 1, 0):
+ rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 3));
+ pstart = x * 4 + 0;
+ break;
+ case MASH(0, 1, 1, 1, 1):
+ pstart = x * 4 + 0;
+ p_score = true;
+ break;
+ case MASH(1, 0, 0, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ p_score = false;
+ break;
+ case MASH(1, 0, 0, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 0, 0, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ p_score = false;
+ break;
+ case MASH(1, 0, 0, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(1, 0, 1, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 1;
+ p_score = false;
+ break;
+ case MASH(1, 0, 1, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 0, 1, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
+ pstart = x * 4 + 1;
+ p_score = false;
+ break;
+ case MASH(1, 0, 1, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
+ pstart = x * 4 + 1;
+ break;
+ case MASH(1, 1, 0, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ p_score = false;
+ break;
+ case MASH(1, 1, 0, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 1, 0, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
+ pstart = x * 4 + 2;
+ p_score = false;
+ break;
+ case MASH(1, 1, 0, 1, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
+ pstart = x * 4 + 2;
+ break;
+ case MASH(1, 1, 1, 0, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
+ p_score = false;
+ break;
+ case MASH(1, 1, 1, 0, 1):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
+ pstart = x * 4 + 3;
+ break;
+ case MASH(1, 1, 1, 1, 0):
+ rngs.emplace_back(ImageRange(pstart, x * 4 + 3));
+ p_score = false;
+ break;
+ case MASH(1, 1, 1, 1, 1):
+ break;
+ }
+
+ for (int i = 0; i < 4; ++i) {
+ if ((v[i * 2] > value) != p_score) {
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, x * 4 + i));
+ } else {
+ pstart = x * 4 + i;
+ }
+ p_score = !p_score;
+ }
+ }
+ }
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, fmt.w));
+ }
+ ranges.push_back(rngs);
+ }
+ return RangeImage(0, std::move(ranges));
+}
+
+#undef MASH
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/threshold.h b/aos/vision/blob/threshold.h
index eef5b20..441a058 100644
--- a/aos/vision/blob/threshold.h
+++ b/aos/vision/blob/threshold.h
@@ -1,15 +1,22 @@
-#ifndef _AOS_VIISON_BLOB_THRESHOLD_H_
-#define _AOS_VIISON_BLOB_THRESHOLD_H_
+#ifndef AOS_VISION_BLOB_THRESHOLD_H_
+#define AOS_VISION_BLOB_THRESHOLD_H_
#include "aos/vision/blob/range_image.h"
#include "aos/vision/image/image_types.h"
namespace aos {
namespace vision {
+namespace threshold_internal {
-// ThresholdFn should be a lambda.
-template <typename ThresholdFn>
-RangeImage DoThreshold(ImageFormat fmt, ThresholdFn &&fn) {
+// Performs thresholding in a given region using a function which determines
+// whether a given point is in or out of the region.
+//
+// fn must return a bool when called with two integers (x, y).
+template <typename PointTestFn>
+RangeImage ThresholdPointsWithFunction(ImageFormat fmt, PointTestFn &&fn) {
+ static_assert(
+ std::is_convertible<PointTestFn, std::function<bool(int, int)>>::value,
+ "Invalid threshold function");
std::vector<std::vector<ImageRange>> ranges;
ranges.reserve(fmt.h);
for (int y = 0; y < fmt.h; ++y) {
@@ -34,23 +41,43 @@
return RangeImage(0, std::move(ranges));
}
-// ThresholdFn should be a lambda.
+} // namespace threshold_internal
+
+// Thresholds an image using a function which determines whether a given pixel
+// value is in or out of the region.
+//
+// fn must return a bool when called with a PixelRef.
template <typename ThresholdFn>
-RangeImage DoThreshold(const ImagePtr &img, ThresholdFn &&fn) {
- return DoThreshold(img.fmt(),
- [&](int x, int y) { return fn(img.get_px(x, y)); });
+RangeImage ThresholdImageWithFunction(const ImagePtr &img, ThresholdFn &&fn) {
+ static_assert(
+ std::is_convertible<ThresholdFn, std::function<bool(PixelRef)>>::value,
+ "Invalid threshold function");
+ return threshold_internal::ThresholdPointsWithFunction(
+ img.fmt(), [&](int x, int y) { return fn(img.get_px(x, y)); });
}
-// YUYV image types:
-inline RangeImage DoThresholdYUYV(ImageFormat fmt, const char *data,
- uint8_t value) {
- return DoThreshold(fmt, [&](int x, int y) {
- uint8_t v = data[y * fmt.w * 2 + x * 2];
- return v > value;
- });
+// Thresholds an image in YUYV format, selecting pixels with a Y (luma) greater
+// than value.
+//
+// This is implemented via a simple function that pulls out the Y values and
+// compares them each. It mostly exists for tests to compare against
+// FastYuyvYThreshold, because it's obviously correct.
+inline RangeImage SlowYuyvYThreshold(ImageFormat fmt, const char *data,
+ uint8_t value) {
+ return threshold_internal::ThresholdPointsWithFunction(
+ fmt, [&](int x, int y) {
+ uint8_t v = data[x * 2 + y * fmt.w * 2];
+ return v > value;
+ });
}
+// Thresholds an image in YUYV format, selecting pixels with a Y (luma) greater
+// than value.
+//
+// This is implemented via some tricky bit shuffling that goes fast.
+RangeImage FastYuyvYThreshold(ImageFormat fmt, const char *data, uint8_t value);
+
} // namespace vision
} // namespace aos
-#endif // _AOS_VIISON_BLOB_THRESHOLD_H_
+#endif // AOS_VISION_BLOB_THRESHOLD_H_
diff --git a/aos/vision/blob/threshold_test.cc b/aos/vision/blob/threshold_test.cc
new file mode 100644
index 0000000..96a2a22
--- /dev/null
+++ b/aos/vision/blob/threshold_test.cc
@@ -0,0 +1,100 @@
+#include "aos/vision/blob/threshold.h"
+
+#include <vector>
+
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/image/image_types.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace vision {
+namespace testing {
+
+class YuyvYThresholdTest : public ::testing::Test {
+};
+
+// Verifies that a simple image is thresholded correctly.
+//
+// Specifically, we want to get this result from the thresholding:
+// --+--
+// +---+
+// -+++-
+// +++-+
+// -----
+// ++-++
+// +++++
+// +-+-+
+TEST_F(YuyvYThresholdTest, SimpleImage) {
+ ImageFormat format;
+ format.w = 5;
+ format.h = 8;
+
+ std::vector<std::vector<ImageRange>> expected_ranges;
+ std::vector<char> image;
+ image.resize(5 * 8 * 2);
+ // --+--
+ image[0 * 2 + 0 * 10] = 0;
+ image[1 * 2 + 0 * 10] = 0;
+ image[2 * 2 + 0 * 10] = 128;
+ image[3 * 2 + 0 * 10] = 127;
+ image[4 * 2 + 0 * 10] = 0;
+ expected_ranges.push_back({{{2, 3}}});
+ // +---+
+ image[0 * 2 + 1 * 10] = 128;
+ image[1 * 2 + 1 * 10] = 0;
+ image[2 * 2 + 1 * 10] = 0;
+ image[3 * 2 + 1 * 10] = 10;
+ image[4 * 2 + 1 * 10] = 255;
+ expected_ranges.push_back({{{0, 1}, {4, 5}}});
+ // -+++-
+ image[0 * 2 + 2 * 10] = 73;
+ image[1 * 2 + 2 * 10] = 250;
+ image[2 * 2 + 2 * 10] = 251;
+ image[3 * 2 + 2 * 10] = 252;
+ image[4 * 2 + 2 * 10] = 45;
+ expected_ranges.push_back({{{1, 4}}});
+ // +++-+
+ image[0 * 2 + 3 * 10] = 128;
+ image[1 * 2 + 3 * 10] = 134;
+ image[2 * 2 + 3 * 10] = 250;
+ image[3 * 2 + 3 * 10] = 0;
+ image[4 * 2 + 3 * 10] = 230;
+ expected_ranges.push_back({{{0, 3}, {4, 5}}});
+ // -----
+ image[0 * 2 + 4 * 10] = 7;
+ image[1 * 2 + 4 * 10] = 120;
+ image[2 * 2 + 4 * 10] = 127;
+ image[3 * 2 + 4 * 10] = 0;
+ image[4 * 2 + 4 * 10] = 50;
+ expected_ranges.push_back({{}});
+ // ++-++
+ image[0 * 2 + 5 * 10] = 140;
+ image[1 * 2 + 5 * 10] = 140;
+ image[2 * 2 + 5 * 10] = 0;
+ image[3 * 2 + 5 * 10] = 140;
+ image[4 * 2 + 5 * 10] = 140;
+ expected_ranges.push_back({{{0, 2}, {3, 5}}});
+ // +++++
+ image[0 * 2 + 6 * 10] = 128;
+ image[1 * 2 + 6 * 10] = 128;
+ image[2 * 2 + 6 * 10] = 128;
+ image[3 * 2 + 6 * 10] = 128;
+ image[4 * 2 + 6 * 10] = 128;
+ expected_ranges.push_back({{{0, 5}}});
+ // +-+-+
+ image[0 * 2 + 7 * 10] = 200;
+ image[1 * 2 + 7 * 10] = 0;
+ image[2 * 2 + 7 * 10] = 200;
+ image[3 * 2 + 7 * 10] = 0;
+ image[4 * 2 + 7 * 10] = 200;
+ expected_ranges.push_back({{{0, 1}, {2, 3}, {4, 5}}});
+ const RangeImage expected_result(0, std::move(expected_ranges));
+
+ const auto slow_result = SlowYuyvYThreshold(format, image.data(), 127);
+ ASSERT_EQ(expected_result, slow_result);
+}
+
+} // namespace testing
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
index 072b57b..806fd80 100644
--- a/aos/vision/tools/jpeg_vision_test.cc
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -74,7 +74,7 @@
prev_data_ = data.to_string();
// Threshold the image with the given lambda.
- RangeImage rimg = DoThreshold(img_ptr, [](PixelRef &px) {
+ RangeImage rimg = ThresholdImageWithFunction(img_ptr, [](PixelRef &px) {
if (px.g > 88) {
uint8_t min = std::min(px.b, px.r);
uint8_t max = std::max(px.b, px.r);
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
index d9208ec..3e29085 100644
--- a/y2016/vision/target_sender.cc
+++ b/y2016/vision/target_sender.cc
@@ -98,8 +98,8 @@
DecodeJpeg(data, &image_);
auto fmt = image_.fmt();
- RangeImage rimg =
- DoThreshold(image_.get(), [](PixelRef &px) { return (px.g > 88); });
+ RangeImage rimg = ThresholdImageWithFunction(
+ image_.get(), [](PixelRef px) { return (px.g > 88); });
// flip the right image as this camera is mount backward
if (camera_index_ == 0) {
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
index a6b049c..91e801e 100644
--- a/y2017/vision/target_finder.cc
+++ b/y2017/vision/target_finder.cc
@@ -103,17 +103,18 @@
}
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
- return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
- if (px.g > 88) {
- uint8_t min = std::min(px.b, px.r);
- uint8_t max = std::max(px.b, px.r);
- if (min >= px.g || max >= px.g) return false;
- uint8_t a = px.g - min;
- uint8_t b = px.g - max;
- return (a > 10 && b > 10);
- }
- return false;
- });
+ return aos::vision::ThresholdImageWithFunction(
+ image, [&](aos::vision::PixelRef px) {
+ if (px.g > 88) {
+ uint8_t min = std::min(px.b, px.r);
+ uint8_t max = std::max(px.b, px.r);
+ if (min >= px.g || max >= px.g) return false;
+ uint8_t a = px.g - min;
+ uint8_t b = px.g - max;
+ return (a > 10 && b > 10);
+ }
+ return false;
+ });
}
void TargetFinder::PreFilter(BlobList &imgs) {
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index b08d6b6..c4a1caf 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -2,6 +2,7 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
+#include <chrono>
#include "aos/actions/actions.h"
#include "aos/init.h"
@@ -316,7 +317,11 @@
LOG(ERROR, "Sending superstructure goal failed.\n");
}
- video_tx_->Send(vision_control_);
+ auto time_now = ::aos::monotonic_clock::now();
+ if (time_now > last_vision_control_ + ::std::chrono::milliseconds(50)) {
+ video_tx_->Send(vision_control_);
+ last_vision_control_ = time_now;
+ }
}
private:
@@ -329,6 +334,8 @@
VisionControl vision_control_;
::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
+ ::aos::monotonic_clock::time_point last_vision_control_ =
+ ::aos::monotonic_clock::time_point::min();
};
} // namespace joysticks
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index 3d1e91f..304f4cb 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -153,7 +153,7 @@
const ::aos::vision::ImageFormat fmt{640, 480};
::aos::vision::BlobList imgs =
- ::aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
+ ::aos::vision::FindBlobs(aos::vision::SlowYuyvYThreshold(
fmt, frame.data.data(), TargetFinder::GetThresholdValue()));
target_finder.PreFilter(&imgs);
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 0f25765..fb2a134 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -11,13 +11,14 @@
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
const uint8_t threshold_value = GetThresholdValue();
- return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
- if (px.g > threshold_value && px.b > threshold_value &&
- px.r > threshold_value) {
- return true;
- }
- return false;
- });
+ return aos::vision::ThresholdImageWithFunction(
+ image, [&](aos::vision::PixelRef px) {
+ if (px.g > threshold_value && px.b > threshold_value &&
+ px.r > threshold_value) {
+ return true;
+ }
+ return false;
+ });
}
// Filter blobs on size.
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 472baf3..3a6fbfe 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -64,213 +64,6 @@
using aos::vision::RangeImage;
using aos::vision::ImageFormat;
-#define MASH(v0, v1, v2, v3, v4) \
- ((uint8_t(v0) << 4) | (uint8_t(v1) << 3) | (uint8_t(v2) << 2) | \
- (uint8_t(v3) << 1) | (uint8_t(v4)))
-
-// YUYV image types:
-inline RangeImage DoThresholdYUYV(ImageFormat fmt, const char *data,
- uint8_t value) {
- std::vector<std::vector<ImageRange>> ranges;
- ranges.reserve(fmt.h);
- for (int y = 0; y < fmt.h; ++y) {
- const char *row = fmt.w * y * 2 + data;
- bool p_score = false;
- int pstart = -1;
- std::vector<ImageRange> rngs;
- for (int x = 0; x < fmt.w / 4; ++x) {
- uint8_t v[8];
- memcpy(&v[0], row + x * 4 * 2, 8);
- uint8_t pattern =
- MASH(p_score, v[0] > value, v[2] > value, v[4] > value, v[6] > value);
- switch (pattern) {
- /*
-# Ruby code to generate the below code:
-32.times do |v|
- puts "case MASH(#{[v[4], v[3], v[2], v[1], v[0]].join(", ")}):"
- p_score = v[4]
- pstart = "pstart"
- 4.times do |i|
- if v[3 - i] != p_score
- if (p_score == 1)
- puts " rngs.emplace_back(ImageRange(#{pstart},
-x * 4 + #{i}));"
- else
- pstart = "x * 4 + #{i}"
- end
- p_score = v[3 - i]
- end
- end
- if (pstart != "pstart")
- puts " pstart = #{pstart};"
- end
- if (p_score != v[4])
- puts " p_score = #{["false", "true"][v[0]]};"
- end
- puts " break;"
-end
-*/
- case MASH(0, 0, 0, 0, 0):
- break;
- case MASH(0, 0, 0, 0, 1):
- pstart = x * 4 + 3;
- p_score = true;
- break;
- case MASH(0, 0, 0, 1, 0):
- rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
- pstart = x * 4 + 2;
- break;
- case MASH(0, 0, 0, 1, 1):
- pstart = x * 4 + 2;
- p_score = true;
- break;
- case MASH(0, 0, 1, 0, 0):
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
- pstart = x * 4 + 1;
- break;
- case MASH(0, 0, 1, 0, 1):
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
- pstart = x * 4 + 3;
- p_score = true;
- break;
- case MASH(0, 0, 1, 1, 0):
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
- pstart = x * 4 + 1;
- break;
- case MASH(0, 0, 1, 1, 1):
- pstart = x * 4 + 1;
- p_score = true;
- break;
- case MASH(0, 1, 0, 0, 0):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
- pstart = x * 4 + 0;
- break;
- case MASH(0, 1, 0, 0, 1):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
- pstart = x * 4 + 3;
- p_score = true;
- break;
- case MASH(0, 1, 0, 1, 0):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
- rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
- pstart = x * 4 + 2;
- break;
- case MASH(0, 1, 0, 1, 1):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 1));
- pstart = x * 4 + 2;
- p_score = true;
- break;
- case MASH(0, 1, 1, 0, 0):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
- pstart = x * 4 + 0;
- break;
- case MASH(0, 1, 1, 0, 1):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 2));
- pstart = x * 4 + 3;
- p_score = true;
- break;
- case MASH(0, 1, 1, 1, 0):
- rngs.emplace_back(ImageRange(x * 4 + 0, x * 4 + 3));
- pstart = x * 4 + 0;
- break;
- case MASH(0, 1, 1, 1, 1):
- pstart = x * 4 + 0;
- p_score = true;
- break;
- case MASH(1, 0, 0, 0, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- p_score = false;
- break;
- case MASH(1, 0, 0, 0, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- pstart = x * 4 + 3;
- break;
- case MASH(1, 0, 0, 1, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
- pstart = x * 4 + 2;
- p_score = false;
- break;
- case MASH(1, 0, 0, 1, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- pstart = x * 4 + 2;
- break;
- case MASH(1, 0, 1, 0, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
- pstart = x * 4 + 1;
- p_score = false;
- break;
- case MASH(1, 0, 1, 0, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 2));
- pstart = x * 4 + 3;
- break;
- case MASH(1, 0, 1, 1, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- rngs.emplace_back(ImageRange(x * 4 + 1, x * 4 + 3));
- pstart = x * 4 + 1;
- p_score = false;
- break;
- case MASH(1, 0, 1, 1, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 0));
- pstart = x * 4 + 1;
- break;
- case MASH(1, 1, 0, 0, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
- p_score = false;
- break;
- case MASH(1, 1, 0, 0, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
- pstart = x * 4 + 3;
- break;
- case MASH(1, 1, 0, 1, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
- rngs.emplace_back(ImageRange(x * 4 + 2, x * 4 + 3));
- pstart = x * 4 + 2;
- p_score = false;
- break;
- case MASH(1, 1, 0, 1, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 1));
- pstart = x * 4 + 2;
- break;
- case MASH(1, 1, 1, 0, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
- p_score = false;
- break;
- case MASH(1, 1, 1, 0, 1):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 2));
- pstart = x * 4 + 3;
- break;
- case MASH(1, 1, 1, 1, 0):
- rngs.emplace_back(ImageRange(pstart, x * 4 + 3));
- p_score = false;
- break;
- case MASH(1, 1, 1, 1, 1):
- break;
- }
-
- for (int i = 0; i < 4; ++i) {
- if ((v[i * 2] > value) != p_score) {
- if (p_score) {
- rngs.emplace_back(ImageRange(pstart, x * 4 + i));
- } else {
- pstart = x * 4 + i;
- }
- p_score = !p_score;
- }
- }
- }
- if (p_score) {
- rngs.emplace_back(ImageRange(pstart, fmt.w));
- }
- ranges.push_back(rngs);
- }
- return RangeImage(0, std::move(ranges));
-}
-
-#undef MASH
-
int main(int argc, char **argv) {
(void)argc;
(void)argv;
@@ -301,9 +94,8 @@
camera0->set_on_frame([&](DataRef data,
monotonic_clock::time_point monotonic_now) {
aos::vision::ImageFormat fmt{640, 480};
- // Use threshold from aos::vision. This will run at 15 FPS.
- aos::vision::BlobList imgs =
- aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::SlowYuyvYThreshold(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
LOG(INFO, "Blobs: (%zu).\n", imgs.size());