测试距离估计成功

config/config.yaml

@@ -11,7 +11,7 @@ model:
     
   distance_estimation:
     focal_length: 1000  # 摄像头焦距
-    sensor_height: 5.6  # 摄像头传感器高度(mm)
+    sensor_height: 3000  # 摄像头传感器高度(mm)
     average_person_height: 1700  # 平均人身高(mm)
 
 api:

docs/传感器尺寸.txt

@@ -0,0 +1,84 @@
+def get_sensor_size_from_camera():
+    """从相机API获取传感器尺寸"""
+    try:
+        import v4l2
+        import fcntl
+        
+        # 打开相机设备
+        fd = open('/dev/video0', 'rb')
+        
+        # 获取相机信息
+        cp = v4l2.v4l2_capability()
+        fcntl.ioctl(fd, v4l2.VIDIOC_QUERYCAP, cp)
+        
+        # 获取相机格式
+        fmt = v4l2.v4l2_format()
+        fmt.type = v4l2.V4L2_BUF_TYPE_VIDEO_CAPTURE
+        fcntl.ioctl(fd, v4l2.VIDIOC_G_FMT, fmt)
+        
+        # 获取传感器信息
+        sensor_info = v4l2.v4l2_sensor_info()
+        fcntl.ioctl(fd, v4l2.VIDIOC_G_SENSOR_INFO, sensor_info)
+        
+        return {
+            'width_mm': sensor_info.physical_width / 1000,  # 转换为mm
+            'height_mm': sensor_info.physical_height / 1000
+        }
+    except Exception as e:
+        print(f"无法从相机API获取传感器尺寸: {e}")
+        return None
+
+def calibrate_sensor_size(real_object_size_mm, object_distance_mm):
+    """通过已知物体标定传感器尺寸"""
+    def capture_calibration_image():
+        # 捕获标定图像
+        cap = cv2.VideoCapture(0)
+        ret, frame = cap.read()
+        cap.release()
+        return frame
+    
+    def measure_object_pixels(frame):
+        # 测量物体在图像中的像素尺寸
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        edges = cv2.Canny(gray, 50, 150)
+        contours, _ = cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+        
+        if contours:
+            largest_contour = max(contours, key=cv2.contourArea)
+            x, y, w, h = cv2.boundingRect(largest_contour)
+            return h  # 返回物体高度（像素）
+        return None
+    
+    # 捕获标定图像
+    frame = capture_calibration_image()
+    if frame is None:
+        return None
+    
+    # 测量物体像素尺寸
+    object_pixels = measure_object_pixels(frame)
+    if object_pixels is None:
+        return None
+    
+    # 使用相似三角形原理计算传感器尺寸
+    frame_height = frame.shape[0]  # 图像高度（像素）
+    focal_length_mm = 35  # 假设焦距为35mm，需要根据实际相机参数调整
+    
+    sensor_height_mm = (real_object_size_mm * focal_length_mm) / (object_distance_mm * object_pixels / frame_height)
+    
+    return sensor_height_mm
+
+SENSOR_SIZES = {
+    '1/4"': {'width_mm': 3.2, 'height_mm': 2.4},
+    '1/3"': {'width_mm': 4.8, 'height_mm': 3.6},
+    '1/2.3"': {'width_mm': 6.17, 'height_mm': 4.55},
+    '1/1.8"': {'width_mm': 7.18, 'height_mm': 5.32},
+    '2/3"': {'width_mm': 8.8, 'height_mm': 6.6},
+    '1"': {'width_mm': 13.2, 'height_mm': 8.8},
+    '4/3"': {'width_mm': 17.3, 'height_mm': 13},
+    'APS-C': {'width_mm': 23.6, 'height_mm': 15.6},
+    'Full Frame': {'width_mm': 36, 'height_mm': 24}
+}
+
+def get_sensor_size(sensor_format):
+    """根据传感器格式获取尺寸"""
+    return SENSOR_SIZES.get(sensor_format, None)

src/distance_estimator.py

@@ -1,15 +1,177 @@
 import numpy as np
+import cv2
+import mediapipe as mp
 
 class DistanceEstimator:
-    def __init__(self, focal_length, sensor_height, avg_person_height):
-        self.focal_length = focal_length
-        self.sensor_height = sensor_height
-        self.avg_person_height = avg_person_height
+    def __init__(self, 
+                 focal_length_mm,      # 焦距(mm)
+                 sensor_width_mm,      # 传感器宽度(mm)
+                 sensor_height_mm,     # 传感器高度(mm)
+                 image_width_pixels,   # 图像宽度(像素)
+                 image_height_pixels,  # 图像高度(像素)
+                 camera_height_mm=1700,# 摄像头安装高度(mm)
+                 camera_tilt_angle=0   # 摄像头俯仰角(度)
+                 ):
+        # 相机参数
+        self.focal_length_mm = focal_length_mm
+        self.sensor_width_mm = sensor_width_mm
+        self.sensor_height_mm = sensor_height_mm
+        self.image_width_pixels = image_width_pixels
+        self.image_height_pixels = image_height_pixels
+        self.camera_height_mm = camera_height_mm
+        self.camera_tilt_angle = np.radians(camera_tilt_angle)
         
-    def estimate_distance(self, person_height_pixels, frame_height):
-        # 使用相似三角形原理估算距离
-        real_height = self.avg_person_height  # mm
-        sensor_height_pixels = frame_height
+        # 计算像素尺寸
+        self.pixel_size_mm = sensor_width_mm / image_width_pixels
         
-        distance = (real_height * self.focal_length) / (person_height_pixels * self.sensor_height / sensor_height_pixels)
-        return distance  # 返回距离(mm)
+        # 计算水平和垂直视场角
+        self.fov_horizontal = 2 * np.arctan(sensor_width_mm / (2 * focal_length_mm))
+        self.fov_vertical = 2 * np.arctan(sensor_height_mm / (2 * focal_length_mm))
+        
+        # 初始化人体关键点检测器
+        self.mp_pose = mp.solutions.pose
+        self.pose = self.mp_pose.Pose(
+            static_image_mode=False,
+            model_complexity=2,
+            min_detection_confidence=0.5,
+            min_tracking_confidence=0.5
+        )
+        
+        # 人体参数（统计平均值，单位：mm）
+        self.body_ratios = {
+            'eye_height_ratio': 0.936,    # 眼睛高度占总高度比例
+            'shoulder_width_ratio': 0.259, # 肩宽占身高比例
+            'hip_width_ratio': 0.191,      # 臀宽占身高比例
+            'avg_male_height': 1700,       # 成年男性平均身高
+            'avg_female_height': 1600      # 成年女性平均身高
+        }
+    
+    def estimate_distance_from_keypoints(self, frame):
+        """使用人体关键点进行距离估计"""
+        results = self.pose.process(cv2.cvtColor(frame, cv2.COLOR_BGR2RGB))
+        if not results.pose_landmarks:
+            return None, None
+        
+        landmarks = results.pose_landmarks.landmark
+        
+        # 获取关键点
+        left_shoulder = landmarks[self.mp_pose.PoseLandmark.LEFT_SHOULDER]
+        right_shoulder = landmarks[self.mp_pose.PoseLandmark.RIGHT_SHOULDER]
+        left_hip = landmarks[self.mp_pose.PoseLandmark.LEFT_HIP]
+        right_hip = landmarks[self.mp_pose.PoseLandmark.RIGHT_HIP]
+        
+        # 计算肩宽和臀宽（像素）
+        shoulder_width_px = np.sqrt(
+            (right_shoulder.x - left_shoulder.x)**2 * self.image_width_pixels**2 +
+            (right_shoulder.y - left_shoulder.y)**2 * self.image_height_pixels**2
+        )
+        
+        hip_width_px = np.sqrt(
+            (right_hip.x - left_hip.x)**2 * self.image_width_pixels**2 +
+            (right_hip.y - left_hip.y)**2 * self.image_height_pixels**2
+        )
+        
+        # 使用肩宽估算距离
+        shoulder_distance = (self.body_ratios['shoulder_width_ratio'] * 
+                           self.body_ratios['avg_male_height'] * 
+                           self.focal_length_mm) / (shoulder_width_px * self.pixel_size_mm)
+        
+        # 使用臀宽估算距离
+        hip_distance = (self.body_ratios['hip_width_ratio'] * 
+                       self.body_ratios['avg_male_height'] * 
+                       self.focal_length_mm) / (hip_width_px * self.pixel_size_mm)
+        
+        # 加权平均两个距离估计
+        estimated_distance = 0.6 * shoulder_distance + 0.4 * hip_distance
+        
+        # 计算置信度（基于关键点可见性）
+        confidence = (left_shoulder.visibility + right_shoulder.visibility + 
+                     left_hip.visibility + right_hip.visibility) / 4
+        
+        return estimated_distance, confidence
+    
+    def estimate_distance_from_bbox(self, bbox_height_px, estimated_height_mm=None):
+        """使用边界框进行距离估计"""
+        if estimated_height_mm is None:
+            estimated_height_mm = self.body_ratios['avg_male_height']
+            
+        # 使用相似三角形原理
+        distance = (estimated_height_mm * self.focal_length_mm) / (bbox_height_px * self.pixel_size_mm)
+        return distance
+    
+    def estimate_distance_from_ground_plane(self, feet_y_px):
+        """使用地平面投影进行距离估计"""
+        # 计算从图像底部到脚部位置的像素距离
+        bottom_to_feet_px = self.image_height_pixels - feet_y_px
+        
+        # 计算视角
+        angle_to_feet = self.camera_tilt_angle + (bottom_to_feet_px / self.image_height_pixels) * self.fov_vertical
+        
+        # 使用三角函数计算距离
+        distance = self.camera_height_mm / np.tan(angle_to_feet)
+        return distance
+    
+    def estimate_distance(self, frame, bbox):
+        """
+        综合多种方法估计距离
+        
+        Args:
+            frame: 输入图像帧
+            bbox: 人物边界框 (x1, y1, x2, y2)
+            
+        Returns:
+            distance_mm: 估计距离（毫米）
+            confidence: 置信度（0-1）
+        """
+        distances = []
+        weights = []
+        
+        # 1. 使用关键点估计
+        keypoint_distance, keypoint_conf = self.estimate_distance_from_keypoints(frame)
+        if keypoint_distance is not None:
+            distances.append(keypoint_distance)
+            weights.append(keypoint_conf * 0.4)  # 关键点方法权重0.4
+            
+        # 2. 使用边界框估计
+        bbox_height = bbox[3] - bbox[1]
+        bbox_distance = self.estimate_distance_from_bbox(bbox_height)
+        distances.append(bbox_distance)
+        weights.append(0.3)  # 边界框方法权重0.3
+        
+        # 3. 使用地平面投影估计
+        ground_distance = self.estimate_distance_from_ground_plane(bbox[3])
+        distances.append(ground_distance)
+        weights.append(0.3)  # 地平面投影方法权重0.3
+        
+        # 加权平均得到最终距离
+        weights = np.array(weights) / np.sum(weights)  # 归一化权重
+        final_distance = np.average(distances, weights=weights)
+        
+        # 计算置信度（基于估计值的一致性）
+        std_dev = np.std(distances)
+        mean_distance = np.mean(distances)
+        consistency = np.exp(-std_dev / mean_distance)  # 估计值越一致，置信度越高
+        
+        # 如果有关键点检测结果，将其纳入最终置信度
+        if keypoint_distance is not None:
+            final_confidence = (consistency + keypoint_conf) / 2
+        else:
+            final_confidence = consistency
+            
+        return final_distance, final_confidence
+        
+    def visualize_estimation(self, frame, bbox, distance_mm, confidence):
+        """可视化距离估计结果"""
+        # vis_frame = frame.copy()
+        vis_frame = frame 
+        x1, y1, x2, y2 = map(int, bbox)
+        
+        # 绘制边界框
+        cv2.rectangle(vis_frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+        
+        # 添加距离和置信度信息
+        text = f"Distance: {distance_mm/1000:.2f}m Conf: {confidence:.2f}"
+        cv2.putText(vis_frame, text, (x1, y1-10), 
+                   cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+        
+        return vis_frame

test.py

@@ -1,6 +1,6 @@
 import asyncio
 from tests.run_tests import run_all_tests
 
-
+# 测试生成程序
 if __name__ == "__main__":
     asyncio.run(run_all_tests()) 

tests/run_tests.py

@@ -9,8 +9,8 @@ async def run_all_tests():
     # print("\n1. 测试摄像头模块")
     # test_rtsp_camera()
     
-    print("\n2. 测试人物检测模块")
-    test_person_detector()
+    # print("\n2. 测试人物检测模块")
+    # test_person_detector()
     
     print("\n3. 测试距离估算模块")
     test_distance_estimator()

tests/test_camera.py

@@ -5,11 +5,11 @@ from src.camera_handler import RTSPCamera
 
 def test_rtsp_camera():
     # 使用本地测试视频或摄像头
-    camera = RTSPCamera(0)  # 使用本地摄像头进行测试
+    camera = RTSPCamera("rtsp://10.0.0.17:8554/camera_test/2")  # 使用本地摄像头进行测试
     
     print("测试摄像头启动...")
     camera.start()
-    time.sleep(2)  # 等待摄像头初始化
+    time.sleep(10)  # 等待摄像头初始化
     
     print("测试获取帧...")
     frame = camera.get_frame()

tests/test_distance_estimator.py

@@ -1,31 +1,73 @@
+import cv2
 import numpy as np
 from src.distance_estimator import DistanceEstimator
+from src.person_detector import PersonDetector
 
 def test_distance_estimator():
-    # 初始化距离估算器
-    focal_length = 35  # mm
-    sensor_height = 24  # mm
-    avg_person_height = 1700  # mm
-    
+    # 初始化距离估算器（使用实际相机参数）
     estimator = DistanceEstimator(
-        focal_length=focal_length,
-        sensor_height=sensor_height,
-        avg_person_height=avg_person_height
+        focal_length_mm=35,
+        sensor_width_mm=23.5,
+        sensor_height_mm=15.6,
+        image_width_pixels=1920,
+        image_height_pixels=1080,
+        camera_height_mm=1700,
+        camera_tilt_angle=15
     )
     
     print("测试距离估算...")
     
-    # 测试场景1：人物占据画面一半高度
-    frame_height = 1080
-    person_height_pixels = frame_height / 2
-    distance = estimator.estimate_distance(person_height_pixels, frame_height)
-    print(f"场景1 - 人物占据画面一半高度时的估算距离: {distance:.2f}mm")
-    assert distance > 0, "距离估算结果应该大于0"
+    # 读取测试图像
+    test_image = cv2.imread("tests/test_data/test_image.jpg")
+    assert test_image is not None, "无法加载测试图像"
+
+    # 初始化检测器
+    detector = PersonDetector("yolov8n.pt", conf_threshold=0.3)
+    persons, vis_path = detector.detect(test_image, save_visualization=True)
+
+    print("len(person)", len(persons))
+
+    for person in persons:
+        assert 'bbox' in person, "检测结果缺少bbox"
+        assert 'confidence' in person, "检测结果缺少confidence"
+        assert len(person['bbox']) == 4, "bbox格式不正确"
+        assert 0 <= person['confidence'] <= 1, "confidence值范围不正确"
+
+
     
-    # 测试场景2：人物占据画面四分之一高度
-    person_height_pixels = frame_height / 4
-    distance = estimator.estimate_distance(person_height_pixels, frame_height)
-    print(f"场景2 - 人物占据画面四分之一高度时的估算距离: {distance:.2f}mm")
-    assert distance > 0, "距离估算结果应该大于0"
+        # 测试边界框方法
+        bbox = person['bbox']  # 示例边界框
+        bbox_distance = estimator.estimate_distance_from_bbox(bbox[3] - bbox[1])
+        print(f"边界框方法估算距离: {bbox_distance:.2f}mm")
+        assert bbox_distance > 0, "距离估算结果应该大于0"
+        
+        # 测试关键点方法
+        keypoint_distance, keypoint_conf = estimator.estimate_distance_from_keypoints(test_image)
+        if keypoint_distance is not None:
+            print(f"关键点方法估算距离: {keypoint_distance:.2f}mm, 置信度: {keypoint_conf:.2f}")
+            assert keypoint_distance > 0, "距离估算结果应该大于0"
+            assert 0 <= keypoint_conf <= 1, "置信度应该在0-1之间"
+        
+        # 测试地平面投影方法
+        ground_distance = estimator.estimate_distance_from_ground_plane(bbox[3])
+        print(f"地平面投影方法估算距离: {ground_distance:.2f}mm")
+        assert ground_distance > 0, "距离估算结果应该大于0"
+        
+        # 测试综合估计方法
+        final_distance, final_confidence = estimator.estimate_distance(test_image, bbox)
+        print(f"综合估计距离: {final_distance:.2f}mm, 置信度: {final_confidence:.2f}")
+        assert final_distance > 0, "距离估算结果应该大于0"
+        assert 0 <= final_confidence <= 1, "置信度应该在0-1之间"
+        
+        # 测试可视化
+        vis_image = estimator.visualize_estimation(test_image, bbox, final_distance, final_confidence)
+        assert vis_image is not None, "可视化结果不应为空"
+        assert vis_image.shape == test_image.shape, "可视化图像尺寸应与原图相同"
+        
+        # 保存可视化结果
+    cv2.imwrite("tests/test_data/distance_estimation_result.jpg", vis_image)
     
-    print("距离估算测试完成!")
+    print("距离估算测试完成!")
+
+if __name__ == "__main__":
+    test_distance_estimator()