#ifndef distance_h__ #define distance_h__ #include #include #include #include "DistancePointEllipse.h" using namespace singleeyefitter; template auto euclidean_distance(const Eigen::MatrixBase& p1, const Eigen::MatrixBase& p2) -> decltype((p1 - p2).norm()) { return (p1 - p2).norm(); } template auto euclidean_distance(const Eigen::MatrixBase& point, const Eigen::ParametrizedLine& line) -> decltype(point.norm()) { return ((line.origin() - point) - ((line.origin() - point).dot(line.direction()))*line.direction()).norm(); } template Scalar euclidean_distance(const Eigen::Matrix& p, const Eigen::Matrix& v, const Eigen::Matrix& w) { // Return minimum distance between line segment vw and point p auto l2 = (v - w).squaredNorm(); if (l2 == 0.0) return euclidean_distance(p, v); // v == w case // Consider the line extending the segment, parameterized as v + t (w - v). // We find projection of point p onto the line. // It falls where t = [(p-v) . (w-v)] / |w-v|^2 auto t = (p - v).dot(w - v) / l2; if (t < 0.0) return euclidean_distance(p, v); // Beyond the 'v' end of the segment else if (t > 1.0) return euclidean_distance(p, w); // Beyond the 'w' end of the segment auto projection = v + t * (w - v); // Projection falls on the segment return euclidean_distance(p, projection); } template Scalar euclidean_distance(const Eigen::Matrix& point, const std::vector>& polygon) { auto from = polygon.back(); Scalar min_distance = std::numeric_limits::infinity(); for (const auto& to : polygon) { min_distance = std::min(min_distance, euclidean_distance(point, from, to)); from = to; } return min_distance; } template Scalar euclidean_distance(const Eigen::Matrix& point, const Ellipse2D& ellipse) { return DistancePointEllipse(ellipse, point[0], point[1]); } template Scalar oneway_hausdorff_distance(const Ellipse2D& ellipse, const TOther& other) { Scalar max_dist = -1; for (Scalar i = 0; i < 100; ++i) { Scalar t = i * 2*PI / 100; auto pt = pointAlongEllipse(ellipse, t); Scalar i_dist = euclidean_distance(pt, other); max_dist = std::max(max_dist, i_dist); } return max_dist; } template Scalar oneway_hausdorff_distance(const std::vector>& polygon, const TOther& other) { Scalar max_dist = -1; for (const auto& pt : polygon) { Scalar pt_dist = euclidean_distance(pt, other); max_dist = std::max(max_dist, pt_dist); } return max_dist; } template Scalar hausdorff_distance(const Ellipse2D& ellipse, const TOther& other) { return std::max(oneway_hausdorff_distance(ellipse, other), oneway_hausdorff_distance(ellipse, other)); } template typename std::enable_if>::value, Scalar>::type hausdorff_distance(const TOther& other, const Ellipse2D& ellipse) { return hausdorff_distance(ellipse, other); } #endif // distance_h__