Rest of 2016 vision code.
Vision2016Debug:
- Added live debug (debug_reciever);
- Added file replay (blob_stream_replay).
- Add gtk event code.
- Updated code and fixed compile errors after rebase.
- Added useful tools for reference. As per Austins directions.
Change-Id: I7c5e7df01eb09057178bcb99dd3e302ca274ac76
diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
index 9d63e8d..bee1dc9 100644
--- a/aos/vision/blob/stream_view.h
+++ b/aos/vision/blob/stream_view.h
@@ -10,30 +10,32 @@
namespace aos {
namespace vision {
-class BlobStreamViewer {
+class BlobStreamViewer : public DebugViewer {
public:
- BlobStreamViewer() : view_(false) {}
+ BlobStreamViewer() : DebugViewer(false) {}
+ explicit BlobStreamViewer(bool flip) : DebugViewer(flip) {}
+
void Submit(ImageFormat fmt, const BlobList &blob_list) {
SetFormatAndClear(fmt);
DrawBlobList(blob_list, {255, 255, 255});
}
inline void SetFormatAndClear(ImageFormat fmt) {
- if (fmt.w != ptr_.fmt().w || fmt.h != ptr_.fmt().h) {
+ if (!image_.fmt().Equals(fmt)) {
printf("resizing data: %d, %d\n", fmt.w, fmt.h);
- outbuf_.reset(new PixelRef[fmt.ImgSize()]);
- ptr_ = ImagePtr{fmt, outbuf_.get()};
- view_.UpdateImage(ptr_);
+ image_ = ImageValue(fmt);
+ UpdateImage(image_.get());
}
- memset(ptr_.data(), 0, fmt.ImgSize() * sizeof(PixelRef));
+ memset(image_.data(), 0, fmt.ImgSize() * sizeof(PixelRef));
}
inline void DrawBlobList(const BlobList &blob_list, PixelRef color) {
- for (const auto &blob : blob_list) {
- for (int i = 0; i < (int)blob.ranges.size(); ++i) {
- for (const auto &range : blob.ranges[i]) {
+ ImagePtr ptr = img();
+ for (const RangeImage &blob : blob_list) {
+ for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+ for (const auto &range : blob.ranges()[i]) {
for (int j = range.st; j < range.ed; ++j) {
- ptr_.get_px(j, i + blob.min_y) = color;
+ ptr.get_px(j, i + blob.min_y()) = color;
}
}
}
@@ -42,15 +44,16 @@
inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
PixelRef color2) {
+ ImagePtr ptr = img();
for (const auto &blob : blob_list) {
- for (int i = 0; i < (int)blob.ranges.size(); ++i) {
- for (const auto &range : blob.ranges[i]) {
+ for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+ for (const auto &range : blob.ranges()[i]) {
for (int j = range.st; j < range.ed; ++j) {
- auto px = ptr_.get_px(j, i + blob.min_y);
+ auto px = ptr.get_px(j, i + blob.min_y());
if (px.r == 0 && px.g == 0 && px.b == 0) {
- ptr_.get_px(j, i + blob.min_y) = color1;
+ ptr.get_px(j, i + blob.min_y()) = color1;
} else {
- ptr_.get_px(j, i + blob.min_y) = color2;
+ ptr.get_px(j, i + blob.min_y()) = color2;
}
}
}
@@ -58,13 +61,13 @@
}
}
- DebugViewer *view() { return &view_; }
+ // Backwards compatible.
+ DebugViewer *view() { return this; }
+
+ ImagePtr img() { return image_.get(); }
private:
- std::unique_ptr<PixelRef[]> outbuf_;
- ImagePtr ptr_;
-
- DebugViewer view_;
+ ImageValue image_;
};
} // namespace vision
diff --git a/aos/vision/events/BUILD b/aos/vision/events/BUILD
index e003dc9..2b5b9e7 100644
--- a/aos/vision/events/BUILD
+++ b/aos/vision/events/BUILD
@@ -1,3 +1,4 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
package(default_visibility = ["//visibility:public"])
cc_library(
@@ -56,3 +57,12 @@
'//aos/testing:googletest',
],
)
+
+gtk_dependent_cc_library(
+ name = "gtk_event",
+ srcs = ["gtk_event.cc"],
+ deps = [
+ ":epoll_events",
+ '@usr_repo//:gtk+-3.0',
+ ],
+)
diff --git a/aos/vision/events/epoll_events.cc b/aos/vision/events/epoll_events.cc
index f191259..e4f789f 100644
--- a/aos/vision/events/epoll_events.cc
+++ b/aos/vision/events/epoll_events.cc
@@ -42,17 +42,10 @@
}
event->ReadEvent();
}
-
- for (EpollWatcher *watcher : watchers_) {
- watcher->Wake();
- }
}
}
void EpollLoop::AddWait(EpollWait *wait) { waits_.push_back(wait); }
-void EpollLoop::AddWatcher(EpollWatcher *watcher) {
- watchers_.push_back(watcher);
-}
// Calculates the new timeout value to pass to epoll_wait.
int EpollLoop::CalculateTimeout() {
diff --git a/aos/vision/events/epoll_events.h b/aos/vision/events/epoll_events.h
index d7574f9..ee288c4 100644
--- a/aos/vision/events/epoll_events.h
+++ b/aos/vision/events/epoll_events.h
@@ -17,6 +17,7 @@
// Performs an asychronous wait using an EpollLoop.
//
// Note: this does not have very high resolution (sub-millisecond).
+// TODO(parker): This is mostly broken.
class EpollWait {
public:
virtual ~EpollWait() {}
@@ -37,10 +38,15 @@
int Recalculate(const monotonic_clock::time_point now) {
if (time_ < monotonic_clock::epoch()) return -1;
if (time_ <= now) {
- Done();
time_ = monotonic_clock::time_point(::std::chrono::seconds(-1));
- return -1;
+ Done();
}
+ // Duplicate above to allow Done to change itself.
+ if (time_ < monotonic_clock::epoch()) return -1;
+ if (time_ <= now) {
+ return -1;// Recalculate(now);
+ }
+
if (time_ - now > ::std::chrono::milliseconds(INT_MAX)) {
return INT_MAX;
} else {
@@ -75,17 +81,6 @@
EpollLoop *loop_ = nullptr;
};
-// Provides a way for code to be notified every time after events are handled by
-// an EpollLoop. This is mainly a hack for the GTK integration and testing;
-// think very carefully before using it anywhere else.
-class EpollWatcher {
- public:
- virtual ~EpollWatcher() {}
-
- // Called after events have been processed each time the event loop wakes up.
- virtual void Wake() = 0;
-};
-
// A file descriptor based event loop implemented with epoll.
class EpollLoop {
public:
@@ -95,7 +90,6 @@
// None of these take ownership of the passed-in objects.
void AddWait(EpollWait *wait);
void Add(EpollEvent *event);
- void AddWatcher(EpollWatcher *watcher);
// Delete event. Note that there are caveats here as this is
// not idiot proof.
@@ -111,6 +105,10 @@
// Loops forever, handling events.
void Run();
+ // Fuses with gtk_main().
+ // Note that the dep for this is separate: //aos/vision/events:gtk_event
+ void RunWithGtkMain();
+
private:
int epoll_fd() { return epoll_fd_.get(); }
@@ -118,7 +116,6 @@
::aos::ScopedFD epoll_fd_;
::std::vector<EpollWait *> waits_;
- ::std::vector<EpollWatcher *> watchers_;
};
} // namespace events
diff --git a/aos/vision/events/gtk_event.cc b/aos/vision/events/gtk_event.cc
new file mode 100644
index 0000000..e8b43aa
--- /dev/null
+++ b/aos/vision/events/gtk_event.cc
@@ -0,0 +1,68 @@
+#include <gtk/gtk.h>
+#include <gdk/gdk.h>
+#include <thread>
+#include <sys/epoll.h>
+#include <mutex>
+#include <condition_variable>
+
+#include "aos/vision/events/epoll_events.h"
+
+namespace aos {
+namespace events {
+
+void EpollLoop::RunWithGtkMain() {
+ int timeout;
+ static constexpr size_t kNumberOfEvents = 64;
+ epoll_event events[kNumberOfEvents];
+ int number_events = 0;
+
+ std::mutex m;
+ std::condition_variable cv;
+ bool all_events_handled = false;
+ auto handle_cb = [&]() {
+ {
+ std::unique_lock<std::mutex> lk(m);
+
+ for (int i = 0; i < number_events; i++) {
+ EpollEvent *event = static_cast<EpollEvent *>(events[i].data.ptr);
+ if ((events[i].events & ~(EPOLLIN | EPOLLPRI)) != 0) {
+ LOG(FATAL, "unexpected epoll events set in %x on %d\n",
+ events[i].events, event->fd());
+ }
+ event->ReadEvent();
+ }
+ timeout = CalculateTimeout();
+
+ all_events_handled = true;
+ }
+ cv.notify_one();
+ };
+ handle_cb();
+ using HandleCBType = decltype(handle_cb);
+
+ std::thread t([&]() {
+ std::unique_lock<std::mutex> lk(m);
+ while (true) {
+ cv.wait(lk, [&all_events_handled]{return all_events_handled;});
+ // Wait for handle_cb to be done.
+ number_events = PCHECK(epoll_wait(epoll_fd(), events, kNumberOfEvents, timeout));
+ all_events_handled = false;
+ // Trigger handle_cb on main_thread to avoid concurrency.
+ gdk_threads_add_idle(+[](gpointer user_data) -> gboolean {
+ auto& handle_cb = *reinterpret_cast<HandleCBType*>(user_data);
+ handle_cb();
+ return G_SOURCE_REMOVE;
+ }, &handle_cb);
+ }
+ });
+ gtk_main();
+
+ // TODO(parker): Allow concurrent proxy onto the normal thread just like Gtk
+ // in order to allow event loop fusion, and make event addition thread-safe.
+
+ // Avoid stack destructors (explicitly shutting down of the thread.)
+ exit(EXIT_SUCCESS);
+}
+
+} // namespace events
+} // namespace aos
diff --git a/aos/vision/events/tcp_client.h b/aos/vision/events/tcp_client.h
index e7b80a6..7ce01f7 100644
--- a/aos/vision/events/tcp_client.h
+++ b/aos/vision/events/tcp_client.h
@@ -1,5 +1,5 @@
-#ifndef _AOS_VISION_DEBUG_TCP_SERVER_H_
-#define _AOS_VISION_DEBUG_TCP_SERVER_H_
+#ifndef _AOS_VISION_DEBUG_TCP_CLIENT_H_
+#define _AOS_VISION_DEBUG_TCP_CLIENT_H_
#include "aos/vision/events/epoll_events.h"
@@ -19,4 +19,4 @@
} // namespace events
} // namespace aos
-#endif // _AOS_VISION_DEBUG_TCP_SERVER_H_
+#endif // _AOS_VISION_DEBUG_TCP_CLIENT_H_
diff --git a/aos/vision/image/jpeg_routines.cc b/aos/vision/image/jpeg_routines.cc
index e0c93dc..b6be41b 100644
--- a/aos/vision/image/jpeg_routines.cc
+++ b/aos/vision/image/jpeg_routines.cc
@@ -224,7 +224,7 @@
int step = cinfo.num_components * cinfo.image_width;
unsigned char *buffers[cinfo.image_height];
for (size_t i = 0; i < cinfo.image_height; ++i) {
- buffers[i] = reinterpret_cast<unsigned char *>(&out[offset]);
+ buffers[i] = &reinterpret_cast<unsigned char *>(out)[offset];
offset += step;
}
diff --git a/aos/vision/tools/BUILD b/aos/vision/tools/BUILD
new file mode 100644
index 0000000..56bbb5b
--- /dev/null
+++ b/aos/vision/tools/BUILD
@@ -0,0 +1,19 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+
+gtk_dependent_cc_binary(name = "jpeg_vision_test",
+ srcs = ["jpeg_vision_test.cc"],
+ deps = [
+ "//aos/common/logging:logging",
+ "//aos/common/logging:implementations",
+ "//aos/vision/math:vector",
+ "//aos/vision/image:reader",
+ "//aos/vision/image:jpeg_routines",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/blob:stream_view",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/events:gtk_event",
+ ],
+)
diff --git a/aos/vision/tools/jpeg_vision_test.cc b/aos/vision/tools/jpeg_vision_test.cc
new file mode 100644
index 0000000..37d9ffe
--- /dev/null
+++ b/aos/vision/tools/jpeg_vision_test.cc
@@ -0,0 +1,100 @@
+// This file is to collect data from a camera to use for debug and testing. This
+// should not placed on a robot. This is okay as it is a utility of limited use
+// only.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <netdb.h>
+#include <poll.h>
+#include <string.h>
+#include <gtk/gtk.h>
+#include <vector>
+#include <memory>
+#include <fstream>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/implementations.h"
+#include "aos/vision/math/vector.h"
+#include "aos/vision/image/reader.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/events/tcp_server.h"
+
+namespace aos {
+namespace vision {
+
+// Connects up a camera with our processing.
+class ChannelImageStream : public ImageStreamEvent {
+ public:
+ ChannelImageStream(const std::string &fname,
+ const camera::CameraParams ¶ms)
+ : ImageStreamEvent(fname, params), view_(true) {
+ // Lambda to record image data to a file on key press.
+ view_.view()->key_press_event = [this](uint32_t /*keyval*/) {
+ std::ofstream ofs("/tmp/test.jpg", std::ofstream::out);
+ ofs << prev_data_;
+ ofs.close();
+ };
+ }
+
+ // Handle an image from the camera.
+ void ProcessImage(DataRef data,
+ aos::monotonic_clock::time_point /*timestamp*/) {
+ ImageFormat fmt = GetFmt(data);
+ if (!fmt.Equals(view_.img().fmt())) view_.SetFormatAndClear(fmt);
+ if (!ProcessJpeg(data, view_.img().data())) return;
+
+ ImagePtr img_ptr = view_.img();
+ prev_data_ = data.to_string();
+
+
+ // Threshold the image with the given lambda.
+ RangeImage rimg = DoThreshold(img_ptr, [](PixelRef &px) {
+ if (px.g > 88) {
+ uint8_t min = std::min(px.b, px.r);
+ uint8_t max = std::max(px.b, px.r);
+ 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;
+ });
+
+ view_.DrawBlobList({rimg}, {255, 255, 255});
+
+ view_.Redraw();
+ }
+
+ private:
+ std::string prev_data_;
+
+ // responsible for handling drawing
+ BlobStreamViewer view_;
+};
+} // namespace aos
+} // namespace vision
+
+int main(int argc, char *argv[]) {
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stdout));
+ aos::events::EpollLoop loop;
+ gtk_init(&argc, &argv);
+
+ camera::CameraParams params = {.width = 640 * 2,
+ .height = 480 * 2,
+ .exposure = 10,
+ .brightness = 128,
+ .gain = 0,
+ .fps = 10};
+
+ aos::vision::ChannelImageStream strm1("/dev/video1", params);
+
+ loop.Add(&strm1);
+ loop.RunWithGtkMain();
+}
diff --git a/y2016/vision/BUILD b/y2016/vision/BUILD
index 59139e1..e8d0b17 100644
--- a/y2016/vision/BUILD
+++ b/y2016/vision/BUILD
@@ -1,5 +1,6 @@
load('/tools/build_rules/protobuf', 'proto_cc_library')
load('/aos/build/queues', 'queue_library')
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
queue_library(
name = 'vision_queue',
@@ -107,6 +108,7 @@
"//aos/vision/blob:hierarchical_contour_merge",
"//aos/vision/blob:codec",
],
+ visibility = ['//visibility:public'],
)
cc_binary(
@@ -129,3 +131,25 @@
'//aos/common:mutex',
],
)
+
+gtk_dependent_cc_binary(
+ name = "debug_receiver",
+ srcs = ["debug_receiver.cc"],
+ visibility = ['//visibility:public'],
+ deps = [
+ "//aos/vision/image:image_types",
+ "//aos/vision/image:jpeg_routines",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:tcp_client",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:gtk_event",
+ "//aos/vision/debug:debug_viewer",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:codec",
+ "//aos/vision/blob:stream_view",
+ ":stereo_geometry",
+ ":blob_filters",
+ ":vision_data",
+ ":calibration",
+ ],
+)
diff --git a/y2016/vision/debug_receiver.cc b/y2016/vision/debug_receiver.cc
new file mode 100644
index 0000000..7039c53
--- /dev/null
+++ b/y2016/vision/debug_receiver.cc
@@ -0,0 +1,193 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <netdb.h>
+#include <unistd.h>
+
+#include <vector>
+#include <memory>
+
+#include <gtk/gtk.h>
+#include "aos/vision/image/image_types.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/tcp_client.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/stream_view.h"
+
+#include "y2016/vision/vision_data.pb.h"
+#include "y2016/vision/stereo_geometry.h"
+#include "y2016/vision/blob_filters.h"
+
+using namespace aos::vision;
+
+class StereoViewer {
+ public:
+ StereoViewer(int width, int height)
+ : blob_filt_(ImageFormat(width, height), 40, 100, 250000) {
+ overlays_.push_back(&overlay_);
+ view_.view()->SetOverlays(&overlays_);
+
+ // Uncomment to enable blob_filt_ overlay.
+ // blob_filt_.EnableOverlay(&overlay_);
+ finder_.EnableOverlay(&overlay_);
+ }
+
+ virtual ~StereoViewer() {}
+
+ void SetBlob(int camera_index, BlobList &blobl) {
+ if (camera_index == 0) {
+ left_blobs.swap(blobl);
+ new_left = true;
+ } else {
+ right_blobs.swap(blobl);
+ new_right = true;
+ }
+ }
+
+ void Process(ImageFormat fmt) {
+ if (new_left && new_right) {
+ overlay_.Reset();
+ DrawCross(overlay_, Vector<2>(fmt.w / 2.0, fmt.h / 2.0), {0, 255, 0});
+
+ view_.SetFormatAndClear(fmt);
+ printf("right (%d) left (%d)\n", (int)left_blobs.size(),
+ (int)right_blobs.size());
+ std::vector<std::pair<Vector<2>, Vector<2>>> cornersA =
+ finder_.Find(blob_filt_.FilterBlobs(left_blobs));
+ std::vector<std::pair<Vector<2>, Vector<2>>> cornersB =
+ finder_.Find(blob_filt_.FilterBlobs(right_blobs));
+ view_.DrawBlobList(left_blobs, {255, 0, 0});
+ view_.DrawSecondBlobList(right_blobs, {0, 255, 0}, {0, 0, 255});
+ view_.view()->Redraw();
+ new_left = false;
+ new_right = false;
+ }
+ }
+
+ private:
+ void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
+ overlay.add_line(Vector<2>(center.x() - 50, center.y()),
+ Vector<2>(center.x() + 50, center.y()), color);
+ overlay.add_line(Vector<2>(center.x(), center.y() - 50),
+ Vector<2>(center.x(), center.y() + 50), color);
+ }
+
+ // where we darw for debugging
+ PixelLinesOverlay overlay_;
+
+ // container for viewer
+ std::vector<OverlayBase *> overlays_;
+ BlobStreamViewer view_;
+
+ bool new_left = false;
+ BlobList left_blobs;
+ bool new_right = false;
+ BlobList right_blobs;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+};
+
+class BufferedLengthDelimReader {
+ public:
+ union data_len {
+ uint32_t len;
+ char buf[4];
+ };
+ BufferedLengthDelimReader() {
+ num_read_ = 0;
+ img_read_ = -1;
+ }
+ template <typename Lamb>
+ void up(int fd, Lamb lam) {
+ ssize_t count;
+ if (img_read_ < 0) {
+ count = read(fd, &len_.buf[num_read_], sizeof(len_.buf) - num_read_);
+ if (count < 0) return;
+ num_read_ += count;
+ if (num_read_ < 4) return;
+ num_read_ = 0;
+ img_read_ = 0;
+ data_.clear();
+ if (len_.len > 200000) {
+ printf("bad size: %d\n", len_.len);
+ exit(-1);
+ }
+ data_.resize(len_.len);
+ } else {
+ count = read(fd, &data_[img_read_], len_.len - img_read_);
+ if (count < 0) return;
+ img_read_ += count;
+ if (img_read_ < (int)len_.len) return;
+ lam(DataRef{&data_[0], len_.len});
+ img_read_ = -1;
+ }
+ }
+ private:
+ data_len len_;
+ int num_read_;
+ std::vector<char> data_;
+ int img_read_;
+};
+
+class ProtoClient : public aos::events::TcpClient {
+ public:
+ ProtoClient(int camera_index, int width, int height, const char *hostname,
+ int portno, StereoViewer *stereo)
+ : aos::events::TcpClient(hostname, portno),
+ camera_index_(camera_index),
+ fmt_(width, height),
+ stereo_(stereo) {}
+
+ void ReadEvent() override {
+ read_.up(fd(), [&](DataRef data) {
+ BlobList blobl;
+ y2016::vision::VisionData target;
+ if (target.ParseFromArray(data.data(), data.size())) {
+ auto &raw = target.raw();
+ ParseBlobList(&blobl, raw.data());
+ stereo_->SetBlob(camera_index_, blobl);
+ stereo_->Process(fmt_);
+ }
+ });
+ }
+
+ int camera_index_;
+
+ ImageFormat fmt_;
+
+ BufferedLengthDelimReader read_;
+ std::unique_ptr<PixelRef[]> outbuf;
+ ImagePtr ptr;
+ StereoViewer *stereo_;
+
+ private:
+};
+
+int main(int argc, char *argv[]) {
+ aos::events::EpollLoop loop;
+ gtk_init(&argc, &argv);
+
+ y2016::vision::Calibration calib =
+ y2016::vision::StereoGeometry("competition").calibration();
+
+ StereoViewer stereo(calib.camera_image_width(), calib.camera_image_height());
+
+ ProtoClient client0(0, calib.camera_image_width(),
+ calib.camera_image_height(),
+ calib.jetson_ip_addr().data(), 8082, &stereo);
+ ProtoClient client1(1, calib.camera_image_width(),
+ calib.camera_image_height(),
+ calib.jetson_ip_addr().data(), 8083, &stereo);
+
+ loop.Add(&client0);
+ loop.Add(&client1);
+ loop.RunWithGtkMain();
+ return EXIT_SUCCESS;
+}
diff --git a/y2016/vision/tools/BUILD b/y2016/vision/tools/BUILD
new file mode 100644
index 0000000..a9e25ac
--- /dev/null
+++ b/y2016/vision/tools/BUILD
@@ -0,0 +1,17 @@
+load('/tools/build_rules/gtk_dependent', 'gtk_dependent_cc_binary', 'gtk_dependent_cc_library')
+
+gtk_dependent_cc_binary(name = "blob_stream_replay",
+ srcs = ["blob_stream_replay.cc"],
+ deps = [
+ "//aos/vision/image:reader",
+ "//aos/vision/image:jpeg_routines",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:gtk_event",
+ "//aos/vision/events:tcp_server",
+ "//aos/vision/debug:debug_viewer",
+ "//aos/vision/blob:range_image",
+ "//aos/vision/blob:stream_view",
+ "//y2016/vision:blob_filters",
+ ],
+)
diff --git a/y2016/vision/tools/blob_stream_replay.cc b/y2016/vision/tools/blob_stream_replay.cc
new file mode 100644
index 0000000..f972fae
--- /dev/null
+++ b/y2016/vision/tools/blob_stream_replay.cc
@@ -0,0 +1,607 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <vector>
+#include <memory>
+#include <endian.h>
+#include <fstream>
+#include <gdk/gdk.h>
+#include <gtk/gtk.h>
+
+#include "aos/vision/image/reader.h"
+#include "aos/vision/image/jpeg_routines.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/events/epoll_events.h"
+#include "aos/vision/events/tcp_server.h"
+#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/blob/stream_view.h"
+#include "y2016/vision/blob_filters.h"
+// #include "y2016/vision/process_targets.h"
+
+namespace y2016 {
+namespace vision {
+using namespace aos::vision;
+
+::aos::vision::Vector<2> CreateCenterFromTarget(double lx, double ly, double rx, double ry) {
+ return ::aos::vision::Vector<2>((lx + rx) / 2.0, (ly + ry) / 2.0);
+}
+
+double TargetWidth(double lx, double ly, double rx, double ry) {
+ double dx = lx - rx;
+ double dy = ly - ry;
+ return ::std::hypot(dx, dy);
+}
+
+void SelectTargets(std::vector<std::pair<Vector<2>, Vector<2>>>& left_target,
+ std::vector<std::pair<Vector<2>, Vector<2>>>&right_target,
+ ::aos::vision::Vector<2> *center_left,
+ ::aos::vision::Vector<2> *center_right) {
+ // No good targets. Let the caller decide defaults.
+ if (right_target.size() == 0 || left_target.size() == 0) {
+ return;
+ }
+
+ // Only one option, we have to go with it.
+ if (right_target.size() == 1 && left_target.size() == 1) {
+ *center_left =
+ CreateCenterFromTarget(left_target[0].first.x(), left_target[0].first.y(),
+ left_target[0].second.x(), left_target[0].second.y());
+ *center_right = CreateCenterFromTarget(
+ right_target[0].first.x(), right_target[0].first.y(), right_target[0].second.x(),
+ right_target[0].second.y());
+ return;
+ }
+
+ // Now we have to make a decision.
+ double min_angle = -1.0;
+ int left_index = 0;
+ // First pick the widest target from the left.
+ for (size_t i = 0; i < left_target.size(); i++) {
+ const double h = left_target[i].first.y() -
+ left_target[i].second.y();
+ const double wid1 = TargetWidth(left_target[i].first.x(),
+ left_target[i].first.y(),
+ left_target[i].second.x(),
+ left_target[i].second.y());
+ const double angle = h / wid1;
+ if (min_angle == -1.0 || ::std::abs(angle) < ::std::abs(min_angle)) {
+ min_angle = angle;
+ left_index = i;
+ }
+ }
+ // Calculate the angle of the bottom edge for the left.
+ double h = left_target[left_index].first.y() -
+ left_target[left_index].second.y();
+
+ double good_ang = min_angle;
+ double min_ang_err = -1.0;
+ int right_index = -1;
+ // Now pick the bottom edge angle from the right that lines up best with the left.
+ for (size_t j = 0; j < right_target.size(); j++) {
+ double wid2 = TargetWidth(right_target[j].first.x(),
+ right_target[j].first.y(),
+ right_target[j].second.x(),
+ right_target[j].second.y());
+ h = right_target[j].first.y() -
+ right_target[j].second.y();
+ double ang = h/ wid2;
+ double ang_err = ::std::abs(good_ang - ang);
+ if (min_ang_err == -1.0 || min_ang_err > ang_err) {
+ min_ang_err = ang_err;
+ right_index = j;
+ }
+ }
+
+ *center_left =
+ CreateCenterFromTarget(left_target[left_index].first.x(),
+ left_target[left_index].first.y(),
+ left_target[left_index].second.x(),
+ left_target[left_index].second.y());
+ *center_right =
+ CreateCenterFromTarget(right_target[right_index].first.x(),
+ right_target[right_index].first.y(),
+ right_target[right_index].second.x(),
+ right_target[right_index].second.y());
+}
+
+
+long GetFileSize(std::string filename) {
+ struct stat stat_buf;
+ int rc = stat(filename.c_str(), &stat_buf);
+ return rc == 0 ? stat_buf.st_size : -1;
+}
+
+class OutputFile {
+ public:
+ OutputFile(const std::string &fname) : ofs(fname, std::ofstream::out) {}
+
+ void Emit(const BlobList &blobl, int64_t timestamp) {
+ int tmp_size = CalculateSize(blobl) + sizeof(int32_t) + sizeof(uint64_t);
+ tmp_buf.resize(tmp_size, 0);
+ {
+ char *buf = Int64Codec::Write(&tmp_buf[0], tmp_size);
+ buf = Int64Codec::Write(buf, timestamp);
+ SerializeBlob(blobl, buf);
+ }
+ ofs.write(&tmp_buf[0], tmp_size);
+ printf("blob_size: %d\n", tmp_size);
+ }
+
+ std::vector<char> tmp_buf;
+
+ std::ofstream ofs;
+};
+
+class InputFile {
+ public:
+ InputFile(const std::string &fname)
+ : ifs_(fname, std::ifstream::in), len_(GetFileSize(fname)) {
+ if (len_ <= 0) {
+ printf("File (%s) not found. Size (%d)\n", fname.c_str(), (int)len_);
+ fflush(stdout);
+ }
+ assert(len_ > 0);
+ tmp_buf_.resize(len_, 0);
+ ifs_.read(&tmp_buf_[0], len_);
+ buf_ = &tmp_buf_[0];
+ }
+
+ bool ReadNext(BlobList *blob_list, uint64_t *timestamp) {
+ if (buf_ - &tmp_buf_[0] >= len_) return false;
+ if (prev_ != nullptr) prev_frames_.emplace_back(prev_);
+ prev_ = buf_;
+ buf_ += sizeof(uint32_t);
+ *timestamp = Int64Codec::Read(&buf_);
+// auto* buf_tmp = buf_;
+ buf_ = ParseBlobList(blob_list, buf_);
+// fprintf(stderr, "read frame: %lu, buf_size: %lu\n", *timestamp, buf_ - buf_tmp);
+ return true;
+ }
+
+ bool ReadPrev(BlobList *blob_list, uint64_t *timestamp) {
+ if (prev_frames_.empty()) return false;
+ buf_ = prev_frames_.back();
+ prev_frames_.pop_back();
+ buf_ += sizeof(uint32_t);
+ *timestamp = Int64Codec::Read(&buf_);
+ buf_ = ParseBlobList(blob_list, buf_);
+ prev_ = nullptr;
+ return true;
+ }
+
+ private:
+ std::vector<const char *> prev_frames_;
+ const char *buf_;
+ const char *prev_ = nullptr;
+ std::ifstream ifs_;
+
+ long len_;
+ std::vector<char> tmp_buf_;
+};
+
+class BlobStreamFrame {
+ public:
+ BlobList blob_list;
+ uint64_t timestamp;
+ void ReadNext(InputFile *fin) {
+ blob_list.clear();
+ if (!fin->ReadNext(&blob_list, ×tamp)) {
+ exit(0);
+ return;
+ }
+ }
+ bool ReadPrev(InputFile *fin) {
+ blob_list.clear();
+ return fin->ReadPrev(&blob_list, ×tamp);
+ }
+};
+
+const char *kHudText =
+ "commands:\n"
+ " SPACE - pause\n"
+ " c - continue to next target\n"
+ " s - single step while paused\n"
+ " k - pause on next target frame\n"
+ " u - change window scaling\n"
+ " a - single step backward\n"
+ " q - quit\n"
+ " h - help\n";
+
+class NetworkForwardingImageStream : public aos::events::EpollWait {
+ public:
+ NetworkForwardingImageStream(ImageFormat fmt, int debug_level,
+ const std::string &fname1,
+ const std::string &fname2)
+ : fmt_(fmt),
+ ifs1_(fname1),
+ ifs2_(fname2),
+ blob_filt_(fmt, 40, 750, 250000),
+ finder_(0.25, 35) {
+ text_overlay_.draw_fn =
+ [this](RenderInterface *render, double /*width*/, double /*height*/) {
+ render->SetSourceRGB(1.0, 1.0, 1.0);
+ if (hud_text) render->Text(20, 20, 0, 0, kHudText);
+ };
+
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(&text_overlay_);
+ view_.view()->SetOverlays(&overlays_);
+
+ if (debug_level > 0) {
+ finder_.EnableOverlay(&overlay_);
+ }
+ if (debug_level > 1) {
+ blob_filt_.EnableOverlay(&overlay_);
+ }
+
+ frame1.ReadNext(&ifs1_);
+ frame2.ReadNext(&ifs2_);
+
+ std::pair<int, int> skip =
+ TickFrame(std::max(frame1.timestamp, frame2.timestamp));
+ printf("Initialzation skipped (%d, %d)\n", skip.first, skip.second);
+
+ ms_event_delta_ = 20;
+ play_forward = true;
+ paused = false;
+ single_step = false;
+ pause_on_next_target = true;
+ continue_to_next_target = false;
+ view_.view()->SetScale(scale_factor);
+ view_.view()->key_press_event = [this](uint32_t keyval) {
+ play_forward = true;
+ switch (keyval) {
+ case GDK_KEY_space:
+ paused = !paused;
+ pause_on_next_target = false;
+ continue_to_next_target = false;
+ break;
+ case GDK_KEY_c:
+ pause_on_next_target = true;
+ continue_to_next_target = true;
+ paused = false;
+ break;
+ case GDK_KEY_s:
+ single_step = true;
+ continue_to_next_target = false;
+ paused = true;
+ break;
+ case GDK_KEY_k:
+ pause_on_next_target = true;
+ continue_to_next_target = false;
+ paused = false;
+ break;
+ case GDK_KEY_u:
+ if (scale_factor == 1.0) {
+ scale_factor = 0.75;
+ view_.view()->SetScale(0.75);
+ } else {
+ scale_factor = 1.0;
+ view_.view()->SetScale(1.0);
+ view_.view()->MoveTo(150, -220);
+ }
+ break;
+ case GDK_KEY_a:
+ play_forward = false;
+ single_step = true;
+ paused = true;
+ break;
+ case GDK_KEY_q:
+ exit(0);
+ case GDK_KEY_h:
+ hud_text = !hud_text;
+ break;
+ default:
+ printf("pressed: %s\n", gdk_keyval_name(keyval));
+ }
+ };
+ }
+
+ double scale_factor = 1.0;
+ bool hud_text = true;
+ bool play_forward;
+ bool paused;
+ bool single_step;
+ bool pause_on_next_target;
+ bool continue_to_next_target;
+
+ std::string distance_text;
+
+ std::pair<int, int> TickFrame(uint64_t time) {
+ timestamp_ += time;
+ return TickToFrame(timestamp_);
+ }
+
+ std::pair<int, int> TickBackFrame(uint64_t time) {
+ timestamp_ -= time;
+ return TickBackToFrame(timestamp_);
+ }
+
+ std::pair<int, int> TickToFrame(uint64_t timestamp) {
+ std::pair<int, int> skip(0, 0);
+ while (frame1.timestamp < timestamp) {
+ frame1.ReadNext(&ifs1_);
+ skip.first++;
+ }
+ while (frame2.timestamp < timestamp) {
+ frame2.ReadNext(&ifs2_);
+ skip.second++;
+ }
+ return skip;
+ }
+
+ std::pair<int, int> TickBackToFrame(uint64_t timestamp) {
+ std::pair<int, int> skip(0, 0);
+ while (frame1.timestamp >= timestamp) {
+ if (!frame1.ReadPrev(&ifs1_)) break;
+ skip.first++;
+ }
+ while (frame2.timestamp >= timestamp) {
+ if (!frame2.ReadPrev(&ifs2_)) break;
+ skip.second++;
+ }
+ frame1.ReadPrev(&ifs1_);
+ frame2.ReadPrev(&ifs2_);
+ return skip;
+ }
+ BlobStreamFrame frame1;
+ BlobStreamFrame frame2;
+ uint64_t timestamp_ = 0;
+
+ Vector<2> GetCenter(const BlobList &blob_list) {
+ std::vector<std::pair<Vector<2>, Vector<2>>> corners =
+ finder_.Find(blob_filt_.FilterBlobs(blob_list));
+
+ if (corners.size() == 1) {
+ Vector<2> center = (corners[0].first + corners[0].second) * (0.5);
+ return center;
+ }
+ return {0, 0};
+ }
+
+ void DrawSuperSpeed() {
+ PixelRef color = {0, 255, 255};
+ // S
+ overlay_.add_line(Vector<2>(200, 100), Vector<2>(100, 100), color);
+ overlay_.add_line(Vector<2>(100, 100), Vector<2>(100, 300), color);
+ overlay_.add_line(Vector<2>(100, 300), Vector<2>(200, 300), color);
+ overlay_.add_line(Vector<2>(200, 300), Vector<2>(200, 500), color);
+ overlay_.add_line(Vector<2>(200, 500), Vector<2>(100, 500), color);
+ // U
+ overlay_.add_line(Vector<2>(250, 100), Vector<2>(250, 500), color);
+ overlay_.add_line(Vector<2>(250, 500), Vector<2>(350, 500), color);
+ overlay_.add_line(Vector<2>(350, 500), Vector<2>(350, 100), color);
+ // P
+ overlay_.add_line(Vector<2>(400, 100), Vector<2>(400, 500), color);
+ overlay_.add_line(Vector<2>(400, 100), Vector<2>(500, 100), color);
+ overlay_.add_line(Vector<2>(500, 100), Vector<2>(500, 300), color);
+ overlay_.add_line(Vector<2>(500, 300), Vector<2>(400, 300), color);
+ // E
+ overlay_.add_line(Vector<2>(550, 100), Vector<2>(550, 500), color);
+ overlay_.add_line(Vector<2>(550, 100), Vector<2>(650, 100), color);
+ overlay_.add_line(Vector<2>(550, 300), Vector<2>(650, 300), color);
+ overlay_.add_line(Vector<2>(550, 500), Vector<2>(650, 500), color);
+ // R
+ overlay_.add_line(Vector<2>(700, 100), Vector<2>(700, 500), color);
+ overlay_.add_line(Vector<2>(700, 100), Vector<2>(800, 100), color);
+ overlay_.add_line(Vector<2>(800, 100), Vector<2>(800, 300), color);
+ overlay_.add_line(Vector<2>(800, 300), Vector<2>(700, 300), color);
+ overlay_.add_line(Vector<2>(700, 350), Vector<2>(800, 500), color);
+ // S
+ overlay_.add_line(Vector<2>(200, 550), Vector<2>(100, 550), color);
+ overlay_.add_line(Vector<2>(100, 550), Vector<2>(100, 750), color);
+ overlay_.add_line(Vector<2>(100, 750), Vector<2>(200, 750), color);
+ overlay_.add_line(Vector<2>(200, 750), Vector<2>(200, 950), color);
+ overlay_.add_line(Vector<2>(200, 950), Vector<2>(100, 950), color);
+ // P
+ overlay_.add_line(Vector<2>(250, 550), Vector<2>(250, 950), color);
+ overlay_.add_line(Vector<2>(250, 550), Vector<2>(350, 550), color);
+ overlay_.add_line(Vector<2>(350, 550), Vector<2>(350, 750), color);
+ overlay_.add_line(Vector<2>(350, 750), Vector<2>(250, 750), color);
+ // E
+ overlay_.add_line(Vector<2>(400, 550), Vector<2>(400, 950), color);
+ overlay_.add_line(Vector<2>(400, 550), Vector<2>(500, 550), color);
+ overlay_.add_line(Vector<2>(400, 750), Vector<2>(500, 750), color);
+ overlay_.add_line(Vector<2>(400, 950), Vector<2>(500, 950), color);
+ // E
+ overlay_.add_line(Vector<2>(550, 550), Vector<2>(550, 950), color);
+ overlay_.add_line(Vector<2>(550, 550), Vector<2>(650, 550), color);
+ overlay_.add_line(Vector<2>(550, 750), Vector<2>(650, 750), color);
+ overlay_.add_line(Vector<2>(550, 950), Vector<2>(650, 950), color);
+ // D
+ overlay_.add_line(Vector<2>(700, 550), Vector<2>(700, 950), color);
+ overlay_.add_line(Vector<2>(700, 550), Vector<2>(800, 575), color);
+ overlay_.add_line(Vector<2>(800, 575), Vector<2>(800, 925), color);
+ overlay_.add_line(Vector<2>(800, 925), Vector<2>(700, 950), color);
+ }
+
+ void UpdateNewTime(int new_delta) {
+ if (new_delta != ms_event_delta_) {
+ ms_event_delta_ = new_delta;
+ SetTime(::std::chrono::milliseconds(ms_event_delta_) + aos::monotonic_clock::now());
+ }
+ }
+
+ void Done() override {
+ SetTime(::std::chrono::milliseconds(ms_event_delta_) + aos::monotonic_clock::now());
+ if (paused && !single_step) return;
+ single_step = false;
+ frame_count_++;
+
+ while (true) {
+ overlay_.Reset();
+ view_.SetFormatAndClear(fmt_);
+ std::pair<int, int> skipped(1, 1);
+ // how far we will step to look for the next target
+ int nano_step = 300 * 1e6;
+ if (play_forward && seeking_target_) {
+ skipped = TickFrame(nano_step);
+ } else if (seeking_target_) {
+ skipped = TickBackFrame(nano_step);
+ } else if (play_forward) {
+ frame1.ReadNext(&ifs1_);
+ frame2.ReadNext(&ifs2_);
+ } else {
+ frame1.ReadPrev(&ifs1_);
+ frame2.ReadPrev(&ifs2_);
+ }
+ // printf("skipped (%d, %d)\n", skipped.first, skipped.second);
+
+ std::vector<std::pair<Vector<2>, Vector<2>>> corner1 =
+ finder_.Find(blob_filt_.FilterBlobs(frame1.blob_list));
+ std::vector<std::pair<Vector<2>, Vector<2>>> corner2 =
+ finder_.Find(blob_filt_.FilterBlobs(frame2.blob_list));
+
+ Vector<2> cent1;
+ Vector<2> cent2;
+ SelectTargets(corner1, corner2, ¢1, ¢2);
+
+ /*
+ int target_count_;
+ if (cent1 == Vector<2>(0, 0) && cent2 == Vector<2>(0, 0)) {
+ missed_count_ += std::min(skipped.first, skipped.second);
+ if (missed_count_ > 15) {
+ seeking_target_ = true;
+ DrawSuperSpeed();
+ SetTime(aos::vision::MsTime(1));
+ if (line_break_) {
+ printf("_-_-_-%d_-_-_-_\n", target_count_);
+ target_count_++;
+ line_break_ = false;
+ }
+ if (continue_to_next_target) {
+ continue_to_next_target = false;
+ }
+ continue;
+ }
+ } else {
+ missed_count_ = 0;
+ }
+ */
+
+ if (seeking_target_) {
+ if (play_forward) {
+ // Go back to the last time we didn't see a target and play from there.
+ TickBackFrame(nano_step);
+ seeking_target_ = false;
+ } else if (seeking_target_) {
+ TickFrame(nano_step);
+ seeking_target_ = false;
+ }
+ continue;
+ }
+
+ // comment out to turn off full blob drawing
+ view_.DrawBlobList(frame1.blob_list, {0, 0, 255});
+ view_.DrawSecondBlobList(frame2.blob_list, {0, 255, 0}, {0, 255, 255});
+
+ DrawCross(overlay_, Vector<2>(fmt_.w / 2.0, fmt_.h / 2.0), {255, 0, 0});
+
+ double timeFromEpoch =
+ 1e-9 * ((double)frame1.timestamp + (double)frame2.timestamp) / 2;
+ printf("timestamp: %g skew: %g\n", timeFromEpoch,
+ 1e-9 * ((double)frame1.timestamp - (double)frame2.timestamp));
+ /*
+ if (cent1 == Vector<2>(0, 0) && cent2 == Vector<2>(0, 0)) {
+ } else {
+ DrawCross(overlay_, cent1, {255, 255, 255});
+ DrawCross(overlay_, cent2, {255, 255, 255});
+ double x = (cent1.x() + cent2.x()) / 2.0;
+ DrawCross(overlay_, Vector<2>(x, fmt_.h / 2.0), {255, 255, 255});
+ SetTime(aos::vision::MsTime(100));
+ if (pause_on_next_target && !continue_to_next_target) {
+ paused = true;
+ pause_on_next_target = false;
+ }
+ line_break_ = true;
+ missed_count_ = 0;
+ }
+ fflush(stdout);
+ */
+ view_.view()->Redraw();
+ break;
+ }
+ }
+
+ void DrawCross(PixelLinesOverlay &overlay, Vector<2> center, PixelRef color) {
+ overlay.add_line(Vector<2>(center.x() - 25, center.y()),
+ Vector<2>(center.x() + 25, center.y()), color);
+ overlay.add_line(Vector<2>(center.x(), center.y() - 25),
+ Vector<2>(center.x(), center.y() + 25), color);
+ }
+
+ void AddTo(aos::events::EpollLoop *loop) {
+ Done();
+ loop->AddWait(this);
+ }
+
+ std::unique_ptr<PixelRef[]> outbuf;
+ ImagePtr ptr;
+
+ BlobStreamViewer view_;
+
+ private:
+ int ms_event_delta_ = 200;
+ public:
+ // basic image size
+ ImageFormat fmt_;
+
+ // where we darw for debugging
+ PixelLinesOverlay overlay_;
+
+ // Where we draw text on the screen.
+ LambdaOverlay text_overlay_;
+ // container for viewer
+ std::vector<OverlayBase *> overlays_;
+
+ InputFile ifs1_;
+ InputFile ifs2_;
+
+ // our blob processing object
+ HistogramBlobFilter blob_filt_;
+
+ // corner finder to align aiming
+ CornerFinder finder_;
+
+ // indicates we have lost a target
+ bool line_break_ = false;
+
+ // indicates we are looking for the next target
+ bool seeking_target_ = false;
+
+ int frame_count_ = 0;
+
+ // count how many frames we miss in a row.
+ int missed_count_ = 16;
+};
+}
+} // namespace y2016::vision
+
+int main(int argc, char *argv[]) {
+ using namespace y2016::vision;
+ aos::events::EpollLoop loop;
+ gtk_init(&argc, &argv);
+
+ if (argc != 3) {
+ printf("Wrong number of arguments. Got (%d) expected 3.\n", argc);
+ printf(
+ " arguments are debug level as {0, 1, 2} and then filename without the "
+ "{_0.dat,_1.dat} suffixes\n");
+ }
+
+ int dbg = std::stoi(argv[1]);
+
+ std::string file(argv[2]);
+ aos::vision::ImageFormat fmt = {640 * 2, 480 * 2};
+
+ printf("file (%s) dbg_lvl (%d)\n", file.c_str(), dbg);
+
+ std::string fname_path = file;
+ NetworkForwardingImageStream strm1(
+ fmt, dbg, fname_path + "_0.dat", fname_path + "_1.dat");
+ fprintf(stderr, "staring main\n");
+ strm1.AddTo(&loop);
+
+ fprintf(stderr, "staring main\n");
+ loop.RunWithGtkMain();
+}