more stable calibration (#427)

* more stable calibration * no commented print
commaai · Nov 9, 2018 · 8b7c726 · 8b7c726
1 parent db24b1e
commit 8b7c726
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 2 deletions.
diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
@@ -30,6 +30,7 @@
 VP_VALIDITY_CORNERS = np.array([[-150., -200.], [150., 200.]])  + VP_INIT
 GRID_WEIGHT_INIT = 2e6
 MAX_LINES = 500    # max lines to avoid over computation
+HOOD_HEIGHT = H*3/4 # the part of image usually free from the car's hood
 
 DEBUG = os.getenv("DEBUG") is not None
 
@@ -135,7 +136,12 @@ def update(self, uvs, yaw_rate, speed):
       return
     rot_speeds = np.array([0.,0.,-yaw_rate])
     uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))
-    good_tracks = np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10
+    # exclude tracks where:
+    # - pixel movement was less than 10 pixels
+    # - tracks are in the "hood region"
+    good_tracks = np.all([np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10,
+                  uvs[:,0,1] < HOOD_HEIGHT,
+                  uvs[:,1,1] < HOOD_HEIGHT], axis = 0)
     uvs = uvs[good_tracks]
     if uvs.shape[0] > MAX_LINES:
       uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :]

diff --git a/selfdrive/locationd/get_vp.c b/selfdrive/locationd/get_vp.c
@@ -11,7 +11,7 @@ int get_intersections(double *lines, double *intersections, long long n) {
       Dx = L1[2] * L2[1] - L1[1] * L2[2];
       Dy = L1[0] * L2[2] - L1[2] * L2[0];
       // only intersect lines from different quadrants and only left-right crossing
-      if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[0]*L2[0] < 0)){
+      if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[1]*L2[1] < 0)){
         x = Dx / D;
         y = Dy / D;
         if ((0 < x) &&