transform to v frame for vis

Author: KenC1014

GEMstack/onboard/perception/cone_detection_parking.py

@@ -234,6 +234,11 @@ def pose_to_matrix(pose):
     T[:3, 3] = np.array([x, y, z])
     return T
 
+def transform_lidar_points(lidar_points, transform):
+    ones_column = np.ones((lidar_points.shape[0], 1))
+    lidar_points_extended = np.hstack((lidar_points, ones_column))
+    lidar_points_transformed = ((transform @ (lidar_points_extended.T)).T)
+    return lidar_points_transformed[:, :3]
 
 def transform_points_l2c(lidar_points, T_l2c):
     N = lidar_points.shape[0]
@@ -308,8 +313,10 @@ def initialize(self):
         self.sync.registerCallback(self.synchronized_callback)
 
         # Publishers
+        self.pub_lidar_top_vehicle_pc2 = rospy.Publisher("point_cloud/lidar_top_vehicle", PointCloud2, queue_size=10)
         self.pub_cones_image_detection = rospy.Publisher("cones/image_detection", Image, queue_size=1)
         self.pub_cones_bboxes_markers = rospy.Publisher("cones/markers/bboxes", MarkerArray, queue_size=10)
+        self.pub_vehicle_marker = rospy.Publisher("vehicle/marker", MarkerArray, queue_size=10)
 
         # Detection model
         self.model_path = os.getcwd() + '/GEMstack/knowledge/detection/cone.pt'
@@ -342,19 +349,28 @@ def viz_object_states(self, cv_image, boxes):
             if agent.dimensions != None and agent.dimensions[0] != None and agent.dimensions[1] != None and agent.dimensions[2] != None:
                 cone_3d_dims.append(agent.dimensions)
 
+        # Transform top lidar pointclouds to vehicle frame for visualization
+        latest_lidar_vehicle = transform_lidar_points(self.latest_lidar, self.T_l2v)
+        ros_lidar_top_vehicle_pc2 = create_point_cloud(latest_lidar_vehicle, (255, 0, 0), "lidar_top")
+        self.pub_lidar_top_vehicle_pc2.publish(ros_lidar_top_vehicle_pc2)
+
         # Draw 2D bboxes
         for ind, bbox in enumerate(boxes):
             xywh = bbox.xywh[0].tolist()
             cv_image = vis_2d_bbox(cv_image, xywh, bbox)
         ros_img = self.bridge.cv2_to_imgmsg(cv_image, 'bgr8')
         self.pub_cones_image_detection.publish(ros_img)  
 
+        # Create vehicle marker
+        ros_vehicle_marker = create_bbox_marker([[0.0, 0.0, 0.0]], [[0.8, 0.5, 0.3]], (1.0, 0.0, 0.0, 1), "lidar_top")
+        self.pub_vehicle_marker.publish(ros_vehicle_marker)
         # Draw 3D cone centers and dimensions
         if len(cone_3d_centers) > 0 and len(cone_3d_dims) > 0:
-            # Create bbox markers from cone dimensions
+            # Delete previous markers
             ros_delete_bboxes_markers = delete_bbox_marker()
             self.pub_cones_bboxes_markers.publish(ros_delete_bboxes_markers)
-            ros_cones_bboxes_markers = create_bbox_marker(cone_3d_centers, cone_3d_dims, "lidar_top")
+            # Create bbox markers from cone dimensions
+            ros_cones_bboxes_markers = create_bbox_marker(cone_3d_centers, cone_3d_dims, (0.0, 1.0, 15, 0.2), "lidar_top")
             self.pub_cones_bboxes_markers.publish(ros_cones_bboxes_markers)
 
 
@@ -468,7 +484,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                 yaw=yaw,
                 pitch=pitch,
                 roll=roll,
-                frame=ObjectFrameEnum.START
+                frame=ObjectFrameEnum.CURRENT
             )
 
             existing_id = match_existing_cone(

GEMstack/onboard/perception/visualization_utils.py

@@ -41,13 +41,13 @@ def vis_2d_bbox(image, xywh, box):
     cv2.putText(image, label, (text_x, text_y - baseline), font, font_scale, font_color, text_thickness)
     return image
 
-def create_point_cloud(points, color=(255, 0, 0)):
+def create_point_cloud(points, color=(255, 0, 0), ref_frame="map"):
     """
     Converts a list of (x, y, z) points into a PointCloud2 message.
     """
     header = rospy.Header()
     header.stamp = rospy.Time.now()
-    header.frame_id = "os_sensor"  # Change to your TF frame
+    header.frame_id = ref_frame  # Change to your TF frame
 
     fields = [
         PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
@@ -64,7 +64,7 @@ def create_point_cloud(points, color=(255, 0, 0)):
 
     return pc2.create_cloud(header, fields, point_cloud_data)
 
-def create_bbox_marker(centroids, dimensions, ref_frame="map"):
+def create_bbox_marker(centroids, dimensions, color = (0.0, 1.0, 1.5, 0.2), ref_frame="map"):
     """
     Create 3D bbox markers from centroids and dimensions
     """
@@ -78,7 +78,7 @@ def create_bbox_marker(centroids, dimensions, ref_frame="map"):
         marker = Marker()
         marker.header.frame_id = ref_frame  # Reference frame
         marker.header.stamp = rospy.Time.now()
-        marker.ns = "bounding_boxes"
+        marker.ns = "markers"
         marker.id = i  # Unique ID for each marker
         marker.type = Marker.CUBE  # Cube for bounding box
         marker.action = Marker.ADD
@@ -108,10 +108,11 @@ def create_bbox_marker(centroids, dimensions, ref_frame="map"):
         marker.scale.z = d_z
 
         # Random colors for each bounding box
-        marker.color.r = 0.0  # Varying colors
-        marker.color.g = 1.0
-        marker.color.b = 1.5
-        marker.color.a = 0.2  # Transparency
+        r, g, b, a = color
+        marker.color.r = r  # Varying colors
+        marker.color.g = g
+        marker.color.b = b
+        marker.color.a = a  # Transparency
 
         marker.lifetime = rospy.Duration()  # Persistent
         marker_array.markers.append(marker)
@@ -125,7 +126,7 @@ def delete_bbox_marker():
     marker_array = MarkerArray()
     for i in range(15):
         marker = Marker()
-        marker.ns = "bounding_boxes"
+        marker.ns = "markers"
         marker.id = i
         marker.action = Marker.DELETE
         marker_array.markers.append(marker)