Checking in blob routines.
Change-Id: I364331d6f9239763ccac492460ed752a0b16871f
diff --git a/aos/vision/blob/BUILD b/aos/vision/blob/BUILD
new file mode 100644
index 0000000..a024aba
--- /dev/null
+++ b/aos/vision/blob/BUILD
@@ -0,0 +1,106 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_library(
+ name = 'range_image',
+ hdrs = ['range_image.h'],
+ srcs = ['range_image.cc'],
+ deps = [
+ '//aos/vision/math:vector',
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:segment',
+ '//aos/vision/image:image_types',
+ '//third_party/eigen',
+ ],
+)
+
+cc_library(
+ name = 'region_alloc',
+ hdrs = ['region_alloc.h'],
+ deps = [
+ '//aos/common/logging',
+ ],
+)
+
+cc_library(
+ name = 'contour',
+ hdrs = ['contour.h'],
+ srcs = ['contour.cc'],
+ deps = [
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:segment',
+ ':range_image',
+ ':region_alloc',
+ ],
+)
+
+cc_library(
+ name = 'threshold',
+ hdrs = ['threshold.h'],
+ deps = [
+ ':range_image',
+ '//aos/vision/image:image_types',
+ ]
+)
+
+cc_library(
+ name = 'hierarchical_contour_merge',
+ hdrs = ['hierarchical_contour_merge.h'],
+ srcs = ['hierarchical_contour_merge.cc'],
+ deps = [
+ ':contour',
+ ':disjoint_set',
+ ':range_image',
+ '//third_party/eigen',
+ ]
+)
+
+cc_library(
+ name = 'disjoint_set',
+ hdrs = ['disjoint_set.h'],
+)
+
+cc_library(
+ name = 'find_blob',
+ hdrs = ['find_blob.h'],
+ srcs = ['find_blob.cc'],
+ deps = [
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:segment',
+ ':disjoint_set',
+ ':range_image',
+ '//third_party/eigen',
+ ]
+)
+
+cc_library(
+ name = 'codec',
+ hdrs = ['codec.h'],
+ srcs = ['codec.cc'],
+ deps = [
+ '//aos/vision/debug:overlay',
+ '//aos/vision/math:segment',
+ ':range_image',
+ '//third_party/eigen',
+ ],
+)
+
+cc_test(
+ name = 'codec_test',
+ srcs = ['codec_test.cc'],
+ deps = [
+ ':codec',
+ '//aos/testing:googletest',
+ ],
+)
+
+"""
+cc_library(
+ name = 'stream_view',
+ hdrs = ['stream_view.h'],
+ deps = [
+ ':range_image',
+ '//aos/vision/debug:debug_viewer',
+ '//aos/vision/image:image_types',
+ ],
+)
+"""
diff --git a/aos/vision/blob/codec.cc b/aos/vision/blob/codec.cc
new file mode 100644
index 0000000..51ae129
--- /dev/null
+++ b/aos/vision/blob/codec.cc
@@ -0,0 +1,59 @@
+#include "aos/vision/blob/codec.h"
+
+namespace aos {
+namespace vision {
+
+size_t CalculateSize(const BlobList &blob_list) {
+ size_t count = Int16Codec::kSize;
+ for (const auto &blob : blob_list) {
+ count += 2 * Int16Codec::kSize;
+ for (int i = 0; i < static_cast<int>(blob.ranges().size()); ++i) {
+ count +=
+ Int16Codec::kSize + 2 * Int16Codec::kSize * blob.ranges()[i].size();
+ }
+ }
+ return count;
+}
+
+void SerializeBlob(const BlobList &blob_list, char *data) {
+ data = Int16Codec::Write(data, static_cast<uint16_t>(blob_list.size()));
+ for (const auto &blob : blob_list) {
+ data = Int16Codec::Write(data, static_cast<uint16_t>(blob.min_y()));
+ data = Int16Codec::Write(data, static_cast<uint16_t>(blob.ranges().size()));
+ for (int i = 0; i < (int)blob.ranges().size(); ++i) {
+ data = Int16Codec::Write(data,
+ static_cast<uint16_t>(blob.ranges()[i].size()));
+ for (const auto &range : blob.ranges()[i]) {
+ data = Int16Codec::Write(data, static_cast<uint16_t>(range.st));
+ data = Int16Codec::Write(data, static_cast<uint16_t>(range.ed));
+ }
+ }
+ }
+}
+
+const char *ParseBlobList(BlobList *blob_list, const char *data) {
+ int num_items = Int16Codec::Read(&data);
+ blob_list->clear();
+ blob_list->reserve(num_items);
+ for (int i = 0; i < num_items; ++i) {
+ std::vector<std::vector<ImageRange>> ranges_image;
+ int min_y = Int16Codec::Read(&data);
+ int num_ranges = Int16Codec::Read(&data);
+ ranges_image.reserve(num_ranges);
+ for (int j = 0; j < num_ranges; ++j) {
+ ranges_image.emplace_back();
+ auto &ranges = ranges_image.back();
+ int num_sub_ranges = Int16Codec::Read(&data);
+ for (int k = 0; k < num_sub_ranges; ++k) {
+ int st = Int16Codec::Read(&data);
+ int ed = Int16Codec::Read(&data);
+ ranges.emplace_back(st, ed);
+ }
+ }
+ blob_list->emplace_back(min_y, std::move(ranges_image));
+ }
+ return data;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/codec.h b/aos/vision/blob/codec.h
new file mode 100644
index 0000000..c2bc30d
--- /dev/null
+++ b/aos/vision/blob/codec.h
@@ -0,0 +1,50 @@
+#ifndef _AOS_VISION_BLOB_CODEC_H_
+#define _AOS_VISION_BLOB_CODEC_H_
+
+#include <string>
+
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+template <typename T>
+struct IntCodec {
+ static constexpr size_t kSize = sizeof(T);
+ static inline char *Write(char *data, T ival) {
+ *(reinterpret_cast<T *>(data)) = ival;
+ return data + kSize;
+ }
+ static inline T Read(const char **data) {
+ auto datum = *(reinterpret_cast<const T *>(*data));
+ *data += kSize;
+ return datum;
+ }
+};
+
+using Int64Codec = IntCodec<uint64_t>;
+using Int32Codec = IntCodec<uint32_t>;
+using Int16Codec = IntCodec<uint16_t>;
+
+// Calculates bytes size of blob_list. This runs the encoding algorithm below
+// to get the exact size that SerializeBlob will require.
+// Consider just using SerializeBlobTo instead of these lower level routines.
+size_t CalculateSize(const BlobList &blob_list);
+// Serializes blob_list to data. Must be valid memory of size returned by
+// PredictSize.
+void SerializeBlob(const BlobList &blob_list, char *data);
+
+// Combines above to serialize to a string.
+inline void SerializeBlobTo(const BlobList &blob_list, std::string *out) {
+ size_t len = CalculateSize(blob_list);
+ out->resize(len, 0);
+ SerializeBlob(blob_list, &(*out)[0]);
+}
+
+// Parses a blob from data (Advancing data pointer by the size of the image).
+const char *ParseBlobList(BlobList *blob_list, const char *data);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_CODEC_H_
diff --git a/aos/vision/blob/codec_test.cc b/aos/vision/blob/codec_test.cc
new file mode 100644
index 0000000..53dc3bb
--- /dev/null
+++ b/aos/vision/blob/codec_test.cc
@@ -0,0 +1,37 @@
+#include "aos/vision/blob/codec.h"
+
+#include <algorithm>
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace vision {
+
+TEST(CodecTest, WriteRead) {
+ BlobList blobl;
+ {
+ std::vector<std::vector<ImageRange>> ranges;
+ ranges.emplace_back(std::vector<ImageRange>{{10, 11}});
+ ranges.emplace_back(std::vector<ImageRange>{{15, 17}});
+ ranges.emplace_back(std::vector<ImageRange>{{19, 30}});
+ blobl.emplace_back(RangeImage(10, std::move(ranges)));
+ }
+
+ {
+ std::vector<std::vector<ImageRange>> ranges;
+ ranges.emplace_back(std::vector<ImageRange>{{18, 19}});
+ ranges.emplace_back(std::vector<ImageRange>{{12, 13}});
+ ranges.emplace_back(std::vector<ImageRange>{{12, 17}});
+ blobl.emplace_back(RangeImage(13, std::move(ranges)));
+ }
+
+ std::string out;
+ SerializeBlobTo(blobl, &out);
+ BlobList blobl2;
+ size_t real_len = ParseBlobList(&blobl2, out.data()) - out.data();
+ EXPECT_EQ(ShortDebugPrint(blobl), ShortDebugPrint(blobl2));
+
+ EXPECT_EQ(real_len, CalculateSize(blobl));
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/contour.cc b/aos/vision/blob/contour.cc
new file mode 100644
index 0000000..6a56f9e
--- /dev/null
+++ b/aos/vision/blob/contour.cc
@@ -0,0 +1,195 @@
+#include "aos/vision/blob/contour.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+// Half-loop of a contour.
+// This represents a range-line in a range-image
+// at the current sweep of the range image propagation.
+struct HalfContour {
+ // start
+ ContourNode *st;
+ // end
+ ContourNode *ed;
+};
+
+// The following helper functions handle different cases for extending
+// overlapping ranges (from the first line to the next line).
+
+// Constructs a half-contour for the first line in a range-image.
+// This happens for ranges that have no overlaps in the previous line.
+// hc.st ----- hc.ed
+// This is symmetric to CloseRange.
+// stx is start_x; edx is end_x; alloc is the region allocator.
+HalfContour FwdRange(int y, int stx, int edx, AnalysisAllocator *alloc) {
+ ContourNode *st = alloc->cons_obj<ContourNode>(stx, y);
+ ContourNode *ed = st;
+ st->next = st;
+ for (int x = stx + 1; x < edx; x++) {
+ ed = alloc->cons_obj<ContourNode>(x, y, ed);
+ }
+ return HalfContour{st, ed};
+}
+
+// Connects hc.st to hc.ed assuming intervening range.
+// hc.st ----- hc.ed
+// This closes out the contour.
+void CloseRange(HalfContour hc, AnalysisAllocator *alloc) {
+ auto p_end = hc.ed;
+ auto p_cur = hc.st;
+ while (p_cur->pt.x < p_end->pt.x - 1) {
+ p_cur = p_cur->append(p_cur->pt.x + 1, p_cur->pt.y, alloc);
+ }
+ if (p_end->pt.x == p_cur->pt.x) {
+ // Remove duplicated pixel for length 1 ranges.
+ p_cur->next = p_end->next;
+ } else {
+ p_cur->next = p_end;
+ }
+}
+
+// Connects pst to the return value (r).
+// pst------
+// r------
+// cst is the x position of r.
+ContourNode *ExtendStart(ContourNode *pst, int cst, AnalysisAllocator *alloc) {
+ while (pst->pt.x < cst) {
+ pst = pst->append(pst->pt.x + 1, pst->pt.y, alloc);
+ }
+ pst = pst->append(pst->pt.x, pst->pt.y + 1, alloc);
+ while (pst->pt.x > cst) {
+ pst = pst->append(pst->pt.x - 1, pst->pt.y, alloc);
+ }
+ return pst;
+}
+
+// Connects pst to the return value (r)
+// ------- pst
+// --- r
+// cst is the x position of r.
+ContourNode *ExtendEnd(ContourNode *pst, int cst, AnalysisAllocator *alloc) {
+ while (pst->pt.x > cst) {
+ pst = pst->pappend(pst->pt.x - 1, pst->pt.y, alloc);
+ }
+ pst = pst->pappend(pst->pt.x, pst->pt.y + 1, alloc);
+ while (pst->pt.x < cst) {
+ pst = pst->pappend(pst->pt.x + 1, pst->pt.y, alloc);
+ }
+ return pst;
+}
+
+// Connects concave contour like this:
+//
+// --pst est--
+// ----------------
+void CapRange(ContourNode *pst, ContourNode *est, AnalysisAllocator *alloc) {
+ est = est->append(est->pt.x, est->pt.y + 1, alloc);
+ while (est->pt.x > pst->pt.x) {
+ est = est->append(est->pt.x - 1, est->pt.y, alloc);
+ }
+ est->next = pst;
+}
+
+// Constructs the starting range like so
+// Return value (r)
+//
+// ----------------------
+// ---r.st r.ed----
+HalfContour MakeCavity(int i, int sti, int edi, AnalysisAllocator *alloc) {
+ ContourNode *st = alloc->cons_obj<ContourNode>(sti, i);
+ ContourNode *ed = st;
+ for (int x = sti + 1; x < edi; x++) {
+ ed = ed->append(x, i, alloc);
+ }
+ return HalfContour{st, ed};
+}
+} // namespace
+
+// Sweepline conversion from RangeImage to ContourNode loop.
+// Internal cavities are not returned.
+ContourNode *RangeImgToContour(const RangeImage &rimg,
+ AnalysisAllocator *alloc) {
+ alloc->reset();
+ // prev list and current list plst is mutated to include ranges from the
+ // current line
+ // becoming clst.
+ std::vector<HalfContour> clst;
+ std::vector<HalfContour> plst;
+
+ for (int x = 0; x < static_cast<int>(rimg.ranges()[0].size()); x++) {
+ ImageRange rng = rimg.ranges()[0][x];
+ plst.emplace_back(FwdRange(rimg.min_y(), rng.st, rng.ed, alloc));
+ }
+
+ for (int i = 1; i < static_cast<int>(rimg.size()); i++) {
+ const std::vector<ImageRange> &pranges = rimg.ranges()[i - 1];
+ const std::vector<ImageRange> &cranges = rimg.ranges()[i];
+ int y = i + rimg.min_y();
+ clst.clear();
+ // prev merge id and current merge id.
+ int mp = 0;
+ int mc = 0;
+
+ // This is a merge-sort of the previous list of ranges and the new-list
+ // of ranges. The HafContour list above have a one to one mapping with
+ // the range images here.
+ while (mp < static_cast<int>(pranges.size()) &&
+ mc < static_cast<int>(cranges.size())) {
+ ImageRange rprev = pranges[mp];
+ ImageRange rcur = cranges[mc];
+ if (rcur.last() < rprev.st) {
+ clst.emplace_back(FwdRange(y, rcur.st, rcur.ed, alloc));
+ mc++;
+ } else if (rprev.last() < rcur.st) {
+ CloseRange(plst[mp], alloc);
+ mp++;
+ } else {
+ ContourNode *within_pb = plst[mp].ed;
+ ContourNode *within_ca = ExtendStart(plst[mp].st, rcur.st, alloc);
+
+ while (true) {
+ if (mp + 1 < static_cast<int>(pranges.size()) &&
+ rcur.last() >= pranges[mp + 1].st) {
+ mp++;
+ CapRange(within_pb, plst[mp].st, alloc);
+ within_pb = plst[mp].ed;
+ rprev = pranges[mp];
+ } else if (mc + 1 < static_cast<int>(cranges.size()) &&
+ rprev.last() >= cranges[mc + 1].st) {
+ auto cav_t = MakeCavity(y, rcur.last(), cranges[mc + 1].st, alloc);
+ clst.emplace_back(HalfContour{within_ca, cav_t.st});
+ within_ca = cav_t.ed;
+ mc++;
+ rcur = cranges[mc];
+ } else {
+ within_pb = ExtendEnd(within_pb, rcur.last(), alloc);
+ clst.emplace_back(HalfContour{within_ca, within_pb});
+ mc++;
+ mp++;
+ break;
+ }
+ }
+ }
+ }
+ while (mc < static_cast<int>(cranges.size())) {
+ ImageRange rcur = cranges[mc];
+ clst.emplace_back(FwdRange(y, rcur.st, rcur.ed, alloc));
+ mc++;
+ }
+
+ while (mp < static_cast<int>(pranges.size())) {
+ CloseRange(plst[mp], alloc);
+ mp++;
+ }
+ std::swap(clst, plst);
+ }
+
+ for (int mp = 0; mp < static_cast<int>(plst.size()); mp++) {
+ CloseRange(plst[mp], alloc);
+ }
+ return plst[0].st;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/contour.h b/aos/vision/blob/contour.h
new file mode 100644
index 0000000..9cd1400
--- /dev/null
+++ b/aos/vision/blob/contour.h
@@ -0,0 +1,40 @@
+#ifndef _AOS_VIISON_BLOB_CONTOUR_H_
+#define _AOS_VIISON_BLOB_CONTOUR_H_
+
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/blob/region_alloc.h"
+
+namespace aos {
+namespace vision {
+
+// Countour nodes are slingly linked list chains of pixels that go around
+// the boundary of a blob.
+struct ContourNode {
+ ContourNode(int x, int y) : pt({x, y}) { next = this; }
+ ContourNode(int x, int y, ContourNode *next) : pt({x, y}), next(next) {}
+ ContourNode() {}
+
+ // Construction routine to attach a node to the end.
+ // Prefer to manipulate contours using RangeImgToContour.
+ ContourNode *append(int x, int y, AnalysisAllocator *alloc) {
+ next = alloc->cons_obj<ContourNode>(x, y);
+ return next;
+ }
+ // Construction routine to attach a node to the beginning.
+ // Prefer to manipulate contours using RangeImgToContour.
+ ContourNode *pappend(int x, int y, AnalysisAllocator *alloc) {
+ return alloc->cons_obj<ContourNode>(x, y, this);
+ }
+
+ Point pt;
+ ContourNode *next;
+};
+
+// Converts range image to contour using sweepline analysis.
+ContourNode *RangeImgToContour(const RangeImage &rimg,
+ AnalysisAllocator *alloc);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VIISON_BLOB_CONTOUR_H_
diff --git a/aos/vision/blob/disjoint_set.h b/aos/vision/blob/disjoint_set.h
new file mode 100644
index 0000000..ed15caf
--- /dev/null
+++ b/aos/vision/blob/disjoint_set.h
@@ -0,0 +1,46 @@
+#ifndef _AOS_VISION_BLOB_DISJOINT_SET_H_
+#define _AOS_VISION_BLOB_DISJOINT_SET_H_
+
+#include <vector>
+
+namespace aos {
+namespace vision {
+
+// Disjoint set algorithm:
+// https://en.wikipedia.org/wiki/Disjoint-set_data_structure
+class DisjointSet {
+ public:
+ DisjointSet() {}
+ DisjointSet(int node_count) : nodes_(node_count, -1) {}
+ // Adds another node. Not connected to anything.
+ // Returns id of the additional node for tracking purposes.
+ int Add() {
+ nodes_.push_back(-1);
+ return nodes_.size() - 1;
+ }
+ int &operator[](int id) { return nodes_[id]; }
+ // Find returns the canonical id of the set containing id.
+ // This id is an arbitrary artifact of the merge process.
+ int Find(int id) {
+ int chained_id = nodes_[id];
+ if (chained_id <= -1) {
+ return id;
+ }
+ return nodes_[id] = Find(chained_id);
+ }
+ // The blob mergeing needs to explicitly override the default union behavior.
+ void ExplicitUnion(int parent_id, int child_id) {
+ nodes_[parent_id] = child_id;
+ }
+ // Check to see if this node is the canonical id.
+ bool IsRoot(int id) { return nodes_[id] < 0; }
+
+ private:
+ // Implemented over a vector.
+ std::vector<int> nodes_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_DISJOINT_SET_H_
diff --git a/aos/vision/blob/find_blob.cc b/aos/vision/blob/find_blob.cc
new file mode 100644
index 0000000..0e2586b
--- /dev/null
+++ b/aos/vision/blob/find_blob.cc
@@ -0,0 +1,149 @@
+#include "aos/vision/blob/find_blob.h"
+
+#include "aos/vision/blob/disjoint_set.h"
+
+namespace aos {
+namespace vision {
+
+struct BlobBuilder {
+ BlobBuilder(int i) : min_y(i) {}
+ void Add(int i, ImageRange rng) {
+ while (static_cast<int>(ranges.size()) <= i - min_y) {
+ ranges.emplace_back();
+ }
+ ranges[i - min_y].push_back(rng);
+ }
+
+ void Merge(const BlobBuilder &o) {
+ // Always will be positive because of std::swap below in
+ // MergeInBlob maintains y order.
+ int diff = o.min_y - min_y;
+ for (int j = 0; j < static_cast<int>(o.ranges.size()); j++) {
+ int i = j + diff;
+ while (static_cast<int>(ranges.size()) <= i) {
+ ranges.emplace_back();
+ }
+
+ std::vector<ImageRange> cur = ranges[i];
+ std::vector<ImageRange> prev = o.ranges[j];
+
+ // Merge sort of cur and prev.
+ ranges[i].clear();
+ int a = 0;
+ int b = 0;
+ while (a < static_cast<int>(cur.size()) &&
+ b < static_cast<int>(prev.size())) {
+ if (cur[a].st < prev[b].st) {
+ ranges[i].push_back(cur[a++]);
+ } else {
+ ranges[i].push_back(prev[b++]);
+ }
+ }
+ while (a < static_cast<int>(cur.size())) {
+ ranges[i].push_back(cur[a++]);
+ }
+ while (b < static_cast<int>(prev.size())) {
+ ranges[i].push_back(prev[b++]);
+ }
+ }
+ }
+ std::vector<std::vector<ImageRange>> ranges;
+ int min_y;
+};
+
+// Uses disjoint set class to track range images.
+// Joins in the disjoint set are done at the same time as joins in the
+// range image.
+class BlobDisjointSet {
+ public:
+ int AddToBlob(int bid, int i, ImageRange img) {
+ bid = disjoint_set.Find(bid);
+ items[bid].Add(i, img);
+ return bid;
+ }
+ int AddBlob(int i, ImageRange img) {
+ int bid = disjoint_set.Add();
+ items.emplace_back(i);
+ items[bid].Add(i, img);
+ return bid;
+ }
+ void MergeInBlob(int cbid, int pbid) {
+ cbid = disjoint_set.Find(cbid);
+ pbid = disjoint_set.Find(pbid);
+ if (cbid != pbid) {
+ if (items[cbid].min_y > items[pbid].min_y) {
+ std::swap(cbid, pbid);
+ }
+ items[cbid].Merge(items[pbid]);
+ disjoint_set.ExplicitUnion(pbid, cbid);
+ }
+ }
+ BlobList MoveBlobs() {
+ std::vector<RangeImage> blobs;
+ for (int i = 0; i < static_cast<int>(items.size()); i++) {
+ if (disjoint_set.IsRoot(i)) {
+ blobs.emplace_back(items[i].min_y, std::move(items[i].ranges));
+ }
+ }
+ return blobs;
+ }
+
+ private:
+ DisjointSet disjoint_set;
+ std::vector<BlobBuilder> items;
+};
+
+BlobList FindBlobs(const RangeImage &rimg) {
+ BlobDisjointSet blob_set;
+ // prev and current ids.
+ std::vector<int> pids;
+ std::vector<int> cids;
+ for (ImageRange rng : rimg.ranges()[0]) {
+ pids.push_back(blob_set.AddBlob(0, rng));
+ }
+
+ for (int i = 1; i < rimg.size(); i++) {
+ int mi = 0;
+ int mj = 0;
+ const std::vector<ImageRange> &pranges = rimg.ranges()[i - 1];
+ const std::vector<ImageRange> &cranges = rimg.ranges()[i];
+ cids.clear();
+
+ // Merge sort pids and cids.
+ while (mi < static_cast<int>(pranges.size()) &&
+ mj < static_cast<int>(cranges.size())) {
+ ImageRange rprev = pranges[mi];
+ ImageRange rcur = cranges[mj];
+ if (rcur.last() < rprev.st) {
+ if (static_cast<int>(cids.size()) == mj) {
+ cids.push_back(blob_set.AddBlob(i, cranges[mj]));
+ }
+ mj++;
+ } else if (rprev.last() < rcur.st) {
+ mi++;
+ } else {
+ if (static_cast<int>(cids.size()) > mj) {
+ blob_set.MergeInBlob(cids[mj], pids[mi]);
+ } else {
+ cids.push_back(blob_set.AddToBlob(pids[mi], i, cranges[mj]));
+ }
+ if (rcur.last() < rprev.last()) {
+ mj++;
+ } else {
+ mi++;
+ }
+ }
+ }
+ while (mj < static_cast<int>(cranges.size())) {
+ if (static_cast<int>(cids.size()) == mj) {
+ cids.push_back(blob_set.AddBlob(i, cranges[mj]));
+ }
+ mj++;
+ }
+ std::swap(pids, cids);
+ }
+ return blob_set.MoveBlobs();
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/find_blob.h b/aos/vision/blob/find_blob.h
new file mode 100644
index 0000000..b167c3e
--- /dev/null
+++ b/aos/vision/blob/find_blob.h
@@ -0,0 +1,16 @@
+#ifndef _AOS_VISION_BLOB_FIND_BLOB_H_
+#define _AOS_VISION_BLOB_FIND_BLOB_H_
+
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+// Uses disjoint sets to group ranges into disjoint RangeImage.
+// ranges that overlap are grouped into the same output RangeImage.
+BlobList FindBlobs(const RangeImage &rimg);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_FIND_BLOB_H_
diff --git a/aos/vision/blob/hierarchical_contour_merge.cc b/aos/vision/blob/hierarchical_contour_merge.cc
new file mode 100644
index 0000000..b3d9f41
--- /dev/null
+++ b/aos/vision/blob/hierarchical_contour_merge.cc
@@ -0,0 +1,252 @@
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+#include <math.h>
+#include <queue>
+
+#include "aos/vision/blob/disjoint_set.h"
+
+namespace aos {
+namespace vision {
+
+namespace {
+
+int Mod(int a, int n) { return a - n * (a / n); }
+
+} // namespace
+
+template <typename T>
+class IntegralArray {
+ public:
+ IntegralArray() {}
+ IntegralArray(int size) { items_.reserve(size); }
+
+ // This is an exclusive range lookup into a modulo ring.
+ // The integral is precomputed in items_ and is inclusive even though
+ // the input is [a, b).
+ T Get(int a, int b) {
+ a = Mod(a, items_.size());
+ b = Mod(b, items_.size());
+ if (a == b) return 0;
+ if (b < a) {
+ if (b == 0) {
+ return items_[items_.size() - 1] - items_[a - 1];
+ }
+ return items_[items_.size() - 1] + items_[b - 1] - items_[a - 1];
+ }
+ if (a == 0) {
+ return items_[b - 1];
+ } else {
+ return items_[b - 1] - items_[a - 1];
+ }
+ }
+ void Add(T t) {
+ if (items_.size() == 0) {
+ items_.push_back(t);
+ } else {
+ items_.push_back(t + items_[items_.size() - 1]);
+ }
+ }
+
+ private:
+ std::vector<T> items_;
+};
+
+class IntegralLineFit {
+ public:
+ IntegralLineFit(int number_of_points, int min_line_length)
+ : xx_(number_of_points),
+ xy_(number_of_points),
+ yy_(number_of_points),
+ x_(number_of_points),
+ y_(number_of_points),
+ // These are not IntegralArrays.
+ n_(number_of_points),
+ min_len_(min_line_length) {}
+
+ void AddPt(Point pt) {
+ xx_.Add(pt.x * pt.x);
+ xy_.Add(pt.x * pt.y);
+ yy_.Add(pt.y * pt.y);
+ x_.Add(pt.x);
+ y_.Add(pt.y);
+ }
+
+ int GetNForRange(int st, int ed) {
+ int nv = (ed + 1) - st;
+ if (ed < st) {
+ nv += n_;
+ }
+ return nv;
+ }
+
+ float GetLineErrorRate(int st, int ed) {
+ int64_t nv = GetNForRange(st, ed);
+
+ int64_t px = x_.Get(st, ed);
+ int64_t py = y_.Get(st, ed);
+ int64_t pxx = xx_.Get(st, ed);
+ int64_t pxy = xy_.Get(st, ed);
+ int64_t pyy = yy_.Get(st, ed);
+
+ double nvsq = nv * nv;
+ double m_xx = (pxx * nv - px * px) / nvsq;
+ double m_xy = (pxy * nv - px * py) / nvsq;
+ double m_yy = (pyy * nv - py * py) / nvsq;
+
+ double b = m_xx + m_yy;
+ double c = m_xx * m_yy - m_xy * m_xy;
+ return ((b - sqrt(b * b - 4 * c)) / 2.0);
+ }
+
+ float GetErrorLineRange(int st, int ed) {
+ int nv = GetNForRange(st, ed);
+ int j = std::max(min_len_ - nv, 0) / 2;
+ return GetLineErrorRate((st - j + n_) % n_, (ed + 1 + j + n_) % n_);
+ }
+
+ FittedLine FitLine(int st, int ed, Point pst, Point ped) {
+ int nv = GetNForRange(st, ed);
+ // Adjust line out to be at least min_len_.
+ int j = std::max(min_len_ - nv, 0) / 2;
+
+ st = Mod(st - j, n_);
+ ed = Mod(ed + 1 + j, n_);
+ if (nv <= min_len_) {
+ return FittedLine{pst, pst};
+ }
+
+ int64_t px = x_.Get(st, ed);
+ int64_t py = y_.Get(st, ed);
+ int64_t pxx = xx_.Get(st, ed);
+ int64_t pxy = xy_.Get(st, ed);
+ int64_t pyy = yy_.Get(st, ed);
+
+ double nvsq = nv * nv;
+ double m_xx = (pxx * nv - px * px) / nvsq;
+ double m_xy = (pxy * nv - px * py) / nvsq;
+ double m_yy = (pyy * nv - py * py) / nvsq;
+ double m_x = px / ((double)nv);
+ double m_y = py / ((double)nv);
+
+ double b = (m_xx + m_yy) / 2.0;
+ double c = m_xx * m_yy - m_xy * m_xy;
+
+ double eiggen = sqrt(b * b - c);
+ double eigv = b - eiggen;
+
+ double vx = m_xx - eigv;
+ double vy = m_xy;
+ double mag = sqrt(vx * vx + vy * vy);
+ vx /= mag;
+ vy /= mag;
+
+ double av = vx * (pst.x - m_x) + vy * (pst.y - m_y);
+ double bv = vx * (ped.x - m_x) + vy * (ped.y - m_y);
+
+ Point apt = {(int)(m_x + vx * av), (int)(m_y + vy * av)};
+ Point bpt = {(int)(m_x + vx * bv), (int)(m_y + vy * bv)};
+
+ return FittedLine{apt, bpt};
+ }
+
+ private:
+ IntegralArray<int> xx_;
+ IntegralArray<int> xy_;
+ IntegralArray<int> yy_;
+ IntegralArray<int> x_;
+ IntegralArray<int> y_;
+
+ // Number of points in contour.
+ int n_;
+
+ // Minimum line length we will look for.
+ int min_len_;
+};
+
+struct JoinEvent {
+ int st;
+ int ed;
+ // All joins defined to be equal in priority.
+ // To be used in std::pair<float, JoinEvent> so need a comparator
+ // event though it isn't used.
+ bool operator<(const JoinEvent & /*o*/) const { return false; }
+};
+
+void HierarchicalMerge(ContourNode *stval, std::vector<FittedLine> *fit_lines,
+ float merge_rate, int min_len) {
+ ContourNode *c = stval;
+ // count the number of points in the contour.
+ int n = 0;
+ do {
+ n++;
+ c = c->next;
+ } while (c != stval);
+ IntegralLineFit fit(n, min_len);
+ c = stval;
+ std::vector<Point> pts;
+ do {
+ fit.AddPt(c->pt);
+ pts.push_back(c->pt);
+ c = c->next;
+ } while (c != stval);
+
+ DisjointSet ids(n);
+
+ std::vector<int> sts;
+ sts.reserve(n);
+ std::vector<int> eds;
+ eds.reserve(n);
+ for (int i = 0; i < n; i++) {
+ sts.push_back(i);
+ eds.push_back(i);
+ }
+
+ // Note priority queue takes a pair, so float is used as the priority.
+ std::priority_queue<std::pair<float, JoinEvent>> events;
+ for (int i = 0; i < n; i++) {
+ float err = fit.GetErrorLineRange(i - 1, i);
+ events.push(
+ std::pair<float, JoinEvent>(err, JoinEvent{(i - 1 + n) % n, i}));
+ }
+
+ while (events.size() > 0) {
+ auto event = events.top().second;
+ // Merge the lines that are most like a line.
+ events.pop();
+ int pi1 = ids.Find(event.st);
+ int pi2 = ids.Find(event.ed);
+ int st = sts[pi1];
+ int ed = eds[pi2];
+ if (st == event.st && ed == event.ed && pi1 != pi2) {
+ ids[pi2] = pi1;
+ int pi = sts[ids.Find((st - 1 + n) % n)];
+ int ni = eds[ids.Find((ed + 1 + n) % n)];
+ eds[pi1] = ed;
+ if (pi != st) {
+ float err = fit.GetErrorLineRange(pi, ed);
+ if (err < merge_rate) {
+ events.push(std::pair<float, JoinEvent>(err, JoinEvent{pi, ed}));
+ }
+ }
+ if (ni != ed) {
+ float err = fit.GetErrorLineRange(st, ni);
+ if (err < merge_rate) {
+ events.push(std::pair<float, JoinEvent>(err, JoinEvent{st, ni}));
+ }
+ }
+ }
+ }
+ for (int i = 0; i < n; i++) {
+ if (ids[i] == -1) {
+ int sti = sts[i];
+ int edi = eds[i];
+ if ((edi - sti + n) % n > min_len) {
+ auto line_fit = fit.FitLine(sti, edi, pts[sti], pts[edi]);
+ fit_lines->emplace_back(line_fit);
+ }
+ }
+ }
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/hierarchical_contour_merge.h b/aos/vision/blob/hierarchical_contour_merge.h
new file mode 100644
index 0000000..9dd471d
--- /dev/null
+++ b/aos/vision/blob/hierarchical_contour_merge.h
@@ -0,0 +1,25 @@
+#ifndef _AOS_VISION_BLOB_HIERARCHICAL_CONTOUR_MERGE_H_
+#define _AOS_VISION_BLOB_HIERARCHICAL_CONTOUR_MERGE_H_
+
+#include <vector>
+
+#include "aos/vision/blob/contour.h"
+#include "aos/vision/blob/range_image.h"
+
+namespace aos {
+namespace vision {
+
+struct FittedLine {
+ Point st;
+ Point ed;
+};
+
+// Merges a contour into a list of best fit lines where the regression value is
+// merge_rate and only emit lines at least min_len pixels long.
+void HierarchicalMerge(ContourNode *stval, std::vector<FittedLine> *fit_lines,
+ float merge_rate = 4.0, int min_len = 15);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_HIERARCHICAL_CONTOUR_MERGE_H_
diff --git a/aos/vision/blob/range_image.cc b/aos/vision/blob/range_image.cc
new file mode 100644
index 0000000..5f2f29f
--- /dev/null
+++ b/aos/vision/blob/range_image.cc
@@ -0,0 +1,124 @@
+#include "aos/vision/blob/range_image.h"
+
+#include <math.h>
+#include <algorithm>
+
+namespace aos {
+namespace vision {
+
+void DrawRangeImage(const RangeImage &rimg, ImagePtr outbuf, PixelRef color) {
+ for (int i = 0; i < (int)rimg.ranges().size(); ++i) {
+ int y = rimg.min_y() + i;
+ for (ImageRange rng : rimg.ranges()[i]) {
+ for (int x = rng.st; x < rng.ed; ++x) {
+ outbuf.get_px(x, y) = color;
+ }
+ }
+ }
+}
+
+RangeImage MergeRangeImage(const BlobList &blobl) {
+ if (blobl.size() == 1) return blobl[0];
+
+ int min_y = blobl[0].min_y();
+ for (const RangeImage &subrimg : blobl) {
+ if (min_y > subrimg.min_y()) min_y = subrimg.min_y();
+ }
+ std::vector<std::vector<ImageRange>> ranges;
+ int i = min_y;
+ while (true) {
+ std::vector<ImageRange> range_lst;
+ int n_missing = 0;
+ for (const RangeImage &subrimg : blobl) {
+ if (subrimg.min_y() > i) continue;
+ int ri = i - subrimg.min_y();
+ if (ri < (int)subrimg.ranges().size()) {
+ for (const auto &span : subrimg.ranges()[ri]) {
+ range_lst.emplace_back(span);
+ }
+ } else {
+ ++n_missing;
+ }
+ }
+ std::sort(range_lst.begin(), range_lst.end());
+ ranges.emplace_back(std::move(range_lst));
+ if (n_missing == static_cast<int>(blobl.size()))
+ return RangeImage(min_y, std::move(ranges));
+ ++i;
+ }
+}
+
+std::string ShortDebugPrint(const BlobList &blobl) {
+ RangeImage rimg = MergeRangeImage(blobl);
+ std::string out;
+ out += "{";
+ out += "min_y: " + std::to_string(rimg.min_y());
+ for (const auto &line : rimg) {
+ out += "{";
+ for (const auto &span : line) {
+ out +=
+ "{" + std::to_string(span.st) + ", " + std::to_string(span.ed) + "},";
+ }
+ out += "},";
+ }
+ out += "}";
+ return out;
+}
+
+void DebugPrint(const BlobList &blobl) {
+ RangeImage rimg = MergeRangeImage(blobl);
+ int minx = rimg.ranges()[0][0].st;
+ int maxx = 0;
+ for (const auto &range : rimg.ranges()) {
+ for (const auto &span : range) {
+ if (span.st < minx) minx = span.st;
+ if (span.ed > maxx) maxx = span.ed;
+ }
+ }
+ LOG(INFO, "maxx: %d minx: %d\n", maxx, minx);
+ char buf[maxx - minx];
+ for (const auto &range : rimg.ranges()) {
+ int i = minx;
+ for (const auto &span : range) {
+ for (; i < span.st; ++i) buf[i - minx] = ' ';
+ for (; i < span.ed; ++i) buf[i - minx] = '#';
+ }
+ for (; i < maxx; ++i) buf[i - minx] = ' ';
+ LOG(INFO, "%.*s\n", maxx - minx, buf);
+ }
+}
+
+void RangeImage::Flip(int image_width, int image_height) {
+ std::reverse(ranges_.begin(), ranges_.end());
+ for (std::vector<ImageRange> &range : ranges_) {
+ std::reverse(range.begin(), range.end());
+ for (ImageRange &interval : range) {
+ int tmp = image_width - interval.ed;
+ interval.ed = image_width - interval.st;
+ interval.st = tmp;
+ }
+ }
+
+ min_y_ = image_height - static_cast<int>(ranges_.size()) - min_y_;
+}
+
+int RangeImage::npixels() {
+ if (npixelsc_ > 0) {
+ return npixelsc_;
+ }
+ npixelsc_ = calc_area();
+ return npixelsc_;
+}
+
+int RangeImage::calc_area() const {
+ int area = 0;
+ for (auto &range : ranges_) {
+ for (auto &interval : range) {
+ area += interval.calc_width();
+ }
+ }
+ return area;
+}
+
+} // namespace vision
+} // namespace aos
diff --git a/aos/vision/blob/range_image.h b/aos/vision/blob/range_image.h
new file mode 100644
index 0000000..109a100
--- /dev/null
+++ b/aos/vision/blob/range_image.h
@@ -0,0 +1,84 @@
+#ifndef _AOS_VISION_BLOB_RANGE_IMAGE_H_
+#define _AOS_VISION_BLOB_RANGE_IMAGE_H_
+
+#include <vector>
+
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+struct Point {
+ int x;
+ int y;
+};
+
+struct ImageRange {
+ ImageRange(int a, int b) : st(a), ed(b) {}
+ int st;
+ int ed;
+ int last() const { return ed - 1; }
+ int calc_width() const { return ed - st; }
+
+ bool operator<(const ImageRange &o) const { return st < o.st; }
+};
+
+// Image in pre-thresholded run-length encoded format.
+class RangeImage {
+ public:
+ RangeImage(int min_y, std::vector<std::vector<ImageRange>> &&ranges)
+ : min_y_(min_y), ranges_(std::move(ranges)) {}
+ explicit RangeImage(int l) { ranges_.reserve(l); }
+ RangeImage() {}
+
+ int size() const { return ranges_.size(); }
+
+ int npixels();
+ int calc_area() const;
+
+ void Flip(ImageFormat fmt) { Flip(fmt.w, fmt.h); }
+ void Flip(int image_width, int image_height);
+
+ std::vector<std::vector<ImageRange>>::const_iterator begin() const {
+ return ranges_.begin();
+ }
+
+ std::vector<std::vector<ImageRange>>::const_iterator end() const {
+ return ranges_.end();
+ }
+
+ const std::vector<std::vector<ImageRange>> &ranges() const { return ranges_; }
+
+ int min_y() const { return min_y_; }
+
+ int mini() const { return min_y_; }
+
+ private:
+ // minimum index in y where the blob starts
+ int min_y_ = 0;
+
+ // ranges are always sorted in y then x order
+ std::vector<std::vector<ImageRange>> ranges_;
+
+ // Cached pixel count.
+ int npixelsc_ = -1;
+};
+
+typedef std::vector<RangeImage> BlobList;
+typedef std::vector<const RangeImage *> BlobLRef;
+
+void DrawRangeImage(const RangeImage &rimg, ImagePtr outbuf, PixelRef color);
+
+// Merge sort of multiple range images into a single range image.
+// They must not overlap.
+RangeImage MergeRangeImage(const BlobList &blobl);
+
+// Debug print range image as ranges.
+std::string ShortDebugPrint(const BlobList &blobl);
+// Debug print range image as ### for the ranges.
+void DebugPrint(const BlobList &blobl);
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_RANGE_IMAGE_H_
diff --git a/aos/vision/blob/region_alloc.h b/aos/vision/blob/region_alloc.h
new file mode 100644
index 0000000..4bc4156
--- /dev/null
+++ b/aos/vision/blob/region_alloc.h
@@ -0,0 +1,60 @@
+#ifndef _AOS_VISION_REGION_ALLOC_H_
+#define _AOS_VISION_REGION_ALLOC_H_
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <memory>
+#include <new>
+#include <utility>
+#include <vector>
+
+#include "aos/common/logging/logging.h"
+
+namespace aos {
+namespace vision {
+
+// Region based allocator. Used for arena allocations in vision code.
+// Example use: Storing contour nodes.
+class AnalysisAllocator {
+ public:
+ template <typename T, typename... Args>
+ T *cons_obj(Args &&... args) {
+ uint8_t *ptr = NULL;
+ if (sizeof(T) + alignof(T) > block_size_) {
+ LOG(FATAL, "allocating %d too much\n", (int)sizeof(T));
+ }
+ while (ptr == NULL) {
+ if (next_free_ >= memory_.size()) {
+ if (next_free_ >= 1024) {
+ LOG(FATAL, "too much alloc\n");
+ }
+ memory_.emplace_back(new uint8_t[block_size_]);
+ } else if ((used_size_ % alignof(T)) != 0) {
+ used_size_ += alignof(T) - (used_size_ % alignof(T));
+ } else if ((used_size_ + sizeof(T)) <= block_size_) {
+ ptr = &memory_[next_free_][used_size_];
+ used_size_ += sizeof(T);
+ } else {
+ used_size_ = 0;
+ next_free_++;
+ }
+ }
+ return new (ptr) T(std::forward<Args>(args)...);
+ }
+ void reset() {
+ next_free_ = 0;
+ used_size_ = 0;
+ }
+
+ private:
+ std::vector<std::unique_ptr<uint8_t[]>> memory_;
+ size_t next_free_ = 0;
+ size_t block_size_ = 1024 * 4;
+ size_t used_size_ = 0;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_IMAGE_REGION_ALLOC_H_
diff --git a/aos/vision/blob/stream_view.h b/aos/vision/blob/stream_view.h
new file mode 100644
index 0000000..9d63e8d
--- /dev/null
+++ b/aos/vision/blob/stream_view.h
@@ -0,0 +1,73 @@
+#ifndef _AOS_VISION_BLOB_STREAM_VIEW_H_
+#define _AOS_VISION_BLOB_STREAM_VIEW_H_
+
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/debug/debug_viewer.h"
+#include "aos/vision/image/image_types.h"
+
+#include <memory>
+
+namespace aos {
+namespace vision {
+
+class BlobStreamViewer {
+ public:
+ BlobStreamViewer() : view_(false) {}
+ 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) {
+ printf("resizing data: %d, %d\n", fmt.w, fmt.h);
+ outbuf_.reset(new PixelRef[fmt.ImgSize()]);
+ ptr_ = ImagePtr{fmt, outbuf_.get()};
+ view_.UpdateImage(ptr_);
+ }
+ memset(ptr_.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]) {
+ for (int j = range.st; j < range.ed; ++j) {
+ ptr_.get_px(j, i + blob.min_y) = color;
+ }
+ }
+ }
+ }
+ }
+
+ inline void DrawSecondBlobList(const BlobList &blob_list, PixelRef color1,
+ PixelRef color2) {
+ 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 j = range.st; j < range.ed; ++j) {
+ 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;
+ } else {
+ ptr_.get_px(j, i + blob.min_y) = color2;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ DebugViewer *view() { return &view_; }
+
+ private:
+ std::unique_ptr<PixelRef[]> outbuf_;
+ ImagePtr ptr_;
+
+ DebugViewer view_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VISION_BLOB_STREAM_VIEW_H_
diff --git a/aos/vision/blob/threshold.h b/aos/vision/blob/threshold.h
new file mode 100644
index 0000000..8b6051f
--- /dev/null
+++ b/aos/vision/blob/threshold.h
@@ -0,0 +1,40 @@
+#ifndef _AOS_VIISON_BLOB_THRESHOLD_H_
+#define _AOS_VIISON_BLOB_THRESHOLD_H_
+
+#include "aos/vision/blob/range_image.h"
+#include "aos/vision/image/image_types.h"
+
+namespace aos {
+namespace vision {
+
+// ThresholdFn should be a lambda.
+template <typename ThresholdFn>
+RangeImage DoThreshold(const ImagePtr &img, ThresholdFn &&fn) {
+ std::vector<std::vector<ImageRange>> ranges;
+ ranges.reserve(img.fmt().h);
+ for (int y = 0; y < img.fmt().h; ++y) {
+ bool p_score = false;
+ int pstart = -1;
+ std::vector<ImageRange> rngs;
+ for (int x = 0; x < img.fmt().w; ++x) {
+ if (fn(img.get_px(x, y)) != p_score) {
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, x));
+ } else {
+ pstart = x;
+ }
+ p_score = !p_score;
+ }
+ }
+ if (p_score) {
+ rngs.emplace_back(ImageRange(pstart, img.fmt().w));
+ }
+ ranges.push_back(rngs);
+ }
+ return RangeImage(0, std::move(ranges));
+}
+
+} // namespace vision
+} // namespace aos
+
+#endif // _AOS_VIISON_BLOB_THRESHOLD_H_
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index 728fa03..fe4927b 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -1,6 +1,13 @@
+package(default_visibility = ['//visibility:public'])
+
+cc_library(
+ name = 'segment',
+ hdrs = ['segment.h'],
+ deps = [':vector'],
+)
+
cc_library(
name = 'vector',
- visibility = ['//visibility:public'],
hdrs = [
'vector.h',
],
diff --git a/aos/vision/math/segment.h b/aos/vision/math/segment.h
new file mode 100644
index 0000000..706beaf
--- /dev/null
+++ b/aos/vision/math/segment.h
@@ -0,0 +1,82 @@
+#ifndef AOS_VISION_COMP_GEO_SEGMENT_H_
+#define AOS_VISION_COMP_GEO_SEGMENT_H_
+
+#include <cmath>
+
+#include "aos/vision/math/vector.h"
+
+namespace aos {
+namespace vision {
+
+template <int Size>
+class Segment {
+ public:
+ Segment() : A_(), B_() {}
+
+ Segment(Vector<Size> A, Vector<Size> B) : A_(A), B_(B) {}
+
+ Segment(double ax, double ay, double az, double bx, double by, double bz)
+ : A_(ax, ay, az), B_(bx, by, bz) {}
+
+ ~Segment() {}
+
+ void Set(Vector<Size> a, Vector<Size> b) {
+ A_ = a;
+ B_ = b;
+ }
+
+ Vector<Size> A() const { return A_; }
+ Vector<Size> B() const { return B_; }
+
+ Vector<Size> AsVector() const { return B_ - A_; }
+ Vector<Size> AsNormed() const { return AsVector().normed(); }
+
+ Vector<Size> Center() const { return (A_ + B_) * (0.5); }
+
+ // Fast part of length.
+ double MagSqr() { return (AsVector()).MagSqr(); }
+
+ // Length of the vector.
+ double Mag() { return std::sqrt(MagSqr()); }
+
+ Segment<Size> Scale(const double &other) {
+ return Segment<Size>(A_ * (other), B_ * (other));
+ }
+
+ // Intersect two lines in a plane.
+ Vector<2> Intersect(const Segment<2> &other) {
+ static_assert(Size == 2, "Only works for size == 2");
+ double x1 = A_.x();
+ double y1 = A_.y();
+ double x2 = B_.x();
+ double y2 = B_.y();
+
+ double x3 = other.A().x();
+ double y3 = other.A().y();
+ double x4 = other.B().x();
+ double y4 = other.B().y();
+
+ // check wikipedia on how to intersect two lines.
+ double xn =
+ (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
+ double xd = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+
+ double yn =
+ (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
+ double yd = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+
+ return Vector<2>(xn / xd, yn / yd);
+ }
+
+ private:
+ // Begining point.
+ Vector<Size> A_;
+
+ // Ending point.
+ Vector<Size> B_;
+};
+
+} // namespace vision
+} // namespace aos
+
+#endif // AOS_VISION_COMP_GEO_VECTOR_H_
diff --git a/aos/vision/math/vector.h b/aos/vision/math/vector.h
index 0d689e6..3205236 100644
--- a/aos/vision/math/vector.h
+++ b/aos/vision/math/vector.h
@@ -20,7 +20,7 @@
template <int size>
class Vector {
public:
- Vector() { data_.SetZero(); }
+ Vector() : data_(::Eigen::Matrix<double, 1, size>::Zero()) {}
Vector(double x, double y) { Set(x, y); }
@@ -175,13 +175,13 @@
}
// Return the angle between this and other.
- double AngleTo(const Vector<size> other) const {
+ double AngleTo(const Vector<size> &other) const {
// cos(theta) = u.dot(v) / (u.magnitude() * v.magnitude())
return ::std::acos(dot(other) / (Mag() * other.Mag()));
}
// Returns the distance between this and other squared.
- double SquaredDistanceTo(const Vector<size> other) {
+ double SquaredDistanceTo(const Vector<size> &other) const {
Vector<size> tmp = *this - other;
return tmp.MagSqr();
}
@@ -195,6 +195,15 @@
inline double PointsCrossProduct(const Vector<2> &a, const Vector<2> &b) {
return a.x() * b.y() - a.y() * b.x();
}
+// scalar multiply
+template <int Size>
+inline Vector<Size> operator*(const double &lhs, Vector<Size> &rhs) {
+ Vector<Size> nv;
+ for (int i = 0; i < Size; i++) {
+ nv.Set(i, lhs * rhs.Get(i));
+ }
+ return nv;
+}
} // namespace vision
} // namespace aos