Floating-point precision fix in meili::Project() fixes map-match issue (valhalla#2946)

ktatterso · web-flow · commit 5fda14881f27 · 2021-03-18T11:06:27.000-06:00
Fixed the segfault by introducing some fp-precision and tolerance logic in meili::helpers::Project() and meili::cut_segments().
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -26,6 +26,7 @@
    * FIXED: Skip bindings if there's no Python development version [#2893](https://github.com/valhalla/valhalla/pull/2878)
    * FIXED: Use CMakes built-in Python variables to configure installation [#2931](https://github.com/valhalla/valhalla/pull/2931)
    * FIXED: Sometimes emitting zero-length route geometry when traffic splits edge twice [#2943](https://github.com/valhalla/valhalla/pull/2943)
+   * FIXED: Fix map-match segfault when gps-points project very near a node [#2946](https://github.com/valhalla/valhalla/pull/2946)
    * FIXED: Use kServiceRoad edges while searching for ferry connection [#2933](https://github.com/valhalla/valhalla/pull/2933)
 
 * **Enhancement**
diff --git a/src/meili/CMakeLists.txt b/src/meili/CMakeLists.txt
@@ -5,6 +5,7 @@ set(sources
   topk_search.cc
   routing.cc
   candidate_search.cc
+  geometry_helpers.cc
   transition_cost_model.cc
   map_matcher.cc
   map_matcher_factory.cc
diff --git a/src/meili/candidate_search.cc b/src/meili/candidate_search.cc
@@ -67,9 +67,9 @@ CandidateCollector::WithinSquaredDistance(const midgard::PointLL& location,
 
     // Projection information
     midgard::PointLL point;
-    float sq_distance = 0.f;
+    double sq_distance = 0.0;
     size_t segment;
-    float offset;
+    double offset;
 
     baldr::GraphId snapped_node;
     baldr::PathLocation correlated(baldr::Location(location, stop_type));
@@ -81,10 +81,10 @@ CandidateCollector::WithinSquaredDistance(const midgard::PointLL& location,
       std::tie(point, sq_distance, segment, offset) = helpers::Project(projector, shape);
 
       if (sq_distance <= sq_search_radius) {
-        const float dist = edge->forward() ? offset : 1.f - offset;
-        if (dist == 1.f) {
+        const double dist = edge->forward() ? offset : 1.0 - offset;
+        if (dist == 1.0) {
           snapped_node = edge->endnode();
-        } else if (dist == 0.f) {
+        } else if (dist == 0.0) {
           snapped_node = opp_edge->endnode();
         }
         correlated.edges.emplace_back(edgeid, dist, point, sq_distance);
@@ -100,10 +100,10 @@ CandidateCollector::WithinSquaredDistance(const midgard::PointLL& location,
         std::tie(point, sq_distance, segment, offset) = helpers::Project(projector, shape);
       }
       if (sq_distance <= sq_search_radius) {
-        const float dist = opp_edge->forward() ? offset : 1.f - offset;
-        if (dist == 1.f) {
+        const double dist = opp_edge->forward() ? offset : 1.0 - offset;
+        if (dist == 1.0) {
           snapped_node = opp_edge->endnode();
-        } else if (dist == 0.f) {
+        } else if (dist == 0.0) {
           snapped_node = edge->endnode();
         }
         correlated.edges.emplace_back(opp_edgeid, dist, point, sq_distance);
diff --git a/src/meili/geometry_helpers.cc b/src/meili/geometry_helpers.cc
@@ -0,0 +1,101 @@
+#include "meili/geometry_helpers.h"
+
+#include <valhalla/midgard/constants.h>
+#include <valhalla/midgard/distanceapproximator.h>
+#include <valhalla/midgard/encoded.h>
+#include <valhalla/midgard/pointll.h>
+#include <valhalla/midgard/util.h>
+
+using namespace valhalla;
+using namespace valhalla::meili;
+using namespace valhalla::midgard;
+
+namespace valhalla {
+namespace meili {
+namespace helpers {
+
+// snapped point, squared distance, segment index, offset
+std::tuple<PointLL, double, typename std::vector<PointLL>::size_type, double>
+Project(const projector_t& p, Shape7Decoder<midgard::PointLL>& shape, double snap_distance) {
+  PointLL first_point(shape.pop());
+  auto closest_point = first_point;
+  auto closest_segment_point = first_point;
+  double closest_distance = std::numeric_limits<double>::max();
+  size_t closest_segment = 0;
+  double closest_partial_length = 0.0;
+  double total_length = 0.0;
+
+  // for each segment
+  auto u = first_point;
+  size_t i = 0;
+  for (; !shape.empty(); ++i) {
+    // project a onto b where b is the origin vector representing this segment
+    // and a is the origin vector to the point we are projecting, (a.b/b.b)*b
+    auto v = shape.pop();
+
+    auto projection = p(u, v);
+
+    // check if this point is better
+    const auto distance = p.approx.DistanceSquared(projection);
+    if (distance < closest_distance) {
+      closest_point = std::move(projection);
+      closest_distance = distance;
+      closest_segment = i;
+      closest_partial_length = total_length;
+      closest_segment_point = u;
+    }
+
+    // total edge length
+    total_length += u.Distance(v);
+    u = v;
+  }
+
+  // percent_along is a double between 0 and 1 representing the location of
+  // the closest point on LineString to the given Point, as a fraction
+  // of total 2d line length.
+  closest_partial_length += closest_segment_point.Distance(closest_point);
+  double percent_along =
+      total_length > 0.0 ? static_cast<double>(closest_partial_length / total_length) : 0.0;
+
+  // Not so much "snapping" as recognizing that floating-point has limited precision.
+  // For example:
+  // double k = 1e-18;         // valid
+  // double r = 1.0 - k;       // sure why not
+  // Result: r == 1.0          // exactly 1.0?! yep.
+  //
+  // Downstream logic looks at percentages along the edge and the opp-edge. We want to be
+  // sure that values very close to 0.0 snap to 0.0 since downstream math may flip this
+  // percentage_along to (1.0-percentage_along) if the the edge is not forward, making it
+  // exactly 1.0. This causes issues because the percentage_along the opp_edge is not
+  // exactly 0.0, which is not expected.
+  //
+  // double has ~16 decimal digits of precision. Hence, we will consider 1e-15 as our
+  // representative small figure, below which we snap to 0.0 and within that distance
+  // of 1.0 will snap to 1.0.
+  constexpr double double_precision_at_one = 1e-15;
+  constexpr double very_close_to_one = 1.0 - double_precision_at_one;
+  if (percent_along < double_precision_at_one) {
+    percent_along = 0.0;
+  } else if (percent_along > very_close_to_one) {
+    percent_along = 1.0;
+  }
+
+  // Snap to nearest node using snap_distance
+  if (total_length * percent_along <= snap_distance) {
+    closest_point = first_point;
+    closest_distance = p.approx.DistanceSquared(closest_point);
+    closest_segment = 0;
+    percent_along = 0.f;
+  } else if (total_length * (1.f - percent_along) <= snap_distance) {
+    closest_point = u;
+    closest_distance = p.approx.DistanceSquared(closest_point);
+    closest_segment = i - 1;
+    percent_along = 1.f;
+  }
+
+  return std::make_tuple(std::move(closest_point), closest_distance, closest_segment, percent_along);
+}
+
+} // namespace helpers
+} // namespace meili
+} // namespace valhalla
diff --git a/src/meili/match_route.cc b/src/meili/match_route.cc
@@ -141,15 +141,22 @@ void cut_segments(const std::vector<MatchResult>& match_results,
     if (!curr_match.is_break_point && curr_idx != last_idx) {
       continue;
     }
+
+    // Allow for some fp-fuzz in this gt comparison
+    bool prev_gt_curr = prev_match.distance_along > curr_match.distance_along + 1e-3;
+
     // we want to handle to loop by locating the correct target edge by comparing the distance alone
-    bool loop = prev_match.edgeid == curr_match.edgeid &&
-                prev_match.distance_along > curr_match.distance_along;
+    bool loop = (prev_match.edgeid == curr_match.edgeid) && prev_gt_curr;
+
     // if it is a loop, we start the search after the first edge
     auto last_segment = std::find_if(first_segment + static_cast<size_t>(loop), segments.end(),
                                      [&curr_match](const EdgeSegment& segment) {
                                        return (segment.edgeid == curr_match.edgeid);
                                      });
-    assert(last_segment != segments.cend());
+
+    if (last_segment == segments.cend()) {
+      throw std::logic_error("In meili::cutsegments(), unexpectedly unable to locate target edge.");
+    }
 
     // we need to close the previous edge
     size_t old_size = new_segments.size();
diff --git a/test/geometry.cc b/test/geometry.cc
@@ -0,0 +1,127 @@
+#include "meili/geometry_helpers.h"
+#include "midgard/encoded.h"
+#include "midgard/polyline2.h"
+#include <gtest/gtest.h>
+
+// Foundational geometric unit tests.
+
+using namespace valhalla;
+using namespace valhalla::midgard;
+
+namespace {
+
+//===========================================================================
+// Take a given real-world road shape. Create a second shape that is the
+// reverse shape-point order of the first.
+// Take a handful of "gps points" and project them onto both shapes.
+// Assert that:
+// 0) for percentage_along's that are small, that they're not so small
+//    that 1.0-percentage_along==1.0. See this line of code in
+//    candidate_search.cc, WithinSquaredDistance():
+//      const double dist = edge->forward() ? offset : 1.0 - offset;
+// 1) the fwd/rev projections project to the same location
+// 2) that the fwd_percentage_along + rev_percentage_along == 1.0 (within tol)
+//===========================================================================
+TEST(geometry, fwd_rev_projection_tols) {
+  // Some real world encoded shape data.
+  const std::string fwd_enc_shape =
+      {-24,  -109, -78,  36,   -90,  -4,   -46,  12,   33,   118,  71,   16,   117, 47,   41,   91,
+       113,  44,   45,   112,  -125, 1,    80,   -67,  1,    54,   -1,   1,    117, -31,  2,    -43,
+       1,    -33,  1,    21,   -93,  2,    15,   -25,  1,    47,   -97,  1,    86,  -123, 1,    -64,
+       1,    51,   -110, 2,    48,   -90,  1,    29,   -84,  1,    67,   -94,  2,   84,   -10,  1,
+       -86,  1,    82,   -106, 2,    70,   -32,  1,    64,   -96,  3,    -20,  1,   -126, 2,    112,
+       -56,  3,    26,   -104, 3,    15,   -30,  3,    101,  -48,  3,    -87,  1,   -28,  3,    -107,
+       1,    -62,  1,    113,  118,  -81,  1,    110,  27,   56,   -106, 1,    71,  -78,  1,    -67,
+       1,    -122, 2,    -7,   1,    -40,  1,    -81,  1,    -86,  1,    17,   108, 26,   -84,  1,
+       -100, 1,    54,   -62,  1,    -111, 2,    -120, 3,    -127, 2,    -16,  1,   127,  -62,  1,
+       -43,  1,    -32,  1,    -67,  2,    38,   -49,  1,    -100, 1,    64,   14,  -74,  1,    105,
+       -100, 2,    -125, 1,    -44,  3,    -81,  1,    -126, 3,    -93,  1,    -64, 1,    -67,  2,
+       -82,  3,    -85,  1,    -36,  1,    -75,  1,    48,   -67,  1,    -128, 1,   -111, 1,    -72,
+       2,    -59,  2,    -8,   3,    -113, 3,    -40,  3,    -85,  2,    -126, 2,   -43,  2,    -68,
+       2,    -17,  1,    -46,  1,    -35,  2,    -4,   2,    42,   -106, 1,    -96, 1,    48,   -82,
+       7,    -38,  4,    -102, 2,    -90,  1,    -74,  3,    -36,  1,    -16,  1,   47,   -38,  2,
+       -105, 2,    -70,  1,    -37,  1,    -34,  2,    -85,  2,    -44,  1,    117, -106, 2,    91,
+       -42,  2,    -68,  1,    -18,  2,    -126, 1,    -62,  7,    -62,  2,    -4,  2,    -106, 1,
+       -2,   3,    -118, 1,    -78,  1,    -118, 1};
+
+  std::vector<PointLL> fwd_shape_points = decode7<std::vector<PointLL>>(fwd_enc_shape);
+  std::vector<PointLL> rev_shape_points(fwd_shape_points);
+  std::reverse(rev_shape_points.begin(), rev_shape_points.end());
+  std::string rev_enc_shape = midgard::encode7(rev_shape_points);
+
+  // This first check ensures that the length of a real-world series of segments
+  // is equal when measured forwards and backwards. This is reasonable to expect.
+  // The possible issues that might come into play is if our underlying Distance()
+  // computation is 1) too approximate, and/or 2) employs some sort of curved
+  // earth bias based on the first point of each segment.
+  double fwd_shape_length = midgard::length(fwd_shape_points);
+  double rev_shape_length = midgard::length(rev_shape_points);
+  EXPECT_TRUE(std::fabs(fwd_shape_length - rev_shape_length) < 1e-5);
+
+  // Project these gps points onto the fwd-shape and the rev-shape.
+  // See that they project to the same point and that their projection's
+  // distance-along percentages sum to 1.0.
+  const PointLL gps_points[] = {{13.262609100000001, 38.159699600000003},
+                                {13.262637200000002, 38.159659599999998},
+                                {13.262741800000005, 38.159575799999997},
+                                {13.262665099999999, 38.159486100000001},
+                                {13.262594400000001, 38.159471100000003},
+                                {13.262595700000003, 38.159450400000005}};
+
+  size_t i = 0;
+  for (const auto& gps_point : gps_points) {
+    // meili::helpers::Project() consumes the incoming shape so we have to
+    // create it with each iteration.
+    midgard::Shape7Decoder<midgard::PointLL> fwd_shape(fwd_enc_shape.c_str(), fwd_enc_shape.size());
+
+    double fwd_sq_distance = -1.0;
+    size_t fwd_segment;
+    double fwd_percentage_along = -1.0;
+    PointLL fwd_proj_point;
+    std::tie(fwd_proj_point, fwd_sq_distance, fwd_segment, fwd_percentage_along) =
+        valhalla::meili::helpers::Project(gps_point, fwd_shape);
+
+    // make sure the fwd_percentage_along is big enough that it doesn't
+    // disappear when subtracted from 1.0.
+    if (fwd_percentage_along > 0.0) {
+      double reverse_pct_along = 1.0 - fwd_percentage_along;
+      ASSERT_NE(reverse_pct_along, 1.0);
+    }
+
+    // meili::helpers::Project() consumes the incoming shape so we have to
+    // create it with each iteration.
+    midgard::Shape7Decoder<midgard::PointLL> rev_shape(rev_enc_shape.c_str(), rev_enc_shape.size());
+
+    double rev_sq_distance = -1.0;
+    size_t rev_segment;
+    double rev_percentage_along = -1.0;
+    PointLL rev_proj_point;
+    std::tie(rev_proj_point, rev_sq_distance, rev_segment, rev_percentage_along) =
+        valhalla::meili::helpers::Project(gps_point, rev_shape);
+
+    // make sure the rev_percentage_along is big enough that it doesn't
+    // disappear when subtracted from 1.0.
+    if (rev_percentage_along > 0.0) {
+      double forward_pct_along = 1.0 - rev_percentage_along;
+      ASSERT_NE(forward_pct_along, 1.0);
+    }
+
+    // The first gps point is a special case! Without the fix, fwd_percentage_along
+    // is ~1e-9; the fix is to snap it to 0.f.
+    if (i++ == 0) {
+      ASSERT_EQ(fwd_percentage_along, 0.0);
+      ASSERT_EQ(rev_percentage_along, 1.0);
+    }
+
+    // 6 decimal digits is roughly 0.1 m, a reasonable expectation
+    constexpr double tenth_of_meter_in_deg = 0.000001;
+    ASSERT_TRUE(std::fabs(fwd_proj_point.lat() - rev_proj_point.lat()) < tenth_of_meter_in_deg);
+    ASSERT_TRUE(std::fabs(fwd_proj_point.lng() - rev_proj_point.lng()) < tenth_of_meter_in_deg);
+
+    // wherever we project, the fwd+rev percentages must add up to 1.0 (within tol)
+    double total_percent = fwd_percentage_along + rev_percentage_along;
+    ASSERT_TRUE(std::fabs(total_percent - 1.0) < 0.0001);
+  }
+}
+
+} // anonymous namespace
diff --git a/valhalla/meili/geometry_helpers.h b/valhalla/meili/geometry_helpers.h
@@ -2,7 +2,6 @@
 #pragma once
 #include <cmath>
 
-#include <algorithm>
 #include <vector>
 
 #include <valhalla/midgard/aabb2.h>
@@ -16,65 +15,11 @@ namespace valhalla {
 namespace meili {
 namespace helpers {
 
-// snapped point, sqaured distance, segment index, offset
-inline std::tuple<midgard::PointLL, float, typename std::vector<midgard::PointLL>::size_type, float>
+// snapped point, squared distance, segment index, offset
+std::tuple<midgard::PointLL, double, typename std::vector<midgard::PointLL>::size_type, double>
 Project(const midgard::projector_t& p,
         midgard::Shape7Decoder<midgard::PointLL>& shape,
-        float snap_distance = 0.f) {
-  midgard::PointLL first_point(shape.pop());
-  auto closest_point = first_point;
-  auto closest_segment_point = first_point;
-  float closest_distance = p.approx.DistanceSquared(closest_point);
-  size_t closest_segment = 0;
-  float closest_partial_length = 0.f;
-  float total_length = 0.f;
-
-  // for each segment
-  auto u = first_point;
-  size_t i = 0;
-  for (; !shape.empty(); ++i) {
-    // project a onto b where b is the origin vector representing this segment
-    // and a is the origin vector to the point we are projecting, (a.b/b.b)*b
-    auto v = shape.pop();
-    auto point = p(u, v);
-
-    // check if this point is better
-    const auto distance = p.approx.DistanceSquared(point);
-    if (distance < closest_distance) {
-      closest_point = std::move(point);
-      closest_distance = distance;
-      closest_segment = i;
-      closest_partial_length = total_length;
-      closest_segment_point = u;
-    }
-
-    // total edge length
-    total_length += u.Distance(v);
-    u = v;
-  }
-
-  // Offset is a float between 0 and 1 representing the location of
-  // the closest point on LineString to the given Point, as a fraction
-  // of total 2d line length.
-  closest_partial_length += closest_segment_point.Distance(closest_point);
-  float offset = total_length > 0.f ? static_cast<float>(closest_partial_length / total_length) : 0.f;
-  offset = std::max(0.f, std::min(offset, 1.f));
-
-  // Snap to vertices if it's close
-  if (total_length * offset <= snap_distance) {
-    closest_point = first_point;
-    closest_distance = p.approx.DistanceSquared(closest_point);
-    closest_segment = 0;
-    offset = 0.f;
-  } else if (total_length * (1.f - offset) <= snap_distance) {
-    closest_point = u;
-    closest_distance = p.approx.DistanceSquared(closest_point);
-    closest_segment = i - 1;
-    offset = 1.f;
-  }
-
-  return std::make_tuple(std::move(closest_point), closest_distance, closest_segment, offset);
-}
+        double snap_distance = 0.0);
 
 } // namespace helpers
 } // namespace meili
