TractorVision初期移植

lib/alg/line_detection.py

@@ -0,0 +1,115 @@
+import cv2
+import numpy as np
+
+def detect_lines_in_depth_image(depth_image, roi,min_depth):
+    """
+    Detect lines in a depth image within a specified Region of Interest (ROI).
+
+    :param depth_image: The depth image in grayscale16 format.
+    :param roi: The Region of Interest (ROI) to limit the detection region within the depth image.
+    :return: The detected lines with coordinates relative to the original depth image.
+    """
+    lines_list = []
+    # Convert the depth image to a format suitable for line detection using OpenCV
+    # Scale the depth image from uint16 to uint8
+    # Extract the ROI from the depth image
+    x, y, w, h = roi
+    roi_image = depth_image[y:y+h, x:x+w]
+
+    min_dist = np.min(roi_image)
+    max_dist = np.max(roi_image)
+
+    print(f"ROI shape：{roi_image.shape}")
+    print(f"ROI深度范围：{min_dist} - {max_dist}")
+    # norm_img = cv2.normalize(roi_image, None, 0,255, cv2.NORM_MINMAX, cv2.CV_8U)
+    
+    min_dist = min_depth
+    # max_dist = 1800
+    norm_img = (roi_image-min_dist)/(max_dist-min_dist)
+    norm_img = norm_img * 255
+    norm_img = np.clip(norm_img,0,255)
+    norm_img = norm_img.astype(np.uint8)
+    
+#    cv2.imwrite('dbg_norm.png',norm_img)
+
+
+    edges = cv2.Canny(norm_img,15,30,apertureSize=3)
+#    cv2.imwrite('dbg_edges.png',edges)
+
+    # Use OpenCV's line detection algorithm (e.g., HoughLines or HoughLinesP) to detect lines within the specified ROI
+    lines = cv2.HoughLinesP(edges, 1, np.pi/180, 20, minLineLength=10, maxLineGap=100)
+
+    norm_depth_img = cv2.normalize(depth_image, None, 0,255, cv2.NORM_MINMAX, cv2.CV_8U)
+    colored_img = cv2.cvtColor(norm_depth_img,cv2.COLOR_GRAY2BGR)
+    # Adjust the line coordinates to be relative to the original depth image
+    if lines is not None:
+        for points in lines:
+            # Extracted points nested in the list
+            x1,y1,x2,y2=points[0]
+            x1=x1+x
+            y1=y1+y
+            x2=x2+x
+            y2=y2+y
+            # Draw the lines joing the points
+            # On the original image
+            cv2.line(colored_img,(x1,y1),(x2,y2),(0,255,0),2)
+            # Maintain a simples lookup list for points
+            lines_list.append([(x1,y1),(x2,y2)])
+            
+    # Save the result image
+    # cv2.imwrite('dbg_detectedLines.png',colored_img)
+    # Return the detected lines with adjusted coordinates
+    return lines_list
+
+def calculate_line_angle(line):
+    """
+    计算线条与水平线（x轴）之间的角度。
+
+    :param line: 线条的两个端点坐标，格式为 [(x1, y1), (x2, y2)]
+    :return: 线条与水平线之间的角度（以度为单位，范围为0-180度）
+    """
+    # 提取两个点的坐标
+    (x1, y1), (x2, y2) = line
+    
+    # 计算斜率，处理垂直线的情况
+    if x2 - x1 == 0:
+        return 90.0  # 垂直线
+    
+    # 计算斜率
+    slope = (y2 - y1) / (x2 - x1)
+    
+    # 计算角度（弧度）并转换为度
+    angle = np.arctan(slope) * 180 / np.pi
+    
+    # 确保角度为正（0-180度范围内）
+    if angle < 0:
+        angle += 180
+    #转换成与相机中线之间的角度
+     
+    return angle
+
+
+
+
+def calculate_distance_point_to_line(point, line):
+    """
+    计算一个点到一条线的距离。
+
+    :param point: 点的坐标，格式为 (x, y)
+    :param line: 线的两个端点坐标，格式为 [(x1, y1), (x2, y2)]
+    :return: 点到线的距离
+    """
+    (x0, y0) = point
+    (x1, y1), (x2, y2) = line
+    numerator = abs((y2 - y1) * x0 - (x2 - x1) * y0 + x2 * y1 - y2 * x1)
+    denominator = np.sqrt((y2 - y1) ** 2 + (x2 - x1) ** 2)
+    return (numerator / denominator)*2.5+5
+
+if __name__ == '__main__':
+    img = cv2.imread('dbg_233900303_z.png',cv2.IMREAD_UNCHANGED)
+    line_list = detect_lines_in_depth_image(img,(320,240,640,480),1100)
+    print(line_list)
+    for line in line_list:
+        angle = calculate_line_angle(line)
+        dist = calculate_distance_point_to_line((320,240),line)
+        print(f'angle={angle} dist={dist}')