Put all the thresholding code in the same place
This will make it easier to write tests validating the fast version.
Change-Id: I27cc372cc848c495d10a138b3c149d550dc0dad9
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 6dcf6c0..1823ebd 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());