add vectorization

Author: KenC1014

GEMstack/onboard/perception/spot_corner_detection.py

@@ -82,18 +82,16 @@ def callback(self, right_cam_msg):
         # Now transform corners to 3D vehicle frame
         if len(corners) > 0:
             for corners_four in corners:
-                corners_four_vehicle_frame = []
-                for c in corners_four:
-                    c_vehicle_frame = fr_cam_2d_to_vehicle_3d(c)
-                    corners_four_vehicle_frame.append(c_vehicle_frame)
+                corners_four_vehicle_frame = fr_cam_2d_to_vehicle_3d(corners_four)
                 corners_3d_vehicle_frame.append(corners_four_vehicle_frame)
 
+        print(f"corners_3d_vehicle_frame: {corners_3d_vehicle_frame}")
         # Store the parking spots corners in vehicle frame
         self.parking_spots_corners = corners_3d_vehicle_frame
 
         # Visualize
         if self.visualization:
-            self.visualize(image_annotated, centers, corners, approxes, corners_3d_vehicle_frame)
+            self.visualize(image_annotated, centers, corners, approxes)
 
 
     # All local helper functions
@@ -118,7 +116,7 @@ def process_image_msg(self, image_msg):
         return cv_image
 
 
-    def visualize(self, image, centers, corners, approxes, corners_3d_vehicle_frame):
+    def visualize(self, image, centers, corners, approxes):
         if len(corners) > 0:
             # Draw centers as red crosses
             for (cx, cy) in centers:

GEMstack/onboard/perception/utils/detection_utils.py

@@ -81,32 +81,35 @@ def cvtFootInch2Meter(ft, inch=0.0):
     [cvtFootInch2Meter(0, 140+30), cvtFootInch2Meter(0, 0   -5)-1.46+0.151],  # RR
 ])
 
-def cvtOriginImgPixel2DToVehicleFrameMeter3D(TransMat, pixelPointXY):
-    """
-    pixelPointXY: (u, v) in original image (pixel)
-    TransMat: 3x3 perspective transform matrix (pixel -> real world meter)
-    """
-    BASE_VEHICLE_DIST = 1.10 
-    
-    point = np.array([pixelPointXY[0], pixelPointXY[1], 1.0], dtype=np.float32).reshape(3, 1)
-    
-    transformed = TransMat @ point
-    transformed /= transformed[2, 0]  
+def cvtOriginImgPixels2DToVehicleFrameMeter2D(TransMat, pixelPointXYs):
+    BASE_VEHICLE_DIST = 1.10  # meter
+
+    # Add homogeneous coordinate: [u, v, 1]
+    N = pixelPointXYs.shape[0]
+    homogeneous_points = np.hstack([pixelPointXYs, np.ones((N, 1), dtype=np.float32)])  # (N, 3)
+
+    # Apply perspective transform
+    transformed = (TransMat @ homogeneous_points.T).T  # (N, 3)
+
+    # Normalize (divide by last coordinate)
+    transformed /= transformed[:, [2]]
 
-    x_real = transformed[0, 0] 
-    y_real = transformed[1, 0] 
-    z_real = 0.0 
+    x_real = transformed[:, 0]
+    y_real = transformed[:, 1]
+    z_real = np.zeros_like(x_real)
 
+    # Convert to vehicle pointcloud coordinate
     x_pc = -y_real
     y_pc = -x_real
     z_pc = z_real
 
-    return np.array([x_pc + BASE_VEHICLE_DIST, y_pc, z_pc], dtype=np.float32)
+    # Return: x + front base distance offset
+    return np.stack([x_pc + BASE_VEHICLE_DIST, y_pc, z_pc], axis=1)
 
 
-def fr_cam_2d_to_vehicle_3d(fr_2d_pt):
+def fr_cam_2d_to_vehicle_3d(fr_2d_pts):
     TransMat = cv2.getPerspectiveTransform( np.float32(pixelFrontRightCamListXY), 
                                             np.float32(worldFrontRightCamListXY))
 
-    result = cvtOriginImgPixel2DToVehicleFrameMeter3D(TransMat, fr_2d_pt)
+    result = cvtOriginImgPixels2DToVehicleFrameMeter2D(TransMat, fr_2d_pts)
     return result.tolist()