mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-22 08:43:54 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.7.6 release

Browse Source
This commit is contained in:
Vehicle Researcher
2020-05-31 13:22:49 -07:00
parent 2d659de09c
commit 4fd2b0e730
733 changed files with 6097 additions and 211199 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/transformations/camera.py
+12 -13
View File
@@ -59,7 +59,7 @@ def vp_from_ke(m):
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])
return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
def vp_from_rpy(rpy):
@@ -81,10 +81,10 @@ def normalize(img_pts, intrinsics=eon_intrinsics):
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 = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
img_pts_normalized = img_pts.dot(intrinsics_inv.T)
img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
return img_pts_normalized[:,:2].reshape(input_shape)
return img_pts_normalized[:, :2].reshape(input_shape)
def denormalize(img_pts, intrinsics=eon_intrinsics):
@@ -93,13 +93,13 @@ def denormalize(img_pts, intrinsics=eon_intrinsics):
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 = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
img_pts_denormalized = img_pts.dot(intrinsics.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)
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):
@@ -124,10 +124,10 @@ def img_from_device(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_view[pt_view[:, 2] < 0] = np.nan
pt_img = pt_view/pt_view[:,2:3]
return pt_img.reshape(input_shape)[:,:2]
pt_img = pt_view/pt_view[:, 2:3]
return pt_img.reshape(input_shape)[:, :2]
def get_camera_frame_from_calib_frame(camera_frame_from_road_frame):
@@ -145,4 +145,3 @@ def pretransform_from_calib(calib):
camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame)
camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame)
return np.linalg.inv(camera_frame_from_calib_frame)
common/transformations/coordinates.py
+6 -4
View File
@@ -18,9 +18,9 @@ def geodetic2ecef(geodetic, radians=False):
geodetic = np.atleast_2d(geodetic)
ratio = 1.0 if radians else (np.pi / 180.0)
lat = ratio*geodetic[:,0]
lon = ratio*geodetic[:,1]
alt = geodetic[:,2]
lat = ratio*geodetic[:, 0]
lon = ratio*geodetic[:, 1]
alt = geodetic[:, 2]
xi = np.sqrt(1 - esq * np.sin(lat)**2)
x = (a / xi + alt) * np.cos(lat) * np.cos(lon)
@@ -52,7 +52,7 @@ def ecef2geodetic(ecef, radians=False):
S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = np.sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
U = np.sqrt(pow((r - esq * r_0), 2) + z * z)
V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
@@ -78,6 +78,8 @@ class LocalCoord():
[-np.sin(lat)*np.sin(lon), np.cos(lon), -np.cos(lat)*np.sin(lon)],
[np.cos(lat), 0, -np.sin(lat)]])
self.ecef2ned_matrix = self.ned2ecef_matrix.T
self.ecef_from_ned_matrix = self.ned2ecef_matrix
self.ned_from_ecef_matrix = self.ecef2ned_matrix
@classmethod
def from_geodetic(cls, init_geodetic):
common/transformations/model.py
+2 -2
View File
@@ -130,9 +130,9 @@ def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
def get_model_frame(snu_full, camera_frame_from_model_frame, size):
idxs = camera_frame_from_model_frame.dot(np.column_stack([np.tile(np.arange(size[0]), size[1]),
np.tile(np.arange(size[1]), (size[0],1)).T.flatten(),
np.tile(np.arange(size[1]), (size[0], 1)).T.flatten(),
np.ones(size[0] * size[1])]).T).T.astype(int)
calib_flat = snu_full[idxs[:,1], idxs[:,0]]
calib_flat = snu_full[idxs[:, 1], idxs[:, 0]]
if len(snu_full.shape) == 3:
calib = calib_flat.reshape((size[1], size[0], 3))
elif len(snu_full.shape) == 2:
common/transformations/orientation.py
+11 -11
View File
@@ -13,11 +13,11 @@ 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)
output_shape = (-1, 4)
else:
output_shape = (4,)
eulers = np.atleast_2d(eulers)
gamma, theta, psi = eulers[:,0], eulers[:,1], eulers[:,2]
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)
@@ -30,7 +30,7 @@ def euler2quat(eulers):
quats = array([q0, q1, q2, q3]).T
for i in range(len(quats)):
if quats[i,0] < 0:
if quats[i, 0] < 0:
quats[i] = -quats[i]
return quats.reshape(output_shape)
@@ -38,11 +38,11 @@ def euler2quat(eulers):
def quat2euler(quats):
quats = array(quats)
if len(quats.shape) > 1:
output_shape = (-1,3)
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]
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))
@@ -101,7 +101,7 @@ def rot2quat(rots):
q = np.empty((len(rots), 4))
for i in range(len(rots)):
_, eigvecs = linalg.eigh(K3[i].T)
eigvecs = eigvecs[:,3:]
eigvecs = eigvecs[:, 3:]
q[i, 0] = eigvecs[-1]
q[i, 1:] = -eigvecs[:-1].flatten()
if q[i, 0] < 0:
@@ -124,8 +124,8 @@ def rot2euler(rots):
quats_from_rotations = rot2quat
quat_from_rot = rot2quat
rotations_from_quats = quat2rot
rot_from_quat= quat2rot
rot_from_quat= quat2rot
rot_from_quat = quat2rot
rot_from_quat = quat2rot
euler_from_rot = rot2euler
euler_from_quat = quat2euler
rot_from_euler = euler2rot
@@ -154,9 +154,9 @@ 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]])
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))