From 8b7c726bea0425d4a9cf7b0da7e09e89c71081c7 Mon Sep 17 00:00:00 2001 From: rbiasini Date: Thu, 8 Nov 2018 19:48:21 -0800 Subject: [PATCH] more stable calibration (#427) * more stable calibration * no commented print --- selfdrive/locationd/calibrationd.py | 8 +++++++- selfdrive/locationd/get_vp.c | 2 +- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 05ba209ed9a771..695e698cee970c 100755 --- 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 index 3e98f995e8370f..8a48c88150b4cb 100644 --- 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) &&