Avoid false shallow-water barriers from depth roundoff

Author: woensug-choi

src/posim_routing/coastal.py

@@ -17,6 +17,7 @@
 from pyproj import Geod
 from scipy.ndimage import distance_transform_edt, maximum_filter, label
 from scipy.spatial import cKDTree
+from .navigation import DEPTH_EPSILON_M
 
 GEO = Geod(ellps="WGS84")
 COAST_DISCOVERY_M = 80000.
@@ -139,7 +140,7 @@ def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None, prog
     field = TerrainField(terrain["xs"], terrain["ys"], terrain["heights"])
     xs, ys, heights = field.xs, field.ys, field.heights
     finite = np.isfinite(heights)
-    land = finite & (heights > -MINIMUM_DEPTH_M)
+    land = finite & (heights > -MINIMUM_DEPTH_M+DEPTH_EPSILON_M)
     if guard is not None:
         land |= ~guard.grid(xs, ys)
     if not land.any():
@@ -159,7 +160,7 @@ def coastal_corridor(terrain, spec, start, target, timeout=25., guard=None, prog
     size = (2*math.ceil(margin/dy)+1, 2*math.ceil(margin/dx)+1)
     bounds = maximum_filter(np.where(finite, heights, math.inf), size=size,
                             mode="constant", cval=math.inf)
-    safe = bounds <= -MINIMUM_DEPTH_M
+    safe = bounds <= -MINIMUM_DEPTH_M+DEPTH_EPSILON_M
     if guard is not None:
         safe &= guard.grid(xs, ys, margin)
     coast_distance = distance_transform_edt(~land, sampling=(dy, dx))

src/posim_routing/navigation.py

@@ -9,6 +9,10 @@
 import time
 import numpy as np
 
+# Interpolating a flat two-metre floor can produce -1.9999999999999998.
+# Ignore arithmetic roundoff without changing the represented depth policy.
+DEPTH_EPSILON_M = 1e-9
+
 
 class TerrainField:
     def __init__(self, xs, ys, heights):
@@ -85,7 +89,7 @@ def __init__(self, field, start, boat, step=4., timeout=25., maximum_radius=600.
     def safe(self, xy, z, radius=None):
         if math.dist(xy,self.origin)>self.maximum_radius:return False
         floor=self.field.maximum(*xy,self.radius if radius is None else radius)
-        return floor is not None and floor<0 and z-floor>=self.clearance
+        return floor is not None and floor<0 and z-floor>=self.clearance-DEPTH_EPSILON_M
 
     def segment(self, a, b, record=False):
         distance=math.dist(a[:2],b[:2]);count=max(1,math.ceil(distance/1.))
@@ -97,7 +101,7 @@ def segment(self, a, b, record=False):
             f=k/count;xy=[a[i]+(b[i]-a[i])*f for i in (0,1)]
             if math.dist(xy,self.origin)>self.maximum_radius:return False
             floor=self.field.maximum(*xy,radius)
-            if floor is None or floor>=0 or z-floor<self.clearance:return False
+            if floor is None or floor>=0 or z-floor<self.clearance-DEPTH_EPSILON_M:return False
             minimum=min(minimum,z-floor)
         if record:self.minimum=min(self.minimum,minimum)
         return True

src/posim_routing/shallow.py

@@ -3,14 +3,15 @@
 import numpy as np
 from scipy.ndimage import distance_transform_edt
 from scipy.interpolate import RegularGridInterpolator
+from .navigation import DEPTH_EPSILON_M
 
 MINIMUM_DEPTH_M=2.
 SHALLOW_BUFFER_M=200.
 
 
 def clearance_field(field):
     dx,dy=float(np.max(np.diff(field.xs))),float(np.max(np.diff(field.ys)))
-    shallow=~np.isfinite(field.heights) | (field.heights > -MINIMUM_DEPTH_M)
+    shallow=~np.isfinite(field.heights) | (field.heights > -MINIMUM_DEPTH_M+DEPTH_EPSILON_M)
     distances=distance_transform_edt(~shallow,sampling=(dy,dx))
     return RegularGridInterpolator((field.ys,field.xs),distances,
                                   bounds_error=False,fill_value=0.)

tests/test_shallow.py

@@ -14,6 +14,22 @@ def terrain():
     return dict(xs=axis.tolist(),ys=axis.tolist(),heights=np.where(x<0,-1.,-15.).tolist())
 
 class ShallowExit(unittest.TestCase):
+    def test_roundoff_on_exact_depth_plateau_does_not_create_a_shoal(self):
+        axis=np.linspace(-1000,1000,41)
+        for floor,expected in ((np.nextafter(-2.,0.),True),(-2.+1e-6,False)):
+            raw=TerrainField(axis,axis,np.full((41,41),floor))
+            planner=DetourPlanner(CachedTerrain(raw),[0,0,0],True)
+            planner.radius=200.;planner.clearance=2.
+            self.assertEqual(planner.safe([0,0],0.),expected)
+            self.assertEqual(planner.segment([0,0,0],[100,100,0]),expected)
+            clearance=clearance_field(raw)
+            self.assertEqual(float(clearance([[0,0]])[0])>0,expected)
+    def test_roundoff_tolerance_does_not_accept_unknown_depth(self):
+        axis=np.linspace(-1000,1000,41);heights=np.full((41,41),-2.)
+        heights[20,20]=np.nan
+        planner=DetourPlanner(CachedTerrain(TerrainField(axis,axis,heights)),[0,0,0],True)
+        planner.radius=200.;planner.clearance=2.
+        self.assertFalse(planner.safe([0,0],0.))
     def test_already_clear_position_needs_no_departure(self):
         self.assertIsNone(exit_corridor(terrain(),SPEC,geo(400,0),geo(1000,2000)))
     def test_inside_buffer_exits_under_propulsion_with_increasing_separation(self):