feat: update algorithm integrations and point cloud overlays

alg/train_measurement_system_v3.2_0629/src/measurements/__init__.py

@@ -0,0 +1,7 @@
+"""measurements包初始化"""
+from .base import BaseMeasurement, MeasurementResult
+from .plane_distance import PlaneToPlaneDistance
+from .point_to_plane import PointToPlaneDistance
+from .point_to_point import PointToPointDistance
+
+__all__ = ['BaseMeasurement', 'MeasurementResult', 'PlaneToPlaneDistance', 'PointToPlaneDistance', 'PointToPointDistance']

alg/train_measurement_system_v3.2_0629/src/measurements/base.py

@@ -0,0 +1,49 @@
+"""测量任务基类"""
+from abc import ABC, abstractmethod
+from typing import Dict, Any, Optional
+from dataclasses import dataclass
+import numpy as np
+
+
+@dataclass
+class MeasurementResult:
+    """测量结果数据类"""
+    task_name: str
+    task_type: str
+    success: bool
+    data: Optional[Dict[str, Any]] = None
+    error_message: Optional[str] = None
+
+
+class BaseMeasurement(ABC):
+    """测量任务基类"""
+
+    def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any], spatial_index=None):
+        """
+        初始化测量任务
+
+        Args:
+            name: 任务名称
+            point_cloud: 完整点云数组 (N, 3)
+            config: 任务配置字典
+            spatial_index: 空间索引对象（可选）
+        """
+        self.name = name
+        self.point_cloud = point_cloud
+        self.config = config
+        self.task_type = config.get('type', 'unknown')
+        self.spatial_index = spatial_index  # 保存空间索引
+
+    @abstractmethod
+    def execute(self) -> MeasurementResult:
+        """
+        执行测量任务
+
+        Returns:
+            MeasurementResult: 测量结果
+        """
+        pass
+
+    def is_enabled(self) -> bool:
+        """检查任务是否启用"""
+        return self.config.get('enabled', True)

alg/train_measurement_system_v3.2_0629/src/measurements/line_to_line.py

@@ -0,0 +1,280 @@
+"""线到线距离测量"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+# 添加父目录到路径
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from edge_line_roi_extractor import EdgeLineROIExtractor
+from line_fitter import LineFitter
+from visualizer import Visualizer
+
+
+class LineToLineDistance(BaseMeasurement):
+    """测量两条边缘线之间的距离"""
+
+    def _log(self, msg: str):
+        """同时输出到控制台和日志文件"""
+        print(msg)
+        if hasattr(self, 'detail_logger') and self.detail_logger:
+            self.detail_logger.log(msg, also_print=False)
+
+    def execute(self) -> MeasurementResult:
+        """执行线到线距离测量"""
+        try:
+            timing_breakdown = {}
+
+            # 获取logger
+            logger = getattr(self, 'detail_logger', None)
+
+            # 获取参数
+            params = self.config.get('params', {})
+            source_roi = self.config.get('source_roi', {})
+            target_roi = self.config.get('target_roi', {})
+
+            # 1. 提取源边缘线ROI（两步法）
+            msg = f"\n  步骤1: 提取源边缘线ROI"
+            if logger: logger.log(msg)
+            print(msg)
+            start_time = time.time()
+
+            # 获取线的端点（优先使用line_start/line_end，否则从bbox推导）
+            if 'line_start' in source_roi and 'line_end' in source_roi:
+                source_p1 = np.array(source_roi.get('line_start'))
+                source_p2 = np.array(source_roi.get('line_end'))
+            else:
+                # 从bbox推导线的端点
+                source_bbox_min = np.array(source_roi.get('bbox_min'))
+                source_bbox_max = np.array(source_roi.get('bbox_max'))
+                source_p1 = source_bbox_min
+                source_p2 = source_bbox_max
+
+            msg = f"    线起点: [{source_p1[0]:.2f}, {source_p1[1]:.2f}, {source_p1[2]:.2f}]"
+            if logger: logger.log(msg)
+            print(msg)
+            msg = f"    线终点: [{source_p2[0]:.2f}, {source_p2[1]:.2f}, {source_p2[2]:.2f}]"
+            if logger: logger.log(msg)
+            print(msg)
+
+            source_points, source_indices, source_stats = EdgeLineROIExtractor.extract_two_step_roi(
+                self.point_cloud,
+                source_p1,
+                source_p2,
+                self.spatial_index,
+                distance_threshold=params.get('roi_distance_threshold', 2.0),
+                bbox_margin=5.0
+            )
+            timing_breakdown['提取源线ROI'] = time.time() - start_time
+
+            self._log(f"    粗ROI bbox: [{source_stats['bbox_min'][0]:.2f}, {source_stats['bbox_min'][1]:.2f}, {source_stats['bbox_min'][2]:.2f}] -> [{source_stats['bbox_max'][0]:.2f}, {source_stats['bbox_max'][1]:.2f}, {source_stats['bbox_max'][2]:.2f}]")
+            self._log(f"    粗筛选: {source_stats['coarse_count']:,} 点")
+            self._log(f"    精筛选(距离<{source_stats['distance_threshold']}mm): {source_stats['fine_count']:,} 点")
+            self._log(f"    耗时: {timing_breakdown['提取源线ROI']:.2f}秒")
+
+            if len(source_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="源边缘线ROI区域内没有点"
+                )
+
+            # 2. 提取目标边缘线ROI（两步法）
+            self._log(f"\n  步骤2: 提取目标边缘线ROI")
+            start_time = time.time()
+
+            # 获取线的端点
+            if 'line_start' in target_roi and 'line_end' in target_roi:
+                target_p1 = np.array(target_roi.get('line_start'))
+                target_p2 = np.array(target_roi.get('line_end'))
+            else:
+                target_bbox_min = np.array(target_roi.get('bbox_min'))
+                target_bbox_max = np.array(target_roi.get('bbox_max'))
+                target_p1 = target_bbox_min
+                target_p2 = target_bbox_max
+
+            self._log(f"    线起点: [{target_p1[0]:.2f}, {target_p1[1]:.2f}, {target_p1[2]:.2f}]")
+            self._log(f"    线终点: [{target_p2[0]:.2f}, {target_p2[1]:.2f}, {target_p2[2]:.2f}]")
+
+            target_points, target_indices, target_stats = EdgeLineROIExtractor.extract_two_step_roi(
+                self.point_cloud,
+                target_p1,
+                target_p2,
+                self.spatial_index,
+                distance_threshold=params.get('roi_distance_threshold', 2.0),
+                bbox_margin=5.0
+            )
+            timing_breakdown['提取目标线ROI'] = time.time() - start_time
+
+            self._log(f"    粗ROI bbox: [{target_stats['bbox_min'][0]:.2f}, {target_stats['bbox_min'][1]:.2f}, {target_stats['bbox_min'][2]:.2f}] -> [{target_stats['bbox_max'][0]:.2f}, {target_stats['bbox_max'][1]:.2f}, {target_stats['bbox_max'][2]:.2f}]")
+            self._log(f"    粗筛选: {target_stats['coarse_count']:,} 点")
+            self._log(f"    精筛选(距离<{target_stats['distance_threshold']}mm): {target_stats['fine_count']:,} 点")
+            self._log(f"    耗时: {timing_breakdown['提取目标线ROI']:.2f}秒")
+
+            if len(target_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="目标边缘线ROI区域内没有点"
+                )
+
+            # 3. 拟合源边缘线（用全部精筛选点）
+            self._log(f"\n  步骤3: 拟合源边缘线")
+            start_time = time.time()
+            self._log(f"    输入点数: {len(source_points)} 个点")
+            source_fitter = LineFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            source_line = source_fitter.fit(source_points)
+            timing_breakdown['源线RANSAC拟合'] = time.time() - start_time
+
+            if source_line is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"源边缘线拟合失败（点数: {len(source_points)}，要求内点比例>={params.get('min_inlier_ratio', 0.6)*100:.0f}%）"
+                )
+
+            self._log(f"    RANSAC参数: 阈值={params.get('ransac_threshold', 2.0)}mm, 迭代={params.get('ransac_max_iterations', 1000)}次, 最小内点比例={params.get('min_inlier_ratio', 0.6)*100:.0f}%")
+            self._log(f"    拟合结果: 内点{len(source_line.inliers)}/{len(source_points)} ({source_line.inlier_ratio*100:.1f}%)")
+            self._log(f"    线上一点: [{source_line.point[0]:.4f}, {source_line.point[1]:.4f}, {source_line.point[2]:.4f}]")
+            self._log(f"    线方向: [{source_line.direction[0]:.4f}, {source_line.direction[1]:.4f}, {source_line.direction[2]:.4f}]")
+            self._log(f"    耗时: {timing_breakdown['源线RANSAC拟合']:.2f}秒")
+
+            # 4. 拟合目标边缘线（用全部精筛选点）
+            self._log(f"\n  步骤4: 拟合目标边缘线")
+            start_time = time.time()
+            self._log(f"    输入点数: {len(target_points)} 个点")
+            target_fitter = LineFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            target_line = target_fitter.fit(target_points)
+            timing_breakdown['目标线RANSAC拟合'] = time.time() - start_time
+
+            if target_line is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"目标边缘线拟合失败（点数: {len(target_points)}，要求内点比例>={params.get('min_inlier_ratio', 0.6)*100:.0f}%）"
+                )
+
+            self._log(f"    RANSAC参数: 阈值={params.get('ransac_threshold', 2.0)}mm, 迭代={params.get('ransac_max_iterations', 1000)}次, 最小内点比例={params.get('min_inlier_ratio', 0.6)*100:.0f}%")
+            self._log(f"    拟合结果: 内点{len(target_line.inliers)}/{len(target_points)} ({target_line.inlier_ratio*100:.1f}%)")
+            self._log(f"    线上一点: [{target_line.point[0]:.4f}, {target_line.point[1]:.4f}, {target_line.point[2]:.4f}]")
+            self._log(f"    线方向: [{target_line.direction[0]:.4f}, {target_line.direction[1]:.4f}, {target_line.direction[2]:.4f}]")
+            self._log(f"    耗时: {timing_breakdown['目标线RANSAC拟合']:.2f}秒")
+
+            # 5. 从源线采样点（从内点中采样）
+            self._log(f"\n  步骤5: 从源线采样点")
+            start_time = time.time()
+            n_samples = params.get('sample_points', 100)
+            source_inlier_points = source_points[source_line.inliers]
+            sampled_points = source_line.sample_points_on_line(n_samples, source_inlier_points)
+            timing_breakdown['采样点选择'] = time.time() - start_time
+
+            self._log(f"    采样数量: {len(sampled_points)} 个点")
+            self._log(f"    耗时: {timing_breakdown['采样点选择']:.4f}秒")
+
+            # 6. 计算采样点到目标线的距离
+            self._log(f"\n  步骤6: 计算距离")
+            start_time = time.time()
+            distances = target_line.distance_to_points(sampled_points)
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            self._log(f"    计算 {len(sampled_points)} 个采样点到目标线的距离")
+            self._log(f"    耗时: {timing_breakdown['距离计算']:.4f}秒")
+
+            # 7. 统计结果
+            self._log(f"\n  步骤7: 统计结果")
+            mean_distance = np.mean(distances)
+            std_distance = np.std(distances)
+            min_distance = np.min(distances)
+            max_distance = np.max(distances)
+
+            self._log(f"    平均距离: {mean_distance:.4f} mm")
+            self._log(f"    标准差: {std_distance:.4f} mm")
+            self._log(f"    最小距离: {min_distance:.4f} mm")
+            self._log(f"    最大距离: {max_distance:.4f} mm")
+
+            # 8. 合格性判断
+            self._log(f"\n  步骤8: 合格性判断")
+            theoretical_dimension = self.config.get('theoretical_dimension')
+            tolerance = self.config.get('tolerance', {})
+            tolerance_upper = tolerance.get('upper', 0)
+            tolerance_lower = tolerance.get('lower', 0)
+
+            is_qualified = None
+            deviation = None
+            if theoretical_dimension is not None:
+                deviation = mean_distance - theoretical_dimension
+                is_qualified = (deviation >= -0.11) and (deviation <= 0.1)
+
+                self._log(f"    理论尺寸: {theoretical_dimension:.4f} mm")
+                self._log(f"    实测平均: {mean_distance:.4f} mm")
+                self._log(f"    偏差: {deviation:+.4f} mm")
+                self._log(f"    公差范围: [{tolerance_lower:+.1f}, {tolerance_upper:+.1f}] mm")
+                status = "✅ 合格" if is_qualified else "❌ 不合格"
+                self._log(f"    判定结果: {status}")
+
+            # 9. 生成可视化（如果配置了输出目录）
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                source_ply_path = vis_dir / f"{self.name}_source_line_roi.ply"
+                target_ply_path = vis_dir / f"{self.name}_target_line_roi.ply"
+                Visualizer.save_roi_pointcloud(source_points, str(source_ply_path))
+                Visualizer.save_roi_pointcloud(target_points, str(target_ply_path))
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            result_data = {
+                'mean_distance': float(mean_distance),
+                'std_distance': float(std_distance),
+                'min_distance': float(min_distance),
+                'max_distance': float(max_distance),
+                'sample_count': len(sampled_points),
+                'source_roi_points': len(source_points),
+                'target_roi_points': len(target_points),
+                'source_inlier_ratio': float(source_line.inlier_ratio),
+                'target_inlier_ratio': float(target_line.inlier_ratio),
+                'source_line_direction': source_line.direction.tolist(),
+                'target_line_direction': target_line.direction.tolist(),
+                'timing_breakdown': timing_breakdown
+            }
+
+            # 添加合格性判断结果
+            if theoretical_dimension is not None:
+                result_data['theoretical_dimension'] = float(theoretical_dimension)
+                result_data['deviation'] = float(deviation)
+                result_data['is_qualified'] = is_qualified
+                result_data['tolerance_upper'] = float(tolerance_upper)
+                result_data['tolerance_lower'] = float(tolerance_lower)
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data=result_data
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/line_to_plane.py

@@ -0,0 +1,252 @@
+"""线到面距离测量"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+# 添加父目录到路径
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from edge_line_roi_extractor import EdgeLineROIExtractor
+from line_fitter import LineFitter
+from roi_extractor import ROIExtractor
+from plane_fitter import PlaneFitter
+from visualizer import Visualizer
+
+
+class LineToPlaneDistance(BaseMeasurement):
+    """测量边缘线到平面的距离"""
+
+    def _log(self, msg: str):
+        """同时输出到控制台和日志文件"""
+        print(msg)
+        if hasattr(self, 'detail_logger') and self.detail_logger:
+            self.detail_logger.log(msg, also_print=False)
+
+    def execute(self) -> MeasurementResult:
+        """执行线到面距离测量"""
+        try:
+            timing_breakdown = {}
+
+            # 获取参数
+            params = self.config.get('params', {})
+            source_roi = self.config.get('source_roi', {})
+            target_roi = self.config.get('target_roi', {})
+
+            # 1. 提取源边缘线ROI（两步法）
+            self._log(f"\n  步骤1: 提取源边缘线ROI")
+            start_time = time.time()
+
+            # 获取线的端点（优先使用line_start/line_end，否则从bbox推导）
+            if 'line_start' in source_roi and 'line_end' in source_roi:
+                source_p1 = np.array(source_roi.get('line_start'))
+                source_p2 = np.array(source_roi.get('line_end'))
+            else:
+                source_bbox_min = np.array(source_roi.get('bbox_min'))
+                source_bbox_max = np.array(source_roi.get('bbox_max'))
+                source_p1 = source_bbox_min
+                source_p2 = source_bbox_max
+
+            self._log(f"    线起点: [{source_p1[0]:.2f}, {source_p1[1]:.2f}, {source_p1[2]:.2f}]")
+            self._log(f"    线终点: [{source_p2[0]:.2f}, {source_p2[1]:.2f}, {source_p2[2]:.2f}]")
+
+            source_points, source_indices, source_stats = EdgeLineROIExtractor.extract_two_step_roi(
+                self.point_cloud,
+                source_p1,
+                source_p2,
+                self.spatial_index,
+                distance_threshold=params.get('roi_distance_threshold', 2.0),
+                bbox_margin=5.0
+            )
+            timing_breakdown['提取源线ROI'] = time.time() - start_time
+
+            self._log(f"    粗ROI bbox: [{source_stats['bbox_min'][0]:.2f}, {source_stats['bbox_min'][1]:.2f}, {source_stats['bbox_min'][2]:.2f}] -> [{source_stats['bbox_max'][0]:.2f}, {source_stats['bbox_max'][1]:.2f}, {source_stats['bbox_max'][2]:.2f}]")
+            self._log(f"    粗筛选: {source_stats['coarse_count']:,} 点")
+            self._log(f"    精筛选(距离<{source_stats['distance_threshold']}mm): {source_stats['fine_count']:,} 点")
+            self._log(f"    耗时: {timing_breakdown['提取源线ROI']:.2f}秒")
+
+            if len(source_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="源边缘线ROI区域内没有点"
+                )
+
+            # 2. 提取目标平面ROI（用bbox）
+            self._log(f"\n  步骤2: 提取目标平面ROI")
+            start_time = time.time()
+            target_points, _ = ROIExtractor.extract_bbox_points(
+                self.point_cloud,
+                target_roi.get('bbox_min'),
+                target_roi.get('bbox_max'),
+                self.spatial_index
+            )
+            timing_breakdown['提取目标平面ROI'] = time.time() - start_time
+
+            self._log(f"    bbox: {target_roi.get('bbox_min')} -> {target_roi.get('bbox_max')}")
+            self._log(f"    提取点数: {len(target_points):,} 点")
+            self._log(f"    耗时: {timing_breakdown['提取目标平面ROI']:.2f}秒")
+
+            if len(target_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="目标平面ROI区域内没有点"
+                )
+
+            # 3. 拟合源边缘线（用全部精筛选点）
+            self._log(f"\n  步骤3: 拟合源边缘线")
+            start_time = time.time()
+            self._log(f"    输入点数: {len(source_points)} 个点")
+            source_fitter = LineFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            source_line = source_fitter.fit(source_points)
+            timing_breakdown['源线RANSAC拟合'] = time.time() - start_time
+
+            if source_line is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"源边缘线拟合失败（点数: {len(source_points)}，要求内点比例>={params.get('min_inlier_ratio', 0.6)*100:.0f}%）"
+                )
+
+            self._log(f"    RANSAC参数: 阈值={params.get('ransac_threshold', 2.0)}mm, 迭代={params.get('ransac_max_iterations', 1000)}次, 最小内点比例={params.get('min_inlier_ratio', 0.6)*100:.0f}%")
+            self._log(f"    拟合结果: 内点{len(source_line.inliers)}/{len(source_points)} ({source_line.inlier_ratio*100:.1f}%)")
+            self._log(f"    线上一点: [{source_line.point[0]:.4f}, {source_line.point[1]:.4f}, {source_line.point[2]:.4f}]")
+            self._log(f"    线方向: [{source_line.direction[0]:.4f}, {source_line.direction[1]:.4f}, {source_line.direction[2]:.4f}]")
+            self._log(f"    耗时: {timing_breakdown['源线RANSAC拟合']:.2f}秒")
+
+            # 4. 拟合目标平面（降采样优化）
+            self._log(f"\n  步骤4: 拟合目标平面")
+            start_time = time.time()
+            self._log(f"    输入点数: {len(target_points)} 个点")
+
+            max_points_for_ransac = 100000
+            target_points_for_fit = target_points
+            if len(target_points) > max_points_for_ransac:
+                sample_indices = np.random.choice(len(target_points), max_points_for_ransac, replace=False)
+                target_points_for_fit = target_points[sample_indices]
+                self._log(f"    降采样优化: {len(target_points):,} -> {len(target_points_for_fit):,} 个点")
+
+            target_fitter = PlaneFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            target_plane = target_fitter.fit(target_points_for_fit)
+            timing_breakdown['目标平面RANSAC拟合'] = time.time() - start_time
+
+            if target_plane is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"目标平面拟合失败（点数: {len(target_points)}，要求内点比例>={params.get('min_inlier_ratio', 0.6)*100:.0f}%）"
+                )
+
+            self._log(f"    RANSAC参数: 阈值={params.get('ransac_threshold', 2.0)}mm, 迭代={params.get('ransac_max_iterations', 1000)}次, 最小内点比例={params.get('min_inlier_ratio', 0.6)*100:.0f}%")
+            self._log(f"    拟合结果: 内点{len(target_plane.inliers)}/{len(target_points_for_fit)} ({target_plane.inlier_ratio*100:.1f}%)")
+            self._log(f"    平面上一点: [{target_plane.point[0]:.4f}, {target_plane.point[1]:.4f}, {target_plane.point[2]:.4f}]")
+            self._log(f"    平面法向量: [{target_plane.normal[0]:.4f}, {target_plane.normal[1]:.4f}, {target_plane.normal[2]:.4f}]")
+            self._log(f"    平面方程: {target_plane.normal[0]:.6f}*x + {target_plane.normal[1]:.6f}*y + {target_plane.normal[2]:.6f}*z + {target_plane.d:.6f} = 0")
+            self._log(f"    耗时: {timing_breakdown['目标平面RANSAC拟合']:.2f}秒")
+
+            # 5. 从源线采样点（从内点中采样）
+            start_time = time.time()
+            n_samples = params.get('sample_points', 100)
+            source_inlier_points = source_points[source_line.inliers]
+            sampled_points = source_line.sample_points_on_line(n_samples, source_inlier_points)
+            timing_breakdown['采样点选择'] = time.time() - start_time
+
+            self._log(f"  采样{len(sampled_points)}个点")
+
+            # 6. 计算采样点到目标平面的距离
+            start_time = time.time()
+            distances = target_plane.distance_to_points(sampled_points)
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            # 7. 统计结果（使用绝对值）
+            mean_distance = np.mean(np.abs(distances))
+            std_distance = np.std(distances)
+            min_distance = np.min(np.abs(distances))
+            max_distance = np.max(np.abs(distances))
+
+            # 8. 合格性判断
+            theoretical_dimension = self.config.get('theoretical_dimension')
+            tolerance = self.config.get('tolerance', {})
+            tolerance_upper = tolerance.get('upper', 0)
+            tolerance_lower = tolerance.get('lower', 0)
+
+            is_qualified = None
+            deviation = None
+            if theoretical_dimension is not None:
+                deviation = mean_distance - theoretical_dimension
+                is_qualified = (deviation >= -0.11) and (deviation <= 0.1)
+
+            self._log(f"  平均距离: {mean_distance:.4f} mm")
+            if theoretical_dimension is not None:
+                self._log(f"  理论尺寸: {theoretical_dimension:.4f} mm")
+                self._log(f"  偏差: {deviation:+.4f} mm")
+                self._log(f"  公差范围: [{tolerance_lower:+.1f}, {tolerance_upper:+.1f}] mm")
+                status = "✅ 合格" if is_qualified else "❌ 不合格"
+                self._log(f"  判定: {status}")
+
+            # 9. 生成可视化（如果配置了输出目录）
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                source_ply_path = vis_dir / f"{self.name}_source_line_roi.ply"
+                target_ply_path = vis_dir / f"{self.name}_target_plane_roi.ply"
+                Visualizer.save_roi_pointcloud(source_points, str(source_ply_path))
+                Visualizer.save_roi_pointcloud(target_points, str(target_ply_path))
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            result_data = {
+                'mean_distance': float(mean_distance),
+                'std_distance': float(std_distance),
+                'min_distance': float(min_distance),
+                'max_distance': float(max_distance),
+                'sample_count': len(sampled_points),
+                'source_roi_points': len(source_points),
+                'target_roi_points': len(target_points),
+                'source_inlier_ratio': float(source_line.inlier_ratio),
+                'target_inlier_ratio': float(target_plane.inlier_ratio),
+                'source_line_direction': source_line.direction.tolist(),
+                'target_plane_normal': target_plane.normal.tolist(),
+                'timing_breakdown': timing_breakdown
+            }
+
+            # 添加合格性判断结果
+            if theoretical_dimension is not None:
+                result_data['theoretical_dimension'] = float(theoretical_dimension)
+                result_data['deviation'] = float(deviation)
+                result_data['is_qualified'] = is_qualified
+                result_data['tolerance_upper'] = float(tolerance_upper)
+                result_data['tolerance_lower'] = float(tolerance_lower)
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data=result_data
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/plane_distance.py

@@ -0,0 +1,277 @@
+"""平面到平面距离测量"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+# 添加父目录到路径
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from roi_extractor import ROIExtractor
+from plane_fitter import PlaneFitter
+from visualizer import Visualizer
+
+
+class PlaneToPlaneDistance(BaseMeasurement):
+    """测量两个平面之间的距离（单向采样）"""
+
+    def execute(self) -> MeasurementResult:
+        """执行平面到平面距离测量"""
+        try:
+            timing_breakdown = {}
+
+            # 1. 提取源平面ROI
+            start_time = time.time()
+            source_roi = self.config.get('source_roi', {})
+            source_points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, source_roi.get('bbox_min'), source_roi.get('bbox_max'), self.spatial_index)
+            timing_breakdown['提取源ROI'] = time.time() - start_time
+
+            # 记录ROI提取信息到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log_roi_extraction(
+                    "源平面ROI",
+                    len(source_points),
+                    source_roi.get('bbox_min'),
+                    source_roi.get('bbox_max')
+                )
+
+            if len(source_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="源平面ROI区域内没有点"
+                )
+
+            # 2. 提取目标平面ROI
+            start_time = time.time()
+            target_roi = self.config.get('target_roi', {})
+            target_points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, target_roi.get('bbox_min'), target_roi.get('bbox_max'), self.spatial_index)
+            timing_breakdown['提取目标ROI'] = time.time() - start_time
+
+            # 记录ROI提取信息到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log_roi_extraction(
+                    "目标平面ROI",
+                    len(target_points),
+                    target_roi.get('bbox_min'),
+                    target_roi.get('bbox_max')
+                )
+
+            if len(target_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message="目标平面ROI区域内没有点"
+                )
+
+            # 3. 拟合源平面（优化：点过多时降采样）
+            start_time = time.time()
+            params = self.config.get('params', {})
+
+            # 降采样优化：如果点数过多，先降采样再RANSAC
+            max_points_for_ransac = 100000  # 10万个点足够拟合平面
+            source_points_for_fit = source_points
+            if len(source_points) > max_points_for_ransac:
+                sample_indices = np.random.choice(len(source_points), max_points_for_ransac, replace=False)
+                source_points_for_fit = source_points[sample_indices]
+                print(f"  源平面RANSAC优化: 从 {len(source_points):,} 降采样到 {len(source_points_for_fit):,} 个点")
+
+            source_fitter = PlaneFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            source_plane = source_fitter.fit(source_points_for_fit)
+            timing_breakdown['源平面RANSAC拟合'] = time.time() - start_time
+
+            if source_plane is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"源平面拟合失败（点数: {len(source_points)}）"
+                )
+
+            # 记录源平面RANSAC拟合信息到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log_ransac_fitting(
+                    "源平面",
+                    len(source_points_for_fit),
+                    len(source_plane.inliers),
+                    source_plane.inlier_ratio,
+                    (source_plane.normal[0], source_plane.normal[1], source_plane.normal[2], source_plane.d)
+                )
+
+            # 4. 拟合目标平面（优化：点过多时降采样）
+            start_time = time.time()
+
+            # 降采样优化：如果点数过多，先降采样再RANSAC
+            target_points_for_fit = target_points
+            if len(target_points) > max_points_for_ransac:
+                sample_indices = np.random.choice(len(target_points), max_points_for_ransac, replace=False)
+                target_points_for_fit = target_points[sample_indices]
+                print(f"  目标平面RANSAC优化: 从 {len(target_points):,} 降采样到 {len(target_points_for_fit):,} 个点")
+
+            target_fitter = PlaneFitter(
+                threshold=params.get('ransac_threshold', 2.0),
+                max_iterations=params.get('ransac_max_iterations', 1000),
+                min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+            )
+            target_plane = target_fitter.fit(target_points_for_fit)
+            timing_breakdown['目标平面RANSAC拟合'] = time.time() - start_time
+
+            if target_plane is None:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"目标平面拟合失败（点数: {len(target_points)}）"
+                )
+
+            # 记录目标平面RANSAC拟合信息到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log_ransac_fitting(
+                    "目标平面",
+                    len(target_points_for_fit),
+                    len(target_plane.inliers),
+                    target_plane.inlier_ratio,
+                    (target_plane.normal[0], target_plane.normal[1], target_plane.normal[2], target_plane.d)
+                )
+
+            # 5. 从源平面采样点
+            start_time = time.time()
+            n_samples = params.get('sample_points', 100)
+            # 修复：inliers索引是相对于source_points_for_fit的，不是source_points
+            source_inlier_points = source_points_for_fit[source_plane.inliers]
+            sampled_points = ROIExtractor.sample_points(source_inlier_points, n_samples)
+            timing_breakdown['采样点选择'] = time.time() - start_time
+
+            # 记录采样信息到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log_sampling(len(sampled_points), "从源平面内点均匀采样")
+
+            # 6. 计算采样点到目标平面的距离
+            start_time = time.time()
+            distances = target_plane.distance_to_points(sampled_points)
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            # 6.5. 可选：检查不同位置的宽度差值（用于门口类测量）
+            check_width_variation = params.get('check_width_variation', False)
+            width_variation_result = None
+
+            if check_width_variation:
+                # 按Y坐标分成3组：上部、中部、下部
+                y_coords = sampled_points[:, 1]
+                y_min, y_max = y_coords.min(), y_coords.max()
+                y_range = y_max - y_min
+
+                # 分组：上部(Y最大的1/3)、中部(中间1/3)、下部(Y最小的1/3)
+                upper_mask = y_coords >= (y_max - y_range / 3)
+                lower_mask = y_coords <= (y_min + y_range / 3)
+                middle_mask = ~upper_mask & ~lower_mask
+
+                upper_distances = np.abs(distances[upper_mask])
+                middle_distances = np.abs(distances[middle_mask])
+                lower_distances = np.abs(distances[lower_mask])
+
+                upper_width = np.mean(upper_distances)
+                middle_width = np.mean(middle_distances)
+                lower_width = np.mean(lower_distances)
+
+                # 计算差值
+                diff_upper_middle = upper_width - middle_width
+                diff_middle_lower = middle_width - lower_width
+                diff_upper_lower = upper_width - lower_width
+
+                # 检查公差（默认±2mm）
+                width_tolerance = params.get('width_variation_tolerance', 2.0)
+                all_diffs = [diff_upper_middle, diff_middle_lower, diff_upper_lower]
+                width_variation_pass = all(abs(d) <= width_tolerance for d in all_diffs)
+
+                width_variation_result = {
+                    'upper_width': float(upper_width),
+                    'middle_width': float(middle_width),
+                    'lower_width': float(lower_width),
+                    'diff_upper_middle': float(diff_upper_middle),
+                    'diff_middle_lower': float(diff_middle_lower),
+                    'diff_upper_lower': float(diff_upper_lower),
+                    'tolerance': float(width_tolerance),
+                    'pass': width_variation_pass,
+                    'upper_point_count': int(upper_mask.sum()),
+                    'middle_point_count': int(middle_mask.sum()),
+                    'lower_point_count': int(lower_mask.sum())
+                }
+
+                # 记录到详细日志
+                if hasattr(self, 'detail_logger') and self.detail_logger:
+                    self.detail_logger.log("")
+                    self.detail_logger.log("  宽度差值检查:")
+                    self.detail_logger.log(f"    上部宽度 (Y:{y_max-y_range/3:.1f}~{y_max:.1f}): {upper_width:.4f} mm ({upper_mask.sum()}点)")
+                    self.detail_logger.log(f"    中部宽度 (Y:{y_min+y_range/3:.1f}~{y_max-y_range/3:.1f}): {middle_width:.4f} mm ({middle_mask.sum()}点)")
+                    self.detail_logger.log(f"    下部宽度 (Y:{y_min:.1f}~{y_min+y_range/3:.1f}): {lower_width:.4f} mm ({lower_mask.sum()}点)")
+                    self.detail_logger.log("")
+                    self.detail_logger.log(f"    差值 (上-中): {diff_upper_middle:+.4f} mm")
+                    self.detail_logger.log(f"    差值 (中-下): {diff_middle_lower:+.4f} mm")
+                    self.detail_logger.log(f"    差值 (上-下): {diff_upper_lower:+.4f} mm")
+                    self.detail_logger.log(f"    公差要求: ±{width_tolerance:.1f} mm")
+                    status = "✅ 通过" if width_variation_pass else "❌ 超差"
+                    self.detail_logger.log(f"    检查结果: {status}")
+
+            # 7. 统计结果
+            mean_distance = np.mean(np.abs(distances))
+            std_distance = np.std(distances)
+            min_distance = np.min(np.abs(distances))
+            max_distance = np.max(np.abs(distances))
+
+            # 8. 生成可视化（如果配置了输出目录）
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                # 只保存ROI点云文件
+                source_ply_path = vis_dir / f"{self.name}_source_roi.ply"
+                target_ply_path = vis_dir / f"{self.name}_target_roi.ply"
+                Visualizer.save_roi_pointcloud(source_points, str(source_ply_path))
+                Visualizer.save_roi_pointcloud(target_points, str(target_ply_path))
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            result_data = {
+                'mean_distance': float(mean_distance),
+                'std_distance': float(std_distance),
+                'min_distance': float(min_distance),
+                'max_distance': float(max_distance),
+                'sample_count': len(sampled_points),
+                'source_roi_points': len(source_points),
+                'target_roi_points': len(target_points),
+                'source_inlier_ratio': float(source_plane.inlier_ratio),
+                'target_inlier_ratio': float(target_plane.inlier_ratio),
+                'source_plane_normal': source_plane.normal.tolist(),
+                'target_plane_normal': target_plane.normal.tolist(),
+                'timing_breakdown': timing_breakdown
+            }
+
+            # 添加宽度差值结果（如果有）
+            if width_variation_result:
+                result_data['width_variation'] = width_variation_result
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data=result_data
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/plane_with_holes.py

@@ -0,0 +1,241 @@
+"""
+带有孔中心检测的平面到平面距离测量
+支持 4_hole_center 方式定义参考平面
+"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from roi_extractor import ROIExtractor
+from plane_fitter import PlaneFitter
+from visualizer import Visualizer
+from hole_center_fitter import HoleCenterFitter
+
+
+class PlaneToPlaneWithHoles(BaseMeasurement):
+    """平面到平面距离测量（支持4孔定位参考平面）"""
+
+    def execute(self) -> MeasurementResult:
+        """执行测量"""
+        try:
+            timing_breakdown = {}
+
+            # 获取boxes配置
+            boxes = self.config.get('boxes', [])
+            if len(boxes) < 5:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"需要至少5个box（4个孔+至少1个平面），但只有{len(boxes)}个"
+                )
+
+            # 1. 提取前4个孔的点云并拟合参考平面
+            start_time = time.time()
+            print(f"  提取4个孔的ROI区域...")
+
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log(f"  提取4个定位孔...")
+
+            hole_points_list = []
+            hole_results = []
+            for i in range(4):
+                box = boxes[i]
+                points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, box['min'], box['max'], self.spatial_index)
+                if len(points) == 0:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"孔 '{box['name']}' ROI区域内没有点"
+                    )
+                hole_points_list.append(points)
+                print(f"    {box['name']}: {len(points)} 个点")
+
+                # 拟合圆心（用于详细日志）
+                result = HoleCenterFitter.fit_hole_center(points)
+                hole_results.append(result)
+
+            # 拟合4孔参考平面
+            reference_plane_result = HoleCenterFitter.fit_4_holes_reference_plane(hole_points_list)
+            plane_y = reference_plane_result['plane_y']
+
+            # 记录到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                for i, (box, result) in enumerate(zip(boxes[:4], hole_results)):
+                    self.detail_logger.log_circle_fitting(
+                        f"定位孔 '{box['name']}'",
+                        tuple(result['center_3d']),
+                        result['radius'],
+                        len(hole_points_list[i])
+                    )
+                self.detail_logger.log("")
+                self.detail_logger.log(f"  4孔中心点: {reference_plane_result['center_point']}")
+                self.detail_logger.log(f"  参考平面: Y = {plane_y:.4f} mm")
+                self.detail_logger.log("")
+
+            timing_breakdown['4孔拟合参考平面'] = time.time() - start_time
+
+            # 2. 提取并拟合源平面（剩余的boxes）
+            start_time = time.time()
+            params = self.config.get('params', {})
+            max_points_for_ransac = 100000
+
+            source_planes = []
+            source_plane_names = []
+
+            print(f"  提取并拟合源平面...")
+            for i in range(4, len(boxes)):
+                box = boxes[i]
+                points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, box['min'], box['max'], self.spatial_index)
+
+                if len(points) == 0:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"源平面 '{box['name']}' ROI区域内没有点"
+                    )
+
+                print(f"    {box['name']}: {len(points)} 个点")
+
+                # 降采样优化
+                points_for_fit = points
+                if len(points) > max_points_for_ransac:
+                    sample_indices = np.random.choice(len(points), max_points_for_ransac, replace=False)
+                    points_for_fit = points[sample_indices]
+                    print(f"      RANSAC优化: 降采样到 {len(points_for_fit):,} 个点")
+
+                # RANSAC拟合平面
+                fitter = PlaneFitter(
+                    threshold=params.get('ransac_threshold', 0.1),
+                    max_iterations=params.get('ransac_max_iterations', 1000),
+                    min_inlier_ratio=params.get('min_inlier_ratio', 0.6)
+                )
+                plane = fitter.fit(points_for_fit)
+
+                if plane is None:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"源平面 '{box['name']}' 拟合失败"
+                    )
+
+                source_planes.append({
+                    'plane': plane,
+                    'points': points,
+                    'name': box['name']
+                })
+                source_plane_names.append(box['name'])
+                print(f"      拟合成功，内点率: {plane.inlier_ratio:.1%}")
+
+                # 记录源平面RANSAC拟合到详细日志
+                if hasattr(self, 'detail_logger') and self.detail_logger:
+                    self.detail_logger.log_ransac_fitting(
+                        f"源平面 '{box['name']}'",
+                        len(points_for_fit),
+                        len(plane.inliers),
+                        plane.inlier_ratio,
+                        (plane.normal[0], plane.normal[1], plane.normal[2], plane.d)
+                    )
+
+            timing_breakdown['源平面RANSAC拟合'] = time.time() - start_time
+
+            # 3. 从每个源平面采样点并计算到参考平面的距离
+            start_time = time.time()
+            n_samples = params.get('sample_points', 100)
+
+            all_distances = []
+            plane_results = []
+
+            print(f"  计算距离...")
+            for plane_info in source_planes:
+                plane = plane_info['plane']
+                points = plane_info['points']
+                name = plane_info['name']
+
+                # 从内点中采样
+                inlier_points = points[plane.inliers]
+                sampled_points = ROIExtractor.sample_points(inlier_points, n_samples)
+
+                # 计算到参考平面的距离（y方向）
+                distances = sampled_points[:, 1] - plane_y
+
+                mean_dist = np.mean(np.abs(distances))
+                print(f"    {name}: 平均距离 = {mean_dist:.2f} mm")
+
+                all_distances.extend(distances)
+                plane_results.append({
+                    'name': name,
+                    'mean_distance': float(mean_dist),
+                    'distances': distances.tolist()
+                })
+
+                # 记录到详细日志
+                if hasattr(self, 'detail_logger') and self.detail_logger:
+                    self.detail_logger.log("")
+                    self.detail_logger.log(f"  平面 '{name}' 到参考平面的距离:")
+                    self.detail_logger.log(f"    采样点数: {len(sampled_points)} 个点")
+                    self.detail_logger.log(f"    平均距离: {mean_dist:.4f} mm")
+                    self.detail_logger.log(f"    距离范围: [{np.min(np.abs(distances)):.4f}, {np.max(np.abs(distances)):.4f}] mm")
+
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            # 4. 统计所有距离
+            all_distances = np.array(all_distances)
+            mean_distance = np.mean(np.abs(all_distances))
+            std_distance = np.std(all_distances)
+            min_distance = np.min(np.abs(all_distances))
+            max_distance = np.max(np.abs(all_distances))
+
+            # 5. 可视化
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                # 保存孔点云
+                for i, points in enumerate(hole_points_list):
+                    ply_path = vis_dir / f"{self.name}_hole_{i+1}.ply"
+                    Visualizer.save_roi_pointcloud(points, str(ply_path))
+
+                # 保存源平面点云
+                for plane_info in source_planes:
+                    ply_path = vis_dir / f"{self.name}_{plane_info['name']}.ply"
+                    Visualizer.save_roi_pointcloud(plane_info['points'], str(ply_path))
+
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data={
+                    'mean_distance': float(mean_distance),
+                    'std_distance': float(std_distance),
+                    'min_distance': float(min_distance),
+                    'max_distance': float(max_distance),
+                    'sample_count': len(all_distances),
+                    'reference_plane_y': float(plane_y),
+                    'reference_plane_centers': reference_plane_result['centers_3d'],
+                    'source_plane_names': source_plane_names,
+                    'plane_results': plane_results,
+                    'timing_breakdown': timing_breakdown
+                }
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/point_to_plane.py

@@ -0,0 +1,255 @@
+"""
+点到平面距离测量模块
+
+支持两种模式：
+1. exact_points: 在点云中查找给定坐标的最近点，计算到平面的距离
+2. bbox_filtered: 从包围盒提取点云，筛选特定条件的点，计算到平面的距离
+"""
+
+import numpy as np
+import time
+import open3d as o3d
+import sys
+from pathlib import Path
+from typing import Dict, Any, List, Tuple
+from scipy.spatial import KDTree
+
+# 添加父目录到路径
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from visualizer import Visualizer
+
+
+class PointToPlaneDistance(BaseMeasurement):
+    """点到平面距离测量"""
+
+    def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any], spatial_index=None):
+        super().__init__(name, point_cloud, config, spatial_index)
+        self.mode = config.get('mode', 'exact_points')
+
+        if self.mode == 'exact_points':
+            self.target_points = np.array(config['target_points'])
+            self.search_radius = config.get('search_radius', 10.0)
+        elif self.mode == 'bbox_filtered':
+            self.bboxes = config['bboxes']
+            self.filter_config = config['filter']
+        else:
+            raise ValueError(f"Unknown mode: {self.mode}")
+
+        # 平面参数 (ax + by + cz + d = 0)
+        plane_config = config['plane']
+        self.plane_normal = np.array([
+            plane_config.get('a', 0),
+            plane_config.get('b', 0),
+            plane_config.get('c', 1)
+        ])
+        self.plane_d = plane_config.get('d', 0)
+
+        # 归一化法向量
+        norm = np.linalg.norm(self.plane_normal)
+        if norm > 0:
+            self.plane_normal = self.plane_normal / norm
+            self.plane_d = self.plane_d / norm
+
+    def execute(self) -> MeasurementResult:
+        """执行测量"""
+        try:
+            if self.mode == 'exact_points':
+                return self._measure_exact_points()
+            elif self.mode == 'bbox_filtered':
+                return self._measure_bbox_filtered()
+        except Exception as e:
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=str(e)
+            )
+
+    def _measure_exact_points(self) -> MeasurementResult:
+        """精确点模式：查找最近点并计算距离（优化版：使用Grid索引）"""
+        from roi_extractor import ROIExtractor
+
+        timing_breakdown = {}
+
+        # 优化：对每个目标点单独用Grid索引提取小区域
+        start_time = time.time()
+
+        found_points = []
+        distances_to_plane = []
+
+        for target_point in self.target_points:
+            # 定义搜索区域（目标点周围的小包围盒）
+            search_margin = self.search_radius * 2
+            bbox_min = target_point - search_margin
+            bbox_max = target_point + search_margin
+
+            # 用Grid索引快速提取局部点云
+            local_points, local_indices = ROIExtractor.extract_bbox_points(
+                self.point_cloud, bbox_min, bbox_max, self.spatial_index
+            )
+
+            if len(local_points) == 0:
+                # 如果没找到点，扩大搜索范围
+                search_margin = self.search_radius * 10
+                bbox_min = target_point - search_margin
+                bbox_max = target_point + search_margin
+                local_points, local_indices = ROIExtractor.extract_bbox_points(
+                    self.point_cloud, bbox_min, bbox_max, self.spatial_index
+                )
+
+                if len(local_points) == 0:
+                    print(f"警告: 目标点 {target_point} 周围 {search_margin}mm 内没有点")
+                    continue
+
+            # 在局部点云中暴力搜索最近点（点数少，很快）
+            distances = np.linalg.norm(local_points - target_point, axis=1)
+            min_idx = np.argmin(distances)
+            min_distance = distances[min_idx]
+            nearest_point = local_points[min_idx]
+
+            if min_distance > self.search_radius:
+                print(f"警告: 目标点 {target_point} 的最近点距离 {min_distance:.2f}mm 超过搜索半径 {self.search_radius}mm")
+
+            found_points.append(nearest_point)
+
+            # 计算点到平面的距离
+            distance = self._point_to_plane_distance(nearest_point)
+            distances_to_plane.append(distance)
+        timing_breakdown['查找最近点'] = time.time() - start_time
+
+        start_time = time.time()
+        distances_array = np.array(distances_to_plane)
+        found_points_array = np.array(found_points)
+        timing_breakdown['数据整理'] = time.time() - start_time
+
+        # 保存找到的点为点云文件
+        start_time = time.time()
+        visualization_dir = self.config.get('visualization_dir')
+        if visualization_dir:
+            vis_dir = Path(visualization_dir)
+            vis_dir.mkdir(parents=True, exist_ok=True)
+            ply_path = vis_dir / f"{self.name}_found_points.ply"
+            Visualizer.save_roi_pointcloud(found_points_array, str(ply_path))
+        timing_breakdown['可视化保存'] = time.time() - start_time
+
+        return MeasurementResult(
+            task_name=self.name,
+            task_type=self.task_type,
+            success=True,
+            data={
+                'mean_distance': float(np.mean(np.abs(distances_array))),
+                'std_distance': float(np.std(np.abs(distances_array))),
+                'min_distance': float(np.min(np.abs(distances_array))),
+                'max_distance': float(np.max(np.abs(distances_array))),
+                'point_count': len(distances_array),
+                'mode': 'exact_points',
+                'target_points': self.target_points.tolist(),
+                'found_points': [p.tolist() for p in found_points],
+                'distances': distances_array.tolist(),
+                'timing_breakdown': timing_breakdown
+            }
+        )
+
+    def _measure_bbox_filtered(self) -> MeasurementResult:
+        """包围盒过滤模式：提取点云，筛选特定点，计算距离"""
+        # 导入ROIExtractor用于Grid索引加速
+        from roi_extractor import ROIExtractor
+
+        timing_breakdown = {}
+        all_filtered_points = []
+        all_distances = []
+
+        start_time = time.time()
+        for bbox_config in self.bboxes:
+            bbox_min = np.array(bbox_config['min'])
+            bbox_max = np.array(bbox_config['max'])
+
+            # 使用Grid索引快速提取包围盒内的点（优化后）
+            bbox_points, _ = ROIExtractor.extract_bbox_points(
+                self.point_cloud, bbox_min, bbox_max, self.spatial_index
+            )
+
+            if len(bbox_points) == 0:
+                print(f"警告: 包围盒 {bbox_min} 到 {bbox_max} 内没有点")
+                continue
+
+            # 应用过滤条件
+            filtered_points = self._apply_filter(bbox_points)
+
+            if len(filtered_points) == 0:
+                print(f"警告: 包围盒 {bbox_min} 到 {bbox_max} 过滤后没有点")
+                continue
+
+            all_filtered_points.extend(filtered_points)
+
+            # 计算每个点到平面的距离
+            for point in filtered_points:
+                distance = self._point_to_plane_distance(point)
+                all_distances.append(distance)
+        timing_breakdown['提取和过滤点云+距离计算'] = time.time() - start_time
+
+        if len(all_distances) == 0:
+            raise ValueError("没有找到符合条件的点")
+
+        start_time = time.time()
+        distances_array = np.array(all_distances)
+        filtered_points_array = np.array(all_filtered_points)
+        timing_breakdown['数据整理'] = time.time() - start_time
+
+        # 保存筛选后的点为点云文件
+        start_time = time.time()
+        visualization_dir = self.config.get('visualization_dir')
+        if visualization_dir:
+            vis_dir = Path(visualization_dir)
+            vis_dir.mkdir(parents=True, exist_ok=True)
+            ply_path = vis_dir / f"{self.name}_filtered_points.ply"
+            Visualizer.save_roi_pointcloud(filtered_points_array, str(ply_path))
+        timing_breakdown['可视化保存'] = time.time() - start_time
+
+        return MeasurementResult(
+            task_name=self.name,
+            task_type=self.task_type,
+            success=True,
+            data={
+                'mean_distance': float(np.mean(np.abs(distances_array))),
+                'std_distance': float(np.std(np.abs(distances_array))),
+                'min_distance': float(np.min(np.abs(distances_array))),
+                'max_distance': float(np.max(np.abs(distances_array))),
+                'point_count': len(distances_array),
+                'mode': 'bbox_filtered',
+                'num_bboxes': len(self.bboxes),
+                'filtered_points': all_filtered_points,
+                'distances': distances_array.tolist(),
+                'timing_breakdown': timing_breakdown
+            }
+        )
+
+    def _apply_filter(self, points: np.ndarray) -> List[np.ndarray]:
+        """应用过滤条件"""
+        filter_type = self.filter_config['type']
+
+        if filter_type == 'max_abs_coordinate':
+            axis = self.filter_config['axis']
+            top_n = self.filter_config['top_n']
+
+            axis_map = {'x': 0, 'y': 1, 'z': 2}
+            axis_idx = axis_map[axis.lower()]
+
+            # 计算绝对值
+            abs_values = np.abs(points[:, axis_idx])
+
+            # 获取top_n个最大值的索引
+            if len(points) <= top_n:
+                return points.tolist()
+
+            top_indices = np.argpartition(abs_values, -top_n)[-top_n:]
+            return points[top_indices].tolist()
+
+        else:
+            raise ValueError(f"Unknown filter type: {filter_type}")
+
+    def _point_to_plane_distance(self, point: np.ndarray) -> float:
+        """计算点到平面的有符号距离"""
+        return np.dot(self.plane_normal, point) + self.plane_d

alg/train_measurement_system_v3.2_0629/src/measurements/point_to_plane_with_holes.py

@@ -0,0 +1,136 @@
+"""
+点到平面距离测量（支持孔中心检测）
+支持 4_hole_center + 1_hole_center 方式
+"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from roi_extractor import ROIExtractor
+from visualizer import Visualizer
+from hole_center_fitter import HoleCenterFitter
+
+
+class PointToPlaneWithHoles(BaseMeasurement):
+    """点到平面距离测量（支持孔中心定位）"""
+
+    def execute(self) -> MeasurementResult:
+        """执行测量"""
+        try:
+            timing_breakdown = {}
+
+            # 获取boxes配置
+            boxes = self.config.get('boxes', [])
+            if len(boxes) < 5:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"需要至少5个box（4个定位孔+1个辅助孔），但只有{len(boxes)}个"
+                )
+
+            # 1. 提取前4个孔的点云并拟合参考平面
+            start_time = time.time()
+            print(f"  提取4个定位孔的ROI区域...")
+            hole_points_list = []
+            for i in range(4):
+                box = boxes[i]
+                points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, box['min'], box['max'], self.spatial_index)
+                if len(points) == 0:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"定位孔 '{box['name']}' ROI区域内没有点"
+                    )
+                hole_points_list.append(points)
+                print(f"    {box['name']}: {len(points)} 个点")
+
+            # 拟合4孔参考平面
+            reference_plane_result = HoleCenterFitter.fit_4_holes_reference_plane(hole_points_list)
+            plane_y = reference_plane_result['plane_y']
+            timing_breakdown['4孔拟合参考平面'] = time.time() - start_time
+
+            # 2. 提取第5个辅助孔的点云并拟合圆心
+            start_time = time.time()
+            auxiliary_box = boxes[4]
+            print(f"  提取辅助孔 '{auxiliary_box['name']}' ROI区域...")
+            auxiliary_points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, auxiliary_box['min'], auxiliary_box['max'], self.spatial_index)
+
+            if len(auxiliary_points) == 0:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"辅助孔 '{auxiliary_box['name']}' ROI区域内没有点"
+                )
+
+            print(f"    {auxiliary_box['name']}: {len(auxiliary_points)} 个点")
+
+            # 拟合辅助孔圆心
+            auxiliary_result = HoleCenterFitter.fit_hole_center(auxiliary_points)
+            auxiliary_center = auxiliary_result['center_3d']
+            print(f"  辅助孔圆心: {auxiliary_center}")
+
+            timing_breakdown['辅助孔拟合'] = time.time() - start_time
+
+            # 3. 计算辅助孔圆心到参考平面的距离
+            start_time = time.time()
+            distance = HoleCenterFitter.compute_distance_to_y_plane(
+                np.array(auxiliary_center),
+                plane_y
+            )
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            print(f"  辅助孔圆心到参考平面的距离: {abs(distance):.2f} mm")
+
+            # 4. 可视化
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                # 保存定位孔点云
+                for i, points in enumerate(hole_points_list):
+                    ply_path = vis_dir / f"{self.name}_locating_hole_{i+1}.ply"
+                    Visualizer.save_roi_pointcloud(points, str(ply_path))
+
+                # 保存辅助孔点云
+                ply_path = vis_dir / f"{self.name}_auxiliary_hole.ply"
+                Visualizer.save_roi_pointcloud(auxiliary_points, str(ply_path))
+
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data={
+                    'mean_distance': float(abs(distance)),
+                    'std_distance': 0.0,  # 单点测量，无标准差
+                    'min_distance': float(abs(distance)),
+                    'max_distance': float(abs(distance)),
+                    'point_count': 1,
+                    'distance': float(distance),  # 带符号的距离
+                    'reference_plane_y': float(plane_y),
+                    'reference_plane_centers': reference_plane_result['centers_3d'],
+                    'auxiliary_center': auxiliary_center,
+                    'auxiliary_radius': float(auxiliary_result['radius']),
+                    'timing_breakdown': timing_breakdown
+                }
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/point_to_point.py

@@ -0,0 +1,354 @@
+"""
+点到点距离测量模块
+
+用于测量点云中两个指定点之间的距离
+"""
+
+import numpy as np
+import time
+import sys
+from pathlib import Path
+from typing import Dict, Any, List, Tuple
+from scipy.spatial import KDTree
+
+# 添加父目录到路径
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from visualizer import Visualizer
+
+
+class PointToPointDistance(BaseMeasurement):
+    """点到点距离测量"""
+
+    def __init__(self, name: str, point_cloud: np.ndarray, config: Dict[str, Any], spatial_index=None):
+        super().__init__(name, point_cloud, config, spatial_index)
+
+        # 检查测量模式
+        self.mode = config.get('mode', 'exact_points')
+
+        if self.mode == 'exact_points':
+            # 点对列表，每对包含两个点的坐标
+            self.point_pairs = config['point_pairs']
+            self.search_radius = config.get('search_radius', 10.0)
+        elif self.mode == 'bbox_filtered':
+            # bbox过滤模式
+            self.bboxes = config.get('bboxes', [])
+            self.filter_config = config.get('filter', {})
+
+    def execute(self) -> MeasurementResult:
+        """执行测量"""
+        if self.mode == 'bbox_filtered':
+            return self._execute_bbox_filtered()
+        else:
+            return self._execute_exact_points()
+
+    def _execute_bbox_filtered(self) -> MeasurementResult:
+        """执行bbox过滤模式的测量"""
+        try:
+            from roi_extractor import ROIExtractor
+
+            timing_breakdown = {}
+
+            # 1. 从每个bbox提取点云
+            start_time = time.time()
+            bbox_points_list = []
+
+            for bbox in self.bboxes:
+                points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, bbox['min'], bbox['max'], self.spatial_index)
+                if len(points) == 0:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"bbox '{bbox['name']}' 区域内没有点"
+                    )
+                bbox_points_list.append({
+                    'name': bbox['name'],
+                    'points': points
+                })
+                print(f"  {bbox['name']}: {len(points)} 个点")
+
+            timing_breakdown['提取bbox点云'] = time.time() - start_time
+
+            # 2. 根据filter配置过滤点
+            start_time = time.time()
+            filter_type = self.filter_config.get('type')
+            axis = self.filter_config.get('axis', 'z')
+            top_n = self.filter_config.get('top_n', 10)
+
+            axis_idx = {'x': 0, 'y': 1, 'z': 2}[axis.lower()]
+
+            filtered_points_list = []
+
+            if filter_type == 'max_abs_coordinate':
+                for bbox_info in bbox_points_list:
+                    points = bbox_info['points']
+
+                    # 按绝对值排序
+                    abs_values = np.abs(points[:, axis_idx])
+                    top_indices = np.argsort(abs_values)[-top_n:]
+                    top_points = points[top_indices]
+
+                    filtered_points_list.append({
+                        'name': bbox_info['name'],
+                        'points': top_points,
+                        'values': points[top_indices, axis_idx]
+                    })
+
+                    print(f"    {bbox_info['name']}: 选取 {axis} 绝对值最大的 {len(top_points)} 个点")
+                    print(f"      {axis} 范围: [{top_points[:, axis_idx].min():.2f}, {top_points[:, axis_idx].max():.2f}]")
+
+            timing_breakdown['过滤点'] = time.time() - start_time
+
+            # 3. 计算距离（两组点的坐标差）
+            start_time = time.time()
+
+            if len(filtered_points_list) != 2:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"需要2个bbox，但有{len(filtered_points_list)}个"
+                )
+
+            # 计算两组点在指定轴上的平均坐标
+            group1_values = filtered_points_list[0]['values']
+            group2_values = filtered_points_list[1]['values']
+
+            mean1 = np.mean(group1_values)
+            mean2 = np.mean(group2_values)
+
+            distance = abs(mean2 - mean1)
+
+            print(f"  组1平均{axis}值: {mean1:.2f}")
+            print(f"  组2平均{axis}值: {mean2:.2f}")
+            print(f"  距离: {distance:.2f} mm")
+
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            # 4. 可视化
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                for filtered_info in filtered_points_list:
+                    ply_path = vis_dir / f"{self.name}_{filtered_info['name']}_filtered.ply"
+                    Visualizer.save_roi_pointcloud(filtered_info['points'], str(ply_path))
+
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data={
+                    'mean_distance': float(distance),
+                    'std_distance': 0.0,
+                    'min_distance': float(distance),
+                    'max_distance': float(distance),
+                    'point_count': top_n * 2,
+                    'group1_mean': float(mean1),
+                    'group2_mean': float(mean2),
+                    'timing_breakdown': timing_breakdown
+                }
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )
+
+    def _execute_exact_points(self) -> MeasurementResult:
+        """执行精确点模式的测量（优化版：使用Grid索引）"""
+        from roi_extractor import ROIExtractor
+
+        try:
+            timing_breakdown = {}
+
+            # 优化：对每个目标点用Grid索引提取小区域
+            start_time = time.time()
+
+            pair_results = []
+            all_found_points = []
+
+            for i, pair in enumerate(self.point_pairs):
+                point1_target = np.array(pair['point1'])
+                point2_target = np.array(pair['point2'])
+                pair_name = pair.get('name', f'点对{i+1}')
+
+                # 为point1找最近点
+                search_margin = self.search_radius * 2
+                bbox_min = point1_target - search_margin
+                bbox_max = point1_target + search_margin
+
+                local_points, _ = ROIExtractor.extract_bbox_points(
+                    self.point_cloud, bbox_min, bbox_max, self.spatial_index
+                )
+
+                if len(local_points) == 0:
+                    search_margin = self.search_radius * 10
+                    bbox_min = point1_target - search_margin
+                    bbox_max = point1_target + search_margin
+                    local_points, _ = ROIExtractor.extract_bbox_points(
+                        self.point_cloud, bbox_min, bbox_max, self.spatial_index
+                    )
+
+                if len(local_points) > 0:
+                    distances_to_p1 = np.linalg.norm(local_points - point1_target, axis=1)
+                    local_idx1 = np.argmin(distances_to_p1)
+                    dist1 = distances_to_p1[local_idx1]
+                    point1_found = local_points[local_idx1]
+                else:
+                    print(f"警告: {pair_name} - 点1 {point1_target} 附近没有点")
+                    continue
+
+                # 为point2找最近点
+                search_margin = self.search_radius * 2
+                bbox_min = point2_target - search_margin
+                bbox_max = point2_target + search_margin
+
+                local_points, _ = ROIExtractor.extract_bbox_points(
+                    self.point_cloud, bbox_min, bbox_max, self.spatial_index
+                )
+
+                if len(local_points) == 0:
+                    search_margin = self.search_radius * 10
+                    bbox_min = point2_target - search_margin
+                    bbox_max = point2_target + search_margin
+                    local_points, _ = ROIExtractor.extract_bbox_points(
+                        self.point_cloud, bbox_min, bbox_max, self.spatial_index
+                    )
+
+                if len(local_points) > 0:
+                    distances_to_p2 = np.linalg.norm(local_points - point2_target, axis=1)
+                    local_idx2 = np.argmin(distances_to_p2)
+                    dist2 = distances_to_p2[local_idx2]
+                    point2_found = local_points[local_idx2]
+                else:
+                    print(f"警告: {pair_name} - 点2 {point2_target} 附近没有点")
+                    continue
+
+                if dist1 > self.search_radius:
+                    print(f"警告: {pair_name} - 点1 {point1_target} 的最近点距离 {dist1:.2f}mm 超过搜索半径 {self.search_radius}mm")
+                if dist2 > self.search_radius:
+                    print(f"警告: {pair_name} - 点2 {point2_target} 的最近点距离 {dist2:.2f}mm 超过搜索半径 {self.search_radius}mm")
+
+                # 计算两点之间的欧氏距离
+                distance = np.linalg.norm(point2_found - point1_found)
+
+                pair_results.append({
+                    'name': pair_name,
+                    'point1_target': point1_target.tolist(),
+                    'point2_target': point2_target.tolist(),
+                    'point1_found': point1_found.tolist(),
+                    'point2_found': point2_found.tolist(),
+                    'point1_search_distance': float(dist1),
+                    'point2_search_distance': float(dist2),
+                    'distance': float(distance)
+                })
+
+                all_found_points.extend([point1_found, point2_found])
+            timing_breakdown['查找点对+距离计算'] = time.time() - start_time
+
+            # 统计所有距离
+            start_time = time.time()
+            distances = np.array([pr['distance'] for pr in pair_results])
+            mean_distance = float(np.mean(distances))
+            std_distance = float(np.std(distances))
+            min_distance = float(np.min(distances))
+            max_distance = float(np.max(distances))
+            timing_breakdown['数据整理'] = time.time() - start_time
+
+            # 检查是否启用差值比较模式
+            compare_mode = self.config.get('compare_mode')
+            comparison_result = None
+
+            if compare_mode == 'difference':
+                compare_pairs = self.config.get('compare_pairs', [])
+                difference_tolerance = self.config.get('difference_tolerance', 0)
+
+                if len(compare_pairs) != 2:
+                    print(f"警告: compare_mode='difference' 需要指定2个点对索引，但得到 {len(compare_pairs)} 个")
+                elif max(compare_pairs) >= len(pair_results):
+                    print(f"警告: compare_pairs 索引超出范围（最大索引: {len(pair_results)-1}）")
+                else:
+                    # 获取两组点对的距离
+                    idx1, idx2 = compare_pairs[0], compare_pairs[1]
+                    distance1 = pair_results[idx1]['distance']
+                    distance2 = pair_results[idx2]['distance']
+
+                    # 计算差值
+                    difference = abs(distance1 - distance2)
+
+                    # 判断是否通过
+                    passed = difference <= difference_tolerance
+
+                    comparison_result = {
+                        'pair1_index': idx1,
+                        'pair1_name': pair_results[idx1]['name'],
+                        'pair1_distance': distance1,
+                        'pair2_index': idx2,
+                        'pair2_name': pair_results[idx2]['name'],
+                        'pair2_distance': distance2,
+                        'difference': float(difference),
+                        'tolerance': float(difference_tolerance),
+                        'passed': passed
+                    }
+
+                    print(f"\n  差值比较结果:")
+                    print(f"    {pair_results[idx1]['name']}: {distance1:.4f} mm")
+                    print(f"    {pair_results[idx2]['name']}: {distance2:.4f} mm")
+                    print(f"    差值: {difference:.4f} mm")
+                    print(f"    公差: ≤{difference_tolerance:.2f} mm")
+                    status = "✅ 通过" if passed else "❌ 不通过"
+                    print(f"    判定: {status}")
+
+            # 保存找到的点为点云文件
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                # 保存所有找到的点
+                all_points_array = np.array(all_found_points)
+                ply_path = vis_dir / f"{self.name}_found_points.ply"
+                Visualizer.save_roi_pointcloud(all_points_array, str(ply_path))
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            result_data = {
+                'mean_distance': mean_distance,
+                'std_distance': std_distance,
+                'min_distance': min_distance,
+                'max_distance': max_distance,
+                'pair_count': len(pair_results),
+                'pair_results': pair_results,
+                'timing_breakdown': timing_breakdown
+            }
+
+            # 添加差值比较结果（如果有）
+            if comparison_result:
+                result_data['comparison'] = comparison_result
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data=result_data
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )

alg/train_measurement_system_v3.2_0629/src/measurements/point_to_point_with_holes.py

@@ -0,0 +1,161 @@
+"""
+点到点距离测量（支持孔中心检测）
+支持 hole_center 方式计算同侧孔中心的距离
+"""
+import numpy as np
+import time
+from typing import Dict, Any
+import sys
+from pathlib import Path
+
+sys.path.insert(0, str(Path(__file__).parent.parent))
+
+from measurements.base import BaseMeasurement, MeasurementResult
+from roi_extractor import ROIExtractor
+from visualizer import Visualizer
+from hole_center_fitter import HoleCenterFitter
+
+
+class PointToPointWithHoles(BaseMeasurement):
+    """点到点距离测量（支持孔中心定位）"""
+
+    def execute(self) -> MeasurementResult:
+        """执行测量"""
+        try:
+            timing_breakdown = {}
+
+            # 获取boxes配置
+            boxes = self.config.get('boxes', [])
+            if len(boxes) != 4:
+                return MeasurementResult(
+                    task_name=self.name,
+                    task_type=self.task_type,
+                    success=False,
+                    error_message=f"需要4个box（左前、右前、左后、右后），但有{len(boxes)}个"
+                )
+
+            # 1. 提取4个孔的点云并拟合圆心
+            start_time = time.time()
+            print(f"  提取4个孔的ROI区域并拟合圆心...")
+
+            # 记录到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log(f"  提取4个孔的ROI区域并拟合圆心...")
+
+            hole_centers = []
+            hole_names = []
+            hole_results = []  # 保存完整的拟合结果
+
+            for i, box in enumerate(boxes):
+                points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, box['min'], box['max'], self.spatial_index)
+                if len(points) == 0:
+                    return MeasurementResult(
+                        task_name=self.name,
+                        task_type=self.task_type,
+                        success=False,
+                        error_message=f"孔 '{box['name']}' ROI区域内没有点"
+                    )
+
+                print(f"    {box['name']}: {len(points)} 个点")
+
+                # 拟合圆心
+                result = HoleCenterFitter.fit_hole_center(points)
+                hole_centers.append(result['center_3d'])
+                hole_names.append(box['name'])
+                hole_results.append(result)
+                print(f"      圆心: {result['center_3d']}, 半径: {result['radius']:.2f} mm")
+
+                # 记录圆孔拟合结果到详细日志
+                if hasattr(self, 'detail_logger') and self.detail_logger:
+                    self.detail_logger.log_circle_fitting(
+                        f"孔 '{box['name']}'",
+                        tuple(result['center_3d']),
+                        result['radius'],
+                        len(points)
+                    )
+
+            timing_breakdown['4孔拟合'] = time.time() - start_time
+
+            # 2. 计算同侧孔中心的X差值
+            # 假设顺序：左前、右前、左后、右后
+            # 左侧距离：左前(0) - 左后(2) 的X差值
+            # 右侧距离：右前(1) - 右后(3) 的X差值
+            start_time = time.time()
+
+            left_front = np.array(hole_centers[0])
+            right_front = np.array(hole_centers[1])
+            left_rear = np.array(hole_centers[2])
+            right_rear = np.array(hole_centers[3])
+
+            # 计算X方向距离（取绝对值）
+            left_distance = abs(left_front[0] - left_rear[0])
+            right_distance = abs(right_front[0] - right_rear[0])
+
+            print(f"  左侧距离（{hole_names[0]} - {hole_names[2]}）: {left_distance:.2f} mm")
+            print(f"  右侧距离（{hole_names[1]} - {hole_names[3]}）: {right_distance:.2f} mm")
+
+            # 计算平均值
+            mean_distance = (left_distance + right_distance) / 2.0
+            min_distance = min(left_distance, right_distance)
+            max_distance = max(left_distance, right_distance)
+            std_distance = np.std([left_distance, right_distance])
+
+            print(f"  平均距离: {mean_distance:.2f} mm")
+
+            # 记录距离计算结果到详细日志
+            if hasattr(self, 'detail_logger') and self.detail_logger:
+                self.detail_logger.log("")
+                self.detail_logger.log(f"  孔中心距离计算:")
+                self.detail_logger.log(f"    {hole_names[0]} X坐标: {left_front[0]:.4f} mm")
+                self.detail_logger.log(f"    {hole_names[2]} X坐标: {left_rear[0]:.4f} mm")
+                self.detail_logger.log(f"    左侧距离: {left_distance:.4f} mm")
+                self.detail_logger.log("")
+                self.detail_logger.log(f"    {hole_names[1]} X坐标: {right_front[0]:.4f} mm")
+                self.detail_logger.log(f"    {hole_names[3]} X坐标: {right_rear[0]:.4f} mm")
+                self.detail_logger.log(f"    右侧距离: {right_distance:.4f} mm")
+                self.detail_logger.log("")
+                self.detail_logger.log_distance_result(mean_distance, std_distance, min_distance, max_distance)
+
+            timing_breakdown['距离计算'] = time.time() - start_time
+
+            # 3. 可视化
+            start_time = time.time()
+            visualization_dir = self.config.get('visualization_dir')
+            if visualization_dir:
+                vis_dir = Path(visualization_dir)
+                vis_dir.mkdir(parents=True, exist_ok=True)
+
+                # 保存每个孔的点云
+                for i, box in enumerate(boxes):
+                    points, _ = ROIExtractor.extract_bbox_points(self.point_cloud, box['min'], box['max'], self.spatial_index)
+                    ply_path = vis_dir / f"{self.name}_{box['name']}.ply"
+                    Visualizer.save_roi_pointcloud(points, str(ply_path))
+
+            timing_breakdown['可视化保存'] = time.time() - start_time
+
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=True,
+                data={
+                    'mean_distance': float(mean_distance),
+                    'std_distance': float(std_distance),
+                    'min_distance': float(min_distance),
+                    'max_distance': float(max_distance),
+                    'point_count': 2,  # 测量了2个距离值
+                    'left_distance': float(left_distance),
+                    'right_distance': float(right_distance),
+                    'hole_centers': [c.tolist() for c in hole_centers],
+                    'hole_names': hole_names,
+                    'timing_breakdown': timing_breakdown
+                }
+            )
+
+        except Exception as e:
+            import traceback
+            return MeasurementResult(
+                task_name=self.name,
+                task_type=self.task_type,
+                success=False,
+                error_message=f"执行过程中发生错误: {str(e)}\n{traceback.format_exc()}"
+            )