De-warp contours before starting the polygon finding.
Change-Id: I7889c8e5e9f826bef2317f4e7f3306bd65dc32f9
diff --git a/aos/vision/blob/contour.h b/aos/vision/blob/contour.h
index 9cd1400..bbd59a9 100644
--- a/aos/vision/blob/contour.h
+++ b/aos/vision/blob/contour.h
@@ -25,6 +25,7 @@
ContourNode *pappend(int x, int y, AnalysisAllocator *alloc) {
return alloc->cons_obj<ContourNode>(x, y, this);
}
+ void set_point(Point new_pt) { pt = new_pt; }
Point pt;
ContourNode *next;
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 4f43c9a..51fe831 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -92,10 +92,23 @@
// Find polygons from blobs.
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ if (draw_contours_) {
+ DrawContour(contour, {255, 0, 0});
+ }
+ finder_.UnWarpContour(contour);
+ if (draw_contours_) {
+ DrawContour(contour, {0, 0, 255});
+ }
+
+ // Process to polygons.
std::vector<Segment<2>> polygon =
- finder_.FillPolygon(blob, draw_raw_poly_);
+ finder_.FillPolygon(contour, draw_raw_poly_);
if (polygon.empty()) {
- DrawBlob(blob, {255, 0, 0});
+ if (!draw_contours_) {
+ DrawBlob(blob, {255, 0, 0});
+ }
} else {
raw_polys.push_back(polygon);
if (draw_select_blob_) {
@@ -168,7 +181,20 @@
} else if (key == 'b') {
draw_raw_poly_ = !draw_raw_poly_;
} else if (key == 'n') {
+ draw_contours_ = !draw_contours_;
+ } else if (key == 'm') {
draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'h') {
+ printf("Key Mappings:\n");
+ printf(" z: Toggle drawing final target pose.\n");
+ printf(" x: Toggle drawing re-projected targets and print solver results.\n");
+ printf(" c: Toggle drawing proposed target groupings.\n");
+ printf(" v: Toggle drawing ordered target components.\n");
+ printf(" b: Toggle drawing proposed target components.\n");
+ printf(" n: Toggle drawing countours before and after warping.\n");
+ printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
+ printf(" h: Print this message.\n");
+ printf(" q: Exit the application.\n");
} else if (key == 'q') {
printf("User requested shutdown.\n");
exit(0);
@@ -178,6 +204,17 @@
};
}
+ void DrawContour(ContourNode *contour, PixelRef color) {
+ if (viewer_) {
+ for (ContourNode *node = contour; node->next != contour;) {
+ Vector<2> a(node->pt.x, node->pt.y);
+ Vector<2> b(node->next->pt.x, node->next->pt.y);
+ overlay_.AddLine(a, b, color);
+ node = node->next;
+ }
+ }
+ }
+
void DrawComponent(const TargetComponent &comp, PixelRef top_color,
PixelRef bot_color, PixelRef in_color,
PixelRef out_color) {
@@ -252,6 +289,7 @@
BlobList imgs_last_;
ImageFormat fmt_last_;
bool draw_select_blob_ = false;
+ bool draw_contours_ = false;
bool draw_raw_poly_ = false;
bool draw_components_ = false;
bool draw_raw_target_ = false;
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index d5f0d4e..bf64310 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -133,7 +133,10 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
if (polygon.empty()) {
} else {
raw_polys.push_back(polygon);
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 6b90080..b133bd3 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -33,13 +33,64 @@
imgs->end());
}
+ContourNode* TargetFinder::GetContour(const RangeImage &blob) {
+ alloc_.reset();
+ return RangeImgToContour(blob, &alloc_);
+}
+
+// TODO(ben): These values will be moved into a configuration file.
+namespace {
+
+constexpr double mtx00 = 481.4957;
+constexpr double mtx02 = 341.215;
+constexpr double mtx11 = 484.314;
+constexpr double mtx12 = 251.29;
+
+constexpr double new_cam00 = 363.1424;
+constexpr double new_cam02 = 337.9895;
+constexpr double new_cam11 = 366.4837;
+constexpr double new_cam12 = 240.0702;
+
+constexpr double dist00 = -0.2739;
+constexpr double dist01 = 0.01583;
+constexpr double dist04 = 0.04201;
+
+constexpr int iterations = 7;
+
+}
+
+Point UnWarpPoint(const Point &point, int iterations) {
+ const double x0 = ((double)point.x - mtx02) / mtx00;
+ const double y0 = ((double)point.y - mtx12) / mtx11;
+ double x = x0;
+ double y = y0;
+ for (int i = 0; i < iterations; i++) {
+ const double r_sqr = x * x + y * y;
+ const double coeff =
+ 1.0 + r_sqr * (dist00 + dist01 * r_sqr * (1.0 + dist04 * r_sqr));
+ x = x0 / coeff;
+ y = y0 / coeff;
+ }
+ double nx = x * new_cam00 + new_cam02;
+ double ny = y * new_cam11 + new_cam12;
+ Point p = {static_cast<int>(nx), static_cast<int>(ny)};
+ return p;
+}
+
+void TargetFinder::UnWarpContour(ContourNode *start) const {
+ ContourNode *node = start;
+ while (node->next != start) {
+ node->set_point(UnWarpPoint(node->pt, iterations));
+ node = node->next;
+ }
+ node->set_point(UnWarpPoint(node->pt, iterations));
+}
+
// TODO: Try hierarchical merge for this.
// Convert blobs into polygons.
std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
- const RangeImage &blob, bool verbose) {
+ ContourNode* start, bool verbose) {
if (verbose) printf("Process Polygon.\n");
- alloc_.reset();
- ContourNode *start = RangeImgToContour(blob, &alloc_);
struct Pt {
float x;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 5957fa6..bdf67f2 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -4,6 +4,7 @@
#include "aos/vision/blob/region_alloc.h"
#include "aos/vision/blob/threshold.h"
#include "aos/vision/blob/transpose.h"
+#include "aos/vision/blob/contour.h"
#include "aos/vision/debug/overlay.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/target_types.h"
@@ -15,6 +16,7 @@
using aos::vision::RangeImage;
using aos::vision::BlobList;
using aos::vision::Vector;
+using aos::vision::ContourNode;
class TargetFinder {
public:
@@ -28,8 +30,11 @@
// filter out obvious or durranged blobs.
void PreFilter(BlobList *imgs);
+ ContourNode* GetContour(const RangeImage &blob);
+ void UnWarpContour(ContourNode* start) const;
+
// Turn a blob into a polgygon.
- std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ std::vector<aos::vision::Segment<2>> FillPolygon(ContourNode *start,
bool verbose);
// Turn a bloblist into components of a target.
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 2617d33..0afeb47 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -309,9 +309,11 @@
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
- std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
- if (polygon.empty()) {
- } else {
+ // Convert blobs to contours in the corrected space.
+ ContourNode* contour = finder_.GetContour(blob);
+ finder_.UnWarpContour(contour);
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}