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))))