Don't allow gaps in the tape and fix goal size
We were having cases where the 5 points in the polyline were being
matched to 3 tape segments. This was returning results reporting
significantly further than the actual distance. Compress any gaps in
the tape numbers. This drives our solution much more to be based off
tape distance centers.
Change-Id: I9c60315fdd903ebd9400a95f7b276eb37b63438c
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
index 174f774..f158626 100644
--- a/y2022/vision/target_estimator.h
+++ b/y2022/vision/target_estimator.h
@@ -68,8 +68,13 @@
const Eigen::Transform<S, 3, Eigen::Affine> &H_hub_camera) const;
template <typename S>
- cv::Point_<S> DistanceFromTape(
+ size_t ClosestTape(size_t centroid_index,
+ const std::vector<cv::Point_<S>> &tape_points) const;
+
+ template <typename S>
+ cv::Point_<S> DistanceFromTapeIndex(
size_t centroid_index,
+ size_t tape_index,
const std::vector<cv::Point_<S>> &tape_points) const;
void DrawProjectedHub(const std::vector<cv::Point2d> &tape_points_proj,