Track the lowest point in each side of the target
Change-Id: Ic02df9c466a82ca7e074beb49c25b6712bbe20d7
diff --git a/aos/vision/debug/overlay.h b/aos/vision/debug/overlay.h
index 8eb8a23..c668e17 100644
--- a/aos/vision/debug/overlay.h
+++ b/aos/vision/debug/overlay.h
@@ -129,6 +129,11 @@
lines_.emplace_back(std::pair<Segment<2>, PixelRef>(seg, newColor));
}
+ void DrawCross(::Eigen::Vector2f center, int width,
+ aos::vision::PixelRef color) {
+ DrawCross(aos::vision::Vector<2>(center.x(), center.y()), width, color);
+ }
+
void DrawCross(aos::vision::Vector<2> center, int width,
aos::vision::PixelRef color) {
using namespace aos::vision;
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 8ef67fa..e217d53 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -96,7 +96,7 @@
target_finder_.PreFilter(&imgs);
// Find polygons from blobs.
- std::vector<Polygon> raw_polys;
+ ::std::vector<Polygon> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
ContourNode *contour = target_finder_.GetContour(blob);
@@ -142,9 +142,10 @@
std::vector<TargetComponent> target_component_list =
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},
+ for (const TargetComponent &component : target_component_list) {
+ DrawComponent(component, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
{0, 0, 255});
+ overlay_.DrawCross(component.bottom_point, 4, {128, 0, 255});
}
}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index fb2a134..b2b5a90 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -34,7 +34,7 @@
imgs->end());
}
-ContourNode* TargetFinder::GetContour(const RangeImage &blob) {
+ContourNode *TargetFinder::GetContour(const RangeImage &blob) {
alloc_.reset();
return RangeImgToContour(blob, &alloc_);
}
@@ -42,6 +42,10 @@
// TODO(ben): These values will be moved into the constants.h file.
namespace {
+::Eigen::Vector2f AosVectorToEigenVector(Vector<2> in) {
+ return ::Eigen::Vector2f(in.x(), in.y());
+}
+
constexpr double f_x = 481.4957;
constexpr double c_x = 341.215;
constexpr double f_y = 484.314;
@@ -323,14 +327,15 @@
const ::std::vector<Polygon> &seg_list, bool verbose) {
::std::vector<TargetComponent> list;
TargetComponent new_target;
- for (const Polygon &poly : seg_list) {
+ for (const Polygon &polygon : seg_list) {
// Reject missized pollygons for now. Maybe rectify them here in the future;
- if (poly.segments.size() != 4) {
+ if (polygon.segments.size() != 4) {
continue;
}
::std::vector<Vector<2>> corners;
for (size_t i = 0; i < 4; ++i) {
- Vector<2> corner = poly.segments[i].Intersect(poly.segments[(i + 1) % 4]);
+ Vector<2> corner =
+ polygon.segments[i].Intersect(polygon.segments[(i + 1) % 4]);
if (::std::isnan(corner.x()) || ::std::isnan(corner.y())) {
break;
}
@@ -428,8 +433,33 @@
}
}
+ // Take the vector which points from the bottom to the top of the target
+ // along the outside edge.
+ const ::Eigen::Vector2f outer_edge_vector =
+ AosVectorToEigenVector(new_target.top - new_target.outside);
+ // Now, dot each point in the perimeter along this vector. The one with the
+ // smallest component will be the one closest to the bottom along this
+ // direction vector.
+ ::Eigen::Vector2f smallest_point = polygon.contour[0];
+ float smallest_value = outer_edge_vector.transpose() * smallest_point;
+ for (const ::Eigen::Vector2f point : polygon.contour) {
+ const float current_value = outer_edge_vector.transpose() * point;
+ if (current_value < smallest_value) {
+ smallest_value = current_value;
+ smallest_point = point;
+ }
+ }
+
+ // This piece of the target should be ready now.
+ new_target.bottom_point = smallest_point;
+ if (verbose) {
+ printf("Lowest point in the blob is (%f, %f)\n", smallest_point.x(),
+ smallest_point.y());
+ }
+
// This piece of the target should be ready now.
list.emplace_back(new_target);
+
if (verbose) printf("Happy with a target\n");
}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index 8467da8..8ee1f4c 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -25,12 +25,20 @@
}
}
bool is_right;
- aos::vision::Vector<2> top;
- aos::vision::Vector<2> inside;
- aos::vision::Vector<2> outside;
- aos::vision::Vector<2> bottom;
+ // The point which is the upper outside point on this side of the target pair.
+ ::aos::vision::Vector<2> top;
+ // The point which is the upper inside point on this side of the target pair.
+ ::aos::vision::Vector<2> inside;
+ // The point which is the outer bottom point on this side of the target pair.
+ ::aos::vision::Vector<2> outside;
+ // The point which is the inner bottom point on this side of the target pair.
+ ::aos::vision::Vector<2> bottom;
aos::vision::Segment<2> major_axis;
+
+ // The point with is the "lowest" along the outer edge. This point is useful
+ // for making sure clipped targets are "big enough" to cover all the pixels.
+ ::Eigen::Vector2f bottom_point;
};
// Convert back to screen space for final result.