diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 05ba209ed9..695e698cee 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -30,6 +30,7 @@ EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) 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 @@ class Calibrator(object): 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 3e98f995e8..8a48c88150 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) &&