how to map lidar bounding box coordinate on the map

[ahsan155]

I am using the waymo perception dataset v1.4.2 that comes with map. The tfrecord file has a field: laser_labels { box { center_x: 39.862915373549185 center_y: 37.57924021856002 center_z: 3.9028802750369778 width: 2.07522034497737 length: 3.6631732945601687 height: 1.7400000000000038 heading: 1.563768235771243 } metadata { speed_x: -8.445226685916751e-29 speed_y: 0.0 accel_x: 1.0556533986614397e-28 accel_y: 0.0 speed_z: 1.6494583370931155e-31 accel_z: -2.0618230442606244e-31 } type: TYPE_VEHICLE id: "-HJlbwWGxigWVjAFBBdTxA" num_lidar_points_in_box: 52 most_visible_camera_name: "FRONT_LEFT" camera_synced_box { center_x: 39.859376980729586 center_y: 37.582941913788076 center_z: 3.902378373465603 width: 2.07522034497737 length: 3.6631732945601687 height: 1.7400000000000038 heading: 1.563768235771243 } num_top_lidar_points_in_box: 53 }

I want to use center_x and center_y in the box field above and plot it on the map of the scenario.

But box coordinate doesn't seem to be consistent with the map. Is there some offset values I need to use to adjust the coordinates? Am I thinking about this the right way. Can the box coordinates be plotted like this? I want to do motion prediction if I can visualize trajectory of bounding box on the map. I know there is a separate motion dataset but I still want to do motion prediction with this dataset.

[scott-ettinger]

Hi,

The issue is that the labels are in the local vehicle frame coordinates but the map data is in global frame coordinates. For each frame you need to use the pose field to transform the label points. Here is some example code to visualize the map data along with the vehicle box centers:

import numpy as np
import plotly.graph_objs as go
from waymo_open_dataset import dataset_pb2
from waymo_open_dataset import label_pb2
from waymo_open_dataset.utils import frame_utils
from waymo_open_dataset.utils import plot_maps

def plot_map_with_boxes(frames: list[dataset_pb2.Frame])->None:
  """Plot the point clouds within the given frames with map data.

  Map data must be populated in the first frame in the list.

  Args:
    frames: A list of frames to be plotted, frames[0] must contain map data.
  """

  # Plot the map features.
  if len(frames) == 0:
    return
  figure = plot_maps.plot_map_features(frames[0].map_features)  

  def get_transform(frame_num: int):
    tform = []
    for v in frames[frame_num].pose.transform:
      tform.append(v)
    tform_tensor = tf.transpose(tf.reshape(tf.convert_to_tensor(tform, dtype=tf.float32), [4, 4]))
    return tform_tensor

  for object_index in range(len(frames[0].laser_labels)):
    # Get the box centers for a single object.
    label = frames[0].laser_labels[object_index]
    if label.type != label_pb2.Label.Type.TYPE_VEHICLE:
      continue
    id = label.id
    points_global = []
    for i in range(1, len(frames)):
      for label in frames[i].laser_labels:
        if label.id == id:
          # Transform the point into global coordinates.
          transform = get_transform(i)
          point = tf.convert_to_tensor([label.box.center_x, label.box.center_y, label.box.center_z, 1.0], tf.float32)[tf.newaxis, :]
          point_global = tf.matmul(point, transform)[0, 0:3].numpy()
          points_global.append([point_global[0], point_global[1], point_global[2]])

    # Plot the box center points.
    points_global = tf.convert_to_tensor(points_global, tf.float32)
    figure.add_trace(
        go.Scatter3d(
            x=points_global[:, 0],
            y=points_global[:, 1],
            z=points_global[:, 2],
                        mode='markers',
                marker=dict(
                    size=4,
                ),
            )
        )

  figure.show()

Example output:

[ahsan155]

@scott-ettinger Thank you for the above code. It worked for me. Just one more issue, I also need heading and velocity of vehicles for motion prediction. does this transformation from local to global frame also apply to heading and velocity? If so, how would I apply pose to transform heading and velocity? There seems to be more than three values in the pose field.

ground truth trajectory (blue) and predicted trajectory (red)

I predicted trajectory with only transformed x,y coordinate. Is not transforming heading and velocity causing this mess?

[scott-ettinger]

The pose field as used in the code above is a standard 4x4 transformation matrix. You can use it to either transform a unit 3 dimensional vector representing the heading or you can use the individual rotation components from the transform to compute the rotation angle in 2D.