MotionNet SourceCode 解读[#2]-from_file_multisweep_bf_sample_d

from_file_multisweep_bf_sample_data()

def from_file_multisweep_bf_sample_data(cls,                                            nusc: 'NuScenes',                                            ref_sd_rec: Dict,                                            nsweeps_back: int = 5,                                            nsweeps_forward: int = 5,                                            return_trans_matrix: bool = False,                                            min_distance: float = 1.0):

red_sd_rec 此项为NuScenes光达Top的sensor sample

# Initpoints = np.zeros((cls.nbr_dims(), 0))all_pc = cls(points)all_times = np.zeros((1, 0))# Get reference pose and timestampref_pose_rec = nusc.get('ego_pose', ref_sd_rec['ego_pose_token'])ref_cs_rec = nusc.get('calibrated_sensor', ref_sd_rec['calibrated_sensor_token'])ref_time = 1e-6 * ref_sd_rec['timestamp']

ref_pose_rec : {'token': 'fdddd75ee1d94f14a09991988dab8b3e', 'timestamp': 1533151603547590, 'rotation': [-0.968669701688471, -0.004043399262151301, -0.007666594265959211, 0.24820129589817977], 'translation': [600.1202137947669, 1647.490776275174, 0.0]}
ref_cs_rec :{'token': 'd051cafdd9fe4d999b413462364d44a0', 'sensor_token': 'dc8b396651c05aedbb9cdaae573bb567', 'translation': [0.985793, 0.0, 1.84019], 'rotation': [0.706749235646644, -0.015300993788500868, 0.01739745181256607, -0.7070846669051719], 'camera_intrinsic': []}
ref_time :1533151603.54759

# Homogeneous transform from ego car frame to reference frame  从car到reference的转换ref_from_car = transform_matrix(ref_cs_rec['translation'], Quaternion(ref_cs_rec['rotation']), inverse=True)# Homogeneous transformation matrix from global to _current_ ego car frame 从global到car的转换这边inverse=True 是因为 原本的translation 跟 rotation 是从car到global的转换car_from_global = transform_matrix(ref_pose_rec['translation'], Quaternion(ref_pose_rec['rotation']),                                   inverse=True)# Aggregate current and previous sweeps.current_sd_rec = ref_sd_rec    # "sample_data"trans_matrix_list = list()skip_frame = 0

ref_from_car : 4x4 quaternion matrix
ref_from_global : 4x4x quaternion matrix

for k in range(nsweeps_back):    # Load up the pointcloud.    current_pc = cls.from_file(osp.join(nusc.dataroot, current_sd_rec['filename']))    # Get past pose.    # 当下这个frame 的 ego pose 数值: token,timestamp, rotation, translation    current_pose_rec = nusc.get('ego_pose', current_sd_rec['ego_pose_token'])    # car到global的转换矩阵    global_from_car = transform_matrix(current_pose_rec['translation'],                                       Quaternion(current_pose_rec['rotation']), inverse=False)        # Homogeneous transformation matrix from sensor coordinate frame to ego car frame.    # calibration 矩阵    current_cs_rec = nusc.get('calibrated_sensor', current_sd_rec['calibrated_sensor_token'])    # 从当下的frame 转换到car的frame    car_from_current = transform_matrix(current_cs_rec['translation'], Quaternion(current_cs_rec['rotation']),inverse=False)        # Fuse four transformation matrices into one and perform transform.    # reduce 在给定的运算式中对给定的数列进行迭带运算 ex reduce[1,2,3,4,5] = (((((1+2)+3)+4)+5)    # 求出ref_from_current 从遍历到的lidar sweep转到当下时间的frame    # 并把current 的lidar frame 转换到reference 的    trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])    current_pc.transform(trans_matrix)        # 把当下的trans_matrix记录下来    # Collect the transformation matrix    trans_matrix_list.append(trans_matrix)        # Remove close points and add timevector.    # 这边的remove 是移除离自驾车一定範围内的点 (min_distance是半径)    current_pc.remove_close(min_distance)    #time lag 是current_frame 到 reference frame的时间差    time_lag = ref_time - 1e-6 * current_sd_rec['timestamp']  # positive difference        # 不太懂...    if k % (skip_frame + 1) == 0:        times = time_lag * np.ones((1, current_pc.nbr_points()))    else:        times = time_lag * np.ones((1, 1))  # dummy value    all_times = np.hstack((all_times, times))        # 把过去的frame的点云叠加到reference frame上    # Merge with key pc.    if k % (skip_frame + 1) == 0:        all_pc.points = np.hstack((all_pc.points, current_pc.points))    else:        tmp_points = np.zeros((4, 1), dtype=np.float32)        all_pc.points = np.hstack((all_pc.points, tmp_points))  # dummy value    if current_sd_rec['prev'] == '':  # Abort if there are no previous sweeps.        break    else:        current_sd_rec = nusc.get('sample_data', current_sd_rec['prev'])

trans_matrix_list = np.stack(trans_matrix_list, axis=0)# shape会变( N , 4, 4 )# Aggregate the future sweeps#reset 现在的frame current_sd_rec = ref_sd_rec# Abort if there are no future sweeps. Return.if current_sd_rec['next'] == '':    return all_pc, np.squeeze(all_times, axis=0)else:    current_sd_rec = nusc.get('sample_data', current_sd_rec['next'])for k in range(1, nsweeps_forward + 1):    # Load up the pointcloud.    current_pc = cls.from_file(osp.join(nusc.dataroot, current_sd_rec['filename']))    # Get the pose for this future sweep.    # 取得future sweep 的pose 还有取得 car到global的转换     current_pose_rec = nusc.get('ego_pose', current_sd_rec['ego_pose_token'])    global_from_car = transform_matrix(current_pose_rec['translation'],                                       Quaternion(current_pose_rec['rotation']), inverse=False)       # 取得 current 到 car的转换 利用calibrated matrix    # Homogeneous transformation matrix from sensor coordinate frame to ego car frame.    current_cs_rec = nusc.get('calibrated_sensor', current_sd_rec['calibrated_sensor_token'])    car_from_current = transform_matrix(current_cs_rec['translation'], Quaternion(current_cs_rec['rotation']),                                        inverse=False)        # Fuse four transformation matrices into one and perform transform.    trans_matrix = reduce(np.dot, [ref_from_car, car_from_global, global_from_car, car_from_current])    current_pc.transform(trans_matrix)    # Remove close points and add timevector.    current_pc.remove_close(min_distance)    #时间差会是负的 因为是未来的frame    time_lag = ref_time - 1e-6 * current_sd_rec['timestamp']  # negative difference    if k % (skip_frame + 1) == 0:        times = time_lag * np.ones((1, current_pc.nbr_points()))    else:        times = time_lag * np.ones((1, 1))  # dummy value    all_times = np.hstack((all_times, times))    # Merge with key pc.    if k % (skip_frame + 1) == 0:        all_pc.points = np.hstack((all_pc.points, current_pc.points))    else:        tmp_points = np.zeros((4, 1), dtype=np.float32)        all_pc.points = np.hstack((all_pc.points, tmp_points))  # dummy value    if current_sd_rec['next'] == '':  # Abort if there are no future sweeps.        break    else:        current_sd_rec = nusc.get('sample_data', current_sd_rec['next'])if return_trans_matrix:    return all_pc, np.squeeze(all_times, 0), trans_matrix_listelse:    return all_pc, np.squeeze(all_times, 0)