Merge "Split StatespaceLoop into a Plant, Controller, and Observer."
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index cf728c5..7d57f50 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -100,6 +100,7 @@
srcs = ['move_scale.cc'],
deps = [
':range_image',
+ '//aos/vision/image:image_types',
],
)
@@ -136,7 +137,7 @@
hdrs = ['stream_view.h'],
deps = [
':range_image',
- '//aos/vision/debug:debug_viewer',
+ '//aos/vision/debug:debug_window',
'//aos/vision/image:image_types',
],
)
diff --git a/aos/vision/blob/move_scale.h b/aos/vision/blob/move_scale.h
index e45e14d..061da5c 100644
--- a/aos/vision/blob/move_scale.h
+++ b/aos/vision/blob/move_scale.h
@@ -5,18 +5,11 @@
#include <vector>
#include "aos/vision/blob/range_image.h"
+#include "aos/vision/image/image_types.h"
namespace aos {
namespace vision {
-// Bounding box for a RangeImage.
-struct ImageBBox {
- int minx = std::numeric_limits<int>::max();
- int maxx = std::numeric_limits<int>::min();
- int miny = std::numeric_limits<int>::max();
- int maxy = std::numeric_limits<int>::min();
-};
-
// Sums img into bbox. bbox is constructed empty and grows with each call
// to GetBBox.
void GetBBox(const RangeImage &img, ImageBBox *bbox);
diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
index a6a661a..4cee7b4 100644
--- a/aos/vision/blob/stream_view.h
+++ b/aos/vision/blob/stream_view.h
@@ -2,7 +2,7 @@
#define _AOS_VISION_BLOB_STREAM_VIEW_H_
#include "aos/vision/blob/range_image.h"
-#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/debug/debug_window.h"
#include "aos/vision/image/image_types.h"
#include <memory>
@@ -10,10 +10,10 @@
namespace aos {
namespace vision {
-class BlobStreamViewer : public DebugViewer {
+class BlobStreamViewer : public DebugWindow {
public:
- BlobStreamViewer() : DebugViewer(false) {}
- explicit BlobStreamViewer(bool flip) : DebugViewer(flip) {}
+ BlobStreamViewer() : DebugWindow(false) {}
+ explicit BlobStreamViewer(bool flip) : DebugWindow(flip) {}
void Submit(ImageFormat fmt, const BlobList &blob_list) {
SetFormatAndClear(fmt);
@@ -77,7 +77,7 @@
}
// Backwards compatible.
- DebugViewer *view() { return this; }
+ DebugWindow *view() { return this; }
ImagePtr img() { return image_.get(); }
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
index a96d25c..e9e45ad 100644
--- a/aos/vision/debug/BUILD
+++ b/aos/vision/debug/BUILD
@@ -11,9 +11,9 @@
],
)
-gtk_dependent_cc_library(name = "debug_viewer",
- srcs = ["debug_viewer.cc"],
- hdrs = ["debug_viewer.h"],
+gtk_dependent_cc_library(name = "debug_window",
+ srcs = ["debug_window.cc"],
+ hdrs = ["debug_window.h"],
deps = [
'@usr_repo//:gtk+-3.0',
"//aos/vision/image:image_types",
@@ -44,7 +44,6 @@
'//aos/vision/image:jpeg_routines',
'//aos/vision/image:image_stream',
'//aos/vision/image:image_types',
- '//aos/vision/debug:debug_viewer',
'//aos/common/util:global_factory',
'@usr_repo//:gtk+-3.0',
],
diff --git a/aos/vision/debug/blob_log-source.cc b/aos/vision/debug/blob_log-source.cc
index f707342..f2bfbb5 100644
--- a/aos/vision/debug/blob_log-source.cc
+++ b/aos/vision/debug/blob_log-source.cc
@@ -15,7 +15,7 @@
namespace {
-long GetFileSize(const std::string& filename) {
+long GetFileSize(const std::string &filename) {
struct stat stat_buf;
int rc = stat(filename.c_str(), &stat_buf);
return rc == 0 ? stat_buf.st_size : -1;
diff --git a/aos/vision/debug/debug_framework.cc b/aos/vision/debug/debug_framework.cc
index 99dfd8b..64f370c 100644
--- a/aos/vision/debug/debug_framework.cc
+++ b/aos/vision/debug/debug_framework.cc
@@ -6,7 +6,6 @@
#include "aos/common/logging/logging.h"
#include "aos/vision/blob/find_blob.h"
#include "aos/vision/blob/stream_view.h"
-#include "aos/vision/debug/debug_viewer.h"
#include "aos/vision/events/epoll_events.h"
#include "aos/vision/image/jpeg_routines.h"
diff --git a/aos/vision/debug/debug_viewer.cc b/aos/vision/debug/debug_window.cc
similarity index 88%
rename from aos/vision/debug/debug_viewer.cc
rename to aos/vision/debug/debug_window.cc
index b0cf295..acbaf8e 100644
--- a/aos/vision/debug/debug_viewer.cc
+++ b/aos/vision/debug/debug_window.cc
@@ -1,4 +1,4 @@
-#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/debug/debug_window.h"
#include <gdk-pixbuf/gdk-pixbuf.h>
#include <gtk/gtk.h>
@@ -26,7 +26,7 @@
g_signal_connect(widget, "draw", G_CALLBACK(fnptr), obj);
}
-struct DebugViewer::Internals {
+struct DebugWindow::Internals {
Internals(bool flip) : flip_(flip) {}
gboolean Draw(cairo_t *cr) {
@@ -73,18 +73,18 @@
bool clear_per_frame_ = true;
};
-void DebugViewer::SetOverlays(std::vector<OverlayBase *> *overlays) {
+void DebugWindow::SetOverlays(std::vector<OverlayBase *> *overlays) {
self->overlays = overlays;
}
-void DebugViewer::Redraw() {
+void DebugWindow::Redraw() {
if (!self->needs_draw) {
gtk_widget_queue_draw(self->drawing_area);
self->needs_draw = true;
}
}
-void DebugViewer::UpdateImage(ImagePtr ptr) {
+void DebugWindow::UpdateImage(ImagePtr ptr) {
if (ptr.data() != self->ptr.data()) {
int w = ptr.fmt().w;
int h = ptr.fmt().h;
@@ -107,11 +107,11 @@
}
}
-void DebugViewer::MoveTo(int x, int y) {
+void DebugWindow::MoveTo(int x, int y) {
gtk_window_move(GTK_WINDOW(self->window), x, y);
}
-void DebugViewer::SetScale(double scale_factor_inp) {
+void DebugWindow::SetScale(double scale_factor_inp) {
int w = window_width_;
int h = window_height_;
@@ -128,12 +128,12 @@
gboolean debug_viewer_key_press_event(GtkWidget * /*widget*/,
GdkEventKey *event, gpointer user_data) {
auto &key_press_cb =
- reinterpret_cast<DebugViewer *>(user_data)->key_press_event;
+ reinterpret_cast<DebugWindow *>(user_data)->key_press_event;
if (key_press_cb) key_press_cb(event->keyval);
return FALSE;
}
-DebugViewer::DebugViewer(bool flip) : self(new Internals(flip)) {
+DebugWindow::DebugWindow(bool flip) : self(new Internals(flip)) {
self->scale_factor = scale_factor;
GtkWidget *window;
auto drawing_area = self->drawing_area = gtk_drawing_area_new();
@@ -141,7 +141,7 @@
window_height_ * scale_factor);
gtk_widget_add_events(drawing_area, GDK_KEY_PRESS_MASK);
- g_draw_signal_connect<DebugViewer::Internals, &DebugViewer::Internals::Draw>(
+ g_draw_signal_connect<DebugWindow::Internals, &DebugWindow::Internals::Draw>(
drawing_area, self.get());
window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
@@ -154,7 +154,7 @@
gtk_container_add(GTK_CONTAINER(window), drawing_area);
}
-DebugViewer::~DebugViewer() {}
+DebugWindow::~DebugWindow() {}
void CairoRender::Text(int x, int y, int /*text_x*/, int /*text_y*/,
const std::string &text) {
diff --git a/aos/vision/debug/debug_viewer.h b/aos/vision/debug/debug_window.h
similarity index 90%
rename from aos/vision/debug/debug_viewer.h
rename to aos/vision/debug/debug_window.h
index ab2c716..906bf3b 100644
--- a/aos/vision/debug/debug_viewer.h
+++ b/aos/vision/debug/debug_window.h
@@ -1,5 +1,5 @@
-#ifndef AOS_VISION_DEBUG_DEBUG_VIEWER_H_
-#define AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#ifndef AOS_VISION_DEBUG_DEBUG_WINDOW_H_
+#define AOS_VISION_DEBUG_DEBUG_WINDOW_H_
#include <cairo.h>
#include <functional>
@@ -39,11 +39,11 @@
};
// Simple debug view window.
-class DebugViewer {
+class DebugWindow {
public:
struct Internals;
- explicit DebugViewer(bool flip);
- ~DebugViewer();
+ explicit DebugWindow(bool flip);
+ ~DebugWindow();
// Explicit redraw queuing (Will not double-queue).
void Redraw();
@@ -76,4 +76,4 @@
} // namespace vision
} // namespace aos
-#endif // AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#endif // AOS_VISION_DEBUG_DEBUG_WINDOW_H_
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
index fbd6838..5a9804f 100644
--- a/aos/vision/debug/overlay.h
+++ b/aos/vision/debug/overlay.h
@@ -117,18 +117,39 @@
~PixelLinesOverlay() {}
// build a segment for this line
- void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+ void AddLine(Vector<2> st, Vector<2> ed) { AddLine(st, ed, color); }
// build a segment for this line
- void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ void AddLine(Vector<2> st, Vector<2> ed, PixelRef newColor) {
lines_.emplace_back(
std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
}
- void start_new_profile() { start_profile = true; }
+ void DrawCross(aos::vision::Vector<2> center, int width,
+ aos::vision::PixelRef color) {
+ using namespace aos::vision;
+ AddLine(Vector<2>(center.x() - width, center.y()),
+ Vector<2>(center.x() + width, center.y()), color);
+ AddLine(Vector<2>(center.x(), center.y() - width),
+ Vector<2>(center.x(), center.y() + width), color);
+ }
+
+ void DrawBBox(const ImageBBox &box, aos::vision::PixelRef color) {
+ using namespace aos::vision;
+ AddLine(Vector<2>(box.minx, box.miny), Vector<2>(box.maxx, box.miny),
+ color);
+ AddLine(Vector<2>(box.maxx, box.miny), Vector<2>(box.maxx, box.maxy),
+ color);
+ AddLine(Vector<2>(box.maxx, box.maxy), Vector<2>(box.minx, box.maxy),
+ color);
+ AddLine(Vector<2>(box.minx, box.maxy), Vector<2>(box.minx, box.miny),
+ color);
+ }
+
+ void StartNewProfile() { start_profile = true; }
// add a new point connected to the last point in the line
- void add_point(Vector<2> pt, PixelRef newColor) {
+ void AddPoint(Vector<2> pt, PixelRef newColor) {
if (lines_.empty() || start_profile) {
lines_.emplace_back(
std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
diff --git a/aos/vision/debug/tcp-source.cc b/aos/vision/debug/tcp-source.cc
index 28feff3..37c38d5 100644
--- a/aos/vision/debug/tcp-source.cc
+++ b/aos/vision/debug/tcp-source.cc
@@ -76,7 +76,8 @@
public:
Impl(const std::string &hostname, int portno,
DebugFrameworkInterface *interface)
- : aos::events::TcpClient(hostname.c_str(), portno), interface_(interface) {}
+ : aos::events::TcpClient(hostname.c_str(), portno),
+ interface_(interface) {}
void ReadEvent() override {
read_.up(fd(), [&](DataRef data) {
diff --git a/aos/vision/image/image_types.h b/aos/vision/image/image_types.h
index 68e6d67..f8e5e65 100644
--- a/aos/vision/image/image_types.h
+++ b/aos/vision/image/image_types.h
@@ -12,6 +12,14 @@
namespace aos {
namespace vision {
+// Bounding box for a RangeImage.
+struct ImageBBox {
+ int minx = std::numeric_limits<int>::max();
+ int maxx = std::numeric_limits<int>::min();
+ int miny = std::numeric_limits<int>::max();
+ int maxy = std::numeric_limits<int>::min();
+};
+
// This will go into c++17. No sense writing my own version.
using DataRef = std::experimental::string_view;
diff --git a/aos/vision/image/reader.cc b/aos/vision/image/reader.cc
index 3a4b349..03e2fc8 100644
--- a/aos/vision/image/reader.cc
+++ b/aos/vision/image/reader.cc
@@ -237,7 +237,6 @@
LOG(FATAL, "Failed to set up camera\n");
}
- // #if 0
// set framerate
struct v4l2_streamparm *setfps;
setfps = (struct v4l2_streamparm *)calloc(1, sizeof(struct v4l2_streamparm));
@@ -251,7 +250,6 @@
LOG(INFO, "framerate ended up at %d/%d\n",
setfps->parm.capture.timeperframe.numerator,
setfps->parm.capture.timeperframe.denominator);
- // #endif
}
aos::vision::ImageFormat Reader::get_format() {
diff --git a/aos/vision/tools/BUILD b/aos/vision/tools/BUILD
index 56bbb5b..ac6a6ff 100644
--- a/aos/vision/tools/BUILD
+++ b/aos/vision/tools/BUILD
@@ -17,3 +17,14 @@
"//aos/vision/events:gtk_event",
],
)
+
+cc_binary(
+ name = 'camera_primer',
+ srcs = ['camera_primer.cc'],
+ deps = [
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/image:image_stream',
+ '//aos/vision/events:epoll_events',
+ ],
+)
diff --git a/aos/vision/tools/camera_primer.cc b/aos/vision/tools/camera_primer.cc
new file mode 100644
index 0000000..a431ac8
--- /dev/null
+++ b/aos/vision/tools/camera_primer.cc
@@ -0,0 +1,49 @@
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/image_stream.h"
+
+class ImageStream : public aos::vision::ImageStreamEvent {
+ public:
+ ImageStream(const std::string &fname, camera::CameraParams params)
+ : ImageStreamEvent(fname, params) {}
+ void ProcessImage(aos::vision::DataRef /*data*/,
+ aos::monotonic_clock::time_point) override {
+ if (i_ > 20) {
+ exit(0);
+ }
+ ++i_;
+ }
+
+ private:
+ int i_ = 0;
+};
+
+// camera_primer drops the first 20 frames. This is to get around issues
+// where the first N frames from the camera are garbage. Thus each year
+// you should write a startup script like so:
+//
+// camera_primer
+// target_sender
+int main(int argc, char **argv) {
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+
+ camera::CameraParams params = {.width = 640 * 2,
+ .height = 480 * 2,
+ .exposure = 10,
+ .brightness = 128,
+ .gain = 0,
+ .fps = 30};
+
+ if (argc != 2) {
+ fprintf(stderr, "usage: %s path_to_camera\n", argv[0]);
+ exit(-1);
+ }
+ ImageStream stream(argv[1], params);
+
+ aos::events::EpollLoop loop;
+ loop.Add(&stream);
+ loop.Run();
+}
diff --git a/build_tests/BUILD b/build_tests/BUILD
index be6a201..72dc6da 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -74,6 +74,14 @@
proto_cc_library(
name = 'proto_build_test_library',
src = 'proto.proto',
+ deps = [
+ ':proto_build_test_library_base',
+ ],
+)
+
+proto_cc_library(
+ name = 'proto_build_test_library_base',
+ src = 'proto_base.proto',
)
cc_test(
diff --git a/build_tests/proto.cc b/build_tests/proto.cc
index 23ff879..bb09003 100644
--- a/build_tests/proto.cc
+++ b/build_tests/proto.cc
@@ -6,6 +6,7 @@
::frc971::TestProto test_proto1, test_proto2;
test_proto1.set_s("Hi!");
test_proto1.set_i(971);
+ test_proto1.mutable_base_proto()->set_a("silly");
::std::string serialized;
ASSERT_TRUE(test_proto1.SerializeToString(&serialized));
diff --git a/build_tests/proto.proto b/build_tests/proto.proto
index 367a650..d0e6fe2 100644
--- a/build_tests/proto.proto
+++ b/build_tests/proto.proto
@@ -4,9 +4,13 @@
import "google/protobuf/empty.proto";
+import "build_tests/proto_base.proto";
+
message TestProto {
string s = 1;
int32 i = 2;
// Making sure that well-known protos work.
.google.protobuf.Empty empty = 3;
+ // Making sure we can depend on other protos.
+ BaseProto base_proto = 4;
}
diff --git a/build_tests/proto_base.proto b/build_tests/proto_base.proto
new file mode 100644
index 0000000..8fc2143
--- /dev/null
+++ b/build_tests/proto_base.proto
@@ -0,0 +1,8 @@
+syntax = "proto2";
+
+package frc971;
+
+message BaseProto {
+ optional string a = 1;
+ optional string b = 2;
+}
diff --git a/tools/build_rules/protobuf.bzl b/tools/build_rules/protobuf.bzl
index d5f4284..c9bb9ef 100644
--- a/tools/build_rules/protobuf.bzl
+++ b/tools/build_rules/protobuf.bzl
@@ -1,16 +1,22 @@
def _do_proto_cc_library_impl(ctx):
+ srcs = ctx.files.srcs
+ deps = []
+ deps += ctx.files.srcs
+
+ for dep in ctx.attr.deps:
+ deps += dep.proto.deps
+
message = 'Building %s and %s from %s' % (ctx.outputs.pb_h.short_path,
ctx.outputs.pb_cc.short_path,
- ctx.file.src.short_path)
+ ctx.files.srcs[0].short_path)
ctx.action(
- inputs = ctx.files.src + ctx.files._well_known_protos,
+ inputs = deps + ctx.files._well_known_protos,
executable = ctx.executable._protoc,
arguments = [
'--cpp_out=%s' % ctx.configuration.genfiles_dir.path,
'-I.',
'-Ithird_party/protobuf/src',
- ctx.file.src.path,
- ],
+ ] + [s.path for s in srcs],
mnemonic = 'ProtocCc',
progress_message = message,
outputs = [
@@ -19,8 +25,15 @@
],
)
-def _do_proto_cc_library_outputs(src):
- basename = src.name[:-len('.proto')]
+ return struct(
+ proto = struct(
+ srcs = srcs,
+ deps = deps,
+ )
+ )
+
+def _do_proto_cc_library_outputs(srcs):
+ basename = srcs[0].name[:-len('.proto')]
return {
'pb_h': '%s.pb.h' % basename,
'pb_cc': '%s.pb.cc' % basename,
@@ -29,11 +42,10 @@
_do_proto_cc_library = rule(
implementation = _do_proto_cc_library_impl,
attrs = {
- 'src': attr.label(
+ 'srcs': attr.label_list(
allow_files = FileType(['.proto']),
- mandatory = True,
- single_file = True,
),
+ 'deps': attr.label_list(providers = ["proto"]),
'_protoc': attr.label(
default = Label('//third_party/protobuf:protoc'),
executable = True,
@@ -47,7 +59,7 @@
output_to_genfiles = True,
)
-def proto_cc_library(name, src, visibility = None):
+def proto_cc_library(name, src, deps = [], visibility = None):
'''Generates a cc_library from a single .proto file. Does not support
dependencies on any .proto files except the well-known ones protobuf comes
with (which are unconditionally depended on).
@@ -58,7 +70,8 @@
_do_proto_cc_library(
name = '%s__proto_srcs' % name,
- src = src,
+ srcs = [src],
+ deps = [('%s__proto_srcs' % o_name) for o_name in deps],
visibility = ['//visibility:private'],
)
basename = src[:-len('.proto')]
@@ -66,6 +79,6 @@
name = name,
srcs = [ '%s.pb.cc' % basename ],
hdrs = [ '%s.pb.h' % basename ],
- deps = [ '//third_party/protobuf' ],
+ deps = [ '//third_party/protobuf' ] + deps,
visibility = visibility,
)
diff --git a/y2016/vision/blob_filters.cc b/y2016/vision/blob_filters.cc
index e9f881d..860fa80 100644
--- a/y2016/vision/blob_filters.cc
+++ b/y2016/vision/blob_filters.cc
@@ -74,7 +74,7 @@
if (do_overlay_) {
for (FittedLine &line : lines) {
- overlay_->add_line(Vector<2>(line.st.x, line.st.y),
+ overlay_->AddLine(Vector<2>(line.st.x, line.st.y),
Vector<2>(line.ed.x, line.ed.y), {255, 0, 0});
}
}
@@ -105,9 +105,9 @@
Segment<2> bottom(MakeLine(bottomLine));
if (do_overlay_) {
- overlay_->add_line(left.A(), left.B(), {155, 0, 255});
- overlay_->add_line(right.A(), right.B(), {255, 155, 0});
- overlay_->add_line(bottom.A(), bottom.B(), {255, 0, 155});
+ overlay_->AddLine(left.A(), left.B(), {155, 0, 255});
+ overlay_->AddLine(right.A(), right.B(), {255, 155, 0});
+ overlay_->AddLine(bottom.A(), bottom.B(), {255, 0, 155});
}
res.emplace_back(left.Intersect(bottom), right.Intersect(bottom));
@@ -238,8 +238,8 @@
if (do_overlay_) {
double a = hist_lr[i];
Vector<2> mid = a * s + (1.0 - a) * e;
- overlay_->add_line(s, mid, {0, 0, 255});
- overlay_->add_line(mid, e, {255, 255, 0});
+ overlay_->AddLine(s, mid, {0, 0, 255});
+ overlay_->AddLine(mid, e, {255, 255, 0});
}
}
double check_vert_up = L22_dist(hist_size_, vert_hist_, hist_lr);
@@ -259,8 +259,8 @@
if (do_overlay_) {
double a = hist_ub[i];
Vector<2> mid = a * s + (1.0 - a) * e;
- overlay_->add_line(s, mid, {0, 0, 255});
- overlay_->add_line(mid, e, {255, 255, 0});
+ overlay_->AddLine(s, mid, {0, 0, 255});
+ overlay_->AddLine(mid, e, {255, 255, 0});
}
}
double check_horiz = L22_dist(hist_size_, horiz_hist_, hist_ub);
@@ -268,16 +268,16 @@
if (do_overlay_) {
Vector<2> A = blob->upper_left + Vector<2>(-10, 10);
Vector<2> B = blob->upper_left - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->upper_right + Vector<2>(-10, 10);
B = blob->upper_right - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->lower_right + Vector<2>(-10, 10);
B = blob->lower_right - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
A = blob->lower_left + Vector<2>(-10, 10);
B = blob->lower_left - Vector<2>(-10, 10);
- overlay_->add_line(A, B, {255, 255, 255});
+ overlay_->AddLine(A, B, {255, 255, 255});
}
// If this target is better upside down, if it is flip the blob.
// The horizontal will not be effected, so we will not change that.
@@ -292,16 +292,16 @@
if (do_overlay_) {
Vector<2> A = blob->upper_left + Vector<2>(10, 10);
Vector<2> B = blob->upper_left - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->upper_right + Vector<2>(10, 10);
B = blob->upper_right - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->lower_right + Vector<2>(10, 10);
B = blob->lower_right - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
A = blob->lower_left + Vector<2>(10, 10);
B = blob->lower_left - Vector<2>(10, 10);
- overlay_->add_line(A, B, {255, 0, 255});
+ overlay_->AddLine(A, B, {255, 0, 255});
}
// average the two distances
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
index 706c348..2790a83 100644
--- a/y2016/vision/target_sender.cc
+++ b/y2016/vision/target_sender.cc
@@ -228,12 +228,12 @@
std::thread cam0([stereo]() {
RunCamera(0, GetCameraParams(stereo.calibration()),
stereo.calibration().right_camera_name(),
- stereo.calibration().roborio_ip_addr(), 8082);
+ stereo.calibration().roborio_ip_addr(), 8080);
});
std::thread cam1([stereo]() {
RunCamera(1, GetCameraParams(stereo.calibration()),
stereo.calibration().left_camera_name(),
- stereo.calibration().roborio_ip_addr(), 8082);
+ stereo.calibration().roborio_ip_addr(), 8080);
});
cam0.join();
cam1.join();
diff --git a/y2017/vision/BUILD b/y2017/vision/BUILD
index c9794fe..b46f01d 100644
--- a/y2017/vision/BUILD
+++ b/y2017/vision/BUILD
@@ -34,3 +34,16 @@
'//aos/common:mutex',
],
)
+
+cc_library(
+ name = 'target_finder',
+ srcs = ['target_finder.cc'],
+ hdrs = ['target_finder.h'],
+ deps = [
+ '//aos/vision/blob:threshold',
+ '//aos/vision/blob:transpose',
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:vector',
+ ],
+)
+)
diff --git a/y2017/vision/target_finder.cc b/y2017/vision/target_finder.cc
new file mode 100644
index 0000000..e39fe73
--- /dev/null
+++ b/y2017/vision/target_finder.cc
@@ -0,0 +1,238 @@
+#include "y2017/vision/target_finder.h"
+
+#include <math.h>
+
+namespace y2017 {
+namespace vision {
+
+// Blobs now come in three types:
+// 0) normal blob.
+// 1) first part of a split blob.
+// 2) second part of a split blob.
+void ComputeXShiftPolynomial(int type, const RangeImage &img,
+ TargetComponent *target) {
+ target->img = &img;
+ RangeImage t_img = Transpose(img);
+ int spacing = 10;
+ int n = t_img.size() - spacing * 2;
+ target->n = n;
+ if (n <= 0) {
+ printf("Empty blob aborting (%d).\n", n);
+ return;
+ }
+ Eigen::MatrixXf A = Eigen::MatrixXf::Zero(n * 2, 4);
+ Eigen::VectorXf b = Eigen::VectorXf::Zero(n * 2);
+ int i = 0;
+ for (const auto &row : t_img) {
+ // We decided this was a split target, but this is not a split row.
+ if (i >= spacing && i - spacing < n) {
+ int j = (i - spacing) * 2;
+ // normal blob or the first part of a split.
+ if (type == 0 || type == 1) {
+ b(j) = row[0].st;
+ } else {
+ b(j) = row[1].st;
+ }
+ A(j, 0) = (i) * (i);
+ A(j, 1) = (i);
+ A(j, 2) = 1;
+ ++j;
+ // normal target or the second part of a split.
+ if (type == 0 || type == 2) {
+ b(j) = row[row.size() - 1].ed;
+ } else {
+ b(j) = row[0].ed;
+ }
+ A(j, 0) = i * i;
+ A(j, 1) = i;
+ A(j, 3) = 1;
+ }
+ ++i;
+ }
+ Eigen::VectorXf sol =
+ A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+ target->a = sol(0);
+ target->b = sol(1);
+ target->c_0 = sol(2);
+ target->c_1 = sol(3);
+
+ target->mini = t_img.min_y();
+
+ Eigen::VectorXf base = A * sol;
+ Eigen::VectorXf error_v = b - base;
+ target->fit_error = error_v.dot(error_v);
+}
+
+double TargetFinder::DetectConnectedTarget(const RangeImage &img) {
+ using namespace aos::vision;
+ RangeImage t_img = Transpose(img);
+ int total = 0;
+ int split = 0;
+ int count = t_img.mini();
+ for (const auto &row : t_img) {
+ if (row.size() == 1) {
+ total++;
+ } else if (row.size() == 2) {
+ split++;
+ }
+ count++;
+ }
+ return (double)split / total;
+}
+
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+ const BlobList &blobs) {
+ std::vector<TargetComponent> list;
+ TargetComponent newTarg;
+ for (std::size_t i = 0; i < blobs.size(); ++i) {
+ double split_ratio;
+ if ((split_ratio = DetectConnectedTarget(blobs[i])) > 0.50) {
+ // Split type blob, do it two parts.
+ ComputeXShiftPolynomial(1, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ ComputeXShiftPolynomial(2, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ } else {
+ // normal type blob.
+ ComputeXShiftPolynomial(0, blobs[i], &newTarg);
+ list.emplace_back(newTarg);
+ }
+ }
+
+ return list;
+}
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+ return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+ if (px.g > 88) {
+ return true;
+ 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) {
+ 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());
+}
+
+bool TargetFinder::FindTargetFromComponents(
+ std::vector<TargetComponent> component_list, Target *final_target) {
+ using namespace aos::vision;
+ if (component_list.size() < 2 || final_target == NULL) {
+ // We don't enough parts for a traget.
+ return false;
+ }
+
+ // A0 * c + A1*s = b
+ Eigen::MatrixXf A = Eigen::MatrixXf::Zero(4, 2);
+ // A0: Offset component will be constant across all equations.
+ A(0, 0) = 1;
+ A(1, 0) = 1;
+ A(2, 0) = 1;
+ A(3, 0) = 1;
+
+ // A1: s defines the scaling and defines an expexted target.
+ // So these are the center heights of the top and bottom of the two targets.
+ A(0, 1) = -1;
+ A(1, 1) = 0;
+ A(2, 1) = 2;
+ A(3, 1) = 4;
+
+ // Track which pair is the best fit.
+ double best_error = -1;
+ double best_offset = -1;
+ Eigen::VectorXf best_v;
+ // Write down the two indicies.
+ std::pair<int, int> selected;
+ // We are regressing the combined estimated center, might write that down.
+ double regressed_y_center;
+
+ Eigen::VectorXf b = Eigen::VectorXf::Zero(4);
+ for (size_t i = 0; i < component_list.size(); i++) {
+ for (size_t j = 0; j < component_list.size(); j++) {
+ if (i == j) {
+ continue;
+ } else {
+ if (component_list[i].a < 0.0 || component_list[j].a < 0.0) {
+ // one of the targets is upside down (ie curved up), this can't
+ // happen.
+ continue;
+ }
+ // b is the target offests.
+ b(0) = component_list[j].EvalMinTop();
+ b(1) = component_list[j].EvalMinBot();
+ b(2) = component_list[i].EvalMinTop();
+ b(3) = component_list[i].EvalMinBot();
+ }
+
+ Eigen::VectorXf sol =
+ A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
+
+ Eigen::VectorXf base = A * sol;
+ Eigen::VectorXf error_v = b - base;
+ double error = error_v.dot(error_v);
+ // offset in scrren x of the two targets.
+ double offset = std::abs(component_list[i].CenterPolyOne() -
+ component_list[j].CenterPolyOne());
+ // How much do we care about offset. As far as I've seen, error is like
+ // 5-20, offset are around 10. Value selected for worst garbage can image.
+ const double offsetWeight = 2.1;
+ error += offsetWeight * offset;
+ if ((best_error < 0 || error < best_error) && !isnan(error)) {
+ best_error = error;
+ best_offset = offset;
+ best_v = error_v;
+ selected.first = i;
+ selected.second = j;
+ regressed_y_center = sol(0);
+ }
+ }
+ }
+
+ // If we missed or the error is ridiculous just give up here.
+ if (best_error < 0 || best_error > 300.0 || isnan(best_error)) {
+ fprintf(stderr, "Bogus target dude (%f).\n", best_error);
+ return false;
+ }
+
+ fprintf(stderr,
+ "Selected (%d, %d):\n\t"
+ "err(%.2f, %.2f, %.2f, %.2f)(%.2f)(%.2f).\n\t"
+ "c00(%.2f, %.2f)(%.2f)\n",
+ selected.first, selected.second, best_v(0), best_v(1), best_v(2),
+ best_v(3), best_error, best_offset,
+ component_list[selected.first].CenterPolyOne(),
+ component_list[selected.second].CenterPolyOne(),
+ component_list[selected.first].CenterPolyOne() -
+ component_list[selected.second].CenterPolyOne());
+
+ double avgOff = (component_list[selected.first].mini +
+ component_list[selected.second].mini) /
+ 2.0;
+ double avgOne = (component_list[selected.first].CenterPolyOne() +
+ component_list[selected.second].CenterPolyOne()) /
+ 2.0;
+
+ final_target->screen_coord.x(avgOne + avgOff);
+ final_target->screen_coord.y(regressed_y_center);
+ final_target->comp1 = component_list[selected.first];
+ final_target->comp2 = component_list[selected.second];
+
+ return true;
+}
+
+} // namespace vision
+} // namespace y2017
diff --git a/y2017/vision/target_finder.h b/y2017/vision/target_finder.h
new file mode 100644
index 0000000..79417d1
--- /dev/null
+++ b/y2017/vision/target_finder.h
@@ -0,0 +1,86 @@
+#ifndef _Y2017_VISION_TARGET_FINDER_H_
+#define _Y2017_VISION_TARGET_FINDER_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"
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+namespace y2017 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ const RangeImage *img = nullptr;
+
+ // Polynomial constants.
+ double a;
+ double b;
+ double c_0;
+ double c_1;
+
+ double mini;
+
+ double CenterPolyOne() { return -b / (2.0 * a); }
+ double CenterPolyTwo() { return (c_0); }
+
+ double EvalMinAt(double c) {
+ double min = CenterPolyOne();
+ return a * (min * min) + b * min + c;
+ }
+
+ double EvalMinTop() { return EvalMinAt(c_1); }
+ double EvalMinBot() { return EvalMinAt(c_0); }
+
+ // Fit error is not normalized by n.
+ double fit_error;
+ int n;
+
+ RangeImage RenderShifted() const;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent comp1;
+
+ TargetComponent comp2;
+ aos::vision::Vector<2> screen_coord;
+};
+
+class TargetFinder {
+ public:
+ // Turn a bloblist into components of a target.
+ std::vector<TargetComponent> FillTargetComponentList(const BlobList &blobs);
+
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList &imgs);
+
+ // Piece the compenents together into a target.
+ bool FindTargetFromComponents(std::vector<TargetComponent> component_list,
+ Target *final_target);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ // TODO(ben): move to overlay
+ void DrawCross(aos::vision::Vector<2> center, aos::vision::PixelRef color);
+
+ aos::vision::PixelLinesOverlay overlay_;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // _Y2017_VISION_TARGET_FINDER_H_