Correct orientation error

Author: rty727433198

GEMstack/onboard/perception/cone_detection.py

@@ -110,7 +110,7 @@ def initialize(self):
         self.lidar_sub = Subscriber('/ouster/points', PointCloud2)
         self.sync = ApproximateTimeSynchronizer([
             self.rgb_sub, self.lidar_sub
-        ], queue_size=500, slop=0.05)
+        ], queue_size=500, slop=0.03)
         self.sync.registerCallback(self.synchronized_callback)
 
         # Initialize the YOLO detector
@@ -185,6 +185,8 @@ def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
             undistorted_img = latest_image.copy()
             orig_H, orig_W = latest_image.shape[:2]
             self.current_K = self.K
+            # print(self.K)
+            # print(self.T_l2c)
         results_normal = self.detector(img_normal, conf=0.35, classes=[0])
         combined_boxes = []
         if not self.enable_tracking:
@@ -198,14 +200,18 @@ def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
             boxes_right = np.array(results_right[0].boxes.xywh.cpu()) if len(results_right) > 0 else []
             for box in boxes_left:
                 cx, cy, w, h = box
-                new_cx = cy
-                new_cy = orig_W - 1 - cx
-                combined_boxes.append((new_cx, new_cy, h, w, ObstacleStateEnum.RIGHT))
+                new_cx = orig_W - 1 - cy
+                new_cy = cx
+                new_w = h  # Swap width and height.
+                new_h = w
+                combined_boxes.append((new_cx, new_cy, new_w, new_h, ObstacleStateEnum.RIGHT))
             for box in boxes_right:
                 cx, cy, w, h = box
-                new_cx = orig_H - 1 - cy
-                new_cy = cx
-                combined_boxes.append((new_cx, new_cy, h, w, ObstacleStateEnum.LEFT))
+                new_cx = cy
+                new_cy = orig_H - 1 - cx
+                new_w = h  # Swap width and height.
+                new_h = w
+                combined_boxes.append((new_cx, new_cy, new_w, new_h, ObstacleStateEnum.LEFT))
 
         boxes_normal = np.array(results_normal[0].boxes.xywh.cpu()) if len(results_normal) > 0 else []
         for box in boxes_normal:
@@ -254,17 +260,20 @@ def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
 
         for i, box_info in enumerate(combined_boxes):
             cx, cy, w, h, activity = box_info
+            # print(cx, cy, w, h)
             left = int(cx - w / 1.6)
             right = int(cx + w / 1.6)
             top = int(cy - h / 2)
             bottom = int(cy + h / 2)
             mask = (projected_pts[:, 0] >= left) & (projected_pts[:, 0] <= right) & \
                    (projected_pts[:, 1] >= top) & (projected_pts[:, 1] <= bottom)
             roi_pts = projected_pts[mask]
+            # print(roi_pts)
             if roi_pts.shape[0] < 5:
                 continue
 
             points_3d = roi_pts[:, 2:5]
+
             points_3d = filter_points_within_threshold(points_3d, 40)
             points_3d = remove_ground_by_min_range(points_3d, z_range=0.08)
             points_3d = filter_depth_points(points_3d, max_depth_diff=0.5)

GEMstack/onboard/perception/pedestrian_detection.py

@@ -96,7 +96,7 @@ def initialize(self):
         self.rgb_sub = Subscriber(rgb_topic, Image)
         self.lidar_sub = Subscriber('/ouster/points', PointCloud2)
         self.sync = ApproximateTimeSynchronizer(
-            [self.rgb_sub, self.lidar_sub], queue_size=500, slop=0.05
+            [self.rgb_sub, self.lidar_sub], queue_size=500, slop=0.025
         )
         self.sync.registerCallback(self.synchronized_callback)
 
@@ -163,6 +163,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
 
         agents: Dict[str, AgentState] = {}
         for (cx, cy, w, h) in boxes2d:
+            # print(cx, cy, w, h)
             # Define ROI in image for each detection
             left = int(cx - w / 2)
             right = int(cx + w / 2)
@@ -257,6 +258,7 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                     )
                     agents[existing] = updated
                     self.tracked_agents[existing] = updated
+
                 else:
                     aid = f"Pedestrian{self.pedestrian_counter}"
                     self.pedestrian_counter += 1
@@ -271,6 +273,15 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
                     )
                     agents[aid] = new_agent
                     self.tracked_agents[aid] = new_agent
+                for agent_id, agent in self.current_agents.items():
+                    p = agent.pose
+                    rospy.loginfo(
+                        f"Agent ID: {agent_id}\n"
+                        f"Pose: (x: {p.x:.3f}, y: {p.y:.3f}, z: {p.z:.3f}, "
+                        f"yaw: {p.yaw:.3f}, pitch: {p.pitch:.3f}, roll: {p.roll:.3f})\n"
+                        f"Velocity: (vx: {agent.velocity[0]:.3f}, vy: {agent.velocity[1]:.3f}, vz: {agent.velocity[2]:.3f})\n"
+                        f"type:{agent.activity}"
+                    )
             else:
                 aid = f"Pedestrian{self.pedestrian_counter}"
                 self.pedestrian_counter += 1