feat: initial commit - massive-cloud point cloud processing system

alg/point_porj/archive/CAD2Mesh.py

@@ -0,0 +1,55 @@
+from OCC.Core.STEPControl import STEPControl_Reader
+from OCC.Core.IFSelect import IFSelect_RetDone
+from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh
+from OCC.Core.StlAPI import StlAPI_Writer
+import time
+import argparse  # 命令行参数解析
+
+def step2lightweight_stl(mesh_precision, input_step, output_stl):
+    """STEP转轻量化STL，纯后台处理，无显示，体积暴减"""
+    start_total = time.time()
+    # 1. 读取STEP文件
+    print(f"【1/3】读取STEP文件：{input_step}")
+    step_reader = STEPControl_Reader()
+    if step_reader.ReadFile(input_step) != IFSelect_RetDone:
+        raise Exception("STEP文件读取失败！检查路径/文件完整性")
+    step_reader.TransferRoots()
+    shape = step_reader.OneShape()
+    print(f"【1/3】读取完成，耗时：{time.time()-start_total:.2f}秒")
+
+    # 2. 核心：网格离散化
+    print(f"【2/3】网格离散化（精度{mesh_precision}mm），开始轻量化...")
+    mesh = BRepMesh_IncrementalMesh(shape, mesh_precision, False)
+    mesh.Perform()
+    if not mesh.IsDone():
+        raise Exception("网格离散化失败！")
+    print(f"【2/3】轻量化完成，耗时：{time.time()-start_total:.2f}秒")
+
+    # 3. 保存为STL
+    print(f"【3/3】保存轻量化STL：{output_stl}")
+    stl_writer = StlAPI_Writer()
+    stl_writer.Write(shape, output_stl)
+
+    total_time = time.time() - start_total
+    print(f"✅ 转换完成！总耗时：{total_time:.2f}秒")
+    print(f"💡 提示：STL文件可直接用FreeCAD/MeshLab/3D Viewer打开")
+
+if __name__ == "__main__":
+    # ========== 命令行参数定义 ==========
+    parser = argparse.ArgumentParser(description="STEP 转 轻量化 STL (命令行args版)")
+    parser.add_argument("--precision", "-p", type=float, default=1.0, help="网格精度 MESH_PRECISION (mm)，越大文件越小")
+    parser.add_argument("--input", "-i", type=str,   default="High_Speed_Data/CAD/11.step", help="输入STEP文件路径 INPUT_STEP_PATH")
+    parser.add_argument("--output", "-o", type=str,  default="High_Speed_Data/Mesh/11.stl", help="输出STL文件路径 OUTPUT_STL_PATH")
+    
+    # 解析参数
+    args = parser.parse_args()
+
+    # 调用转换函数
+    try:
+        step2lightweight_stl(
+            mesh_precision=args.precision,
+            input_step=args.input,
+            output_stl=args.output
+        )
+    except Exception as e:
+        print(f"❌ 处理失败：{str(e)}")

alg/point_porj/archive/Contour_Flatness.py

@@ -0,0 +1,799 @@
+import argparse
+from pathlib import Path
+from zipfile import ZipFile, ZIP_DEFLATED
+from xml.sax.saxutils import escape
+import numpy as np
+import torch
+import trimesh
+import matplotlib.pyplot as plt
+
+from OCC.Core.STEPControl import STEPControl_Reader
+from OCC.Core.IFSelect import IFSelect_RetDone
+from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh
+
+from OCC.Core.TopExp import TopExp_Explorer
+from OCC.Core.TopAbs import TopAbs_FACE
+from OCC.Core.BRep import BRep_Tool
+
+# 兼容不同 pythonocc 版本
+try:
+    from OCC.Core.TopoDS import topods
+    USE_TOPODS = "topods"
+except ImportError:
+    from OCC.Core.TopoDS import TopoDS
+    USE_TOPODS = "TopoDS"
+
+try:
+    import open3d as o3d
+except ModuleNotFoundError:
+    o3d = None
+
+
+# ============================================================
+# 设备与精度
+# ============================================================
+
+device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
+dtype = torch.float64
+
+DEFAULT_TASK_IDS = [
+    11, 13, 17, 20, 25, 26, 27, 31, 34,
+    35, 36, 37, 38, 39, 40, 41, 42, 45
+]
+
+task_distance_data = {}
+
+
+# ============================================================
+# STEP 读取
+# ============================================================
+
+def read_step_file(step_path):
+    reader = STEPControl_Reader()
+    status = reader.ReadFile(step_path)
+
+    if status != IFSelect_RetDone:
+        raise RuntimeError(f"STEP 文件读取失败: {step_path}")
+
+    reader.TransferRoots()
+    shape = reader.OneShape()
+
+    print("STEP 文件读取完成")
+
+    return shape
+
+
+# ============================================================
+# OCC Shape -> numpy mesh
+# ============================================================
+
+def occ_shape_to_numpy_mesh(shape):
+    """
+    从 OCC Shape 中提取三角网格。
+
+    返回:
+        vertices_np: [N, 3]
+        faces_np: [M, 3]
+    """
+
+    vertices = []
+    faces = []
+
+    explorer = TopExp_Explorer(shape, TopAbs_FACE)
+
+    face_count = 0
+    triangle_count = 0
+
+    while explorer.More():
+        current_shape = explorer.Current()
+
+        if USE_TOPODS == "topods":
+            face = topods.Face(current_shape)
+        else:
+            face = TopoDS.Face(current_shape)
+
+        location = face.Location()
+
+        triangulation = BRep_Tool.Triangulation(face, location)
+
+        if triangulation is None:
+            explorer.Next()
+            continue
+
+        trsf = location.Transformation()
+
+        base_index = len(vertices)
+
+        # 读取当前 Face 的顶点
+        for i in range(1, triangulation.NbNodes() + 1):
+            pnt = triangulation.Node(i)
+
+            # 应用局部变换
+            pnt.Transform(trsf)
+
+            vertices.append([
+                pnt.X(),
+                pnt.Y(),
+                pnt.Z()
+            ])
+
+        # 读取当前 Face 的三角形
+        for i in range(1, triangulation.NbTriangles() + 1):
+            tri = triangulation.Triangle(i)
+
+            n1, n2, n3 = tri.Get()
+
+            # OCC 索引从 1 开始，numpy / torch 索引从 0 开始
+            faces.append([
+                base_index + n1 - 1,
+                base_index + n2 - 1,
+                base_index + n3 - 1
+            ])
+
+            triangle_count += 1
+
+        face_count += 1
+        explorer.Next()
+
+    vertices_np = np.asarray(vertices, dtype=np.float64)
+    faces_np = np.asarray(faces, dtype=np.int64)
+
+    print("CAD 网格提取完成")
+    print(f"CAD 面数量: {face_count}")
+    print(f"网格顶点: {vertices_np.shape}")
+    print(f"网格三角面: {faces_np.shape}")
+
+    if vertices_np.shape[0] == 0 or faces_np.shape[0] == 0:
+        raise RuntimeError(
+            "CAD 三角网格提取失败。可以尝试减小 MESH_DEFLECTION，或者检查 STEP 文件是否正常。"
+        )
+
+    return vertices_np, faces_np
+
+
+# ============================================================
+# 读取点云
+# ============================================================
+
+def read_point_cloud(pointcloud_path):
+    if o3d is not None:
+        pcd = o3d.io.read_point_cloud(pointcloud_path)
+        points_np = np.asarray(pcd.points, dtype=np.float64)
+    else:
+        pcd = trimesh.load(pointcloud_path)
+        points_np = np.asarray(pcd.vertices, dtype=np.float64)
+
+    if points_np.shape[0] == 0:
+        raise RuntimeError(f"点云读取失败或点数为 0: {pointcloud_path}")
+
+    print("点云读取完成")
+    print(f"点云点数: {points_np.shape}")
+
+    return points_np
+
+
+# ============================================================
+# 点到三角形距离
+# ============================================================
+
+def point_to_triangle_distance_squared(points, triangles):
+    """
+    points:
+        [P, 3]
+    triangles:
+        [T, 3, 3]
+
+    返回:
+        dist2: [P, T]
+    """
+
+    p = points[:, None, :]
+
+    a = triangles[None, :, 0, :]
+    b = triangles[None, :, 1, :]
+    c = triangles[None, :, 2, :]
+
+    ab = b - a
+    ac = c - a
+    ap = p - a
+
+    d1 = (ab * ap).sum(dim=-1)
+    d2 = (ac * ap).sum(dim=-1)
+
+    inf = torch.full_like(d1, torch.inf)
+
+    # 区域 A
+    mask = (d1 <= 0.0) & (d2 <= 0.0)
+    dist2 = torch.where(mask, (ap * ap).sum(dim=-1), inf)
+
+    # 区域 B
+    bp = p - b
+    d3 = (ab * bp).sum(dim=-1)
+    d4 = (ac * bp).sum(dim=-1)
+
+    mask = (d3 >= 0.0) & (d4 <= d3)
+    dist2 = torch.minimum(
+        dist2,
+        torch.where(mask, (bp * bp).sum(dim=-1), inf)
+    )
+
+    # 边 AB
+    vc = d1 * d4 - d3 * d2
+    mask = (vc <= 0.0) & (d1 >= 0.0) & (d3 <= 0.0)
+
+    v = d1 / (d1 - d3).clamp_min(torch.finfo(points.dtype).eps)
+    closest = a + v[..., None] * ab
+    edge_dist2 = ((p - closest) ** 2).sum(dim=-1)
+
+    dist2 = torch.minimum(
+        dist2,
+        torch.where(mask, edge_dist2, inf)
+    )
+
+    # 区域 C
+    cp = p - c
+    d5 = (ab * cp).sum(dim=-1)
+    d6 = (ac * cp).sum(dim=-1)
+
+    mask = (d6 >= 0.0) & (d5 <= d6)
+    dist2 = torch.minimum(
+        dist2,
+        torch.where(mask, (cp * cp).sum(dim=-1), inf)
+    )
+
+    # 边 AC
+    vb = d5 * d2 - d1 * d6
+    mask = (vb <= 0.0) & (d2 >= 0.0) & (d6 <= 0.0)
+
+    w = d2 / (d2 - d6).clamp_min(torch.finfo(points.dtype).eps)
+    closest = a + w[..., None] * ac
+    edge_dist2 = ((p - closest) ** 2).sum(dim=-1)
+
+    dist2 = torch.minimum(
+        dist2,
+        torch.where(mask, edge_dist2, inf)
+    )
+
+    # 边 BC
+    va = d3 * d6 - d5 * d4
+    mask = (va <= 0.0) & ((d4 - d3) >= 0.0) & ((d5 - d6) >= 0.0)
+
+    w = (d4 - d3) / ((d4 - d3) + (d5 - d6)).clamp_min(
+        torch.finfo(points.dtype).eps
+    )
+    closest = b + w[..., None] * (c - b)
+    edge_dist2 = ((p - closest) ** 2).sum(dim=-1)
+
+    dist2 = torch.minimum(
+        dist2,
+        torch.where(mask, edge_dist2, inf)
+    )
+
+    # 三角形内部区域
+    denom = (va + vb + vc).clamp_min(torch.finfo(points.dtype).eps)
+
+    v = vb / denom
+    w = vc / denom
+
+    closest = a + ab * v[..., None] + ac * w[..., None]
+    face_dist2 = ((p - closest) ** 2).sum(dim=-1)
+
+    dist2 = torch.minimum(dist2, face_dist2)
+
+    return dist2
+
+
+# ============================================================
+# 分块计算最近距离
+# ============================================================
+
+def nearest_mesh_distances(
+    points,
+    triangles,
+    point_chunk_size=2048,
+    triangle_chunk_size=4096
+):
+    """
+    为了避免显存爆炸，点和三角面都分块计算。
+
+    points:
+        [N, 3]
+    triangles:
+        [M, 3, 3]
+
+    返回:
+        distances: [N]
+    """
+
+    all_distances = []
+
+    num_points = points.shape[0]
+    num_triangles = triangles.shape[0]
+
+    for p_start in range(0, num_points, point_chunk_size):
+        p_end = min(p_start + point_chunk_size, num_points)
+
+        point_chunk = points[p_start:p_end]
+
+        min_dist2 = torch.full(
+            (point_chunk.shape[0],),
+            torch.inf,
+            dtype=points.dtype,
+            device=points.device
+        )
+
+        for t_start in range(0, num_triangles, triangle_chunk_size):
+            t_end = min(t_start + triangle_chunk_size, num_triangles)
+
+            triangle_chunk = triangles[t_start:t_end]
+
+            dist2 = point_to_triangle_distance_squared(
+                point_chunk,
+                triangle_chunk
+            )
+
+            min_dist2 = torch.minimum(
+                min_dist2,
+                dist2.min(dim=1).values
+            )
+
+        all_distances.append(torch.sqrt(min_dist2))
+
+        # print(f"processed points: {p_end}/{num_points}")
+
+    distances = torch.cat(all_distances, dim=0)
+
+    return distances
+
+
+# ============================================================
+# 热力图显示
+# ============================================================
+
+def visualize_point_cloud_heatmap(points_np, distances_np):
+    if o3d is None:
+        print("未安装 open3d，跳过可视化。")
+        return
+
+    d_min = distances_np.min()
+    d_max = distances_np.max()
+
+    dist_norm = (distances_np - d_min) / (d_max - d_min + 1e-12)
+
+    colors = plt.cm.turbo(dist_norm)[:, :3]
+
+    pcd_show = o3d.geometry.PointCloud()
+    pcd_show.points = o3d.utility.Vector3dVector(points_np)
+    pcd_show.colors = o3d.utility.Vector3dVector(colors)
+
+    print("色阶规则：深蓝/紫色 = 最近，红色 = 最远")
+
+    o3d.visualization.draw_geometries(
+        [pcd_show],
+        window_name="点云到 CAD 距离热力图"
+    )
+
+
+def visualize_task_result(task_results, task_id):
+    """
+    task_results: run_batch_tasks 返回的字典，key 为任务号。
+    task_id: 需要可视化的任务号。
+    """
+
+    result = task_results.get(int(task_id))
+
+    if result is None:
+        raise KeyError(f"未找到任务 {task_id} 的计算结果。")
+
+    visualize_point_cloud_heatmap(
+        result["points_np"],
+        result["distances_np"]
+    )
+
+
+def _xlsx_cell_ref(row_index, col_index):
+    letters = ""
+    col = col_index
+
+    while col:
+        col, remainder = divmod(col - 1, 26)
+        letters = chr(65 + remainder) + letters
+
+    return f"{letters}{row_index}"
+
+
+def _xlsx_cell_xml(row_index, col_index, value):
+    cell_ref = _xlsx_cell_ref(row_index, col_index)
+
+    if isinstance(value, (int, float, np.integer, np.floating)) and not isinstance(value, bool):
+        return f'<c r="{cell_ref}"><v>{value}</v></c>'
+
+    text = escape("" if value is None else str(value))
+    return f'<c r="{cell_ref}" t="inlineStr"><is><t>{text}</t></is></c>'
+
+
+def write_flatness_excel(rows, output_path, sheet_name="平整度"):
+    """
+    使用 Python 标准库生成 .xlsx，避免额外依赖。
+    """
+
+    output_path = Path(output_path)
+    output_path.parent.mkdir(parents=True, exist_ok=True)
+
+    headers = ["任务号", "平均距离(mm)", "最大距离(mm)", "状态", "说明"]
+    table = [headers]
+
+    for row in rows:
+        table.append([
+            row.get("task_id"),
+            row.get("mean_dist"),
+            row.get("max_dist"),
+            row.get("status", ""),
+            row.get("message", "")
+        ])
+
+    sheet_rows = []
+    for row_index, row_values in enumerate(table, start=1):
+        cells = [
+            _xlsx_cell_xml(row_index, col_index, value)
+            for col_index, value in enumerate(row_values, start=1)
+        ]
+        sheet_rows.append(f'<row r="{row_index}">{"".join(cells)}</row>')
+
+    safe_sheet_name = escape(sheet_name[:31])
+
+    workbook_xml = (
+        '<?xml version="1.0" encoding="UTF-8" standalone="yes"?>'
+        '<workbook xmlns="http://schemas.openxmlformats.org/spreadsheetml/2006/main" '
+        'xmlns:r="http://schemas.openxmlformats.org/officeDocument/2006/relationships">'
+        f'<sheets><sheet name="{safe_sheet_name}" sheetId="1" r:id="rId1"/></sheets>'
+        '</workbook>'
+    )
+
+    worksheet_xml = (
+        '<?xml version="1.0" encoding="UTF-8" standalone="yes"?>'
+        '<worksheet xmlns="http://schemas.openxmlformats.org/spreadsheetml/2006/main">'
+        '<sheetData>'
+        f'{"".join(sheet_rows)}'
+        '</sheetData>'
+        '</worksheet>'
+    )
+
+    content_types_xml = (
+        '<?xml version="1.0" encoding="UTF-8" standalone="yes"?>'
+        '<Types xmlns="http://schemas.openxmlformats.org/package/2006/content-types">'
+        '<Default Extension="rels" ContentType="application/vnd.openxmlformats-package.relationships+xml"/>'
+        '<Default Extension="xml" ContentType="application/xml"/>'
+        '<Override PartName="/xl/workbook.xml" '
+        'ContentType="application/vnd.openxmlformats-officedocument.spreadsheetml.sheet.main+xml"/>'
+        '<Override PartName="/xl/worksheets/sheet1.xml" '
+        'ContentType="application/vnd.openxmlformats-officedocument.spreadsheetml.worksheet+xml"/>'
+        '</Types>'
+    )
+
+    root_rels_xml = (
+        '<?xml version="1.0" encoding="UTF-8" standalone="yes"?>'
+        '<Relationships xmlns="http://schemas.openxmlformats.org/package/2006/relationships">'
+        '<Relationship Id="rId1" '
+        'Type="http://schemas.openxmlformats.org/officeDocument/2006/relationships/officeDocument" '
+        'Target="xl/workbook.xml"/>'
+        '</Relationships>'
+    )
+
+    workbook_rels_xml = (
+        '<?xml version="1.0" encoding="UTF-8" standalone="yes"?>'
+        '<Relationships xmlns="http://schemas.openxmlformats.org/package/2006/relationships">'
+        '<Relationship Id="rId1" '
+        'Type="http://schemas.openxmlformats.org/officeDocument/2006/relationships/worksheet" '
+        'Target="worksheets/sheet1.xml"/>'
+        '</Relationships>'
+    )
+
+    with ZipFile(output_path, "w", ZIP_DEFLATED) as xlsx:
+        xlsx.writestr("[Content_Types].xml", content_types_xml)
+        xlsx.writestr("_rels/.rels", root_rels_xml)
+        xlsx.writestr("xl/workbook.xml", workbook_xml)
+        xlsx.writestr("xl/_rels/workbook.xml.rels", workbook_rels_xml)
+        xlsx.writestr("xl/worksheets/sheet1.xml", worksheet_xml)
+
+    return output_path
+
+
+# ============================================================
+# 主函数
+# ============================================================
+
+def compute_point_CAD_distance(
+    CAD_path,
+    pointcloud_path,
+    mesh_deflection=0.1,
+    point_chunk_size=2048,
+    triangle_chunk_size=4096,
+    visualize=False
+):
+    print("计算设备:", device)
+    print("数据精度:", dtype)
+
+    # STEP -> OCC Shape
+    shape = read_step_file(CAD_path)
+
+    # OCC Shape 三角化
+    print("正在对 CAD 模型进行三角化...")
+    mesh = BRepMesh_IncrementalMesh(shape, mesh_deflection)
+    mesh.Perform()
+
+    # 提取 numpy 网格
+    vertices_np, faces_np = occ_shape_to_numpy_mesh(shape)
+
+    # 读取点云
+    points_np = read_point_cloud(pointcloud_path)
+
+    # numpy -> torch
+    points = torch.as_tensor(points_np, dtype=dtype, device=device)
+    vertices = torch.as_tensor(vertices_np, dtype=dtype, device=device)
+    faces = torch.as_tensor(faces_np, dtype=torch.long, device=device)
+
+    triangles = vertices[faces]
+
+    print("开始计算点云到 CAD 的最近距离...")
+    print("点云张量:", points.shape)
+    print("三角面张量:", triangles.shape)
+
+    with torch.no_grad():
+        distances_torch = nearest_mesh_distances(
+            points,
+            triangles,
+            point_chunk_size=point_chunk_size,
+            triangle_chunk_size=triangle_chunk_size
+        )
+
+    distances_np = distances_torch.detach().cpu().numpy()
+
+    mean_dist = float(distances_np.mean())
+    max_dist = float(distances_np.max())
+    min_dist = float(distances_np.min())
+
+    # print("Distance computation finished")
+    print(f"最小距离: {min_dist:.8f}")
+    print(f"平均距离: {mean_dist:.8f}")
+    print(f"最大距离: {max_dist:.8f}")
+
+    if visualize:
+        visualize_point_cloud_heatmap(points_np, distances_np)
+
+    return {
+        "mean_dist": mean_dist,
+        "max_dist": max_dist,
+        "min_dist": min_dist,
+        "points_np": points_np,
+        "distances_np": distances_np
+    }
+
+
+def get_task_paths(task_id, data_root="High_Speed_Data", pointcloud_kind="noise"):
+    data_root = Path(data_root)
+    task_id = int(task_id)
+
+    cad_path = data_root / "CAD" / f"{task_id}.step"
+
+    if pointcloud_kind == "clean":
+        pointcloud_path = data_root / "PointCloud" / f"{task_id}.ply"
+    else:
+        pointcloud_path = data_root / "Noise_PointCloud" / f"{task_id}_noisy.ply"
+
+    return cad_path, pointcloud_path
+
+
+def run_batch_tasks(
+    task_ids=DEFAULT_TASK_IDS,
+    data_root="High_Speed_Data",
+    pointcloud_kind="noise",
+    excel_output="平整度.xlsx",
+    mesh_deflection=0.1,
+    point_chunk_size=2048,
+    triangle_chunk_size=4096
+):
+    global task_distance_data
+
+    result_rows = []
+    task_distance_data = {}
+
+    for task_id in task_ids:
+        task_id = int(task_id)
+        cad_path, pointcloud_path = get_task_paths(
+            task_id,
+            data_root=data_root,
+            pointcloud_kind=pointcloud_kind
+        )
+
+        print(f"===== 任务 {task_id} =====")
+
+        missing_paths = [
+            str(path)
+            for path in [cad_path, pointcloud_path]
+            if not path.exists()
+        ]
+
+        if missing_paths:
+            message = "缺少文件: " + "; ".join(missing_paths)
+            print(message)
+            result_rows.append({
+                "task_id": task_id,
+                "mean_dist": None,
+                "max_dist": None,
+                "status": "失败",
+                "message": message
+            })
+            continue
+
+        try:
+            result = compute_point_CAD_distance(
+                CAD_path=str(cad_path),
+                pointcloud_path=str(pointcloud_path),
+                mesh_deflection=mesh_deflection,
+                point_chunk_size=point_chunk_size,
+                triangle_chunk_size=triangle_chunk_size,
+                visualize=False
+            )
+        except Exception as exc:
+            message = str(exc)
+            print(f"任务 {task_id} 失败: {message}")
+            result_rows.append({
+                "task_id": task_id,
+                "mean_dist": None,
+                "max_dist": None,
+                "status": "失败",
+                "message": message
+            })
+            continue
+
+        task_distance_data[task_id] = {
+            "points_np": result["points_np"],
+            "distances_np": result["distances_np"]
+        }
+
+        result_rows.append({
+            "task_id": task_id,
+            "mean_dist": result["mean_dist"],
+            "max_dist": result["max_dist"],
+            "status": "完成",
+            "message": ""
+        })
+
+    excel_path = write_flatness_excel(
+        result_rows,
+        excel_output,
+        sheet_name="平整度"
+    )
+
+    print(f"Excel 已保存: {excel_path}")
+
+    return result_rows, task_distance_data
+
+
+# ============================================================
+# 命令行入口
+# ============================================================
+
+if __name__ == "__main__":
+
+    parser = argparse.ArgumentParser(
+        description="计算点云到 CAD STEP 模型的最近距离，支持单任务或批量任务。"
+    )
+
+    parser.add_argument(
+        "--CAD",
+        "-c",
+        default="High_Speed_Data/CAD/11.step",
+        help="CAD STEP 文件路径，仅在 --single 单任务模式下使用。"
+    )
+
+    parser.add_argument(
+        "--pointcloud",
+        "-p",
+        default="High_Speed_Data/Noise_PointCloud/11_noisy.ply",
+        help="点云 PLY 文件路径，仅在 --single 单任务模式下使用。"
+    )
+
+    parser.add_argument(
+        "--tasks",
+        default=",".join(str(task_id) for task_id in DEFAULT_TASK_IDS),
+        help="批量任务号，多个任务用英文逗号分隔。"
+    )
+
+    parser.add_argument(
+        "--data_root",
+        default="High_Speed_Data",
+        help="批量任务的数据根目录。"
+    )
+
+    parser.add_argument(
+        "--pointcloud_kind",
+        choices=["noise", "clean"],
+        default="noise",
+        help="批量任务点云来源：noise=Noise_PointCloud，clean=PointCloud。"
+    )
+
+    parser.add_argument(
+        "--excel_output",
+        default="平整度.xlsx",
+        help="批量结果 Excel 输出路径。"
+    )
+
+    parser.add_argument(
+        "--single",
+        action="store_true",
+        help="使用 --CAD 和 --pointcloud 执行单任务，而不是默认批量任务。"
+    )
+
+    parser.add_argument(
+        "--visualize",
+        action="store_true",
+        help="显示单任务热力图。批量模式不会自动可视化。"
+    )
+
+    parser.add_argument(
+        "--mesh_deflection",
+        default=0.1,
+        type=float,
+        help="CAD 三角化精度，越小越精细，但计算越慢。"
+    )
+
+    parser.add_argument(
+        "--point_chunk_size",
+        default=2048,
+        type=int,
+        help="点云分块大小，显存不够时可调小。"
+    )
+
+    parser.add_argument(
+        "--triangle_chunk_size",
+        default=4096,
+        type=int,
+        help="三角面分块大小，显存不够时可调小。"
+    )
+
+    args = parser.parse_args()
+
+    if args.single:
+        result = compute_point_CAD_distance(
+            CAD_path=args.CAD,
+            pointcloud_path=args.pointcloud,
+            mesh_deflection=args.mesh_deflection,
+            point_chunk_size=args.point_chunk_size,
+            triangle_chunk_size=args.triangle_chunk_size,
+            visualize=args.visualize
+        )
+
+        print("===== 计算结果 =====")
+        print(f"平均距离: {result['mean_dist']:.8f} mm")
+        print(f"最大距离: {result['max_dist']:.8f} mm")
+    else:
+        task_ids = [
+            int(task_id.strip())
+            for task_id in args.tasks.split(",")
+            if task_id.strip()
+        ]
+
+        result_rows, task_distance_data = run_batch_tasks(
+            task_ids=task_ids,
+            data_root=args.data_root,
+            pointcloud_kind=args.pointcloud_kind,
+            excel_output=args.excel_output,
+            mesh_deflection=args.mesh_deflection,
+            point_chunk_size=args.point_chunk_size,
+            triangle_chunk_size=args.triangle_chunk_size
+        )
+
+        print("===== 批量计算结果 =====")
+        for row in result_rows:
+            if row["status"] == "完成":
+                print(
+                    f"{row['task_id']}: "
+                    f"平均距离={row['mean_dist']:.8f} mm, "
+                    f"最大距离={row['max_dist']:.8f} mm"
+                )
+            else:
+                print(f"{row['task_id']}: {row['status']} - {row['message']}")
+
+        print("task_distance_data 已按任务号保存 points_np 和 distances_np。")

alg/point_porj/archive/Curvature_Estimation.py

@@ -0,0 +1,127 @@
+import open3d as o3d
+import numpy as np
+import argparse
+import os
+
+
+def compute_curvature_open3d(pcd, radius=0.05, max_nn=30):
+    """
+    使用 Open3D 计算点云曲率
+
+    Args:
+        pcd: Open3D 点云对象
+        radius: 搜索半径
+        max_nn: 最大邻居数
+
+    Returns:
+        curvatures: (N,) 曲率数组
+    """
+    # 估计法线（同时获取协方差矩阵信息）
+    pcd.estimate_normals(
+        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)
+    )
+
+    # 获取点云数据
+    points = np.asarray(pcd.points)
+    n_points = len(points)
+    curvatures = np.zeros(n_points)
+
+    # 构建 KD-Tree
+    tree = o3d.geometry.KDTreeFlann(pcd)
+
+    for i in range(n_points):
+        # 搜索邻域点
+        [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius)
+
+        if k < 3:
+            curvatures[i] = 1.0
+            continue
+
+        # 获取邻域点坐标
+        neighbors = points[idx]
+
+        # 计算协方差矩阵
+        centroid = np.mean(neighbors, axis=0)
+        centered = neighbors - centroid
+        cov = np.cov(centered.T)
+
+        # 特征值分解
+        eigenvalues = np.linalg.eigvalsh(cov)
+        eigenvalues = np.sort(eigenvalues)
+
+        # 计算曲率
+        curvatures[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8)
+
+    return curvatures
+
+
+def visualize_curvature(pcd, curvatures, color_scale=3.0):
+    """
+    根据曲率可视化点云
+
+    Args:
+        pcd: Open3D 点云对象
+        curvatures: 曲率数组
+        color_scale: 颜色缩放因子
+    """
+    colors = np.zeros((len(pcd.points), 3))
+    # 红色通道表示曲率（高曲率区域为红色）
+    colors[:, 0] = np.clip(curvatures * color_scale, 0, 1)
+    # 绿色通道为互补色（低曲率区域为绿色）
+    colors[:, 1] = np.clip(1 - curvatures * color_scale, 0, 1)
+    # 蓝色通道设为0
+    colors[:, 2] = 0
+
+    pcd.colors = o3d.utility.Vector3dVector(colors)
+
+
+def main():
+    parser = argparse.ArgumentParser(description='计算点云曲率并可视化')
+    parser.add_argument('--ply_path', type=str,default="./High_Speed_Data/PointCloud/11.ply", help='输入点云文件路径 (.ply)')
+    parser.add_argument('--radius', type=float, default=2, help='搜索半径 (默认: 50)')
+    parser.add_argument('--max_nn', type=int, default=30, help='最大邻居数 (默认: 30)')
+    parser.add_argument('--color_scale', type=float, default=3.0, help='颜色缩放因子 (默认: 3.0)')
+    parser.add_argument('--save', type=str, default=None, help='保存可视化图像路径 (可选)')
+
+    args = parser.parse_args()
+
+    # 检查文件是否存在
+    if not os.path.exists(args.ply_path):
+        print(f"错误: 文件不存在 - {args.ply_path}")
+        return
+
+    print(f"读取点云: {args.ply_path}")
+    pcd = o3d.io.read_point_cloud(args.ply_path)
+    # points=np.array(pcd.points)
+    if len(pcd.points) == 0:
+        print("错误: 读取点云失败，文件可能为空或格式不正确")
+        return
+
+    print(f"点云点数: {len(pcd.points)}")
+
+    # 计算曲率
+    print("计算曲率中...")
+    curvatures = compute_curvature_open3d(pcd, radius=args.radius, max_nn=args.max_nn)
+
+    # 统计曲率信息
+    print(f"曲率统计:")
+    print(f"  最小值: {curvatures.min():.6f}")
+    print(f"  最大值: {curvatures.max():.6f}")
+    print(f"  平均值: {curvatures.mean():.6f}")
+    print(f"  标准差: {curvatures.std():.6f}")
+
+    # 可视化
+    print("可视化曲率...")
+    visualize_curvature(pcd, curvatures, color_scale=args.color_scale)
+
+    # 保存截图（如果指定）
+    if args.save:
+        o3d.io.write_point_cloud(args.save, pcd)
+        print(f"点云已保存: {args.save}")
+
+    # 显示窗口
+    o3d.visualization.draw_geometries([pcd], window_name=f"Curvature Visualization - {os.path.basename(args.ply_path)}")
+
+
+if __name__ == "__main__":
+    main()

alg/point_porj/archive/Denoise.py

@@ -0,0 +1,431 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+import copy
+import argparse
+import os
+
+
+# ========== 1. Chamfer Distance 计算 ==========
+def chamfer_distance(pcd1, pcd2, sqrt=True):
+    """
+    计算两个点云之间的 CD 距离（Chamfer Distance）
+
+    Args:
+        pcd1: Open3D 点云对象1
+        pcd2: Open3D 点云对象2
+        sqrt: 是否对最终结果开平方根
+
+    Returns:
+        cd: Chamfer Distance 值
+        cd1: 点云1 到 点云2 的平均距离
+        cd2: 点云2 到 点云1 的平均距离
+    """
+    points1 = np.asarray(pcd1.points)
+    points2 = np.asarray(pcd2.points)
+
+    if len(points1) == 0 or len(points2) == 0:
+        return float('inf'), 0, 0
+
+    tree1 = KDTree(points1)
+    tree2 = KDTree(points2)
+
+    dist1, _ = tree2.query(points1, k=1)
+    cd1 = np.mean(dist1 ** 2)
+
+    dist2, _ = tree1.query(points2, k=1)
+    cd2 = np.mean(dist2 ** 2)
+
+    cd = cd1 + cd2
+
+    if sqrt:
+        return np.sqrt(cd), cd1, cd2
+    return cd, cd1, cd2
+
+
+# ========== 添加高斯噪声 ==========
+def add_gaussian_noise(pcd, noise_std=4.0):
+    """
+    给点云添加高斯噪声
+
+    Args:
+        pcd: Open3D 点云对象
+        noise_std: 噪声标准差
+
+    Returns:
+        noisy_pcd: 添加噪声后的点云
+    """
+    noisy_pcd = copy.deepcopy(pcd)
+    points = np.asarray(noisy_pcd.points)
+
+    noise = np.random.normal(0, noise_std, points.shape)
+    noisy_points = points + noise
+
+    noisy_pcd.points = o3d.utility.Vector3dVector(noisy_points)
+
+    return noisy_pcd
+
+
+# ========== 2. 统计离群点移除 (SOR) ==========
+def statistical_outlier_removal(pcd, nb_neighbors=20, std_ratio=2.0):
+    """
+    统计离群点移除
+    """
+    cl, ind = pcd.remove_statistical_outlier(nb_neighbors=nb_neighbors, std_ratio=std_ratio)
+
+    inlier_cloud = pcd.select_by_index(ind)
+    outlier_cloud = pcd.select_by_index(ind, invert=True)
+
+    metrics = {
+        'original_points': len(pcd.points),
+        'filtered_points': len(inlier_cloud.points),
+        'removed_points': len(outlier_cloud.points),
+        'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0,
+        'method': 'SOR',
+        'params': {'nb_neighbors': nb_neighbors, 'std_ratio': std_ratio}
+    }
+
+    return inlier_cloud, outlier_cloud, metrics
+
+
+# ========== 3. 半径离群点移除 (ROR) ==========
+def radius_outlier_removal(pcd, nb_points=5, radius=20):
+    """
+    半径离群点移除
+    """
+    cl, ind = pcd.remove_radius_outlier(nb_points=nb_points, radius=radius)
+
+    inlier_cloud = pcd.select_by_index(ind)
+    outlier_cloud = pcd.select_by_index(ind, invert=True)
+
+    metrics = {
+        'original_points': len(pcd.points),
+        'filtered_points': len(inlier_cloud.points),
+        'removed_points': len(outlier_cloud.points),
+        'keep_ratio': len(inlier_cloud.points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0,
+        'method': 'ROR',
+        'params': {'nb_points': nb_points, 'radius': radius}
+    }
+
+    return inlier_cloud, outlier_cloud, metrics
+
+
+# ========== 4. 直通滤波 (PF) ==========
+def pass_through_filter(pcd, axis='z', min_val=-6000.0, max_val=6000.0):
+    """
+    直通滤波
+    """
+    points = np.asarray(pcd.points)
+    axis_map = {'x': 0, 'y': 1, 'z': 2}
+    idx = axis_map[axis]
+
+    mask = (points[:, idx] >= min_val) & (points[:, idx] <= max_val)
+    filtered_points = points[mask]
+
+    filtered_pcd = o3d.geometry.PointCloud()
+    filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
+
+    metrics = {
+        'original_points': len(pcd.points),
+        'filtered_points': len(filtered_points),
+        'removed_points': len(pcd.points) - len(filtered_points),
+        'keep_ratio': len(filtered_points) / len(pcd.points) * 100 if len(pcd.points) > 0 else 0,
+        'method': 'PF',
+        'params': {'axis': axis, 'min_val': min_val, 'max_val': max_val}
+    }
+
+    return filtered_pcd, metrics
+
+
+def compute_keep_ratio_and_density(pcd, filtered_pcd, sample_size=1000):
+    """计算直通滤波的保留率和密度变化"""
+    n_orig = len(pcd.points)
+    n_filt = len(filtered_pcd.points)
+
+    if n_orig == 0 or n_filt == 0:
+        return 0, 0
+
+    keep_ratio = n_filt / n_orig * 100
+
+    points_orig = np.asarray(pcd.points)
+    points_filt = np.asarray(filtered_pcd.points)
+
+    if len(points_orig) > sample_size:
+        idx_orig = np.random.choice(len(points_orig), sample_size, replace=False)
+        points_orig_sample = points_orig[idx_orig]
+    else:
+        points_orig_sample = points_orig
+
+    if len(points_filt) > sample_size:
+        idx_filt = np.random.choice(len(points_filt), sample_size, replace=False)
+        points_filt_sample = points_filt[idx_filt]
+    else:
+        points_filt_sample = points_filt
+
+    if len(points_orig_sample) > 5:
+        tree_orig = KDTree(points_orig_sample)
+        dist_orig, _ = tree_orig.query(points_orig_sample, k=2)
+        avg_dist_orig = np.mean(dist_orig[:, 1])
+    else:
+        avg_dist_orig = 0
+
+    if len(points_filt_sample) > 5:
+        tree_filt = KDTree(points_filt_sample)
+        dist_filt, _ = tree_filt.query(points_filt_sample, k=2)
+        avg_dist_filt = np.mean(dist_filt[:, 1])
+    else:
+        avg_dist_filt = 0
+
+    if avg_dist_orig > 0:
+        density_change = (avg_dist_filt - avg_dist_orig) / avg_dist_orig * 100
+    else:
+        density_change = 0
+
+    return keep_ratio, density_change
+
+
+# ========== 5. 体素滤波 (VGF) ==========
+def voxel_grid_filter(pcd, leaf_size=0.05):
+    """
+    体素滤波
+    """
+    filtered_pcd = pcd.voxel_down_sample(leaf_size)
+
+    n_orig = len(pcd.points)
+    n_filt = len(filtered_pcd.points)
+
+    metrics = {
+        'original_points': n_orig,
+        'filtered_points': n_filt,
+        'removed_points': n_orig - n_filt,
+        'keep_ratio': n_filt / n_orig * 100 if n_orig > 0 else 0,
+        'downsample_rate': (n_orig - n_filt) / n_orig * 100 if n_orig > 0 else 0,
+        'compression_ratio': n_orig / n_filt if n_filt > 0 else 0,
+        'method': 'VGF',
+        'params': {'leaf_size': leaf_size}
+    }
+
+    if n_filt > 5:
+        points_sample = np.asarray(filtered_pcd.points)
+        sample_size = min(1000, n_filt)
+        if n_filt > sample_size:
+            idx = np.random.choice(n_filt, sample_size, replace=False)
+            points_sample = points_sample[idx]
+        tree = KDTree(points_sample)
+        dist, _ = tree.query(points_sample, k=2)
+        metrics['avg_point_distance'] = np.mean(dist[:, 1])
+
+    return filtered_pcd, metrics
+
+
+# ========== 6. 双边滤波 (BF) ==========
+def bilateral_filter(pcd, sigma_s=0.01, sigma_r=0.1):
+    """
+    双边滤波
+    """
+    if len(pcd.points) == 0:
+        return pcd, {'original_points': 0, 'filtered_points': 0, 'removed_points': 0, 'keep_ratio': 0, 'method': 'BF', 'params': {}}
+
+    # 确保有法线
+    if not pcd.has_normals():
+        pcd.estimate_normals()
+
+    points = np.asarray(pcd.points)
+    normals = np.asarray(pcd.normals)
+
+    tree = KDTree(points)
+    filtered_points = np.zeros_like(points)
+
+    for i, p in enumerate(points):
+        indices = tree.query_ball_point(p, r=2 * sigma_s)
+        if len(indices) <= 1:
+            filtered_points[i] = p
+            continue
+
+        neigh_points = points[indices]
+        neigh_normals = normals[indices]
+
+        spatial_dist = np.linalg.norm(neigh_points - p, axis=1)
+        w_s = np.exp(-spatial_dist ** 2 / (2 * sigma_s ** 2))
+
+        normal_diff = np.linalg.norm(neigh_normals - normals[i], axis=1)
+        w_r = np.exp(-normal_diff ** 2 / (2 * sigma_r ** 2))
+
+        w = w_s * w_r
+        if w.sum() > 0:
+            w = w / w.sum()
+        filtered_points[i] = np.sum(neigh_points.T * w, axis=1)
+
+    filtered_pcd = o3d.geometry.PointCloud()
+    filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
+    filtered_pcd.normals = o3d.utility.Vector3dVector(normals)
+
+    metrics = {
+        'original_points': len(pcd.points),
+        'filtered_points': len(filtered_points),
+        'removed_points': 0,
+        'keep_ratio': 100,
+        'method': 'BF',
+        'params': {'sigma_s': sigma_s, 'sigma_r': sigma_r}
+    }
+
+    return filtered_pcd, metrics
+
+
+# ========== 主函数 ==========
+def main():
+    parser = argparse.ArgumentParser(description='点云去噪算法对比 (SOR, ROR, PF, VGF, BF)')
+    parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/PointCloud/11.ply",
+                        help='输入点云文件路径 (.ply)')
+    parser.add_argument('--noise_std', '-n', type=float, default=4.0,
+                        help='高斯噪声标准差 (默认: 4.0)')
+    parser.add_argument('--voxel_size', '-v', type=float, default=0.05,
+                        help='体素滤波大小 (默认: 0.05)')
+    parser.add_argument('--sor_neighbors', type=int, default=20,
+                        help='SOR 邻域点数 (默认: 20)')
+    parser.add_argument('--sor_std_ratio', type=float, default=2.0,
+                        help='SOR 标准差倍数 (默认: 2.0)')
+    parser.add_argument('--ror_points', type=int, default=5,
+                        help='ROR 半径内最小点数 (默认: 5)')
+    parser.add_argument('--ror_radius', type=float, default=20,
+                        help='ROR 搜索半径 (默认: 20)')
+    parser.add_argument('--bf_sigma_s', type=float, default=0.01,
+                        help='双边滤波空间标准差 (默认: 0.01)')
+    parser.add_argument('--bf_sigma_r', type=float, default=0.1,
+                        help='双边滤波特征标准差 (默认: 0.1)')
+    parser.add_argument('--no_visualize', action='store_true',
+                        help='不显示可视化窗口')
+
+    args = parser.parse_args()
+
+    # 检查文件是否存在
+    if not os.path.exists(args.ply_path):
+        print(f"错误: 点云文件不存在 - {args.ply_path}")
+        return
+
+    # 1. 读取原始点云
+    print(f"读取点云: {args.ply_path}")
+    pcd_clean = o3d.io.read_point_cloud(args.ply_path)
+
+    if len(pcd_clean.points) == 0:
+        print("读取失败，请检查文件路径")
+        return
+
+    print(f"原始无噪点云点数: {len(pcd_clean.points)}")
+
+    # ========== 添加高斯噪声 ==========
+    print("\n" + "=" * 50)
+    print(f"添加高斯噪声 (std={args.noise_std})")
+    print("=" * 50)
+    pcd_noisy = add_gaussian_noise(pcd_clean, args.noise_std)
+    print(f"加噪后点云点数: {len(pcd_noisy.points)}")
+
+    # 计算噪声水平
+    points_clean = np.asarray(pcd_clean.points)
+    points_noisy = np.asarray(pcd_noisy.points)
+    noise_level = np.mean(np.linalg.norm(points_noisy - points_clean, axis=1))
+    print(f"平均噪声偏移量: {noise_level:.4f}")
+
+    # 加噪点云着色
+    pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5])
+
+    # ========== 运行各去噪算法 ==========
+    results = {}
+
+    # 1. SOR
+    print("\n" + "=" * 50)
+    print("统计离群点移除 (SOR)")
+    print("=" * 50)
+    sor_filtered, sor_outlier, sor_metrics = statistical_outlier_removal(
+        pcd_noisy, nb_neighbors=args.sor_neighbors, std_ratio=args.sor_std_ratio)
+    results['SOR'] = sor_metrics
+    print(f"保留率: {sor_metrics['keep_ratio']:.2f}%")
+    print(f"移除: {sor_metrics['removed_points']} 点")
+
+    # 2. ROR
+    print("\n" + "=" * 50)
+    print("半径离群点移除 (ROR)")
+    print("=" * 50)
+    ror_filtered, ror_outlier, ror_metrics = radius_outlier_removal(
+        pcd_noisy, nb_points=args.ror_points, radius=args.ror_radius)
+    results['ROR'] = ror_metrics
+    print(f"保留率: {ror_metrics['keep_ratio']:.2f}%")
+    print(f"移除: {ror_metrics['removed_points']} 点")
+
+    # 3. 直通滤波 (PF)
+    print("\n" + "=" * 50)
+    print("直通滤波 (PF)")
+    print("=" * 50)
+    points = np.asarray(pcd_noisy.points)
+    z_min, z_max = points[:, 2].min(), points[:, 2].max()
+    pf_filtered, pf_metrics = pass_through_filter(pcd_noisy, axis='z', min_val=z_min, max_val=z_max)
+    results['PF'] = pf_metrics
+    print(f"保留率: {pf_metrics['keep_ratio']:.2f}%")
+    keep_ratio, density_change = compute_keep_ratio_and_density(pcd_noisy, pf_filtered)
+    print(f"密度变化: {density_change:+.2f}%")
+
+    # 4. 体素滤波 (VGF)
+    print("\n" + "=" * 50)
+    print("体素滤波 (VGF)")
+    print("=" * 50)
+    vgf_filtered, vgf_metrics = voxel_grid_filter(pcd_noisy, leaf_size=args.voxel_size)
+    results['VGF'] = vgf_metrics
+    print(f"保留率: {vgf_metrics['keep_ratio']:.2f}%")
+    print(f"压缩比: {vgf_metrics['compression_ratio']:.1f}x")
+
+    # 5. 双边滤波 (BF)
+    print("\n" + "=" * 50)
+    print("双边滤波 (BF)")
+    print("=" * 50)
+    bf_filtered, bf_metrics = bilateral_filter(pcd_noisy, sigma_s=args.bf_sigma_s, sigma_r=args.bf_sigma_r)
+    results['BF'] = bf_metrics
+    print(f"保留率: {bf_metrics['keep_ratio']:.2f}%")
+
+    # Chamfer Distance 计算
+    print("\n" + "=" * 50)
+    print("Chamfer Distance 评估 (与干净点云对比)")
+    print("=" * 50)
+
+    for name, metrics in results.items():
+        if name == 'BF':
+            filtered = bf_filtered
+        elif name == 'SOR':
+            filtered = sor_filtered
+        elif name == 'ROR':
+            filtered = ror_filtered
+        elif name == 'PF':
+            filtered = pf_filtered
+        elif name == 'VGF':
+            filtered = vgf_filtered
+        else:
+            continue
+
+        if len(filtered.points) > 0:
+            cd, _, _ = chamfer_distance(pcd_clean, filtered)
+            print(f"{name}: CD = {cd:.6f} m")
+
+    print("-" * 40)
+    cd_noisy, _, _ = chamfer_distance(pcd_clean, pcd_noisy)
+    print(f"加噪点云 (未经滤波): CD = {cd_noisy:.6f} m")
+
+    # 可视化
+    if not args.no_visualize:
+        print("\n可视化对比...")
+        pcd_clean.paint_uniform_color([0, 0, 1])      # 蓝色 - 原始干净点云
+        pcd_noisy.paint_uniform_color([1.0, 0.7, 0.5])  # 浅橙色 - 加噪点云
+
+        sor_filtered.paint_uniform_color([0, 1, 0])    # 绿色 - SOR 结果
+
+        o3d.visualization.draw_geometries(
+            [pcd_clean, pcd_noisy],
+            window_name=f"Clean (Blue, {len(pcd_clean.points)}) vs Noisy (Orange, {len(pcd_noisy.points)})"
+        )
+
+        o3d.visualization.draw_geometries(
+            [pcd_clean, sor_filtered],
+            window_name=f"SOR: Clean (Blue, {len(pcd_clean.points)}) vs Filtered (Green, {len(sor_filtered.points)})"
+        )
+
+
+if __name__ == "__main__":
+    main()

alg/point_porj/archive/Plane_Estimation.py

@@ -0,0 +1,380 @@
+# import numpy as np
+# import open3d as o3d
+# import random
+#
+#
+# def ransac_plane(points, threshold=0.01, max_iterations=1000, min_inliers=100):
+#     """
+#     RANSAC 平面拟合（单个）
+#     """
+#     best_inliers = []
+#     best_plane = None
+#     n_points = len(points)
+#
+#     if n_points < 3:
+#         return None, []
+#
+#     for _ in range(max_iterations):
+#         sample_idx = random.sample(range(n_points), 3)
+#         p1, p2, p3 = points[sample_idx]
+#
+#         v1 = p2 - p1
+#         v2 = p3 - p1
+#         normal = np.cross(v1, v2)
+#         norm = np.linalg.norm(normal)
+#         if norm < 1e-8:
+#             continue
+#         normal = normal / norm
+#         a, b, c = normal
+#         d = -np.dot(normal, p1)
+#
+#         distances = np.abs(np.dot(points, normal) + d)
+#         inliers = np.where(distances < threshold)[0]
+#
+#         if len(inliers) > len(best_inliers) and len(inliers) >= min_inliers:
+#             best_inliers = inliers
+#             best_plane = (a, b, c, d)
+#
+#             if len(best_inliers) > 0.9 * n_points:
+#                 break
+#
+#     # 用内点重新拟合
+#     if best_inliers is not None and len(best_inliers) > 3:
+#         inlier_points = points[best_inliers]
+#         centroid = np.mean(inlier_points, axis=0)
+#         _, _, vh = np.linalg.svd(inlier_points - centroid)
+#         normal = vh[2]
+#         normal = normal / np.linalg.norm(normal)
+#         a, b, c = normal
+#         d = -np.dot(normal, centroid)
+#         best_plane = (a, b, c, d)
+#
+#     return best_plane, best_inliers
+#
+#
+# def sequential_ransac_planes(points, threshold=0.01, max_iterations=1000, min_inliers=100, max_planes=5):
+#     """
+#     迭代 RANSAC 拟合多个平面
+#     """
+#     remaining_points = points.copy()
+#     remaining_indices = np.arange(len(points))
+#
+#     planes = []
+#     inliers_list = []
+#
+#     for i in range(max_planes):
+#         if len(remaining_points) < min_inliers:
+#             break
+#
+#         plane, inliers = ransac_plane(remaining_points, threshold, max_iterations, min_inliers)
+#
+#         if plane is None or len(inliers) < min_inliers:
+#             break
+#
+#         planes.append(plane)
+#         original_indices = remaining_indices[inliers]
+#         inliers_list.append(original_indices)
+#
+#         # 移除内点
+#         mask = np.ones(len(remaining_points), dtype=bool)
+#         mask[inliers] = False
+#         remaining_points = remaining_points[mask]
+#         remaining_indices = remaining_indices[mask]
+#
+#         print(f"平面 {len(planes)}: {len(inliers)} 个内点, 剩余 {len(remaining_points)} 个点")
+#
+#     return planes, inliers_list
+#
+#
+# def create_plane_mesh(plane, bounds):
+#     """
+#     创建平面网格用于可视化
+#     """
+#     a, b, c, d = plane
+#     x_min, x_max = bounds[0]
+#     y_min, y_max = bounds[1]
+#
+#     x = np.linspace(x_min, x_max, 50)
+#     y = np.linspace(y_min, y_max, 50)
+#     X, Y = np.meshgrid(x, y)
+#
+#     if abs(c) > 1e-8:
+#         Z = -(a * X + b * Y + d) / c
+#     else:
+#         Z = np.zeros_like(X)
+#
+#     vertices = []
+#     triangles = []
+#     rows, cols = X.shape
+#
+#     for i in range(rows):
+#         for j in range(cols):
+#             vertices.append([X[i, j], Y[i, j], Z[i, j]])
+#
+#     for i in range(rows - 1):
+#         for j in range(cols - 1):
+#             idx = i * cols + j
+#             triangles.append([idx, idx + 1, idx + cols])
+#             triangles.append([idx + 1, idx + cols + 1, idx + cols])
+#
+#     mesh = o3d.geometry.TriangleMesh()
+#     mesh.vertices = o3d.utility.Vector3dVector(np.array(vertices))
+#     mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles))
+#     mesh.compute_vertex_normals()
+#
+#     return mesh
+#
+#
+# def visualize_multiple_planes(points, planes, inliers_list):
+#     """
+#     可视化多个平面（点云 + 平面网格）
+#
+#     Args:
+#         points: (N, 3) 点云坐标
+#         planes: 平面方程列表 [(a,b,c,d), ...]
+#         inliers_list: 每个平面对应的内点索引列表
+#     """
+#     # 颜色列表（每个平面不同颜色）
+#     colors = [
+#         [1, 0, 0],  # 红色
+#         [0, 1, 0],  # 绿色
+#         [0, 0, 1],  # 蓝色
+#         [1, 1, 0],  # 黄色
+#         [1, 0, 1],  # 品红
+#         [0, 1, 1],  # 青色
+#         [1, 0.5, 0],  # 橙色
+#         [0.5, 0, 1],  # 紫色
+#     ]
+#
+#     geometries = []
+#
+#     # 1. 为每个平面的内点着色
+#     point_colors = np.zeros((len(points), 3))
+#     for i, inliers in enumerate(inliers_list):
+#         color = colors[i % len(colors)]
+#         point_colors[inliers] = color
+#
+#     # 未分配的点（剩余点）设为灰色
+#     unassigned = np.where(np.sum(point_colors, axis=1) == 0)[0]
+#     point_colors[unassigned] = [0.5, 0.5, 0.5]
+#
+#     colored_pcd = o3d.geometry.PointCloud()
+#     colored_pcd.points = o3d.utility.Vector3dVector(points)
+#     colored_pcd.colors = o3d.utility.Vector3dVector(point_colors)
+#     geometries.append(colored_pcd)
+#
+#     # 2. 为每个平面创建网格
+#     for i, (plane, inliers) in enumerate(zip(planes, inliers_list)):
+#         # 计算该平面对应点的范围
+#         inlier_points = points[inliers]
+#         if len(inlier_points) > 0:
+#             x_min, x_max = inlier_points[:, 0].min(), inlier_points[:, 0].max()
+#             y_min, y_max = inlier_points[:, 1].min(), inlier_points[:, 1].max()
+#
+#             # 稍微扩展边界
+#             x_range = x_max - x_min
+#             y_range = y_max - y_min
+#             x_min -= x_range * 0.1
+#             x_max += x_range * 0.1
+#             y_min -= y_range * 0.1
+#             y_max += y_range * 0.1
+#
+#             mesh = create_plane_mesh(plane, [[x_min, x_max], [y_min, y_max]])
+#             mesh.paint_uniform_color(colors[i % len(colors)])
+#             # 设置半透明
+#             mesh.paint_uniform_color([colors[i % len(colors)][0],
+#                                       colors[i % len(colors)][1],
+#                                       colors[i % len(colors)][2]])
+#             geometries.append(mesh)
+#
+#     # 可视化
+#     o3d.visualization.draw_geometries(
+#         geometries,
+#         window_name="Multiple Planes - RANSAC",
+#         width=1024,
+#         height=768
+#     )
+#
+#
+# def visualize_single_plane(points, plane, inliers):
+#     """
+#     可视化单个平面（兼容单平面情况）
+#     """
+#     visualize_multiple_planes(points, [plane], [inliers])
+#
+#
+# # ========== 主程序 ==========
+# if __name__ == "__main__":
+#     # 读取点云
+#     file_path = r"E:\point_distance\data\part_car_head.ply"
+#     pcd = o3d.io.read_point_cloud(file_path)
+#
+#     if len(pcd.points) == 0:
+#         print("读取失败，请检查文件路径")
+#         exit()
+#
+#     points = np.asarray(pcd.points)
+#     print(f"原始点云点数: {len(points)}")
+#
+#     # 计算点云范围
+#     points_range = points.max(axis=0) - points.min(axis=0)
+#     print(f"点云范围: X[{points_range[0]:.3f}], Y[{points_range[1]:.3f}], Z[{points_range[2]:.3f}]")
+#
+#     # 设置阈值
+#     threshold = points_range[2] * 0.02
+#     print(f"使用阈值: {threshold:.4f}")
+#
+#     # ========== 单平面拟合 ==========
+#     # print("\n" + "=" * 50)
+#     # print("单平面拟合")
+#     # print("=" * 50)
+#     #
+#     # plane, inliers = ransac_plane(points, threshold=threshold, max_iterations=1, min_inliers=100)
+#     #
+#     # if plane is not None:
+#     #     a, b, c, d = plane
+#     #     print(f"拟合平面: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0")
+#     #     print(f"内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)")
+#     #     visualize_single_plane(points, plane, inliers)
+#
+#     # ========== 多平面拟合 ==========
+#     print("\n" + "=" * 50)
+#     print("多平面拟合")
+#     print("=" * 50)
+#
+#     planes, inliers_list = sequential_ransac_planes(
+#         points,
+#         threshold=threshold,
+#         max_iterations=1,
+#         min_inliers=100,
+#         max_planes=1
+#     )
+#
+#     print(f"共提取 {len(planes)} 个平面")
+#     for i, (plane, inliers) in enumerate(zip(planes, inliers_list)):
+#         a, b, c, d = plane
+#         print(f"平面 {i + 1}: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0, 内点数: {len(inliers)}")
+#
+#     if len(planes) > 0:
+#         visualize_multiple_planes(points, planes, inliers_list)
+
+import numpy as np
+import open3d as o3d
+import random
+
+
+def create_square_plane_with_noise(size_mm=1000, noise_std_mm=0.5, num_points=10000):
+    """创建正方形平面点云并添加高斯噪声"""
+    size_m = size_mm / 1000.0
+    noise_std_m = noise_std_mm / 1000.0
+
+    half_size = size_m / 2
+    x = np.random.uniform(-half_size, half_size, num_points)
+    y = np.random.uniform(-half_size, half_size, num_points)
+    z = np.zeros(num_points)
+
+    points = np.column_stack([x, y, z])
+    noise_z = np.random.normal(0, noise_std_m, num_points)
+    points[:, 2] += noise_z
+
+    return points
+
+
+def estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000):
+    """使用 Open3D 内置 RANSAC 平面估计"""
+    pcd = o3d.geometry.PointCloud()
+    pcd.points = o3d.utility.Vector3dVector(points)
+
+    threshold_m = threshold_mm / 1000.0
+
+    plane_model, inliers = pcd.segment_plane(
+        distance_threshold=threshold_m,
+        ransac_n=3,
+        num_iterations=max_iterations
+    )
+
+    a, b, c, d = plane_model
+
+    # 计算误差
+    ideal_normal = np.array([0, 0, 1])
+    estimated_normal = np.array([a, b, c])
+    cos_angle = np.dot(ideal_normal, estimated_normal)
+    cos_angle = np.clip(cos_angle, -1, 1)
+    angle_error = np.degrees(np.arccos(cos_angle))
+    offset_error = abs(d)
+
+    inlier_points = points[inliers]
+    distances = np.abs(np.dot(inlier_points, estimated_normal) + d)
+    rmse = np.sqrt(np.mean(distances ** 2))
+
+    print(f"\nRANSAC 平面拟合结果 (Open3D):")
+    print(f"  估计平面: {a:.6f}x + {b:.6f}y + {c:.6f}z + {d:.6f} = 0")
+    print(f"  内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)")
+    print(f"  法线角度误差: {angle_error:.4f} 度")
+    print(f"  偏移误差: {offset_error:.6f} m ({offset_error * 1000:.3f} mm)")
+    print(f"  平面拟合 RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)")
+
+    return plane_model, inliers
+
+
+def visualize_plane_fitting(points, plane):
+    """可视化原始点云和拟合平面"""
+    # 原始点云
+    pcd = o3d.geometry.PointCloud()
+    pcd.points = o3d.utility.Vector3dVector(points)
+    pcd.paint_uniform_color([0.6, 0.8, 1.0])
+
+    # 创建平面点云
+    a, b, c, d = plane
+    x_min, x_max = points[:, 0].min(), points[:, 0].max()
+    y_min, y_max = points[:, 1].min(), points[:, 1].max()
+
+    x = np.linspace(x_min, x_max, 50)
+    y = np.linspace(y_min, y_max, 50)
+    X, Y = np.meshgrid(x, y)
+
+    if abs(c) > 1e-8:
+        Z = -(a * X + b * Y + d) / c
+    else:
+        Z = np.zeros_like(X)
+
+    plane_points = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()])
+
+    pcd_plane = o3d.geometry.PointCloud()
+    pcd_plane.points = o3d.utility.Vector3dVector(plane_points)
+    pcd_plane.paint_uniform_color([1, 0, 0])  # 红色
+
+    o3d.visualization.draw_geometries(
+        [pcd, pcd_plane],
+        window_name="Plane Fitting",
+        width=1024,
+        height=768
+    )
+
+
+# ========== 主程序 ==========
+if __name__ == "__main__":
+    # 参数设置
+    size_mm = 1000
+    noise_std_mm = 0.5
+    num_points = 100000
+
+    print(f"参数:")
+    print(f"  正方形边长: {size_mm} mm")
+    print(f"  噪声标准差: {noise_std_mm} mm")
+    print(f"  点数: {num_points}")
+
+    # 生成点云
+    points = create_square_plane_with_noise(size_mm, noise_std_mm, num_points)
+
+    # 检查数据范围
+    print(f"\n数据范围:")
+    print(f"  X: [{points[:, 0].min():.3f}, {points[:, 0].max():.3f}]")
+    print(f"  Y: [{points[:, 1].min():.3f}, {points[:, 1].max():.3f}]")
+    print(f"  Z: [{points[:, 2].min():.6f}, {points[:, 2].max():.6f}]")
+
+    # 使用 Open3D RANSAC
+    plane, inliers = estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000)
+
+    # 可视化
+    visualize_plane_fitting(points, plane)

alg/point_porj/archive/Point_Fitness.py

@@ -0,0 +1,246 @@
+import copy
+import argparse
+import os
+from scipy.spatial.transform import Rotation as R
+import trimesh
+import numpy as np
+import open3d as o3d
+import time
+
+
+def rotation_matrix_scipy(roll_deg=10, pitch_deg=10, yaw_deg=10):
+    """
+    使用 scipy 生成绕 XYZ 轴旋转的矩阵
+    """
+    r = R.from_euler('xyz', [roll_deg, pitch_deg, yaw_deg], degrees=True)
+    return r.as_matrix()
+
+
+def random_rotation_matrix():
+    """生成随机旋转矩阵"""
+    angle = np.random.uniform(0, 2 * np.pi)
+    axis = np.random.randn(3)
+    axis = axis / np.linalg.norm(axis)
+
+    K = np.array([
+        [0, -axis[2], axis[1]],
+        [axis[2], 0, -axis[0]],
+        [-axis[1], axis[0], 0]
+    ])
+    R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K
+    return R
+
+
+def random_translation_vector(range_xyz=0.5):
+    """生成随机平移向量"""
+    return np.random.uniform(-range_xyz, range_xyz, 3)
+
+
+def transform_point_cloud(pcd, R, t):
+    """对点云应用变换"""
+    transformed_pcd = copy.deepcopy(pcd)
+    points = np.asarray(transformed_pcd.points)
+    points_transformed = (R @ points.T).T + t
+    transformed_pcd.points = o3d.utility.Vector3dVector(points_transformed)
+    return transformed_pcd
+
+
+def registration_open3d(source_pcd, target_pcd, voxel_size=0.005):
+    """
+    点云配准（RANSAC + ICP）
+
+    Returns:
+        transformation: 4x4 变换矩阵
+        fitness: 配准 fitness 分数
+        rmse: 配准 RMSE
+    """
+    # 1. 下采样
+    source_down = source_pcd.voxel_down_sample(voxel_size)
+    target_down = target_pcd.voxel_down_sample(voxel_size)
+
+    # 2. 估计法线
+    radius = voxel_size * 2
+    source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30))
+    target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30))
+
+    # 3. 计算 FPFH 特征
+    radius_feature = voxel_size * 5
+    source_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        source_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
+    target_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        target_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
+
+    # 4. RANSAC 粗配准
+    distance_threshold = voxel_size * 1.5
+    result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+        source_down, target_down, source_fpfh, target_fpfh,
+        mutual_filter=True,
+        max_correspondence_distance=distance_threshold,
+        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
+        ransac_n=3,
+        checkers=[
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)
+        ],
+        criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
+
+    print(f"RANSAC 配准完成")
+    source_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
+    target_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
+
+    # 5. ICP 精配准
+    result_icp = o3d.pipelines.registration.registration_icp(
+        source_pcd, target_pcd, distance_threshold / 2, result_ransac.transformation,
+        o3d.pipelines.registration.TransformationEstimationPointToPlane(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200))
+
+    print(f"ICP 精配准完成")
+    print(f"  Fitness: {result_icp.fitness:.4f}")
+    print(f"  RMSE: {result_icp.inlier_rmse:.6f} m")
+
+    return result_icp.transformation, result_icp.fitness, result_icp.inlier_rmse
+
+
+def visualize_registration_result(original_pcd, transformed_pcd, registered_pcd):
+    """可视化原始点云、变换后点云和配准后点云"""
+    source_colored = copy.deepcopy(original_pcd)
+    target_colored = copy.deepcopy(transformed_pcd)
+    registered_colored = copy.deepcopy(registered_pcd)
+
+    source_colored.paint_uniform_color([0, 0, 1])  # 蓝色 - 原始点云
+    target_colored.paint_uniform_color([0, 1, 0])  # 绿色 - 目标点云
+    registered_colored.paint_uniform_color([1, 0, 0])  # 红色 - 配准后点云
+
+    o3d.visualization.draw_geometries(
+        [source_colored, target_colored],
+        window_name="Before Registration (Blue: Source, Green: Target)",
+        width=1024, height=768
+    )
+
+    o3d.visualization.draw_geometries(
+        [registered_colored, target_colored],
+        window_name="After Registration (Red: Registered, Green: Target)",
+        width=1024, height=768
+    )
+
+
+def visualize_registration_comparison(original_pcd, transformed_pcd, registered_pcd):
+    """并排可视化配准前后的对比"""
+    source_before = copy.deepcopy(original_pcd)
+    source_before.paint_uniform_color([0, 0, 1])  # 蓝色
+
+    target_before = copy.deepcopy(transformed_pcd)
+    target_before.paint_uniform_color([0, 1, 0])  # 绿色
+
+    registered_after = copy.deepcopy(registered_pcd)
+    registered_after.paint_uniform_color([1, 0, 0])  # 红色
+
+    o3d.visualization.draw_geometries(
+        [source_before, target_before],
+        window_name="Before Registration (Blue: Source, Green: Target)",
+        width=1024, height=768
+    )
+
+    o3d.visualization.draw_geometries(
+        [registered_after, target_before],
+        window_name="After Registration (Red: Registered, Green: Target)",
+        width=1024, height=768
+    )
+
+
+def compute_point_to_mesh_distance_trimesh(points, mesh_path):
+    """使用 trimesh 计算点云到 Mesh 的距离"""
+    mesh_trimesh = trimesh.load(mesh_path)
+    closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points)
+    return distances
+
+
+# ========== 主程序 ==========
+def main():
+    parser = argparse.ArgumentParser(description='点云配准与 Mesh 距离计算')
+    parser.add_argument('--ply_path', '-p', type=str, default="./High_Speed_Data/Noise_PointCloud/11_noisy.ply",
+                        help='输入点云文件路径 (.ply)')
+    parser.add_argument('--mesh_path', '-m', type=str, default="./High_Speed_Data/Mesh/11.stl",
+                        help='输入 Mesh 文件路径 (.stl, .ply, .obj 等)')
+    parser.add_argument('--voxel_size', '-v', type=float, default=10,
+                        help='体素滤波大小 (默认: 10)')
+    parser.add_argument('--roll', type=float, default=10,
+                        help='绕 X 轴旋转角度 (度, 默认: 10)')
+    parser.add_argument('--pitch', type=float, default=10,
+                        help='绕 Y 轴旋转角度 (度, 默认: 10)')
+    parser.add_argument('--yaw', type=float, default=10,
+                        help='绕 Z 轴旋转角度 (度, 默认: 10)')
+    parser.add_argument('--trans_range', type=float, default=0,
+                        help='平移范围 (米, 默认: 0)')
+    parser.add_argument('--no_visualize', action='store_true',
+                        help='不显示可视化窗口')
+
+    args = parser.parse_args()
+
+    # 检查文件是否存在
+    if not os.path.exists(args.ply_path):
+        print(f"错误: 点云文件不存在 - {args.ply_path}")
+        return
+
+    if not os.path.exists(args.mesh_path):
+        print(f"错误: Mesh 文件不存在 - {args.mesh_path}")
+        return
+
+    # 1. 读取原始点云
+    print(f"读取点云: {args.ply_path}")
+    source_pcd = o3d.io.read_point_cloud(args.ply_path)
+
+    if len(source_pcd.points) == 0:
+        print("错误: 点云读取失败")
+        return
+
+    print(f"原始点云点数: {len(source_pcd.points)}")
+
+    # 2. 生成变换
+    R = rotation_matrix_scipy(args.roll, args.pitch, args.yaw)
+    t = random_translation_vector(range_xyz=args.trans_range)
+
+    print(f"旋转矩阵:\n{R}")
+    print(f"平移向量: {t}")
+
+    # 3. 创建目标点云（变换后的源点云）
+    target_pcd = transform_point_cloud(source_pcd, R, t)
+
+    # 4. 点云配准
+    print("\n开始点云配准...")
+    t0 = time.time()
+    transformation, fitness, rmse = registration_open3d(source_pcd, target_pcd, voxel_size=args.voxel_size)
+
+    # 5. 将目标点云变换回源点云坐标系
+    inv_transformation = np.linalg.inv(transformation)
+    target_recovered = copy.deepcopy(target_pcd)
+    target_recovered.transform(inv_transformation)
+
+    t1 = time.time()
+    print(f"\n配准结果:")
+    print(f"  Fitness: {fitness:.4f}")
+    print(f"  RMSE: {rmse:.6f} m")
+    print(f"  估计的变换矩阵:\n{transformation}")
+    print(f"配准耗时: {t1 - t0:.4f} 秒")
+
+    # 6. 计算点到 Mesh 的距离
+    print("\n计算点到 Mesh 距离...")
+    t2 = time.time()
+    points = np.asarray(target_recovered.points)
+    distances = compute_point_to_mesh_distance_trimesh(points, args.mesh_path)
+    t3 = time.time()
+
+    print(f"距离统计:")
+    print(f"  平均距离: {np.mean(distances):.6f} mm ({np.mean(distances) * 1000:.3f} mm)")
+    print(f"  最大距离: {np.max(distances):.6f} mm ({np.max(distances) * 1000:.3f} mm)")
+    print(f"  RMSE: {np.sqrt(np.mean(distances ** 2)):.6f} m")
+    print(f"距离计算耗时: {t3 - t2:.4f} 秒")
+
+    # 7. 可视化
+    if not args.no_visualize:
+        print("\n开始可视化...")
+        visualize_registration_comparison(source_pcd, target_pcd, target_recovered)
+
+
+if __name__ == "__main__":
+    main()

alg/point_porj/archive/Readme.md

@@ -0,0 +1,270 @@
+# 工业零件几何测量工具使用指南
+> 支持测量种类：平整度、平面度、轮廓测量、曲率分析、点云去噪、平面拟合、点云配准评估
+> 支持测量任务编号：11、13、17、20、25、26、27、31、34、35、36、37、38、39、40、41、42、45
+
+## 一、环境配置
+### 基础环境要求
+- Python 3.10.8
+- CUDA 11.8（建议，无CUDA时自动降级为CPU计算）
+- PyTorch 2.2
+
+### 依赖安装
+```bash
+conda install -f requirements.txt
+```
+> 注：需确保环境中已安装OCC Core（STEP/STL处理）、trimesh、open3d、numpy、scipy等依赖
+
+---
+
+## 二、CAD转轻量化Mesh（CAD2Mesh.py）
+### 功能说明
+将STEP格式的CAD模型转换为轻量化STL网格文件，通过调整网格精度控制文件体积（精度值越大，文件越小，精度越低），纯后台处理无界面显示，转换效率高。
+
+### 输入输出
+- **输入**：STEP格式CAD文件（路径示例：`High_Speed_Data/CAD/11.step`）
+- **输出**：STL格式轻量化网格文件（路径示例：`High_Speed_Data/Mesh/11.stl`）
+
+### 命令行参数说明
+| 参数         | 简写 | 类型   | 默认值                          | 说明                                   |
+|--------------|------|--------|---------------------------------|----------------------------------------|
+| --precision  | -p   | float  | 1.0                             | 网格精度（单位：mm），值越大文件体积越小 |
+| --input      | -i   | string | High_Speed_Data/CAD/11.step     | 输入STEP文件路径                       |
+| --output     | -o   | string | High_Speed_Data/Mesh/11.stl     | 输出STL文件路径                        |
+
+### 运行示例
+```bash
+# 完整参数形式
+python CAD2Mesh.py --precision 1 --input "High_Speed_Data/CAD/11.step" --output "High_Speed_Data/Mesh/11.stl"
+
+# 简写参数形式
+python CAD2Mesh.py -p 1 -i "High_Speed_Data/CAD/11.step" -o "High_Speed_Data/Mesh/11.stl"
+```
+
+### 运行日志说明
+程序会输出三步处理日志及耗时：
+1. STEP文件读取阶段
+2. 网格离散化（轻量化）阶段
+3. STL文件保存阶段
+转换完成后会提示总耗时及STL文件打开工具建议（FreeCAD/MeshLab/3D Viewer）。
+
+---
+
+## 三、轮廓-平整度测量（Contour_Flatness.py）
+### 功能说明
+高精度计算点云到 CAD STEP 模型三角化表面的最近距离，输出平均距离（mean_dist）、最大距离（max_dist）和最小距离（min_dist），用于评估零件的平整度、平面度、轮廓偏差等几何精度指标；支持 CUDA 加速（自动检测），采用 torch.float64 高精度计算。
+
+脚本现在支持两种模式：
+- 默认批量模式：按任务编号从 `High_Speed_Data/CAD/` 和点云目录中读取多组数据，并输出 Excel 汇总。
+- 单任务模式：添加 `--single` 后，使用 `--CAD` 和 `--pointcloud` 计算一组文件，可选 `--visualize` 显示热力图。
+
+### 输入输出
+- **输入**：
+  - CAD STEP 文件：路径示例 `High_Speed_Data/CAD/11.step`
+  - 点云 PLY 文件：批量模式默认读取 `High_Speed_Data/Noise_PointCloud/{id}_noisy.ply`，也可用 `--pointcloud_kind clean` 读取 `High_Speed_Data/PointCloud/{id}.ply`
+- **输出**：
+  - 平均距离（mean_dist）：点云中所有点到 CAD 三角面的平均最近距离（单位：mm）
+  - 最大距离（max_dist）：点云中所有点到 CAD 三角面的最大最近距离（单位：mm）
+  - 最小距离（min_dist）：点云中所有点到 CAD 三角面的最小最近距离（单位：mm）
+  - 批量模式会生成 Excel 汇总文件，默认 `平整度.xlsx`
+
+### 命令行参数说明
+| 参数 | 简写 | 类型 | 默认值 | 说明 |
+|------|------|------|--------|------|
+| --CAD | -c | string | High_Speed_Data/CAD/11.step | 单任务模式 CAD STEP 文件路径，仅 `--single` 时使用 |
+| --pointcloud | -p | string | High_Speed_Data/Noise_PointCloud/11_noisy.ply | 单任务模式点云路径，仅 `--single` 时使用 |
+| --tasks | - | string | 11,13,17,20,25,26,27,31,34,35,36,37,38,39,40,41,42,45 | 批量任务编号，多个编号用英文逗号分隔 |
+| --data_root | - | string | High_Speed_Data | 批量任务数据根目录 |
+| --pointcloud_kind | - | string | noise | 批量模式点云来源：`noise` 读取 Noise_PointCloud，`clean` 读取 PointCloud |
+| --excel_output | - | string | 平整度.xlsx | 批量结果 Excel 输出路径 |
+| --single | - | flag | false | 启用单任务模式 |
+| --visualize | - | flag | false | 单任务模式下显示距离热力图 |
+| --mesh_deflection | - | float | 0.1 | CAD 三角化精度，越小越精细但越慢 |
+| --point_chunk_size | - | int | 2048 | 点云分块大小，显存/内存不足时可调小 |
+| --triangle_chunk_size | - | int | 4096 | 三角面分块大小，显存/内存不足时可调小 |
+
+### 运行示例
+```bash
+# 默认批量模式，使用 DEFAULT_TASK_IDS 并输出 平整度.xlsx
+python Contour_Flatness.py
+
+# 指定批量任务、点云来源和 Excel 输出
+python Contour_Flatness.py --tasks "11,31,40" --data_root "High_Speed_Data" --pointcloud_kind noise --excel_output "vis/flatness.xlsx"
+
+# 单任务模式
+python Contour_Flatness.py --single --CAD "High_Speed_Data/CAD/11.step" --pointcloud "High_Speed_Data/Noise_PointCloud/11_noisy.ply"
+
+# 单任务模式并显示热力图
+python Contour_Flatness.py --single -c "High_Speed_Data/CAD/11.step" -p "High_Speed_Data/PointCloud/11.ply" --visualize
+```
+
+### 计算说明
+1. CAD 读取：通过 pythonocc-core 读取 STEP，并使用 `mesh_deflection` 三角化。
+2. 点云读取：优先使用 open3d（若安装），否则使用 trimesh 兼容读取。
+3. 距离计算：点云和 CAD 三角面都分块处理，避免内存/显存溢出，支持大规模点云计算。
+4. 批量结果：每个任务会记录成功/失败状态，成功任务会把 `points_np` 和 `distances_np` 缓存在 `task_distance_data` 中，便于后续可视化。
+5. 精度保障：采用 torch.float64 高精度计算，支持 GPU/CPU 自动切换。
+
+---
+
+## 四、点云曲率估计（curvature_estimation.py）
+### 功能说明
+基于Open3D对工业零件点云进行曲率计算，通过邻域协方差矩阵分解获取曲率值，支持曲率彩色可视化，用于识别零件边缘、棱角、曲面等特征区域，为表面缺陷检测提供依据。
+
+### 输入输出
+- **输入**：PLY格式点云文件（路径示例：`High_Speed_Data/PointCloud/11.ply`）
+- **输出**：曲率统计值、彩色可视化点云、可保存带曲率信息的PLY文件
+
+### 命令行参数说明
+| 参数         | 简写 | 类型   | 默认值                          | 说明                                   |
+|--------------|------|--------|---------------------------------|----------------------------------------|
+| --ply_path   | —    | string | ./High_Speed_Data/PointCloud/11.ply | 输入点云路径                      |
+| --radius     | —    | float  | 2.0                             | 邻域搜索半径                           |
+| --max_nn     | —    | int    | 30                              | 最大邻域点数                           |
+| --color_scale| —    | float  | 3.0                             | 曲率颜色缩放因子                       |
+| --save       | —    | string | None                            | 保存着色点云路径（可选）|
+
+### 运行示例
+```bash
+# 基础运行
+python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply"
+
+# 保存结果
+python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply" --save "High_Speed_Data/Result/11_curvature.ply"
+```
+
+### 计算说明
+1. 自动估计法线并构建KD-Tree加速邻域搜索
+2. 邻域点不足3个时自动标记为高曲率
+3. 颜色映射：红色=高曲率，绿色=低曲率
+
+---
+
+## 五、点云去噪处理（denoise.py）
+### 功能说明
+集成SOR、ROR、PF、VGF、BF五种去噪与滤波算法，自动添加高斯噪声并对比效果，通过Chamfer Distance量化评估去噪质量，用于扫描点云预处理，提升测量精度。
+
+### 输入输出
+- **输入**：PLY格式干净点云（路径示例：`High_Speed_Data/PointCloud/11.ply`）
+- **输出**：各算法去噪指标、保留率、移除点数、CD距离、可视化对比
+
+### 命令行参数说明
+| 参数         | 简写 | 类型   | 默认值                          | 说明                                   |
+|--------------|------|--------|---------------------------------|----------------------------------------|
+| --ply_path   | -p   | string | ./High_Speed_Data/PointCloud/11.ply | 输入点云路径                       |
+| --noise_std  | -n   | float  | 4.0                             | 高斯噪声标准差                         |
+| --voxel_size | -v   | float  | 0.05                            | 体素滤波大小                           |
+| --no_visualize | —  | bool   | False                           | 关闭可视化窗口                        |
+
+### 运行示例
+```bash
+# 基础去噪对比
+python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --noise_std 2.0
+
+# 无可视化运行
+python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --no_visualize
+```
+
+### 计算说明
+1. 自动给点云加噪，模拟真实扫描噪声
+2. 依次运行SOR/ROR/PF/VGF/BF算法
+3. 输出保留率、移除点数、Chamfer Distance评估结果
+
+---
+
+## 六、平面拟合估计（plane_estimation.py）
+### 功能说明
+基于Open3D内置RANSAC算法自动拟合点云中平面结构，输出平面方程、内点占比、法线误差、拟合RMSE，用于提取零件基准平面。
+
+### 输入输出
+- **输入**：可自定义生成带噪平面点云
+- **输出**：平面方程、误差统计、拟合可视化
+
+### 命令行参数说明
+| 参数           | 简写 | 类型   | 默认值 | 说明                     |
+|----------------|------|--------|--------|--------------------------|
+| --threshold_mm | -t   | float  | 0.5    | 平面距离阈值（mm）|
+| --max_iterations | -i | int    | 2000   | RANSAC迭代次数           |
+
+### 运行示例
+```bash
+# 默认运行
+python plane_estimation.py
+
+# 自定义参数
+python plane_estimation.py --threshold_mm 0.3 --max_iterations 1500
+```
+
+### 计算说明
+1. 支持生成带噪声标准平面用于测试
+2. 输出平面法向量与理想Z轴夹角误差、偏移误差、拟合RMSE
+
+---
+
+## 七、点云配准与适应度评估（point-fitness.py）
+### 功能说明
+对点云执行旋转/平移变换，通过RANSAC+ICP完成高精度配准，输出配准适应度Fitness、RMSE，并计算配准后点云到参考网格的距离，用于评估配准精度与零件偏差。
+
+### 输入输出
+- **输入**：待配准PLY点云、参考STL网格
+- **输出**：变换矩阵、配准分数、误差、点云到网格距离统计、配准可视化
+
+### 命令行参数说明
+| 参数         | 简写 | 类型   | 默认值                                      | 说明               |
+|--------------|------|--------|---------------------------------------------|--------------------|
+| --ply_path   | -p   | string | ./High_Speed_Data/Noise_PointCloud/11_noisy.ply | 点云路径       |
+| --mesh_path  | -m   | string | ./High_Speed_Data/Mesh/11.stl               | 网格路径           |
+| --voxel_size | -v   | float  | 10                                          | 配准下采样大小     |
+| --no_visualize | —  | bool   | False                                       | 关闭可视化         |
+
+### 运行示例
+```bash
+# 完整配准+距离计算
+python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl" -v 5
+
+# 无可视化
+python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl" --no_visualize
+```
+
+### 计算说明
+1. 支持指定XYZ三轴旋转与随机平移
+2. RANSAC粗配准 → ICP精配准
+3. 输出Fitness（越高越好）、RMSE（越低越好）
+
+---
+
+## 八、完整工作流程示例（任务11全流程）
+1. CAD转轻量化网格
+```bash
+python CAD2Mesh.py -p 0.5 -i "High_Speed_Data/CAD/11.step" -o "High_Speed_Data/Mesh/11.stl"
+```
+2. 点云去噪预处理
+```bash
+python denoise.py -p "High_Speed_Data/PointCloud/11.ply" --noise_std 2.0
+```
+3. 平面基准拟合
+```bash
+python plane_estimation.py
+```
+4. 曲率特征分析
+```bash
+python curvature_estimation.py --ply_path "High_Speed_Data/PointCloud/11.ply"
+```
+5. 轮廓-平整度测量
+```bash
+python Contour_Flatness.py --tasks "11,31,40" --data_root "High_Speed_Data" --excel_output "vis/flatness.xlsx"
+```
+6. 点云配准评估
+```bash
+python point-fitness.py -p "High_Speed_Data/Noise_PointCloud/11_noisy.ply" -m "High_Speed_Data/Mesh/11.stl"
+```
+
+---
+
+## 九、常见问题
+1. STEP文件读取失败：检查路径、文件完整性、OCC Core安装
+2. 点云读取报错：安装open3d（`pip install open3d`）
+3. CUDA Out of Memory：减小分块大小或降低点云密度
+4. 去噪效果差：调小SOR标准差、增大ROR半径
+5. 平面拟合失败：提高RANSAC迭代次数、降低距离阈值
+6. 配准Fitness低：减小voxel_size，降低旋转/平移幅度
+7. 可视化无窗口：服务器环境加`--no_visualize`参数

alg/point_porj/archive/generate_shifted_train_pointcloud.py

@@ -0,0 +1,447 @@
+import os
+import argparse
+import numpy as np
+import open3d as o3d
+
+from OCC.Core.STEPControl import STEPControl_Reader
+from OCC.Core.IFSelect import IFSelect_RetDone
+from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh
+from OCC.Core.TopExp import TopExp_Explorer
+from OCC.Core.TopAbs import TopAbs_FACE
+from OCC.Core.BRep import BRep_Tool
+
+try:
+    from OCC.Core.TopoDS import topods
+    USE_TOPODS = "topods"
+except ImportError:
+    from OCC.Core.TopoDS import TopoDS
+    USE_TOPODS = "TopoDS"
+
+
+# ============================================================
+# STEP -> Open3D Mesh
+# ============================================================
+
+def read_step_file(step_path):
+    reader = STEPControl_Reader()
+    status = reader.ReadFile(step_path)
+
+    if status != IFSelect_RetDone:
+        raise RuntimeError(f"STEP 文件读取失败: {step_path}")
+
+    reader.TransferRoots()
+    shape = reader.OneShape()
+
+    print("[OK] STEP loaded:", step_path)
+    return shape
+
+
+def occ_shape_to_numpy_mesh(shape):
+    vertices = []
+    triangles = []
+
+    explorer = TopExp_Explorer(shape, TopAbs_FACE)
+
+    face_count = 0
+    tri_count = 0
+
+    while explorer.More():
+        current_shape = explorer.Current()
+
+        if USE_TOPODS == "topods":
+            face = topods.Face(current_shape)
+        else:
+            face = TopoDS.Face(current_shape)
+
+        location = face.Location()
+        triangulation = BRep_Tool.Triangulation(face, location)
+
+        if triangulation is None:
+            explorer.Next()
+            continue
+
+        trsf = location.Transformation()
+        base_index = len(vertices)
+
+        for i in range(1, triangulation.NbNodes() + 1):
+            pnt = triangulation.Node(i)
+            pnt.Transform(trsf)
+
+            vertices.append([
+                pnt.X(),
+                pnt.Y(),
+                pnt.Z()
+            ])
+
+        for i in range(1, triangulation.NbTriangles() + 1):
+            tri = triangulation.Triangle(i)
+            n1, n2, n3 = tri.Get()
+
+            triangles.append([
+                base_index + n1 - 1,
+                base_index + n2 - 1,
+                base_index + n3 - 1
+            ])
+
+            tri_count += 1
+
+        face_count += 1
+        explorer.Next()
+
+    vertices = np.asarray(vertices, dtype=np.float64)
+    triangles = np.asarray(triangles, dtype=np.int32)
+
+    print("[OK] CAD mesh extracted")
+    print("     CAD faces:", face_count)
+    print("     mesh vertices:", vertices.shape)
+    print("     mesh triangles:", triangles.shape)
+
+    if vertices.shape[0] == 0 or triangles.shape[0] == 0:
+        raise RuntimeError("CAD 三角网格提取失败")
+
+    return vertices, triangles
+
+
+def step_to_open3d_mesh(step_path, mesh_deflection=0.1):
+    shape = read_step_file(step_path)
+
+    print("[INFO] Meshing CAD...")
+    mesh_algo = BRepMesh_IncrementalMesh(shape, mesh_deflection)
+    mesh_algo.Perform()
+
+    vertices, triangles = occ_shape_to_numpy_mesh(shape)
+
+    mesh = o3d.geometry.TriangleMesh()
+    mesh.vertices = o3d.utility.Vector3dVector(vertices)
+    mesh.triangles = o3d.utility.Vector3iVector(triangles)
+
+    mesh.remove_duplicated_vertices()
+    mesh.remove_duplicated_triangles()
+    mesh.remove_degenerate_triangles()
+    mesh.compute_vertex_normals()
+
+    return mesh
+
+
+# ============================================================
+# 点云处理
+# ============================================================
+
+def make_translation_matrix(dx, dy, dz):
+    T = np.eye(4, dtype=np.float64)
+    T[0, 3] = dx
+    T[1, 3] = dy
+    T[2, 3] = dz
+    return T
+
+
+def crop_front_tail_by_x(pcd, ratio=0.08):
+    """
+    按 X 轴裁剪车头和车尾。
+
+    front: X 最小端
+    tail:  X 最大端
+    """
+
+    points = np.asarray(pcd.points)
+
+    min_bound = points.min(axis=0)
+    max_bound = points.max(axis=0)
+
+    x_min = min_bound[0]
+    x_max = max_bound[0]
+    x_len = x_max - x_min
+
+    front_min = min_bound.copy()
+    front_max = max_bound.copy()
+    front_min[0] = x_min
+    front_max[0] = x_min + ratio * x_len
+
+    tail_min = min_bound.copy()
+    tail_max = max_bound.copy()
+    tail_min[0] = x_max - ratio * x_len
+    tail_max[0] = x_max
+
+    front_box = o3d.geometry.AxisAlignedBoundingBox(
+        min_bound=front_min,
+        max_bound=front_max
+    )
+
+    tail_box = o3d.geometry.AxisAlignedBoundingBox(
+        min_bound=tail_min,
+        max_bound=tail_max
+    )
+
+    front_pcd = pcd.crop(front_box)
+    tail_pcd = pcd.crop(tail_box)
+
+    print("[OK] Crop front/tail by X")
+    print("     X range:", x_min, x_max)
+    print("     front points:", np.asarray(front_pcd.points).shape)
+    print("     tail points:", np.asarray(tail_pcd.points).shape)
+
+    return front_pcd, tail_pcd
+
+
+def add_gaussian_noise(pcd, noise_std=0.0):
+    """
+    给点云添加高斯噪声。
+    noise_std=0 表示不加噪声。
+    """
+
+    if noise_std <= 0:
+        return pcd
+
+    points = np.asarray(pcd.points)
+    noise = np.random.normal(
+        loc=0.0,
+        scale=noise_std,
+        size=points.shape
+    )
+
+    noisy_points = points + noise
+
+    noisy_pcd = o3d.geometry.PointCloud()
+    noisy_pcd.points = o3d.utility.Vector3dVector(noisy_points)
+
+    if pcd.has_colors():
+        noisy_pcd.colors = pcd.colors
+
+    if pcd.has_normals():
+        noisy_pcd.normals = pcd.normals
+
+    return noisy_pcd
+
+
+def visualize_pair(cad_pcd, shifted_pcd):
+    cad_vis = o3d.geometry.PointCloud(cad_pcd)
+    shifted_vis = o3d.geometry.PointCloud(shifted_pcd)
+
+    cad_vis.paint_uniform_color([0.0, 0.5, 1.0])
+    shifted_vis.paint_uniform_color([1.0, 0.0, 0.0])
+
+    o3d.visualization.draw_geometries(
+        [cad_vis, shifted_vis],
+        window_name="Blue = CAD sampled point cloud, Red = shifted test point cloud"
+    )
+
+
+# ============================================================
+# 主函数
+# ============================================================
+
+def generate_shifted_train_pointcloud(
+    cad_path,
+    output_dir,
+    sample_points=1000000,
+    mesh_deflection=0.1,
+    crop_ratio=0.08,
+    dx=100.0,
+    dy=20.0,
+    dz=-10.0,
+    noise_std=0.0,
+    visualize=True
+):
+    os.makedirs(output_dir, exist_ok=True)
+
+    # 1. CAD -> mesh
+    cad_mesh = step_to_open3d_mesh(
+        cad_path,
+        mesh_deflection=mesh_deflection
+    )
+
+    # 2. mesh -> 点云
+    print(f"[INFO] Sampling CAD surface: {sample_points} points")
+    cad_pcd = cad_mesh.sample_points_uniformly(
+        number_of_points=sample_points
+    )
+
+    cad_pcd_path = os.path.join(output_dir, "cad_sampled_whole.ply")
+    o3d.io.write_point_cloud(cad_pcd_path, cad_pcd)
+    print("[OK] Saved:", cad_pcd_path)
+
+    # 3. 裁剪原始 CAD 点云的车头 / 车尾
+    cad_front, cad_tail = crop_front_tail_by_x(
+        cad_pcd,
+        ratio=crop_ratio
+    )
+
+    o3d.io.write_point_cloud(
+        os.path.join(output_dir, "cad_front.ply"),
+        cad_front
+    )
+
+    o3d.io.write_point_cloud(
+        os.path.join(output_dir, "cad_tail.ply"),
+        cad_tail
+    )
+
+    # 4. 生成固定平移矩阵
+    T = make_translation_matrix(dx, dy, dz)
+
+    print("\n========== Fixed Translation ==========")
+    print("dx:", dx)
+    print("dy:", dy)
+    print("dz:", dz)
+    print("T:")
+    print(T)
+
+    # 5. 对整车点云施加固定平移
+    shifted_pcd = o3d.geometry.PointCloud(cad_pcd)
+    shifted_pcd.transform(T)
+
+    # 6. 可选：加噪声，模拟真实扫描点云
+    shifted_pcd = add_gaussian_noise(
+        shifted_pcd,
+        noise_std=noise_std
+    )
+
+    shifted_path = os.path.join(output_dir, "shifted_whole_pointcloud.ply")
+    o3d.io.write_point_cloud(shifted_path, shifted_pcd)
+    print("[OK] Saved:", shifted_path)
+
+    # 7. 裁剪平移后的车头 / 车尾
+    shifted_front, shifted_tail = crop_front_tail_by_x(
+        shifted_pcd,
+        ratio=crop_ratio
+    )
+
+    shifted_front_path = os.path.join(output_dir, "shifted_front.ply")
+    shifted_tail_path = os.path.join(output_dir, "shifted_tail.ply")
+
+    o3d.io.write_point_cloud(shifted_front_path, shifted_front)
+    o3d.io.write_point_cloud(shifted_tail_path, shifted_tail)
+
+    print("[OK] Saved:", shifted_front_path)
+    print("[OK] Saved:", shifted_tail_path)
+
+    # 8. 保存真实位移矩阵
+    matrix_path = os.path.join(output_dir, "GT_translation_matrix.txt")
+    np.savetxt(matrix_path, T, fmt="%.10f")
+    print("[OK] Saved:", matrix_path)
+
+    # 9. 保存真实位移向量
+    translation_path = os.path.join(output_dir, "GT_translation_vector.txt")
+    np.savetxt(
+        translation_path,
+        np.array([dx, dy, dz], dtype=np.float64),
+        fmt="%.10f"
+    )
+    print("[OK] Saved:", translation_path)
+
+    # 10. 可视化
+    if visualize:
+        # 百万级点云显示会卡，所以这里可视化时降采样
+        cad_vis = cad_pcd.voxel_down_sample(voxel_size=20.0)
+        shifted_vis = shifted_pcd.voxel_down_sample(voxel_size=20.0)
+
+        visualize_pair(cad_vis, shifted_vis)
+
+    return {
+        "cad_sampled_path": cad_pcd_path,
+        "shifted_pointcloud_path": shifted_path,
+        "shifted_front_path": shifted_front_path,
+        "shifted_tail_path": shifted_tail_path,
+        "translation_matrix_path": matrix_path,
+        "translation_vector": np.array([dx, dy, dz], dtype=np.float64)
+    }
+
+
+# ============================================================
+# 命令行入口
+# ============================================================
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(
+        description="从整车 CAD 生成测试点云，并施加固定前后/整体位移"
+    )
+
+    parser.add_argument(
+        "--cad",
+        default=r"datasets/CAD/M02车.stp",
+        help="整车 CAD STEP/STP 文件路径"
+    )
+
+    parser.add_argument(
+        "--output_dir",
+        default=r"datasets\shift_test_output",
+        help="输出目录"
+    )
+
+    parser.add_argument(
+        "--sample_points",
+        type=int,
+        default=1000000,
+        help="从 CAD 表面采样的点数，默认 1000000"
+    )
+
+    parser.add_argument(
+        "--mesh_deflection",
+        type=float,
+        default=0.1,
+        help="CAD 三角化精度，越小越精细，默认 0.1"
+    )
+
+    parser.add_argument(
+        "--crop_ratio",
+        type=float,
+        default=0.08,
+        help="车头/车尾裁剪比例，默认 0.08"
+    )
+
+    parser.add_argument(
+        "--dx",
+        type=float,
+        default=100.0,
+        help="X 方向固定平移量"
+    )
+
+    parser.add_argument(
+        "--dy",
+        type=float,
+        default=20.0,
+        help="Y 方向固定平移量"
+    )
+
+    parser.add_argument(
+        "--dz",
+        type=float,
+        default=-10.0,
+        help="Z 方向固定平移量"
+    )
+
+    parser.add_argument(
+        "--noise_std",
+        type=float,
+        default=0.0,
+        help="高斯噪声标准差，默认 0 表示不加噪声"
+    )
+
+    parser.add_argument(
+        "--no_visualize",
+        action="store_true",
+        help="不显示可视化窗口"
+    )
+
+    args = parser.parse_args()
+
+    result = generate_shifted_train_pointcloud(
+        cad_path=args.cad,
+        output_dir=args.output_dir,
+        sample_points=args.sample_points,
+        mesh_deflection=args.mesh_deflection,
+        crop_ratio=args.crop_ratio,
+        dx=args.dx,
+        dy=args.dy,
+        dz=args.dz,
+        noise_std=args.noise_std,
+        visualize=not args.no_visualize
+    )
+
+    print("\n========== Result ==========")
+    print("CAD sampled point cloud:", result["cad_sampled_path"])
+    print("Shifted whole point cloud:", result["shifted_pointcloud_path"])
+    print("Shifted front point cloud:", result["shifted_front_path"])
+    print("Shifted tail point cloud:", result["shifted_tail_path"])
+    print("GT translation vector:", result["translation_vector"])
+    print("GT matrix:", result["translation_matrix_path"])

alg/point_porj/archive/relative_code/3D Douglas-Peucker.py

@@ -0,0 +1,362 @@
+import numpy as np
+from scipy.spatial import KDTree
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+
+def point_to_line_distance(point, line_start, line_end):
+    """
+    计算点到三维线段的最短距离
+
+    Args:
+        point: 待计算点 (x, y, z)
+        line_start: 线段起点 (x, y, z)
+        line_end: 线段终点 (x, y, z)
+
+    Returns:
+        点到线段的垂直距离
+    """
+    p = np.array(point)
+    a = np.array(line_start)
+    b = np.array(line_end)
+
+    # 计算向量
+    ab = b - a
+    ap = p - a
+
+    # 投影参数 t
+    t = np.dot(ap, ab) / np.dot(ab, ab)
+
+    # 找到线段上的最近点
+    if t <= 0:
+        closest = a
+    elif t >= 1:
+        closest = b
+    else:
+        closest = a + t * ab
+
+    # 计算距离
+    distance = np.linalg.norm(p - closest)
+    return distance
+
+
+def point_to_plane_distance(point, plane_point1, plane_point2, plane_point3):
+    """
+    计算点到三维平面的距离（三点确定一个平面）
+
+    Args:
+        point: 待计算点 (x, y, z)
+        plane_point1, plane_point2, plane_point3: 平面上的三个点
+
+    Returns:
+        点到平面的垂直距离
+    """
+    p = np.array(point)
+    p1 = np.array(plane_point1)
+    p2 = np.array(plane_point2)
+    p3 = np.array(plane_point3)
+
+    # 计算平面法向量
+    v1 = p2 - p1
+    v2 = p3 - p1
+    normal = np.cross(v1, v2)
+    normal = normal / np.linalg.norm(normal)
+
+    # 计算点到平面的距离
+    distance = abs(np.dot(normal, p - p1))
+    return distance
+
+
+def douglas_peucker_3d(points, epsilon, use_plane=False):
+    """
+    3D Douglas-Peucker 算法
+
+    Args:
+        points: 三维点列表或numpy数组，形状 (N, 3)
+        epsilon: 容差阈值
+        use_plane: 是否使用平面距离（True）还是线段距离（False）
+
+    Returns:
+        简化后的点云列表
+    """
+    if len(points) <= 2:
+        return points.tolist() if isinstance(points, np.ndarray) else points
+
+    # 转换为列表格式
+    if isinstance(points, np.ndarray):
+        points = points.tolist()
+
+    # 首尾点
+    start = points[0]
+    end = points[-1]
+
+    # 找到距离最大的点
+    max_dist = 0
+    max_index = 0
+
+    for i in range(1, len(points) - 1):
+        if use_plane and len(points) > 2:
+            # 使用平面距离（需要至少3个点确定平面）
+            if i > 0 and i < len(points) - 1:
+                # 使用当前点、前一个点和后一个点确定平面
+                plane_p1 = points[i - 1]
+                plane_p2 = points[i]
+                plane_p3 = points[i + 1]
+                dist = point_to_plane_distance(points[i], plane_p1, plane_p2, plane_p3)
+            else:
+                dist = point_to_line_distance(points[i], start, end)
+        else:
+            # 使用线段距离
+            dist = point_to_line_distance(points[i], start, end)
+
+        if dist > max_dist:
+            max_dist = dist
+            max_index = i
+
+    # 递归简化
+    if max_dist > epsilon:
+        # 保留该点，递归处理左右两部分
+        left = douglas_peucker_3d(points[:max_index + 1], epsilon, use_plane)
+        right = douglas_peucker_3d(points[max_index:], epsilon, use_plane)
+
+        # 合并结果（避免重复端点）
+        return left[:-1] + right
+    else:
+        # 只保留首尾点
+        return [start, end]
+
+
+class DouglasPeucker3D:
+    """
+    3D Douglas-Peucker 算法的面向对象封装
+    支持点云简化、轨迹压缩、轮廓提取等
+    """
+
+    def __init__(self, epsilon=0.01, use_plane=False):
+        """
+        Args:
+            epsilon: 容差阈值
+            use_plane: 是否使用平面距离（更适合曲面点云）
+        """
+        self.epsilon = epsilon
+        self.use_plane = use_plane
+
+    def simplify(self, points):
+        """
+        简化点云
+
+        Args:
+            points: (N, 3) 的 numpy 数组或列表
+
+        Returns:
+            简化后的点云 (N', 3) numpy 数组
+        """
+        if isinstance(points, np.ndarray):
+            points = points.tolist()
+
+        simplified = douglas_peucker_3d(points, self.epsilon, self.use_plane)
+        return np.array(simplified)
+
+    def simplify_trajectory(self, trajectory, time_weight=False):
+        """
+        专门用于轨迹简化（考虑时间维度）
+
+        Args:
+            trajectory: (N, 4) 的数组 [x, y, z, t] 或 [x, y, z]
+            time_weight: 是否加入时间权重
+
+        Returns:
+            简化后的轨迹
+        """
+        if time_weight and trajectory.shape[1] == 4:
+            # 时间加权：将时间作为第四维参与距离计算
+            # 需要自定义距离函数
+            pass
+
+        return self.simplify(trajectory[:, :3])
+
+    def extract_contour(self, points, return_indices=False):
+        """
+        提取点云轮廓（结合法线估计）
+
+        Args:
+            points: (N, 3) 点云
+            return_indices: 是否返回原始点索引
+
+        Returns:
+            轮廓点集
+        """
+        # 先进行粗略简化
+        simplified = self.simplify(points)
+
+        # 这里可以结合法线估计进一步提取边界点
+        # 简化实现：返回简化后的点作为轮廓候选
+
+        if return_indices:
+            # 查找简化点在原始点云中的最近邻索引
+            from scipy.spatial import KDTree
+            tree = KDTree(points)
+            indices = []
+            for p in simplified:
+                _, idx = tree.query(p)
+                indices.append(idx)
+            return simplified, np.array(indices)
+
+        return simplified
+
+
+# ========== 示例和使用 ==========
+
+def generate_test_curve(n_points=100, noise=0.02):
+    """
+    生成测试用的三维曲线（螺旋线 + 噪声）
+    """
+    t = np.linspace(0, 4 * np.pi, n_points)
+    x = np.sin(t)
+    y = np.cos(t)
+    z = t / (2 * np.pi)
+
+    points = np.column_stack([x, y, z])
+
+    # 添加噪声
+    if noise > 0:
+        points += np.random.randn(n_points, 3) * noise
+
+    return points
+
+
+def visualize_simplification(original, simplified, title="3D Douglas-Peucker Simplification"):
+    """
+    可视化原始点云和简化后的点云
+    """
+    fig = plt.figure(figsize=(12, 6))
+
+    # 原始点云
+    ax1 = fig.add_subplot(121, projection='3d')
+    ax1.scatter(original[:, 0], original[:, 1], original[:, 2],
+                c='blue', s=10, alpha=0.6, label='Original')
+    ax1.plot(original[:, 0], original[:, 1], original[:, 2],
+             'b-', alpha=0.3)
+    ax1.set_title(f'Original: {len(original)} points')
+    ax1.legend()
+
+    # 简化后点云
+    ax2 = fig.add_subplot(122, projection='3d')
+    ax2.scatter(simplified[:, 0], simplified[:, 1], simplified[:, 2],
+                c='red', s=30, alpha=0.8, label='Simplified')
+    ax2.plot(simplified[:, 0], simplified[:, 1], simplified[:, 2],
+             'r-', linewidth=2)
+    ax2.set_title(f'Simplified: {len(simplified)} points (ε={epsilon})')
+    ax2.legend()
+
+    plt.suptitle(title)
+    plt.tight_layout()
+    plt.show()
+
+
+def compare_epsilon(points, epsilons):
+    """
+    比较不同 epsilon 值的简化效果
+    """
+    results = {}
+    for eps in epsilons:
+        dp = DouglasPeucker3D(epsilon=eps)
+        simplified = dp.simplify(points)
+        results[eps] = {
+            'points': simplified,
+            'count': len(simplified),
+            'compression_ratio': len(simplified) / len(points)
+        }
+        print(f"ε={eps:.4f}: {len(simplified)} 点 (压缩率: {results[eps]['compression_ratio']:.2%})")
+
+    return results
+
+
+def demo_point_cloud():
+    """
+    点云简化演示（使用螺旋线模拟）
+    """
+    print("=" * 50)
+    print("3D Douglas-Peucker 算法演示")
+    print("=" * 50)
+
+    # 生成测试数据
+    points = generate_test_curve(200, noise=0.01)
+    print(f"\n原始点云: {len(points)} 个点")
+
+    # 测试不同 epsilon
+    epsilons = [0.01, 0.03, 0.05, 0.1, 0.2]
+    print("\n不同容差阈值的简化效果:")
+    results = compare_epsilon(points, epsilons)
+
+    # 可视化
+    dp = DouglasPeucker3D(epsilon=0.05)
+    simplified = dp.simplify(points)
+    visualize_simplification(points, simplified,
+                             f"3D DP Simplification (ε=0.05, {len(simplified)}/{len(points)} points)")
+
+
+def demo_trajectory():
+    """
+    轨迹简化演示（模拟移动轨迹）
+    """
+    print("\n" + "=" * 50)
+    print("3D 轨迹简化演示")
+    print("=" * 50)
+
+    # 生成测试轨迹（随机路径）
+    np.random.seed(42)
+    n_points = 100
+    t = np.linspace(0, 10, n_points)
+    x = np.sin(t) + np.random.randn(n_points) * 0.02
+    y = np.cos(t) + np.random.randn(n_points) * 0.02
+    z = t / 5 + np.random.randn(n_points) * 0.01
+
+    trajectory = np.column_stack([x, y, z])
+    print(f"原始轨迹: {len(trajectory)} 个点")
+
+    # 简化轨迹
+    dp = DouglasPeucker3D(epsilon=0.08)
+    simplified = dp.simplify_trajectory(trajectory)
+    print(f"简化后轨迹: {len(simplified)} 个点 (压缩率: {len(simplified) / len(trajectory):.2%})")
+
+    # 可视化
+    visualize_simplification(trajectory, simplified, "3D Trajectory Simplification")
+
+
+def demo_point_cloud_from_file(pcd_file=None):
+    """
+    从文件加载点云并简化
+    """
+    try:
+        import open3d as o3d
+
+        if pcd_file is None:
+            # 使用示例：生成随机点云
+            print("\n生成随机点云进行演示...")
+            points = np.random.randn(500, 3) * 0.5
+        else:
+            pcd = o3d.io.read_point_cloud(pcd_file)
+            points = np.asarray(pcd.points)
+
+        print(f"原始点云: {len(points)} 个点")
+
+        # 简化
+        dp = DouglasPeucker3D(epsilon=0.05)
+
+        # 注意：传统 DP 算法需要有序点集，对于无序点云需要先排序或使用其他方法
+        # 这里演示对有序轨迹/轮廓的应用
+        print("\n注意: DP算法适用于有序点集（如轮廓线、轨迹），")
+        print("对于无序点云，建议先进行排序或使用其他简化方法（如体素滤波）")
+
+    except ImportError:
+        print("需要安装 open3d 来读取点云文件: pip install open3d")
+
+
+if __name__ == "__main__":
+    # 运行演示
+    demo_point_cloud()
+    demo_trajectory()
+
+    # 如果需要处理实际点云文件
+    # demo_point_cloud_from_file("your_pointcloud.pcd")

alg/point_porj/archive/relative_code/BF.py

@@ -0,0 +1,95 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+import numpy as np
+from scipy.spatial import KDTree
+def chamfer_distance(pcd1, pcd2):
+    """
+    计算两个点云之间的 CD 距离（Chamfer Distance）
+
+    Args:
+        pcd1: Open3D 点云对象1
+        pcd2: Open3D 点云对象2
+
+    Returns:
+        cd: Chamfer Distance 值
+        cd1: 点云1 到 点云2 的平均距离
+        cd2: 点云2 到 点云1 的平均距离
+    """
+    # 获取点坐标
+    points1 = np.asarray(pcd1.points)
+    points2 = np.asarray(pcd2.points)
+
+    # 构建 KD-Tree 加速最近邻搜索
+    tree1 = KDTree(points1)
+    tree2 = KDTree(points2)
+
+    # 计算 points1 中每个点到 points2 的最近距离
+    dist1, _ = tree2.query(points1, k=1)
+    cd1 = np.mean(dist1 ** 2)  # 平方距离的平均值
+
+    # 计算 points2 中每个点到 points1 的最近距离
+    dist2, _ = tree1.query(points2, k=1)
+    cd2 = np.mean(dist2 ** 2)  # 平方距离的平均值
+
+    # 总 CD 距离（双向平均）
+    cd = cd1 + cd2
+
+    return np.sqrt(cd), cd1, cd2
+
+def bilateral_filter(pcd, sigma_s=0.01, sigma_r=0.1):
+    """
+    对点云进行双边滤波
+    Args:
+        pcd: Open3D点云对象
+        sigma_s: 空间标准差
+        sigma_r: 特征（法线）标准差
+    """
+    points = np.asarray(pcd.points)
+    normals = np.asarray(pcd.normals)
+
+    # 构建KD树
+    tree = KDTree(points)
+
+    filtered_points = np.zeros_like(points)
+
+    for i, p in enumerate(points):
+        # 搜索邻域点
+        indices = tree.query_ball_point(p, r=2 * sigma_s)
+        if len(indices) <= 1:
+            filtered_points[i] = p
+            continue
+
+        neigh_points = points[indices]
+        neigh_normals = normals[indices]
+
+        # 计算空间权重
+        spatial_dist = np.linalg.norm(neigh_points - p, axis=1)
+        w_s = np.exp(-spatial_dist ** 2 / (2 * sigma_s ** 2))
+
+        # 计算特征（法线）权重
+        normal_diff = np.linalg.norm(neigh_normals - normals[i], axis=1)
+        w_r = np.exp(-normal_diff ** 2 / (2 * sigma_r ** 2))
+
+        # 双边权重
+        w = w_s * w_r
+        w = w / w.sum()  # 归一化
+
+        # 加权平均
+        filtered_points[i] = np.sum(neigh_points.T * w, axis=1)
+
+    # 更新点云
+    filtered_pcd = o3d.geometry.PointCloud()
+    filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
+    filtered_pcd.normals = o3d.utility.Vector3dVector(normals)
+
+    return filtered_pcd
+
+
+# 使用示例
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply")
+pcd.estimate_normals()
+pcd_filtered = bilateral_filter(pcd, sigma_s=10, sigma_r=10)
+
+cd=chamfer_distance(pcd,pcd_filtered)
+print(cd)

alg/point_porj/archive/relative_code/CE.py

@@ -0,0 +1,54 @@
+import open3d as o3d
+import numpy as np
+
+
+def compute_curvature_open3d(pcd, radius=0.1, max_nn=30):
+    """
+    使用 Open3D 计算曲率
+
+    Args:
+        pcd: Open3D 点云对象
+        radius: 搜索半径
+        max_nn: 最大邻居数
+
+    Returns:
+        curvature: 曲率数组
+    """
+    # 估计法线
+    pcd.estimate_normals(
+        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)
+    )
+
+    # 计算曲率（Open3D 在估计法线时已计算）
+    # 曲率值存储在 pcd.point.normals 的第三个分量？
+    # 实际上 Open3D 需要单独计算
+
+    # 方法：使用 PCA 重新计算
+    points = np.asarray(pcd.points)
+    n_points = len(points)
+    curvature = np.zeros(n_points)
+
+    tree = o3d.geometry.KDTreeFlann(pcd)
+
+    for i in range(n_points):
+        [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius)
+
+        if k < 3:
+            curvature[i] = 1.0
+            continue
+
+        neighbors = points[idx]
+        centroid = np.mean(neighbors, axis=0)
+        centered = neighbors - centroid
+        cov = np.cov(centered.T)
+        eigenvalues = np.linalg.eigvalsh(cov)
+        eigenvalues = np.sort(eigenvalues)
+
+        curvature[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8)
+
+    return curvature
+
+
+# 使用示例
+pcd = o3d.io.read_point_cloud("pointcloud.pcd")
+curvature = compute_curvature_open3d(pcd, radius=0.05)

alg/point_porj/archive/relative_code/DBSCAN.py

@@ -0,0 +1,50 @@
+import open3d as o3d
+import numpy as np
+
+
+def dbscan_open3d(pcd, eps=0.1, min_points=10):
+    """
+    使用 Open3D 进行 DBSCAN 聚类
+    Args:
+        pcd: Open3D 点云对象
+        eps: 邻域半径
+        min_points: 最小邻域点数
+    Returns:
+        labels: 每个点的簇标签（-1 表示噪声）
+        clusters: 各簇的点索引列表
+    """
+    # DBSCAN 聚类
+    labels = np.array(pcd.cluster_dbscan(eps=eps, min_points=min_points))
+
+    # 提取各簇索引
+    unique_labels = np.unique(labels)
+    clusters = []
+    for label in unique_labels:
+        if label == -1:
+            continue
+        clusters.append(np.where(labels == label)[0])
+
+    return labels, clusters
+
+
+# 使用示例
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply")
+
+# 预处理
+pcd = pcd.voxel_down_sample(voxel_size=0.05)
+pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
+
+# DBSCAN 聚类
+labels, clusters = dbscan_open3d(pcd, eps=0.1, min_points=50)
+
+print(f"找到 {len(clusters)} 个聚类")
+print(f"噪声点: {np.sum(labels == -1)}")
+
+# 为每个聚类着色
+colors = np.random.rand(len(clusters), 3)
+colored_points = np.zeros((len(pcd.points), 3))
+for i, cluster in enumerate(clusters):
+    colored_points[cluster] = colors[i]
+pcd.colors = o3d.utility.Vector3dVector(colored_points)
+
+o3d.visualization.draw_geometries([pcd])

alg/point_porj/archive/relative_code/EN.py

@@ -0,0 +1,65 @@
+import open3d as o3d
+import numpy as np
+
+
+def compute_curvature_open3d(pcd, radius=0.05, max_nn=30):
+    """
+    使用 Open3D 计算点云曲率
+
+    Args:
+        pcd: Open3D 点云对象
+        radius: 搜索半径
+        max_nn: 最大邻居数
+
+    Returns:
+        curvatures: (N,) 曲率数组
+    """
+    # 估计法线（同时获取协方差矩阵信息）
+    pcd.estimate_normals(
+        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=max_nn)
+    )
+
+    # 获取点云数据
+    points = np.asarray(pcd.points)
+    n_points = len(points)
+    curvatures = np.zeros(n_points)
+
+    # 构建 KD-Tree
+    tree = o3d.geometry.KDTreeFlann(pcd)
+
+    for i in range(n_points):
+        # 搜索邻域点
+        [k, idx, _] = tree.search_radius_vector_3d(pcd.points[i], radius)
+
+        if k < 3:
+            curvatures[i] = 1.0
+            continue
+
+        # 获取邻域点坐标
+        neighbors = points[idx]
+
+        # 计算协方差矩阵
+        centroid = np.mean(neighbors, axis=0)
+        centered = neighbors - centroid
+        cov = np.cov(centered.T)
+
+        # 特征值分解
+        eigenvalues = np.linalg.eigvalsh(cov)
+        eigenvalues = np.sort(eigenvalues)
+
+        # 计算曲率
+        curvatures[i] = eigenvalues[0] / (eigenvalues.sum() + 1e-8)
+
+    return curvatures
+
+
+# 使用示例
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\PointCloud\\38.ply")
+curvatures = compute_curvature_open3d(pcd, radius=50, max_nn=30)
+
+# 将曲率作为颜色可视化
+colors = np.zeros((len(pcd.points), 3))
+colors[:, 0] = curvatures*3  # 红色通道表示曲率
+colors[:, 1] = 1 - curvatures*3  # 绿色通道为互补色
+pcd.colors = o3d.utility.Vector3dVector(colors)
+o3d.visualization.draw_geometries([pcd])

alg/point_porj/archive/relative_code/Eucilid.py

@@ -0,0 +1,47 @@
+import open3d as o3d
+import numpy as np
+
+
+def euclidean_clustering_open3d(pcd, radius=0.1, min_cluster_size=50, max_cluster_size=10000):
+    """
+    使用 Open3D 进行欧几里得聚类
+    Args:
+        pcd: Open3D 点云对象
+        radius: 聚类半径
+        min_cluster_size: 最小簇大小
+        max_cluster_size: 最大簇大小
+    Returns:
+        labels: 每个点的簇标签（-1 表示噪声/无效簇）
+        clusters: 各簇的点索引列表
+    """
+    # 欧几里得聚类
+    labels = np.array(pcd.cluster_dbscan(eps=radius, min_points=min_cluster_size))
+
+    # 统计簇信息
+    unique_labels = np.unique(labels)
+    clusters = []
+
+    for label in unique_labels:
+        if label == -1:  # -1 表示噪声点
+            continue
+        cluster_indices = np.where(labels == label)[0]
+        if min_cluster_size <= len(cluster_indices) <= max_cluster_size:
+            clusters.append(cluster_indices)
+
+    return labels, clusters
+
+
+# 使用示例
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_left_side.ply")
+labels, clusters = euclidean_clustering_open3d(pcd, radius=4, min_cluster_size=100)
+
+print(f"找到 {len(clusters)} 个聚类")
+
+# 为每个聚类着色
+colors = np.random.rand(len(clusters), 3)
+colored_points = np.zeros((len(pcd.points), 3))
+for i, cluster in enumerate(clusters):
+    colored_points[cluster] = colors[i]
+
+pcd.colors = o3d.utility.Vector3dVector(colored_points)
+o3d.visualization.draw_geometries([pcd])

alg/point_porj/archive/relative_code/FPFH.py

@@ -0,0 +1,16 @@
+import open3d as o3d
+import numpy as np
+
+pcd = o3d.io.read_point_cloud("pointcloud.pcd")
+
+# 1. 计算法线
+pcd.estimate_normals(search_param=o3d.geometry.KDTreeSearchParamKNN(k=30))
+
+# 2. 提取FPFH特征
+fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+    pcd,
+    o3d.geometry.KDTreeSearchParamKNN(k=50)
+)
+
+# fpfh.data 是 (33, N) 的矩阵，每列是一个点的33维FPFH描述子
+print(fpfh.data.shape)  # (33, N)

alg/point_porj/archive/relative_code/NE.py

@@ -0,0 +1,17 @@
+import open3d as o3d
+import numpy as np
+pcd = o3d.io.read_point_cloud("pointcloud.pcd")
+
+# 估计法线
+pcd.estimate_normals(
+    search_param=o3d.geometry.KDTreeSearchParamHybrid(
+        radius=0.1,   # 搜索半径
+        max_nn=30     # 最大邻居数
+    )
+)
+
+# 可视化法线
+o3d.visualization.draw_geometries([pcd], point_show_normal=True)
+
+# 获取法线数据
+normals = np.asarray(pcd.normals)

alg/point_porj/archive/relative_code/P2P.py

@@ -0,0 +1,315 @@
+
+import copy
+from scipy.spatial.transform import Rotation as R
+import trimesh
+import numpy as np
+import open3d as o3d
+import time
+def rotation_matrix_scipy(roll_deg=10, pitch_deg=10, yaw_deg=10):
+    """
+    使用 scipy 生成绕 XYZ 轴旋转的矩阵
+    """
+    r = R.from_euler('xyz', [roll_deg, pitch_deg, yaw_deg], degrees=True)
+    return r.as_matrix()
+
+
+
+
+def random_rotation_matrix():
+    """生成随机旋转矩阵"""
+    angle = np.random.uniform(0, 2 * np.pi)
+    axis = np.random.randn(3)
+    axis = axis / np.linalg.norm(axis)
+
+    K = np.array([
+        [0, -axis[2], axis[1]],
+        [axis[2], 0, -axis[0]],
+        [-axis[1], axis[0], 0]
+    ])
+    R = np.eye(3) + np.sin(angle) * K + (1 - np.cos(angle)) * K @ K
+    return R
+
+
+def random_translation_vector(range_xyz=0.5):
+    """生成随机平移向量"""
+    return np.random.uniform(-range_xyz, range_xyz, 3)
+
+
+def transform_point_cloud(pcd, R, t):
+    """对点云应用变换"""
+    transformed_pcd = copy.deepcopy(pcd)
+    points = np.asarray(transformed_pcd.points)
+    points_transformed = (R @ points.T).T + t
+    transformed_pcd.points = o3d.utility.Vector3dVector(points_transformed)
+    return transformed_pcd
+
+
+def registration_open3d(source_pcd, target_pcd, voxel_size=0.005):
+    """
+    点云配准（RANSAC + ICP）
+
+    Returns:
+        transformation: 4x4 变换矩阵
+        fitness: 配准 fitness 分数
+        rmse: 配准 RMSE
+    """
+    # 1. 下采样
+    source_down = source_pcd.voxel_down_sample(voxel_size)
+    target_down = target_pcd.voxel_down_sample(voxel_size)
+
+    # 2. 估计法线
+    radius = voxel_size * 2
+    source_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30))
+    target_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=radius, max_nn=30))
+
+    # 3. 计算 FPFH 特征
+    radius_feature = voxel_size * 5
+    source_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        source_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
+    target_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
+        target_down, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
+
+    # 4. RANSAC 粗配准
+    distance_threshold = voxel_size * 1.5
+    result_ransac = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
+        source_down, target_down, source_fpfh, target_fpfh,
+        mutual_filter=True,
+        max_correspondence_distance=distance_threshold,
+        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPoint(),
+        ransac_n=3,
+        checkers=[
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
+            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)
+        ],
+        criteria=o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))
+
+    print(f"RANSAC 配准完成")
+    source_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
+    target_pcd.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30))
+    # 5. ICP 精配准
+    result_icp = o3d.pipelines.registration.registration_icp(
+        source_pcd, target_pcd, distance_threshold / 2, result_ransac.transformation,
+        o3d.pipelines.registration.TransformationEstimationPointToPlane(),
+        o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=200))
+
+    print(f"ICP 精配准完成")
+    print(f"  Fitness: {result_icp.fitness:.4f}")
+    print(f"  RMSE: {result_icp.inlier_rmse:.6f} m")
+
+    return result_icp.transformation, result_icp.fitness, result_icp.inlier_rmse
+
+
+def visualize_registration_result(original_pcd, transformed_pcd, registered_pcd, title="Registration Result"):
+    """
+    可视化原始点云、变换后点云和配准后点云
+
+    Args:
+        original_pcd: 原始点云（源）
+        transformed_pcd: 应用随机变换后的点云（目标）
+        registered_pcd: 配准后的点云
+        title: 窗口标题
+    """
+    # 复制并着色
+    source_colored = copy.deepcopy(original_pcd)
+    target_colored = copy.deepcopy(transformed_pcd)
+    registered_colored = copy.deepcopy(registered_pcd)
+
+    # 设置颜色
+    source_colored.paint_uniform_color([0, 0, 1])  # 蓝色 - 原始点云
+    target_colored.paint_uniform_color([0, 1, 0])  # 绿色 - 目标点云
+    registered_colored.paint_uniform_color([1, 0, 0])  # 红色 - 配准后点云
+
+    # 创建坐标系（可选）
+    # coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
+
+    # 可视化
+    o3d.visualization.draw_geometries(
+        [source_colored, target_colored],
+        window_name="Before Registration (Blue: Source, Green: Target)",
+        width=1024,
+        height=768
+    )
+
+    o3d.visualization.draw_geometries(
+        [registered_colored, target_colored],
+        window_name="After Registration (Red: Registered, Green: Target)",
+        width=1024,
+        height=768
+    )
+
+
+def visualize_registration_comparison(original_pcd, transformed_pcd, registered_pcd):
+    """
+    并排可视化配准前后的对比
+
+    Args:
+        original_pcd: 原始点云
+        transformed_pcd: 变换后的点云（目标）
+        registered_pcd: 配准后的点云
+    """
+    # 复制并着色
+    source_before = copy.deepcopy(original_pcd)
+    source_before.paint_uniform_color([0, 0, 1])  # 蓝色
+
+    target_before = copy.deepcopy(transformed_pcd)
+    target_before.paint_uniform_color([0, 1, 0])  # 绿色
+
+    registered_after = copy.deepcopy(registered_pcd)
+    registered_after.paint_uniform_color([1, 0, 0])  # 红色
+
+    target_after = copy.deepcopy(transformed_pcd)
+    target_after.paint_uniform_color([0, 1, 0])  # 绿色
+
+    # 创建坐标系
+    coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
+
+    # 方法1：分别显示两个窗口
+    o3d.visualization.draw_geometries(
+        [source_before],
+        window_name="Before Registration (Blue: Source, Green: Target)",
+        width=1024,
+        height=768
+    )
+    o3d.visualization.draw_geometries(
+         [target_before],
+        window_name="Before Registration (Blue: Source, Green: Target)",
+        width=1024,
+        height=768
+    )
+    o3d.visualization.draw_geometries(
+        [registered_after, source_before],
+        window_name="After Registration (Red: Registered, Green: Target)",
+        width=1024,
+        height=768
+    )
+
+
+
+
+def compute_point_to_mesh_distance(point_cloud, mesh):
+    """
+    计算点云到 Mesh 的距离
+
+    Args:
+        point_cloud: Open3D 点云对象
+        mesh: Open3D 网格对象
+
+    Returns:
+        distances: 每个点到最近三角形面的距离
+        mean_distance: 平均距离
+        rmse: 均方根误差
+        max_distance: 最大距离
+    """
+    # 计算点到 mesh 的距离
+    distances = mesh.compute_point_cloud_distance(point_cloud)
+    distances = np.asarray(distances)
+
+    mean_dist = np.mean(distances)
+    rmse = np.sqrt(np.mean(distances ** 2))
+    max_dist = np.max(distances)
+    std_dist = np.std(distances)
+
+    return distances, mean_dist, rmse, max_dist, std_dist
+
+
+# 使用示例
+def demo(pcd,mesh):
+    # 加载点云和 mesh
+    # 计算距离
+    distances, mean_dist, rmse, max_dist, std_dist = compute_point_to_mesh_distance(pcd, mesh)
+
+    print(f"平均距离: {mean_dist:.6f} m ({mean_dist * 1000:.3f} mm)")
+    print(f"RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)")
+    print(f"最大距离: {max_dist:.6f} m ({max_dist * 1000:.3f} mm)")
+    print(f"标准差: {std_dist:.6f} m ({std_dist * 1000:.3f} mm)")
+
+    # 可视化（按距离着色）
+    pcd.colors = o3d.utility.Vector3dVector(get_color_by_distance(distances))
+    o3d.visualization.draw_geometries([pcd, mesh])
+
+
+def get_color_by_distance(distances, max_dist=None):
+    """
+    根据距离生成颜色（红：远，蓝：近）
+    """
+    if max_dist is None:
+        max_dist = np.percentile(distances, 95)  # 使用95分位数
+
+    colors = np.zeros((len(distances), 3))
+    norm_dist = np.clip(distances / (max_dist + 1e-8), 0, 1)
+
+    # 红色通道（距离越远越红）
+    colors[:, 0] = norm_dist
+    # 蓝色通道（距离越近越蓝）
+    colors[:, 2] = 1 - norm_dist
+
+    return colors
+# ========== 主程序 ==========
+if __name__ == "__main__":
+    # 1. 读取原始点云
+    file_path = "E:\point_distance\data\\40_noisy.ply"
+
+    source_pcd = o3d.io.read_point_cloud(file_path)
+
+    # points=np.asarray(source_pcd.points)
+
+    t0=time.time()
+    if len(source_pcd.points) == 0:
+        print("读取失败，请检查文件路径")
+        exit()
+
+    print(f"原始点云点数: {len(source_pcd.points)}")
+
+    # 2. 生成随机变换
+    # R = random_rotation_matrix()
+    R = rotation_matrix_scipy(10, 10, 10)
+    t = random_translation_vector(range_xyz=0)
+
+    print(f"随机旋转矩阵:\n{R}")
+    print(f"随机平移向量: {t}")
+
+    # 3. 创建目标点云（变换后的源点云）
+    target_pcd = transform_point_cloud(source_pcd, R, t)
+
+    # 4. 点云配准
+    print("\n开始点云配准...")
+    transformation, fitness, rmse = registration_open3d(source_pcd, target_pcd, voxel_size=10)
+
+    # 5. 应用配准变换
+    # registered_pcd = copy.deepcopy(source_pcd)
+    # registered_pcd.transform(transformation)
+    inv_transformation = np.linalg.inv(transformation)
+    # 将目标点云变换回源点云坐标系
+    target_recovered = copy.deepcopy(target_pcd)
+    target_recovered.transform(inv_transformation)
+
+
+    t1 = time.time()
+    print(f"\n配准结果:")
+    print(f"  Fitness: {fitness:.4f}")
+    print(f"  RMSE: {rmse:.6f} m")
+    print(f"  估计的变换矩阵:\n{transformation}")
+    print(f"配准time:\n{t1-t0}")
+
+
+    t2=time.time()
+    # 1. 读取数据
+
+    points = np.asarray(target_recovered.points)
+    mesh_trimesh = trimesh.load("E:\point_distance\data\\40.stl")
+    # ========== 1. 读取数据 ==========
+    # 2. 使用 trimesh 计算每个点到 Mesh 的最近距离
+    # trimesh 的 nearest 方法非常高效且精确
+    closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points)
+    t3 = time.time()
+
+    # distances 即为所求的距离数组
+    print(f"平均距离: {np.mean(distances)}")
+    print(f"最大距离: {np.max(distances)}")
+    print(f"  距离计算:   {t3 - t2:.4f} 秒")
+    # 如果需要将结果存回 Open3D 点云进行可视化
+
+
+    # 6. 可视化对比
+    print("\n开始可视化...")
+    visualize_registration_comparison(source_pcd, target_pcd,target_recovered)

alg/point_porj/archive/relative_code/PF.py

@@ -0,0 +1,94 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+
+
+def compute_keep_ratio_and_density(pcd, filtered_pcd, sample_size=1000):
+    """
+    计算直通滤波的保留率和密度变化
+
+    Args:
+        pcd: 原始点云
+        filtered_pcd: 滤波后点云
+        sample_size: 密度计算采样点数（加速计算）
+
+    Returns:
+        keep_ratio: 保留率 (%)
+        density_change: 密度变化 (%)
+    """
+    n_orig = len(pcd.points)
+    n_filt = len(filtered_pcd.points)
+
+    # 1. 保留率
+    keep_ratio = n_filt / n_orig * 100
+
+    # 2. 密度变化（基于平均最近邻距离）
+    points_orig = np.asarray(pcd.points)
+    points_filt = np.asarray(filtered_pcd.points)
+
+    # 采样（点数太多时加速）
+    if len(points_orig) > sample_size:
+        idx_orig = np.random.choice(len(points_orig), sample_size, replace=False)
+        points_orig_sample = points_orig[idx_orig]
+    else:
+        points_orig_sample = points_orig
+
+    if len(points_filt) > sample_size:
+        idx_filt = np.random.choice(len(points_filt), sample_size, replace=False)
+        points_filt_sample = points_filt[idx_filt]
+    else:
+        points_filt_sample = points_filt
+
+    # 计算平均最近邻距离（密度指标）
+    if len(points_orig_sample) > 5:
+        tree_orig = KDTree(points_orig_sample)
+        dist_orig, _ = tree_orig.query(points_orig_sample, k=2)  # k=2 排除自身
+        avg_dist_orig = np.mean(dist_orig[:, 1])
+    else:
+        avg_dist_orig = 0
+
+    if len(points_filt_sample) > 5:
+        tree_filt = KDTree(points_filt_sample)
+        dist_filt, _ = tree_filt.query(points_filt_sample, k=2)
+        avg_dist_filt = np.mean(dist_filt[:, 1])
+    else:
+        avg_dist_filt = 0
+
+    # 密度变化（距离增大 = 密度减小）
+    if avg_dist_orig > 0:
+        density_change = (avg_dist_filt - avg_dist_orig) / avg_dist_orig * 100
+    else:
+        density_change = 0
+
+    return keep_ratio, density_change
+
+
+# ========== 使用示例 ==========
+if __name__ == "__main__":
+    # 读取点云
+    pcd = o3d.io.read_point_cloud(r"E:\point_distance\data\part_car_head.ply")
+
+    # 直通滤波（Z轴）
+    points = np.asarray(pcd.points)
+    mask = (points[:, 2] >= 0.0) & (points[:, 2] <= 2.0)
+    filtered_points = points[mask]
+
+    filtered_pcd = o3d.geometry.PointCloud()
+    filtered_pcd.points = o3d.utility.Vector3dVector(filtered_points)
+
+    # 计算指标
+    keep_ratio, density_change = compute_keep_ratio_and_density(pcd, filtered_pcd)
+
+    print("=" * 40)
+    print("直通滤波指标")
+    print("=" * 40)
+    print(f"保留率: {keep_ratio:.2f}%")
+    print(f"密度变化: {density_change:+.2f}%")
+
+    # 解释
+    if density_change > 0:
+        print(f"  → 点云变稀疏了 {density_change:.2f}%")
+    elif density_change < 0:
+        print(f"  → 点云变密集了 {abs(density_change):.2f}%")
+    else:
+        print(f"  → 密度基本不变")

alg/point_porj/archive/relative_code/RANSC.py

@@ -0,0 +1,380 @@
+# import numpy as np
+# import open3d as o3d
+# import random
+#
+#
+# def ransac_plane(points, threshold=0.01, max_iterations=1000, min_inliers=100):
+#     """
+#     RANSAC 平面拟合（单个）
+#     """
+#     best_inliers = []
+#     best_plane = None
+#     n_points = len(points)
+#
+#     if n_points < 3:
+#         return None, []
+#
+#     for _ in range(max_iterations):
+#         sample_idx = random.sample(range(n_points), 3)
+#         p1, p2, p3 = points[sample_idx]
+#
+#         v1 = p2 - p1
+#         v2 = p3 - p1
+#         normal = np.cross(v1, v2)
+#         norm = np.linalg.norm(normal)
+#         if norm < 1e-8:
+#             continue
+#         normal = normal / norm
+#         a, b, c = normal
+#         d = -np.dot(normal, p1)
+#
+#         distances = np.abs(np.dot(points, normal) + d)
+#         inliers = np.where(distances < threshold)[0]
+#
+#         if len(inliers) > len(best_inliers) and len(inliers) >= min_inliers:
+#             best_inliers = inliers
+#             best_plane = (a, b, c, d)
+#
+#             if len(best_inliers) > 0.9 * n_points:
+#                 break
+#
+#     # 用内点重新拟合
+#     if best_inliers is not None and len(best_inliers) > 3:
+#         inlier_points = points[best_inliers]
+#         centroid = np.mean(inlier_points, axis=0)
+#         _, _, vh = np.linalg.svd(inlier_points - centroid)
+#         normal = vh[2]
+#         normal = normal / np.linalg.norm(normal)
+#         a, b, c = normal
+#         d = -np.dot(normal, centroid)
+#         best_plane = (a, b, c, d)
+#
+#     return best_plane, best_inliers
+#
+#
+# def sequential_ransac_planes(points, threshold=0.01, max_iterations=1000, min_inliers=100, max_planes=5):
+#     """
+#     迭代 RANSAC 拟合多个平面
+#     """
+#     remaining_points = points.copy()
+#     remaining_indices = np.arange(len(points))
+#
+#     planes = []
+#     inliers_list = []
+#
+#     for i in range(max_planes):
+#         if len(remaining_points) < min_inliers:
+#             break
+#
+#         plane, inliers = ransac_plane(remaining_points, threshold, max_iterations, min_inliers)
+#
+#         if plane is None or len(inliers) < min_inliers:
+#             break
+#
+#         planes.append(plane)
+#         original_indices = remaining_indices[inliers]
+#         inliers_list.append(original_indices)
+#
+#         # 移除内点
+#         mask = np.ones(len(remaining_points), dtype=bool)
+#         mask[inliers] = False
+#         remaining_points = remaining_points[mask]
+#         remaining_indices = remaining_indices[mask]
+#
+#         print(f"平面 {len(planes)}: {len(inliers)} 个内点, 剩余 {len(remaining_points)} 个点")
+#
+#     return planes, inliers_list
+#
+#
+# def create_plane_mesh(plane, bounds):
+#     """
+#     创建平面网格用于可视化
+#     """
+#     a, b, c, d = plane
+#     x_min, x_max = bounds[0]
+#     y_min, y_max = bounds[1]
+#
+#     x = np.linspace(x_min, x_max, 50)
+#     y = np.linspace(y_min, y_max, 50)
+#     X, Y = np.meshgrid(x, y)
+#
+#     if abs(c) > 1e-8:
+#         Z = -(a * X + b * Y + d) / c
+#     else:
+#         Z = np.zeros_like(X)
+#
+#     vertices = []
+#     triangles = []
+#     rows, cols = X.shape
+#
+#     for i in range(rows):
+#         for j in range(cols):
+#             vertices.append([X[i, j], Y[i, j], Z[i, j]])
+#
+#     for i in range(rows - 1):
+#         for j in range(cols - 1):
+#             idx = i * cols + j
+#             triangles.append([idx, idx + 1, idx + cols])
+#             triangles.append([idx + 1, idx + cols + 1, idx + cols])
+#
+#     mesh = o3d.geometry.TriangleMesh()
+#     mesh.vertices = o3d.utility.Vector3dVector(np.array(vertices))
+#     mesh.triangles = o3d.utility.Vector3iVector(np.array(triangles))
+#     mesh.compute_vertex_normals()
+#
+#     return mesh
+#
+#
+# def visualize_multiple_planes(points, planes, inliers_list):
+#     """
+#     可视化多个平面（点云 + 平面网格）
+#
+#     Args:
+#         points: (N, 3) 点云坐标
+#         planes: 平面方程列表 [(a,b,c,d), ...]
+#         inliers_list: 每个平面对应的内点索引列表
+#     """
+#     # 颜色列表（每个平面不同颜色）
+#     colors = [
+#         [1, 0, 0],  # 红色
+#         [0, 1, 0],  # 绿色
+#         [0, 0, 1],  # 蓝色
+#         [1, 1, 0],  # 黄色
+#         [1, 0, 1],  # 品红
+#         [0, 1, 1],  # 青色
+#         [1, 0.5, 0],  # 橙色
+#         [0.5, 0, 1],  # 紫色
+#     ]
+#
+#     geometries = []
+#
+#     # 1. 为每个平面的内点着色
+#     point_colors = np.zeros((len(points), 3))
+#     for i, inliers in enumerate(inliers_list):
+#         color = colors[i % len(colors)]
+#         point_colors[inliers] = color
+#
+#     # 未分配的点（剩余点）设为灰色
+#     unassigned = np.where(np.sum(point_colors, axis=1) == 0)[0]
+#     point_colors[unassigned] = [0.5, 0.5, 0.5]
+#
+#     colored_pcd = o3d.geometry.PointCloud()
+#     colored_pcd.points = o3d.utility.Vector3dVector(points)
+#     colored_pcd.colors = o3d.utility.Vector3dVector(point_colors)
+#     geometries.append(colored_pcd)
+#
+#     # 2. 为每个平面创建网格
+#     for i, (plane, inliers) in enumerate(zip(planes, inliers_list)):
+#         # 计算该平面对应点的范围
+#         inlier_points = points[inliers]
+#         if len(inlier_points) > 0:
+#             x_min, x_max = inlier_points[:, 0].min(), inlier_points[:, 0].max()
+#             y_min, y_max = inlier_points[:, 1].min(), inlier_points[:, 1].max()
+#
+#             # 稍微扩展边界
+#             x_range = x_max - x_min
+#             y_range = y_max - y_min
+#             x_min -= x_range * 0.1
+#             x_max += x_range * 0.1
+#             y_min -= y_range * 0.1
+#             y_max += y_range * 0.1
+#
+#             mesh = create_plane_mesh(plane, [[x_min, x_max], [y_min, y_max]])
+#             mesh.paint_uniform_color(colors[i % len(colors)])
+#             # 设置半透明
+#             mesh.paint_uniform_color([colors[i % len(colors)][0],
+#                                       colors[i % len(colors)][1],
+#                                       colors[i % len(colors)][2]])
+#             geometries.append(mesh)
+#
+#     # 可视化
+#     o3d.visualization.draw_geometries(
+#         geometries,
+#         window_name="Multiple Planes - RANSAC",
+#         width=1024,
+#         height=768
+#     )
+#
+#
+# def visualize_single_plane(points, plane, inliers):
+#     """
+#     可视化单个平面（兼容单平面情况）
+#     """
+#     visualize_multiple_planes(points, [plane], [inliers])
+#
+#
+# # ========== 主程序 ==========
+# if __name__ == "__main__":
+#     # 读取点云
+#     file_path = r"E:\point_distance\data\part_car_head.ply"
+#     pcd = o3d.io.read_point_cloud(file_path)
+#
+#     if len(pcd.points) == 0:
+#         print("读取失败，请检查文件路径")
+#         exit()
+#
+#     points = np.asarray(pcd.points)
+#     print(f"原始点云点数: {len(points)}")
+#
+#     # 计算点云范围
+#     points_range = points.max(axis=0) - points.min(axis=0)
+#     print(f"点云范围: X[{points_range[0]:.3f}], Y[{points_range[1]:.3f}], Z[{points_range[2]:.3f}]")
+#
+#     # 设置阈值
+#     threshold = points_range[2] * 0.02
+#     print(f"使用阈值: {threshold:.4f}")
+#
+#     # ========== 单平面拟合 ==========
+#     # print("\n" + "=" * 50)
+#     # print("单平面拟合")
+#     # print("=" * 50)
+#     #
+#     # plane, inliers = ransac_plane(points, threshold=threshold, max_iterations=1, min_inliers=100)
+#     #
+#     # if plane is not None:
+#     #     a, b, c, d = plane
+#     #     print(f"拟合平面: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0")
+#     #     print(f"内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)")
+#     #     visualize_single_plane(points, plane, inliers)
+#
+#     # ========== 多平面拟合 ==========
+#     print("\n" + "=" * 50)
+#     print("多平面拟合")
+#     print("=" * 50)
+#
+#     planes, inliers_list = sequential_ransac_planes(
+#         points,
+#         threshold=threshold,
+#         max_iterations=1,
+#         min_inliers=100,
+#         max_planes=1
+#     )
+#
+#     print(f"共提取 {len(planes)} 个平面")
+#     for i, (plane, inliers) in enumerate(zip(planes, inliers_list)):
+#         a, b, c, d = plane
+#         print(f"平面 {i + 1}: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0, 内点数: {len(inliers)}")
+#
+#     if len(planes) > 0:
+#         visualize_multiple_planes(points, planes, inliers_list)
+
+import numpy as np
+import open3d as o3d
+import random
+
+
+def create_square_plane_with_noise(size_mm=1000, noise_std_mm=0.5, num_points=10000):
+    """创建正方形平面点云并添加高斯噪声"""
+    size_m = size_mm / 1000.0
+    noise_std_m = noise_std_mm / 1000.0
+
+    half_size = size_m / 2
+    x = np.random.uniform(-half_size, half_size, num_points)
+    y = np.random.uniform(-half_size, half_size, num_points)
+    z = np.zeros(num_points)
+
+    points = np.column_stack([x, y, z])
+    noise_z = np.random.normal(0, noise_std_m, num_points)
+    points[:, 2] += noise_z
+
+    return points
+
+
+def estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000):
+    """使用 Open3D 内置 RANSAC 平面估计"""
+    pcd = o3d.geometry.PointCloud()
+    pcd.points = o3d.utility.Vector3dVector(points)
+
+    threshold_m = threshold_mm / 1000.0
+
+    plane_model, inliers = pcd.segment_plane(
+        distance_threshold=threshold_m,
+        ransac_n=3,
+        num_iterations=max_iterations
+    )
+
+    a, b, c, d = plane_model
+
+    # 计算误差
+    ideal_normal = np.array([0, 0, 1])
+    estimated_normal = np.array([a, b, c])
+    cos_angle = np.dot(ideal_normal, estimated_normal)
+    cos_angle = np.clip(cos_angle, -1, 1)
+    angle_error = np.degrees(np.arccos(cos_angle))
+    offset_error = abs(d)
+
+    inlier_points = points[inliers]
+    distances = np.abs(np.dot(inlier_points, estimated_normal) + d)
+    rmse = np.sqrt(np.mean(distances ** 2))
+
+    print(f"\nRANSAC 平面拟合结果 (Open3D):")
+    print(f"  估计平面: {a:.6f}x + {b:.6f}y + {c:.6f}z + {d:.6f} = 0")
+    print(f"  内点数量: {len(inliers)} / {len(points)} ({len(inliers) / len(points) * 100:.1f}%)")
+    print(f"  法线角度误差: {angle_error:.4f} 度")
+    print(f"  偏移误差: {offset_error:.6f} m ({offset_error * 1000:.3f} mm)")
+    print(f"  平面拟合 RMSE: {rmse:.6f} m ({rmse * 1000:.3f} mm)")
+
+    return plane_model, inliers
+
+
+def visualize_plane_fitting(points, plane):
+    """可视化原始点云和拟合平面"""
+    # 原始点云
+    pcd = o3d.geometry.PointCloud()
+    pcd.points = o3d.utility.Vector3dVector(points)
+    pcd.paint_uniform_color([0.6, 0.8, 1.0])
+
+    # 创建平面点云
+    a, b, c, d = plane
+    x_min, x_max = points[:, 0].min(), points[:, 0].max()
+    y_min, y_max = points[:, 1].min(), points[:, 1].max()
+
+    x = np.linspace(x_min, x_max, 50)
+    y = np.linspace(y_min, y_max, 50)
+    X, Y = np.meshgrid(x, y)
+
+    if abs(c) > 1e-8:
+        Z = -(a * X + b * Y + d) / c
+    else:
+        Z = np.zeros_like(X)
+
+    plane_points = np.column_stack([X.ravel(), Y.ravel(), Z.ravel()])
+
+    pcd_plane = o3d.geometry.PointCloud()
+    pcd_plane.points = o3d.utility.Vector3dVector(plane_points)
+    pcd_plane.paint_uniform_color([1, 0, 0])  # 红色
+
+    o3d.visualization.draw_geometries(
+        [pcd, pcd_plane],
+        window_name="Plane Fitting",
+        width=1024,
+        height=768
+    )
+
+
+# ========== 主程序 ==========
+if __name__ == "__main__":
+    # 参数设置
+    size_mm = 1000
+    noise_std_mm = 0.5
+    num_points = 100000
+
+    print(f"参数:")
+    print(f"  正方形边长: {size_mm} mm")
+    print(f"  噪声标准差: {noise_std_mm} mm")
+    print(f"  点数: {num_points}")
+
+    # 生成点云
+    points = create_square_plane_with_noise(size_mm, noise_std_mm, num_points)
+
+    # 检查数据范围
+    print(f"\n数据范围:")
+    print(f"  X: [{points[:, 0].min():.3f}, {points[:, 0].max():.3f}]")
+    print(f"  Y: [{points[:, 1].min():.3f}, {points[:, 1].max():.3f}]")
+    print(f"  Z: [{points[:, 2].min():.6f}, {points[:, 2].max():.6f}]")
+
+    # 使用 Open3D RANSAC
+    plane, inliers = estimate_plane_ransac_open3d(points, threshold_mm=0.5, max_iterations=2000)
+
+    # 可视化
+    visualize_plane_fitting(points, plane)

alg/point_porj/archive/relative_code/RG.py

@@ -0,0 +1,147 @@
+import numpy as np
+from scipy.spatial import KDTree
+from sklearn.neighbors import NearestNeighbors
+
+
+def estimate_normals(points, k=20):
+    """用 PCA 估计法线"""
+    n_points = len(points)
+    normals = np.zeros_like(points)
+
+    nbrs = NearestNeighbors(n_neighbors=k).fit(points)
+    distances, indices = nbrs.kneighbors(points)
+
+    for i in range(n_points):
+        neighbors = points[indices[i]]
+        centroid = np.mean(neighbors, axis=0)
+        cov = np.cov((neighbors - centroid).T)
+        eig_vals, eig_vecs = np.linalg.eig(cov)
+        normal = eig_vecs[:, np.argmin(eig_vals)]
+        # 统一法线方向
+        if normal[2] < 0:
+            normal = -normal
+        normals[i] = normal
+
+    return normals
+
+
+def compute_curvature(points, normals, k=20):
+    """计算曲率（特征值比值）"""
+    curvature = np.zeros(len(points))
+    nbrs = NearestNeighbors(n_neighbors=k).fit(points)
+    distances, indices = nbrs.kneighbors(points)
+
+    for i in range(len(points)):
+        neighbors = points[indices[i]]
+        centroid = np.mean(neighbors, axis=0)
+        cov = np.cov((neighbors - centroid).T)
+        eig_vals, _ = np.linalg.eig(cov)
+        curvature[i] = np.min(eig_vals) / (np.sum(eig_vals) + 1e-8)
+
+    return curvature
+
+
+def region_growing(points, normals, curvature,
+                   angle_threshold=30.0,  # 法线夹角阈值（度）
+                   curvature_threshold=0.05,  # 曲率阈值
+                   min_cluster_size=100):
+    """
+    区域生长算法
+    Args:
+        points: (N, 3) 点云坐标
+        normals: (N, 3) 法线
+        curvature: (N,) 曲率
+        angle_threshold: 法线夹角阈值（度）
+        curvature_threshold: 曲率阈值
+        min_cluster_size: 最小簇大小
+    Returns:
+        labels: (N,) 每个点的簇标签（-1 表示未分类/噪声）
+    """
+    n_points = len(points)
+    labels = np.full(n_points, -1, dtype=int)
+
+    # 按曲率升序排序（曲率小的作为种子）
+    sorted_indices = np.argsort(curvature)
+
+    # 构建 KD-Tree 用于快速邻域搜索
+    tree = KDTree(points)
+
+    label_id = 0
+    angle_threshold_rad = np.radians(angle_threshold)
+    cos_threshold = np.cos(angle_threshold_rad)
+
+    for seed_idx in sorted_indices:
+        if labels[seed_idx] != -1:
+            continue
+
+        # 开始一个新区域
+        current_region = []
+        seeds = [seed_idx]
+        labels[seed_idx] = label_id
+
+        while seeds:
+            current_seed = seeds.pop(0)
+            current_region.append(current_seed)
+
+            # 搜索半径内的邻居（动态半径或固定半径）
+            neighbors = tree.query_ball_point(points[current_seed], r=0.2)
+
+            for neighbor in neighbors:
+                if labels[neighbor] != -1:
+                    continue
+
+                # 检查法线夹角
+                dot_product = np.abs(np.dot(normals[current_seed], normals[neighbor]))
+                if dot_product > cos_threshold:
+                    labels[neighbor] = label_id
+
+                    # 如果曲率足够小，作为新种子
+                    if curvature[neighbor] < curvature_threshold:
+                        seeds.append(neighbor)
+
+        # 检查区域大小
+        region_size = np.sum(labels == label_id)
+        if region_size < min_cluster_size:
+            # 区域太小，标记为噪声
+            labels[labels == label_id] = -1
+        else:
+            label_id += 1
+
+    return labels
+
+
+# 使用示例
+if __name__ == "__main__":
+    import open3d as o3d
+
+    # 读取点云
+    pcd = o3d.io.read_point_cloud("E:\point_distance\data\\bell_pepper_6.pcd")
+    points = np.asarray(pcd.points)
+
+    # 预处理：体素下采样 + 去噪
+    pcd = pcd.voxel_down_sample(0.0002)
+    pcd, _ = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
+    points = np.asarray(pcd.points)
+
+    # 计算法线和曲率
+    print("计算法线...")
+    normals = estimate_normals(points, k=30)
+
+    print("计算曲率...")
+    curvature = compute_curvature(points, normals, k=30)
+
+    # 区域生长分割
+    print("区域生长...")
+    labels = region_growing(points, normals, curvature,
+                            angle_threshold=25.0,
+                            curvature_threshold=0.05,
+                            min_cluster_size=100)
+
+    print(f"分割完成，共 {len(np.unique(labels)) - (1 if -1 in labels else 0)} 个区域")
+
+    # 可视化
+    colors = np.random.rand(len(np.unique(labels)), 3)
+    point_colors = np.array([colors[label] if label != -1 else [0.5, 0.5, 0.5]
+                             for label in labels])
+    pcd.colors = o3d.utility.Vector3dVector(point_colors)
+    o3d.visualization.draw_geometries([pcd])

alg/point_porj/archive/relative_code/ROR.py

@@ -0,0 +1,52 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+def chamfer_distance(pcd1, pcd2):
+    """
+    计算两个点云之间的 CD 距离（Chamfer Distance）
+
+    Args:
+        pcd1: Open3D 点云对象1
+        pcd2: Open3D 点云对象2
+
+    Returns:
+        cd: Chamfer Distance 值
+        cd1: 点云1 到 点云2 的平均距离
+        cd2: 点云2 到 点云1 的平均距离
+    """
+    # 获取点坐标
+    points1 = np.asarray(pcd1.points)
+    points2 = np.asarray(pcd2.points)
+
+    # 构建 KD-Tree 加速最近邻搜索
+    tree1 = KDTree(points1)
+    tree2 = KDTree(points2)
+
+    # 计算 points1 中每个点到 points2 的最近距离
+    dist1, _ = tree2.query(points1, k=1)
+    cd1 = np.mean(dist1 ** 2)  # 平方距离的平均值
+
+    # 计算 points2 中每个点到 points1 的最近距离
+    dist2, _ = tree1.query(points2, k=1)
+    cd2 = np.mean(dist2 ** 2)  # 平方距离的平均值
+
+    # 总 CD 距离（双向平均）
+    cd = cd1 + cd2
+
+    return np.sqrt(cd), cd1, cd2
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_left_side.ply")
+points = np.asarray(pcd.points)
+colors = np.tile([0.8, 0.7, 1.0], (len(pcd.points), 1))
+pcd.colors = o3d.utility.Vector3dVector(colors)
+o3d.visualization.draw_geometries([pcd],window_name="1")
+# 半径离群点移除
+cl, ind = pcd.remove_radius_outlier(nb_points=5, radius=20)
+
+# 提取内点
+inlier_cloud = pcd.select_by_index(ind)
+outlier_cloud = pcd.select_by_index(ind, invert=True)
+
+print(f"原始点云: {len(pcd.points)} 点")
+print(f"滤波后: {len(inlier_cloud.points)} 点")
+cd,cd1,cd2=chamfer_distance(pcd1=pcd,pcd2=inlier_cloud)
+print(cd)

alg/point_porj/archive/relative_code/SOR.py

@@ -0,0 +1,60 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+from plyfile import PlyData
+def chamfer_distance(pcd1, pcd2):
+    """
+    计算两个点云之间的 CD 距离（Chamfer Distance）
+
+    Args:
+        pcd1: Open3D 点云对象1
+        pcd2: Open3D 点云对象2
+
+    Returns:
+        cd: Chamfer Distance 值
+        cd1: 点云1 到 点云2 的平均距离
+        cd2: 点云2 到 点云1 的平均距离
+    """
+    # 获取点坐标
+    points1 = np.asarray(pcd1.points)
+    points2 = np.asarray(pcd2.points)
+
+    # 构建 KD-Tree 加速最近邻搜索
+    tree1 = KDTree(points1)
+    tree2 = KDTree(points2)
+
+    # 计算 points1 中每个点到 points2 的最近距离
+    dist1, _ = tree2.query(points1, k=1)
+    cd1 = np.mean(dist1 ** 2)  # 平方距离的平均值
+
+    # 计算 points2 中每个点到 points1 的最近距离
+    dist2, _ = tree1.query(points2, k=1)
+    cd2 = np.mean(dist2 ** 2)  # 平方距离的平均值
+
+    # 总 CD 距离（双向平均）
+    cd = cd1 + cd2
+
+    return cd, cd1, cd2
+
+pcd = o3d.io.read_point_cloud("E:\point_distance\data\part_car_head.ply")
+colors = np.tile([1, 0, 0], (len(pcd.points), 1))
+pcd.colors = o3d.utility.Vector3dVector(colors)
+# ply_data = PlyData.read(r"E:\point_distance\data\part_car_head.ply")
+# colors=np.asarray(ply_data.colors)
+# with open("E:\point_distance\data\part_car_head.ply", 'r',encoding="utf-8") as f:
+#     lines = f.readlines()
+#     for line in lines[:30]:  # 打印头部
+#         print(line.strip())
+# o3d.visualization.draw_geometries([pcd])
+# points = np.asarray(pcd.points)
+# colors = np.asarray(pcd.colors)
+# normals = np.asarray(pcd.normals)
+cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=50.0)
+inlier_cloud = pcd.select_by_index(ind)
+colors=np.tile([0,1,0],(len(inlier_cloud.points),1))
+inlier_cloud.colors=o3d.utility.Vector3dVector(colors)
+o3d.visualization.draw_geometries([inlier_cloud])
+o3d.visualization.draw_geometries([pcd])
+o3d.visualization.draw_geometries()
+cd,cd1,cd2=chamfer_distance(pcd1=pcd,pcd2=inlier_cloud)
+print(cd)

alg/point_porj/archive/relative_code/VGF.py

@@ -0,0 +1,96 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+
+
+def evaluate_voxel_filter(pcd, leaf_size=0.05):
+    """
+    评估体素滤波效果，计算各项指标
+
+    Args:
+        pcd: 原始点云
+        leaf_size: 体素边长
+
+    Returns:
+        metrics: 指标字典
+        filtered_pcd: 滤波后点云
+    """
+    # 执行体素滤波
+    filtered_pcd = pcd.voxel_down_sample(leaf_size)
+
+    n_orig = len(pcd.points)
+    n_filt = len(filtered_pcd.points)
+
+    # 1. 基础统计指标
+    keep_ratio = n_filt / n_orig * 100
+    downsample_rate = (n_orig - n_filt) / n_orig * 100
+    compression_ratio = n_orig / n_filt if n_filt > 0 else 0
+
+    # 2. 密度变化指标
+    points_orig = np.asarray(pcd.points)
+    points_filt = np.asarray(filtered_pcd.points)
+
+    # 计算平均最近邻距离（采样加速）
+    sample_size = min(1000, n_filt)
+    if n_filt > sample_size:
+        idx = np.random.choice(n_filt, sample_size, replace=False)
+        points_sample = points_filt[idx]
+    else:
+        points_sample = points_filt
+
+    if len(points_sample) > 5:
+        tree = KDTree(points_sample)
+        dist, _ = tree.query(points_sample, k=2)
+        avg_distance = np.mean(dist[:, 1])
+    else:
+        avg_distance = 0
+
+    # 3. 输出指标
+    metrics = {
+        'leaf_size': leaf_size,
+        'n_orig': n_orig,
+        'n_filt': n_filt,
+        'keep_ratio': keep_ratio,
+        'downsample_rate': downsample_rate,
+        'compression_ratio': compression_ratio,
+        'avg_point_distance': avg_distance,
+        'estimated_density': 1 / (avg_distance ** 3) if avg_distance > 0 else 0
+    }
+
+    return metrics, filtered_pcd
+
+
+# ========== 使用示例 ==========
+if __name__ == "__main__":
+    # 读取点云
+    pcd = o3d.io.read_point_cloud(r"E:\point_distance\data\part_car_head.ply")
+
+    # 测试不同体素大小
+    leaf_sizes = [2, 15, 20, 2]
+
+    print("=" * 60)
+    print("体素滤波指标评估")
+    print("=" * 60)
+    print(f"{'体素边长':<10} {'保留率':<12} {'下采样率':<12} {'压缩比':<10} {'平均间距':<12}")
+    print("-" * 60)
+
+    for leaf_size in leaf_sizes:
+        metrics, filtered = evaluate_voxel_filter(pcd, leaf_size)
+        print(f"{leaf_size:<10} {metrics['keep_ratio']:>8.2f}%    "
+              f"{metrics['downsample_rate']:>8.2f}%    "
+              f"{metrics['compression_ratio']:>8.1f}x    "
+              f"{metrics['avg_point_distance']:>8.4f}m")
+
+    # 可视化对比
+    print("\n可视化对比...")
+    orig_colored = o3d.geometry.PointCloud()
+    orig_colored.points = pcd.points
+    orig_colored.paint_uniform_color([0, 0, 1])  # 蓝色
+    # o3d.visualization.draw_geometries([orig_colored],window_name=f"orignal (blue,{len(pcd.points)}")
+    metrics, filtered = evaluate_voxel_filter(pcd, leaf_size=20)
+    filtered.paint_uniform_color([1, 0, 0])  # 红色
+    # o3d.visualization.draw_geometries([filtered], window_name=f"Voxel (red,{len(filtered.points)}")
+    o3d.visualization.draw_geometries(
+        [orig_colored, filtered],
+        window_name=f"Original (Blue, {len(pcd.points)} pts) vs Voxel (Red, {len(filtered.points)} pts)"
+    )

alg/point_porj/archive/relative_code/distance_mesh.py

@@ -0,0 +1,141 @@
+import open3d as o3d
+import numpy as np
+from scipy.spatial import KDTree
+import copy
+
+
+def compute_point_to_mesh_distance(point_cloud, mesh):
+    """
+    计算点云到 Mesh 的距离
+
+    Args:
+        point_cloud: Open3D 点云对象
+        mesh: Open3D 网格对象
+
+    Returns:
+        distances: 每个点到最近三角形面的距离
+    """
+    # 方法1：使用 Open3D 内置函数（新版本）
+    try:
+        distances = mesh.compute_point_cloud_distance(point_cloud)
+        return np.asarray(distances)
+    except AttributeError:
+        pass
+
+    # 方法2：使用 mesh 顶点最近邻（近似）
+    mesh_points = np.asarray(mesh.vertices)
+    points = np.asarray(point_cloud.points)
+
+    tree = KDTree(mesh_points)
+    distances, indices = tree.query(points)
+
+    return distances
+
+
+def compute_statistics(distances):
+    """计算距离统计信息"""
+    stats = {
+        'mean': np.mean(distances),
+        'rmse': np.sqrt(np.mean(distances ** 2)),
+        'max': np.max(distances),
+        'min': np.min(distances),
+        'std': np.std(distances),
+        'median': np.median(distances),
+    }
+
+    # 分位数
+    percentiles = [50, 75, 90, 95, 99]
+    for p in percentiles:
+        stats[f'percentile_{p}'] = np.percentile(distances, p)
+
+    return stats
+
+
+def visualize_distance(point_cloud, mesh, distances, max_dist=None):
+    """可视化距离（按距离着色）"""
+    pcd_colored = copy.deepcopy(point_cloud)
+
+    if max_dist is None:
+        max_dist = np.percentile(distances, 95)
+
+    # 生成颜色（红：远，蓝：近）
+    colors = np.zeros((len(distances), 3))
+    norm_dist = np.clip(distances / (max_dist + 1e-8), 0, 1)
+    colors[:, 0] = norm_dist  # 红色通道（远）
+    colors[:, 2] = 1 - norm_dist  # 蓝色通道（近）
+
+    pcd_colored.colors = o3d.utility.Vector3dVector(colors)
+
+    # Mesh 着色为灰色
+    mesh_colored = copy.deepcopy(mesh)
+    mesh_colored.paint_uniform_color([0.5, 0.5, 0.5])
+
+    # 可视化
+    o3d.visualization.draw_geometries(
+        [mesh_colored, pcd_colored],
+        window_name="Distance to Mesh (Blue=Near, Red=Far)",
+        width=1024,
+        height=768
+    )
+
+
+def print_statistics(stats):
+    """打印统计信息"""
+    print("=" * 60)
+    print("点云到 Mesh 距离统计")
+    print("=" * 60)
+    print(f"平均距离: {stats['mean']:.6f} m ({stats['mean'] * 1000:.3f} mm)")
+    print(f"RMSE:     {stats['rmse']:.6f} m ({stats['rmse'] * 1000:.3f} mm)")
+    print(f"中位数:   {stats['median']:.6f} m ({stats['median'] * 1000:.3f} mm)")
+    print(f"标准差:   {stats['std']:.6f} m ({stats['std'] * 1000:.3f} mm)")
+    print(f"最大距离: {stats['max']:.6f} m ({stats['max'] * 1000:.3f} mm)")
+    print(f"最小距离: {stats['min']:.6f} m ({stats['min'] * 1000:.3f} mm)")
+    print("\n距离分位数:")
+    print(f"  50%:  {stats['percentile_50']:.6f} m ({stats['percentile_50'] * 1000:.3f} mm)")
+    print(f"  75%:  {stats['percentile_75']:.6f} m ({stats['percentile_75'] * 1000:.3f} mm)")
+    print(f"  90%:  {stats['percentile_90']:.6f} m ({stats['percentile_90'] * 1000:.3f} mm)")
+    print(f"  95%:  {stats['percentile_95']:.6f} m ({stats['percentile_95'] * 1000:.3f} mm)")
+    print(f"  99%:  {stats['percentile_99']:.6f} m ({stats['percentile_99'] * 1000:.3f} mm)")
+
+
+# ========== 主程序 ==========
+if __name__ == "__main__":
+    # 读取文件（使用原始字符串避免转义问题）
+    file_path = r"E:\point_distance\data\40_noisy.ply"
+    mesh_path = r"E:\point_distance\data\40.stl"
+
+    print("读取点云...")
+    source_pcd = o3d.io.read_point_cloud(file_path)
+
+    print("读取 Mesh...")
+    mesh = o3d.io.read_triangle_mesh(mesh_path)
+
+    # 检查读取是否成功
+    if len(source_pcd.points) == 0:
+        print("错误：点云读取失败，请检查文件路径")
+        exit()
+
+    if len(mesh.vertices) == 0:
+        print("错误：Mesh 读取失败，请检查文件路径")
+        exit()
+
+    print(f"\n点云点数: {len(source_pcd.points)}")
+    print(f"Mesh 顶点数: {len(mesh.vertices)}")
+    print(f"Mesh 面数: {len(mesh.triangles)}")
+
+    # 可选：Mesh 法线计算（用于更好的可视化）
+    mesh.compute_vertex_normals()
+
+    # 计算距离
+    print("\n计算点云到 Mesh 的距离...")
+    distances = compute_point_to_mesh_distance(source_pcd, mesh)
+
+    # 计算统计信息
+    stats = compute_statistics(distances)
+
+    # 打印统计信息
+    print_statistics(stats)
+
+    # 可视化
+    print("\n可视化...")
+    visualize_distance(source_pcd, mesh, distances)

alg/point_porj/archive/relative_code/test5.py

@@ -0,0 +1,25 @@
+import trimesh
+import numpy as np
+import open3d as o3d
+import time
+# 1. 读取数据
+mesh_trimesh = trimesh.load("E:\point_distance\data\\40.stl")
+pcd_o3d = o3d.io.read_point_cloud("E:\point_distance\data\\40_noisy.ply")
+points = np.asarray(pcd_o3d.points)
+start_time = time.time()
+
+# ========== 1. 读取数据 ==========
+
+t0 = time.time()
+# 2. 使用 trimesh 计算每个点到 Mesh 的最近距离
+# trimesh 的 nearest 方法非常高效且精确
+closest_points, distances, triangle_ids = mesh_trimesh.nearest.on_surface(points)
+t1=time.time()
+
+# distances 即为所求的距离数组
+print(f"平均距离: {np.mean(distances)}")
+print(f"最大距离: {np.max(distances)}")
+print(f"  距离计算:   {t1 - t0:.4f} 秒")
+# 如果需要将结果存回 Open3D 点云进行可视化
+dists_o3d = o3d.utility.DoubleVector(distances.tolist())
+# 注意：Open3D 点云对象本身不直接存储标量距离字段用于内置可视化着色

alg/point_porj/archive/requirements.txt

@@ -0,0 +1,10 @@
+#python 3.10.8
+--extra-index-url https://download.pytorch.org/whl/cu118
+torch
+torchvision
+torchaudio
+trimesh==4.5.3
+numpy==2.2.2
+open3d==0.19.0
+pythonocc-core==7.5.1 # conda install -c conda-forge pythonocc-core=
+git+https://github.com/AutodeskAILab/occwl #pip install git+https://github.com/AutodeskAILab/occwl