Checking in debug_view, some extra missing utils, and the y2016 target_sender code.
Change-Id: I241947265da8f332c39862f4d0ddcdc2d29c7b68
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;
+}