def test_basic():
v1 = np.array([1, 1, 0])
v2 = np.array([-1, 1, 0])
look = vg.basis.z
assert vg.signed_angle(v1, v2, look) == 90
assert vg.signed_angle(v2, v1, look) == -90
assert isinstance(vg.signed_angle(v1, v2, look), float)
def preprocess_angle(y):
temp = y[:, 4, :]
temp = temp.reshape(-1, 25, 3)
#temp = temp[:,5:,:]
y = []
for f in temp:
#thumb
thumb_v = f[4] - f[0]
thumb_v = thumb_v / np.linalg.norm(thumb_v)
f = f[5:, :]
for i in range(4): #index~pinky finger
for j in range(1, 4):
v1 = f[5 * i + j] - f[5 * i + j - 1] #get previous bone vector
v2 = f[5 * i + j + 1] - f[5 * i + j] #get current bone vector
if j == 1: #MCP
#angle = vg.signed_angle(v1, v2, look=np.array([-1,0,0]))
angle = vg.signed_angle(v1, v2, look=np.array([0, -1, 0]))
y.append(angle)
angle = vg.signed_angle(v1, v2, look=np.array([-1, 0, 0]))
y.append(angle)
for i in thumb_v:
y.append(i)
y = np.array(y).reshape(-1, 19)
return y
def tilted(self, new_point, coplanar_point):
"""
Create a new plane, tilted so it passes through `new_point`. Also
specify a `coplanar_point` which the old and new planes should have
in common.
Args:
new_point (np.arraylike): A point on the desired plane, with shape
`(3,)`.
coplanar_point (np.arraylike): The `(3,)` point which the old and
new planes have in common.
Returns:
Plane: The adjusted plane.
"""
vg.shape.check(locals(), "new_point", (3, ))
vg.shape.check(locals(), "coplanar_point", (3, ))
vector_along_old_plane = self.project_point(new_point) - coplanar_point
vector_along_new_plane = new_point - coplanar_point
axis_of_rotation = vg.perpendicular(vector_along_old_plane,
self.normal)
angle_between_vectors = vg.signed_angle(
vector_along_old_plane,
vector_along_new_plane,
look=axis_of_rotation,
units="rad",
)
new_normal = vg.rotate(
self.normal,
around_axis=axis_of_rotation,
angle=angle_between_vectors,
units="rad",
)
return Plane(point_on_plane=coplanar_point, unit_normal=new_normal)
def vertex_vector_orientations(self,
units='degrees',
return_distance=False):
"""Returns the angular orientations of each centroid-to-vertex vector.
The orientation is defined by two angles, theta and phi.
Theta is the angle with respect to [ 0, 0, 1 ] and ranges from 0 to 180 degrees.
Phi is the angle with respect to [ 1, 0, 0 ] and ranges from -180 to +180 degrees.
Args:
units (:obj:`str`, optional): Optionally select the units for the calculated angles.
Options are `degrees` or `radians`.
Default is `degrees`.
return_distance (:obj:`bool`, optional): Optionally also return the distance.
Returns:
(list(tuple)): A list of `(theta,phi)` tuple pairs.
"""
vg_units = {'degrees': 'deg', 'radians': 'rad'}
theta = [
vg.angle(np.array([0.0, 0.0, 1.0]), point, units=vg_units[units])
for point in self.vertex_vectors
]
phi = [
vg.signed_angle(np.array([1.0, 0.0, 0.0]),
point,
look=np.array([0.0, 0.0, 1.0]),
units=vg_units[units])
for point in self.vertex_vectors
]
if return_distance:
distance = [vg.magnitude(point) for point in self.vertex_vectors]
return list(zip(theta, phi, distance))
return list(zip(theta, phi))
def calculateHeading(frame, converted, carLength):
if frame == 0 or frame >= maxFrames:
return 0
difference = getDiffConvertedFrames(frame - 1, frame, converted)
"""get front and rear points, assuming front has biggest dx"""
front = difference.loc[abs(difference["dx"]).idxmax()]['pixel']
rear = difference.loc[abs(difference["dx"]).idxmin()]['pixel']
"""check if bycicle model or simple motion with fix heading"""
if (difference[["dx", "dz"]].round(3).nunique() == 1).all():
"""if fix heading return 0"""
return 0
"""get vector of plane and normalise"""
surface = getConvertedFramePixel(frame, front,
converted) - getConvertedFramePixel(
frame, rear, converted)
"""get vector of steer"""
steer = getConvertedFramePixel(frame, front,
converted) - getConvertedFramePixel(
frame - 1, front, converted)
"""calculate delta while looking down in y axes"""
delta = vg.signed_angle(surface, steer, np.array([0, 1, 0]), units="rad")
"""if steer angle is big it can also mean fix heading"""
if (np.degrees(delta) > 80):
"""if fix heading return 0"""
return 0
"""calculate velocity vector at rear assuming dt =1 s"""
velo = math.sqrt(difference.loc[difference['pixel'] == rear]['dz']**2 +
difference.loc[difference['pixel'] == rear]['dx']**2) / dt
"""vehicle length"""
"""calculate heading according to bicycle model"""
R = carLength / math.tan(delta)
HeadingChange = velo / R
return math.degrees(HeadingChange)
def evaluate_point_normal(gts,preds):
#diff = np.abs(gts-preds)
pos = []
norm = []
ang = []
for i in range(len(gts)):
diff = gts[i]-preds[i]
pos.append(diff[:3])
norm.append(diff[3:])
#angles = [np.arctan2(math.sqrt(diff[4]*diff[4]+diff[5]*diff[5]), diff[3]),
# np.arctan2(math.sqrt(diff[3]*diff[3]+diff[5]*diff[5]), diff[4]),
# np.arctan2(math.sqrt(diff[4]*diff[4]+diff[3]*diff[3]), diff[5])]
angles = np.array([vg.signed_angle(np.array([diff[3],diff[4],diff[5]]),np.array([1,0,0]), look=vg.basis.y, units="deg"),
vg.signed_angle(np.array([diff[3],diff[4],diff[5]]),np.array([0,1,0]), look=vg.basis.z, units="deg"),
vg.signed_angle(np.array([diff[3],diff[4],diff[5]]),np.array([0,0,1]), look=vg.basis.x, units="deg")])
ang.append(angles)
'''
gt_ang = np.array([vg.signed_angle(np.array([gts[i,3],gts[i,4],gts[i,5]]), np.array([1,0,0]), look=vg.basis.y, units="deg"),
vg.signed_angle(np.array([gts[i,3],gts[i,4],gts[i,5]]), np.array([0,1,0]), look=vg.basis.z, units="deg"),
vg.signed_angle(np.array([gts[i,3],gts[i,4],gts[i,5]]), np.array([0,0,1]), look=vg.basis.x, units="deg")])
pred_ang = np.array([vg.signed_angle(np.array([preds[i,3],preds[i,4],preds[i,5]]), np.array([1,0,0]), look=vg.basis.y, units="deg"),
vg.signed_angle(np.array([preds[i,3],preds[i,4],preds[i,5]]), np.array([0,1,0]), look=vg.basis.z, units="deg"),
vg.signed_angle(np.array([preds[i,3],preds[i,4],preds[i,5]]) ,np.array([0,0,1]), look=vg.basis.x, units="deg")])
ang.append(pred_ang-gt_ang)
'''
df = pd.DataFrame(np.concatenate((np.array(pos),np.array(norm)), axis = 1),columns=['dx', 'dy', 'dz','dnx', 'dny', 'dnz'])
#print(df.describe())
ax = sns.boxplot(data=df)
ax.set_title('Absolute error compared to demo')
ax.set_ylabel('[m]')
plt.savefig('plots/pos_norm_err.png')
#plt.show()
ang_df = pd.DataFrame(np.array(ang),columns=['dax', 'day', 'daz'])
#print(ang_df.describe())
ax = sns.boxplot(data=ang_df)
ax.set_title('Absolute error compared to demo')
ax.set_ylabel('[deg]')
plt.savefig('plots/ang_err.png')
#plt.show()
np.save("output/{}_pos_error".format("val"),np.array(pos))
np.save("output/{}_norm_error".format("val"),np.array(norm))
def angle(pos1, pos2, rot):
v1 = np.array([0, 0, -1], np.float)
v2 = pos1 - pos2
angl = vg.signed_angle(v2, v1, look=vg.basis.y) + rot[0]
if np.isnan(angl):
return 0.0
return -int(norm_angle(angl))
def evaluate_point_normal(gts, preds):
#diff = np.abs(gts-preds)
pos = []
ang = []
for i in range(len(gts)):
diff = gts[i] - preds[i]
pos.append(diff[:3])
#angles = [np.arctan2(math.sqrt(diff[4]*diff[4]+diff[5]*diff[5]), diff[3]),
# np.arctan2(math.sqrt(diff[3]*diff[3]+diff[5]*diff[5]), diff[4]),
# np.arctan2(math.sqrt(diff[4]*diff[4]+diff[3]*diff[3]), diff[5])]
angles = [
vg.signed_angle(np.array([diff[3], diff[4], diff[5]]),
np.array([1, 0, 0]),
look=vg.basis.y,
units="deg"),
vg.signed_angle(np.array([diff[3], diff[4], diff[5]]),
np.array([0, 1, 0]),
look=vg.basis.z,
units="deg"),
vg.signed_angle(np.array([diff[3], diff[4], diff[5]]),
np.array([0, 0, 1]),
look=vg.basis.x,
units="deg")
]
ang.append(angles)
# Creates two subplots and unpacks the output array immediately
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(12, 6))
ax1 = sns.boxplot(data=pd.DataFrame(np.array(pos),
columns=['dx', 'dy', 'dz']),
ax=ax1)
ax1.set_title('Absolute error compared to demo')
ax1.set_ylabel('[m]')
ax2 = sns.boxplot(data=pd.DataFrame(np.array(ang),
columns=['ax', 'ay', 'az']),
ax=ax2)
ax2.set_title('Absolute error compared to demo')
ax2.set_ylabel('[deg]')
plt.tight_layout()
plt.savefig('plots/pose_error.png')
def evaluate_point_normals(gts, preds):
pos, dist, norm, ang = [], [], [], []
idx = 0
for gt, pred in zip(gts, preds):
# numeric differences
diff = gt - pred
pos.append(diff[:3])
dist.append(np.linalg.norm(diff[:3]))
norm.append(diff[3:])
# angle difference
angles = [
vg.signed_angle(gt[3:], pred[3:], look=vg.basis.y, units="deg"),
vg.signed_angle(gt[3:], pred[3:], look=vg.basis.z, units="deg"),
vg.signed_angle(gt[3:], pred[3:], look=vg.basis.x, units="deg"),
vg.angle(gt[3:], pred[3:], units="deg")
]
ang.append(angles)
idx += 1
return np.array(pos), np.array(dist), np.array(norm), np.array(ang)
def calculatYawFromWorld(frame):
"""calculate Yaw"""
distances = squareform(
pdist(world.loc[world["Frame"] == frame].to_numpy()[:, 1:4]))
points = np.where(distances == np.max(distances))
surface = getWorldFramePixel(frame, points[0][1]) - getWorldFramePixel(
frame, points[0][0])
"""calculate angle to Z axes"""
return vg.signed_angle(np.array([0, 0, 1]),
surface,
np.array([0, 1, 0]),
units="deg")
def calcDistance(self):
if self.isMove == True:
if self.isLinear == True: #find the linear distance between points
self.distance = round(math.sqrt((self.endPoint.x-self.startPoint.x)**2+(self.endPoint.y-self.startPoint.y)**2+(self.endPoint.z-self.startPoint.z)**2), 4)
elif self.isIJKArc == True: #find the arc distance using IJK
#boop the coordinates into vectors
if self.g == 2: #G02 is clockwise
vec2 = numpy.array([self.startPoint.x - self.i, self.startPoint.y - self.j, self.startPoint.z - self.k])
vec1 = numpy.array([self.endPoint.x - self.i, self.endPoint.y - self.j, self.endPoint.z - self.k])
else: #counterclockwise
vec1 = numpy.array([self.startPoint.x - self.i, self.startPoint.y - self.j, self.startPoint.z - self.k])
vec2 = numpy.array([self.endPoint.x - self.i, self.endPoint.y - self.j, self.endPoint.z - self.k])
#find the radius
#Old: radius = round(math.sqrt((self.endPoint.x - self.i)**2 + (self.endPoint.y - self.j)**2 + (self.endPoint.z - self.k)**2), 4)
#need to figure out the center point instead of the offset
radius = round(math.sqrt((self.endPoint.x - self.startPoint.x - self.i)**2 + (self.endPoint.y - self.startPoint.y - self.j)**2 + (self.endPoint.z - self.startPoint.z - self.k)**2), 4)
if self.startPoint == self.endPoint: #this is a complete circle
self.distance = round(2*numpy.pi*radius, 4)
else:
#calculate the angle based on the plane
if self.plane == 'XY':
angle = vg.signed_angle(vec1, vec2, look=vg.basis.z)
elif self.plane == 'ZX':
angle = vg.signed_angle(vec1, vec2, look=vg.basis.y)
elif self.plane == 'YZ':
angle = vg.signed_angle(vec1, vec2, look=vg.basis.x)
#negative angle indicates obtuse
if angle < 0:
angle += 360
self.distance = round(2*numpy.pi*radius*(angle/360),4)
else: #find the arc distance using R (not implemented yet)
pass
else:
self.distance = None
def vec2azel(ref, type='y', units="deg"):
ref = np.array(ref)
if type == 'y':
a = vg.signed_angle(ref, vg.basis.x, look=vg.basis.z, units=units)
e = vg.signed_angle(ref, vg.basis.y, look=vg.basis.x, units=units)
elif type == 'z':
a = vg.signed_angle(ref, vg.basis.x, look=vg.basis.y, units=units)
e = vg.signed_angle(ref, vg.basis.z, look=vg.basis.x, units=units)
else:
a = vg.signed_angle(ref, vg.basis.z, look=vg.basis.y, units=units)
e = vg.signed_angle(ref, vg.basis.x, look=vg.basis.z, units=units)
print('azimuth = ' + str(a) + ', elevation = ' + str(e))
return -a, -e
def angle_between(self, other: "Vector", normal: "Vector" = None) -> float:
"""Get the angle between this vector & another
This is the angle necessary to rotate this vector into alignment with
the other vector, about the normal
Note:
If the vectors are antiparallel, 180.0 will be arbitrarily returned
(as -180.0 would be equally correct).
Args:
other: The other angle, to which to calculate the angle
normal: An optional, specified normal; if not privided, the
cross product between this vector & the other vector will be
used (which will ensure that the result is positive)
"""
try:
return vg.signed_angle(self.array,
other.array,
look=(normal or self.normal(other)).array)
except NoNormalForParallels:
return 0.0
except NoNormalForAntiparallels:
return 180.0
ang_dist=()
tl=[]
name=[]
Data={}
for i in mol[0].get_residues():
for j in mol[0].get_residues():
if(i.get_id() < j.get_id() and i.get_name() not in restricted_res and j.get_name() not in restricted_res):
res1_vec=i.get_calpha().get_location() - i.get_tip().get_location()
res2_vec=j.get_calpha().get_location() - j.get_tip().get_location()
#name.append(i.get_name() + '-' + j.get_name())
#name1=i.get_name() + '-' + j.get_name()
#angle=np.arccos(np.dot(res1_vec,res2_vec)/(np.linalg.norm(res1_vec)*np.linalg.norm(res2_vec)))
angle=vg.signed_angle(res1_vec,res2_vec,look=vg.basis.z)
if angle<0:
angle=angle+360
res1 = i.get_tip().get_location()
#print(res1)
res2 = j.get_tip().get_location()
#print(res2)
distmin=np.linalg.norm(res1-res2)
#if distmin > 13:
#print(i.get_id(), j.get_id(), distmin)
key=sorted([i.get_name(),j.get_name()])
def process_a_split(data_dir,
target_data_dir,
split_file_path,
bev_bnds,
bev_meter_per_pixel,
bev_wanted_classes,
offset=0):
"""
Args:
data_dir: directory that contains data corresponding to A SPECIFIC
SPLIT (train, validation, test)
target_data_dir: the dir to write to. Contains data corresponding to
A SPECIFIC SPLIT in KITTI (training, testing)
split_file_path: location to write all the sample_idx of a split
bev_bnds: [4] containing [x_min, x_max, y_min, y_max] in meter for the bev
bev_meter_per_pixel: number of meters in a pixel in bev, most often fractional
bev_wanted_classes: [VEHICLE, BICYCLIST, PEDESTRIAN, ...] the classes to be
for bev
offset: first sample_idx to start counting from, needed to differentiate
training and validation sample_idx because in KITTI they are under
the same dir
"""
print("Processing", data_dir)
target_velodyne_dir = os.path.join(target_data_dir, 'velodyne')
target_velodyne_reduced_dir = os.path.join(target_data_dir,
'velodyne_reduced')
target_calib_dir = os.path.join(target_data_dir, 'calib')
target_image_2_dir = os.path.join(target_data_dir, 'image_2')
if 'test' not in split_file_path:
target_label_2_dir = os.path.join(target_data_dir, 'label_2')
os.makedirs(target_velodyne_dir, exist_ok=True)
os.makedirs(target_velodyne_reduced_dir, exist_ok=True)
os.makedirs(target_calib_dir, exist_ok=True)
os.makedirs(target_image_2_dir, exist_ok=True)
if 'test' not in split_file_path:
os.makedirs(target_label_2_dir, exist_ok=True)
############################## for saving BEV segmentation masks paths
target_bev_drivable_dir = os.path.join(target_data_dir, 'bev_DRIVABLE')
os.makedirs(target_bev_drivable_dir, exist_ok=True)
target_bev_fov_dir = os.path.join(target_data_dir, 'bev_FOV')
os.makedirs(target_bev_fov_dir, exist_ok=True)
target_bev_cls_dirs = []
for wanted_cls in bev_wanted_classes:
target_bev_cls_dir = os.path.join(target_data_dir,
'bev_{}'.format(wanted_cls))
os.makedirs(target_bev_cls_dir, exist_ok=True)
target_bev_cls_dirs.append(target_bev_cls_dir)
############################## end for saving BEV segmentation masks paths
# Check the number of logs, defined as one continuous trajectory
argoverse_loader = ArgoverseTrackingLoader(data_dir)
print('Total number of logs:', len(argoverse_loader))
argoverse_loader.print_all()
print('\n')
cams = [
'ring_front_center', 'ring_front_left', 'ring_front_right',
'ring_rear_left', 'ring_rear_right', 'ring_side_left',
'ring_side_right'
]
# count total number of files
total_number = 0
for q in argoverse_loader.log_list:
log_lidar_path = os.path.join(data_dir, q, 'lidar')
path, dirs, files = next(os.walk(log_lidar_path))
total_number = total_number + len(files)
total_number = total_number * len(cams)
print('Now write sample indices to split file.')
write_split_file(split_file_path, offset, total_number)
bar = progressbar.ProgressBar(maxval=total_number, \
widgets=[progressbar.Bar('=', '[', ']'), ' ', progressbar.Percentage()])
print('Total number of files: {}. Translation starts...'.format(
total_number))
print('Progress:')
bar.start()
i = 0
kitti_to_argo_mapping = {}
for log_id in sorted(argoverse_loader.log_list):
argoverse_data = argoverse_loader.get(log_id)
from argoverse.map_representation.map_api import ArgoverseMap
argoverse_map = ArgoverseMap()
for cam in cams:
############################## Calibration for this whole log ##############################
log_calib_path = os.path.join(data_dir, log_id,
'vehicle_calibration_info.json')
calibration_data = calibration.load_calib(log_calib_path)[cam]
calib_file_content = construct_calib_str(calibration_data,
pts_in_cam_ego_coord=True)
log_lidar_dir = os.path.join(data_dir, log_id, 'lidar')
# Loop through the each lidar frame (10Hz) to rename, copy, and adapt
# all images, lidars, calibration files, and label files.
for frame_idx, timestamp in enumerate(
sorted(argoverse_data.lidar_timestamp_list)):
idx = str(i + offset).zfill(9)
# recording the mapping from kitti to argo
# (log index, the lidar frame index) uniquely identify a sample
kitti_to_argo_mapping[idx] = (log_id, frame_idx)
i += 1
if i < total_number:
bar.update(i + 1)
############################## Lidar ##############################
# Save lidar file into .bin format under the new directory
lidar_file_path = os.path.join(
log_lidar_dir, 'PC_{}.ply'.format(str(timestamp)))
target_lidar_file_path = os.path.join(target_velodyne_dir,
idx + '.bin')
lidar_data = load_ply(lidar_file_path)
# run this block of code only if we are using the cam coord instead of the ego coord
if True:
in_cam_coord = calibration_data.project_ego_to_cam(
lidar_data)
in_cam_coord = in_cam_coord[:, [2, 0,
1]] # tranpose the xyz
in_cam_coord[:, [1, 2]] *= -1 # flip 2 of the axes
lidar_data = in_cam_coord
lidar_data_augmented = np.concatenate(
(lidar_data, np.zeros([lidar_data.shape[0], 1])), axis=1)
lidar_data_augmented = lidar_data_augmented.astype('float32')
lidar_data_augmented.tofile(target_lidar_file_path)
############################## Image ##############################
# Save the image file into .png format under the new directory
cam_file_path = argoverse_data.image_list_sync[cam][frame_idx]
target_cam_file_path = os.path.join(target_image_2_dir,
idx + '.png')
copyfile(cam_file_path, target_cam_file_path)
target_calib_file_path = os.path.join(target_calib_dir,
idx + '.txt')
file = open(target_calib_file_path, 'w+')
file.write(calib_file_content)
file.close()
############################## BEV binary masks ##############################
bev_drivable_save_path = os.path.join(target_bev_drivable_dir,
idx + '.png')
bev_wanted_save_paths = [
os.path.join(cls_dir, idx + '.png')
for cls_dir in target_bev_cls_dirs
]
bev_fov_save_path = os.path.join(target_bev_fov_dir,
idx + '.png')
get_bev(argoverse_data,
argoverse_map,
log_id,
frame_idx,
bev_bnds,
bev_meter_per_pixel,
bev_drivable_save_path,
bev_wanted_classes,
bev_wanted_save_paths,
bev_fov_save_path,
visualize=False,
camera_calib=calibration_data)
############################## Labels ##############################
if 'test' in split_file_path:
continue
label_object_list = argoverse_data.get_label_object(frame_idx)
target_label_2_file_path = os.path.join(
target_label_2_dir, idx + '.txt')
file = open(target_label_2_file_path, 'w+')
# DontCare objects must appear at the end as per KITTI label files
object_lines = []
dontcare_lines = []
for detected_object in label_object_list:
classes = detected_object.label_class
classes = OBJ_CLASS_MAPPING_DICT[
classes] # map class type from artoverse to KITTI
occulusion = round(detected_object.occlusion / 25)
height = detected_object.height
length = detected_object.length
width = detected_object.width
truncated = 0
center = detected_object.translation # in ego frame
corners_ego_frame = detected_object.as_3d_bbox(
) # all eight points in ego frame
corners_cam_frame = calibration_data.project_ego_to_cam(
corners_ego_frame
) # all eight points in the camera frame
image_corners = calibration_data.project_ego_to_image(
corners_ego_frame)
image_bbox = [
min(image_corners[:, 0]),
min(image_corners[:, 1]),
max(image_corners[:, 0]),
max(image_corners[:, 1])
]
# the four coordinates we need for KITTI
image_bbox = [round(x) for x in image_bbox]
center_cam_frame = calibration_data.project_ego_to_cam(
np.array([center]))
# if 0 < center_cam_frame[0][2] < max_d and 0 < image_bbox[0] < 1920 and 0 < image_bbox[1] < 1200 and 0 < \
# image_bbox[2] < 1920 and 0 < image_bbox[3] < 1200:
if True:
# the center coordinates in cam frame we need for KITTI
# for the orientation, we choose point 1 and point 5 for application
p1 = corners_cam_frame[1]
p5 = corners_cam_frame[5]
# dz = p1[2] - p5[2]
# dx = p1[0] - p5[0]
# angle = math.atan2(dz, dx)
angle_vec = p1 - p5
# norm vec along the x axis, points to the right in KITTI cam rect coordinate
origin_vec = np.array([1, 0, 0])
import vg
angle = vg.signed_angle(origin_vec,
angle_vec,
look=vg.basis.y,
units='rad')
# the orientation angle of the car
beta = math.atan2(center_cam_frame[0][2],
center_cam_frame[0][0])
alpha = angle + beta - math.pi / 2
line = classes + ' {} {} {} {} {} {} {} {} {} {} {} {} {} {} \n'.format(
round(truncated, 2), occulusion, round(alpha, 2),
round(image_bbox[0], 2), round(image_bbox[1], 2),
round(image_bbox[2], 2), round(image_bbox[3], 2),
round(height, 2), round(width, 2), round(
length, 2), round(center_cam_frame[0][0], 2),
round(center_cam_frame[0][1], 2) + height / 2,
round(center_cam_frame[0][2], 2), round(angle, 2))
# separate the object lines so we can move all the dontcare lines at the end
if classes == DONT_CARE:
dontcare_lines.append(line)
else:
object_lines.append(line)
for line in object_lines:
file.write(line)
for line in dontcare_lines:
file.write(line)
file.close()
# write kitti_to_argo_mapping to a file
split_dir = os.path.dirname(split_file_path)
split_name = os.path.basename(split_file_path)
split_name = os.path.splitext(split_name)[0]
prefix = 'kitti_to_argo_mapping'
kitti_to_argo_mapping_path = os.path.join(
split_dir, "{}_{}.json".format(prefix, split_name))
file_handle = open(kitti_to_argo_mapping_path, 'w')
json.dump(kitti_to_argo_mapping, file_handle)
file_handle.close()
bar.finish()
print('Translation finished, processed {} files'.format(i))
def main():
# Nombre de cycle nécessaire pour envoyer un message (1 msg/4 cycles)
PERIODICITY = 4
cycles_without_sending = 0 # variable indiquant le cycle actuel
# précompilation du regex, indication des motifs de mots à enlever
regex = re.compile(r"prepro.*")
last_angles = []
while True:
line = sys.stdin.readline() # Lis ligne dans le pipelin stdin
line = regex.sub(
"", line
) # enlève les motifs de mot prédeterminés de la string par substitution
# si on a plus d'entrée
if not line:
break # on sort de la boucle while (fin du programme)
# si on a que des espaces blanc
if line.isspace() or "[]" in line:
continue # on passe à la prochaine itération
givenArray = literal_eval(line)[0][
'coordinates'] # on extirpe seulement la liste les coordonnées de la string
if len(givenArray) < 3:
sys.stdout.write(repr(givenArray))
sys.stdout.flush()
continue
# Extrait les coordonnées des 3 joints du bras gauche
# a, b, c = np.array((givenArray[6],givenArray[8],givenArray[10]))
# Extrait les coordonnées des 3 joints du bras droit
a, b, c = np.array((givenArray[5], givenArray[7], givenArray[9]))
# Vectors
ab = b - a
bc = c - b
# Get angles on the 2D plane by projecting on it (z is the normal here)
# TODO: Change when dealing with 3d later
if np.any(ab != 0):
angle1 = vg.signed_angle(ab, np.array((1, 0, 0)), vg.basis.neg_z)
else:
angle1 = math.nan
if np.any(bc != 0):
angle2 = vg.signed_angle(bc, np.array((1, 0, 0)), vg.basis.neg_z)
else:
angle2 = math.nan
# met a jour la liste de tous les angles calculés depuis la dernière période
last_angles = update_liste_angles(last_angles, [angle1, angle2],
PERIODICITY)
# on incrémente le compteur de cycle sans message
cycles_without_sending += 1
# si le compteur de cycle correspond à n période
if cycles_without_sending % PERIODICITY == 0:
cycles_without_sending = 0 # on remet le compte à 0 pour ne pas dépasser la valeur max du type
# calcule les angles avec la moyenne mobile
angles_to_send = moyenne_mobile(last_angles)
output_str = str(angles_to_send[0]) + " " + str(
angles_to_send[1]) + "\n"
# envoi l'information au prochain dans std out
sys.stdout.write(output_str)
sys.stdout.flush(
) # force l'écriture en .vitant le buffering (pratique pour python2.7)
def test_stacked_basic():
v1 = np.array([[1, 1, 0], [1, 1, 0]])
v2 = np.array([[-1, 1, 0], [-1, -1, 0]])
look = vg.basis.z
np.testing.assert_array_almost_equal(vg.signed_angle(v2, v1, look),
np.array([-90, 180]))
def angle(v1, v2):
if v1.shape[0] == 2:
v1 = np.append(v1, 0)
if v2.shape[0] == 2:
v2 = np.append(v2, 0)
return vg.signed_angle(v1, v2, look=vg.basis.z, units='rad')
