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)