116 lines
3.9 KiB
Python
116 lines
3.9 KiB
Python
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}')
|