def test_is_unit(self):
q1 = Quaternion()
q2 = Quaternion(1.0, 0, 0, 0.0001)
self.assertTrue(q1.is_unit())
self.assertFalse(q2.is_unit())
self.assertTrue(q2.is_unit(0.001))
def pointcloud_color_from_image(nusc: NuScenes, pointsensor_token: str,
camera_token: str):
"""
Given a point sensor (lidar/radar) token and camera sample_data token, load point-cloud and map it to the image
plane, then retrieve the colors of the closest image pixels.
:param nusc: NuScenes instance.
:param pointsensor_token: Lidar/radar sample_data token.
:param camera_token: Camera sample data token.
:return (coloring <np.float: 3, n>, mask <np.bool: m>). Returns the colors for n points that reproject into the
image out of m total points. The mask indicates which points are selected.
"""
cam = nusc.get('sample_data', camera_token)
pointsensor = nusc.get('sample_data', pointsensor_token)
pc = LidarPointCloud.from_file(
osp.join(nusc.dataroot, pointsensor['filename']))
im = Image.open(osp.join(nusc.dataroot, cam['filename']))
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the point-cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
pointsensor['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Second step: transform to the global frame.
poserecord = nusc.get('ego_pose', pointsensor['ego_pose_token'])
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
# Third step: transform into the ego vehicle frame for the timestamp of the image.
poserecord = nusc.get('ego_pose', cam['ego_pose_token'])
pc.translate(-np.array(poserecord['translation']))
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix.T)
# Fourth step: transform into the camera.
cs_record = nusc.get('calibrated_sensor', cam['calibrated_sensor_token'])
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
# Fifth step: actually take a "picture" of the point cloud.
# Grab the depths (camera frame z axis points away from the camera).
depths = pc.points[2, :]
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record['camera_intrinsic']),
normalize=True)
# Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
mask = np.ones(depths.shape[0], dtype=bool)
mask = np.logical_and(mask, depths > 0)
mask = np.logical_and(mask, points[0, :] > 1)
mask = np.logical_and(mask, points[0, :] < im.size[0] - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < im.size[1] - 1)
points = points[:, mask]
# Pick the colors of the points
im_data = np.array(im)
coloring = np.zeros(points.shape)
for i, p in enumerate(points.transpose()):
point = p[:2].round().astype(np.int32)
coloring[:, i] = im_data[point[1], point[0], :]
return coloring, mask
def generate_propagation_parameters(self):
'''
---------------------------------------------
generate_propagation_parameters()
---------------------------------------------
This method finds the parameters to pass to the opencl kernel to correctly
define the sampling grid.
'''
# Find the unit normal vector to the plane.
unit_normal = (1 /
np.linalg.norm(self.normal_vector)) * self.normal_vector
# Define the quaternion required to rotate the parallel vector by an angle of pi/2.
quaternion = Quaternion(
axis=[unit_normal[0], unit_normal[1], unit_normal[2]],
angle=tau / 4)
# Rotate the parallel vector to get a new vector which is perpendicular to the normal and the parallel vector.
self.vector_2 = quaternion.rotate(self.parallel_vector)
# Find the norm of these vectors.
parallel_vector_norm = np.linalg.norm(self.parallel_vector)
vector_2_norm = np.linalg.norm(self.vector_2)
# Find the unit vectors.
unit_parallel_vector = (1 /
parallel_vector_norm) * self.parallel_vector
unit_vector_2 = (1 / vector_2_norm) * self.vector_2
# Compute the angle between these vectors to check that is equal to pi/2.
angle_1 = int(
np.round(
np.rad2deg(
np.arccos(np.dot(unit_parallel_vector, unit_vector_2))),
1))
# Compute the angle between each of these vectors and the normal to ensure that they parameterise the plane perpendicular to the nromal.
angle_2 = int(
np.round(
np.rad2deg(
np.arccos(
np.dot(self.parallel_vector, self.normal_vector) /
(parallel_vector_norm * vector_2_norm))), 1))
angle_3 = int(
np.round(
np.rad2deg(
np.arccos(
np.dot(self.vector_2, self.normal_vector) /
(parallel_vector_norm * vector_2_norm))), 1))
# Throw error if they are not.
if np.bitwise_or(angle_1 != 90,
np.bitwise_or(int(angle_2) != 90,
int(angle_3) != 90)):
raise Exception(
'Could not parameterise the requested sampling grid correctly!'
)
self.x_lim = np.float32(self.N_x / 2)
self.y_lim = np.float32(self.N_y / 2)
self.vx1 = np.float32(self.parallel_vector[0])
self.vy1 = np.float32(self.parallel_vector[1])
self.vz1 = np.float32(self.parallel_vector[2])
self.x0 = np.float32(self.origin[0])
self.y0 = np.float32(self.origin[1])
self.z0 = np.float32(self.origin[2])
self.vx2 = np.float32(np.round(self.vector_2[0], 4))
self.vy2 = np.float32(np.round(self.vector_2[1], 4))
self.vz2 = np.float32(np.round(self.vector_2[2], 4))
return q.normalised
# setup 3D plot
fig = plt.figure('Quaternion attitude control')
ay = fig.add_subplot(111, projection='3d')
ay.set_xlim([-5, 1])
ay.set_ylim([-5, 1])
ay.set_zlim([-5, 1])
ay.set_xlabel('X')
ay.set_ylabel('Y')
ay.set_zlabel('Z')
# initialize state
steps = 0
q = qd = Quaternion() # atttitude state
v = np.array([0., 0., 0.]) # velocity state
p = np.array([0., 0., 0.]) # position state
# specify goal and parameters
pd = np.array([-5, 0, 0]) # desired position
yd = radians(180) # desired yaw
dt = 0.2 # time steps
# run simulation until the goal is reached
while steps < 1000 and (not np.isclose(p, pd).all() or
(q.inverse * qd).degrees > 0.1):
# plot current vehicle body state abstraction
plotrotc(q, p)
# run minimal position & velocity control
def load_calib_data(path_total_dataset, name_camera_calib, tf_tree):
"""
:param path_total_dataset: Path to dataset root dir
:param name_camera_calib: Camera calib file containing image intrinsic
:param tf_tree: TF (transformation) tree containing translations from velodyne to cameras
:return:
"""
with open(os.path.join(path_total_dataset, name_camera_calib), 'r') as f:
data_camera = json.load(f)
with open(os.path.join(path_total_dataset, tf_tree), 'r') as f:
data_extrinsics = json.load(f)
calib_dict = {
'calib_cam_stereo_left.json': 'cam_stereo_left_optical',
'calib_cam_stereo_right.json': 'cam_stereo_right_optical',
'calib_gated_bwv.json': 'bwv_cam_optical'
}
cam_name = calib_dict[name_camera_calib]
# Scan data extrinsics for transformation from lidar to camera
important_translations = ['lidar_hdl64_s3_roof', 'radar_ars300', cam_name]
translations = []
for item in data_extrinsics:
if item['child_frame_id'] in important_translations:
translations.append(item)
if item['child_frame_id'] == cam_name:
T_cam = item['transform']
elif item['child_frame_id'] == 'lidar_hdl64_s3_roof':
T_velodyne = item['transform']
elif item['child_frame_id'] == 'radar_ars300':
T_radar = item['transform']
# Use pyquaternion to setup rotation matrices properly
R_c_quaternion = Quaternion(w=T_cam['rotation']['w'] * 360 / 2 / np.pi,
x=T_cam['rotation']['x'] * 360 / 2 / np.pi,
y=T_cam['rotation']['y'] * 360 / 2 / np.pi,
z=T_cam['rotation']['z'] * 360 / 2 / np.pi)
R_v_quaternion = Quaternion(
w=T_velodyne['rotation']['w'] * 360 / 2 / np.pi,
x=T_velodyne['rotation']['x'] * 360 / 2 / np.pi,
y=T_velodyne['rotation']['y'] * 360 / 2 / np.pi,
z=T_velodyne['rotation']['z'] * 360 / 2 / np.pi)
# Setup quaternion values as 3x3 orthogonal rotation matrices
R_c_matrix = R_c_quaternion.rotation_matrix
R_v_matrix = R_v_quaternion.rotation_matrix
# Setup translation Vectors
Tr_cam = np.asarray([
T_cam['translation']['x'], T_cam['translation']['y'],
T_cam['translation']['z']
])
Tr_velodyne = np.asarray([
T_velodyne['translation']['x'], T_velodyne['translation']['y'],
T_velodyne['translation']['z']
])
Tr_radar = np.asarray([
T_radar['translation']['x'], T_radar['translation']['y'],
T_radar['translation']['z']
])
# Setup Translation Matrix camera to lidar -> ROS spans transformation from its children to its parents
# Therefore one inversion step is needed for zero_to_camera -> <parent_child>
zero_to_camera = np.zeros((3, 4))
zero_to_camera[0:3, 0:3] = R_c_matrix
zero_to_camera[0:3, 3] = Tr_cam
zero_to_camera = np.vstack((zero_to_camera, np.array([0, 0, 0, 1])))
zero_to_velodyne = np.zeros((3, 4))
zero_to_velodyne[0:3, 0:3] = R_v_matrix
zero_to_velodyne[0:3, 3] = Tr_velodyne
zero_to_velodyne = np.vstack((zero_to_velodyne, np.array([0, 0, 0, 1])))
zero_to_radar = zero_to_velodyne.copy()
zero_to_radar[0:3, 3] = Tr_radar
# Calculate total extrinsic transformation to camera
velodyne_to_camera = np.matmul(np.linalg.inv(zero_to_camera),
zero_to_velodyne)
camera_to_velodyne = np.matmul(np.linalg.inv(zero_to_velodyne),
zero_to_camera)
radar_to_camera = np.matmul(np.linalg.inv(zero_to_camera), zero_to_radar)
# Read projection matrix P and camera rectification matrix R
P = np.reshape(data_camera['P'], [3, 4])
# In our case rectification matrix R has to be equal to the identity as the projection matrix P contains the
# R matrix w.r.t KITTI definition
R = np.identity(4)
# Calculate total transformation matrix from velodyne to camera
vtc = np.matmul(np.matmul(P, R), velodyne_to_camera)
return velodyne_to_camera, camera_to_velodyne, P, R, vtc, radar_to_camera, zero_to_camera
CAM_EGO = CAM_SAMPLE['ego_pose_token']
EGO = nusc.get('ego_pose', token=CAM_EGO)
CAM_SAMPLE_next_token = first_token_meta['next']
next_sample = nusc.get('sample', token=CAM_SAMPLE_next_token)
CAM_SAMPLE_next = nusc.get('sample_data', token=next_sample['data'][sensor])
CAM_EGO_next = CAM_SAMPLE_next['ego_pose_token']
EGO_next = nusc.get('ego_pose', token=CAM_EGO_next)
current_to_next = get_relative_pose(EGO, EGO_next)
H_pose1 = np.zeros((4, 4))
H_pose2 = np.zeros((4, 4))
H_pose1[3, 3] = 1
H_pose2[3, 3] = 1
pose1_rot = Quaternion(EGO['rotation']).rotation_matrix
pose1_tran = EGO['translation']
H_pose1[0:3, 0:3] = pose1_rot
H_pose1[0:3, 3] = pose1_tran
pose2_rot = Quaternion(EGO_next['rotation']).rotation_matrix
pose2_tran = EGO_next['translation']
H_pose2[0:3, 0:3] = pose2_rot
H_pose2[0:3, 3] = pose2_tran
H2_recovered = np.dot(H_pose1, current_to_next)
pose1_rot = Quaternion(pose1['rotation']).rotation_matrix
pose1_tran = pose1['translation']
H_pose1[0:3, 0:3] = pose1_rot
H_pose1[:, 3] = pose1_tran
loc = Localization(name, config['model'], config, build_db=args.build_db)
query_file = f'{args.slice}.queries_with_intrinsics.txt'
queries, query_dataset = loc.init_queries(query_file, config['cmu'])
logging.info('Starting evaluation')
metrics, results = evaluate(loc,
queries,
query_dataset,
max_iter=args.max_iter)
logging.info('Evaluation metrics: \n' + pformat(metrics))
output = {'config': config, 'metrics': metrics}
output_dir = Path(EXPER_PATH, 'eval/cmu')
output_dir.mkdir(exist_ok=True, parents=True)
eval_filename = f'{args.eval_name}_{args.slice}'
eval_path = Path(output_dir, f'{eval_filename}.yaml')
with open(eval_path, 'w') as f:
yaml.dump(output, f, default_flow_style=False)
if args.export_poses:
poses_path = Path(output_dir, f'{eval_filename}_poses.txt')
with open(poses_path, 'w') as f:
for query, result in zip(queries, results):
query_T_w = np.linalg.inv(result.T)
qvec_nvm = list(Quaternion(matrix=query_T_w))
pos_nvm = query_T_w[:3, 3].tolist()
name = '/'.join(query.name.split('/')[-2:])
line = name + ' ' + ' '.join(map(str, qvec_nvm + pos_nvm))
f.write(line + '\n')
def test_sym_distance(self):
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=0, vector=[0,1,0])
self.assertEqual(pi/2, Quaternion.sym_distance(q,p))
q = Quaternion(angle=pi/2, axis=[1,0,0])
p = Quaternion(angle=pi/2, axis=[0,1,0])
self.assertAlmostEqual(pi/3, Quaternion.sym_distance(q,p), places=6)
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=0, vector=[0,-1,0])
self.assertEqual(pi/2, Quaternion.sym_distance(q,p))
q = Quaternion(scalar=1, vector=[1,1,1])
p = Quaternion(scalar=-1, vector=[-1,-1,-1])
p._normalise()
q._normalise()
self.assertAlmostEqual(pi, Quaternion.sym_distance(q,p), places=8)
def test_slerp(self):
q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
q2 = Quaternion(axis=[1, 0, 0], angle=pi/2)
q3 = Quaternion.slerp(q1, q2, 0.5)
self.assertEqual(q3, Quaternion(axis=[1,0,0], angle=pi/4))
def test_log(self):
from math import log
q = Quaternion(axis=[1,0,0], angle=pi)
log_q = Quaternion.log(q)
self.assertEqual(log_q, Quaternion(scalar=0, vector=[pi/2,0,0]))
def test_absolute_distance(self):
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=0, vector=[0,1,0])
self.assertEqual((q-p).norm, Quaternion.absolute_distance(q,p))
q = Quaternion(angle=pi/2, axis=[1,0,0])
p = Quaternion(angle=pi/2, axis=[0,1,0])
self.assertEqual((q-p).norm, Quaternion.absolute_distance(q,p))
q = Quaternion(scalar=0, vector=[1,0,0])
p = Quaternion(scalar=-1, vector=[0,-1,0])
self.assertEqual((q+p).norm, Quaternion.absolute_distance(q,p))
q = Quaternion(scalar=1, vector=[1,1,1])
p = Quaternion(scalar=-1, vector=[-1,-1,-1])
p._normalise()
q._normalise()
self.assertAlmostEqual(0, Quaternion.absolute_distance(q,p), places=8)
def test_exp(self):
from math import exp
q = Quaternion(axis=[1,0,0], angle=pi)
exp_q = Quaternion.exp(q)
self.assertEqual(exp_q, exp(0) * Quaternion(scalar=cos(1.0), vector=[sin(1.0), 0,0]))
def test_rotate(self):
q = Quaternion(axis=[1,1,1], angle=2*pi/3)
q2 = Quaternion(axis=[1, 0, 0], angle=-pi)
q3 = Quaternion(axis=[1, 0, 0], angle=pi)
precision = ALMOST_EQUAL_TOLERANCE
for r in [1, 3.8976, -69.7, -0.000001]:
# use np.testing.assert_almost_equal() to compare float sequences
np.testing.assert_almost_equal(q.rotate((r, 0, 0)), (0, r, 0), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q.rotate([0, r, 0]), [0, 0, r], decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q.rotate(np.array([0, 0, r])), np.array([r, 0, 0]), decimal=ALMOST_EQUAL_TOLERANCE)
self.assertEqual(q.rotate(Quaternion(vector=[-r, 0, 0])), Quaternion(vector=[0, -r, 0]))
np.testing.assert_almost_equal(q.rotate([0, -r, 0]), [0, 0, -r], decimal=ALMOST_EQUAL_TOLERANCE)
self.assertEqual(q.rotate(Quaternion(vector=[0, 0, -r])), Quaternion(vector=[-r, 0, 0]))
np.testing.assert_almost_equal(q2.rotate((r, 0, 0)), q3.rotate((r, 0, 0)), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q2.rotate((0, r, 0)), q3.rotate((0, r, 0)), decimal=ALMOST_EQUAL_TOLERANCE)
np.testing.assert_almost_equal(q2.rotate((0, 0, r)), q3.rotate((0, 0, r)), decimal=ALMOST_EQUAL_TOLERANCE)
def test_output_of_elements(self):
r = randomElements()
q = Quaternion(*r)
self.assertEqual(tuple(q.elements), r)
def record(client, start_pos, end_pos, start_quat, end_quat, N):
# generate positions
pos_diff = end_pos - start_pos
pos_diff = np.repeat(np.reshape(pos_diff, [1, 3]), N, 0)
inc = np.reshape(np.linspace(0, 1, num=N), [N, 1])
pos_diff *= inc
start_pos = np.repeat(np.reshape(start_pos, [1, 3]), N, 0)
pos = start_pos + pos_diff
# generate orientations
qs = Quaternion.intermediates(start_quat,
end_quat,
N,
include_endpoints=False)
# combine
for i, q in enumerate(qs):
print(pos[i, :], q.rotation_matrix)
responses = get_image_and_depth(client, pos[i, :], q)
for j, response in enumerate(responses):
if response.pixels_as_float:
airsim.write_pfm(
os.path.normpath('depth{:07d}.pfm'.format(i + 1)),
airsim.get_pfm_array(response))
else:
airsim.write_file(
os.path.normpath('image{:07d}.png'.format(i + 1)),
response.image_data_uint8)
R = q.rotation_matrix
rx = R[1, :]
ry = -R[2, :]
rz = -R[0, :]
def rotm(rx, ry, rz):
return np.array([-rz, rx, -ry])
savemat("pose{:07d}.mat".format(i + 1), {'C': pos[i, :], 'R': R})
thetas = [-20, -10, 10, 20]
for theta in thetas:
th = np.radians(theta)
R = rotm(rx * cos(th) + ry * sin(th), -rx * sin(th) + ry * cos(th),
rz)
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_rotz{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_rotz{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(-rz * sin(th) + rx * cos(th), ry,
rz * cos(th) + rx * sin(th))
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_roty{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_roty{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
R = rotm(rx,
ry * cos(th) + rz * sin(th), -ry * sin(th) + rz * cos(th))
response = get_image(client, pos[i, :], Quaternion(matrix=R))
airsim.write_file(
os.path.normpath('image{:07d}_rotx{}.png'.format(i + 1,
theta)),
response.image_data_uint8)
savemat("pose{:07d}_rotx{}.mat".format(i + 1, theta), {
'C': pos[i, :],
'R': R
})
def extract_db(config, dir_meta, cameras):
"""
:param config: config from config.yaml
:param dir_meta: determine extraction of train or test
:param cameras:
:return:
"""
dataset = [] # all images of train or test
all_joints = dt.vicon_joints
for dir, meta in dir_meta: # one action contains 8 cam views
meta_sub = meta['subject']
meta_act = config['action_reverse_map'][
meta['action']] # action string name
meta_subact = meta['subaction']
gt_pos_path = os.path.join(dir, 'gt_skel_gbl_pos.txt')
gt_ori_path = os.path.join(dir, 'gt_skel_gbl_ori.txt')
calib_imu_bone_path = os.path.join(
dir, 's{}_{}{}_calib_imu_bone.txt'.format(meta_sub, meta_act,
meta_subact))
calib_imu_ref_path = os.path.join(
dir, 's{}_{}{}_calib_imu_ref.txt'.format(meta_sub, meta_act,
meta_subact))
imu_data_path = os.path.join(
dir, 's{}_{}{}_Xsens.sensors'.format(meta_sub, meta_act,
meta_subact))
bvh_path = os.path.join(
dir, '{}{}_BlenderZXY_YmZ.bvh'.format(meta_act, meta_subact))
gt_pos = dt.parse_vicon_gt_pos(gt_pos_path)
gt_ori = dt.parse_vicon_gt_ori(gt_ori_path)
imu_data = dt.parse_sensor_6axis(imu_data_path)
calib_imu_bone = dt.parse_calib_imu_bone(calib_imu_bone_path)
calib_imu_ref = dt.parse_calib_imu_ref(calib_imu_ref_path)
bone_info = dt.parse_imu_bone_info(bvh_path)
canvas_size = (1079., 1919.) # height width
filtered_joints = config['joints_filter']
# bone vector / orientation, not camera related
bones = [
'Head', 'Sternum', 'Pelvis', 'L_UpArm', 'R_UpArm', 'L_LowArm',
'R_LowArm', 'L_UpLeg', 'R_UpLeg', 'L_LowLeg', 'R_LowLeg'
]
# obtain ref for all bones
bone_refs = dict()
for bone in bones:
joint_p = bone_info[bone][0]
joint_c = bone_info[bone][1]
bone_vec = np.array(bone_info[bone][2]) * 25.4
q_TI = calib_imu_ref[bone]
q_bi = calib_imu_bone[bone]
q_TI = Quaternion(q_TI)
q_bi = Quaternion(q_bi)
q_ib = q_bi.conjugate
bone_refs[bone] = {
'joint_p': joint_p,
'joint_c': joint_c,
'bone_vec': bone_vec,
'q_TI': q_TI,
'q_ib': q_ib
}
bone_vectors = dict() # of all frames
for c in range(8):
mp4_file_name = 'TC_S{}_{}{}_cam{}.mp4'.format(
meta_sub, meta_act, meta_subact, c + 1)
mp4_file_path = os.path.join(dir, mp4_file_name)
cam = cameras[c]
vid_info = ffprobe(mp4_file_path)
vid_frame_num = int(vid_info['video']['@nb_frames'])
# print(mp4_file_name, vid_info['video']['@nb_frames'], len(gt_pos)- int(vid_info['video']['@nb_frames']),
# vid_info['video']['@bit_rate'])
out_path = os.path.join(config['db_out_dir'], 'marked')
out_path = os.path.join(
out_path, 'sub{}_{}_{}_cam{}'.format(meta_sub, meta_act,
meta_subact, c + 1))
if config['save_visualization']:
if not os.path.exists(out_path):
os.makedirs(out_path)
# where to save extract frames
seq_dir_name = 's_{:0>2}_act_{:0>2}_subact_{:0>2}_ca_{:0>2}'.format(
meta_sub, meta['action'], meta_subact, c + 1)
seq_dir_path = os.path.join(config['db_out_dir'], seq_dir_name)
if config['save_frame']:
if not os.path.exists(seq_dir_path):
os.makedirs(seq_dir_path)
vid_ff = vreader(mp4_file_path)
min_frame_to_iter = min(vid_frame_num, len(gt_pos), len(gt_ori),
len(imu_data))
for idx in tqdm(range(min_frame_to_iter)):
pose3d = np.zeros([3, len(all_joints)])
for idx_j, j in enumerate(all_joints):
pose3d[:, idx_j] = gt_pos[idx][j]
pose3d = pose3d * 0.0254 # inch to meter
pose2d = project_pose3d_to_2d(pose3d, cam, do_distor_corr=True)
if config['save_visualization'] or config['save_frame']:
aframe = next(vid_ff)
if config[
'save_visualization']: # skeleton visualization save to disk
out_file_path = os.path.join(out_path,
'{:0>6d}.jpg'.format(idx))
marked_img = _visualize_one_frame(
aframe, pose2d) # todo vis box on image
img_4save = cv2.cvtColor(marked_img, cv2.COLOR_RGB2BGR)
cv2.imwrite(out_file_path, img_4save)
# cropping box information
p2d, p3d_cam, p3d, vis = filter_and_project_2d_pose(
gt_pos[idx],
filtered_joints,
cam,
canvas_size,
do_distor_corr=True)
mvpose_vis = np.reshape([vis / 2., vis / 2., vis / 2.],
(3, -1))
# vis follow coco protocol, divide 2 and copy 3 times to follow mvpose
root_joint = project_pose3d_to_cam(
np.reshape(gt_pos[idx]['Hips'], (3, -1)) * 0.0254, cam)
tl_joint = np.copy(root_joint) # shape (3,1)
br_joint = np.copy(root_joint)
tl_joint[0, 0] -= 1.0000
tl_joint[1, 0] -= 0.9000
br_joint[0, 0] += 1.0000
br_joint[1, 0] += 1.1000
bbox_25d = np.concatenate((root_joint, tl_joint, br_joint),
axis=1)
bbox = project_cam_to_uv(
bbox_25d, cam,
do_distor_corr=True) # contain 3 point: center, tl, br
box_center = tuple(bbox[:, 0]) # (x, y)
box_scale = tuple((bbox[:, 2] - bbox[:, 1]) / 200.)
box = tuple(np.concatenate([bbox[:, 2], bbox[:, 1]
])) # (x_tl, y_tl, x_br, y_br)
frame_file_name = '{:0>6d}.jpg'.format(idx)
frame_file_path = os.path.join(seq_dir_path, frame_file_name)
if config['save_frame']: # save video frame to disk
frame_to_cv = cv2.cvtColor(aframe, cv2.COLOR_RGB2BGR)
cv2.imwrite(frame_file_path, frame_to_cv)
# notice: Difference between totalcapture and h36m project,
# (1) joints_3d in mm
# (2) camera['T'] in mm
# (3) in totalcapture: point_Camera = R.dot(point_Tracking) + T (point and T in m);
# in h36m: point_Camera = R.dot(point_Tracking - T) (point and T in mm)
# aka in h36m: point_Tracking = R^{-1}.dot(point_Camera) + T
# (4) coordinates shape is (num_cords, 3), aka row vector, but I like col vector more
cam_in_h36m_format = copy.deepcopy(cam)
cam_in_h36m_format['R'] = cam_in_h36m_format['R']
cam_in_h36m_format['T'] = cam_in_h36m_format['T'] * 1000. * (
-1.)
cam_in_h36m_format['T'] = cam_in_h36m_format['R'].T.dot(
cam_in_h36m_format['T'])
del cam_in_h36m_format['intri_mat']
del cam_in_h36m_format['extri_mat']
# bone vector
# avoid parsing in each view, only in first view
if idx not in bone_vectors:
bone_vector_of_one_frame = dict()
for bone in bones:
q_TI = bone_refs[bone]['q_TI']
q_ib = bone_refs[bone]['q_ib']
bone_vec = bone_refs[bone]['bone_vec']
ori = imu_data[idx][bone][0]
q_Ii = Quaternion(ori)
q_Tb = q_TI * q_Ii * q_ib
rotated_bone_vec = q_Tb.rotate(bone_vec)
bone_vector_of_one_frame[bone] = rotated_bone_vec
bone_vectors[idx] = bone_vector_of_one_frame
dataitem = {
'image': os.path.join(seq_dir_name,
'{:0>6d}.jpg'.format(idx)),
'joints_2d': p2d.T,
'joints_3d':
(p3d_cam *
1000.).T, # 3d pose in camera frame, for psm evaluation
'joints_vis': mvpose_vis.T, # 0: in-visible, 1: visible.
'center': box_center,
'scale': box_scale,
'box': box,
'video_id': mp4_file_name, # mp4 file name # todo
'image_id': idx,
'subject': meta['subject'],
'action': meta['action'],
'subaction': meta['subaction'],
'camera_id': c, # start from 0
'camera': cam_in_h36m_format,
'source': 'totalcapture',
'bone_vec': bone_vectors[idx],
'joints_gt': p3d.T * 1000. # groundtruth in tracking frame
}
dataset.append(dataitem)
return dataset
import time import math import numpy as np from pyquaternion import Quaternion from isecc import transform from isecc import starparse from isecc import symops from isecc import checks from isecc import utils from isecc.isecc_classes import Particle I1Quaternions = symops.getSymOps() my_vector = np.array( [0, 0.618, 1] ) my_arctan = np.arctan( np.true_divide( my_vector[1], my_vector[2] ) ) quaternion_toZ = Quaternion(axis=(1.0,0.0,0.0), radians=my_arctan) my_particle = Particle( 'dummy.mrc', 0.256, -0.432, 58.46, 126.2, -70.5 ) my_particle.add_defocus_info( 32350.43, 30611.52, 13.18, 0 ) print( my_particle ) my_particle.rotateParticle(I1Quaternions[0], quaternion_toZ) print( vars(my_particle) ) my_particle.defineSubparticle(my_vector) print( vars(my_particle) )
def yaw2quaternion(yaw: float) -> Quaternion:
"""
"""
return Quaternion(axis=[0, 0, 1], radians=yaw)
def map_pointcloud_to_image(nusc,
pointsensor_token: str,
camera_token: str,
min_dist: float = 1.0,
render_intensity: bool = False,
show_lidarseg: bool = False):
"""
Given a point sensor (lidar/radar) token and camera sample_data token, load pointcloud and map it to the image
plane.
:param pointsensor_token: Lidar/radar sample_data token.
:param camera_token: Camera sample_data token.
:param min_dist: Distance from the camera below which points are discarded.
:param render_intensity: Whether to render lidar intensity instead of point depth.
:param show_lidarseg: Whether to render lidar intensity instead of point depth.
:param filter_lidarseg_labels: Only show lidar points which belong to the given list of classes. If None
or the list is empty, all classes will be displayed.
:param lidarseg_preds_bin_path: A path to the .bin file which contains the user's lidar segmentation
predictions for the sample.
:return (pointcloud <np.float: 2, n)>, coloring <np.float: n>, image <Image>).
"""
cam = nusc.get('sample_data', camera_token)
pointsensor = nusc.get('sample_data', pointsensor_token)
pcl_path = osp.join(nusc.dataroot, pointsensor['filename'])
if pointsensor['sensor_modality'] == 'lidar':
# Ensure that lidar pointcloud is from a keyframe.
assert pointsensor['is_key_frame'], \
'Error: Only pointclouds which are keyframes have lidar segmentation labels. Rendering aborted.'
pc = LidarPointCloud.from_file(pcl_path)
else:
pc = RadarPointCloud.from_file(pcl_path)
im = Image.open(osp.join(nusc.dataroot, cam['filename']))
# Points live in the point sensor frame. So they need to be transformed via global to the image plane.
# First step: transform the pointcloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
pointsensor['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Second step: transform from ego to the global frame.
poserecord = nusc.get('ego_pose', pointsensor['ego_pose_token'])
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
# Third step: transform from global into the ego vehicle frame for the timestamp of the image.
poserecord = nusc.get('ego_pose', cam['ego_pose_token'])
pc.translate(-np.array(poserecord['translation']))
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix.T)
# Fourth step: transform from ego into the camera.
cs_record = nusc.get('calibrated_sensor', cam['calibrated_sensor_token'])
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
# Fifth step: actually take a "picture" of the point cloud.
# Grab the depths (camera frame z axis points away from the camera).
depths = pc.points[2, :]
if render_intensity:
assert pointsensor['sensor_modality'] == 'lidar', 'Error: Can only render intensity for lidar, ' \
'not %s!' % pointsensor['sensor_modality']
# Retrieve the color from the intensities.
# Performs arbitary scaling to achieve more visually pleasing results.
intensities = pc.points[3, :]
intensities = (intensities - np.min(intensities)) / (
np.max(intensities) - np.min(intensities))
intensities = intensities**0.1
intensities = np.maximum(0, intensities - 0.5)
coloring = intensities
else:
# Retrieve the color from the depth.
coloring = depths
# Take the actual picture (matrix multiplication with camera-matrix + renormalization).
points = view_points(pc.points[:3, :],
np.array(cs_record['camera_intrinsic']),
normalize=True)
# Remove points that are either outside or behind the camera. Leave a margin of 1 pixel for aesthetic reasons.
# Also make sure points are at least 1m in front of the camera to avoid seeing the lidar points on the camera
# casing for non-keyframes which are slightly out of sync.
mask = np.ones(depths.shape[0], dtype=bool)
mask = np.logical_and(mask, depths > min_dist)
mask = np.logical_and(mask, points[0, :] > 1)
mask = np.logical_and(mask, points[0, :] < im.size[0] - 1)
mask = np.logical_and(mask, points[1, :] > 1)
mask = np.logical_and(mask, points[1, :] < im.size[1] - 1)
points = points[:, mask]
coloring = coloring[mask]
return points, coloring, im
def convert_track(args):
tracking_result_path = os.path.join(args.work_dir, 'output.json')
print(f'Loading tracking result from {tracking_result_path}')
with open(tracking_result_path, 'rb') as f:
tracking_result = json.load(f)
with open(args.gt_anns, 'rb') as f:
gt = json.load(f)
nusc = NuScenes(version=f'{args.version}',
dataroot=f'{args.root}', verbose=True)
if args.version == 'v1.0-mini':
start_seq_id = seq_map['mini_val_seq_id']
vid_num = 2
elif args.version == 'v1.0-trainval':
start_seq_id = seq_map['val_seq_id']
vid_num = 150
else:
start_seq_id = seq_map['test_seq_id']
vid_num = 150
for sensor in USED_SENSOR:
# grep scene token
for vid_info in tracking_result['videos']:
for gt_vid_info in gt['videos']:
if f"{int(vid_info['id']) - vid_num*(SENSOR_ID[sensor]-1) + start_seq_id:05}_{SENSOR_ID[sensor]}" == gt_vid_info['name']:
vid_info['scene_token'] = gt_vid_info['scene_token']
vid_info['sensor'] = sensor
del gt
nusc_annos = {
"results": {},
"meta": None,
}
first_frame_id = -1
memo = []
first_frame_token = []
print("Begin Converting Tracking Result")
for i in tqdm(range(len(tracking_result['videos']))):
sensor = tracking_result['videos'][i]['sensor']
scene = nusc.get('scene', tracking_result['videos'][i]['scene_token'])
# find start frame for each video
for frame in tracking_result['images']:
if frame['first_frame'] and not frame['id'] in memo:
first_frame_id = frame['id']
memo.append(frame['id'])
break
# First sample (key frame)
token = scene['first_sample_token']
sample = nusc.get('sample', token)
first_frame_token.append(token)
for sample_id in range(scene['nbr_samples']):
annos = []
img_id = first_frame_id + sample_id
pose_dict = tracking_result['images'][img_id]['pose']
projection = tracking_result['images'][img_id]['cali']
cam_to_global = R.from_euler(
'xyz', pose_dict['rotation']).as_matrix()
outputs = [ann for ann in tracking_result['annotations'] if ann['image_id'] == img_id]
for output in outputs:
# remove non-tracking objects
cat = ''
for key in model_cats:
if cats_mapping[output['category_id']] == key:
cat = key.lower()
break
if cat == '':
continue
# move to world coordinate
translation = np.dot(
cam_to_global, np.array(output['translation']))
translation += np.array(pose_dict['position'])
quat = Quaternion(axis=[0, 1, 0], radians=output['roty'])
x, y, z, w = R.from_matrix(cam_to_global).as_quat()
rotation = Quaternion([w, x, y, z]) * quat
tracking_id = str(output['instance_id'])
h, w, l = output['dimension']
nusc_anno = {
"sample_token": token,
"translation": translation.tolist(),
"size": [w, l, h],
"rotation": rotation.elements.tolist(),
"velocity": [0., 0.],
"tracking_id": tracking_id,
"tracking_name": cat,
"tracking_score": output['score'] * output['uncertainty'],
}
# align six camera
if nusc_annos["results"].get(token) is not None:
nms_flag = 0
for all_cam_ann in nusc_annos["results"][token]:
if nusc_anno['tracking_name'] == all_cam_ann['tracking_name']:
translation = nusc_anno['translation']
ref_translation = all_cam_ann['translation']
translation_diff = (translation[0] - ref_translation[0],
translation[1] - ref_translation[1],
translation[2] - ref_translation[2])
if nusc_anno['tracking_name'] in ['pedestrian']:
nms_dist = 1
else:
nms_dist = 2
if np.sqrt(np.sum(np.array(translation_diff[:2]) ** 2)) < nms_dist:
if all_cam_ann['tracking_score'] < nusc_anno['tracking_score']:
all_cam_ann = nusc_anno
nms_flag = 1
break
if nms_flag == 0:
annos.append(nusc_anno)
else:
annos.append(nusc_anno)
if nusc_annos["results"].get(token) is not None:
annos += nusc_annos["results"][token]
nusc_annos["results"].update({token: annos})
token = sample['next']
if token != '':
sample = nusc.get('sample', token)
nusc_annos["meta"] = {
"use_camera": True,
"use_lidar": False,
"use_radar": False,
"use_map": False,
"use_external": False,
}
with open(os.path.join(args.work_dir, 'tracking_result.json'), "w") as f:
json.dump(nusc_annos, f)
return nusc
dtheda_xh = phierr * d2r dtheda_yh = thetaerr * d2r dtheda_zh = psierr * d2r # initial gyro bias predict data bgx_h = 0 bgy_h = 0 bgz_h = 0 # initial quaternion dq11 = -dtheda_xh / 2 dq21 = -dtheda_yh / 2 dq31 = -dtheda_zh / 2 q2 = -dq11 q3 = -dq21 q4 = -dq31 q1 = np.sqrt(1 - np.square(q2) - np.square(q3) - np.square(q4)) dQerr = Quaternion(q1, q2, q3, q4) Q_E_B = Quaternion(1, 0, 0, 0) QE_B_m = dQerr.normalised * Q_E_B.normalised # initial state matrix bgx0 = gyro_bias_flag * 0.05 * d2r bgy0 = gyro_bias_flag * (-0.05) * d2r bgz0 = gyro_bias_flag * 0.05 * d2r s6_xz_h = np.zeros([6, 1]) s6_xz_h[0, 0] = phierr * d2r s6_xz_h[1, 0] = thetaerr * d2r s6_xz_h[2, 0] = psierr * d2r s6_xz_h[3, 0] = bgx0 s6_xz_h[4, 0] = bgy0 s6_xz_h[5, 0] = bgz0 # initial P matrix
def process_scene(sample_token, processed_sample_tokens, env, nusc, helper,
data_path, data_class, half_dt, dynamic):
data = pd.DataFrame(columns=[
'frame_id', 'type', 'node_id', 'robot', 'x', 'y', 'z', 'length',
'width', 'height', 'heading'
])
samples = aggregate_samples(nusc,
start=sample_token,
data_class=data_class)
attribute_dict = defaultdict(set)
frame_id = 0
for sample in samples:
annotation_tokens = sample['anns']
for annotation_token in annotation_tokens:
annotation = nusc.get('sample_annotation', annotation_token)
category = annotation['category_name']
if len(annotation['attribute_tokens']):
attribute = nusc.get('attribute',
annotation['attribute_tokens'][0])['name']
else:
if 'vehicle' in category:
attribute = ''
else:
continue
if 'pedestrian' in category and 'stroller' not in category and 'wheelchair' not in category:
our_category = env.NodeType.PEDESTRIAN
elif 'vehicle' in category and 'bicycle' not in category and 'motorcycle' not in category: # and 'parked' not in attribute:
our_category = env.NodeType.VEHICLE
else:
continue
attribute_dict[annotation['instance_token']].add(attribute)
data_point = pd.Series({
'frame_id':
frame_id,
'type':
our_category,
'node_id':
annotation['instance_token'],
'robot':
False,
'x':
annotation['translation'][0],
'y':
annotation['translation'][1],
'z':
annotation['translation'][2],
'length':
annotation['size'][0],
'width':
annotation['size'][1],
'height':
annotation['size'][2],
'heading':
Quaternion(annotation['rotation']).yaw_pitch_roll[0]
})
data = data.append(data_point, ignore_index=True)
# Ego Vehicle
our_category = env.NodeType.VEHICLE
sample_data = nusc.get('sample_data', sample['data']['CAM_FRONT'])
annotation = nusc.get('ego_pose', sample_data['ego_pose_token'])
data_point = pd.Series({
'frame_id':
frame_id,
'type':
our_category,
'node_id':
'ego',
'robot':
True,
'x':
annotation['translation'][0],
'y':
annotation['translation'][1],
'z':
annotation['translation'][2],
'length':
4,
'width':
1.7,
'height':
1.5,
'heading':
Quaternion(annotation['rotation']).yaw_pitch_roll[0],
'orientation':
None
})
data = data.append(data_point, ignore_index=True)
processed_sample_tokens.add(sample['token'])
frame_id += 1
if len(data.index) == 0:
return None
data.sort_values('frame_id', inplace=True)
max_timesteps = data['frame_id'].max()
x_min = np.round(data['x'].min() - 50)
x_max = np.round(data['x'].max() + 50)
y_min = np.round(data['y'].min() - 50)
y_max = np.round(data['y'].max() + 50)
data['x'] = data['x'] - x_min
data['y'] = data['y'] - y_min
scene = Scene(timesteps=max_timesteps + 1,
dt=dt,
name=sample_token,
aug_func=augment,
sample_tokens=[sample['token'] for sample in samples],
x_min=x_min,
y_min=y_min)
# Generate Maps
map_name = helper.get_map_name_from_sample_token(sample_token)
nusc_map = NuScenesMap(dataroot=data_path, map_name=map_name)
type_map = dict()
x_size = x_max - x_min
y_size = y_max - y_min
patch_box = (x_min + 0.5 * (x_max - x_min), y_min + 0.5 * (y_max - y_min),
y_size, x_size)
patch_angle = 0 # Default orientation where North is up
canvas_size = (np.round(3 * y_size).astype(int),
np.round(3 * x_size).astype(int))
homography = np.array([[3., 0., 0.], [0., 3., 0.], [0., 0., 3.]])
layer_names = [
'lane', 'road_segment', 'drivable_area', 'road_divider',
'lane_divider', 'stop_line', 'ped_crossing', 'stop_line',
'ped_crossing', 'walkway'
]
map_mask = (nusc_map.get_map_mask(patch_box, patch_angle, layer_names,
canvas_size) * 255.0).astype(np.uint8)
map_mask = np.swapaxes(map_mask, 1, 2) # x axis comes first
# PEDESTRIANS
map_mask_pedestrian = np.stack(
(map_mask[9], map_mask[8], np.max(map_mask[:3], axis=0)), axis=0)
type_map['PEDESTRIAN'] = GeometricMap(data=map_mask_pedestrian,
homography=homography,
description=', '.join(layer_names))
# VEHICLES
map_mask_vehicle = np.stack(
(np.max(map_mask[:3], axis=0), map_mask[3], map_mask[4]), axis=0)
type_map['VEHICLE'] = GeometricMap(data=map_mask_vehicle,
homography=homography,
description=', '.join(layer_names))
map_mask_plot = np.stack(
((np.max(map_mask[:3], axis=0) -
(map_mask[3] + 0.5 * map_mask[4]).clip(max=255)).clip(min=0).astype(
np.uint8), map_mask[8], map_mask[9]),
axis=0)
type_map['VISUALIZATION'] = GeometricMap(
data=map_mask_plot,
homography=homography,
description=', '.join(layer_names))
scene.map = type_map
del map_mask
del map_mask_pedestrian
del map_mask_vehicle
del map_mask_plot
for node_id in pd.unique(data['node_id']):
node_frequency_multiplier = 1
node_df = data[data['node_id'] == node_id]
if dynamic:
if 'vehicle.parked' in attribute_dict[
node_id] or 'vehicle.stopped' in attribute_dict[node_id]:
continue
elif len(attribute_dict[node_id]
) == 1 and 'vehicle.parked' in attribute_dict[node_id]:
# Catching instances of vehicles that were parked and then moving, but not allowing
# only parked vehicles through.
continue
if node_df['x'].shape[0] < 2:
continue
if not np.all(np.diff(node_df['frame_id']) == 1):
min_index = node_df['frame_id'].min()
max_index = node_df['frame_id'].max()
node_df = node_df.set_index('frame_id').reindex(
range(min_index, max_index + 1)).interpolate().reset_index()
node_df['type'] = node_df['type'].mode()[0]
node_df['node_id'] = node_id
node_df['robot'] = False
# print('Occlusion')
# continue # TODO Make better
node_values = node_df[['x', 'y']].values
x = node_values[:, 0]
y = node_values[:, 1]
heading = node_df['heading'].values
if node_df.iloc[0][
'type'] == env.NodeType.VEHICLE and not node_id == 'ego':
# Kalman filter Agent
vx = derivative_of(x, scene.dt)
vy = derivative_of(y, scene.dt)
velocity = np.linalg.norm(np.stack((vx, vy), axis=-1), axis=-1)
filter_veh = NonlinearKinematicBicycle(dt=scene.dt,
sMeasurement=1.0)
P_matrix = None
for i in range(len(x)):
if i == 0: # initalize KF
# initial P_matrix
P_matrix = np.identity(4)
elif i < len(x):
# assign new est values
x[i] = x_vec_est_new[0][0]
y[i] = x_vec_est_new[1][0]
heading[i] = x_vec_est_new[2][0]
velocity[i] = x_vec_est_new[3][0]
if i < len(x) - 1: # no action on last data
# filtering
x_vec_est = np.array([[x[i]], [y[i]], [heading[i]],
[velocity[i]]])
z_new = np.array([[x[i + 1]], [y[i + 1]], [heading[i + 1]],
[velocity[i + 1]]])
x_vec_est_new, P_matrix_new = filter_veh.predict_and_update(
x_vec_est=x_vec_est,
u_vec=np.array([[0.], [0.]]),
P_matrix=P_matrix,
z_new=z_new)
P_matrix = P_matrix_new
curvature, pl, _ = trajectory_curvature(np.stack((x, y), axis=-1))
if pl < 1.0: # vehicle is "not" moving
x = x[0].repeat(max_timesteps + 1)
y = y[0].repeat(max_timesteps + 1)
heading = heading[0].repeat(max_timesteps + 1)
global total
global curv_0_2
global curv_0_1
total += 1
if pl > 1.0:
if curvature > .2:
curv_0_2 += 1
node_frequency_multiplier = 3 * int(
np.floor(total / curv_0_2))
elif curvature > .1:
curv_0_1 += 1
node_frequency_multiplier = 3 * int(
np.floor(total / curv_0_1))
if half_dt:
t_old = np.linspace(0, 1, x.shape[0])
t_new = np.linspace(0, 1, 2 * x.shape[0])
x = np.interp(t_new, t_old, x)
y = np.interp(t_new, t_old, y)
heading = np.interp(
t_new, t_old, heading + np.pi, period=2 * np.pi) - np.pi
vx = derivative_of(x, scene.dt / 2)
vy = derivative_of(y, scene.dt / 2)
ax = derivative_of(vx, scene.dt / 2)
ay = derivative_of(vy, scene.dt / 2)
else:
vx = derivative_of(x, scene.dt)
vy = derivative_of(y, scene.dt)
ax = derivative_of(vx, scene.dt)
ay = derivative_of(vy, scene.dt)
if node_df.iloc[0]['type'] == env.NodeType.VEHICLE:
v = np.stack((vx, vy), axis=-1)
v_norm = np.linalg.norm(np.stack((vx, vy), axis=-1),
axis=-1,
keepdims=True)
heading_v = np.divide(v,
v_norm,
out=np.zeros_like(v),
where=(v_norm > 1.))
heading_x = heading_v[:, 0]
heading_y = heading_v[:, 1]
a_norm = np.divide(ax * vx + ay * vy,
v_norm[..., 0],
out=np.zeros_like(ax),
where=(v_norm[..., 0] > 1.))
if half_dt:
d_heading = derivative_of(heading, scene.dt / 2, radian=True)
else:
d_heading = derivative_of(heading, scene.dt, radian=True)
data_dict = {
('position', 'x'):
x,
('position', 'y'):
y,
('velocity', 'x'):
vx,
('velocity', 'y'):
vy,
('velocity', 'norm'):
np.linalg.norm(np.stack((vx, vy), axis=-1), axis=-1),
('acceleration', 'x'):
ax,
('acceleration', 'y'):
ay,
('acceleration', 'norm'):
a_norm,
('heading', 'x'):
heading_x,
('heading', 'y'):
heading_y,
('heading', '°'):
heading,
('heading', 'd°'):
d_heading
}
node_data = pd.DataFrame(data_dict, columns=data_columns_vehicle)
else:
data_dict = {
('position', 'x'): x,
('position', 'y'): y,
('velocity', 'x'): vx,
('velocity', 'y'): vy,
('acceleration', 'x'): ax,
('acceleration', 'y'): ay
}
node_data = pd.DataFrame(data_dict,
columns=data_columns_pedestrian)
node = Node(node_type=node_df.iloc[0]['type'],
node_id=0 if node_id == 'ego' else abs(hash(node_id)),
data=node_data,
frequency_multiplier=node_frequency_multiplier,
description=node_id)
node.first_timestep = node_df['frame_id'].iloc[0]
if half_dt:
node.first_timestep *= 2
if node_df.iloc[0]['robot'] == True:
node.is_robot = True
scene.robot = node
scene.nodes.append(node)
if half_dt:
# We are interpolating to make the overall dt half of what it was.
scene.dt /= 2.0
scene.timesteps *= 2
return scene
def pc_to_sensor(pc_orig,
cs_record,
global_coordinates=False,
ego_pose=None):
"""
Tramsform the input point cloud from global/vehicle coordinates to
sensor coordinates
"""
assert pc_orig is not None, 'Pointcloud cannot be None. Nothing to translate'
assert global_coordinates is False or (global_coordinates and ego_pose is not None), \
'when in global coordinates, ego_pose is required'
## Copy is required to prevent the original pointcloud from being manipulate
pc = copy.deepcopy(pc_orig)
if isinstance(pc, PointCloud):
if global_coordinates:
## Transform from global to vehicle
pc.translate(np.array(-np.array(ego_pose['translation'])))
pc.rotate(Quaternion(ego_pose['rotation']).rotation_matrix.T)
## Transform from vehicle to sensor
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix.T)
elif isinstance(pc, np.ndarray):
if global_coordinates:
## Transform from global to vehicle
for i in range(3):
pc[i, :] = pc[i, :] + np.array(
-np.array(ego_pose['translation']))[i]
pc[:3, :] = np.dot(
Quaternion(ego_pose['rotation']).rotation_matrix.T,
pc[:3, :])
## Transform from vehicle to sensor
for i in range(3):
pc[i, :] = pc[i, :] - np.array(cs_record['translation'])[i]
pc[:3, :] = np.dot(
Quaternion(cs_record['rotation']).rotation_matrix.T, pc[:3, :])
elif isinstance(pc, list):
if len(pc) == 0:
return []
if isinstance(pc[0], Box):
new_list = []
for box in pc:
if global_coordinates:
## Transform from global to vehicle
box.translate(-np.array(ego_pose['translation']))
box.rotate(Quaternion(ego_pose['rotation']).inverse)
## Transform from vehicle to sensor
box.translate(-np.array(cs_record['translation']))
box.rotate(Quaternion(cs_record['rotation']).inverse)
new_list.append(box)
return new_list
elif isinstance(pc, Box):
if global_coordinates:
## Transform from global to vehicle
pc.translate(-np.array(ego_pose['translation']))
pc.rotate(Quaternion(ego_pose['rotation']).inverse)
## Transform from vehicle to sensor
pc.translate(-np.array(cs_record['translation']))
pc.rotate(Quaternion(cs_record['rotation']).inverse)
else:
raise TypeError('cannot filter object with type {}'.format(
type(pc)))
return pc
def computeEndEffQuat(self):
quat = Quaternion(matrix=self.Hmat[5])
self.endeffQuat = quat.elements
def main():
parser = getparser()
args = parser.parse_args()
f_index = args.frame_index
if os.path.splitext(f_index)[1] == '.csv':
frame_index = pd.read_csv(f_index)
else:
frame_index = pd.read_pickle(f_index)
logging.info("sample fn {}".format(
glob.glob(
os.path.join(args.camera_folder,
'*{}*.tsai'.format(frame_index['name'].values[0])))))
cam_list = [
glob.glob(
os.path.join(args.camera_folder,
'*{}*.tsai'.format(os.path.basename(frame))))[0]
for frame in frame_index['name'].values
]
if not args.gcp_folder:
gcp_folder = args.camera_folder
else:
gcp_folder = args.gcp_folder
if not os.path.exists(args.outfol):
os.makedirs(args.outfol)
gcp_list = [
glob.glob(
os.path.join(gcp_folder,
'*{}*.gcp'.format(os.path.basename(frame))))[0]
for frame in frame_index['name'].values
]
CX, CY, CZ = [
frame_index.x_sat_ecef_km.values * 1000,
frame_index.y_sat_ecef_km * 1000, frame_index.z_sat_ecef_km * 1000
]
rotation_matrices = [
Quaternion(matrix=(
np.reshape(asp.read_tsai_dict(x)['rotation_matrix'], (3, 3))))
for x in cam_list
]
fu, fv = asp.read_tsai_dict(cam_list[0])['focal_length']
cu, cv = asp.read_tsai_dict(cam_list[0])['optical_center']
pitch = asp.read_tsai_dict(cam_list[0])['pitch']
q1 = [x[0] for x in rotation_matrices]
q2 = [x[1] for x in rotation_matrices]
q3 = [x[2] for x in rotation_matrices]
q4 = [x[3] for x in rotation_matrices]
for idx, row in frame_index.iterrows():
identifier = os.path.basename(row['name'])
gcp = pd.read_csv(glob.glob(
os.path.join(gcp_folder, '*{}*.gcp'.format(identifier)))[0],
header=None,
sep=' ')
im_x, im_y = [gcp[8].values, gcp[9].values]
lon, lat, ht = [gcp[2].values, gcp[1].values, gcp[3].values]
X, Y, Z = geolib.ll2ecef(lon, lat, ht)
CX_idx, CY_idx, CZ_idx = [CX[idx], CY[idx], CZ[idx]]
q1_idx, q2_idx, q3_idx, q4_idx = [q1[idx], q2[idx], q3[idx], q4[idx]]
#tpl_int = (q1_idx,q2_idx,q3_idx,q4_idx)
print(idx)
Q1, Q2, Q3, Q4 = optimiser_quaternion(q1_idx, q2_idx, q3_idx, q4_idx,
CX_idx, CY_idx, CZ_idx, cu, cv,
fu, fv, pitch, X, Y, Z, im_x,
im_y)
rot_mat = Quaternion([Q1, Q2, Q3, Q4]).rotation_matrix
out_cam = os.path.join(args.outfol, '{}_scipy.tsai'.format(identifier))
asp.make_tsai(out_cam, cu, cv, fu, fv, rot_mat,
[CX_idx, CY_idx, CZ_idx], pitch)
logging.info("Successfully created optimised camera models at {}".format(
args.outfol))
def quatError2AngluerVel(self, quatError_array):
quatError = Quaternion(numpy.array(quatError_array))
theta = quatError.angle
angular_vel = theta * quatError.elements[1:4] / quatError.norm
return angular_vel
def export_scene_pointcloud(nusc: NuScenes,
out_path: str,
scene_token: str,
channel: str = 'LIDAR_TOP',
min_dist: float = 3.0,
max_dist: float = 30.0,
verbose: bool = True):
"""
Export fused point clouds of a scene to a Wavefront OBJ file.
This point-cloud can be viewed in your favorite 3D rendering tool, e.g. Meshlab or Maya.
:param nusc: NuScenes instance.
:param out_path: Output path to write the point-cloud to.
:param scene_token: Unique identifier of scene to render.
:param channel: Channel to render.
:param min_dist: Minimum distance to ego vehicle below which points are dropped.
:param max_dist: Maximum distance to ego vehicle above which points are dropped.
:param verbose: Whether to print messages to stdout.
"""
# Check inputs.
valid_channels = [
'LIDAR_TOP', 'RADAR_FRONT', 'RADAR_FRONT_RIGHT', 'RADAR_FRONT_LEFT',
'RADAR_BACK_LEFT', 'RADAR_BACK_RIGHT'
]
camera_channels = [
'CAM_FRONT_LEFT', 'CAM_FRONT', 'CAM_FRONT_RIGHT', 'CAM_BACK_LEFT',
'CAM_BACK', 'CAM_BACK_RIGHT'
]
assert channel in valid_channels, 'Input channel {} not valid.'.format(
channel)
# Get records from DB.
scene_rec = nusc.get('scene', scene_token)
start_sample_rec = nusc.get('sample', scene_rec['first_sample_token'])
sd_rec = nusc.get('sample_data', start_sample_rec['data'][channel])
# Make list of frames
cur_sd_rec = sd_rec
sd_tokens = []
while cur_sd_rec['next'] != '':
cur_sd_rec = nusc.get('sample_data', cur_sd_rec['next'])
sd_tokens.append(cur_sd_rec['token'])
# Write point-cloud.
with open(out_path, 'w') as f:
f.write("OBJ File:\n")
for sd_token in tqdm(sd_tokens):
if verbose:
print('Processing {}'.format(sd_rec['filename']))
sc_rec = nusc.get('sample_data', sd_token)
sample_rec = nusc.get('sample', sc_rec['sample_token'])
lidar_token = sd_rec['token']
lidar_rec = nusc.get('sample_data', lidar_token)
pc = LidarPointCloud.from_file(
osp.join(nusc.dataroot, lidar_rec['filename']))
# Get point cloud colors.
coloring = np.ones((3, pc.points.shape[1])) * -1
for channel in camera_channels:
camera_token = sample_rec['data'][channel]
cam_coloring, cam_mask = pointcloud_color_from_image(
nusc, lidar_token, camera_token)
coloring[:, cam_mask] = cam_coloring
# Points live in their own reference frame. So they need to be transformed via global to the image plane.
# First step: transform the point cloud to the ego vehicle frame for the timestamp of the sweep.
cs_record = nusc.get('calibrated_sensor',
lidar_rec['calibrated_sensor_token'])
pc.rotate(Quaternion(cs_record['rotation']).rotation_matrix)
pc.translate(np.array(cs_record['translation']))
# Optional Filter by distance to remove the ego vehicle.
dists_origin = np.sqrt(np.sum(pc.points[:3, :]**2, axis=0))
keep = np.logical_and(min_dist <= dists_origin,
dists_origin <= max_dist)
pc.points = pc.points[:, keep]
coloring = coloring[:, keep]
if verbose:
print('Distance filter: Keeping %d of %d points...' %
(keep.sum(), len(keep)))
# Second step: transform to the global frame.
poserecord = nusc.get('ego_pose', lidar_rec['ego_pose_token'])
pc.rotate(Quaternion(poserecord['rotation']).rotation_matrix)
pc.translate(np.array(poserecord['translation']))
# Write points to file
for (v, c) in zip(pc.points.transpose(), coloring.transpose()):
if (c == -1).any():
# Ignore points without a color.
pass
else:
f.write(
"v {v[0]:.8f} {v[1]:.8f} {v[2]:.8f} {c[0]:.4f} {c[1]:.4f} {c[2]:.4f}\n"
.format(v=v, c=c / 255.0))
if not sd_rec['next'] == "":
sd_rec = nusc.get('sample_data', sd_rec['next'])
def normalizeQuat(self, quat_array):
quat = Quaternion(numpy.array(quat_array))
return quat.normalised.elements
def main():
imu_params = scipy.io.loadmat('./IMUParams.mat')
acc_scale = imu_params['IMUParams'][0]
acc_bias = imu_params['IMUParams'][1]
Parser = argparse.ArgumentParser()
Parser.add_argument('--imu_data',
default=' ',
help="The path of data file being used")
Parser.add_argument('--vicon_data',
default=' ',
help="The path of vicon data file being used")
Args = Parser.parse_args()
imu_data = Args.imu_data
vicon_data = Args.vicon_data
data = scipy.io.loadmat(imu_data)
# print data
time_stamp = data['ts']
# print "ts_imu, ", time_stamp[0]
if vicon_data is not ' ':
imu_data_vicon = scipy.io.loadmat(vicon_data)
ts_vicon = imu_data_vicon['ts']
# print "ts_vicon ", ts_vicon[0][0]-time_stamp[0][0]
acc = data['vals'][:3, :]
acc = np.float32(acc)
gyro = np.float32(data['vals'][-3:, :])
# print gyro.shape
# input('a')
align_num = 0
align_minus = 160
acc_all, gyro_all = process_data(acc, acc_scale, acc_bias, gyro)
q_est = Quaternion(1, 0, 0, 0)
q_init = Quaternion(1, 0, 0, 0)
angles = np.zeros((align_num, 3))
acc_pose = np.array([0, 0, 0])
gyro_pose = np.array([0, 0, 0])
mk = madgwick(q=q_init)
thresh = acc_all.shape[1] - 1
i = 0
while i <= thresh:
acc = np.reshape(acc_all[:, i], (3, 1))
gyro = np.reshape(gyro_all[:, i], (3, 1))
q, acc_int, gyro_int = mk.imu_update(acc, gyro, q_est)
phi, thetha, psy = quat_to_euler(quaternConj(q))
temp = np.array([phi, thetha, psy])
angles = np.vstack((angles, temp))
phi, thetha, psy = quat_to_euler(quaternConj(acc_int))
temp = np.array([phi, thetha, psy])
acc_pose = np.vstack((acc_pose, temp))
phi, thetha, psy = quat_to_euler(quaternConj(gyro_int))
temp = np.array([phi, thetha, psy])
gyro_pose = np.vstack((gyro_pose, temp))
q_est = Quaternion(q[0], q[1], q[2], q[3])
i += 1
angles = angles[1:]
num = 0
acc_all = acc_all[num:]
gyro_all = gyro_all[num:]
if vicon_data is not ' ':
#PLot vicon data
rots = imu_data_vicon['rots']
vicon_data = np.zeros((3, rots.shape[2] + align_minus),
dtype=np.float32)
for ang in range(rots.shape[2]):
vicon_data[:, ang +
align_minus] = (180 / np.pi) * rot2eul(rots[:, :, ang])
# print(vicon_data.shape)
t_vicon = np.linspace(1, vicon_data.shape[1], num=vicon_data.shape[1])
# input('aaaa')
# print("befpre angle shape = ", angles.shape)
# app_angles = np.zeros((1,align_num))
# angles = np.concatenate(app_angles,angles)
# print("after angle shape = ", angles.shape)
t = np.linspace(1, int(acc_all.shape[1]), num=int(acc_all.shape[1]))
t_angles = np.linspace(1, int(angles.shape[0]), num=int(angles.shape[0]))
fig = plt.figure(1)
# plt.rcParams['figure.figsize'] = [10,10]
# if vicon_data is not ' ':
# a1 = plt.subplot(3,4,1)
# plt.plot(t[:thresh],acc_all[0,:thresh],'r-')
# a1.title.set_text('X-accel')
# a2 = plt.subplot(3,4,2)
# plt.plot(t[:thresh],gyro_all[0,:thresh],'r-')
# a2.title.set_text('X-gyro')
# # print t.shape,angles[:,0].shape
# a3 = plt.subplot(3,4,3)
# plt.plot(t_angles,angles[:,0],'r-')
# a3.title.set_text('X-madgwick')
# a10 = plt.subplot(3,4,4)
# plt.plot(t_vicon,vicon_data[0,:],'r-')
# a10.title.set_text('X-Vicon')
# a4 = plt.subplot(3,4,5)
# plt.plot(t[:thresh],acc_all[1,:thresh],'g-')
# a4.title.set_text('Y-accel')
# a5 = plt.subplot(3,4,6)
# plt.plot(t[:thresh],gyro_all[1,:thresh],'g-')
# a5.title.set_text('Y-gyro')
# a6 = plt.subplot(3,4,7)
# plt.plot(t_angles,angles[:,1],'g-')
# a6.title.set_text('Y-madgwick')
# a11 = plt.subplot(3,4,8)
# plt.plot(t_vicon,vicon_data[1,:],'g-')
# a11.title.set_text('Y-Vicon')
# a7 = plt.subplot(3,4,9)
# plt.plot(t[:thresh],acc_all[2,:thresh],'b-')
# a7.title.set_text('Z-accel')
# a8 = plt.subplot(3,4,10)
# plt.plot(t[:thresh],gyro_all[2,:thresh],'b-')
# a8.title.set_text('Z-gyro')
# a9 = plt.subplot(3,4,11)
# plt.plot(t_angles,angles[:,2],'b-')
# a9.title.set_text('Z-madgwick')
# a12 = plt.subplot(3,4,12)
# plt.plot(t_vicon,vicon_data[2,:],'b-')
# a12.title.set_text('Z-Vicon')
# else:
# a1 = plt.subplot(3,3,1)
# plt.plot(t[:thresh],acc_all[0,:thresh],'r-')
# a1.title.set_text('X-accel')
# a2 = plt.subplot(3,3,2)
# plt.plot(t[:thresh],gyro_all[0,:thresh],'r-')
# a2.title.set_text('X-gyro')
# # print t.shape,angles[:,0].shape
# a3 = plt.subplot(3,3,3)
# plt.plot(t_angles,angles[:,0],'r-')
# a3.title.set_text('X-madgwick')
# a4 = plt.subplot(3,3,4)
# plt.plot(t[:thresh],acc_all[1,:thresh],'g-')
# a4.title.set_text('Y-accel')
# a5 = plt.subplot(3,3,5)
# plt.plot(t[:thresh],gyro_all[1,:thresh],'g-')
# a5.title.set_text('Y-gyro')
# # print t.shape,angles[:,0].shape
# a6 = plt.subplot(3,3,6)
# plt.plot(t_angles,angles[:,1],'g-')
# a6.title.set_text('Y-madgwick')
# a7 = plt.subplot(3,3,7)
# plt.plot(t[:thresh],acc_all[2,:thresh],'b-')
# a7.title.set_text('Z-accel')
# a8 = plt.subplot(3,3,8)
# plt.plot(t[:thresh],gyro_all[2,:thresh],'b-')
# a8.title.set_text('Z-gyro')
# # print t.shape,angles[:,0].shape
# a9 = plt.subplot(3,3,9)
# plt.plot(t_angles,angles[:,2],'b-')
# a9.title.set_text('Z-madgwick')
a1 = plt.subplot(3, 1, 1)
line1, = a1.plot(t_angles, angles[:, 0], 'r-')
line1.set_label('Madgwick')
if vicon_data is not ' ':
line2, = a1.plot(t_vicon, vicon_data[0, :], '-b')
line2.set_label('Vicon')
a1.title.set_text('X-data')
a1.legend()
a2 = plt.subplot(3, 1, 2)
line3, = a2.plot(t_angles, angles[:, 1], 'r-')
line3.set_label('Madgwick')
if vicon_data is not ' ':
line4, = a2.plot(t_vicon, vicon_data[1, :], '-b')
line4.set_label('Vicon')
a2.title.set_text('Y-data')
a2.legend()
a3 = plt.subplot(3, 1, 3)
line5, = a3.plot(t_angles, angles[:, 2], 'r-')
line5.set_label('Madgwick')
if vicon_data is not ' ':
line6, = a3.plot(t_vicon, vicon_data[2, :], '-b')
line6.set_label('Vicon')
a3.title.set_text('Z-data')
a3.legend()
# print(len(angles))
plt.show()
def test_init_random(self):
r1 = Quaternion.random()
r2 = Quaternion.random()
self.assertAlmostEqual(r1.norm, 1.0, ALMOST_EQUAL_TOLERANCE)
self.assertIsInstance(r1, Quaternion)
