Add debug_viewer, target_finder, target_sender.
Change-Id: I50c3512c7444aa58cb8b80e1e46fe26637c68c81
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
new file mode 100644
index 0000000..6736c24
--- /dev/null
+++ b/y2019/vision/BUILD
@@ -0,0 +1,80 @@
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+VISION_TARGETS = [ "//tools:k8", "//tools:armhf-debian"]
+
+cc_library(
+ name = "target_finder",
+ srcs = ["target_finder.cc", "target_geometry.cc"],
+ hdrs = ["target_finder.h", "target_types.h"],
+ deps = [
+ "@com_google_ceres_solver//:ceres",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:region_alloc",
+ "//aos/vision/blob:contour",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:overlay",
+ "//aos/vision/math:vector",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+gtk_dependent_cc_binary(
+ name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/vision/blob:move_scale",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:debug_framework",
+ "//aos/vision/math:vector",
+ ],
+ copts = ["-Wno-unused-variable"],
+ restricted_to = VISION_TARGETS,
+)
+
+cc_binary(
+ name = "target_sender",
+ srcs = ["target_sender.cc"],
+ deps = [
+ ":target_finder",
+ "//y2019/jevois:serial",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+"""
+cc_binary(
+ name = "calibration",
+ srcs = ["calibration.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+"""
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
new file mode 100644
index 0000000..4f43c9a
--- /dev/null
+++ b/y2019/vision/debug_viewer.cc
@@ -0,0 +1,269 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/move_scale.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::ImageFormat;
+using aos::vision::RangeImage;
+using aos::vision::AnalysisAllocator;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+using aos::vision::Segment;
+using aos::vision::PixelRef;
+
+namespace y2019 {
+namespace vision {
+
+std::vector<PixelRef> GetNColors(size_t num_colors) {
+ std::vector<PixelRef> colors;
+ for (size_t i = 0; i < num_colors; ++i) {
+ int quadrent = i * 6 / num_colors;
+ uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
+ uint8_t inv_alpha = 255 - alpha;
+ switch (quadrent) {
+ case 0:
+ colors.push_back(PixelRef{255, alpha, 0});
+ break;
+ case 1:
+ colors.push_back(PixelRef{inv_alpha, 255, 0});
+ break;
+ case 2:
+ colors.push_back(PixelRef{0, 255, alpha});
+ break;
+ case 3:
+ colors.push_back(PixelRef{0, inv_alpha, 255});
+ break;
+ case 4:
+ colors.push_back(PixelRef{alpha, 0, 255});
+ break;
+ case 5:
+ colors.push_back(PixelRef{255, 0, inv_alpha});
+ break;
+ }
+ }
+ return colors;
+}
+
+class FilterHarness : public aos::vision::FilterHarness {
+ public:
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
+ return finder_.Threshold(image);
+ }
+
+ void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
+ viewer_ = viewer;
+ viewer_->SetScale(0.75);
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(finder_.GetOverlay());
+ viewer_->view()->SetOverlays(&overlays_);
+ }
+
+ void DrawBlob(const RangeImage &blob, PixelRef color) {
+ if (viewer_) {
+ BlobList list;
+ list.push_back(blob);
+ viewer_->DrawBlobList(list, color);
+ }
+ }
+
+ bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ imgs_last_ = imgs;
+ fmt_last_ = fmt;
+ // reset for next drawing cycle
+ for (auto &overlay : overlays_) {
+ overlay->Reset();
+ }
+
+ if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
+ draw_raw_target_ || draw_raw_IR_ || draw_results_) {
+ printf("_____ New Image _____\n");
+ }
+
+ // Remove bad blobs.
+ finder_.PreFilter(&imgs);
+
+ // Find polygons from blobs.
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon =
+ finder_.FillPolygon(blob, draw_raw_poly_);
+ if (polygon.empty()) {
+ DrawBlob(blob, {255, 0, 0});
+ } else {
+ raw_polys.push_back(polygon);
+ if (draw_select_blob_) {
+ DrawBlob(blob, {0, 0, 255});
+ }
+ if (draw_raw_poly_) {
+ std::vector<PixelRef> colors = GetNColors(polygon.size());
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+ }
+
+ for (size_t i = 0; i < 4; ++i) {
+ overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+ }
+ }
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+ if (draw_components_) {
+ for (const TargetComponent &comp : target_component_list) {
+ DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
+ {0, 0, 255});
+ }
+ }
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+ target_component_list, draw_raw_target_);
+ if (draw_raw_target_) {
+ for (const Target &target : target_list) {
+ DrawTarget(target);
+ }
+ }
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+ }
+
+ // Check that our current results match possible solutions.
+ results = finder_.FilterResults(results);
+ if (draw_results_) {
+ for (const IntermediateResult &res : results) {
+ DrawResult(res, {0, 255, 0});
+ DrawTarget(res, {0, 255, 0});
+ }
+ }
+
+ // If the target list is not empty then we found a target.
+ return !results.empty();
+ }
+
+ std::function<void(uint32_t)> RegisterKeyPress() override {
+ return [this](uint32_t key) {
+ (void)key;
+ if (key == 'z') {
+ draw_results_ = !draw_results_;
+ } else if (key == 'x') {
+ draw_raw_IR_ = !draw_raw_IR_;
+ } else if (key == 'c') {
+ draw_raw_target_ = !draw_raw_target_;
+ } else if (key == 'v') {
+ draw_components_ = !draw_components_;
+ } else if (key == 'b') {
+ draw_raw_poly_ = !draw_raw_poly_;
+ } else if (key == 'n') {
+ draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'q') {
+ printf("User requested shutdown.\n");
+ exit(0);
+ }
+ HandleBlobs(imgs_last_, fmt_last_);
+ viewer_->Redraw();
+ };
+ }
+
+ void DrawComponent(const TargetComponent &comp, PixelRef top_color,
+ PixelRef bot_color, PixelRef in_color,
+ PixelRef out_color) {
+ overlay_.AddLine(comp.top, comp.inside, top_color);
+ overlay_.AddLine(comp.bottom, comp.outside, bot_color);
+
+ overlay_.AddLine(comp.bottom, comp.inside, in_color);
+ overlay_.AddLine(comp.top, comp.outside, out_color);
+ }
+
+ void DrawTarget(const Target &target) {
+ Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
+ Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
+ overlay_.AddLine(leftTop, rightTop, {255, 215, 0});
+
+ Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
+ Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
+ overlay_.AddLine(leftBot, rightBot, {255, 215, 0});
+
+ overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
+ overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
+ }
+
+ void DrawResult(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ DrawComponent(target.left, color, color, color, color);
+ DrawComponent(target.right, color, color, color, color);
+ }
+
+ void DrawTarget(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
+ (target.left.bottom + target.left.outside) * 0.5);
+ leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
+ leftAx.B() * 0.9 + leftAx.A() * 0.1);
+ overlay_.AddLine(leftAx, color);
+
+ Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
+ (target.right.bottom + target.right.outside) * 0.5);
+ rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
+ rightAx.B() * 0.9 + rightAx.A() * 0.1);
+ overlay_.AddLine(rightAx, color);
+
+ overlay_.AddLine(leftAx.A(), rightAx.A(), color);
+ overlay_.AddLine(leftAx.B(), rightAx.B(), color);
+ Vector<3> p1(0.0, 0.0, 100.0);
+
+ Vector<3> p2 =
+ Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
+ Vector<2> p3(p2.x(), p2.y());
+ overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
+ overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});
+
+ overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
+ }
+
+ const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+
+ private:
+ // implementation of the filter pipeline.
+ TargetFinder finder_;
+ aos::vision::BlobStreamViewer *viewer_ = nullptr;
+ aos::vision::PixelLinesOverlay overlay_;
+ std::vector<aos::vision::OverlayBase *> overlays_;
+ BlobList imgs_last_;
+ ImageFormat fmt_last_;
+ bool draw_select_blob_ = false;
+ bool draw_raw_poly_ = false;
+ bool draw_components_ = false;
+ bool draw_raw_target_ = false;
+ bool draw_raw_IR_ = false;
+ bool draw_results_ = true;
+};
+
+} // namespace vision
+} // namespace y2017
+
+int main(int argc, char **argv) {
+ y2019::vision::FilterHarness filter_harness;
+ aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
+ aos::vision::CameraParams());
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
new file mode 100644
index 0000000..f69e987
--- /dev/null
+++ b/y2019/vision/target_finder.cc
@@ -0,0 +1,379 @@
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+using namespace aos::vision;
+
+namespace y2019 {
+namespace vision {
+
+TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+
+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;
+ });
+}
+
+// Filter blobs on size.
+void TargetFinder::PreFilter(BlobList *imgs) {
+ imgs->erase(
+ std::remove_if(imgs->begin(), imgs->end(),
+ [](RangeImage &img) {
+ // We can drop images with a small number of
+ // pixels, but images
+ // must be over 20px or the math will have issues.
+ return (img.npixels() < 100 || img.height() < 25);
+ }),
+ imgs->end());
+}
+
+// TODO: Try hierarchical merge for this.
+// Convert blobs into polygons.
+std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
+ const RangeImage &blob, bool verbose) {
+ if (verbose) printf("Process Polygon.\n");
+ alloc_.reset();
+ auto *st = RangeImgToContour(blob, &alloc_);
+
+ struct Pt {
+ float x;
+ float y;
+ };
+ std::vector<Pt> pts;
+
+ // Collect all slopes from the contour.
+ auto opt = st->pt;
+ for (auto *node = st; node->next != st;) {
+ node = node->next;
+
+ auto npt = node->pt;
+
+ pts.push_back(
+ {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+
+ opt = npt;
+ }
+
+ const int n = pts.size();
+ auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+
+ std::vector<Pt> pts_new = pts;
+ auto run_box_filter = [&](int window_size) {
+ for (size_t i = 0; i < pts.size(); ++i) {
+ Pt a{0.0, 0.0};
+ for (int j = -window_size; j <= window_size; ++j) {
+ Pt p = get_pt(j + i);
+ a.x += p.x;
+ a.y += p.y;
+ }
+ a.x /= (window_size * 2 + 1);
+ a.y /= (window_size * 2 + 1);
+
+ float scale = 1.0 + (i / float(pts.size() * 10));
+ a.x *= scale;
+ a.y *= scale;
+ pts_new[i] = a;
+ }
+ pts = pts_new;
+ };
+ // Three box filter makith a guassian?
+ // Run gaussian filter over the slopes.
+ run_box_filter(2);
+ run_box_filter(2);
+ run_box_filter(2);
+
+ // Heuristic which says if a particular slope is part of a corner.
+ auto is_corner = [&](size_t i) {
+ Pt a = get_pt(i - 3);
+ Pt b = get_pt(i + 3);
+ double dx = (a.x - b.x);
+ double dy = (a.y - b.y);
+ return dx * dx + dy * dy > 0.25;
+ };
+
+ bool prev_v = is_corner(-1);
+
+ // Find all centers of corners.
+ // Because they round, multiple points may be a corner.
+ std::vector<size_t> edges;
+ size_t kBad = pts.size() + 10;
+ size_t prev_up = kBad;
+ size_t wrapped_n = prev_up;
+
+ for (size_t i = 0; i < pts.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);
+ }
+ }
+ if (v && !prev_v) {
+ prev_up = i;
+ }
+ prev_v = v;
+ }
+
+ if (wrapped_n != kBad) {
+ edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+ }
+
+ if (verbose) printf("Edge Count (%zu).\n", edges.size());
+
+ // Get all CountourNodes from the contour.
+ using aos::vision::PixelRef;
+ std::vector<ContourNode *> segments;
+ {
+ std::vector<ContourNode *> segments_all;
+
+ for (ContourNode *node = st; node->next != st;) {
+ node = node->next;
+ segments_all.push_back(node);
+ }
+ for (size_t i : edges) {
+ segments.push_back(segments_all[i]);
+ }
+ }
+ if (verbose) printf("Segment Count (%zu).\n", segments.size());
+
+ // Run best-fits over each line segment.
+ std::vector<Segment<2>> seg_list;
+ if (segments.size() == 4) {
+ for (size_t i = 0; i < segments.size(); ++i) {
+ auto *ed = segments[(i + 1) % segments.size()];
+ auto *st = segments[i];
+ float mx = 0.0;
+ float my = 0.0;
+ int n = 0;
+ for (auto *node = st; node != ed; node = node->next) {
+ mx += node->pt.x;
+ my += node->pt.y;
+ ++n;
+ // (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
+ }
+ mx /= n;
+ my /= n;
+
+ float xx = 0.0;
+ float xy = 0.0;
+ float yy = 0.0;
+ for (auto *node = st; node != ed; node = node->next) {
+ float x = node->pt.x - mx;
+ float y = node->pt.y - my;
+ xx += x * x;
+ xy += x * y;
+ yy += y * y;
+ }
+
+ // TODO: Extract common to hierarchical merge.
+ float neg_b_over_2 = (xx + yy) / 2.0;
+ float c = (xx * yy - xy * xy);
+
+ float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+
+ {
+ float lam = neg_b_over_2 + sqr;
+ float x = xy;
+ float y = lam - xx;
+
+ float norm = sqrt(x * x + y * y);
+ x /= norm;
+ y /= norm;
+
+ seg_list.push_back(
+ Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
+ }
+
+ /* Characteristic polynomial
+ 1 lam^2 - (xx + yy) lam + (xx * yy - xy * xy) = 0
+
+ [a b]
+ [c d]
+
+ // covariance matrix.
+ [xx xy] [nx]
+ [xy yy] [ny]
+ */
+ }
+ }
+ if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
+ return seg_list;
+}
+
+// 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;
+ TargetComponent new_target;
+ for (const auto &poly : seg_list) {
+ // Reject missized pollygons for now. Maybe rectify them here in the future;
+ if (poly.size() != 4) continue;
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+ }
+
+ // Select the closest two points. Short side of the rectangle.
+ double min_dist = -1;
+ 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]);
+ if (min_dist == -1 || nd < min_dist) {
+ min_dist = nd;
+ closest.first = i;
+ closest.second = next;
+ }
+ }
+
+ // Verify our top is above the bottom.
+ size_t bot_index = closest.first;
+ size_t top_index = (closest.first + 2) % 4;
+ if (corners[top_index].y() < corners[bot_index].y()) {
+ closest.first = top_index;
+ closest.second = (top_index + 1) % 4;
+ }
+
+ // Find the major axis.
+ size_t far_first = (closest.first + 2) % 4;
+ size_t far_second = (closest.second + 2) % 4;
+ Segment<2> major_axis(
+ (corners[closest.first] + corners[closest.second]) * 0.5,
+ (corners[far_first] + corners[far_second]) * 0.5);
+ if (major_axis.AsVector().AngleToZero() > M_PI / 180.0 * 120.0 ||
+ major_axis.AsVector().AngleToZero() < M_PI / 180.0 * 60.0) {
+ // Target is angled way too much, drop it.
+ continue;
+ }
+
+ // organize the top points.
+ Vector<2> topA = corners[closest.first] - major_axis.B();
+ new_target.major_axis = major_axis;
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a left target since we are leaning positive.
+ new_target.is_right = false;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ }
+ } else {
+ // We have a right target since we are leaning negative.
+ new_target.is_right = true;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ }
+ }
+
+ // organize the top points.
+ Vector<2> botA = corners[far_first] - major_axis.A();
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a right target since we are leaning positive.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ }
+ } else {
+ // We have a left target since we are leaning negative.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ }
+ }
+
+ // This piece of the target should be ready now.
+ list.emplace_back(new_target);
+ }
+
+ return list;
+}
+
+// Match components into targets.
+std::vector<Target> TargetFinder::FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose) {
+ std::vector<Target> target_list;
+ using namespace aos::vision;
+ if (component_list.size() < 2) {
+ // We don't enough parts for a target.
+ return target_list;
+ }
+
+ for (size_t i = 0; i < component_list.size(); i++) {
+ const TargetComponent &a = component_list[i];
+ for (size_t j = 0; j < i; j++) {
+ bool target_valid = false;
+ Target new_target;
+ const TargetComponent &b = component_list[j];
+
+ // Reject targets that are too far off vertically.
+ Vector<2> a_center = a.major_axis.Center();
+ if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
+ continue;
+ }
+ Vector<2> b_center = b.major_axis.Center();
+ if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
+ continue;
+ }
+
+ if (a.is_right && !b.is_right) {
+ if (a.top.x() > b.top.x()) {
+ new_target.right = a;
+ new_target.left = b;
+ target_valid = true;
+ }
+ } else if (!a.is_right && b.is_right) {
+ if (b.top.x() > a.top.x()) {
+ new_target.right = b;
+ new_target.left = a;
+ target_valid = true;
+ }
+ }
+ if (target_valid) {
+ target_list.emplace_back(new_target);
+ }
+ }
+ }
+ if (verbose) printf("Possible Target: %zu.\n", target_list.size());
+ return target_list;
+}
+
+std::vector<IntermediateResult> TargetFinder::FilterResults(
+ const std::vector<IntermediateResult> &results) {
+ std::vector<IntermediateResult> filtered;
+ for (const IntermediateResult &res : results) {
+ if (res.solver_error < 75.0) {
+ filtered.emplace_back(res);
+ }
+ }
+ return filtered;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
new file mode 100644
index 0000000..633733a
--- /dev/null
+++ b/y2019/vision/target_finder.h
@@ -0,0 +1,80 @@
+#ifndef _Y2019_VISION_TARGET_FINDER_H_
+#define _Y2019_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/region_alloc.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/vector.h"
+#include "y2019/vision/target_types.h"
+
+namespace y2019 {
+namespace vision {
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+class TargetFinder {
+ public:
+ TargetFinder();
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // Value against which we threshold.
+ uint8_t GetThresholdValue() { return 120; }
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList *imgs);
+
+ // Turn a blob into a polgygon.
+ std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ 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);
+
+ // Piece the compenents together into a target.
+ std::vector<Target> FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose);
+
+ // Given a target solve for the transformation of the template target.
+ IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+
+ std::vector<IntermediateResult> FilterResults(
+ const std::vector<IntermediateResult> &results);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2> &target, double down_angle,
+ double *dist, double *angle);
+
+ // Return the template target in a normalized space.
+ const Target &GetTemplateTarget() { return target_template_; }
+
+ const IntrinsicParams &intrinsics() const { return intrinsics_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ aos::vision::PixelLinesOverlay overlay_;
+
+ aos::vision::AnalysisAllocator alloc_;
+
+ // The template for the default target in the standard space.
+ Target target_template_;
+
+ IntrinsicParams intrinsics_;
+
+ ExtrinsicParams default_extrinsics_;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
new file mode 100644
index 0000000..77adc83
--- /dev/null
+++ b/y2019/vision/target_geometry.cc
@@ -0,0 +1,186 @@
+#include "y2019/vision/target_finder.h"
+
+#include "ceres/ceres.h"
+
+#include <math.h>
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+
+using namespace aos::vision;
+using aos::vision::Vector;
+
+Target Target::MakeTemplate() {
+ Target out;
+ // This is how off-vertical the tape is.
+ const double theta = 14.5 * M_PI / 180.0;
+
+ const double tape_offset = 4 * kInchesToMeters;
+ const double tape_width = 2 * kInchesToMeters;
+ const double tape_length = 5.5 * kInchesToMeters;
+
+ const double s = sin(theta);
+ const double c = cos(theta);
+ out.right.top = Vector<2>(tape_offset, 0.0);
+ out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
+ out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
+ tape_width * s - tape_length * c);
+ out.right.outside =
+ Vector<2>(tape_offset + tape_length * s, -tape_length * c);
+
+ out.right.is_right = true;
+ out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
+ out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
+ out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
+ out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
+ return out;
+}
+
+std::array<Vector<2>, 8> Target::toPointList() const {
+ return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
+ right.outside, left.top, left.inside,
+ left.bottom, left.outside}};
+}
+
+Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ double y = extrinsics.y;
+ double z = extrinsics.z;
+ double r1 = extrinsics.r1;
+ double r2 = extrinsics.r2;
+ double rup = intrinsics.mount_angle;
+ double fl = intrinsics.focal_length;
+
+ ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
+
+ {
+ double theta = r1;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ pts(2) += z;
+
+ {
+ double theta = r2;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ double theta = rup;
+ double s = sin(theta);
+ double c = cos(theta);
+
+ pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Final image projection.
+ ::Eigen::Matrix<double, 1, 3> res = pts;
+
+ float scale = fl / res.z();
+ return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
+}
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ auto project = [&](Vector<2> pt) {
+ return Project(pt, intrinsics, extrinsics);
+ };
+ Target new_targ;
+ new_targ.right.is_right = true;
+ new_targ.right.top = project(target.right.top);
+ new_targ.right.inside = project(target.right.inside);
+ new_targ.right.bottom = project(target.right.bottom);
+ new_targ.right.outside = project(target.right.outside);
+
+ new_targ.left.top = project(target.left.top);
+ new_targ.left.inside = project(target.left.inside);
+ new_targ.left.bottom = project(target.left.bottom);
+ new_targ.left.outside = project(target.left.outside);
+
+ return new_targ;
+}
+
+// Used at runtime on a single image given camera parameters.
+struct RuntimeCostFunctor {
+ RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+ IntrinsicParams intrinsics)
+ : result(result), template_pt(template_pt), intrinsics(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ auto extrinsics = ExtrinsicParams::get(x);
+ auto pt = result - Project(template_pt, intrinsics, extrinsics);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ }
+
+ Vector<2> result;
+ Vector<2> template_pt;
+ IntrinsicParams intrinsics;
+};
+
+IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
+ bool verbose) {
+ // Memory for the ceres solver.
+ double params[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms[0]);
+
+ Problem problem;
+
+ auto target_value = target.toPointList();
+ auto template_value = target_template_.toPointList();
+
+ for (size_t i = 0; i < 8; ++i) {
+ auto a = template_value[i];
+ auto b = target_value[i];
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
+ new RuntimeCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms[0]);
+ }
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ IntermediateResult IR;
+ IR.extrinsics = ExtrinsicParams::get(¶ms[0]);
+ IR.solver_error = summary.final_cost;
+
+ if (verbose) {
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
+ std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
+ std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
+ std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+ }
+ return IR;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
new file mode 100644
index 0000000..0d53e2c
--- /dev/null
+++ b/y2019/vision/target_sender.cc
@@ -0,0 +1,136 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+
+#include "y2019/jevois/serial.h"
+#include "y2019/vision/target_finder.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+using aos::vision::Segment;
+
+class CameraStream : public ::aos::vision::ImageStreamEvent {
+ public:
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
+ : ImageStreamEvent(fname, params) {}
+
+ void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
+ LOG(INFO, "got frame: %d\n", (int)data.size());
+
+ static unsigned i = 0;
+
+ /*
+ std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
+ std::to_string(i) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ */
+ if (on_frame) on_frame(data, monotonic_now);
+ ++i;
+
+ if (i == 200) exit(-1);
+ }
+
+ std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+};
+
+int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
+
+std::string GetFileContents(const std::string &filename) {
+ std::ifstream in(filename, std::ios::in | std::ios::binary);
+ if (in) {
+ std::string contents;
+ in.seekg(0, std::ios::end);
+ contents.resize(in.tellg());
+ in.seekg(0, std::ios::beg);
+ in.read(&contents[0], contents.size());
+ in.close();
+ return (contents);
+ }
+ fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+ exit(-1);
+}
+
+int main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int itsDev = open_terminos("/dev/ttyS0");
+ dup2(itsDev, 1);
+ dup2(itsDev, 2);
+
+ TargetFinder finder_;
+
+ // Check that our current results match possible solutions.
+ aos::vision::CameraParams params0;
+ params0.set_exposure(0);
+ params0.set_brightness(40);
+ params0.set_width(640);
+ // params0.set_fps(10);
+ params0.set_height(480);
+
+ ::std::unique_ptr<CameraStream> camera0(
+ new CameraStream(params0, "/dev/video0"));
+ camera0->on_frame = [&](DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ bool verbose = false;
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ if (polygon.empty()) {
+ } else {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+ }
+
+ results = finder_.FilterResults(results);
+ };
+
+ aos::events::EpollLoop loop;
+
+ for (int i = 0; i < 100; ++i) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ camera0->ReadEvent();
+ }
+
+ // TODO: Fix event loop on jevois:
+ // loop.Add(camera0.get());
+ // loop.Run();
+}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
new file mode 100644
index 0000000..557b04a
--- /dev/null
+++ b/y2019/vision/target_types.h
@@ -0,0 +1,128 @@
+#ifndef _Y2019_VISION_TARGET_TYPES_H_
+#define _Y2019_VISION_TARGET_TYPES_H_
+
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace y2019 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ aos::vision::Vector<2> GetByIndex(size_t i) const {
+ switch (i) {
+ case 0:
+ return top;
+ case 1:
+ return inside;
+ case 2:
+ return bottom;
+ case 3:
+ return outside;
+ default:
+ return aos::vision::Vector<2>();
+ }
+ }
+ bool is_right;
+ aos::vision::Vector<2> top;
+ aos::vision::Vector<2> inside;
+ aos::vision::Vector<2> outside;
+ aos::vision::Vector<2> bottom;
+
+ aos::vision::Segment<2> major_axis;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent left;
+ TargetComponent right;
+
+ static Target MakeTemplate();
+ // Get the points in some order (will match against the template).
+ std::array<aos::vision::Vector<2>, 8> toPointList() const;
+};
+
+struct IntrinsicParams {
+ static constexpr size_t kNumParams = 2;
+
+ double mount_angle = 10.0481 / 180.0 * M_PI;
+ double focal_length = 729.445;
+
+ void set(double *data) {
+ data[0] = mount_angle;
+ data[1] = focal_length;
+ }
+ static IntrinsicParams get(const double *data) {
+ IntrinsicParams out;
+ out.mount_angle = data[0];
+ out.focal_length = data[1];
+ return out;
+ }
+};
+
+struct ExtrinsicParams {
+ static constexpr size_t kNumParams = 4;
+
+ double y = 18.0 * 0.0254;
+ double z = 23.0 * 0.0254;
+ double r1 = 1.0 / 180 * M_PI;
+ double r2 = -1.0 / 180 * M_PI;
+
+ void set(double *data) {
+ data[0] = y;
+ data[1] = z;
+ data[2] = r1;
+ data[3] = r2;
+ }
+ static ExtrinsicParams get(const double *data) {
+ ExtrinsicParams out;
+ out.y = data[0];
+ out.z = data[1];
+ out.r1 = data[2];
+ out.r2 = data[3];
+ return out;
+ }
+};
+// Projects a point from idealized template space to camera space.
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+// An intermediate for of the results.
+// These are actual awnsers, but in a form from the solver that is not ready for
+// the wire.
+struct IntermediateResult {
+ ExtrinsicParams extrinsics;
+
+ // Error from solver calulations.
+ double solver_error;
+};
+
+// Final foramtting ready for output on the wire.
+struct TargetResult {
+ // Distance to the target in meters. Specifically, the distance from the
+ // center of the camera's image plane to the center of the target.
+ float distance;
+ // Height of the target in meters. Specifically, the distance from the floor
+ // to the center of the target.
+ float height;
+
+ // Heading of the center of the target in radians. Zero is straight out
+ // perpendicular to the camera's image plane. Images to the left (looking at a
+ // camera image) are at a positive angle.
+ float heading;
+
+ // The angle between the target and the camera's image plane. This is
+ // projected so both are assumed to be perpendicular to the floor. Parallel
+ // targets have a skew of zero. Targets rotated such that their left edge
+ // (looking at a camera image) is closer are at a positive angle.
+ float skew;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif