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/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();
+}