自动驾驶坐标系 ego_pose

目录

ego_pose 2cam

VCS含义

ego_pose含义

cam2vcs

lidar2vcs_t lidar2vcs_q 什么意思

详细解释

综合说明

ego_pose和ego_motion

关系：

简单总结：

跟踪，动态物体

---

ego_pose 2cam

ego_pose 是世界坐标系，ext是外参，

pose = ego_cam_poses[cam, frame]

            c2w = pose @ ext

c2w是右边到左边，world转cam。

    for cam_name in valid_cams :
        lidar2cam = np.array(bev_4d_data['parameters_cam'][cam_name]["extrinsics"], dtype=np.float32)

        lidar2vcs_t = bev_4d_data['parameters_lidar_at128']['lidar2vcs']['translation']  # xyz
        lidar2vcs_q = bev_4d_data['parameters_lidar_at128']['lidar2vcs']['quaternion']  # wxyz
        lidar2vcs_q = points_transform.list_to_array([lidar2vcs_q['qw'], lidar2vcs_q['qx'], lidar2vcs_q['qy'], lidar2vcs_q['qz']], (4))
        lidar2vcs_mat = Quaternion(lidar2vcs_q).rotation_matrix
        lidar2vcs_t = points_transform.list_to_array([lidar2vcs_t['x'], lidar2vcs_t['y'], lidar2vcs_t['z']], (3))
        lidar2vcs = points_transform.get_RT_4x4(lidar2vcs_mat, lidar2vcs_t)

        cam2vcs = lidar2vcs @ np.linalg.inv(lidar2cam)

VCS含义

“VCS”通常指的是“Vehicle Control System”（车辆控制系统）。这个系统负责控制车辆的动态行为，包括加速、制动和转向，以实现自动驾驶功能

ego_pose含义

“Ego_pose” 是指自动驾驶车辆在世界坐标系中的位置和姿态信息。它通常包括车辆的三维坐标（x, y, z）和朝向（如航向角或四元数），用于描述车辆相对于周围环境的动态状态。缺少“ego_pose” 信息时，车辆的状态可能被认为是静止的，因为没有足够的信息来描述其运动变化。通过实时更新“ego_pose”，系统能够有效地进行路径规划、环境感知和动态避障。

ext = extrinsics[cam]

pose = ego_cam_poses[cam, frame]

c2w = pose @ ext

cam2vcs

看代码中 cam2vcs 的计算方式，需要明确理解每个变换矩阵的含义。

• lidar2cam 是从激光雷达坐标系到相机坐标系的变换矩阵。
• lidar2vcs 是从激光雷达坐标系到车辆坐标系的变换矩阵。

你在计算 cam2vcs 时的表达式：

cam2vcs = lidar2vcs @ np.linalg.inv(lidar2cam)

此表达式的意思是先将点从相机坐标系转换到激光雷达坐标系（通过 lidar2cam 的逆矩阵 np.linalg.inv(lidar2cam)），然后从激光雷达坐标系转换到车辆坐标系（通过 lidar2vcs 变换）。

这种计算逻辑是合理的，即先应用从相机到激光雷达的逆变换，再应用从激光雷达到车辆的正变换，将点从相机坐标系转换到车辆坐标系。

lidar2vcs_t lidar2vcs_q 什么意思

在代码中，lidar2vcs_t 和 lidar2vcs_q 分别表示从激光雷达坐标系（lidar）到车辆坐标系（vcs，Vehicle Coordinate System）的平移向量和四元数旋转。

详细解释

1. lidar2vcs_t：这是一个平移向量（translation vector），表示激光雷达坐标系在车辆坐标系中的位置。这个向量通常包含 x、y、z 三个分量，表示激光雷达相对于车辆坐标系的偏移量。

   • lidar2vcs_t = bev_4d_data['parameters_lidar_at128']['lidar2vcs']['translation']
   • 例如：lidar2vcs_t = {'x': 1.0, 'y': 0.5, 'z': 0.2} 表示激光雷达相对于车辆坐标系在 x、y、z 方向上分别有 1.0、0.5 和 0.2 个单位的平移。

2. lidar2vcs_q：这是一个四元数（quaternion），用于描述激光雷达坐标系相对于车辆坐标系的旋转。四元数通常以四个分量表示，w, x, y, z，其中 w 表示实部，x, y, z 表示虚部，用于描述三维空间中的旋转角度。

   • lidar2vcs_q = bev_4d_data['parameters_lidar_at128']['lidar2vcs']['quaternion']
   • 例如：lidar2vcs_q = {'qw': 0.707, 'qx': 0.0, 'qy': 0.707, 'qz': 0.0} 表示旋转的四元数。
   • 这个四元数可以通过库函数（如 Quaternion 类）转换为旋转矩阵，用于变换坐标。

综合说明

• lidar2vcs_t 和 lidar2vcs_q 的组合可以定义一个完整的 3D 变换（即从激光雷达坐标系到车辆坐标系的 4x4 变换矩阵），用于在不同坐标系之间进行点的转换。

ego_pose和ego_motion

ego_pose和ego_motion是自动驾驶和机器人领域中密切相关的概念，但它们的侧重点不同。

1. Ego_pose：

   • 描述车辆在世界坐标系中的位置和姿态，包括三维坐标（x, y, z）和方向（如航向角或四元数）。
   • 它提供了车辆的静态信息，反映了车辆在某一时刻的确切位置和方向。

2. Ego_motion：

   • 描述车辆在一段时间内的运动状态，通常涉及位置和姿态的变化。
   • 这包括车辆的速度、加速度、转向角等动态信息，反映车辆的运动轨迹和行为。

关系：

• Ego_pose 是 Ego_motion 的基础。通过对连续时间点的 ego_pose 进行比较，可以推导出 ego_motion。
• 例如，如果已知某时刻的 ego_pose，通过对后续时刻的 ego_pose 进行分析，可以计算出车辆的运动状态（例如速度和加速度）。

简单总结：

• Ego_pose 提供位置和姿态的快照，而 Ego_motion 描述了在时间维度上这些参数的变化。两者共同帮助自动驾驶系统理解车辆的状态和行为。

跟踪，动态物体

            for tracklet in object_tracklets_vehicle[i]:
                track_id = int(tracklet[0])
                if track_id >= 0:
                    obj_pose_vehicle = np.eye(4)
                    obj_pose_vehicle[:3, :3] = quaternion_to_matrix_numpy(tracklet[4:8])
                    obj_pose_vehicle[:3, 3] = tracklet[1:4]
                    vehicle2local = np.linalg.inv(obj_pose_vehicle)
                    
                    points_xyz_obj = points_xyz_vehicle @ vehicle2local.T
                    points_xyz_obj = points_xyz_obj[..., :3]
                    
                    length = object_info[track_id]['length']
                    width = object_info[track_id]['width']
                    height = object_info[track_id]['height']
                    bbox = [[-length/2, -width/2, -height/2], [length/2, width/2, height/2]]
                    obj_corners_3d_local = bbox_to_corner3d(bbox)
                    
                    points_xyz_inbbox = inbbox_points(points_xyz_obj, obj_corners_3d_local)
                    points_xyz_obj_mask = np.logical_or(points_xyz_obj_mask, points_xyz_inbbox)
                    points_xyz_dict[f'obj_{track_id:03d}'].append(points_xyz_obj[points_xyz_inbbox])
                    points_rgb_dict[f'obj_{track_id:03d}'].append(points_rgb[points_xyz_inbbox])