def __init__(self, K): self.MAX_TRACKS = 6000 self.K = K #Array of tracks, each track #has K 5D features preceded #by 5 params that inidicate #[f_idx, last_idx, updated, complete, valid] # f_idx: idx of current last feature in track # idx of of last feature in frame # bool for whether this track has been update # bool for whether this track is complete # bool for whether this track is valid self.tracks = np.zeros((self.MAX_TRACKS, K + 1, 5)) self.tracks[:] = np.nan # Wrap c code for slow matching c_header = "\nvoid merge_features(double *tracks, double *features, long long *empty_idxs);" c_code = "#define K %d\n" % K c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "feature_handler.c")).read() ffi, lib = ffi_wrap('feature_handler', c_code, c_header) def merge_features_c(tracks, features, empty_idxs): lib.merge_features(ffi.cast("double *", tracks.ctypes.data), ffi.cast("double *", features.ctypes.data), ffi.cast("long long *", empty_idxs.ctypes.data)) #self.merge_features = self.merge_features_python self.merge_features = merge_features_c
VP_INIT = np.array([W/2., H/2.]) EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) # big model is 864x288 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 # Wrap c code for slow grid incrementation c_header = "\nvoid increment_grid(double *grid, double *lines, long long n);" c_code = "#define H %d\n" % H c_code += "#define W %d\n" % W c_code += "\n" + open(os.path.join(EXTERNAL_PATH, "get_vp.c")).read() ffi, lib = ffi_wrap('get_vp', c_code, c_header) def increment_grid_c(grid, lines, n): lib.increment_grid(ffi.cast("double *", grid.ctypes.data), ffi.cast("double *", lines.ctypes.data), ffi.cast("long long", n)) def get_lines(p): A = (p[:,0,1] - p[:,1,1]) B = (p[:,1,0] - p[:,0,0]) C = (p[:,0,0]*p[:,1,1] - p[:,1,0]*p[:,0,1]) return np.column_stack((A, B, -C)) def correct_pts(pts, rot_speeds, dt): pts = np.hstack((pts, np.ones((pts.shape[0],1))))