Add a foothill finder
We were having some corners which were really close to primary corners
but also kinda high. The fix for those is to ignore any peaks which
don't become small enough before running into previously counted peaks.
We call these peaks foothills.
Change-Id: I58243cd22d0ec9d224c48623551705ad5f3d1168
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index b282367..2bc41ac 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -139,7 +139,7 @@
// Calculate each component side of a possible target.
std::vector<TargetComponent> target_component_list =
- target_finder_.FillTargetComponentList(raw_polys);
+ target_finder_.FillTargetComponentList(raw_polys, draw_components_);
if (draw_components_) {
for (const TargetComponent &comp : target_component_list) {
DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index c9646fc..6d4a21e 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -173,7 +173,7 @@
// Calculate each component side of a possible target.
const ::std::vector<TargetComponent> target_component_list =
- target_finder.FillTargetComponentList(raw_polys);
+ target_finder.FillTargetComponentList(raw_polys, verbose);
// Put the compenents together into targets.
const ::std::vector<Target> target_list =
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index 8d92265..85cf452 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -155,24 +155,23 @@
// Because they round, multiple slopes may be a corner.
::std::vector<size_t> edges;
- ::std::vector<size_t> peaks;
-
constexpr float peak_acceptance_ratio = 0.16;
constexpr float valley_ratio = 0.75;
float highest_peak_value = 0.0;
// Nth higest points.
- for (int j = 0; j < 5; ++j) {
+ while (edges.size() < 5) {
const ::std::vector<float>::iterator max_element =
::std::max_element(corner_metric.begin(), corner_metric.end());
const size_t highest_index =
::std::distance(corner_metric.begin(), max_element);
const float max_value = *max_element;
- if (j == 0) {
+ if (edges.size() == 0) {
highest_peak_value = max_value;
}
- if (max_value < highest_peak_value * peak_acceptance_ratio && j == 4) {
+ if (max_value < highest_peak_value * peak_acceptance_ratio &&
+ edges.size() == 4) {
if (verbose)
printf("Rejecting index: %zu, %f (%f %%)\n", highest_index, max_value,
max_value / highest_peak_value);
@@ -184,19 +183,28 @@
printf("Highest index: %zu, %f (%f %%)\n", highest_index, max_value,
max_value / highest_peak_value);
+ bool foothill = false;
{
float min_value = max_value;
size_t fwd_index = (highest_index + 1) % corner_metric.size();
while (true) {
const float current_value = corner_metric[fwd_index];
+
+ if (current_value == -1.0) {
+ if (min_value >= valley_value) {
+ if (verbose) printf("Foothill\n");
+ foothill = true;
+ }
+ break;
+ }
+
min_value = ::std::min(current_value, min_value);
- if (current_value == 0.0 ||
- (min_value < valley_value && current_value > min_value)) {
+ if (min_value < valley_value && current_value > min_value) {
break;
}
// Kill!!!
- corner_metric[fwd_index] = 0.0;
+ corner_metric[fwd_index] = -1.0;
fwd_index = (fwd_index + 1) % corner_metric.size();
}
@@ -208,22 +216,32 @@
(highest_index - 1 + corner_metric.size()) % corner_metric.size();
while (true) {
const float current_value = corner_metric[rev_index];
+
+ if (current_value == -1.0) {
+ if (min_value >= valley_value) {
+ if (verbose) printf("Foothill\n");
+ foothill = true;
+ }
+ break;
+ }
+
min_value = ::std::min(current_value, min_value);
- if (current_value == 0.0 ||
- (min_value < valley_value && current_value > min_value)) {
+ if (min_value < valley_value && current_value > min_value) {
break;
}
// Kill!!!
- corner_metric[rev_index] = 0.0;
+ corner_metric[rev_index] = -1.0;
rev_index =
(rev_index - 1 + corner_metric.size()) % corner_metric.size();
}
}
- *max_element = 0.0;
- edges.push_back(highest_index);
+ *max_element = -1.0;
+ if (!foothill) {
+ edges.push_back(highest_index);
+ }
}
::std::sort(edges.begin(), edges.end());
@@ -300,7 +318,7 @@
// Convert segments into target components (left or right)
std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
- const std::vector<std::vector<Segment<2>>> &seg_list) {
+ const std::vector<std::vector<Segment<2>>> &seg_list, bool verbose) {
std::vector<TargetComponent> list;
TargetComponent new_target;
for (const std::vector<Segment<2>> &poly : seg_list) {
@@ -308,7 +326,7 @@
if (poly.size() != 4) {
continue;
}
- std::vector<Vector<2>> corners;
+ ::std::vector<Vector<2>> corners;
for (size_t i = 0; i < 4; ++i) {
Vector<2> corner = poly[i].Intersect(poly[(i + 1) % 4]);
if (::std::isnan(corner.x()) || ::std::isnan(corner.y())) {
@@ -322,7 +340,7 @@
// Select the closest two points. Short side of the rectangle.
double min_dist = -1;
- std::pair<size_t, size_t> closest;
+ ::std::pair<size_t, size_t> closest;
for (size_t i = 0; i < 4; ++i) {
size_t next = (i + 1) % 4;
double nd = corners[i].SquaredDistanceTo(corners[next]);
@@ -410,6 +428,7 @@
// This piece of the target should be ready now.
list.emplace_back(new_target);
+ if (verbose) printf("Happy with a target\n");
}
return list;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index fcde358..83a1ebd 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -39,7 +39,8 @@
// Turn a bloblist into components of a target.
std::vector<TargetComponent> FillTargetComponentList(
- const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+ const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list,
+ bool verbose);
// Piece the compenents together into a target.
std::vector<Target> FindTargetsFromComponents(
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 80a47e0..f67cea1 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -307,8 +307,8 @@
finder_.PreFilter(&imgs);
LOG(INFO, "Blobs: (%zu).\n", imgs.size());
- bool verbose = false;
- std::vector<std::vector<Segment<2>>> raw_polys;
+ constexpr bool verbose = false;
+ ::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);
@@ -323,17 +323,17 @@
LOG(INFO, "Polygons: (%zu).\n", raw_polys.size());
// Calculate each component side of a possible target.
- std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys);
+ ::std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys, verbose);
LOG(INFO, "Components: (%zu).\n", target_component_list.size());
// Put the compenents together into targets.
- std::vector<Target> target_list =
+ ::std::vector<Target> target_list =
finder_.FindTargetsFromComponents(target_component_list, verbose);
LOG(INFO, "Potential Target: (%zu).\n", target_list.size());
// Use the solver to generate an intermediate version of our results.
- std::vector<IntermediateResult> results;
+ ::std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
}