Vehicle Researcher
7 years ago
parent
db96b4b912
commit
0129a8a4ff
47 changed files with 2266 additions and 229 deletions
@ -0,0 +1,42 @@ |
|||||||
|
import os |
||||||
|
import sys |
||||||
|
import fcntl |
||||||
|
import hashlib |
||||||
|
from cffi import FFI |
||||||
|
|
||||||
|
TMPDIR = "/tmp/ccache" |
||||||
|
|
||||||
|
def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR): |
||||||
|
cache = name + "_" + hashlib.sha1(c_code).hexdigest() |
||||||
|
try: |
||||||
|
os.mkdir(tmpdir) |
||||||
|
except OSError: |
||||||
|
pass |
||||||
|
|
||||||
|
fd = os.open(tmpdir, 0) |
||||||
|
fcntl.flock(fd, fcntl.LOCK_EX) |
||||||
|
try: |
||||||
|
sys.path.append(tmpdir) |
||||||
|
try: |
||||||
|
mod = __import__(cache) |
||||||
|
except Exception: |
||||||
|
print "cache miss", cache |
||||||
|
compile_code(cache, c_code, c_header, tmpdir) |
||||||
|
mod = __import__(cache) |
||||||
|
finally: |
||||||
|
os.close(fd) |
||||||
|
|
||||||
|
return mod.ffi, mod.lib |
||||||
|
|
||||||
|
def compile_code(name, c_code, c_header, directory): |
||||||
|
ffibuilder = FFI() |
||||||
|
ffibuilder.set_source(name, c_code, source_extension='.cpp') |
||||||
|
ffibuilder.cdef(c_header) |
||||||
|
os.environ['OPT'] = "-fwrapv -O2 -DNDEBUG -std=c++11" |
||||||
|
os.environ['CFLAGS'] = "" |
||||||
|
ffibuilder.compile(verbose=True, debug=False, tmpdir=directory) |
||||||
|
|
||||||
|
def wrap_compiled(name, directory): |
||||||
|
sys.path.append(directory) |
||||||
|
mod = __import__(name) |
||||||
|
return mod.ffi, mod.lib |
||||||
@ -0,0 +1,115 @@ |
|||||||
|
import numpy as np |
||||||
|
import common.transformations.orientation as orient |
||||||
|
|
||||||
|
FULL_FRAME_SIZE = (1164, 874) |
||||||
|
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] |
||||||
|
eon_focal_length = FOCAL = 910.0 |
||||||
|
|
||||||
|
# aka 'K' aka camera_frame_from_view_frame |
||||||
|
eon_intrinsics = np.array([ |
||||||
|
[FOCAL, 0., W/2.], |
||||||
|
[ 0., FOCAL, H/2.], |
||||||
|
[ 0., 0., 1.]]) |
||||||
|
|
||||||
|
# aka 'K_inv' aka view_frame_from_camera_frame |
||||||
|
eon_intrinsics_inv = np.linalg.inv(eon_intrinsics) |
||||||
|
|
||||||
|
|
||||||
|
# device/mesh : x->forward, y-> right, z->down |
||||||
|
# view : x->right, y->down, z->forward |
||||||
|
device_frame_from_view_frame = np.array([ |
||||||
|
[ 0., 0., 1.], |
||||||
|
[ 1., 0., 0.], |
||||||
|
[ 0., 1., 0.] |
||||||
|
]) |
||||||
|
view_frame_from_device_frame = device_frame_from_view_frame.T |
||||||
|
|
||||||
|
|
||||||
|
def get_calib_from_vp(vp): |
||||||
|
vp_norm = normalize(vp) |
||||||
|
yaw_calib = np.arctan(vp_norm[0]) |
||||||
|
pitch_calib = np.arctan(vp_norm[1]*np.cos(yaw_calib)) |
||||||
|
# TODO should be, this but written |
||||||
|
# to be compatible with meshcalib and |
||||||
|
# get_view_frame_from_road_fram |
||||||
|
#pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) |
||||||
|
roll_calib = 0 |
||||||
|
return roll_calib, pitch_calib, yaw_calib |
||||||
|
|
||||||
|
# aka 'extrinsic_matrix' |
||||||
|
# road : x->forward, y -> left, z->up |
||||||
|
def get_view_frame_from_road_frame(roll, pitch, yaw, height): |
||||||
|
# TODO |
||||||
|
# calibration pitch is currently defined |
||||||
|
# opposite to pitch in device frame |
||||||
|
pitch = -pitch |
||||||
|
device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1])) |
||||||
|
view_from_road = view_frame_from_device_frame.dot(device_from_road) |
||||||
|
return np.hstack((view_from_road, [[0], [height], [0]])) |
||||||
|
|
||||||
|
|
||||||
|
def vp_from_ke(m): |
||||||
|
""" |
||||||
|
Computes the vanishing point from the product of the intrinsic and extrinsic |
||||||
|
matrices C = KE. |
||||||
|
|
||||||
|
The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T |
||||||
|
""" |
||||||
|
return (m[0, 0]/m[2,0], m[1,0]/m[2,0]) |
||||||
|
|
||||||
|
def roll_from_ke(m): |
||||||
|
# note: different from calibration.h/RollAnglefromKE: i think that one's just wrong |
||||||
|
return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]), |
||||||
|
-(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) |
||||||
|
|
||||||
|
def normalize(img_pts): |
||||||
|
# normalizes image coordinates |
||||||
|
# accepts single pt or array of pts |
||||||
|
img_pts = np.array(img_pts) |
||||||
|
input_shape = img_pts.shape |
||||||
|
img_pts = np.atleast_2d(img_pts) |
||||||
|
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1)))) |
||||||
|
img_pts_normalized = eon_intrinsics_inv.dot(img_pts.T).T |
||||||
|
img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan |
||||||
|
return img_pts_normalized[:,:2].reshape(input_shape) |
||||||
|
|
||||||
|
def denormalize(img_pts): |
||||||
|
# denormalizes image coordinates |
||||||
|
# accepts single pt or array of pts |
||||||
|
img_pts = np.array(img_pts) |
||||||
|
input_shape = img_pts.shape |
||||||
|
img_pts = np.atleast_2d(img_pts) |
||||||
|
img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1)))) |
||||||
|
img_pts_denormalized = eon_intrinsics.dot(img_pts.T).T |
||||||
|
img_pts_denormalized[img_pts_denormalized[:,0] > W] = np.nan |
||||||
|
img_pts_denormalized[img_pts_denormalized[:,0] < 0] = np.nan |
||||||
|
img_pts_denormalized[img_pts_denormalized[:,1] > H] = np.nan |
||||||
|
img_pts_denormalized[img_pts_denormalized[:,1] < 0] = np.nan |
||||||
|
return img_pts_denormalized[:,:2].reshape(input_shape) |
||||||
|
|
||||||
|
def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef): |
||||||
|
# device from ecef frame |
||||||
|
# device frame is x -> forward, y-> right, z -> down |
||||||
|
# accepts single pt or array of pts |
||||||
|
input_shape = pt_ecef.shape |
||||||
|
pt_ecef = np.atleast_2d(pt_ecef) |
||||||
|
ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef) |
||||||
|
device_from_ecef_rot = ecef_from_device_rot.T |
||||||
|
pt_ecef_rel = pt_ecef - pos_ecef |
||||||
|
pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel) |
||||||
|
return pt_device.reshape(input_shape) |
||||||
|
|
||||||
|
def img_from_device(pt_device): |
||||||
|
# img coordinates from pts in device frame |
||||||
|
# first transforms to view frame, then to img coords |
||||||
|
# accepts single pt or array of pts |
||||||
|
input_shape = pt_device.shape |
||||||
|
pt_device = np.atleast_2d(pt_device) |
||||||
|
pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device) |
||||||
|
|
||||||
|
# This function should never return negative depths |
||||||
|
pt_view[pt_view[:,2] < 0] = np.nan |
||||||
|
|
||||||
|
pt_img = pt_view/pt_view[:,2:3] |
||||||
|
return pt_img.reshape(input_shape)[:,:2] |
||||||
|
|
||||||
@ -0,0 +1,112 @@ |
|||||||
|
import numpy as np |
||||||
|
|
||||||
|
from common.transformations.camera import eon_focal_length, \ |
||||||
|
vp_from_ke, \ |
||||||
|
get_view_frame_from_road_frame, \ |
||||||
|
FULL_FRAME_SIZE |
||||||
|
|
||||||
|
# segnet |
||||||
|
|
||||||
|
SEGNET_SIZE = (512, 384) |
||||||
|
|
||||||
|
segnet_frame_from_camera_frame = np.array([ |
||||||
|
[float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0], 0., ], |
||||||
|
[ 0., float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]]) |
||||||
|
|
||||||
|
|
||||||
|
# model |
||||||
|
|
||||||
|
MODEL_INPUT_SIZE = (320, 160) |
||||||
|
MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2) |
||||||
|
MODEL_CX = MODEL_INPUT_SIZE[0]/2. |
||||||
|
MODEL_CY = 21. |
||||||
|
|
||||||
|
model_zoom = 1.25 |
||||||
|
model_height = 1.22 |
||||||
|
|
||||||
|
# canonical model transform |
||||||
|
model_intrinsics = np.array( |
||||||
|
[[ eon_focal_length / model_zoom, 0. , MODEL_CX], |
||||||
|
[ 0. , eon_focal_length / model_zoom, MODEL_CY], |
||||||
|
[ 0. , 0. , 1.]]) |
||||||
|
|
||||||
|
|
||||||
|
# BIG model |
||||||
|
|
||||||
|
BIGMODEL_INPUT_SIZE = (864, 288) |
||||||
|
BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) |
||||||
|
|
||||||
|
bigmodel_zoom = 1. |
||||||
|
bigmodel_intrinsics = np.array( |
||||||
|
[[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], |
||||||
|
[ 0. , eon_focal_length / bigmodel_zoom, 0.2 * BIGMODEL_INPUT_SIZE[1]], |
||||||
|
[ 0. , 0. , 1.]]) |
||||||
|
|
||||||
|
|
||||||
|
bigmodel_border = np.array([ |
||||||
|
[0,0,1], |
||||||
|
[BIGMODEL_INPUT_SIZE[0], 0, 1], |
||||||
|
[BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1], 1], |
||||||
|
[0, BIGMODEL_INPUT_SIZE[1], 1], |
||||||
|
]) |
||||||
|
|
||||||
|
|
||||||
|
model_frame_from_road_frame = np.dot(model_intrinsics, |
||||||
|
get_view_frame_from_road_frame(0, 0, 0, model_height)) |
||||||
|
|
||||||
|
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics, |
||||||
|
get_view_frame_from_road_frame(0, 0, 0, model_height)) |
||||||
|
|
||||||
|
# 'camera from model camera' |
||||||
|
def get_model_height_transform(camera_frame_from_road_frame, height): |
||||||
|
camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([ |
||||||
|
[1, 0, 0], |
||||||
|
[0, 1, 0], |
||||||
|
[0, 0, 0], |
||||||
|
[0, 0, 1], |
||||||
|
])) |
||||||
|
|
||||||
|
camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([ |
||||||
|
[1, 0, 0], |
||||||
|
[0, 1, 0], |
||||||
|
[0, 0, height - model_height], |
||||||
|
[0, 0, 1], |
||||||
|
])) |
||||||
|
|
||||||
|
ground_from_camera_frame = np.linalg.inv(camera_frame_from_road_ground) |
||||||
|
|
||||||
|
low_camera_from_high_camera = np.dot(camera_frame_from_road_high, ground_from_camera_frame) |
||||||
|
high_camera_from_low_camera = np.linalg.inv(low_camera_from_high_camera) |
||||||
|
|
||||||
|
return high_camera_from_low_camera |
||||||
|
|
||||||
|
|
||||||
|
# camera_frame_from_model_frame aka 'warp matrix' |
||||||
|
# was: calibration.h/CalibrationTransform |
||||||
|
def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height): |
||||||
|
vp = vp_from_ke(camera_frame_from_road_frame) |
||||||
|
|
||||||
|
model_camera_from_model_frame = np.array([ |
||||||
|
[model_zoom, 0., vp[0] - MODEL_CX * model_zoom], |
||||||
|
[ 0., model_zoom, vp[1] - MODEL_CY * model_zoom], |
||||||
|
[ 0., 0., 1.], |
||||||
|
]) |
||||||
|
|
||||||
|
# This function is super slow, so skip it if height is very close to canonical |
||||||
|
# TODO: speed it up! |
||||||
|
if abs(height - model_height) > 0.001: # |
||||||
|
camera_from_model_camera = get_model_height_transform(camera_frame_from_road_frame, height) |
||||||
|
else: |
||||||
|
camera_from_model_camera = np.eye(3) |
||||||
|
|
||||||
|
return np.dot(camera_from_model_camera, model_camera_from_model_frame) |
||||||
|
|
||||||
|
|
||||||
|
def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame): |
||||||
|
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)] |
||||||
|
bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)] |
||||||
|
|
||||||
|
ground_from_bigmodel_frame = np.linalg.inv(bigmodel_frame_from_ground) |
||||||
|
camera_frame_from_bigmodel_frame = np.dot(camera_frame_from_ground, ground_from_bigmodel_frame) |
||||||
|
|
||||||
|
return camera_frame_from_bigmodel_frame |
||||||
@ -0,0 +1,293 @@ |
|||||||
|
import numpy as np |
||||||
|
from numpy import dot, inner, array, linalg |
||||||
|
from common.transformations.coordinates import LocalCoord |
||||||
|
|
||||||
|
|
||||||
|
''' |
||||||
|
Vectorized functions that transform between |
||||||
|
rotation matrices, euler angles and quaternions. |
||||||
|
All support lists, array or array of arrays as inputs. |
||||||
|
Supports both x2y and y_from_x format (y_from_x preferred!). |
||||||
|
''' |
||||||
|
|
||||||
|
def euler2quat(eulers): |
||||||
|
eulers = array(eulers) |
||||||
|
if len(eulers.shape) > 1: |
||||||
|
output_shape = (-1,4) |
||||||
|
else: |
||||||
|
output_shape = (4,) |
||||||
|
eulers = np.atleast_2d(eulers) |
||||||
|
gamma, theta, psi = eulers[:,0], eulers[:,1], eulers[:,2] |
||||||
|
|
||||||
|
q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \ |
||||||
|
np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2) |
||||||
|
q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \ |
||||||
|
np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2) |
||||||
|
q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \ |
||||||
|
np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) |
||||||
|
q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \ |
||||||
|
np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) |
||||||
|
|
||||||
|
quats = array([q0, q1, q2, q3]).T |
||||||
|
for i in xrange(len(quats)): |
||||||
|
if quats[i,0] < 0: |
||||||
|
quats[i] = -quats[i] |
||||||
|
return quats.reshape(output_shape) |
||||||
|
|
||||||
|
|
||||||
|
def quat2euler(quats): |
||||||
|
quats = array(quats) |
||||||
|
if len(quats.shape) > 1: |
||||||
|
output_shape = (-1,3) |
||||||
|
else: |
||||||
|
output_shape = (3,) |
||||||
|
quats = np.atleast_2d(quats) |
||||||
|
q0, q1, q2, q3 = quats[:,0], quats[:,1], quats[:,2], quats[:,3] |
||||||
|
|
||||||
|
gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) |
||||||
|
theta = np.arcsin(2 * (q0 * q2 - q3 * q1)) |
||||||
|
psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) |
||||||
|
|
||||||
|
eulers = array([gamma, theta, psi]).T |
||||||
|
return eulers.reshape(output_shape) |
||||||
|
|
||||||
|
|
||||||
|
def quat2rot(quats): |
||||||
|
quats = array(quats) |
||||||
|
input_shape = quats.shape |
||||||
|
quats = np.atleast_2d(quats) |
||||||
|
Rs = np.zeros((quats.shape[0], 3, 3)) |
||||||
|
q0 = quats[:, 0] |
||||||
|
q1 = quats[:, 1] |
||||||
|
q2 = quats[:, 2] |
||||||
|
q3 = quats[:, 3] |
||||||
|
Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3 |
||||||
|
Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3) |
||||||
|
Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3) |
||||||
|
Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3) |
||||||
|
Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3 |
||||||
|
Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1) |
||||||
|
Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2) |
||||||
|
Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3) |
||||||
|
Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3 |
||||||
|
|
||||||
|
if len(input_shape) < 2: |
||||||
|
return Rs[0] |
||||||
|
else: |
||||||
|
return Rs |
||||||
|
|
||||||
|
|
||||||
|
def rot2quat(rots): |
||||||
|
input_shape = rots.shape |
||||||
|
if len(input_shape) < 3: |
||||||
|
rots = array([rots]) |
||||||
|
K3 = np.empty((len(rots), 4, 4)) |
||||||
|
K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0 |
||||||
|
K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0 |
||||||
|
K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0 |
||||||
|
K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0 |
||||||
|
K3[:, 1, 0] = K3[:, 0, 1] |
||||||
|
K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0 |
||||||
|
K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0 |
||||||
|
K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0 |
||||||
|
K3[:, 2, 0] = K3[:, 0, 2] |
||||||
|
K3[:, 2, 1] = K3[:, 1, 2] |
||||||
|
K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0 |
||||||
|
K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0 |
||||||
|
K3[:, 3, 0] = K3[:, 0, 3] |
||||||
|
K3[:, 3, 1] = K3[:, 1, 3] |
||||||
|
K3[:, 3, 2] = K3[:, 2, 3] |
||||||
|
K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0 |
||||||
|
q = np.empty((len(rots), 4)) |
||||||
|
for i in xrange(len(rots)): |
||||||
|
_, eigvecs = linalg.eigh(K3[i].T) |
||||||
|
eigvecs = eigvecs[:,3:] |
||||||
|
q[i, 0] = eigvecs[-1] |
||||||
|
q[i, 1:] = -eigvecs[:-1].flatten() |
||||||
|
if q[i, 0] < 0: |
||||||
|
q[i] = -q[i] |
||||||
|
|
||||||
|
if len(input_shape) < 3: |
||||||
|
return q[0] |
||||||
|
else: |
||||||
|
return q |
||||||
|
|
||||||
|
|
||||||
|
def euler2rot(eulers): |
||||||
|
return rotations_from_quats(euler2quat(eulers)) |
||||||
|
|
||||||
|
|
||||||
|
def rot2euler(rots): |
||||||
|
return quat2euler(quats_from_rotations(rots)) |
||||||
|
|
||||||
|
|
||||||
|
quats_from_rotations = rot2quat |
||||||
|
quat_from_rot = rot2quat |
||||||
|
rotations_from_quats = quat2rot |
||||||
|
rot_from_quat= quat2rot |
||||||
|
rot_from_quat= quat2rot |
||||||
|
euler_from_rot = rot2euler |
||||||
|
euler_from_quat = quat2euler |
||||||
|
rot_from_euler = euler2rot |
||||||
|
quat_from_euler = euler2quat |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
''' |
||||||
|
Random helpers below |
||||||
|
''' |
||||||
|
|
||||||
|
|
||||||
|
def quat_product(q, r): |
||||||
|
t = np.zeros(4) |
||||||
|
t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3] |
||||||
|
t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2] |
||||||
|
t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1] |
||||||
|
t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0] |
||||||
|
return t |
||||||
|
|
||||||
|
|
||||||
|
def rot_matrix(roll, pitch, yaw): |
||||||
|
cr, sr = np.cos(roll), np.sin(roll) |
||||||
|
cp, sp = np.cos(pitch), np.sin(pitch) |
||||||
|
cy, sy = np.cos(yaw), np.sin(yaw) |
||||||
|
rr = array([[1,0,0],[0, cr,-sr],[0, sr, cr]]) |
||||||
|
rp = array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]]) |
||||||
|
ry = array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]]) |
||||||
|
return ry.dot(rp.dot(rr)) |
||||||
|
|
||||||
|
|
||||||
|
def rot(axis, angle): |
||||||
|
# Rotates around an arbitrary axis |
||||||
|
ret_1 = (1 - np.cos(angle)) * array([[axis[0]**2, axis[0] * axis[1], axis[0] * axis[2]], [ |
||||||
|
axis[1] * axis[0], axis[1]**2, axis[1] * axis[2] |
||||||
|
], [axis[2] * axis[0], axis[2] * axis[1], axis[2]**2]]) |
||||||
|
ret_2 = np.cos(angle) * np.eye(3) |
||||||
|
ret_3 = np.sin(angle) * array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], |
||||||
|
[-axis[1], axis[0], 0]]) |
||||||
|
return ret_1 + ret_2 + ret_3 |
||||||
|
|
||||||
|
|
||||||
|
def ecef_euler_from_ned(ned_ecef_init, ned_pose): |
||||||
|
''' |
||||||
|
Got it from here: |
||||||
|
Using Rotations to Build Aerospace Coordinate Systems |
||||||
|
-Don Koks |
||||||
|
''' |
||||||
|
converter = LocalCoord.from_ecef(ned_ecef_init) |
||||||
|
x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
|
||||||
|
x1 = rot(z0, ned_pose[2]).dot(x0) |
||||||
|
y1 = rot(z0, ned_pose[2]).dot(y0) |
||||||
|
z1 = rot(z0, ned_pose[2]).dot(z0) |
||||||
|
|
||||||
|
x2 = rot(y1, ned_pose[1]).dot(x1) |
||||||
|
y2 = rot(y1, ned_pose[1]).dot(y1) |
||||||
|
z2 = rot(y1, ned_pose[1]).dot(z1) |
||||||
|
|
||||||
|
x3 = rot(x2, ned_pose[0]).dot(x2) |
||||||
|
y3 = rot(x2, ned_pose[0]).dot(y2) |
||||||
|
#z3 = rot(x2, ned_pose[0]).dot(z2) |
||||||
|
|
||||||
|
x0 = array([1, 0, 0]) |
||||||
|
y0 = array([0, 1, 0]) |
||||||
|
z0 = array([0, 0, 1]) |
||||||
|
|
||||||
|
psi = np.arctan2(inner(x3, y0), inner(x3, x0)) |
||||||
|
theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2)) |
||||||
|
y2 = rot(z0, psi).dot(y0) |
||||||
|
z2 = rot(y2, theta).dot(z0) |
||||||
|
phi = np.arctan2(inner(y3, z2), inner(y3, y2)) |
||||||
|
|
||||||
|
ret = array([phi, theta, psi]) |
||||||
|
return ret |
||||||
|
|
||||||
|
|
||||||
|
def ned_euler_from_ecef(ned_ecef_init, ecef_poses): |
||||||
|
''' |
||||||
|
Got the math from here: |
||||||
|
Using Rotations to Build Aerospace Coordinate Systems |
||||||
|
-Don Koks |
||||||
|
|
||||||
|
Also accepts array of ecef_poses and array of ned_ecef_inits. |
||||||
|
Where each row is a pose and an ecef_init. |
||||||
|
''' |
||||||
|
ned_ecef_init = array(ned_ecef_init) |
||||||
|
ecef_poses = array(ecef_poses) |
||||||
|
output_shape = ecef_poses.shape |
||||||
|
ned_ecef_init = np.atleast_2d(ned_ecef_init) |
||||||
|
ecef_poses = np.atleast_2d(ecef_poses) |
||||||
|
|
||||||
|
ned_poses = np.zeros(ecef_poses.shape) |
||||||
|
for i, ecef_pose in enumerate(ecef_poses): |
||||||
|
converter = LocalCoord.from_ecef(ned_ecef_init[i]) |
||||||
|
x0 = array([1, 0, 0]) |
||||||
|
y0 = array([0, 1, 0]) |
||||||
|
z0 = array([0, 0, 1]) |
||||||
|
|
||||||
|
x1 = rot(z0, ecef_pose[2]).dot(x0) |
||||||
|
y1 = rot(z0, ecef_pose[2]).dot(y0) |
||||||
|
z1 = rot(z0, ecef_pose[2]).dot(z0) |
||||||
|
|
||||||
|
x2 = rot(y1, ecef_pose[1]).dot(x1) |
||||||
|
y2 = rot(y1, ecef_pose[1]).dot(y1) |
||||||
|
z2 = rot(y1, ecef_pose[1]).dot(z1) |
||||||
|
|
||||||
|
x3 = rot(x2, ecef_pose[0]).dot(x2) |
||||||
|
y3 = rot(x2, ecef_pose[0]).dot(y2) |
||||||
|
#z3 = rot(x2, ecef_pose[0]).dot(z2) |
||||||
|
|
||||||
|
x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0]) |
||||||
|
|
||||||
|
psi = np.arctan2(inner(x3, y0), inner(x3, x0)) |
||||||
|
theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2)) |
||||||
|
y2 = rot(z0, psi).dot(y0) |
||||||
|
z2 = rot(y2, theta).dot(z0) |
||||||
|
phi = np.arctan2(inner(y3, z2), inner(y3, y2)) |
||||||
|
ned_poses[i] = array([phi, theta, psi]) |
||||||
|
|
||||||
|
return ned_poses.reshape(output_shape) |
||||||
|
|
||||||
|
|
||||||
|
def ecef2car(car_ecef, psi, theta, points_ecef, ned_converter): |
||||||
|
""" |
||||||
|
TODO: add roll rotation |
||||||
|
Converts an array of points in ecef coordinates into |
||||||
|
x-forward, y-left, z-up coordinates |
||||||
|
Parameters |
||||||
|
---------- |
||||||
|
psi: yaw, radian |
||||||
|
theta: pitch, radian |
||||||
|
Returns |
||||||
|
------- |
||||||
|
[x, y, z] coordinates in car frame |
||||||
|
""" |
||||||
|
|
||||||
|
# input is an array of points in ecef cocrdinates |
||||||
|
# output is an array of points in car's coordinate (x-front, y-left, z-up) |
||||||
|
|
||||||
|
# convert points to NED |
||||||
|
points_ned = [] |
||||||
|
for p in points_ecef: |
||||||
|
points_ned.append(ned_converter.ecef2ned_matrix.dot(array(p) - car_ecef)) |
||||||
|
|
||||||
|
points_ned = np.vstack(points_ned).T |
||||||
|
|
||||||
|
# n, e, d -> x, y, z |
||||||
|
# Calculate relative postions and rotate wrt to heading and pitch of car |
||||||
|
invert_R = array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]]) |
||||||
|
|
||||||
|
c, s = np.cos(psi), np.sin(psi) |
||||||
|
yaw_R = array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]]) |
||||||
|
|
||||||
|
c, s = np.cos(theta), np.sin(theta) |
||||||
|
pitch_R = array([[c, 0., -s], [0., 1., 0.], [s, 0., c]]) |
||||||
|
|
||||||
|
return dot(pitch_R, dot(yaw_R, dot(invert_R, points_ned))) |
||||||
@ -1 +1 @@ |
|||||||
#define COMMA_VERSION "0.5.1-release" |
#define COMMA_VERSION "0.5.2-release" |
||||||
|
|||||||
@ -0,0 +1,246 @@ |
|||||||
|
#!/usr/bin/env python |
||||||
|
import os |
||||||
|
import zmq |
||||||
|
import cv2 |
||||||
|
import copy |
||||||
|
import math |
||||||
|
import json |
||||||
|
import numpy as np |
||||||
|
import selfdrive.messaging as messaging |
||||||
|
from selfdrive.swaglog import cloudlog |
||||||
|
from selfdrive.services import service_list |
||||||
|
from common.params import Params |
||||||
|
from common.ffi_wrapper import ffi_wrap |
||||||
|
import common.transformations.orientation as orient |
||||||
|
from common.transformations.model import model_height, get_camera_frame_from_model_frame |
||||||
|
from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \ |
||||||
|
eon_intrinsics, get_calib_from_vp, normalize, denormalize, H, W |
||||||
|
|
||||||
|
|
||||||
|
MIN_SPEED_FILTER = 7 # m/s (~15.5mph) |
||||||
|
MAX_YAW_RATE_FILTER = math.radians(3) # per second |
||||||
|
FRAMES_NEEDED = 120 # allow to update VP every so many frames |
||||||
|
VP_CYCLES_NEEDED = 2 |
||||||
|
CALIBRATION_CYCLES_NEEDED = FRAMES_NEEDED * VP_CYCLES_NEEDED |
||||||
|
DT = 0.1 # nominal time step of 10Hz (orbd_live runs slower on pc) |
||||||
|
VP_RATE_LIM = 2. * DT # 2 px/s |
||||||
|
VP_INIT = np.array([W/2., H/2.]) |
||||||
|
EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__)) |
||||||
|
VP_VALIDITY_CORNERS = np.array([[-200., -200.], [200., 200.]]) + VP_INIT |
||||||
|
GRID_WEIGHT_INIT = 2e6 |
||||||
|
MAX_LINES = 500 # max lines to avoid over computation |
||||||
|
|
||||||
|
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) |
||||||
|
|
||||||
|
|
||||||
|
class Calibration: |
||||||
|
UNCALIBRATED = 0 |
||||||
|
CALIBRATED = 1 |
||||||
|
INVALID = 2 |
||||||
|
|
||||||
|
|
||||||
|
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)))) |
||||||
|
cam_rot = dt * view_frame_from_device_frame.dot(rot_speeds) |
||||||
|
rot = orient.rot_matrix(*cam_rot.T).T |
||||||
|
pts_corrected = rot.dot(pts.T).T |
||||||
|
pts_corrected[:,0] /= pts_corrected[:,2] |
||||||
|
pts_corrected[:,1] /= pts_corrected[:,2] |
||||||
|
return pts_corrected[:,:2] |
||||||
|
|
||||||
|
def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy): |
||||||
|
y, x = np.mgrid[-sizey:sizey+1, -sizex:sizex+1] |
||||||
|
g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2))) |
||||||
|
return g / g.sum() |
||||||
|
|
||||||
|
def blur_image(img, kernel): |
||||||
|
return cv2.filter2D(img.astype(np.uint16), -1, kernel) |
||||||
|
|
||||||
|
def is_calibration_valid(vp): |
||||||
|
return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \ |
||||||
|
vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1] |
||||||
|
|
||||||
|
class Calibrator(object): |
||||||
|
def __init__(self, param_put=False): |
||||||
|
self.param_put = param_put |
||||||
|
self.speed = 0 |
||||||
|
self.vp_cycles = 0 |
||||||
|
self.angle_offset = 0. |
||||||
|
self.yaw_rate = 0. |
||||||
|
self.l100_last_updated = 0 |
||||||
|
self.prev_orbs = None |
||||||
|
self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0) |
||||||
|
|
||||||
|
self.vp = copy.copy(VP_INIT) |
||||||
|
self.cal_status = Calibration.UNCALIBRATED |
||||||
|
self.frame_counter = 0 |
||||||
|
self.params = Params() |
||||||
|
calibration_params = self.params.get("CalibrationParams") |
||||||
|
if calibration_params: |
||||||
|
try: |
||||||
|
calibration_params = json.loads(calibration_params) |
||||||
|
self.vp = np.array(calibration_params["vanishing_point"]) |
||||||
|
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID |
||||||
|
self.vp_cycles = VP_CYCLES_NEEDED |
||||||
|
self.frame_counter = CALIBRATION_CYCLES_NEEDED |
||||||
|
except Exception: |
||||||
|
cloudlog.exception("CalibrationParams file found but error encountered") |
||||||
|
|
||||||
|
self.vp_unfilt = self.vp |
||||||
|
self.orb_last_updated = 0. |
||||||
|
self.reset_grid() |
||||||
|
|
||||||
|
def reset_grid(self): |
||||||
|
if self.cal_status == Calibration.UNCALIBRATED: |
||||||
|
# empty grid |
||||||
|
self.grid = np.zeros((H+1, W+1), dtype=np.float) |
||||||
|
else: |
||||||
|
# gaussian grid, centered at vp |
||||||
|
self.grid = gaussian_kernel(W/2., H/2., 16, 16, self.vp[0] - W/2., self.vp[1] - H/2.) * GRID_WEIGHT_INIT |
||||||
|
|
||||||
|
def rescale_grid(self): |
||||||
|
self.grid *= 0.9 |
||||||
|
|
||||||
|
def update(self, uvs, yaw_rate, speed): |
||||||
|
if len(uvs) < 10 or \ |
||||||
|
abs(yaw_rate) > MAX_YAW_RATE_FILTER or \ |
||||||
|
speed < MIN_SPEED_FILTER: |
||||||
|
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 |
||||||
|
uvs = uvs[good_tracks] |
||||||
|
if uvs.shape[0] > MAX_LINES: |
||||||
|
uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :] |
||||||
|
lines = get_lines(uvs) |
||||||
|
|
||||||
|
increment_grid_c(self.grid, lines, len(lines)) |
||||||
|
self.frame_counter += 1 |
||||||
|
if (self.frame_counter % FRAMES_NEEDED) == 0: |
||||||
|
grid = blur_image(self.grid, self.kernel) |
||||||
|
argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1] |
||||||
|
self.rescale_grid() |
||||||
|
self.vp_unfilt = np.array(argmax_vp) |
||||||
|
self.vp_cycles += 1 |
||||||
|
if (self.vp_cycles - VP_CYCLES_NEEDED) % 10 == 0: # update file every 10 |
||||||
|
# skip rate_lim before writing the file to avoid writing a lagged vp |
||||||
|
if self.cal_status != Calibration.CALIBRATED: |
||||||
|
self.vp = self.vp_unfilt |
||||||
|
if self.param_put: |
||||||
|
cal_params = {"vanishing_point": list(self.vp), "angle_offset2": self.angle_offset} |
||||||
|
self.params.put("CalibrationParams", json.dumps(cal_params)) |
||||||
|
return self.vp_unfilt |
||||||
|
|
||||||
|
def parse_orb_features(self, log): |
||||||
|
matches = np.array(log.orbFeatures.matches) |
||||||
|
n = len(log.orbFeatures.matches) |
||||||
|
t = float(log.orbFeatures.timestampLastEof)*1e-9 |
||||||
|
if t == 0 or n == 0: |
||||||
|
return t, np.zeros((0,2,2)) |
||||||
|
orbs = denormalize(np.column_stack((log.orbFeatures.xs, |
||||||
|
log.orbFeatures.ys))) |
||||||
|
if self.prev_orbs is not None: |
||||||
|
valid_matches = (matches > -1) & (matches < len(self.prev_orbs)) |
||||||
|
tracks = np.hstack((orbs[valid_matches], self.prev_orbs[matches[valid_matches]])).reshape((-1,2,2)) |
||||||
|
else: |
||||||
|
tracks = np.zeros((0,2,2)) |
||||||
|
self.prev_orbs = orbs |
||||||
|
return t, tracks |
||||||
|
|
||||||
|
def update_vp(self): |
||||||
|
# rate limit to not move VP too fast |
||||||
|
self.vp = np.clip(self.vp_unfilt, self.vp - VP_RATE_LIM, self.vp + VP_RATE_LIM) |
||||||
|
if self.vp_cycles < VP_CYCLES_NEEDED: |
||||||
|
self.cal_status = Calibration.UNCALIBRATED |
||||||
|
else: |
||||||
|
self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID |
||||||
|
|
||||||
|
def handle_orb_features(self, log): |
||||||
|
self.update_vp() |
||||||
|
self.time_orb, frame_raw = self.parse_orb_features(log) |
||||||
|
self.dt = min(self.time_orb - self.orb_last_updated, 1.) |
||||||
|
self.orb_last_updated = self.time_orb |
||||||
|
if self.time_orb - self.l100_last_updated < 1: |
||||||
|
return self.update(frame_raw, self.yaw_rate, self.speed) |
||||||
|
|
||||||
|
def handle_live100(self, log): |
||||||
|
self.l100_last_updated = self.time_orb |
||||||
|
self.speed = log.live100.vEgo |
||||||
|
self.angle_offset = log.live100.angleOffset |
||||||
|
self.yaw_rate = log.live100.curvature * self.speed |
||||||
|
|
||||||
|
def handle_debug(self): |
||||||
|
grid_blurred = blur_image(self.grid, self.kernel) |
||||||
|
grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255) |
||||||
|
grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2) |
||||||
|
grid_color[:,:,0] = 0 |
||||||
|
cv2.circle(grid_color, tuple(self.vp.astype(np.int64)), 2, (255, 255, 0), -1) |
||||||
|
cv2.imshow("debug", grid_color.astype(np.uint8)) |
||||||
|
|
||||||
|
cv2.waitKey(1) |
||||||
|
|
||||||
|
def send_data(self, livecalibration): |
||||||
|
calib = get_calib_from_vp(self.vp) |
||||||
|
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) |
||||||
|
ke = eon_intrinsics.dot(extrinsic_matrix) |
||||||
|
warp_matrix = get_camera_frame_from_model_frame(ke, model_height) |
||||||
|
|
||||||
|
cal_send = messaging.new_message() |
||||||
|
cal_send.init('liveCalibration') |
||||||
|
cal_send.liveCalibration.calStatus = self.cal_status |
||||||
|
cal_send.liveCalibration.calPerc = min(self.frame_counter * 100 / CALIBRATION_CYCLES_NEEDED, 100) |
||||||
|
cal_send.liveCalibration.warpMatrix2 = map(float, warp_matrix.flatten()) |
||||||
|
cal_send.liveCalibration.extrinsicMatrix = map(float, extrinsic_matrix.flatten()) |
||||||
|
|
||||||
|
livecalibration.send(cal_send.to_bytes()) |
||||||
|
|
||||||
|
|
||||||
|
def calibrationd_thread(gctx=None, addr="127.0.0.1"): |
||||||
|
context = zmq.Context() |
||||||
|
|
||||||
|
live100 = messaging.sub_sock(context, service_list['live100'].port, addr=addr, conflate=True) |
||||||
|
orbfeatures = messaging.sub_sock(context, service_list['orbFeatures'].port, addr=addr, conflate=True) |
||||||
|
livecalibration = messaging.pub_sock(context, service_list['liveCalibration'].port) |
||||||
|
|
||||||
|
calibrator = Calibrator(param_put = True) |
||||||
|
|
||||||
|
# buffer with all the messages that still need to be input into the kalman |
||||||
|
while 1: |
||||||
|
of = messaging.recv_one(orbfeatures) |
||||||
|
l100 = messaging.recv_one_or_none(live100) |
||||||
|
|
||||||
|
new_vp = calibrator.handle_orb_features(of) |
||||||
|
if DEBUG and new_vp is not None: |
||||||
|
print 'got new vp', new_vp |
||||||
|
if l100 is not None: |
||||||
|
calibrator.handle_live100(l100) |
||||||
|
if DEBUG: |
||||||
|
calibrator.handle_debug() |
||||||
|
|
||||||
|
calibrator.send_data(livecalibration) |
||||||
|
|
||||||
|
|
||||||
|
def main(gctx=None, addr="127.0.0.1"): |
||||||
|
calibrationd_thread(gctx, addr) |
||||||
|
|
||||||
|
if __name__ == "__main__": |
||||||
|
main() |
||||||
|
|
||||||
@ -0,0 +1,41 @@ |
|||||||
|
int get_intersections(double *lines, double *intersections, long long n) { |
||||||
|
double D, Dx, Dy; |
||||||
|
double x, y; |
||||||
|
double *L1, *L2; |
||||||
|
int k = 0; |
||||||
|
for (int i=0; i < n; i++) { |
||||||
|
for (int j=0; j < n; j++) { |
||||||
|
L1 = lines + i*3; |
||||||
|
L2 = lines + j*3; |
||||||
|
D = L1[0] * L2[1] - L1[1] * L2[0]; |
||||||
|
Dx = L1[2] * L2[1] - L1[1] * L2[2]; |
||||||
|
Dy = L1[0] * L2[2] - L1[2] * L2[0]; |
||||||
|
// only intersect lines from different quadrants
|
||||||
|
if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0)){ |
||||||
|
x = Dx / D; |
||||||
|
y = Dy / D; |
||||||
|
if ((0 < x) && |
||||||
|
(x < W) && |
||||||
|
(0 < y) && |
||||||
|
(y < H)){ |
||||||
|
intersections[k*2 + 0] = x; |
||||||
|
intersections[k*2 + 1] = y; |
||||||
|
k++; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
return k; |
||||||
|
} |
||||||
|
|
||||||
|
void increment_grid(double *grid, double *lines, long long n) { |
||||||
|
double *intersections = (double*) malloc(n*n*2*sizeof(double)); |
||||||
|
int y, x, k; |
||||||
|
k = get_intersections(lines, intersections, n); |
||||||
|
for (int i=0; i < k; i++) { |
||||||
|
x = (int) (intersections[i*2 + 0] + 0.5); |
||||||
|
y = (int) (intersections[i*2 + 1] + 0.5); |
||||||
|
grid[y*(W+1) + x] += 1.; |
||||||
|
} |
||||||
|
free(intersections); |
||||||
|
} |
||||||
Binary file not shown.
@ -0,0 +1,8 @@ |
|||||||
|
orbd |
||||||
|
orbd_cpu |
||||||
|
test/turbocv_profile |
||||||
|
test/turbocv_test |
||||||
|
dspout/* |
||||||
|
dumb_test |
||||||
|
bilinear_lut.h |
||||||
|
orb_lut.h |
||||||
@ -0,0 +1,105 @@ |
|||||||
|
# CPU
|
||||||
|
|
||||||
|
CC = clang
|
||||||
|
CXX = clang++
|
||||||
|
|
||||||
|
WARN_FLAGS = -Werror=implicit-function-declaration \
|
||||||
|
-Werror=incompatible-pointer-types \
|
||||||
|
-Werror=int-conversion \
|
||||||
|
-Werror=return-type \
|
||||||
|
-Werror=format-extra-args
|
||||||
|
|
||||||
|
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||||
|
|
||||||
|
CFLAGS = -std=gnu11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
||||||
|
CXXFLAGS = -std=c++11 -g -O2 -fPIC $(WARN_FLAGS) -Iinclude $(JSON_FLAGS) -I.
|
||||||
|
LDFLAGS =
|
||||||
|
|
||||||
|
# profile
|
||||||
|
# CXXFLAGS += -DTURBOCV_PROFILE=1
|
||||||
|
|
||||||
|
PHONELIBS = ../../phonelibs
|
||||||
|
BASEDIR = ../..
|
||||||
|
EXTERNAL = ../../external
|
||||||
|
PYTHONLIBS =
|
||||||
|
|
||||||
|
UNAME_M := $(shell uname -m)
|
||||||
|
|
||||||
|
ifeq ($(UNAME_M),x86_64) |
||||||
|
# computer
|
||||||
|
|
||||||
|
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||||
|
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
|
||||||
|
-l:libczmq.a -l:libzmq.a -lpthread
|
||||||
|
|
||||||
|
OPENCV_LIBS = -lopencv_core -lopencv_highgui -lopencv_features2d -lopencv_imgproc
|
||||||
|
|
||||||
|
CXXFLAGS += -fopenmp
|
||||||
|
LDFLAGS += -lomp
|
||||||
|
|
||||||
|
else |
||||||
|
# phone
|
||||||
|
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||||
|
ZMQ_LIBS = -L$(PHONELIBS)/zmq/aarch64/lib \
|
||||||
|
-l:libczmq.a -l:libzmq.a \
|
||||||
|
-lgnustl_shared
|
||||||
|
|
||||||
|
OPENCV_FLAGS = -I$(PHONELIBS)/opencv/include
|
||||||
|
OPENCV_LIBS = -Wl,--enable-new-dtags -Wl,-rpath,/usr/local/lib/python2.7/site-packages -L/usr/local/lib/python2.7/site-packages -l:cv2.so
|
||||||
|
|
||||||
|
endif |
||||||
|
|
||||||
|
.PHONY: all |
||||||
|
all: orbd |
||||||
|
|
||||||
|
include ../common/cereal.mk |
||||||
|
|
||||||
|
DEP_OBJS = ../common/visionipc.o ../common/swaglog.o $(PHONELIBS)/json/src/json.o
|
||||||
|
|
||||||
|
orbd: orbd_dsp.o $(DEP_OBJS) calculator_stub.o freethedsp.o |
||||||
|
@echo "[ LINK ] $@"
|
||||||
|
$(CXX) -fPIC -o '$@' $^ \
|
||||||
|
$(LDFLAGS) \
|
||||||
|
$(ZMQ_LIBS) \
|
||||||
|
$(CEREAL_LIBS) \
|
||||||
|
-L/usr/lib \
|
||||||
|
-L/system/vendor/lib64 \
|
||||||
|
-ladsprpc \
|
||||||
|
-lm -lz -llog
|
||||||
|
|
||||||
|
%.o: %.c |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) \
|
||||||
|
$(ZMQ_FLAGS) \
|
||||||
|
-I../ \
|
||||||
|
-I../../ \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
orbd_dsp.o: orbd.cc |
||||||
|
@echo "[ CXX ] $@"
|
||||||
|
$(CXX) $(CXXFLAGS) \
|
||||||
|
$(CEREAL_CXXFLAGS) \
|
||||||
|
$(ZMQ_FLAGS) \
|
||||||
|
$(OPENCV_FLAGS) \
|
||||||
|
-DDSP \
|
||||||
|
-I../ \
|
||||||
|
-I../../ \
|
||||||
|
-I../../../ \
|
||||||
|
-I./include \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
freethedsp.o: dsp/freethedsp.c |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) \
|
||||||
|
-c -o '$@' '$<'
|
||||||
|
|
||||||
|
calculator_stub.o: dsp/gen/calculator_stub.c |
||||||
|
@echo "[ CC ] $@"
|
||||||
|
$(CC) $(CFLAGS) -I./include -c -o '$@' '$<'
|
||||||
|
|
||||||
|
-include internal.mk |
||||||
|
|
||||||
|
.PHONY: clean |
||||||
|
clean: |
||||||
|
rm -f *.o turbocv.so orbd test/turbocv_profile test/turbocv_test test/*.o *_lut.h
|
||||||
|
|
||||||
@ -0,0 +1,119 @@ |
|||||||
|
// freethedsp by geohot
|
||||||
|
// (because the DSP should be free)
|
||||||
|
// released under MIT License
|
||||||
|
|
||||||
|
// usage instructions:
|
||||||
|
// 1. Compile an example from the Qualcomm Hexagon SDK
|
||||||
|
// 2. Try to run it on your phone
|
||||||
|
// 3. Be very sad when "adsprpc ... dlopen error: ... signature verify start failed for ..." appears in logcat
|
||||||
|
// ...here is where people would give up before freethedsp
|
||||||
|
// 4. Compile freethedsp with 'clang -shared freethedsp.c -o freethedsp.so' (or statically link it to your program)
|
||||||
|
// 5. Run your program with 'LD_PRELOAD=./freethedsp.so ./<your_prog>'
|
||||||
|
// 6. OMG THE DSP WORKS
|
||||||
|
// 7. Be happy.
|
||||||
|
|
||||||
|
// *** patch may have to change for your phone ***
|
||||||
|
|
||||||
|
// this is patching /dsp/fastrpc_shell_0
|
||||||
|
// correct if sha hash of fastrpc_shell_0 is "fbadc96848aefad99a95aa4edb560929dcdf78f8"
|
||||||
|
// patch to return 0xFFFFFFFF from is_test_enabled instead of 0
|
||||||
|
// your fastrpc_shell_0 may vary
|
||||||
|
#define PATCH_ADDR 0x5200c |
||||||
|
#define PATCH_OLD "\x40\x3f\x20\x50" |
||||||
|
#define PATCH_NEW "\x40\x3f\x00\x5a" |
||||||
|
#define PATCH_LEN (sizeof(PATCH_OLD)-1) |
||||||
|
#define _BITS_IOCTL_H_ |
||||||
|
|
||||||
|
// under 100 lines of code begins now
|
||||||
|
#include <stdio.h> |
||||||
|
#include <dlfcn.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <unistd.h> |
||||||
|
|
||||||
|
// ioctl stuff
|
||||||
|
#define IOC_OUT 0x40000000 /* copy out parameters */ |
||||||
|
#define IOC_IN 0x80000000 /* copy in parameters */ |
||||||
|
#define IOC_INOUT (IOC_IN|IOC_OUT) |
||||||
|
#define IOCPARM_MASK 0x1fff /* parameter length, at most 13 bits */ |
||||||
|
|
||||||
|
#define _IOC(inout,group,num,len) \ |
||||||
|
(inout | ((len & IOCPARM_MASK) << 16) | ((group) << 8) | (num)) |
||||||
|
#define _IOWR(g,n,t) _IOC(IOC_INOUT, (g), (n), sizeof(t)) |
||||||
|
|
||||||
|
// ion ioctls
|
||||||
|
#include <linux/ion.h> |
||||||
|
#define ION_IOC_MSM_MAGIC 'M' |
||||||
|
#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, \ |
||||||
|
struct ion_flush_data) |
||||||
|
|
||||||
|
struct ion_flush_data { |
||||||
|
ion_user_handle_t handle; |
||||||
|
int fd; |
||||||
|
void *vaddr; |
||||||
|
unsigned int offset; |
||||||
|
unsigned int length; |
||||||
|
}; |
||||||
|
|
||||||
|
// fastrpc ioctls
|
||||||
|
#define FASTRPC_IOCTL_INIT _IOWR('R', 6, struct fastrpc_ioctl_init) |
||||||
|
|
||||||
|
struct fastrpc_ioctl_init { |
||||||
|
uint32_t flags; /* one of FASTRPC_INIT_* macros */ |
||||||
|
uintptr_t __user file; /* pointer to elf file */ |
||||||
|
int32_t filelen; /* elf file length */ |
||||||
|
int32_t filefd; /* ION fd for the file */ |
||||||
|
uintptr_t __user mem; /* mem for the PD */ |
||||||
|
int32_t memlen; /* mem length */ |
||||||
|
int32_t memfd; /* ION fd for the mem */ |
||||||
|
}; |
||||||
|
|
||||||
|
int ioctl(int fd, unsigned long request, void *arg) { |
||||||
|
static void *handle = NULL; |
||||||
|
static int (*orig_ioctl)(int, int, void*); |
||||||
|
|
||||||
|
if (handle == NULL) { |
||||||
|
handle = dlopen("/system/lib64/libc.so", RTLD_LAZY); |
||||||
|
assert(handle != NULL); |
||||||
|
orig_ioctl = dlsym(handle, "ioctl"); |
||||||
|
} |
||||||
|
|
||||||
|
int ret = orig_ioctl(fd, request, arg); |
||||||
|
|
||||||
|
// carefully modify this one
|
||||||
|
if (request == FASTRPC_IOCTL_INIT) { |
||||||
|
struct fastrpc_ioctl_init *init = (struct fastrpc_ioctl_init *)arg; |
||||||
|
|
||||||
|
// confirm patch is correct and do the patch
|
||||||
|
assert(memcmp((void*)(init->mem+PATCH_ADDR), PATCH_OLD, PATCH_LEN) == 0); |
||||||
|
memcpy((void*)(init->mem+PATCH_ADDR), PATCH_NEW, PATCH_LEN); |
||||||
|
|
||||||
|
// flush cache
|
||||||
|
int ionfd = open("/dev/ion", O_RDONLY); |
||||||
|
assert(ionfd > 0); |
||||||
|
|
||||||
|
struct ion_fd_data fd_data; |
||||||
|
fd_data.fd = init->memfd; |
||||||
|
int ret = ioctl(ionfd, ION_IOC_IMPORT, &fd_data); |
||||||
|
assert(ret == 0); |
||||||
|
|
||||||
|
struct ion_flush_data flush_data; |
||||||
|
flush_data.handle = fd_data.handle; |
||||||
|
flush_data.vaddr = (void*)init->mem; |
||||||
|
flush_data.offset = 0; |
||||||
|
flush_data.length = init->memlen; |
||||||
|
ret = ioctl(ionfd, ION_IOC_CLEAN_INV_CACHES, &flush_data); |
||||||
|
assert(ret == 0); |
||||||
|
|
||||||
|
struct ion_handle_data handle_data; |
||||||
|
handle_data.handle = fd_data.handle; |
||||||
|
ret = ioctl(ionfd, ION_IOC_FREE, &handle_data); |
||||||
|
assert(ret == 0); |
||||||
|
|
||||||
|
// cleanup
|
||||||
|
close(ionfd); |
||||||
|
} |
||||||
|
|
||||||
|
return ret; |
||||||
|
} |
||||||
|
|
||||||
@ -0,0 +1,39 @@ |
|||||||
|
#ifndef _CALCULATOR_H |
||||||
|
#define _CALCULATOR_H |
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
typedef uint8_t uint8; |
||||||
|
typedef uint32_t uint32; |
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER |
||||||
|
#define __QAIC_HEADER(ff) ff |
||||||
|
#endif //__QAIC_HEADER
|
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER_EXPORT |
||||||
|
#define __QAIC_HEADER_EXPORT |
||||||
|
#endif // __QAIC_HEADER_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER_ATTRIBUTE |
||||||
|
#define __QAIC_HEADER_ATTRIBUTE |
||||||
|
#endif // __QAIC_HEADER_ATTRIBUTE
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL |
||||||
|
#define __QAIC_IMPL(ff) ff |
||||||
|
#endif //__QAIC_IMPL
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL_EXPORT |
||||||
|
#define __QAIC_IMPL_EXPORT |
||||||
|
#endif // __QAIC_IMPL_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL_ATTRIBUTE |
||||||
|
#define __QAIC_IMPL_ATTRIBUTE |
||||||
|
#endif // __QAIC_IMPL_ATTRIBUTE
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_init)(uint32* leet) __QAIC_HEADER_ATTRIBUTE; |
||||||
|
__QAIC_HEADER_EXPORT int __QAIC_HEADER(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_HEADER_ATTRIBUTE; |
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
#endif //_CALCULATOR_H
|
||||||
@ -0,0 +1,613 @@ |
|||||||
|
#ifndef _CALCULATOR_STUB_H |
||||||
|
#define _CALCULATOR_STUB_H |
||||||
|
#include "calculator.h" |
||||||
|
|
||||||
|
// remote.h
|
||||||
|
#include <stdint.h> |
||||||
|
#include <sys/types.h> |
||||||
|
|
||||||
|
typedef uint32_t remote_handle; |
||||||
|
typedef uint64_t remote_handle64; |
||||||
|
|
||||||
|
typedef struct { |
||||||
|
void *pv; |
||||||
|
size_t nLen; |
||||||
|
} remote_buf; |
||||||
|
|
||||||
|
typedef struct { |
||||||
|
int32_t fd; |
||||||
|
uint32_t offset; |
||||||
|
} remote_dma_handle; |
||||||
|
|
||||||
|
typedef union { |
||||||
|
remote_buf buf; |
||||||
|
remote_handle h; |
||||||
|
remote_handle64 h64; |
||||||
|
remote_dma_handle dma; |
||||||
|
} remote_arg; |
||||||
|
|
||||||
|
int remote_handle_open(const char* name, remote_handle *ph); |
||||||
|
int remote_handle_invoke(remote_handle h, uint32_t dwScalars, remote_arg *pra); |
||||||
|
int remote_handle_close(remote_handle h); |
||||||
|
|
||||||
|
#define REMOTE_SCALARS_MAKEX(nAttr,nMethod,nIn,nOut,noIn,noOut) \ |
||||||
|
((((uint32_t) (nAttr) & 0x7) << 29) | \
|
||||||
|
(((uint32_t) (nMethod) & 0x1f) << 24) | \
|
||||||
|
(((uint32_t) (nIn) & 0xff) << 16) | \
|
||||||
|
(((uint32_t) (nOut) & 0xff) << 8) | \
|
||||||
|
(((uint32_t) (noIn) & 0x0f) << 4) | \
|
||||||
|
((uint32_t) (noOut) & 0x0f)) |
||||||
|
|
||||||
|
#ifndef _QAIC_ENV_H |
||||||
|
#define _QAIC_ENV_H |
||||||
|
|
||||||
|
#ifdef __GNUC__ |
||||||
|
#ifdef __clang__ |
||||||
|
#pragma GCC diagnostic ignored "-Wunknown-pragmas" |
||||||
|
#else |
||||||
|
#pragma GCC diagnostic ignored "-Wpragmas" |
||||||
|
#endif |
||||||
|
#pragma GCC diagnostic ignored "-Wuninitialized" |
||||||
|
#pragma GCC diagnostic ignored "-Wunused-parameter" |
||||||
|
#pragma GCC diagnostic ignored "-Wunused-function" |
||||||
|
#endif |
||||||
|
|
||||||
|
#ifndef _ATTRIBUTE_UNUSED |
||||||
|
|
||||||
|
#ifdef _WIN32 |
||||||
|
#define _ATTRIBUTE_UNUSED |
||||||
|
#else |
||||||
|
#define _ATTRIBUTE_UNUSED __attribute__ ((unused)) |
||||||
|
#endif |
||||||
|
|
||||||
|
#endif // _ATTRIBUTE_UNUSED
|
||||||
|
|
||||||
|
#ifndef __QAIC_REMOTE |
||||||
|
#define __QAIC_REMOTE(ff) ff |
||||||
|
#endif //__QAIC_REMOTE
|
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER |
||||||
|
#define __QAIC_HEADER(ff) ff |
||||||
|
#endif //__QAIC_HEADER
|
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER_EXPORT |
||||||
|
#define __QAIC_HEADER_EXPORT |
||||||
|
#endif // __QAIC_HEADER_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_HEADER_ATTRIBUTE |
||||||
|
#define __QAIC_HEADER_ATTRIBUTE |
||||||
|
#endif // __QAIC_HEADER_ATTRIBUTE
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL |
||||||
|
#define __QAIC_IMPL(ff) ff |
||||||
|
#endif //__QAIC_IMPL
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL_EXPORT |
||||||
|
#define __QAIC_IMPL_EXPORT |
||||||
|
#endif // __QAIC_IMPL_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_IMPL_ATTRIBUTE |
||||||
|
#define __QAIC_IMPL_ATTRIBUTE |
||||||
|
#endif // __QAIC_IMPL_ATTRIBUTE
|
||||||
|
|
||||||
|
#ifndef __QAIC_STUB |
||||||
|
#define __QAIC_STUB(ff) ff |
||||||
|
#endif //__QAIC_STUB
|
||||||
|
|
||||||
|
#ifndef __QAIC_STUB_EXPORT |
||||||
|
#define __QAIC_STUB_EXPORT |
||||||
|
#endif // __QAIC_STUB_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_STUB_ATTRIBUTE |
||||||
|
#define __QAIC_STUB_ATTRIBUTE |
||||||
|
#endif // __QAIC_STUB_ATTRIBUTE
|
||||||
|
|
||||||
|
#ifndef __QAIC_SKEL |
||||||
|
#define __QAIC_SKEL(ff) ff |
||||||
|
#endif //__QAIC_SKEL__
|
||||||
|
|
||||||
|
#ifndef __QAIC_SKEL_EXPORT |
||||||
|
#define __QAIC_SKEL_EXPORT |
||||||
|
#endif // __QAIC_SKEL_EXPORT
|
||||||
|
|
||||||
|
#ifndef __QAIC_SKEL_ATTRIBUTE |
||||||
|
#define __QAIC_SKEL_ATTRIBUTE |
||||||
|
#endif // __QAIC_SKEL_ATTRIBUTE
|
||||||
|
|
||||||
|
#ifdef __QAIC_DEBUG__ |
||||||
|
#ifndef __QAIC_DBG_PRINTF__ |
||||||
|
#include <stdio.h> |
||||||
|
#define __QAIC_DBG_PRINTF__( ee ) do { printf ee ; } while(0) |
||||||
|
#endif |
||||||
|
#else |
||||||
|
#define __QAIC_DBG_PRINTF__( ee ) (void)0 |
||||||
|
#endif |
||||||
|
|
||||||
|
|
||||||
|
#define _OFFSET(src, sof) ((void*)(((char*)(src)) + (sof))) |
||||||
|
|
||||||
|
#define _COPY(dst, dof, src, sof, sz) \ |
||||||
|
do {\
|
||||||
|
struct __copy { \
|
||||||
|
char ar[sz]; \
|
||||||
|
};\
|
||||||
|
*(struct __copy*)_OFFSET(dst, dof) = *(struct __copy*)_OFFSET(src, sof);\
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
#define _COPYIF(dst, dof, src, sof, sz) \ |
||||||
|
do {\
|
||||||
|
if(_OFFSET(dst, dof) != _OFFSET(src, sof)) {\
|
||||||
|
_COPY(dst, dof, src, sof, sz); \
|
||||||
|
} \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
_ATTRIBUTE_UNUSED |
||||||
|
static __inline void _qaic_memmove(void* dst, void* src, int size) { |
||||||
|
int i; |
||||||
|
for(i = 0; i < size; ++i) { |
||||||
|
((char*)dst)[i] = ((char*)src)[i]; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
#define _MEMMOVEIF(dst, src, sz) \ |
||||||
|
do {\
|
||||||
|
if(dst != src) {\
|
||||||
|
_qaic_memmove(dst, src, sz);\
|
||||||
|
} \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
|
||||||
|
#define _ASSIGN(dst, src, sof) \ |
||||||
|
do {\
|
||||||
|
dst = OFFSET(src, sof); \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
#define _STD_STRLEN_IF(str) (str == 0 ? 0 : strlen(str)) |
||||||
|
|
||||||
|
#define AEE_SUCCESS 0 |
||||||
|
#define AEE_EOFFSET 0x80000400 |
||||||
|
#define AEE_EBADPARM (AEE_EOFFSET + 0x00E) |
||||||
|
|
||||||
|
#define _TRY(ee, func) \ |
||||||
|
do { \
|
||||||
|
if (AEE_SUCCESS != ((ee) = func)) {\
|
||||||
|
__QAIC_DBG_PRINTF__((__FILE__ ":%d:error:%d:%s\n", __LINE__, (int)(ee),#func));\
|
||||||
|
goto ee##bail;\
|
||||||
|
} \
|
||||||
|
} while (0) |
||||||
|
|
||||||
|
#define _CATCH(exception) exception##bail: if (exception != AEE_SUCCESS) |
||||||
|
|
||||||
|
#define _ASSERT(nErr, ff) _TRY(nErr, 0 == (ff) ? AEE_EBADPARM : AEE_SUCCESS) |
||||||
|
|
||||||
|
#ifdef __QAIC_DEBUG__ |
||||||
|
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, __FILE_LINE__, size, alignment, (void**)&pv)) |
||||||
|
#else |
||||||
|
#define _ALLOCATE(nErr, pal, size, alignment, pv) _TRY(nErr, _allocator_alloc(pal, 0, size, alignment, (void**)&pv)) |
||||||
|
#endif |
||||||
|
|
||||||
|
|
||||||
|
#endif // _QAIC_ENV_H
|
||||||
|
|
||||||
|
#ifndef _ALLOCATOR_H |
||||||
|
#define _ALLOCATOR_H |
||||||
|
|
||||||
|
#include <stdlib.h> |
||||||
|
#include <stdint.h> |
||||||
|
|
||||||
|
typedef struct _heap _heap; |
||||||
|
struct _heap { |
||||||
|
_heap* pPrev; |
||||||
|
const char* loc; |
||||||
|
uint64_t buf; |
||||||
|
}; |
||||||
|
|
||||||
|
typedef struct _allocator { |
||||||
|
_heap* pheap; |
||||||
|
uint8_t* stack; |
||||||
|
uint8_t* stackEnd; |
||||||
|
int nSize; |
||||||
|
} _allocator; |
||||||
|
|
||||||
|
_ATTRIBUTE_UNUSED |
||||||
|
static __inline int _heap_alloc(_heap** ppa, const char* loc, int size, void** ppbuf) { |
||||||
|
_heap* pn = 0; |
||||||
|
pn = malloc(size + sizeof(_heap) - sizeof(uint64_t)); |
||||||
|
if(pn != 0) { |
||||||
|
pn->pPrev = *ppa; |
||||||
|
pn->loc = loc; |
||||||
|
*ppa = pn; |
||||||
|
*ppbuf = (void*)&(pn->buf); |
||||||
|
return 0; |
||||||
|
} else { |
||||||
|
return -1; |
||||||
|
} |
||||||
|
} |
||||||
|
#define _ALIGN_SIZE(x, y) (((x) + (y-1)) & ~(y-1)) |
||||||
|
|
||||||
|
_ATTRIBUTE_UNUSED |
||||||
|
static __inline int _allocator_alloc(_allocator* me, |
||||||
|
const char* loc, |
||||||
|
int size, |
||||||
|
unsigned int al, |
||||||
|
void** ppbuf) { |
||||||
|
if(size < 0) { |
||||||
|
return -1; |
||||||
|
} else if (size == 0) { |
||||||
|
*ppbuf = 0; |
||||||
|
return 0; |
||||||
|
} |
||||||
|
if((_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size) < (uintptr_t)me->stack + me->nSize) { |
||||||
|
*ppbuf = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al); |
||||||
|
me->stackEnd = (uint8_t*)_ALIGN_SIZE((uintptr_t)me->stackEnd, al) + size; |
||||||
|
return 0; |
||||||
|
} else { |
||||||
|
return _heap_alloc(&me->pheap, loc, size, ppbuf); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
_ATTRIBUTE_UNUSED |
||||||
|
static __inline void _allocator_deinit(_allocator* me) { |
||||||
|
_heap* pa = me->pheap; |
||||||
|
while(pa != 0) { |
||||||
|
_heap* pn = pa; |
||||||
|
const char* loc = pn->loc; |
||||||
|
(void)loc; |
||||||
|
pa = pn->pPrev; |
||||||
|
free(pn); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
_ATTRIBUTE_UNUSED |
||||||
|
static __inline void _allocator_init(_allocator* me, uint8_t* stack, int stackSize) { |
||||||
|
me->stack = stack; |
||||||
|
me->stackEnd = stack + stackSize; |
||||||
|
me->nSize = stackSize; |
||||||
|
me->pheap = 0; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
#endif // _ALLOCATOR_H
|
||||||
|
|
||||||
|
#ifndef SLIM_H |
||||||
|
#define SLIM_H |
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
|
||||||
|
//a C data structure for the idl types that can be used to implement
|
||||||
|
//static and dynamic language bindings fairly efficiently.
|
||||||
|
//
|
||||||
|
//the goal is to have a minimal ROM and RAM footprint and without
|
||||||
|
//doing too many allocations. A good way to package these things seemed
|
||||||
|
//like the module boundary, so all the idls within one module can share
|
||||||
|
//all the type references.
|
||||||
|
|
||||||
|
|
||||||
|
#define PARAMETER_IN 0x0 |
||||||
|
#define PARAMETER_OUT 0x1 |
||||||
|
#define PARAMETER_INOUT 0x2 |
||||||
|
#define PARAMETER_ROUT 0x3 |
||||||
|
#define PARAMETER_INROUT 0x4 |
||||||
|
|
||||||
|
//the types that we get from idl
|
||||||
|
#define TYPE_OBJECT 0x0 |
||||||
|
#define TYPE_INTERFACE 0x1 |
||||||
|
#define TYPE_PRIMITIVE 0x2 |
||||||
|
#define TYPE_ENUM 0x3 |
||||||
|
#define TYPE_STRING 0x4 |
||||||
|
#define TYPE_WSTRING 0x5 |
||||||
|
#define TYPE_STRUCTURE 0x6 |
||||||
|
#define TYPE_UNION 0x7 |
||||||
|
#define TYPE_ARRAY 0x8 |
||||||
|
#define TYPE_SEQUENCE 0x9 |
||||||
|
|
||||||
|
//these require the pack/unpack to recurse
|
||||||
|
//so it's a hint to those languages that can optimize in cases where
|
||||||
|
//recursion isn't necessary.
|
||||||
|
#define TYPE_COMPLEX_STRUCTURE (0x10 | TYPE_STRUCTURE) |
||||||
|
#define TYPE_COMPLEX_UNION (0x10 | TYPE_UNION) |
||||||
|
#define TYPE_COMPLEX_ARRAY (0x10 | TYPE_ARRAY) |
||||||
|
#define TYPE_COMPLEX_SEQUENCE (0x10 | TYPE_SEQUENCE) |
||||||
|
|
||||||
|
|
||||||
|
typedef struct Type Type; |
||||||
|
|
||||||
|
#define INHERIT_TYPE\ |
||||||
|
int32_t nativeSize; /*in the simple case its the same as wire size and alignment*/\
|
||||||
|
union {\
|
||||||
|
struct {\
|
||||||
|
const uintptr_t p1;\
|
||||||
|
const uintptr_t p2;\
|
||||||
|
} _cast;\
|
||||||
|
struct {\
|
||||||
|
uint32_t iid;\
|
||||||
|
uint32_t bNotNil;\
|
||||||
|
} object;\
|
||||||
|
struct {\
|
||||||
|
const Type *arrayType;\
|
||||||
|
int32_t nItems;\
|
||||||
|
} array;\
|
||||||
|
struct {\
|
||||||
|
const Type *seqType;\
|
||||||
|
int32_t nMaxLen;\
|
||||||
|
} seqSimple; \
|
||||||
|
struct {\
|
||||||
|
uint32_t bFloating;\
|
||||||
|
uint32_t bSigned;\
|
||||||
|
} prim; \
|
||||||
|
const SequenceType* seqComplex;\
|
||||||
|
const UnionType *unionType;\
|
||||||
|
const StructType *structType;\
|
||||||
|
int32_t stringMaxLen;\
|
||||||
|
uint8_t bInterfaceNotNil;\
|
||||||
|
} param;\
|
||||||
|
uint8_t type;\
|
||||||
|
uint8_t nativeAlignment\
|
||||||
|
|
||||||
|
typedef struct UnionType UnionType; |
||||||
|
typedef struct StructType StructType; |
||||||
|
typedef struct SequenceType SequenceType; |
||||||
|
struct Type { |
||||||
|
INHERIT_TYPE; |
||||||
|
}; |
||||||
|
|
||||||
|
struct SequenceType { |
||||||
|
const Type * seqType; |
||||||
|
uint32_t nMaxLen; |
||||||
|
uint32_t inSize; |
||||||
|
uint32_t routSizePrimIn; |
||||||
|
uint32_t routSizePrimROut; |
||||||
|
}; |
||||||
|
|
||||||
|
//byte offset from the start of the case values for
|
||||||
|
//this unions case value array. it MUST be aligned
|
||||||
|
//at the alignment requrements for the descriptor
|
||||||
|
//
|
||||||
|
//if negative it means that the unions cases are
|
||||||
|
//simple enumerators, so the value read from the descriptor
|
||||||
|
//can be used directly to find the correct case
|
||||||
|
typedef union CaseValuePtr CaseValuePtr; |
||||||
|
union CaseValuePtr { |
||||||
|
const uint8_t* value8s; |
||||||
|
const uint16_t* value16s; |
||||||
|
const uint32_t* value32s; |
||||||
|
const uint64_t* value64s; |
||||||
|
}; |
||||||
|
|
||||||
|
//these are only used in complex cases
|
||||||
|
//so I pulled them out of the type definition as references to make
|
||||||
|
//the type smaller
|
||||||
|
struct UnionType { |
||||||
|
const Type *descriptor; |
||||||
|
uint32_t nCases; |
||||||
|
const CaseValuePtr caseValues; |
||||||
|
const Type * const *cases; |
||||||
|
int32_t inSize; |
||||||
|
int32_t routSizePrimIn; |
||||||
|
int32_t routSizePrimROut; |
||||||
|
uint8_t inAlignment; |
||||||
|
uint8_t routAlignmentPrimIn; |
||||||
|
uint8_t routAlignmentPrimROut; |
||||||
|
uint8_t inCaseAlignment; |
||||||
|
uint8_t routCaseAlignmentPrimIn; |
||||||
|
uint8_t routCaseAlignmentPrimROut; |
||||||
|
uint8_t nativeCaseAlignment; |
||||||
|
uint8_t bDefaultCase; |
||||||
|
}; |
||||||
|
|
||||||
|
struct StructType { |
||||||
|
uint32_t nMembers; |
||||||
|
const Type * const *members; |
||||||
|
int32_t inSize; |
||||||
|
int32_t routSizePrimIn; |
||||||
|
int32_t routSizePrimROut; |
||||||
|
uint8_t inAlignment; |
||||||
|
uint8_t routAlignmentPrimIn; |
||||||
|
uint8_t routAlignmentPrimROut; |
||||||
|
}; |
||||||
|
|
||||||
|
typedef struct Parameter Parameter; |
||||||
|
struct Parameter { |
||||||
|
INHERIT_TYPE; |
||||||
|
uint8_t mode; |
||||||
|
uint8_t bNotNil; |
||||||
|
}; |
||||||
|
|
||||||
|
#define SLIM_IFPTR32(is32,is64) (sizeof(uintptr_t) == 4 ? (is32) : (is64)) |
||||||
|
#define SLIM_SCALARS_IS_DYNAMIC(u) (((u) & 0x00ffffff) == 0x00ffffff) |
||||||
|
|
||||||
|
typedef struct Method Method; |
||||||
|
struct Method { |
||||||
|
uint32_t uScalars; //no method index
|
||||||
|
int32_t primInSize; |
||||||
|
int32_t primROutSize; |
||||||
|
int maxArgs; |
||||||
|
int numParams; |
||||||
|
const Parameter * const *params; |
||||||
|
uint8_t primInAlignment; |
||||||
|
uint8_t primROutAlignment; |
||||||
|
}; |
||||||
|
|
||||||
|
typedef struct Interface Interface; |
||||||
|
|
||||||
|
struct Interface { |
||||||
|
int nMethods; |
||||||
|
const Method * const *methodArray; |
||||||
|
int nIIds; |
||||||
|
const uint32_t *iids; |
||||||
|
const uint16_t* methodStringArray; |
||||||
|
const uint16_t* methodStrings; |
||||||
|
const char* strings; |
||||||
|
}; |
||||||
|
|
||||||
|
|
||||||
|
#endif //SLIM_H
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef _CALCULATOR_SLIM_H |
||||||
|
#define _CALCULATOR_SLIM_H |
||||||
|
|
||||||
|
// remote.h
|
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
|
||||||
|
#ifndef __QAIC_SLIM |
||||||
|
#define __QAIC_SLIM(ff) ff |
||||||
|
#endif |
||||||
|
#ifndef __QAIC_SLIM_EXPORT |
||||||
|
#define __QAIC_SLIM_EXPORT |
||||||
|
#endif |
||||||
|
|
||||||
|
static const Type types[1]; |
||||||
|
static const Type types[1] = {{0x1,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x1}}; |
||||||
|
static const Parameter parameters[3] = {{0x4,{{(const uintptr_t)0,(const uintptr_t)0}}, 2,0x4,3,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),0,0},{SLIM_IFPTR32(0x8,0x10),{{(const uintptr_t)&(types[0]),(const uintptr_t)0x0}}, 9,SLIM_IFPTR32(0x4,0x8),3,0}}; |
||||||
|
static const Parameter* const parameterArrays[3] = {(&(parameters[1])),(&(parameters[2])),(&(parameters[0]))}; |
||||||
|
static const Method methods[2] = {{REMOTE_SCALARS_MAKEX(0,0,0x0,0x1,0x0,0x0),0x0,0x4,1,1,(&(parameterArrays[2])),0x1,0x4},{REMOTE_SCALARS_MAKEX(0,0,0x2,0x1,0x0,0x0),0x8,0x0,5,2,(&(parameterArrays[0])),0x4,0x1}}; |
||||||
|
static const Method* const methodArrays[2] = {&(methods[0]),&(methods[1])}; |
||||||
|
static const char strings[41] = "extract_and_match\0features\0leet\0init\0img\0"; |
||||||
|
static const uint16_t methodStrings[5] = {0,37,18,32,27}; |
||||||
|
static const uint16_t methodStringsArrays[2] = {3,0}; |
||||||
|
__QAIC_SLIM_EXPORT const Interface __QAIC_SLIM(calculator_slim) = {2,&(methodArrays[0]),0,0,&(methodStringsArrays [0]),methodStrings,strings}; |
||||||
|
#endif //_CALCULATOR_SLIM_H
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
#ifndef _const_calculator_handle |
||||||
|
#define _const_calculator_handle ((remote_handle)-1) |
||||||
|
#endif //_const_calculator_handle
|
||||||
|
|
||||||
|
static void _calculator_pls_dtor(void* data) { |
||||||
|
remote_handle* ph = (remote_handle*)data; |
||||||
|
if(_const_calculator_handle != *ph) { |
||||||
|
(void)__QAIC_REMOTE(remote_handle_close)(*ph); |
||||||
|
*ph = _const_calculator_handle; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
static int _calculator_pls_ctor(void* ctx, void* data) { |
||||||
|
remote_handle* ph = (remote_handle*)data; |
||||||
|
*ph = _const_calculator_handle; |
||||||
|
if(*ph == (remote_handle)-1) { |
||||||
|
return __QAIC_REMOTE(remote_handle_open)((const char*)ctx, ph); |
||||||
|
} |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
|
#if (defined __qdsp6__) || (defined __hexagon__) |
||||||
|
#pragma weak adsp_pls_add_lookup |
||||||
|
extern int adsp_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
||||||
|
#pragma weak HAP_pls_add_lookup |
||||||
|
extern int HAP_pls_add_lookup(uint32_t type, uint32_t key, int size, int (*ctor)(void* ctx, void* data), void* ctx, void (*dtor)(void* ctx), void** ppo); |
||||||
|
|
||||||
|
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
||||||
|
remote_handle* ph; |
||||||
|
if(adsp_pls_add_lookup) { |
||||||
|
if(0 == adsp_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
||||||
|
return *ph; |
||||||
|
} |
||||||
|
return (remote_handle)-1; |
||||||
|
} else if(HAP_pls_add_lookup) { |
||||||
|
if(0 == HAP_pls_add_lookup((uint32_t)_calculator_handle, 0, sizeof(*ph), _calculator_pls_ctor, "calculator", _calculator_pls_dtor, (void**)&ph)) { |
||||||
|
return *ph; |
||||||
|
} |
||||||
|
return (remote_handle)-1; |
||||||
|
} |
||||||
|
return(remote_handle)-1; |
||||||
|
} |
||||||
|
|
||||||
|
#else //__qdsp6__ || __hexagon__
|
||||||
|
|
||||||
|
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare); |
||||||
|
|
||||||
|
#ifdef _WIN32 |
||||||
|
#include "Windows.h" |
||||||
|
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
||||||
|
return (uint32_t)InterlockedCompareExchange((volatile LONG*)puDest, (LONG)uExchange, (LONG)uCompare); |
||||||
|
} |
||||||
|
#elif __GNUC__ |
||||||
|
uint32_t _calculator_atomic_CompareAndExchange(uint32_t * volatile puDest, uint32_t uExchange, uint32_t uCompare) { |
||||||
|
return __sync_val_compare_and_swap(puDest, uCompare, uExchange); |
||||||
|
} |
||||||
|
#endif //_WIN32
|
||||||
|
|
||||||
|
|
||||||
|
__QAIC_STUB_EXPORT remote_handle _calculator_handle(void) { |
||||||
|
static remote_handle handle = _const_calculator_handle; |
||||||
|
if((remote_handle)-1 != handle) { |
||||||
|
return handle; |
||||||
|
} else { |
||||||
|
remote_handle tmp; |
||||||
|
int nErr = _calculator_pls_ctor("calculator", (void*)&tmp); |
||||||
|
if(nErr) { |
||||||
|
return (remote_handle)-1; |
||||||
|
} |
||||||
|
if(((remote_handle)-1 != handle) || ((remote_handle)-1 != (remote_handle)_calculator_atomic_CompareAndExchange((uint32_t*)&handle, (uint32_t)tmp, (uint32_t)-1))) { |
||||||
|
_calculator_pls_dtor(&tmp); |
||||||
|
} |
||||||
|
return handle; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
#endif //__qdsp6__
|
||||||
|
|
||||||
|
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_skel_invoke)(uint32_t _sc, remote_arg* _pra) __QAIC_STUB_ATTRIBUTE { |
||||||
|
return __QAIC_REMOTE(remote_handle_invoke)(_calculator_handle(), _sc, _pra); |
||||||
|
} |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
extern int remote_register_dma_handle(int, uint32_t); |
||||||
|
static __inline int _stub_method(remote_handle _handle, uint32_t _mid, uint32_t _rout0[1]) { |
||||||
|
int _numIn[1]; |
||||||
|
remote_arg _pra[1]; |
||||||
|
uint32_t _primROut[1]; |
||||||
|
int _nErr = 0; |
||||||
|
_numIn[0] = 0; |
||||||
|
_pra[(_numIn[0] + 0)].buf.pv = (void*)_primROut; |
||||||
|
_pra[(_numIn[0] + 0)].buf.nLen = sizeof(_primROut); |
||||||
|
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 0, 1, 0, 0), _pra)); |
||||||
|
_COPY(_rout0, 0, _primROut, 0, 4); |
||||||
|
_CATCH(_nErr) {} |
||||||
|
return _nErr; |
||||||
|
} |
||||||
|
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_init)(uint32* leet) __QAIC_STUB_ATTRIBUTE { |
||||||
|
uint32_t _mid = 0; |
||||||
|
return _stub_method(_calculator_handle(), _mid, (uint32_t*)leet); |
||||||
|
} |
||||||
|
static __inline int _stub_method_1(remote_handle _handle, uint32_t _mid, char* _in0[1], uint32_t _in0Len[1], char* _rout1[1], uint32_t _rout1Len[1]) { |
||||||
|
int _numIn[1]; |
||||||
|
remote_arg _pra[3]; |
||||||
|
uint32_t _primIn[2]; |
||||||
|
remote_arg* _praIn; |
||||||
|
remote_arg* _praROut; |
||||||
|
int _nErr = 0; |
||||||
|
_numIn[0] = 1; |
||||||
|
_pra[0].buf.pv = (void*)_primIn; |
||||||
|
_pra[0].buf.nLen = sizeof(_primIn); |
||||||
|
_COPY(_primIn, 0, _in0Len, 0, 4); |
||||||
|
_praIn = (_pra + 1); |
||||||
|
_praIn[0].buf.pv = _in0[0]; |
||||||
|
_praIn[0].buf.nLen = (1 * _in0Len[0]); |
||||||
|
_COPY(_primIn, 4, _rout1Len, 0, 4); |
||||||
|
_praROut = (_praIn + _numIn[0] + 0); |
||||||
|
_praROut[0].buf.pv = _rout1[0]; |
||||||
|
_praROut[0].buf.nLen = (1 * _rout1Len[0]); |
||||||
|
_TRY(_nErr, __QAIC_REMOTE(remote_handle_invoke)(_handle, REMOTE_SCALARS_MAKEX(0, _mid, 2, 1, 0, 0), _pra)); |
||||||
|
_CATCH(_nErr) {} |
||||||
|
return _nErr; |
||||||
|
} |
||||||
|
__QAIC_STUB_EXPORT int __QAIC_STUB(calculator_extract_and_match)(const uint8* img, int imgLen, uint8* features, int featuresLen) __QAIC_STUB_ATTRIBUTE { |
||||||
|
uint32_t _mid = 1; |
||||||
|
return _stub_method_1(_calculator_handle(), _mid, (char**)&img, (uint32_t*)&imgLen, (char**)&features, (uint32_t*)&featuresLen); |
||||||
|
} |
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
#endif //_CALCULATOR_STUB_H
|
||||||
Binary file not shown.
@ -0,0 +1,38 @@ |
|||||||
|
#ifndef EXTRACTOR_H |
||||||
|
#define EXTRACTOR_H |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
extern "C" { |
||||||
|
#endif |
||||||
|
|
||||||
|
#include <stdint.h> |
||||||
|
|
||||||
|
#define ORBD_KEYPOINTS 3000 |
||||||
|
#define ORBD_DESCRIPTOR_LENGTH 32 |
||||||
|
#define ORBD_HEIGHT 874 |
||||||
|
#define ORBD_WIDTH 1164 |
||||||
|
#define ORBD_FOCAL 910 |
||||||
|
|
||||||
|
// matches OrbFeatures from log.capnp
|
||||||
|
struct orb_features { |
||||||
|
// align this
|
||||||
|
uint16_t n_corners; |
||||||
|
uint16_t xy[ORBD_KEYPOINTS][2]; |
||||||
|
uint8_t octave[ORBD_KEYPOINTS]; |
||||||
|
uint8_t des[ORBD_KEYPOINTS][ORBD_DESCRIPTOR_LENGTH]; |
||||||
|
int16_t matches[ORBD_KEYPOINTS]; |
||||||
|
}; |
||||||
|
|
||||||
|
// forward declare this
|
||||||
|
struct pyramid; |
||||||
|
|
||||||
|
// manage the pyramids in extractor.c
|
||||||
|
void init_gpyrs(); |
||||||
|
int extract_and_match_gpyrs(const uint8_t *img, struct orb_features *); |
||||||
|
int extract_and_match(const uint8_t *img, struct pyramid *pyrs, struct pyramid *prev_pyrs, struct orb_features *); |
||||||
|
|
||||||
|
#ifdef __cplusplus |
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
#endif // EXTRACTOR_H
|
||||||
@ -0,0 +1,191 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <stdlib.h> |
||||||
|
#include <signal.h> |
||||||
|
#include <unistd.h> |
||||||
|
#include <stdint.h> |
||||||
|
#include <assert.h> |
||||||
|
#include <sys/resource.h> |
||||||
|
|
||||||
|
#include "common/visionipc.h" |
||||||
|
#include "common/swaglog.h" |
||||||
|
|
||||||
|
#include "extractor.h" |
||||||
|
|
||||||
|
#ifdef DSP |
||||||
|
#include "dsp/gen/calculator.h" |
||||||
|
#else |
||||||
|
#include "turbocv.h" |
||||||
|
#endif |
||||||
|
|
||||||
|
#include <zmq.h> |
||||||
|
#include <capnp/serialize.h> |
||||||
|
#include "cereal/gen/cpp/log.capnp.h" |
||||||
|
|
||||||
|
#ifndef PATH_MAX |
||||||
|
#include <linux/limits.h> |
||||||
|
#endif |
||||||
|
|
||||||
|
volatile int do_exit = 0; |
||||||
|
|
||||||
|
static void set_do_exit(int sig) { |
||||||
|
do_exit = 1; |
||||||
|
} |
||||||
|
|
||||||
|
int main(int argc, char *argv[]) { |
||||||
|
int err; |
||||||
|
setpriority(PRIO_PROCESS, 0, -13); |
||||||
|
printf("starting orbd\n"); |
||||||
|
|
||||||
|
#ifdef DSP |
||||||
|
uint32_t test_leet = 0; |
||||||
|
char my_path[PATH_MAX+1]; |
||||||
|
memset(my_path, 0, sizeof(my_path)); |
||||||
|
|
||||||
|
ssize_t len = readlink("/proc/self/exe", my_path, sizeof(my_path)); |
||||||
|
assert(len > 5); |
||||||
|
my_path[len-5] = '\0'; |
||||||
|
LOGW("running from %s with PATH_MAX %d", my_path, PATH_MAX); |
||||||
|
|
||||||
|
char adsp_path[PATH_MAX+1]; |
||||||
|
snprintf(adsp_path, PATH_MAX, "ADSP_LIBRARY_PATH=%s/dsp/gen", my_path); |
||||||
|
assert(putenv(adsp_path) == 0); |
||||||
|
|
||||||
|
assert(calculator_init(&test_leet) == 0); |
||||||
|
assert(test_leet == 0x1337); |
||||||
|
LOGW("orbd init complete"); |
||||||
|
#else |
||||||
|
init_gpyrs(); |
||||||
|
#endif |
||||||
|
|
||||||
|
signal(SIGINT, (sighandler_t) set_do_exit); |
||||||
|
signal(SIGTERM, (sighandler_t) set_do_exit); |
||||||
|
|
||||||
|
void *ctx = zmq_ctx_new(); |
||||||
|
|
||||||
|
void *orb_features_sock = zmq_socket(ctx, ZMQ_PUB); |
||||||
|
assert(orb_features_sock); |
||||||
|
zmq_bind(orb_features_sock, "tcp://*:8058"); |
||||||
|
|
||||||
|
void *orb_features_summary_sock = zmq_socket(ctx, ZMQ_PUB); |
||||||
|
assert(orb_features_summary_sock); |
||||||
|
zmq_bind(orb_features_summary_sock, "tcp://*:8062"); |
||||||
|
|
||||||
|
struct orb_features *features = (struct orb_features *)malloc(sizeof(struct orb_features)); |
||||||
|
int last_frame_id = 0; |
||||||
|
uint64_t frame_count = 0; |
||||||
|
|
||||||
|
// every other frame
|
||||||
|
const int RATE = 2; |
||||||
|
|
||||||
|
VisionStream stream; |
||||||
|
while (!do_exit) { |
||||||
|
VisionStreamBufs buf_info; |
||||||
|
err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); |
||||||
|
if (err) { |
||||||
|
printf("visionstream connect fail\n"); |
||||||
|
usleep(100000); |
||||||
|
continue; |
||||||
|
} |
||||||
|
uint64_t timestamp_last_eof = 0; |
||||||
|
while (!do_exit) { |
||||||
|
VIPCBuf *buf; |
||||||
|
VIPCBufExtra extra; |
||||||
|
buf = visionstream_get(&stream, &extra); |
||||||
|
if (buf == NULL) { |
||||||
|
printf("visionstream get failed\n"); |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
// every other frame
|
||||||
|
frame_count++; |
||||||
|
if ((frame_count%RATE) != 0) { |
||||||
|
continue; |
||||||
|
} |
||||||
|
|
||||||
|
uint64_t start = nanos_since_boot(); |
||||||
|
#ifdef DSP |
||||||
|
int ret = calculator_extract_and_match((uint8_t *)buf->addr, ORBD_HEIGHT*ORBD_WIDTH, (uint8_t *)features, sizeof(struct orb_features)); |
||||||
|
#else |
||||||
|
int ret = extract_and_match_gpyrs((uint8_t *) buf->addr, features); |
||||||
|
#endif |
||||||
|
uint64_t end = nanos_since_boot(); |
||||||
|
LOGD("total(%d): %6.2f ms to get %4d features on %d", ret, (end-start)/1000000.0, features->n_corners, extra.frame_id); |
||||||
|
assert(ret == 0); |
||||||
|
|
||||||
|
if (last_frame_id+RATE != extra.frame_id) { |
||||||
|
LOGW("dropped frame!"); |
||||||
|
} |
||||||
|
|
||||||
|
last_frame_id = extra.frame_id; |
||||||
|
|
||||||
|
if (timestamp_last_eof == 0) { |
||||||
|
timestamp_last_eof = extra.timestamp_eof; |
||||||
|
continue; |
||||||
|
} |
||||||
|
|
||||||
|
int match_count = 0; |
||||||
|
|
||||||
|
// *** send OrbFeatures ***
|
||||||
|
{ |
||||||
|
// create capnp message
|
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
|
||||||
|
auto orb_features = event.initOrbFeatures(); |
||||||
|
|
||||||
|
// set timestamps
|
||||||
|
orb_features.setTimestampEof(extra.timestamp_eof); |
||||||
|
orb_features.setTimestampLastEof(timestamp_last_eof); |
||||||
|
|
||||||
|
// init descriptors for send
|
||||||
|
kj::ArrayPtr<capnp::byte> descriptorsPtr = kj::arrayPtr((uint8_t *)features->des, ORBD_DESCRIPTOR_LENGTH * features->n_corners); |
||||||
|
orb_features.setDescriptors(descriptorsPtr); |
||||||
|
|
||||||
|
auto xs = orb_features.initXs(features->n_corners); |
||||||
|
auto ys = orb_features.initYs(features->n_corners); |
||||||
|
auto octaves = orb_features.initOctaves(features->n_corners); |
||||||
|
auto matches = orb_features.initMatches(features->n_corners); |
||||||
|
|
||||||
|
// copy out normalized keypoints
|
||||||
|
for (int i = 0; i < features->n_corners; i++) { |
||||||
|
xs.set(i, (features->xy[i][0] * 1.0f - ORBD_WIDTH / 2) / ORBD_FOCAL); |
||||||
|
ys.set(i, (features->xy[i][1] * 1.0f - ORBD_HEIGHT / 2) / ORBD_FOCAL); |
||||||
|
octaves.set(i, features->octave[i]); |
||||||
|
matches.set(i, features->matches[i]); |
||||||
|
match_count += features->matches[i] != -1; |
||||||
|
} |
||||||
|
|
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
zmq_send(orb_features_sock, bytes.begin(), bytes.size(), 0); |
||||||
|
} |
||||||
|
|
||||||
|
// *** send OrbFeaturesSummary ***
|
||||||
|
|
||||||
|
{ |
||||||
|
// create capnp message
|
||||||
|
capnp::MallocMessageBuilder msg; |
||||||
|
cereal::Event::Builder event = msg.initRoot<cereal::Event>(); |
||||||
|
event.setLogMonoTime(nanos_since_boot()); |
||||||
|
|
||||||
|
auto orb_features_summary = event.initOrbFeaturesSummary(); |
||||||
|
|
||||||
|
orb_features_summary.setTimestampEof(extra.timestamp_eof); |
||||||
|
orb_features_summary.setTimestampLastEof(timestamp_last_eof); |
||||||
|
orb_features_summary.setFeatureCount(features->n_corners); |
||||||
|
orb_features_summary.setMatchCount(match_count); |
||||||
|
orb_features_summary.setComputeNs(end-start); |
||||||
|
|
||||||
|
auto words = capnp::messageToFlatArray(msg); |
||||||
|
auto bytes = words.asBytes(); |
||||||
|
zmq_send(orb_features_summary_sock, bytes.begin(), bytes.size(), 0); |
||||||
|
} |
||||||
|
|
||||||
|
timestamp_last_eof = extra.timestamp_eof; |
||||||
|
} |
||||||
|
} |
||||||
|
visionstream_destroy(&stream); |
||||||
|
return 0; |
||||||
|
} |
||||||
|
|
||||||
@ -0,0 +1,13 @@ |
|||||||
|
#!/bin/sh |
||||||
|
finish() { |
||||||
|
echo "exiting orbd" |
||||||
|
pkill -SIGINT -P $$ |
||||||
|
} |
||||||
|
|
||||||
|
trap finish EXIT |
||||||
|
|
||||||
|
while true; do |
||||||
|
./orbd & |
||||||
|
wait $! |
||||||
|
done |
||||||
|
|
||||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue