Merge changes I7494fd6c,I6c0a13ad
* changes:
Add drivetrain to y2017
Add intake queue
diff --git a/aos/common/scoped_fd.h b/aos/common/scoped_fd.h
index 57ecdd8..29ccf6e 100644
--- a/aos/common/scoped_fd.h
+++ b/aos/common/scoped_fd.h
@@ -1,7 +1,10 @@
+#ifndef _AOS_COMMON_SCOPED_FD_
+#define _AOS_COMMON_SCOPED_FD_
+
#include <unistd.h>
-#include "aos/common/macros.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/macros.h"
namespace aos {
@@ -9,9 +12,7 @@
class ScopedFD {
public:
explicit ScopedFD(int fd = -1) : fd_(fd) {}
- ~ScopedFD() {
- Close();
- }
+ ~ScopedFD() { Close(); }
int get() const { return fd_; }
int release() {
const int r = fd_;
@@ -25,6 +26,7 @@
}
}
operator bool() const { return fd_ != -1; }
+
private:
int fd_;
void Close() {
@@ -38,3 +40,5 @@
};
} // namespace aos
+
+#endif // _AOS_COMMON_SCOPED_FD_
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
index a024aba..8cb4373 100644
--- a/aos/vision/blob/BUILD
+++ b/aos/vision/blob/BUILD
@@ -93,7 +93,6 @@
],
)
-"""
cc_library(
name = 'stream_view',
hdrs = ['stream_view.h'],
@@ -103,4 +102,3 @@
'//aos/vision/image:image_types',
],
)
-"""
diff --git a/aos/vision/debug/BUILD b/aos/vision/debug/BUILD
new file mode 100644
index 0000000..91111b2
--- /dev/null
+++ b/aos/vision/debug/BUILD
@@ -0,0 +1,22 @@
+#!/usr/bin/python3
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "overlay",
+ hdrs = ["overlay.h"],
+ deps = [
+ '//aos/vision/math:vector',
+ '//aos/vision/math:segment',
+ '//aos/vision/image:image_types',
+ ],
+)
+
+cc_library(name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ hdrs = ["debug_viewer.h"],
+ deps = [
+ '@usr_repo//:gtk+-3.0',
+ "//aos/vision/image:image_types",
+ ":overlay",
+ ]
+)
diff --git a/aos/vision/debug/debug_viewer.cc b/aos/vision/debug/debug_viewer.cc
new file mode 100644
index 0000000..331f733
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.cc
@@ -0,0 +1,166 @@
+#include "aos/vision/debug/debug_viewer.h"
+
+#include <gdk-pixbuf/gdk-pixbuf.h>
+#include <gtk/gtk.h>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <functional>
+#include <memory>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+gboolean DrawCallback(GtkWidget *, cairo_t *cr, gpointer data) {
+ return ((*((T *)data)).*DrawMethod)(cr);
+}
+
+template <typename T, gboolean (T::*DrawMethod)(cairo_t *cr)>
+void g_draw_signal_connect(GtkWidget *widget, T *obj) {
+ gboolean (*fnptr)(GtkWidget *, cairo_t *, gpointer) =
+ DrawCallback<T, DrawMethod>;
+ g_signal_connect(widget, "draw", G_CALLBACK(fnptr), obj);
+}
+
+struct DebugViewer::Internals {
+ Internals(bool flip) : flip_(flip) {}
+
+ gboolean Draw(cairo_t *cr) {
+ needs_draw = false;
+ cairo_scale(cr, scale_factor, scale_factor);
+ if (pixbuf != nullptr) {
+ cairo_save(cr);
+ if (flip_) {
+ cairo_translate(cr, ptr.fmt().w, ptr.fmt().h);
+ cairo_scale(cr, -1, -1);
+ }
+ gdk_cairo_set_source_pixbuf(cr, pixbuf, 0.0, 0.0);
+ cairo_paint(cr);
+ cairo_restore(cr);
+ }
+
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ if (overlays) {
+ for (const auto &ov : *overlays) {
+ cairo_save(cr);
+ CairoRender render(cr);
+ // move the drawing to match the window size
+ ov->Draw(&render, w, h);
+ cairo_restore(cr);
+ }
+ }
+
+ return FALSE;
+ }
+
+ GdkPixbuf *pixbuf = nullptr;
+ GtkWidget *drawing_area = nullptr;
+ ImagePtr ptr;
+ bool needs_draw = true;
+ GtkWidget *window;
+ std::vector<OverlayBase *> *overlays = nullptr;
+ double scale_factor;
+
+ // flip the image rows on drawing
+ bool flip_ = false;
+
+ // clear per frame
+ bool clear_per_frame_ = true;
+};
+
+void DebugViewer::SetOverlays(std::vector<OverlayBase *> *overlays) {
+ self->overlays = overlays;
+}
+
+void DebugViewer::Redraw() {
+ if (!self->needs_draw) {
+ gtk_widget_queue_draw(self->drawing_area);
+ self->needs_draw = true;
+ }
+}
+
+void DebugViewer::UpdateImage(ImagePtr ptr) {
+ if (ptr.data() != self->ptr.data()) {
+ int w = ptr.fmt().w;
+ int h = ptr.fmt().h;
+ self->pixbuf = gdk_pixbuf_new_from_data(
+ (const unsigned char *)ptr.data(), GDK_COLORSPACE_RGB, FALSE, 8,
+ ptr.fmt().w, ptr.fmt().h, 3 * ptr.fmt().w, NULL, NULL);
+ self->ptr = ptr;
+
+ gtk_window_set_default_size(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+ window_height_ = h;
+ window_width_ = w;
+ }
+}
+
+void DebugViewer::MoveTo(int x, int y) {
+ gtk_window_move(GTK_WINDOW(self->window), x, y);
+}
+
+void DebugViewer::SetScale(double scale_factor_inp) {
+ int w = window_width_;
+ int h = window_height_;
+
+ scale_factor = scale_factor_inp;
+ self->scale_factor = scale_factor;
+
+ gtk_window_resize(GTK_WINDOW(self->window), w * scale_factor,
+ h * scale_factor);
+
+ gtk_widget_set_size_request(self->drawing_area, w * scale_factor,
+ h * scale_factor);
+}
+
+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;
+ if (key_press_cb) key_press_cb(event->keyval);
+ return FALSE;
+}
+
+DebugViewer::DebugViewer(bool flip) : self(new Internals(flip)) {
+ self->scale_factor = scale_factor;
+ GtkWidget *window;
+ auto drawing_area = self->drawing_area = gtk_drawing_area_new();
+ gtk_widget_set_size_request(drawing_area, window_width_ * scale_factor,
+ window_height_ * scale_factor);
+ gtk_widget_add_events(drawing_area, GDK_KEY_PRESS_MASK);
+
+ g_draw_signal_connect<DebugViewer::Internals, &DebugViewer::Internals::Draw>(
+ drawing_area, self.get());
+
+ window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ self->window = window;
+ g_signal_connect(window, "key-press-event",
+ G_CALLBACK(debug_viewer_key_press_event), this);
+ gtk_window_set_title(GTK_WINDOW(window), "Window");
+ gtk_window_set_default_size(GTK_WINDOW(window), window_width_ * scale_factor,
+ window_height_ * scale_factor);
+
+ gtk_container_add(GTK_CONTAINER(window), drawing_area);
+ gtk_widget_show_all(window);
+}
+DebugViewer::~DebugViewer() {}
+
+void CairoRender::Text(int x, int y, int /*text_x*/, int /*text_y*/,
+ const std::string &text) {
+ auto *pango_lay = pango_cairo_create_layout(cr_);
+ cairo_move_to(cr_, x, y);
+ pango_layout_set_text(pango_lay, text.data(), text.size());
+ pango_cairo_show_layout(cr_, pango_lay);
+ g_object_unref(pango_lay);
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/debug/debug_viewer.h b/aos/vision/debug/debug_viewer.h
new file mode 100644
index 0000000..3bada28
--- /dev/null
+++ b/aos/vision/debug/debug_viewer.h
@@ -0,0 +1,78 @@
+#ifndef AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+#define AOS_VISION_DEBUG_DEBUG_VIEWER_H_
+
+#include <cairo.h>
+#include <functional>
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// Implement Cairo version of RenderInterface.
+class CairoRender : public RenderInterface {
+ public:
+ explicit CairoRender(cairo_t *cr) : cr_(cr) {}
+ virtual ~CairoRender() {}
+
+ void Translate(double x, double y) override { cairo_translate(cr_, x, y); }
+
+ void SetSourceRGB(double r, double g, double b) override {
+ cairo_set_source_rgb(cr_, r, g, b);
+ }
+
+ void MoveTo(double x, double y) override { cairo_move_to(cr_, x, y); }
+
+ void LineTo(double x, double y) override { cairo_line_to(cr_, x, y); }
+
+ void Circle(double x, double y, double r) override {
+ cairo_arc(cr_, x, y, r, 0.0, 2 * M_PI);
+ }
+
+ void Stroke() override { cairo_stroke(cr_); }
+
+ void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) override;
+
+ private:
+ cairo_t *cr_;
+};
+
+// Simple debug view window.
+class DebugViewer {
+ public:
+ struct Internals;
+ explicit DebugViewer(bool flip);
+ ~DebugViewer();
+ // Explicit redraw queuing (Will not double-queue).
+ void Redraw();
+
+ // This will resize the window as well as updating to draw from the
+ // (not owned) ptr. When you change ptr, you should call Redraw();
+ void UpdateImage(ImagePtr ptr);
+
+ // Sets up the window to draw a list of overlays.
+ // See overlay.h for more info.
+ void SetOverlays(std::vector<OverlayBase *> *overlay);
+
+ // Resizes the window.
+ void SetScale(double scale_factor);
+
+ // Move window.
+ void MoveTo(int x, int y);
+
+ // Set to change the key_press behaviour.
+ // The argument type is a constant that looks like: GDK_KEY_#{key_val_name}
+ std::function<void(uint32_t)> key_press_event;
+
+ private:
+ double scale_factor = 1.0;
+ int window_width_ = 100;
+ int window_height_ = 100;
+ std::unique_ptr<Internals> self;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_DEBUG_DEBUG_VIEWER_H_
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
new file mode 100644
index 0000000..fbd6838
--- /dev/null
+++ b/aos/vision/debug/overlay.h
@@ -0,0 +1,198 @@
+#ifndef _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+#define _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
+
+#include <string>
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+// Abstract away rendering to avoid compiling gtk for arm.
+// This should match a reduced cairo rendering api.
+class RenderInterface {
+ public:
+ RenderInterface() {}
+ RenderInterface(RenderInterface &&other) = delete;
+ RenderInterface(const RenderInterface &other) = delete;
+ ~RenderInterface() {}
+
+ virtual void Translate(double x, double y) = 0;
+ virtual void SetSourceRGB(double r, double g, double b) = 0;
+ virtual void MoveTo(double x, double y) = 0;
+ virtual void LineTo(double x, double y) = 0;
+ virtual void Circle(double x, double y, double r) = 0;
+ // negative in x, y, text_x, text_y measures from max in those value
+ virtual void Text(int x, int y, int text_x, int text_y,
+ const std::string &text) = 0;
+ virtual void Stroke() = 0;
+};
+
+// Interface for a list of overlays to be drawn onto a debug image.
+// These will be passed into the running vision algorithms to output debug info,
+// so they must not have costly side-effects.
+class OverlayBase {
+ public:
+ OverlayBase() {}
+ virtual ~OverlayBase() {}
+
+ // Draws this overlay to the given canvas.
+ virtual void Draw(RenderInterface *render, double /* width */,
+ double /* height */) = 0;
+
+ // Clears the entire overlay.
+ virtual void Reset() = 0;
+
+ PixelRef color = {255, 0, 0};
+ double scale = 1.0;
+};
+
+// A lambda that renders directly to the render interface.
+class LambdaOverlay : public OverlayBase {
+ public:
+ std::function<void(RenderInterface *, double, double)> draw_fn;
+ void Draw(RenderInterface *render, double width, double height) override {
+ if (draw_fn) draw_fn(render, width, height);
+ }
+ void Reset() override {}
+};
+
+// Lines rendered in a coordinate system where the origin is the center
+// of the screen
+class LinesOverlay : public OverlayBase {
+ public:
+ LinesOverlay() : OverlayBase() {}
+ ~LinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed, PixelRef newColor) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, ed), newColor));
+ }
+
+ void add_point(Vector<2> pt) { add_point(pt, color); }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty()) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double w, double h) override {
+ render->Translate(w / 2.0, h / 2.0);
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(scale * ln.first.A().x(), -scale * ln.first.A().y());
+ render->LineTo(scale * ln.first.B().x(), -scale * ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // lines in this over lay
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+};
+
+// Lines rendered in pixel coordinates (Should match up with the screen.)
+class PixelLinesOverlay : public OverlayBase {
+ public:
+ PixelLinesOverlay() : OverlayBase() {}
+ ~PixelLinesOverlay() {}
+
+ // build a segment for this line
+ void add_line(Vector<2> st, Vector<2> ed) { add_line(st, ed, color); }
+
+ // build a segment for this line
+ void add_line(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; }
+
+ // add a new point connected to the last point in the line
+ void add_point(Vector<2> pt, PixelRef newColor) {
+ if (lines_.empty() || start_profile) {
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(pt, pt), newColor));
+ start_profile = false;
+ } else {
+ Vector<2> st = lines_.back().first.B();
+ lines_.emplace_back(
+ std::pair<Segment<2>, PixelRef>(Segment<2>(st, pt), newColor));
+ }
+ }
+
+ void Draw(RenderInterface *render, double /*width*/,
+ double /*hieght*/) override {
+ for (const auto &ln : lines_) {
+ PixelRef localColor = ln.second;
+ render->SetSourceRGB(localColor.r / 255.0, localColor.g / 255.0,
+ localColor.b / 255.0);
+ render->MoveTo(ln.first.A().x(), ln.first.A().y());
+ render->LineTo(ln.first.B().x(), ln.first.B().y());
+ render->Stroke();
+ }
+ }
+
+ // Empting the list will blank the whole overlay.
+ void Reset() override { lines_.clear(); }
+
+ private:
+ // Lines in this overlay.
+ std::vector<std::pair<Segment<2>, PixelRef>> lines_;
+ bool start_profile = false;
+};
+
+// Circles rendered in a coordinate system where the origin is the center
+// of the screen.
+class CircleOverlay : public OverlayBase {
+ public:
+ CircleOverlay() : OverlayBase() {}
+ ~CircleOverlay() {}
+
+ // build a circle as a point and radius
+ std::pair<Vector<2>, double> *add_circle(Vector<2> center, double radius) {
+ circles_.emplace_back(std::pair<Vector<2>, double>(center, radius));
+ return &(circles_.back());
+ }
+
+ void Draw(RenderInterface *render, double w, double h) {
+ render->Translate(w / 2.0, h / 2.0);
+ render->SetSourceRGB(color.r / 255.0, color.g / 255.0, color.b / 255.0);
+ for (const auto &circle : circles_) {
+ render->Circle(scale * circle.first.x(), -scale * circle.first.y(),
+ scale * circle.second);
+ render->Stroke();
+ }
+ }
+
+ // empting the list will blank the whole overlay
+ void Reset() { circles_.clear(); }
+
+ private:
+ // circles in this overlay
+ std::vector<std::pair<Vector<2>, double>> circles_;
+};
+
+} // vision
+} // aos
+
+#endif // _AOS_VISION_IMAGE_DEBUG_OVERLAY_H_
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index c3400fe..e003dc9 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -11,6 +11,14 @@
],
)
+cc_library(name = "socket_types",
+ hdrs = ["socket_types.h"],
+ deps = [
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/image:image_types",
+ ],
+)
+
cc_library(
name = 'intrusive_free_list',
hdrs = ['intrusive_free_list.h'],
diff --git a/aos/vision/events/socket_types.h b/aos/vision/events/socket_types.h
new file mode 100644
index 0000000..2432cb9
--- /dev/null
+++ b/aos/vision/events/socket_types.h
@@ -0,0 +1,78 @@
+#ifndef _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+#define _AOS_VISION_EVENTS_SOCKET_TYPES_H_
+
+#include <poll.h>
+#include <stdint.h>
+
+#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/image/image_types.h"
+#include "google/protobuf/message.h"
+
+namespace aos {
+namespace events {
+
+// Simple TCP client connection that sends messages prefixed by length.
+// Useful to broadcast to a message all connected clients.
+class DataSocket : public events::SocketConnection {
+ public:
+ // Aliasing cast.
+ union data_len {
+ uint32_t len;
+ char buf[4];
+ };
+
+ DataSocket(events::TCPServerBase *serv, int fd)
+ : events::SocketConnection(serv, fd) {}
+
+ ~DataSocket() { printf("Closed connection on descriptor %d\n", fd()); }
+
+ void ReadEvent() override {
+ // Ignore reads, but don't leave them pending.
+ ssize_t count;
+ char buf[512];
+ while (true) {
+ count = read(fd(), &buf, sizeof buf);
+ if (count <= 0) return;
+ }
+ }
+
+ void Emit(const google::protobuf::Message &data) {
+ std::string d;
+ if (data.SerializeToString(&d)) {
+ Emit(d);
+ }
+ }
+
+ void Emit(vision::DataRef data) {
+ data_len len;
+ len.len = data.size();
+ int res = write(fd(), len.buf, sizeof len.buf);
+ if (res == -1) {
+ printf("Emit Error on write\n");
+ }
+ size_t write_count = 0;
+ while (write_count < data.size()) {
+ int len =
+ write(fd(), &data.data()[write_count], data.size() - write_count);
+ if (len == -1) {
+ if (errno == EAGAIN) {
+ struct pollfd waiting;
+ waiting.fd = fd();
+ waiting.events = POLLOUT;
+ poll(&waiting, 1, -1);
+ } else {
+ close(fd());
+ return;
+ }
+ } else {
+ write_count += len;
+ }
+ if (write_count != data.size()) printf("wrote: %d\n", len);
+ }
+ }
+};
+
+} // namespace events
+} // namespace aos
+
+#endif // _AOS_VISION_EVENTS_SOCKET_TYPES_H_
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 2d3d1d1..b5367f6 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -5,24 +5,24 @@
#include "aos/common/logging/logging.h"
namespace aos {
-namespace vision {
+namespace events {
-TXUdpSocket::TXUdpSocket(const char *ip_addr, int port)
+TXUdpSocket::TXUdpSocket(const std::string &ip_addr, int port)
: fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
sockaddr_in destination_in;
memset(&destination_in, 0, sizeof(destination_in));
destination_in.sin_family = AF_INET;
destination_in.sin_port = htons(port);
- if (inet_aton(ip_addr, &destination_in.sin_addr) == 0) {
- LOG(FATAL, "invalid IP address %s\n", ip_addr);
+ if (inet_aton(ip_addr.c_str(), &destination_in.sin_addr) == 0) {
+ LOG(FATAL, "invalid IP address %s\n", ip_addr.c_str());
}
PCHECK(connect(fd_.get(), reinterpret_cast<sockaddr *>(&destination_in),
sizeof(destination_in)));
}
-int TXUdpSocket::Send(const void *data, int size) {
- return PCHECK(send(fd_.get(), static_cast<const char *>(data), size, 0));
+int TXUdpSocket::Send(const char *data, int size) {
+ return PCHECK(send(fd_.get(), data, size, 0));
}
RXUdpSocket::RXUdpSocket(int port)
@@ -42,5 +42,5 @@
return PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/events/udp.h b/aos/vision/events/udp.h
index a12a8bf..ed4a8e7 100644
--- a/aos/vision/events/udp.h
+++ b/aos/vision/events/udp.h
@@ -1,27 +1,28 @@
-#ifndef AOS_VISION_IMAGE_UDP_H_
-#define AOS_VISION_IMAGE_UDP_H_
+#ifndef AOS_VISION_EVENTS_UDP_H_
+#define AOS_VISION_EVENTS_UDP_H_
#include <arpa/inet.h>
-#include <sys/socket.h>
#include <math.h>
+#include <sys/socket.h>
#include <unistd.h>
+#include <string>
#include <vector>
#include "aos/common/macros.h"
#include "aos/common/scoped_fd.h"
namespace aos {
-namespace vision {
+namespace events {
// Simple wrapper around a transmitting UDP socket.
//
// LOG(FATAL)s for all errors, including from Send.
class TXUdpSocket {
public:
- TXUdpSocket(const char *ip_addr, int port);
+ TXUdpSocket(const std::string &ip_addr, int port);
// Returns the number of bytes actually sent.
- int Send(const void *data, int size);
+ int Send(const char *data, int size);
private:
ScopedFD fd_;
@@ -45,7 +46,7 @@
DISALLOW_COPY_AND_ASSIGN(RXUdpSocket);
};
-} // namespace vision
+} // namespace events
} // namespace aos
-#endif // AOS_VISION_IMAGE_UDP_H_
+#endif // AOS_VISION_EVENTS_UDP_H_
diff --git a/aos/vision/events/udp_test.cc b/aos/vision/events/udp_test.cc
index 87046ba..7e77b68 100644
--- a/aos/vision/events/udp_test.cc
+++ b/aos/vision/events/udp_test.cc
@@ -3,7 +3,7 @@
#include "gtest/gtest.h"
namespace aos {
-namespace vision {
+namespace events {
TEST(UDPTest, SendRecv) {
RXUdpSocket rx(1109);
@@ -19,5 +19,5 @@
EXPECT_EQ(txdata[3], rxdata[3]);
}
-} // namespace vision
+} // namespace events
} // namespace aos
diff --git a/aos/vision/image/image_stream.h b/aos/vision/image/image_stream.h
index ca2d8be..bdfbc31 100644
--- a/aos/vision/image/image_stream.h
+++ b/aos/vision/image/image_stream.h
@@ -9,6 +9,8 @@
namespace aos {
namespace vision {
+// Converts a camera reader into a virtual base class that calls ProcessImage
+// on each new image.
class ImageStreamEvent : public ::aos::events::EpollEvent {
public:
static std::unique_ptr<::camera::Reader> GetCamera(
@@ -17,28 +19,28 @@
using namespace std::placeholders;
std::unique_ptr<::camera::Reader> camread(new ::camera::Reader(
fname,
- std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params);
+ std::bind(&ImageStreamEvent::ProcessHelper, obj, _1, _2), params));
camread->StartAsync();
return camread;
}
explicit ImageStreamEvent(std::unique_ptr<::camera::Reader> reader)
- : ::aos::events::EpollEvent(reader->fd()), reader_(reader) {}
+ : ::aos::events::EpollEvent(reader->fd()), reader_(std::move(reader)) {}
explicit ImageStreamEvent(const std::string &fname,
camera::CameraParams params)
: ImageStreamEvent(GetCamera(fname, this, params)) {}
- void ProcessHelper(DataRef data, uint64_t timestamp) {
+ void ProcessHelper(DataRef data, aos::monotonic_clock::time_point timestamp) {
if (data.size() < 300) {
- LOG(INFO, "got bad img: %d of size(%lu)\n", (int)timestamp, data.size());
+ LOG(INFO, "got bad img of size(%lu)\n", data.size());
return;
}
ProcessImage(data, timestamp);
}
- virtual void ProcessImage(DataRef data, uint64_t timestamp) = 0;
+ virtual void ProcessImage(DataRef data, aos::monotonic_clock::time_point timestamp) = 0;
- void ReadEvent(Context /*ctx*/) override { reader_->HandleFrame(); }
+ void ReadEvent() override { reader_->HandleFrame(); }
private:
std::unique_ptr<::camera::Reader> reader_;
diff --git a/debian/usr.BUILD b/debian/usr.BUILD
index 7ee1bf3..403d069 100644
--- a/debian/usr.BUILD
+++ b/debian/usr.BUILD
@@ -86,3 +86,61 @@
],
visibility = ['//visibility:public'],
)
+
+cc_library(
+ name = 'gtk+-3.0',
+ hdrs = glob([
+ 'include/gtk-3.0/**/*.h',
+ 'include/at-spi2-atk/2.0/**/*.h',
+ 'include/at-spi-2.0/**/*.h',
+ 'include/dbus-1.0/**/*.h',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include/**/*.h',
+ 'include/gtk-3.0/**/*.h',
+ 'include/gio-unix-2.0/**/*.h',
+ 'include/cairo/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/harfbuzz/**/*.h',
+ 'include/pango-1.0/**/*.h',
+ 'include/atk-1.0/**/*.h',
+ 'include/pixman-1/**/*.h',
+ 'include/freetype2/**/*.h',
+ 'include/libpng12/**/*.h',
+ 'include/gdk-pixbuf-2.0/**/*.h',
+ 'include/glib-2.0/**/*.h',
+ 'lib/x86_64-linux-gnu/glib-2.0/include/**/*.h',
+ ]),
+ includes = [
+ 'include/gtk-3.0',
+ 'include/at-spi2-atk/2.0',
+ 'include/at-spi-2.0',
+ 'include/dbus-1.0',
+ 'lib/x86_64-linux-gnu/dbus-1.0/include',
+ 'include/gtk-3.0',
+ 'include/gio-unix-2.0/',
+ 'include/cairo',
+ 'include/pango-1.0',
+ 'include/harfbuzz',
+ 'include/pango-1.0',
+ 'include/atk-1.0',
+ 'include/pixman-1',
+ 'include/freetype2',
+ 'include/libpng12',
+ 'include/gdk-pixbuf-2.0',
+ 'include/glib-2.0',
+ 'lib/x86_64-linux-gnu/glib-2.0/include',
+ ],
+ linkopts = [
+ '-lgtk-3',
+ '-lgdk-3',
+ '-lpangocairo-1.0',
+ '-lpango-1.0',
+ '-latk-1.0',
+ '-lcairo-gobject',
+ '-lcairo',
+ '-lgdk_pixbuf-2.0',
+ '-lgio-2.0',
+ '-lgobject-2.0',
+ '-lglib-2.0'
+ ],
+ visibility = ['//visibility:public'],
+)
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index f41ea84..59139e1 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -76,6 +76,39 @@
],
)
+cc_binary(name = 'target_sender',
+ srcs = ['target_sender.cc'],
+ deps = [
+ '//aos/common:time',
+ '//aos/common/logging:logging',
+ '//aos/common/logging:implementations',
+ '//aos/vision/image:reader',
+ '//aos/vision/image:jpeg_routines',
+ '//aos/vision/image:image_stream',
+ '//y2016/vision:blob_filters',
+ '//aos/vision/events:udp',
+ '//aos/vision/events:epoll_events',
+ '//aos/vision/events:socket_types',
+ ':stereo_geometry',
+ ':vision_data',
+ ':calibration',
+ ]
+)
+
+cc_library(name = "blob_filters",
+ srcs = ["blob_filters.cc"],
+ hdrs = ["blob_filters.h"],
+ deps = [
+ "//aos/vision/math:vector",
+ "//aos/vision/math:segment",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:codec",
+ ],
+)
+
cc_binary(
name = 'target_receiver',
srcs = [
diff --git a/y2016/vision/blob_filters.cc b/y2016/vision/blob_filters.cc
new file mode 100644
index 0000000..e9f881d
--- /dev/null
+++ b/y2016/vision/blob_filters.cc
@@ -0,0 +1,393 @@
+#include "y2016/vision/blob_filters.h"
+#include <unistd.h>
+
+namespace aos {
+namespace vision {
+
+double CornerFinder::LineScore(Vector<2> A, Vector<2> B, FittedLine line) {
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double dist_st_a = A.SquaredDistanceTo(st);
+ double dist_st_b = B.SquaredDistanceTo(st);
+
+ if (dist_st_b < dist_st_a) {
+ Vector<2> tmp = B;
+ B = A;
+ A = tmp;
+ }
+
+ return A.SquaredDistanceTo(st) + B.SquaredDistanceTo(ed);
+}
+
+void CornerFinder::EnqueueLine(std::vector<FittedLine> *list, FittedLine line) {
+ if (list->size() < 2) {
+ list->emplace_back(line);
+ return;
+ }
+
+ Vector<2> st(line.st.x, line.st.y);
+ Vector<2> ed(line.ed.x, line.ed.y);
+
+ double llen = st.SquaredDistanceTo(ed);
+ FittedLine ins = line;
+ for (int i = 0; i < (int)list->size(); i++) {
+ Vector<2> ist((*list)[i].st.x, (*list)[i].st.y);
+ Vector<2> ied((*list)[i].ed.x, (*list)[i].ed.y);
+ double ilen = ist.SquaredDistanceTo(ied);
+ if (ilen < llen) {
+ // our new line is longer, so lets use that.
+ // but we still need to check the one we are replacing against the
+ // others.
+ FittedLine tmp = (*list)[i];
+ (*list)[i] = ins;
+ ins = tmp;
+ llen = ilen;
+ }
+ }
+}
+
+Segment<2> CornerFinder::MakeLine(const std::vector<FittedLine> &list) {
+ assert(list.size() == 2);
+ Vector<2> st0(list[0].st.x, list[0].st.y);
+ Vector<2> ed0(list[0].ed.x, list[0].ed.y);
+ Vector<2> st1(list[1].st.x, list[1].st.y);
+ Vector<2> ed1(list[1].ed.x, list[1].ed.y);
+
+ if (ed1.SquaredDistanceTo(st0) < st1.SquaredDistanceTo(st0)) {
+ Vector<2> sttmp = st1;
+ st1 = ed1;
+ ed1 = sttmp;
+ }
+
+ return Segment<2>((st0 + st1), (ed0 + ed1)).Scale(0.5);
+}
+
+std::vector<std::pair<Vector<2>, Vector<2>>> CornerFinder::Find(
+ const std::vector<SelectedBlob> &blobl) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> res;
+ alloc_.reset();
+ for (const SelectedBlob &blob : blobl) {
+ ContourNode *n = RangeImgToContour(blob.blob, &alloc_);
+ std::vector<FittedLine> lines;
+ HierarchicalMerge(n, &lines, merge_rate_, min_len_);
+
+ if (do_overlay_) {
+ for (FittedLine &line : lines) {
+ overlay_->add_line(Vector<2>(line.st.x, line.st.y),
+ Vector<2>(line.ed.x, line.ed.y), {255, 0, 0});
+ }
+ }
+
+ std::vector<FittedLine> leftLine;
+ std::vector<FittedLine> rightLine;
+ std::vector<FittedLine> bottomLine;
+
+ for (auto &line : lines) {
+ double left_score = LineScore(blob.upper_left, blob.lower_left, line);
+ double right_score = LineScore(blob.upper_right, blob.lower_right, line);
+ double bottom_score = LineScore(blob.lower_left, blob.lower_right, line);
+ if (left_score < right_score && left_score < bottom_score) {
+ if (line.st.x > 0 && line.st.y > 0 && line.ed.x > 0 && line.ed.y > 0) {
+ EnqueueLine(&leftLine, line);
+ }
+ } else if (right_score < left_score && right_score < bottom_score) {
+ EnqueueLine(&rightLine, line);
+ } else {
+ EnqueueLine(&bottomLine, line);
+ }
+ }
+
+ if (leftLine.size() == 2 && rightLine.size() == 2 &&
+ bottomLine.size() == 2) {
+ Segment<2> left(MakeLine(leftLine));
+ Segment<2> right(MakeLine(rightLine));
+ 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});
+ }
+
+ res.emplace_back(left.Intersect(bottom), right.Intersect(bottom));
+ }
+ }
+ return res;
+}
+
+//*****************************************************************************
+
+std::vector<SelectedBlob> BlobFilterBase::PreFilter(const BlobList &blobl) {
+ std::vector<SelectedBlob> filt;
+ for (const RangeImage &blob : blobl) {
+ int area = blob.calc_area();
+ if (area > min_area_ && area < max_area_) {
+ filt.emplace_back(SelectedBlob(blob));
+ }
+ }
+ return filt;
+}
+
+//*****************************************************************************
+
+bool HistogramBlobFilter::PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B) {
+ double sq_dist_a = goal.SquaredDistanceTo(A);
+ double sq_dist_b = goal.SquaredDistanceTo(B);
+ if (sq_dist_a < sq_dist_b) {
+ // A is closest
+ return true;
+ } else {
+ // B is closest
+ return false;
+ }
+}
+
+namespace {
+void CalcBoundingBox(const RangeImage &rimage, Vector<2> &ul, Vector<2> &ur,
+ Vector<2> &lr, Vector<2> &ll) {
+ const auto &ranges = rimage.ranges();
+ int mini = rimage.mini();
+ double x_min = ranges[0][0].st;
+ double x_max = ranges[0][0].ed;
+ double y_min = mini;
+ double y_max = mini + ranges.size();
+ for (auto &range : ranges) {
+ for (auto &interval : range) {
+ if (interval.st < x_min) {
+ x_min = interval.st;
+ }
+ if (interval.ed > x_max) {
+ x_max = interval.ed;
+ }
+ }
+ }
+ ul = Vector<2>(x_min, y_min);
+ ur = Vector<2>(x_max, y_min);
+ lr = Vector<2>(x_max, y_max);
+ ll = Vector<2>(x_min, y_max);
+}
+} // namespace
+
+std::vector<SelectedBlob> HistogramBlobFilter::PostFilter(
+ std::vector<SelectedBlob> blobl) {
+ std::vector<SelectedBlob> ret;
+ for (int ti = 0; ti < (int)blobl.size(); ti++) {
+ Vector<2> upper_left(1280, 960);
+ Vector<2> upper_right(0.0, 960);
+ Vector<2> lower_right(0.0, 0.0);
+ Vector<2> lower_left(1280, 0.0);
+
+ Vector<2> tul;
+ Vector<2> tur;
+ Vector<2> tlr;
+ Vector<2> tll;
+ CalcBoundingBox(blobl[ti].blob, tul, tur, tlr, tll);
+ for (int j = 0; j < (int)blobl[ti].blob.ranges().size(); j++) {
+ auto &range = blobl[ti].blob.ranges()[j];
+ Vector<2> first(range[0].st, j + blobl[ti].blob.mini());
+ Vector<2> last(range.back().ed, j + blobl[ti].blob.mini());
+ if (!PickClosest(tul, upper_left, first)) {
+ upper_left = first;
+ }
+ if (!PickClosest(tll, lower_left, first)) {
+ lower_left = first;
+ }
+ if (!PickClosest(tur, upper_right, last)) {
+ upper_right = last;
+ }
+ if (!PickClosest(tlr, lower_right, last)) {
+ lower_right = last;
+ }
+ }
+ blobl[ti].upper_left = upper_left;
+ blobl[ti].upper_right = upper_right;
+ blobl[ti].lower_right = lower_right;
+ blobl[ti].lower_left = lower_left;
+
+ double error = CheckHistogram(&blobl[ti], upper_left, upper_right,
+ lower_right, lower_left);
+ const double k_hist_threshold = 0.05;
+ if (error < k_hist_threshold) {
+ ret.emplace_back(blobl[ti]);
+ }
+ }
+
+ return ret;
+}
+
+double HistogramBlobFilter::CheckHistogram(SelectedBlob *blob,
+ const Vector<2> &ul,
+ const Vector<2> &ur,
+ const Vector<2> &lr,
+ const Vector<2> &ll) {
+ // found horiz histogram
+ std::vector<double> hist_lr(hist_size_);
+ // step size along left edge
+ Vector<2> delta_left = (ul - ll) * (hist_step_);
+ // step size along right edge
+ Vector<2> delta_right = (ur - lr) * (hist_step_);
+ // sum each left to right line for the histogram
+ Vector<2> s;
+ Vector<2> e;
+ for (int i = 0; i < hist_size_; i++) {
+ s = ll + (i * delta_left);
+ e = lr + (i * delta_right);
+ hist_lr[i] = calcHistComponent(&blob->blob, s, e);
+ 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});
+ }
+ }
+ double check_vert_up = L22_dist(hist_size_, vert_hist_, hist_lr);
+ double check_vert_fliped = L22_dist(hist_size_, vert_hist_fliped_, hist_lr);
+
+ // found vert histogram
+ std::vector<double> hist_ub(hist_size_);
+ // step size along bottom edge
+ Vector<2> delta_bottom = (ll - lr) * (hist_step_);
+ // step size along top edge
+ Vector<2> delta_top = (ul - ur) * (hist_step_);
+ // sum each top to bottom line for the histogram
+ for (int i = 0; i < hist_size_; i++) {
+ s = ur + (i * delta_top);
+ e = lr + (i * delta_bottom);
+ hist_ub[i] = calcHistComponent(&blob->blob, s, e);
+ 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});
+ }
+ }
+ double check_horiz = L22_dist(hist_size_, horiz_hist_, hist_ub);
+
+ 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});
+ A = blob->upper_right + Vector<2>(-10, 10);
+ B = blob->upper_right - Vector<2>(-10, 10);
+ overlay_->add_line(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});
+ A = blob->lower_left + Vector<2>(-10, 10);
+ B = blob->lower_left - Vector<2>(-10, 10);
+ overlay_->add_line(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.
+ double check_vert;
+ if (check_vert_up < check_vert_fliped) {
+ // normal one is better, leave it alone
+ check_vert = check_vert_up;
+ } else {
+ check_vert = check_vert_fliped;
+ blob->Flip(fmt_);
+ }
+ 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});
+ A = blob->upper_right + Vector<2>(10, 10);
+ B = blob->upper_right - Vector<2>(10, 10);
+ overlay_->add_line(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});
+ A = blob->lower_left + Vector<2>(10, 10);
+ B = blob->lower_left - Vector<2>(10, 10);
+ overlay_->add_line(A, B, {255, 0, 255});
+ }
+
+ // average the two distances
+ double check = (check_vert + check_horiz) / (2.0 * hist_size_);
+ return check;
+}
+
+double HistogramBlobFilter::calcHistComponent(const RangeImage *blob,
+ const Vector<2> &start,
+ const Vector<2> &end) {
+ int startx = (int)std::floor(start.x());
+ int endx = (int)std::floor(end.x());
+ int starty = (int)std::floor(start.y()) - blob->mini();
+ int endy = (int)std::floor(end.y()) - blob->mini();
+ int dx = std::abs(endx - startx);
+ int dy = std::abs(endy - starty);
+ int sx = (startx < endx) ? 1 : -1;
+ int sy = (starty < endy) ? 1 : -1;
+ int error = dx - dy;
+
+ int total = 0;
+ int value = 0;
+ int total_error;
+ while (1) {
+ total++;
+ if (starty < 0 || starty >= (int)blob->ranges().size()) {
+ printf("starty (%d) size(%d)\n", starty, (int)blob->ranges().size());
+ fflush(stdout);
+ return 0;
+ }
+ const std::vector<ImageRange> &rangel = blob->ranges()[starty];
+ for (const ImageRange &range : rangel) {
+ if (startx >= range.st && startx <= range.ed) {
+ value++;
+ if (do_imgdbg_) {
+ image_->get_px(startx, starty + blob->mini()) = {255, 255, 255};
+ }
+ }
+ }
+
+ // bresenham logic to move along a line
+ if (startx == endx && starty == endy) break;
+ total_error = 2 * error;
+ if (total_error > -dy) {
+ error -= dy;
+ startx += sx;
+ }
+ if (total_error < dx) {
+ error += dx;
+ starty += sy;
+ }
+ }
+ return (double)value / (double)total;
+}
+
+void HistogramBlobFilter::MakeGoalHist(bool is_90) {
+ // calculate a desired histogram before we start
+ double targ_height = 14.0;
+ double targ_width = 20.0;
+ double tape_width = 2.0;
+ horiz_hist_.resize(hist_size_);
+ vert_hist_fliped_.resize(hist_size_);
+ vert_hist_.resize(hist_size_);
+ int j = 0;
+ for (double i = 0; j < hist_size_; i += hist_step_) {
+ if (is_90) {
+ assert(false);
+ } else {
+ if (i < (tape_width / targ_height)) {
+ vert_hist_[j] = 1.0;
+ } else {
+ vert_hist_[j] = 2 * tape_width / targ_width;
+ }
+
+ if (i < tape_width / targ_width || i > 1.0 - (tape_width / targ_width)) {
+ horiz_hist_[j] = 1.0;
+ } else {
+ horiz_hist_[j] = tape_width / targ_height;
+ }
+ }
+ j++;
+ }
+ for (int i = 0; i < hist_size_; i++) {
+ vert_hist_fliped_[hist_size_ - i - 1] = vert_hist_[i];
+ }
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/y2016/vision/blob_filters.h b/y2016/vision/blob_filters.h
new file mode 100644
index 0000000..85d2c94
--- /dev/null
+++ b/y2016/vision/blob_filters.h
@@ -0,0 +1,202 @@
+#ifndef Y2016_VISION_BLOB_FILTERS_H_
+#define Y2016_VISION_BLOB_FILTERS_H_
+
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+struct SelectedBlob {
+ SelectedBlob(const RangeImage &blob_inp) : blob(blob_inp) {}
+ RangeImage blob;
+ Vector<2> upper_left;
+ Vector<2> upper_right;
+ Vector<2> lower_right;
+ Vector<2> lower_left;
+
+ void Flip(ImageFormat fmt) {
+ auto image_width = fmt.w;
+ auto image_height = fmt.h;
+ blob.Flip(fmt);
+
+ Vector<2> tmp = lower_right;
+ lower_right = upper_left;
+ upper_left = tmp;
+ tmp = lower_left;
+ lower_left = upper_right;
+ upper_right = tmp;
+
+ // now flip the box
+ lower_right = Vector<2>(image_width - lower_right.x(),
+ image_height - lower_right.y());
+ upper_right = Vector<2>(image_width - upper_right.x(),
+ image_height - upper_right.y());
+ lower_left =
+ Vector<2>(image_width - lower_left.x(), image_height - lower_left.y());
+ upper_left =
+ Vector<2>(image_width - upper_left.x(), image_height - upper_left.y());
+ }
+};
+
+class CornerFinder {
+ public:
+ CornerFinder(float merge_rate, int min_line_length)
+ : merge_rate_(merge_rate), min_len_(min_line_length) {}
+
+ CornerFinder() : CornerFinder(1.0, 25) {}
+
+ // score how well the line matches the points. Lower score is better.
+ double LineScore(Vector<2> A, Vector<2> B, FittedLine line);
+
+ // We want to save the "best" two lines. Here we will difine that as the
+ // longest
+ // two we see.
+ void EnqueueLine(std::vector<FittedLine> *list, FittedLine line);
+ // Lines come in as two sides of the tape, we will flip them around so they
+ // have the same orientation then avrage the two end points to get a line in
+ // the center.
+ Segment<2> MakeLine(const std::vector<FittedLine> &list);
+
+ // take the given blob and find lines to represent it, then return the
+ // target
+ // corners from those lines. Left first, then bottom.
+ std::vector<std::pair<Vector<2>, Vector<2>>> Find(
+ const std::vector<SelectedBlob> &blobl);
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ // Parker did some sort of optimization with the memory.
+ AnalysisAllocator alloc_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+
+ // how smooth do we want the lines
+ float merge_rate_;
+ // how short do we want the lines
+ int min_len_;
+};
+
+class BlobFilterBase {
+ public:
+ BlobFilterBase(int min_blob_area, int max_blob_area)
+ : min_area_(min_blob_area), max_area_(max_blob_area) {}
+
+ std::vector<SelectedBlob> PreFilter(const BlobList &blobl);
+
+ std::vector<SelectedBlob> FilterBlobs(const BlobList &blobl) {
+ return PostFilter(PreFilter(blobl));
+ }
+
+ // Enable overlay debugging.
+ void EnableOverlay(PixelLinesOverlay *overlay) {
+ do_overlay_ = true;
+ overlay_ = overlay;
+ }
+
+ private:
+ virtual std::vector<SelectedBlob> PostFilter(
+ std::vector<SelectedBlob> blobl) = 0;
+
+ protected:
+ // absolute minimum for even looking at a blob.
+ int min_area_;
+ // absolute maximum for even looking at a blob.
+ int max_area_;
+
+ // check if we shuld draw the overlay
+ bool do_overlay_ = false;
+ PixelLinesOverlay *overlay_ = NULL;
+};
+
+class HistogramBlobFilter : public BlobFilterBase {
+ public:
+ HistogramBlobFilter(ImageFormat fmt, int hist_size, int min_blob_area,
+ int max_blob_area)
+ : BlobFilterBase(min_blob_area, max_blob_area),
+ fmt_(fmt),
+ hist_size_(hist_size),
+ hist_step_(1.0 / (double)hist_size) {
+ MakeGoalHist(false);
+ }
+
+ // Enable image debugging.
+ void EnableImageHist(ImagePtr *img) {
+ do_imgdbg_ = true;
+ image_ = img;
+ }
+
+ private:
+ // Returns the point closest to the goal point.
+ bool PickClosest(const Vector<2> &goal, const Vector<2> &A,
+ const Vector<2> &B);
+ // Main filter function.
+ std::vector<SelectedBlob> PostFilter(std::vector<SelectedBlob> blobl);
+
+ // calc and compare the histograms to the desired
+ double CheckHistogram(SelectedBlob *blob, const Vector<2> &ul,
+ const Vector<2> &ur, const Vector<2> &lr,
+ const Vector<2> &ll);
+
+ // sum over values between these two points and normalize
+ // see Bresenham's Line Algorithm for the logic of moving
+ // over all the pixels between these two points.
+ double calcHistComponent(const RangeImage *blob, const Vector<2> &start,
+ const Vector<2> &end);
+
+ void MakeGoalHist(bool is_90);
+
+ // just a distance function
+ double chiSquared(int length, double *histA, double *histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = *(histB + i) - *(histA + i);
+ sum += (diff * diff) / *(histA + i);
+ }
+ return sum;
+ }
+
+ // squared euclidiean dist function
+ double L22_dist(int length, std::vector<double> &histA,
+ std::vector<double> histB) {
+ double sum = 0;
+ for (int i = 0; i < length; i++) {
+ double diff = histB[i] - histA[i];
+ sum += (diff * diff);
+ }
+ return sum;
+ }
+
+ // size of image
+ ImageFormat fmt_;
+ // Number of elements in eaach histogram.
+ int hist_size_;
+ // Percent step of the above size.
+ double hist_step_;
+ // histogram summing in y.
+ std::vector<double> horiz_hist_;
+ // histogram summing in x direction.
+ std::vector<double> vert_hist_;
+ // histogram summing in x from greatest to least.
+ std::vector<double> vert_hist_fliped_;
+ bool do_imgdbg_ = false;
+ ImagePtr *image_ = NULL;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // Y2016_VISION_BLOB_FILTERS_H_
diff --git a/y2016/vision/stereo_geometry.h b/y2016/vision/stereo_geometry.h
index 93f3892..a758f1c 100644
--- a/y2016/vision/stereo_geometry.h
+++ b/y2016/vision/stereo_geometry.h
@@ -3,8 +3,8 @@
#include <string>
-#include "aos/vision/math/vector.h"
#include "aos/common/logging/logging.h"
+#include "aos/vision/math/vector.h"
#include "y2016/vision/calibration.pb.h"
diff --git a/y2016/vision/target_receiver.cc b/y2016/vision/target_receiver.cc
index 7ade8e7..3e4465d 100644
--- a/y2016/vision/target_receiver.cc
+++ b/y2016/vision/target_receiver.cc
@@ -1,5 +1,5 @@
-#include <stdlib.h>
#include <netdb.h>
+#include <stdlib.h>
#include <unistd.h>
#include <array>
@@ -10,19 +10,19 @@
#include <thread>
#include <vector>
-#include "aos/linux_code/init.h"
-#include "aos/common/time.h"
#include "aos/common/logging/logging.h"
#include "aos/common/logging/queue_logging.h"
-#include "aos/vision/events/udp.h"
#include "aos/common/mutex.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
+#include "y2016/constants.h"
+#include "y2016/vision/stereo_geometry.h"
#include "y2016/vision/vision.q.h"
#include "y2016/vision/vision_data.pb.h"
-#include "y2016/vision/stereo_geometry.h"
-#include "y2016/constants.h"
namespace y2016 {
namespace vision {
@@ -179,11 +179,12 @@
double angle, double last_angle,
::aos::vision::Vector<2> *interpolated_result,
double *interpolated_angle) {
- const double age_ratio =
- chrono::duration_cast<chrono::duration<double>>(
- older.capture_time() - newer.last_capture_time()).count() /
- chrono::duration_cast<chrono::duration<double>>(
- newer.capture_time() - newer.last_capture_time()).count();
+ const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
+ older.capture_time() - newer.last_capture_time())
+ .count() /
+ chrono::duration_cast<chrono::duration<double>>(
+ newer.capture_time() - newer.last_capture_time())
+ .count();
interpolated_result->Set(
newer_center.x() * age_ratio + (1 - age_ratio) * last_newer_center.x(),
newer_center.y() * age_ratio + (1 - age_ratio) * last_newer_center.y());
@@ -226,9 +227,11 @@
status->drivetrain_right_position = before.right;
} else {
const double age_ratio = chrono::duration_cast<chrono::duration<double>>(
- capture_time - before.time).count() /
+ capture_time - before.time)
+ .count() /
chrono::duration_cast<chrono::duration<double>>(
- after.time - before.time).count();
+ after.time - before.time)
+ .count();
status->drivetrain_left_position =
before.left * (1 - age_ratio) + after.left * age_ratio;
status->drivetrain_right_position =
@@ -313,7 +316,7 @@
CameraHandler left;
CameraHandler right;
- ::aos::vision::RXUdpSocket recv(8080);
+ ::aos::events::RXUdpSocket recv(8080);
char rawData[65507];
while (true) {
@@ -358,8 +361,8 @@
double last_angle_left;
double last_angle_right;
SelectTargets(left.last_target(), right.last_target(),
- &last_center_left, &last_center_right,
- &last_angle_left, &last_angle_right);
+ &last_center_left, &last_center_right, &last_angle_left,
+ &last_angle_right);
::aos::vision::Vector<2> filtered_center_left(0.0, 0.0);
::aos::vision::Vector<2> filtered_center_right(0.0, 0.0);
@@ -370,7 +373,8 @@
filtered_angle_left = angle_left;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- left.capture_time().time_since_epoch()).count();
+ left.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(left, right, center_right, last_center_right,
angle_right, last_angle_right,
&filtered_center_right, &filtered_angle_right);
@@ -379,7 +383,8 @@
filtered_angle_right = angle_right;
new_vision_status->target_time =
chrono::duration_cast<chrono::nanoseconds>(
- right.capture_time().time_since_epoch()).count();
+ right.capture_time().time_since_epoch())
+ .count();
CalculateFiltered(right, left, center_left, last_center_left,
angle_left, last_angle_left, &filtered_center_left,
&filtered_angle_left);
@@ -393,7 +398,8 @@
new_vision_status->right_image_timestamp =
right.target().image_timestamp();
new_vision_status->left_send_timestamp = left.target().send_timestamp();
- new_vision_status->right_send_timestamp = right.target().send_timestamp();
+ new_vision_status->right_send_timestamp =
+ right.target().send_timestamp();
new_vision_status->horizontal_angle = horizontal_angle;
new_vision_status->vertical_angle = vertical_angle;
new_vision_status->distance = distance;
diff --git a/y2016/vision/target_sender.cc b/y2016/vision/target_sender.cc
new file mode 100644
index 0000000..706c348
--- /dev/null
+++ b/y2016/vision/target_sender.cc
@@ -0,0 +1,242 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include "aos/common/logging/implementations.h"
+#include "aos/common/logging/logging.h"
+#include "aos/common/time.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/jpeg_routines.h"
+#include "aos/vision/image/reader.h"
+#include "y2016/vision/blob_filters.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/vision_data.pb.h"
+
+namespace y2016 {
+namespace vision {
+using aos::vision::ImageStreamEvent;
+using aos::vision::DataRef;
+using aos::events::TCPServer;
+using aos::vision::BlobLRef;
+using aos::vision::Vector;
+using aos::vision::Int32Codec;
+using aos::vision::BlobList;
+using aos::vision::RangeImage;
+using aos::vision::PixelRef;
+using aos::vision::ImageValue;
+using aos::vision::HistogramBlobFilter;
+using aos::vision::CornerFinder;
+using aos::vision::Int64Codec;
+using aos::events::TXUdpSocket;
+using aos::events::DataSocket;
+using aos::vision::ImageFormat;
+
+::camera::CameraParams GetCameraParams(const Calibration &calibration) {
+ return ::camera::CameraParams{.width = calibration.camera_image_width(),
+ .height = calibration.camera_image_height(),
+ .exposure = calibration.camera_exposure(),
+ .brightness = calibration.camera_brightness(),
+ .gain = calibration.camera_gain(),
+ .fps = calibration.camera_fps()};
+}
+
+int64_t Nanos(aos::monotonic_clock::duration time_diff) {
+ return std::chrono::duration_cast<std::chrono::nanoseconds>(time_diff)
+ .count();
+}
+
+int64_t NowNanos() {
+ return Nanos(aos::monotonic_clock::now().time_since_epoch());
+}
+
+inline bool FileExist(const std::string &name) {
+ struct stat buffer;
+ return (stat(name.c_str(), &buffer) == 0);
+}
+
+class ImageSender : public ImageStreamEvent {
+ public:
+ ImageSender(int camera_index, camera::CameraParams params,
+ const std::string &fname, const std::string &ipadder, int port)
+ : ImageStreamEvent(fname, params),
+ camera_index_(camera_index),
+ udp_serv_(ipadder, 8080),
+ tcp_serv_(port),
+ blob_filt_(ImageFormat(params.width, params.height), 40, 750, 250000),
+ finder_(0.25, 35) {
+ int index = 0;
+ while (true) {
+ std::string file = "./logging/blob_record_" + std::to_string(index) +
+ "_" + std::to_string(camera_index_) + ".dat";
+ if (FileExist(file)) {
+ index++;
+ } else {
+ printf("Logging to file (%s)\n", file.c_str());
+ ofst_.open(file);
+ assert(ofst_.is_open());
+ break;
+ }
+ }
+ }
+
+ ~ImageSender() { ofst_.close(); }
+
+ void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
+ int64_t timestamp = std::chrono::duration_cast<std::chrono::nanoseconds>(
+ tp.time_since_epoch())
+ .count();
+ DecodeJpeg(data, &image_);
+ auto fmt = image_.fmt();
+
+ RangeImage rimg = DoThreshold(image_.get(), [](PixelRef &px) {
+ return (px.g > 88);
+ });
+
+ // flip the right image as this camera is mount backward
+ if (camera_index_ == 0) {
+ rimg.Flip(fmt.w, fmt.h);
+ }
+
+ BlobList blobl = aos::vision::FindBlobs(rimg);
+ auto whatever = blob_filt_.FilterBlobs(blobl);
+
+ VisionData target;
+ target.set_camera_index(camera_index_);
+ target.set_image_timestamp(timestamp);
+
+ if (!whatever.empty()) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+ finder_.Find(whatever);
+
+ if (!corners.empty()) {
+ for (int i = 0; i < (int)corners.size(); i++) {
+ Target *pos = target.add_target();
+ pos->set_left_corner_x(corners[i].first.x());
+ pos->set_left_corner_y(corners[i].first.y());
+ pos->set_right_corner_x(corners[i].second.x());
+ pos->set_right_corner_y(corners[i].second.y());
+ }
+ }
+ }
+ target.set_send_timestamp(NowNanos());
+
+ // always send data
+ std::string dat;
+ if (target.SerializeToString(&dat)) {
+ if (print_once_) {
+ printf("Beginning data streaming camera %d...\n", camera_index_);
+ print_once_ = false;
+ }
+
+ // Send only target over udp.
+ udp_serv_.Send(dat.data(), dat.size());
+ }
+
+ // Write blob to file for logging.
+ int blob_size = CalculateSize(blobl);
+ int tmp_size = blob_size + sizeof(int32_t) + sizeof(uint64_t);
+ char *buf;
+ blob_data_.resize(tmp_size, 0);
+ {
+ buf = Int32Codec::Write(&blob_data_[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ SerializeBlob(blobl, buf);
+ }
+ ofst_.write(&blob_data_[0], tmp_size);
+
+ // Add blob debug
+ bool debug = true;
+ if (debug) {
+ target.set_raw(buf, blob_size);
+ if (target.SerializeToString(&dat)) {
+ tcp_serv_.Broadcast([&](DataSocket *client) { client->Emit(dat); });
+ }
+ }
+
+ bool timing = false;
+ if (timing) {
+ if (n_time > 0) {
+ auto now = aos::monotonic_clock::now();
+ printf("%g, %g\n",
+ (((double)Nanos(now - tstart)) / (double)(n_time)) / 1e6,
+ (double)Nanos(now - tp) / 1e6);
+ } else {
+ tstart = aos::monotonic_clock::now();
+ }
+ ++n_time;
+ }
+ }
+
+ TCPServer<DataSocket> *GetTCPServ() { return &tcp_serv_; }
+
+ // print when we start
+ bool print_once_ = true;
+
+ // left or right camera
+ int camera_index_;
+
+ // udp socket on which to send to robot
+ TXUdpSocket udp_serv_;
+
+ // tcp socket on which to debug to laptop
+ TCPServer<DataSocket> tcp_serv_;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+
+ ImageValue image_;
+ std::string blob_data_;
+ std::ofstream ofst_;
+ aos::monotonic_clock::time_point tstart;
+ int n_time = 0;
+
+ private:
+};
+
+void RunCamera(int camera_index, camera::CameraParams params,
+ const std::string &device, const std::string &ip_addr,
+ int port) {
+ printf("Creating camera %d (%d, %d).\n", camera_index, params.width,
+ params.height);
+ ImageSender strm(camera_index, params, device, ip_addr, port);
+
+ aos::events::EpollLoop loop;
+ loop.Add(strm.GetTCPServ());
+ loop.Add(&strm);
+ printf("Running Camera (%d)\n", camera_index);
+ loop.Run();
+}
+
+} // namespace vision
+} // namespace y2016
+
+int main(int, char **) {
+ using namespace y2016::vision;
+ StereoGeometry stereo("./stereo_rig.calib");
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ std::thread cam0([stereo]() {
+ RunCamera(0, GetCameraParams(stereo.calibration()),
+ stereo.calibration().right_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ std::thread cam1([stereo]() {
+ RunCamera(1, GetCameraParams(stereo.calibration()),
+ stereo.calibration().left_camera_name(),
+ stereo.calibration().roborio_ip_addr(), 8082);
+ });
+ cam0.join();
+ cam1.join();
+
+ return EXIT_SUCCESS;
+}