Merge "Decrease rate of vision control messages"
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..613d2ca 100644
--- a/aos/vision/blob/range_image.cc
+++ b/aos/vision/blob/range_image.cc
@@ -88,6 +88,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 +106,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..201ffb2 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,6 +37,13 @@
   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(); }
 
   int npixels();
@@ -59,13 +72,16 @@
   // 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;
 
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/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index b282367..14063c2 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -96,23 +96,23 @@
     target_finder_.PreFilter(&imgs);
 
     // Find polygons from blobs.
-    std::vector<std::vector<Segment<2>>> raw_polys;
+    std::vector<Polygon> raw_polys;
     for (const RangeImage &blob : imgs) {
       // Convert blobs to contours in the corrected space.
       ContourNode *contour = target_finder_.GetContour(blob);
       if (draw_contours_) {
         DrawContour(contour, {255, 0, 0});
       }
-      const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+      ::std::vector<::Eigen::Vector2f> unwarped_contour =
           target_finder_.UnWarpContour(contour);
       if (draw_contours_) {
         DrawContour(unwarped_contour, {0, 0, 255});
       }
 
       // Process to polygons.
-      std::vector<Segment<2>> polygon =
-          target_finder_.FillPolygon(unwarped_contour, draw_raw_poly_);
-      if (polygon.empty()) {
+      const Polygon polygon = target_finder_.FindPolygon(
+          ::std::move(unwarped_contour), draw_raw_poly_);
+      if (polygon.segments.empty()) {
         if (!draw_contours_) {
           DrawBlob(blob, {255, 0, 0});
         }
@@ -122,15 +122,16 @@
           DrawBlob(blob, {0, 0, 255});
         }
         if (draw_raw_poly_) {
-          std::vector<PixelRef> colors = GetNColors(polygon.size());
+          std::vector<PixelRef> colors = GetNColors(polygon.segments.size());
           std::vector<Vector<2>> corners;
-          for (size_t i = 0; i < polygon.size(); ++i) {
-            corners.push_back(
-                polygon[i].Intersect(polygon[(i + 1) % polygon.size()]));
+          for (size_t i = 0; i < polygon.segments.size(); ++i) {
+            corners.push_back(polygon.segments[i].Intersect(
+                polygon.segments[(i + 1) % polygon.segments.size()]));
           }
 
-          for (size_t i = 0; i < polygon.size(); ++i) {
-            overlay_.AddLine(corners[i], corners[(i + 1) % polygon.size()],
+          for (size_t i = 0; i < polygon.segments.size(); ++i) {
+            overlay_.AddLine(corners[i],
+                             corners[(i + 1) % polygon.segments.size()],
                              colors[i]);
           }
         }
@@ -139,7 +140,7 @@
 
     // Calculate each component side of a possible target.
     std::vector<TargetComponent> target_component_list =
-        target_finder_.FillTargetComponentList(raw_polys);
+        target_finder_.FillTargetComponentList(raw_polys, draw_components_);
     if (draw_components_) {
       for (const TargetComponent &comp : target_component_list) {
         DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index c9646fc..304f4cb 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -153,27 +153,27 @@
 
     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);
 
     constexpr bool verbose = false;
-    ::std::vector<std::vector<Segment<2>>> raw_polys;
+    ::std::vector<Polygon> raw_polys;
     for (const RangeImage &blob : imgs) {
       // Convert blobs to contours in the corrected space.
       ContourNode *contour = target_finder.GetContour(blob);
-      const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+      ::std::vector<::Eigen::Vector2f> unwarped_contour =
           target_finder.UnWarpContour(contour);
-      const ::std::vector<Segment<2>> polygon =
-          target_finder.FillPolygon(unwarped_contour, verbose);
-      if (!polygon.empty()) {
+      const Polygon polygon =
+          target_finder.FindPolygon(::std::move(unwarped_contour), verbose);
+      if (!polygon.segments.empty()) {
         raw_polys.push_back(polygon);
       }
     }
 
     // Calculate each component side of a possible target.
     const ::std::vector<TargetComponent> target_component_list =
-        target_finder.FillTargetComponentList(raw_polys);
+        target_finder.FillTargetComponentList(raw_polys, verbose);
 
     // Put the compenents together into targets.
     const ::std::vector<Target> target_list =
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 1d991ff..82611f3 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.
@@ -90,8 +91,8 @@
 
 // TODO: Try hierarchical merge for this.
 // Convert blobs into polygons.
-std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
-    const ::std::vector<::Eigen::Vector2f> &contour, bool verbose) {
+Polygon TargetFinder::FindPolygon(::std::vector<::Eigen::Vector2f> &&contour,
+                                  bool verbose) {
   if (verbose) printf("Process Polygon.\n");
 
   ::std::vector<::Eigen::Vector2f> slopes;
@@ -111,12 +112,17 @@
     return slopes[(i + num_points * 2) % num_points];
   };
 
+  // Bigger objects should be more filtered.  Filter roughly proportional to the
+  // perimeter of the object.
+  const int range = slopes.size() / 50;
+  if (verbose) printf("Corner range: %d.\n", range);
+
   ::std::vector<::Eigen::Vector2f> filtered_slopes = slopes;
   // Three box filter makith a guassian?
   // Run gaussian filter over the slopes 3 times.  That'll get us pretty close
   // to running a gausian over it.
   for (int k = 0; k < 3; ++k) {
-    const int window_size = 2;
+    const int window_size = ::std::max(2, range);
     for (size_t i = 0; i < slopes.size(); ++i) {
       ::Eigen::Vector2f a = ::Eigen::Vector2f::Zero();
       for (int j = -window_size; j <= window_size; ++j) {
@@ -125,55 +131,127 @@
       }
       a /= (window_size * 2 + 1);
 
-      const float scale = 1.0 + (i / float(slopes.size() * 10));
-      a *= scale;
       filtered_slopes[i] = a;
     }
     slopes = filtered_slopes;
   }
+  if (verbose) printf("Point count: %zu.\n", slopes.size());
 
-  // Heuristic which says if a particular slope is part of a corner.
-  auto is_corner = [&](size_t i) {
-    const ::Eigen::Vector2f a = get_pt(i - 3);
-    const ::Eigen::Vector2f b = get_pt(i + 3);
-    const double dx = (a.x() - b.x());
-    const double dy = (a.y() - b.y());
-    return dx * dx + dy * dy > 0.25;
-  };
+  ::std::vector<float> corner_metric(slopes.size(), 0.0);
 
-  bool prev_v = is_corner(-1);
+  for (size_t i = 0; i < slopes.size(); ++i) {
+    const ::Eigen::Vector2f a = get_pt(i - ::std::max(3, range));
+    const ::Eigen::Vector2f b = get_pt(i + ::std::max(3, range));
+    corner_metric[i] = (a - b).squaredNorm();
+  }
+
+  // We want to find the Nth highest peaks.
+  // Clever algorithm: Find the highest point.  Then, walk forwards and
+  // backwards to find the next valley each direction which is over x% lower
+  // than the peak.
+  // We want to ignore those points in the future.  Set them to 0.
+  // Repeat until we've found the Nth highest peak.
 
   // Find all centers of corners.
   // Because they round, multiple slopes may be a corner.
   ::std::vector<size_t> edges;
-  const size_t kBad = slopes.size() + 10;
-  size_t prev_up = kBad;
-  size_t wrapped_n = prev_up;
 
-  for (size_t i = 0; i < slopes.size(); ++i) {
-    bool v = is_corner(i);
-    if (prev_v && !v) {
-      if (prev_up == kBad) {
-        wrapped_n = i;
-      } else {
-        edges.push_back((prev_up + i - 1) / 2);
+  constexpr float peak_acceptance_ratio = 0.16;
+  constexpr float valley_ratio = 0.75;
+
+  float highest_peak_value = 0.0;
+
+  // Nth higest points.
+  while (edges.size() < 5) {
+    const ::std::vector<float>::iterator max_element =
+        ::std::max_element(corner_metric.begin(), corner_metric.end());
+    const size_t highest_index =
+        ::std::distance(corner_metric.begin(), max_element);
+    const float max_value = *max_element;
+    if (edges.size() == 0) {
+      highest_peak_value = max_value;
+    }
+    if (max_value < highest_peak_value * peak_acceptance_ratio &&
+        edges.size() == 4) {
+      if (verbose)
+        printf("Rejecting index: %zu, %f (%f %%)\n", highest_index, max_value,
+               max_value / highest_peak_value);
+      break;
+    }
+    const float valley_value = max_value * valley_ratio;
+
+    if (verbose)
+      printf("Highest index: %zu, %f (%f %%)\n", highest_index, max_value,
+             max_value / highest_peak_value);
+
+    bool foothill = false;
+    {
+      float min_value = max_value;
+      size_t fwd_index = (highest_index + 1) % corner_metric.size();
+      while (true) {
+        const float current_value = corner_metric[fwd_index];
+
+        if (current_value == -1.0) {
+          if (min_value >= valley_value) {
+            if (verbose) printf("Foothill\n");
+            foothill = true;
+          }
+          break;
+        }
+
+        min_value = ::std::min(current_value, min_value);
+
+        if (min_value < valley_value && current_value > min_value) {
+          break;
+        }
+        // Kill!!!
+        corner_metric[fwd_index] = -1.0;
+
+        fwd_index = (fwd_index + 1) % corner_metric.size();
       }
     }
-    if (v && !prev_v) {
-      prev_up = i;
+
+    {
+      float min_value = max_value;
+      size_t rev_index =
+          (highest_index - 1 + corner_metric.size()) % corner_metric.size();
+      while (true) {
+        const float current_value = corner_metric[rev_index];
+
+        if (current_value == -1.0) {
+          if (min_value >= valley_value) {
+            if (verbose) printf("Foothill\n");
+            foothill = true;
+          }
+          break;
+        }
+
+        min_value = ::std::min(current_value, min_value);
+
+        if (min_value < valley_value && current_value > min_value) {
+          break;
+        }
+        // Kill!!!
+        corner_metric[rev_index] = -1.0;
+
+        rev_index =
+            (rev_index - 1 + corner_metric.size()) % corner_metric.size();
+      }
     }
-    prev_v = v;
+
+    *max_element = -1.0;
+    if (!foothill) {
+      edges.push_back(highest_index);
+    }
   }
 
-  if (wrapped_n != kBad) {
-    edges.push_back(((prev_up + slopes.size() + wrapped_n - 1) / 2) % slopes.size());
-  }
+  ::std::sort(edges.begin(), edges.end());
 
   if (verbose) printf("Edge Count (%zu).\n", edges.size());
 
   // Run best-fits over each line segment.
-  ::std::vector<Segment<2>> seg_list;
-  if (edges.size() == 4) {
+  Polygon polygon;
+  if (edges.size() >= 3) {
     for (size_t i = 0; i < edges.size(); ++i) {
       // Include the corners in both line fits.
       const size_t segment_start_index = edges[i];
@@ -219,7 +297,7 @@
         x /= norm;
         y /= norm;
 
-        seg_list.push_back(
+        polygon.segments.push_back(
             Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
       }
 
@@ -235,23 +313,24 @@
       */
     }
   }
-  if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
-  return seg_list;
+  if (verbose) printf("Poly Count (%zu).\n", polygon.segments.size());
+  polygon.contour = ::std::move(contour);
+  return polygon;
 }
 
 // Convert segments into target components (left or right)
-std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
-    const std::vector<std::vector<Segment<2>>> &seg_list) {
-  std::vector<TargetComponent> list;
+::std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+    const ::std::vector<Polygon> &seg_list, bool verbose) {
+  ::std::vector<TargetComponent> list;
   TargetComponent new_target;
-  for (const std::vector<Segment<2>> &poly : seg_list) {
+  for (const Polygon &poly : seg_list) {
     // Reject missized pollygons for now. Maybe rectify them here in the future;
-    if (poly.size() != 4) {
+    if (poly.segments.size() != 4) {
       continue;
     }
-    std::vector<Vector<2>> corners;
+    ::std::vector<Vector<2>> corners;
     for (size_t i = 0; i < 4; ++i) {
-      Vector<2> corner = poly[i].Intersect(poly[(i + 1) % 4]);
+      Vector<2> corner = poly.segments[i].Intersect(poly.segments[(i + 1) % 4]);
       if (::std::isnan(corner.x()) || ::std::isnan(corner.y())) {
         break;
       }
@@ -263,7 +342,7 @@
 
     // Select the closest two points. Short side of the rectangle.
     double min_dist = -1;
-    std::pair<size_t, size_t> closest;
+    ::std::pair<size_t, size_t> closest;
     for (size_t i = 0; i < 4; ++i) {
       size_t next = (i + 1) % 4;
       double nd = corners[i].SquaredDistanceTo(corners[next]);
@@ -351,6 +430,7 @@
 
     // This piece of the target should be ready now.
     list.emplace_back(new_target);
+    if (verbose) printf("Happy with a target\n");
   }
 
   return list;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index fcde358..ebae3b4 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -18,6 +18,11 @@
 using aos::vision::Vector;
 using aos::vision::ContourNode;
 
+struct Polygon {
+  ::std::vector<aos::vision::Segment<2>> segments;
+  ::std::vector<::Eigen::Vector2f> contour;
+};
+
 class TargetFinder {
  public:
   TargetFinder();
@@ -34,12 +39,11 @@
   ::std::vector<::Eigen::Vector2f> UnWarpContour(ContourNode *start) const;
 
   // Turn a blob into a polgygon.
-  std::vector<aos::vision::Segment<2>> FillPolygon(
-      const ::std::vector<::Eigen::Vector2f> &contour, bool verbose);
+  Polygon FindPolygon(::std::vector<::Eigen::Vector2f> &&contour, bool verbose);
 
   // Turn a bloblist into components of a target.
   std::vector<TargetComponent> FillTargetComponentList(
-      const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+      const ::std::vector<Polygon> &seg_list, bool verbose);
 
   // Piece the compenents together into a target.
   std::vector<Target> FindTargetsFromComponents(
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 80a47e0..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,39 +94,38 @@
   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());
 
-    bool verbose = false;
-    std::vector<std::vector<Segment<2>>> raw_polys;
+    constexpr bool verbose = false;
+    ::std::vector<Polygon> raw_polys;
     for (const RangeImage &blob : imgs) {
       // Convert blobs to contours in the corrected space.
       ContourNode* contour = finder_.GetContour(blob);
-      const ::std::vector<::Eigen::Vector2f> unwarped_contour =
+      ::std::vector<::Eigen::Vector2f> unwarped_contour =
           finder_.UnWarpContour(contour);
-      ::std::vector<Segment<2>> polygon =
-          finder_.FillPolygon(unwarped_contour, verbose);
-      if (!polygon.empty()) {
+      const Polygon polygon =
+          finder_.FindPolygon(::std::move(unwarped_contour), verbose);
+      if (!polygon.segments.empty()) {
         raw_polys.push_back(polygon);
       }
     }
     LOG(INFO, "Polygons: (%zu).\n", raw_polys.size());
 
     // Calculate each component side of a possible target.
-    std::vector<TargetComponent> target_component_list =
-        finder_.FillTargetComponentList(raw_polys);
+    ::std::vector<TargetComponent> target_component_list =
+        finder_.FillTargetComponentList(raw_polys, verbose);
     LOG(INFO, "Components: (%zu).\n", target_component_list.size());
 
     // Put the compenents together into targets.
-    std::vector<Target> target_list =
+    ::std::vector<Target> target_list =
         finder_.FindTargetsFromComponents(target_component_list, verbose);
     LOG(INFO, "Potential Target: (%zu).\n", target_list.size());
 
     // Use the solver to generate an intermediate version of our results.
-    std::vector<IntermediateResult> results;
+    ::std::vector<IntermediateResult> results;
     for (const Target &target : target_list) {
       results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
     }