def updateModule(self):
# synchronize on rgb stream
rgb = self.rgb_in.read(True)
depth = self.depth_in.read(False)
mask = self.mask_in.read(False)
camera_pose = self.camera_pose_in.read(False)
if (rgb is not None) and (depth is not None) and (mask is not None) and (camera_pose is not None):
camera_position = numpy.array([camera_pose[0], camera_pose[1], camera_pose[2]])
camera_axis = numpy.array([camera_pose[3], camera_pose[4], camera_pose[5]])
camera_angle = camera_pose[6]
self.rgb_image.copy(rgb)
rgb_frame = numpy.frombuffer(self.rgb_buffer, dtype=numpy.uint8).reshape(self.options.height, self.options.width, 3)
self.depth_image.copy(depth)
depth_frame = numpy.frombuffer(self.depth_buffer, dtype=numpy.float32).reshape(self.options.height, self.options.width)
self.mask_image.copy(mask)
mask_frame = numpy.frombuffer(self.mask_buffer, dtype=numpy.uint8).reshape(self.options.height, self.options.width)
prediction = self.inference.inference(self.options.object_id, rgb_frame, depth_frame, mask_frame)
if len(prediction) != 0:
object_orientation = Quaternion(axis=[prediction[3], prediction[4], prediction[5]], angle = prediction[6])
camera_orientation = Quaternion(axis=camera_axis, angle=camera_angle).rotation_matrix
total_orientation = camera_orientation.dot(object_orientation.rotation_matrix)
total_orientation_axis = Quaternion(matrix=total_orientation).axis
total_orientation_angle = Quaternion(matrix=total_orientation).angle
total_position = camera_position + camera_orientation.dot(prediction[0:3])
stamp = yarp.Stamp()
self.rgb_in.getEnvelope(stamp)
new_stamp = stamp.getTime()
velocity = [0, 0, 0, 0, 0, 0]
if self.last_stamp > 0:
elapsed = new_stamp - self.last_stamp
velocity[0:3] = (total_position - self.last_total_position) / elapsed
relative_orientation = total_orientation.dot(self.last_total_orientation.transpose())
acos_argument = (relative_orientation.trace() - 1) / 2.0
if acos_argument < -1.0:
acos_argument = -1.0
else:
acos_argument = 1.0
theta = math.acos(acos_argument) + sys.float_info.min
ang_velocity_skew = 1.0 / (2.0 * elapsed) * theta / math.sin(theta) * (relative_orientation - relative_orientation.transpose())
velocity[3:6] = [-ang_velocity_skew[1, 2], ang_velocity_skew[0, 2], -ang_velocity_skew[0, 1]]
self.last_stamp = new_stamp
self.last_total_position = total_position
self.last_total_orientation = total_orientation
output_vector = yarp.Vector()
output_vector.resize(13)
output_vector[0] = total_position[0]
output_vector[1] = total_position[1]
output_vector[2] = total_position[2]
output_vector[3] = total_orientation_axis[0]
output_vector[4] = total_orientation_axis[1]
output_vector[5] = total_orientation_axis[2]
output_vector[6] = total_orientation_angle
output_vector[7] = velocity[0]
output_vector[8] = velocity[1]
output_vector[9] = velocity[2]
output_vector[10] = velocity[3]
output_vector[11] = velocity[4]
output_vector[12] = velocity[5]
self.prediction_out.write(output_vector)
# print(1.0 / (time.time() - start_time))
return True
def kalman_smoother(df, scene_id):
df = df.sort_values(by=['FRAME'])
df.loc[:, "OCCLUSION"] = int(0) # Wheter the occlusions is dealt with.
df.loc[:, "KALMAN"] = int(0) # Wheter the Kalman Smoothing is done.
token_list = df["INSTANCE_TOKEN"].unique()
output_df_rows = []
for instance_token in token_list:
instance_df = df[df["INSTANCE_TOKEN"] == instance_token].copy()
category = instance_df["OBJECT_CATEGORY"].iloc[0]
if ("vehicle" not in category) and ("human" not in category):
# Do not include this instance
# if category does not belong to vehicles or humans.
continue
else:
raw_frames = instance_df['FRAME'].to_numpy()
raw_frames_diff = np.diff(raw_frames)
initial_raw_frame = raw_frames[0]
last_raw_frame = raw_frames[-1]
raw_frames_shifted = raw_frames - initial_raw_frame
len_raw_trajectory = last_raw_frame - initial_raw_frame + 1
# Detect static agents with the attribute condition set as
# vehicle.parked, vehicle.stopped, pedestrian.sitting_lying_down,
# cycle.without_rider, or pedestrian.standing for the entire sequence of frames.
static_frame = [np.any([cond in attribute for cond in ['stopped', 'parked', 'sitting_lying_down', 'standing', 'without_rider']]) for attribute in instance_df.OBJECT_ATTRIBUTE]
trajectory_is_static = np.all(static_frame)
if trajectory_is_static:
# No smoothing is performed for static agents.
# Check if there exists any occlussion for this static trajectory.
occlusion_mask = np.ones(len_raw_trajectory, dtype="int")
occlusion_mask[raw_frames_shifted] = 0
has_occlusion = 1 in occlusion_mask
if has_occlusion:
repeat_counts = raw_frames_diff.tolist() + [1]
# Repeat instance_df rows at occluded points.
instance_df_rep = pd.DataFrame(np.repeat(instance_df.values, repeat_counts, axis=0))
instance_df_rep.columns = instance_df.columns
instance_df_rep.loc[:, 'OCCLUSION'] = occlusion_mask
instance_df_rep.loc[:, 'FRAME'] = [x for x in range(initial_raw_frame, last_raw_frame+1)]
output_df_rows.append(instance_df_rep)
else:
# instance_df is direct-copied if trajectory_is_static and not trajectory_has_occlusion.
output_df_rows.append(instance_df)
else:
# Kalamn Smoothing for non-static agents.
# A trajectory is splitted at points where occlusions persisted for more than 3 frames.
split_points = (raw_frames_diff > 3).nonzero()[0]
if len(split_points) > 0:
split_points += 1
frames_split = np.split(raw_frames, split_points)
for frames in frames_split:
frames_df = instance_df[np.isin(instance_df.FRAME, frames)].copy()
initial_frame = frames[0]
last_frame = frames[-1]
len_trajectory = last_frame - initial_frame + 1
if len_trajectory < 3:
# Do not perform Kalman smoothing for this split.
# (the sequence legnth is too short)
output_df_rows.append(frames_df)
continue
frames_df.loc[:, 'KALMAN'] = int(1)
trajectory = frames_df[['X_CITY','Y_CITY','Z_CITY']].to_numpy()
translation = np.expand_dims(trajectory[0], axis=1)
rotation = frames_df[frames_df.FRAME == initial_frame][['QW','QX','QY','QZ']].to_numpy().squeeze()
rot_matrix = Quaternion(rotation).rotation_matrix
invrot_matrix = rot_matrix.T
# Normalize this split to ego coordinate sys.
trajectory_ego = invrot_matrix.dot(trajectory.T-translation)
# Calculate initial states for Kalman smoother.
initial_position = trajectory_ego[:, 0]
initial_velocity = (trajectory_ego[:, 1] - trajectory_ego[:, 0]) / (frames[1] - frames[0]) * 2.0
kf_initial_state = np.concatenate([initial_position, initial_velocity], axis=0)
kf_initial_state_cov = 0.1 * np.eye(6)
kf_initial_transition = np.array([[1.0, 0.0, 0.0, 0.5, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0, 0.5, 0.0],
[0.0, 0.0, 1.0, 0.0, 0.0, 0.5],
[0.0, 0.0, 0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, 1.0]])
kf_initial_transition_cov = np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]) * np.eye(6)
kf_initial_observation = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 1.0, 0.0, 0.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0, 0.0, 0.0]])
kf_initial_observation_cov = 0.5 * np.eye(3)
kf = KalmanFilter(transition_matrices=kf_initial_transition, observation_matrices=kf_initial_observation,
transition_covariance=kf_initial_transition_cov, observation_covariance=kf_initial_observation_cov,
initial_state_mean=kf_initial_state, initial_state_covariance=kf_initial_state_cov,
em_vars=['transition_covariance', 'observation_covariance', 'initial_state_mean', 'initial_state_covariance'])
# Prepare a masked array for Kalman Smoothing.
frames_shifted = frames - initial_frame
array = np.zeros([len_trajectory, 3])
array[frames_shifted, :] = trajectory_ego.T
occlusion_mask = np.ones(len_trajectory, dtype="int")
occlusion_mask[frames_shifted] = 0
masked_array = ma.array(array,
mask=np.repeat(occlusion_mask.reshape((-1, 1)), 3, axis=1),
copy=True)
# EM fit the KF initial states
kf = kf.em(masked_array, n_iter=5)
smoothed_state, _ = kf.smooth(masked_array)
smoothed_trajectory_ego = smoothed_state[:,:3]
smoothed_frames = [x for x in range(initial_frame, last_frame+1)]
# Denormalize this split to global coordinate sys
smoothed_trajectory = rot_matrix.dot(smoothed_trajectory_ego.T) + translation
smoothed_trajectory = smoothed_trajectory.T # Dim X Time to Time X Dim
has_occlusion = 1 in occlusion_mask
if has_occlusion:
# Replace X_CITY,Y_CITY,Z_CITY with the smoothed trajectory.
# With potentially appending occluded points.
repeat_counts = np.diff(frames).tolist() + [1]
# Repeat frames_df rows at occluded points,
# then repalce their values with the smoothed ones.
frames_df_rep = pd.DataFrame(np.repeat(frames_df.values, repeat_counts, axis=0))
frames_df_rep.columns = frames_df.columns
frames_df_rep[['X_CITY','Y_CITY','Z_CITY']] = smoothed_trajectory.astype("float")
frames_df_rep['OCCLUSION'] = occlusion_mask
frames_df_rep['FRAME'] = smoothed_frames
output_df_rows.append(frames_df_rep)
else:
frames_df[['X_CITY','Y_CITY','Z_CITY']] = smoothed_trajectory.astype("float")
output_df_rows.append(frames_df)
smoothed_df = pd.concat(output_df_rows)
dtypes = {"CITY_NAME": "str",
"SAMPLE_TOKEN": "str",
"INSTANCE_TOKEN": "str",
"OBJECT_CATEGORY": "str",
"OBJECT_ATTRIBUTE": "str",
"TIMESTAMP": "int64",
"FRAME": "int32",
"X_CITY": "float64",
"Y_CITY": "float64",
"Z_CITY": "float64",
"QW": "float64",
"QX": "float64",
"QY": "float64",
"QZ": "float64",
"OCCLUSION": "int32",
"KALMAN": "int32"}
smoothed_df = smoothed_df.astype(dtypes)
smoothed_df = smoothed_df.sort_values(by=['FRAME'])
return smoothed_df, scene_id
def transfer_rotation(self, sdk_frame, bone_structure):
root_bone = bone_structure.bone_dict['Hips']
bone_structure.reset()
sdk_joint_R = [None for i in range(self.n_joints)]
rotations = [
np.array([1., 0., 0., 0.])
for i in range(len(self.bld_joint_names))
]
# for debug
# example=VlzExample('joint_test/debug')
# line_width=8
# axis_length=0.5
bone_structure.reset()
# for pi,ci in self.l:
for pi, ci in [[9, 8], [8, 1], [1, 18], [18, 0], [1, 2], [2, 3],
[1, 5], [5, 6], [9, 10], [10, 11], [11, 12], [9, 13],
[13, 14], [14, 15]]:
if self.sdk_to_bld_joint_map[ci] is not None:
bld_name = self.sdk_to_bld_joint_map[ci]
bld_bone = bone_structure.bone_dict[bld_name]
bld_id = self.bld_name_to_id[bld_name]
bld_world_R = bld_bone.world_frame[:3, :3]
world_v = sdk_frame[ci][0] - sdk_frame[pi][0]
q = rot_between_v(bld_world_R[:, 1], world_v)
world_rot = Quaternion(q).rotation_matrix
sdk_world_R = world_rot.dot(bld_world_R)
sdk_joint_R[pi] = sdk_world_R
if pi == BASESPLINE:
parent_world_R = root_bone.world_frame[:3, :3]
else:
parent_world_R = sdk_joint_R[self.sdk_joint_parents[pi]]
local_R = parent_world_R.T.dot(sdk_world_R)
local_rot = bld_bone.local_frame[:3, :3].T.dot(local_R)
rotations[bld_id] = Quaternion(matrix=local_rot).q
bone_structure.apply_local_rotations({bld_name: local_rot})
# vs=[p for p,R in sdk_frame]
# colors=[np.array([1.,1.,1.]) for i in range(len(vs))]
# example.draw_lines(vs,self.l,colors)
# p=sdk_frame[pi][0]
# example.proto_debug_utils.Add_Line(p,p+axis_length*sdk_world_R[:,0],color=[1.,0.,0.],width=line_width)
# example.proto_debug_utils.Add_Line(p,p+axis_length*sdk_world_R[:,1],color=[0.,1.,0.],width=line_width)
# example.proto_debug_utils.Add_Line(p,p+axis_length*sdk_world_R[:,2],color=[0.,0.,1.],width=line_width)
# v,l=bone_structure.get_bone_obj()
# example.draw_lines(v+np.array([7.,0,0]),l,line_colors=np.ones(v.shape))
# example.add_frame()
for gpi, pi, ci in [[2, 3, 4], [5, 6, 7]]:
bld_name = self.sdk_to_bld_joint_map[ci]
bld_bone = bone_structure.bone_dict[bld_name]
bld_id = self.bld_name_to_id[bld_name]
vpc = sdk_frame[ci][0] - sdk_frame[pi][0]
vgpp = sdk_frame[gpi][0] - sdk_frame[pi][0]
cT = -np.inner(vpc, vgpp) / norm(vpc) / norm(vgpp)
t = np.arccos(cT)
qc = from_axis_angle(np.array([1, 0, 0]), t)
rotations[bld_id] = qc
Rc = Quaternion(qc).rotation_matrix
bone_structure.apply_local_rotations({bld_name: Rc})
# vs=[p for p,R in sdk_frame]
# colors=[np.array([1.,1.,1.]) for i in range(len(vs))]
# example.draw_lines(vs,self.l,colors)
# v,l=bone_structure.get_bone_obj()
# colors=np.ones((len(v),3))
# example.draw_lines(v+np.array([8,0,0]),l,colors)
# example.add_frame()
bld_name_p = self.sdk_to_bld_joint_map[pi]
bld_bone_p = bone_structure.bone_dict[bld_name_p]
bld_id_p = self.bld_name_to_id[bld_name_p]
vpc_src = Rc[:, 1]
vpc_tgt = bld_bone_p.world_frame[:3, :3].T.dot(vpc)
t = find_rot_angle(np.array([0, 1, 0]), vpc_src, vpc_tgt)
qp = qmul(rotations[bld_id_p],
from_axis_angle(np.array([0, 1, 0]), t))
Rp = Quaternion(qp).rotation_matrix
bone_structure.apply_local_rotations({bld_name_p: Rp})
rotations[bld_id_p] = qp
# colors=[np.array([1.,1.,1.]) for i in range(len(vs))]
# example.draw_lines(vs,self.l,colors)
# v,l=bone_structure.get_bone_obj()
# colors=np.ones((len(v),3))
# example.draw_lines(v+np.array([8,0,0]),l,colors)
# example.add_frame()
return np.array(rotations)
