Occupancy 后处理

文章目录

- bev坐标与自车坐标转换
- 如何创建旋转矩阵 (R_veh) 偏航
- 3D Voxel -> 2D Grid

在进行占据空间（occupancy）后处理时，需要将不同感知模块的输出进行综合融合，以实现更精确的空间占据和环境感知。以下是针对您提到的几个方面的详细处理说明：

与其他感知输出的融合：
- 单目3D检测的结果（如bounding box）可以与占据网格（occupancy grid）结合，以在空间中确定物体的确切位置。
- 高精度地图的在线矢量化方案（如maptr/vectormapnet等）输出的路线和特征点可以与占据网格融合，以提高定位和导航的准确性。
- 语义场景补全的结果可以用来填补占据网格中由于视线遮挡或感知限制而产生的空缺。
- 运动规划（motion planning）的结果可以帮助确定需要保留的占据空间，以规避障碍并优化路径。
保留特定类别：
- 可以根据业务需求，选择只保留某些特定类别的占据信息，例如：
  - 道路边界：类似于您提到的Tesla的占据方案中显示为绿色的部分。
  - 与人物相关的voxels：如TeslaBot示例中仅保留与行人相关的体素，这对于行人检测和交互至关重要。
保留特定高度的voxels：
- 根据实际应用场景的需要，您可能只想保留一定高度范围内的voxels。例如，对于驾驶车辆，可能只关注地面附近的voxels；而对于无人机，可能需要一个更广泛的高度范围。
获得freespace信息：
- 在处理占据网格时，除了标识占据的体素外，还需要识别和标记自由空间（freespace），这对于安全导航和避障至关重要。

bev坐标与自车坐标转换

P_bev = (X_bev, Y_bev, Z_bev) ： BEV 坐标中的一个点
P_self = (X_self, Y_self, Z_self) ：自车辆坐标中的相同点
T_veh = (T_x, T_y, T_z)：在 BEV 坐标中代表车辆位置的平移矢量。
R_veh：代表车辆方位（航向、俯仰和翻滚）的 3x3 旋转矩阵。
转换：自车辆坐标到 BEV 坐标

平移：

X_bev = X_self - T_x
Y_bev = Y_self - T_y
Z_bev = Z_self - T_z

import numpy as np

def self_vehicle_to_bev(point_self, vehicle_position_bev, vehicle_rotation_matrix):
    """Transforms a point from self-vehicle coordinates to BEV coordinates.

    Args:
        point_self (np.array): 3D point in self-vehicle coordinates (x, y, z).
        vehicle_position_bev (np.array): 3D position of the vehicle in BEV coordinates (x, y, z).
        vehicle_rotation_matrix (np.array): 3x3 rotation matrix representing the vehicle's orientation.

    Returns:
        np.array: 3D point in BEV coordinates.
    """

    # Translation
    point_translated = point_self - vehicle_position_bev

    # Rotation
    point_bev = np.dot(vehicle_rotation_matrix, point_translated)

    return point_bev

旋转：

P_bev = R_veh * P_self
(这是矩阵乘法）

如何创建旋转矩阵 (R_veh) 偏航

旋转矩阵取决于您如何表示车辆的方向。下面是一种常见的方法，假定你有车辆的航向（偏航）角度：

def create_rotation_matrix(yaw_angle)：
    """根据偏航角（围绕 Z 轴）创建一个 3x3 旋转矩阵。
    cos_yaw = np.cos(yaw_angle)
    sin_yaw = np.sin(yaw_angle)

    return np.array([
        [cos_yaw, -sin_yaw, 0]、
        [sin_yaw, cos_yaw, 0]、
        [0, 0, 1] 
    ])

参考：
https://en.wikipedia.org/wiki/Rotation_matrix
中文

3D Voxel -> 2D Grid

假设3D的voxel volume的尺寸是(x, y, z), 投影到的平面BEV的尺寸就是(x, y)
将三维体素表示转换为二维网格需要选择特定的投影方向，然后将体素占位信息映射到所选的二维平面上。下面是这一过程的详细说明、公式和示例：

解释：

投影平面：您需要选择一个投影平面。常见的选择包括

XY 平面（鸟瞰图）：将体素投影到地平面上（X 轴：向前，Y 轴：向左）。
XZ 平面（侧视图）：将体素投影到侧视平面上（X 轴：向前，Z 轴：向上）。
YZ 平面（正视图）：将体素投射到前平面上（Y 轴：左，Z 轴：上）。
体素占位：每个体素都有一个值，表示它是被占用（1）还是空的（0）。

映射：投影过程包括确定每个体素在二维平面中属于哪个网格单元。这取决于所选的投影平面和体素大小。

公式：

具体公式因所选投影平面而异。下面以鸟瞰图（XY 平面）投影为例：

设 voxel_origin 为体素左下角的（X、Y、Z）坐标。
设 voxel_size 为体素在每个维度上的大小（假设为立方体）。
设 grid_width 和 grid_height 为输出二维网格的尺寸。
体素对应的网格单元坐标计算公式如下

grid_X = int(floor(voxel_origin[0] / voxel_size))
grid_Y =int(floor(voxel_origin[1] / voxel_size)) 注：floor
函数确保我们得到体素左下角所在网格单元的整数索引。

def project_voxel_to_grid_bev(voxel_origin, voxel_size, grid_width, grid_height):
  """
  Projects a 3D voxel onto a 2D grid (Bird's Eye View).

  Args:
      voxel_origin (tuple): (X, Y, Z) coordinates of the voxel's bottom-left corner.
      voxel_size (float): Size of a voxel along each dimension.
      grid_width (int): Width of the output 2D grid.
      grid_height (int): Height of the output 2D grid.

  Returns:
      tuple: (grid_X, grid_Y) coordinates of the corresponding grid cell.
  """

  grid_X = int(np.floor(voxel_origin[0] / voxel_size))
  grid_Y = int(np.floor(voxel_origin[1] / voxel_size))

  # Handle edge cases (voxel goes beyond grid boundaries)
  grid_X = max(0, min(grid_X, grid_width - 1))
  grid_Y = max(0, min(grid_Y, grid_height - 1))

  return grid_X, grid_Y

# Example usage
voxel_origin = (2.5, 1.0, 0.5)  # (X, Y, Z) coordinates
voxel_size = 0.5
grid_width = 100
grid_height = 50

grid_cell = project_voxel_to_grid_bev(voxel_origin, voxel_size, grid_width, grid_height)
print(f"Voxel projects to grid cell: ({grid_cell[0]}, {grid_cell[1]})")