find valid parking lot

Author: leizhenyu-lzy

.gitignore

@@ -176,4 +176,5 @@ cuda/
 
 **/rosbag/*
 **/logs/*
+**/*.bag
 

Fusion/ClickPoints.py

@@ -0,0 +1,59 @@
+import os
+import cv2
+import time
+import numpy as np
+
+
+def clickPoints(imgPath=None, numPoints=4):
+    clicked_pts = []
+
+    def mouse_callback(event, x, y, flags, param):
+        if event == cv2.EVENT_LBUTTONDOWN and len(param) < numPoints:
+            param.append((x, y))
+            print(f"点击第 {len(param)} 个点: ({x}, {y})")
+
+    # 初始化图像
+    if imgPath:
+        img = cv2.imread(imgPath, flags=cv2.IMREAD_COLOR)
+    else:
+        img = np.ones((600, 800, 3), dtype=np.uint8) * 255  # 白色背景
+
+    cv2.namedWindow("Click Points")
+    cv2.setMouseCallback("Click Points", mouse_callback, clicked_pts)
+
+    while True:
+        img_copy = img.copy()
+
+        # 画点击点
+        for pt in clicked_pts:
+            cv2.circle(img_copy, pt, 5, (0, 0, 255), -1)
+
+        if len(clicked_pts) == numPoints:
+            cv2.polylines(img_copy, [np.array(clicked_pts)], isClosed=True, color=(0, 255, 0), thickness=2)
+
+        cv2.imshow("Click Points", img_copy)
+
+        key = cv2.waitKey(10)
+        if key == 27 or key == ord("q"):  # ESC
+            break
+        elif key == ord('r'):
+            clicked_pts.clear()
+        
+    cv2.destroyAllWindows()
+
+    # 输出
+    if len(clicked_pts) == numPoints:
+        print("\n最终点击的坐标：")
+        for i, pt in enumerate(clicked_pts):
+            print(f"点 {i+1}: {pt}")
+
+    return np.array(clicked_pts, dtype=np.float32)
+
+
+if __name__ == "__main__":
+    imgPath = os.path.join(
+        os.path.dirname(__file__),
+        r"./Pics/FrontCam.png"
+    )
+    
+    clickPoints(imgPath)

Fusion/FindRectangle.py

@@ -0,0 +1,222 @@
+import cv2
+import os
+import numpy as np
+
+from ClickPoints import clickPoints
+
+
+
+GEM_E4_LENGTH = 3.6  # m
+GEM_E4_WIDTH  = 1.5  # m
+
+
+
+
+
+
+def judgeRectInPolygon(rectPts:np.ndarray, polygonPts:np.ndarray):
+    polygon = polygonPts.reshape((-1, 1, 2)).astype(np.float32)
+    for pt in rectPts:
+        inside = cv2.pointPolygonTest(polygon, tuple(pt), measureDist=False)
+        if inside < 0:
+            return False
+    return True
+
+
+def findAllCandidateParkingLot(cornerPts, angleStepDegree=5, positionStrideMeter=0.5):
+    
+    cornerPts = np.array(cornerPts, dtype=np.float32)
+    
+    # 先得到 bounding box 范围（缩小一些，避免跑出区域）
+    min_x = np.min(cornerPts[:, 0]) + 0.5
+    max_x = np.max(cornerPts[:, 0]) - 0.5
+    min_y = np.min(cornerPts[:, 1]) + 0.5
+    max_y = np.max(cornerPts[:, 1]) - 0.5
+    
+    candidates = []
+    
+    for angleDegree in np.arange(-90, 90, angleStepDegree):
+        rect = ((0, 0), (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree))
+        carBox = cv2.boxPoints(rect)  # 车体角点，以原点为中心
+        
+        for cx in np.arange(min_x, max_x, positionStrideMeter):
+            for cy in np.arange(min_y, max_y, positionStrideMeter):
+                car_box_shifted = carBox + np.array([cx, cy])
+                
+                if judgeRectInPolygon(car_box_shifted, cornerPts):
+                    candidates.append((cx, cy, angleDegree))
+    
+    return candidates
+
+
+def drawCarPose(img, center, angleDegree, color=(0, 0, 255), scale=100):
+    rect = (center, (GEM_E4_LENGTH, GEM_E4_WIDTH), float(angleDegree))
+    box = cv2.boxPoints(rect)  # 四个角点
+    box_scaled = np.int32(box * scale)
+
+    cv2.polylines(img, [box_scaled], isClosed=True, color=color, thickness=2)
+
+
+def visualizeCandidateCarPoses(cornerPts, candidates, scale=100):
+    img = np.zeros((1000, 1000, 3), dtype=np.uint8)
+
+    # 画停车位边框
+    parking_polygon = np.int32(cornerPts * scale)
+    cv2.polylines(img, [parking_polygon], isClosed=True, color=(0, 255, 0), thickness=2)
+
+    # 画所有合法停车姿态
+    for (x, y, angle) in candidates:
+        drawCarPose(img, (x, y), angle, color=(100, 100, 255), scale=scale)
+
+    cv2.imshow("Parking Candidates", img)
+    cv2.waitKey(0)
+    cv2.destroyAllWindows()
+
+
+if __name__ == "__main__":
+    # cornerPts = np.array([
+    #     [8,9],
+    #     [4,5],
+    #     [1,15],
+    #     [5,9],
+    # ]).astype(np.float32)  # any order
+    
+    cornerPts = np.random.randint(
+        low=1,
+        high=10,
+        size=(4,2)
+    ).astype(np.float32)
+    
+    cornerPts = clickPoints(
+        imgPath = os.path.join(
+            os.path.dirname(__file__),
+            r"./Pics/FrontCam.png"
+        )
+    )/100
+    
+    print(f"\
+        cornerPts = np.array([\n\
+            [{cornerPts[0,0]},{cornerPts[0,1]}],\n\
+            [{cornerPts[1,0]},{cornerPts[1,1]}],\n\
+            [{cornerPts[2,0]},{cornerPts[2,1]}],\n\
+            [{cornerPts[3,0]},{cornerPts[3,1]}],\n\
+        ]).astype(np.float32)  # any order \
+    ")
+    
+    candidates = findAllCandidateParkingLot(cornerPts)
+    
+    print(f"find {len(candidates)} candidates")
+    for idx, pose in enumerate(candidates[::10]):
+        thetaDegree = -(90 - (-pose[2])) if abs(- (90 - (-pose[2]))) < 90 else 180 + (-(90 - (-pose[2])))
+        print(f"Candidate {idx}, Pose: centerXY:({pose[0]}, {pose[1]}), thetaDegree:{thetaDegree}")
+        # OpenCV (rotation is from the lowest point, range [-90, 0])
+        # counter-clock-wise is negative
+        # clock-wise is positive
+
+
+    visualizeCandidateCarPoses(cornerPts, candidates[::10])
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+# def checkFitWithMinAreaRect(pts:np.ndarray, rectLongMin=GEM_E4_LENGTH, rectShortMin=GEM_E4_WIDTH):
+    
+#     def getRotatedRectBox(center, size, angleDegree):
+#         """
+#         给定中心坐标、尺寸和角度，返回矩形4个顶点
+#         center: (x, y)
+#         size: (width, height)
+#         angleDegree: 逆时针旋转角度（OpenCV风格）
+#         """
+#         rect = (center, size, angleDegree)
+#         box = cv2.boxPoints(rect)  # 得到4个点
+#         return box  # 转换为整数像素点或保留float点
+    
+#     # 外接矩形
+#     outRect = cv2.minAreaRect(pts)
+#     (centerX, centerY), (width, length), rotation = outRect
+#     print(f"rotation: {rotation}")
+#     # OpenCV 返回的 rotation 单位是 角度，范围为 [-90°, 0°]
+#     # -90°	矩形长边在 Y 轴方向
+#     # 0°	长边在 X 轴方向
+
+#     SwitchOrder = False
+#     if width > length:
+#         outBoxLong, outBoxShort = width, length
+#     else:
+#         outBoxLong, outBoxShort = length, width
+#         SwitchOrder = True
+#         print("SwitchOrder")
+
+#     canFit = True
+#     if SwitchOrder is True:
+#         rotation += 90
+    
+
+#     # judge fitting
+#     if outBoxLong < rectLongMin:
+#         print(f"CAN NOT FIT : outBoxLong{outBoxLong} < rectLongMin{rectLongMin}")
+#         canFit = False
+#     if outBoxShort < rectShortMin:
+#         print(f"CAN NOT FIT : outBoxShort{outBoxShort} < rectShortMin{rectShortMin}")
+#         canFit = False
+
+#     centerXY = (centerX, centerY)
+    
+    
+#     outerBoxPts = getRotatedRectBox(centerXY, (outBoxLong, outBoxShort), rotation)
+#     innerBoxPts = getRotatedRectBox(centerXY, (rectLongMin, rectShortMin), rotation)
+    
+#     return canFit, centerXY, rotation, innerBoxPts, outerBoxPts
+
+
+# if __name__ == "__main__":
+    # cornerPts = np.array([
+    #     [8,10],
+    #     [3,10],
+    #     [5,5],
+    #     [1,5],
+
+    # ]).astype(np.float32)  # any order
+
+    # cornerPts = np.random.randint(
+    #     low=1,
+    #     high=10,
+    #     size=(4,2)
+    # ).astype(np.float32)
+    
+    # print(cornerPts)
+
+
+    # canFit, center, rotation, boxInner, boxOuter = checkFitWithMinAreaRect(cornerPts)
+    
+    # print(canFit, rotation)
+
+    # img = np.zeros((600, 600, 3), dtype=np.uint8)
+    # ptsInt = np.int32(cornerPts * 40)  # 放大方便看
+    # boxIntInner = np.int32(boxInner * 40)
+    # boxIntOuter = np.int32(boxOuter * 40)
+
+    # cv2.polylines(img, [ptsInt], isClosed=True, color=(0,255,0), thickness=2)
+    # cv2.polylines(img, [boxIntInner], isClosed=True, color=(255,0,0), thickness=2)
+    # cv2.polylines(img, [boxIntOuter], isClosed=True, color=(0,0,255), thickness=2)
+
+    # cv2.imshow("fit check", img)
+    # cv2.waitKey(0)
+    # cv2.destroyAllWindows()
+    
+    

Fusion/Pics/FrontCam.png

@@ -0,0 +1,61 @@
+import rospy
+import cv2
+import numpy as np
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, PointCloud2
+from std_msgs.msg import Header
+import sensor_msgs.point_cloud2 as pc2
+
+
+from stitch import  cvtImg2PointCloudMsg, maskCamKeyArea, \
+                    PerspectiveTransform, \
+                    srcFrontCamListXY, dstFrontCamListXY
+
+
+class SubImagePubPointCloudNode:
+    def __init__(self):
+        rospy.init_node("image_to_pointcloud_node")
+
+        self.bridge = CvBridge()
+        self.image_sub = rospy.Subscriber("/oak/rgb/image_raw", Image, self.image_callback, queue_size=1)
+        self.pc_pub = rospy.Publisher("/image_pointcloud", PointCloud2, queue_size=1)
+
+        self.latest_msg = None
+
+        # 相机参数
+        self.src_pts = srcFrontCamListXY
+        self.dst_pts = dstFrontCamListXY
+
+        rospy.loginfo("Image to PointCloud2 node started.")
+
+    def image_callback(self, msg):
+        try:
+            img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        except Exception as e:
+            rospy.logwarn(f"CV Bridge error: {e}")
+            return
+
+        masked_img = maskCamKeyArea(img, "FRONT")
+        transformer = PerspectiveTransform(masked_img, self.src_pts.copy(), self.dst_pts.copy())
+        transformed_img = transformer.getTransformedImg()
+
+        pc_msg = cvtImg2PointCloudMsg(
+            transformed_img, 
+            frameId="base_link", 
+            pixelSize=0.02, 
+            stride=6,
+            stamp = msg.header.stamp
+        )
+        self.pc_pub.publish(pc_msg)
+
+
+
+if __name__ == "__main__":
+    try:
+        node = SubImagePubPointCloudNode()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        pass
+
+
+# TODO : try down-sample

Fusion/SubImagePubPointCloudNode.py

@@ -0,0 +1,14 @@
+roscore
+
+rosbag play -l ~/Projects/GEMstack/logs/three_02_05_25/vehicle.bag --rate=0.2
+
+rosbag play -l ~/Projects/GEMstack/logs/parking/FirstParking.bag
+
+
+roslaunch fusion sensors.launch 
+
+python3 SubImagePubPointCloudNode.py  # publish image2pointcloud topic
+
+
+
+

Fusion/TestStitchWithRosbag.py

@@ -0,0 +1,30 @@
+import rospy
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+from readImageFromRosbag import readImageFromRosbag  # 你已有的函数
+
+
+def publish_image_loop(image, topic="/oak/rgb/image_raw", frame_id="oak_camera", rate_hz=10):
+    rospy.init_node("manual_image_publisher", anonymous=True)
+    bridge = CvBridge()
+    pub = rospy.Publisher(topic, Image, queue_size=1)
+    
+    msg = bridge.cv2_to_imgmsg(image, encoding="bgr8")
+    msg.header.frame_id = frame_id
+
+    rate = rospy.Rate(rate_hz)
+    while not rospy.is_shutdown():
+        msg.header.stamp = rospy.Time.now()
+        pub.publish(msg)
+        rate.sleep()
+
+if __name__ == "__main__":
+    bag_path = "/your/path/front.bag"
+    image_topic = "/oak/rgb/image_raw"
+    
+    # 从 bag 中读取一张图
+    image = readImageFromRosbag(bag_path, imageTopic=image_topic)
+    
+    # 循环持续发布该图像
+    publish_image_loop(image, topic=image_topic, frame_id="oak_camera", rate_hz=10)