mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-15 06:12:11 +08:00
Code Issues Packages Projects Releases Wiki Activity

more stable calibration (#427)

Browse Source 
* more stable calibration

* no commented print

old-commit-hash: 8b7c726bea
This commit is contained in:
rbiasini
2018-11-08 19:48:21 -08:00
committed by GitHub
parent d6728e6b95
commit 0e7559d7ea
2 changed files with 8 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+7 -1
View File
@@ -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), :]
selfdrive/locationd/get_vp.c
+1 -1
View File
@@ -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) &&