【Python】Open3d用于3D测高项目

Open3D 非常适合 3D 测高项目 （如物体高度测量、地形高程分析、点云垂直距离计算等），核心思路是：通过点云数据获取目标物体 / 场景的 3D 坐标，结合「基准面（如地面）」或「参考点」计算垂直距离（高度）。以下是完整的实现方案，涵盖 单物体测高、多物体批量测高、地形高程分析 三大场景，兼容 Open3D 全版本。

一、核心原理

3D 测高中的 "高度" 本质是 目标点与基准面（或参考点）在垂直方向上的距离，常见两种计算方式：

1. 基准面法（推荐）：先拟合 / 生成地面（基准面），计算每个目标点到基准面的垂直距离（即高度）；
2. 参考点法：以某个已知高度的点（如地面上的参考点）为基准，计算目标点与参考点的 Z 轴（或自定义垂直轴）差值。

本文重点讲解 基准面法（更通用，适配复杂场景），步骤如下：

1. 加载 / 预处理点云（去噪、下采样，提升计算效率）；
2. 生成 / 拟合基准面（地面），确保覆盖目标区域；
3. 提取目标物体的点云（或关键顶点）；
4. 计算目标点到基准面的垂直距离，得到高度。

二、完整实现代码（三大场景）

依赖准备

pip install open3d numpy scipy  # scipy 用于点云分割（可选）

代码总览（含工具函数 + 场景测试）

import open3d as o3d
import numpy as np
from scipy.spatial import cKDTree  # 用于最近点查询（可选）

# ---------------------- 基础工具函数（复用之前的兼容版平面生成+新增测高工具）----------------------
def create_plane_compatible(
    width: float = 1.0,
    height: float = 1.0,
    resolution: tuple[int, int] = (1, 1),
    center: tuple[float, float, float] = (0.0, 0.0, 0.0),
    direction: tuple[float, float, float] = (0.0, 0.0, 1.0)
) -> o3d.geometry.TriangleMesh:
    """兼容低版本的平面生成函数（基准面用）"""
    x_res, y_res = resolution
    if x_res < 1 or y_res < 1:
        raise ValueError("分辨率必须为正整数")
    if width <= 0 or height <= 0:
        raise ValueError("宽度/高度必须为正数")
    
    # 生成本地 XY 平面顶点
    x = np.linspace(-width/2, width/2, x_res + 1)
    y = np.linspace(-height/2, height/2, y_res + 1)
    xx, yy = np.meshgrid(x, y)
    zz = np.zeros_like(xx)
    vertices_local = np.column_stack([xx.ravel(), yy.ravel(), zz.ravel()])
    
    # 生成三角形索引
    triangles = []
    for i in range(y_res):
        for j in range(x_res):
            v0 = i * (x_res + 1) + j
            v1 = v0 + 1
            v2 = (i + 1) * (x_res + 1) + j
            v3 = v2 + 1
            triangles.append([v0, v1, v3])
            triangles.append([v0, v3, v2])
    triangles = np.array(triangles, dtype=np.int32)
    
    # 旋转平面以匹配法向量
    dir_norm = np.array(direction)
    dir_norm = dir_norm / np.linalg.norm(dir_norm) if np.linalg.norm(dir_norm) != 0 else np.array([0,0,1])
    default_normal = np.array([0, 0, 1])
    if not np.allclose(dir_norm, default_normal):
        axis = np.cross(default_normal, dir_norm)
        axis = axis / np.linalg.norm(axis) if np.linalg.norm(axis) != 0 else np.array([1,0,0])
        angle = np.arccos(np.clip(np.dot(default_normal, dir_norm), -1.0, 1.0))
        rotation_mat = o3d.geometry.get_rotation_matrix_from_axis_angle(axis * angle)
        vertices_local = (rotation_mat @ vertices_local.T).T
    
    # 平移到目标中心
    vertices_local += np.array(center)
    
    # 创建网格并计算法向量
    mesh = o3d.geometry.TriangleMesh()
    mesh.vertices = o3d.utility.Vector3dVector(vertices_local)
    mesh.triangles = o3d.utility.Vector3iVector(triangles)
    mesh.compute_vertex_normals()
    mesh.compute_triangle_normals()
    return mesh

def point_to_plane_distance(points: np.ndarray, plane_params: tuple[float, float, float, float]) -> np.ndarray:
    """
    计算点到平面的垂直距离（高度核心函数）
    参数：
        points: (N, 3) 点云坐标数组
        plane_params: 平面方程参数 (A, B, C, D)，满足 Ax + By + Cz + D = 0
    返回：
        distances: (N,) 每个点到平面的垂直距离（正值表示在平面法向量一侧，负值相反）
    """
    A, B, C, D = plane_params
    numerator = np.abs(A * points[:, 0] + B * points[:, 1] + C * points[:, 2] + D)
    denominator = np.sqrt(A**2 + B**2 + C**2)
    return numerator / denominator  # 距离非负（高度为绝对值）

def fit_ground_plane(points: np.ndarray, z_threshold: float = 0.1) -> tuple[tuple[float, float, float, float], np.ndarray]:
    """
    从点云中拟合地面基准面（RANSAC 算法，抗噪性强）
    参数：
        points: (N, 3) 点云坐标数组
        z_threshold: 地面点的高度阈值（用于筛选地面候选点）
    返回：
        plane_params: 平面方程 (A, B, C, D)
        ground_points: 筛选出的地面点云坐标
    """
    # 1. 筛选 Z 轴较低的候选地面点（减少非地面点干扰）
    z_min = np.min(points[:, 2])
    ground_candidates = points[points[:, 2] < z_min + z_threshold]
    if len(ground_candidates) < 3:
        raise ValueError("地面候选点不足，无法拟合平面！")
    
    # 2. 使用 RANSAC 拟合平面（Open3D 内置，抗噪性强）
    pcd_candidates = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(ground_candidates))
    plane_model, inliers = pcd_candidates.segment_plane(
        distance_threshold=0.05,  # 内点距离阈值（可调整）
        ransac_n=3,               # 拟合平面所需最少点数
        num_iterations=1000       # 迭代次数
    )
    A, B, C, D = plane_model
    ground_points = ground_candidates[inliers]  # 最终筛选的地面点
    
    return (A, B, C, D), ground_points

# ---------------------- 场景1：单物体测高（已知目标点云/包围盒）----------------------
def measure_single_object_height(
    point_cloud: o3d.geometry.PointCloud,
    use_fitted_plane: bool = True  # True=拟合地面，False=使用包围盒底部作为基准
) -> tuple[float, o3d.geometry.TriangleMesh, tuple[float, float, float, float] | None]:
    """
    测量单个物体的高度（点云整体高度）
    参数：
        point_cloud: 目标物体的点云（需单独提取，不含地面）
        use_fitted_plane: 是否拟合地面作为基准（推荐True，更准确）
    返回：
        height: 物体高度（单位：与点云坐标一致）
        base_plane: 基准面网格（可视化用）
        plane_params: 平面方程参数（use_fitted_plane=True时返回）
    """
    points = np.asarray(point_cloud.points)
    if len(points) == 0:
        raise ValueError("目标物体点云为空！")
    
    # 1. 确定基准面
    if use_fitted_plane:
        # 拟合地面基准面（假设输入点云包含地面，或单独传入地面点云）
        plane_params, ground_points = fit_ground_plane(points)
        # 生成基准面网格（覆盖地面点云范围）
        ground_aabb = o3d.geometry.AxisAlignedBoundingBox().create_from_points(o3d.utility.Vector3dVector(ground_points))
        ground_center = ground_aabb.get_center()
        ground_width = ground_aabb.max_bound[0] - ground_aabb.min_bound[0]
        ground_height = ground_aabb.max_bound[1] - ground_aabb.min_bound[1]
        base_plane = create_plane_compatible(
            width=ground_width * 1.2,  # 扩大20%确保覆盖
            height=ground_height * 1.2,
            center=ground_center,
            direction=(plane_params[0], plane_params[1], plane_params[2])  # 平面法向量
        )
    else:
        # 简化方案：用物体包围盒底部作为基准（适合无地面点云场景）
        aabb = point_cloud.get_axis_aligned_bounding_box()
        base_z = aabb.min_bound[2]
        plane_params = None
        # 生成基准面（覆盖物体包围盒）
        base_plane = create_plane_compatible(
            width=aabb.max_bound[0] - aabb.min_bound[0],
            height=aabb.max_bound[1] - aabb.min_bound[1],
            center=aabb.get_center(),
            direction=(0, 0, 1)
        )
        # 平移基准面到包围盒底部
        base_plane.translate([0, 0, base_z - aabb.get_center()[2]])
    
    # 2. 计算物体高度（最高点到基准面的距离）
    if use_fitted_plane:
        distances = point_to_plane_distance(points, plane_params)
        max_distance = np.max(distances)  # 最高点高度
        height = max_distance
    else:
        # 包围盒高度（Z轴最大-最小）
        aabb = point_cloud.get_axis_aligned_bounding_box()
        height = aabb.max_bound[2] - aabb.min_bound[2]
    
    return height, base_plane, plane_params

# ---------------------- 场景2：多物体批量测高（点云分割后）----------------------
def measure_multi_objects_height(
    full_point_cloud: o3d.geometry.PointCloud,
    cluster_threshold: float = 0.1,  # 聚类距离阈值（控制物体分割）
    min_points_per_object: int = 50   # 最小物体点数（过滤噪声）
) -> tuple[list[float], list[o3d.geometry.PointCloud], o3d.geometry.TriangleMesh]:
    """
    批量测量点云中多个物体的高度（自动分割物体）
    参数：
        full_point_cloud: 包含地面+多个物体的完整点云
        cluster_threshold: DBSCAN 聚类距离阈值
        min_points_per_object: 每个物体的最小点数
    返回：
        heights: 每个物体的高度列表
        object_pcds: 每个物体的点云列表
        ground_plane: 地面基准面网格
    """
    # 1. 预处理：下采样（减少计算量）+ 去噪
    pcd_down = full_point_cloud.voxel_down_sample(voxel_size=0.02)
    cl, ind = pcd_down.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
    pcd_clean = pcd_down.select_by_index(ind)
    points_clean = np.asarray(pcd_clean.points)
    
    # 2. 拟合地面并分割（分离地面和物体）
    plane_params, ground_points = fit_ground_plane(points_clean)
    ground_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(ground_points))
    # 计算所有点到地面的距离，筛选非地面点（物体点）
    distances = point_to_plane_distance(points_clean, plane_params)
    object_indices = np.where(distances > 0.05)[0]  # 距离地面>0.05的为物体点
    object_pcd = pcd_clean.select_by_index(object_indices)
    
    # 3. 物体聚类（分割多个物体）
    with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error):
        labels = np.array(object_pcd.cluster_dbscan(
            eps=cluster_threshold,
            min_points=min_points_per_object,
            print_progress=False
        ))
    max_label = labels.max()
    if max_label < 0:
        raise ValueError("未检测到物体点云！")
    
    # 4. 批量计算每个物体的高度
    heights = []
    object_pcds = []
    for label in range(max_label + 1):
        object_idx = np.where(labels == label)[0]
        single_object_pcd = object_pcd.select_by_index(object_idx)
        # 计算当前物体高度（最高点到地面的距离）
        single_points = np.asarray(single_object_pcd.points)
        single_distances = point_to_plane_distance(single_points, plane_params)
        height = np.max(single_distances)
        heights.append(round(height, 3))
        object_pcds.append(single_object_pcd)
    
    # 5. 生成地面基准面网格
    ground_aabb = o3d.geometry.AxisAlignedBoundingBox().create_from_points(ground_pcd.points)
    ground_center = ground_aabb.get_center()
    ground_width = ground_aabb.max_bound[0] - ground_aabb.min_bound[0]
    ground_height = ground_aabb.max_bound[1] - ground_aabb.min_bound[1]
    ground_plane = create_plane_compatible(
        width=ground_width * 1.2,
        height=ground_height * 1.2,
        center=ground_center,
        direction=(plane_params[0], plane_params[1], plane_params[2])
    )
    
    return heights, object_pcds, ground_plane

# ---------------------- 场景3：地形高程分析（逐点测高+可视化）----------------------
def terrain_elevation_analysis(
    terrain_point_cloud: o3d.geometry.PointCloud,
    grid_resolution: float = 0.5  # 高程网格分辨率（单位：米/格）
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    地形高程分析（生成高程热力图网格）
    参数：
        terrain_point_cloud: 地形点云（含地面）
        grid_resolution: 高程网格的单元格大小
    返回：
        elevation_grid: 高程网格（每个单元格的平均高度）
        x_coords: 网格X轴坐标
        y_coords: 网格Y轴坐标
    """
    points = np.asarray(terrain_point_cloud.points)
    
    # 1. 拟合地面基准面（作为高程零点）
    plane_params, _ = fit_ground_plane(points)
    
    # 2. 计算每个点的高程（到基准面的距离）
    elevations = point_to_plane_distance(points, plane_params)
    
    # 3. 生成网格并统计每个网格的平均高程
    x_min, x_max = np.min(points[:, 0]), np.max(points[:, 0])
    y_min, y_max = np.min(points[:, 1]), np.max(points[:, 1])
    
    # 网格坐标
    x_coords = np.arange(x_min, x_max, grid_resolution)
    y_coords = np.arange(y_min, y_max, grid_resolution)
    x_grid, y_grid = np.meshgrid(x_coords, y_coords)
    
    # 统计每个网格的平均高程
    elevation_grid = np.zeros_like(x_grid)
    for i in range(len(y_coords)):
        for j in range(len(x_coords)):
            # 筛选当前网格内的点
            x_mask = (points[:, 0] >= x_coords[j]) & (points[:, 0] < x_coords[j] + grid_resolution)
            y_mask = (points[:, 1] >= y_coords[i]) & (points[:, 1] < y_coords[i] + grid_resolution)
            grid_points_idx = np.where(x_mask & y_mask)[0]
            if len(grid_points_idx) > 0:
                elevation_grid[i, j] = np.mean(elevations[grid_points_idx])
            else:
                elevation_grid[i, j] = np.nan  # 无点的网格设为NaN
    
    return elevation_grid, x_coords, y_coords

# ---------------------- 测试示例 ----------------------
if __name__ == "__main__":
    # 生成测试点云（模拟：地面 + 2个立方体物体）
    def generate_test_point_cloud():
        # 1. 生成地面点云（Z=0，范围(-5,5)x(-5,5)）
        ground_x = np.linspace(-5, 5, 200)
        ground_y = np.linspace(-5, 5, 200)
        gx, gy = np.meshgrid(ground_x, ground_y)
        gz = np.zeros_like(gx) + np.random.normal(0, 0.01, gx.shape)  # 地面微小噪声
        ground_points = np.column_stack([gx.ravel(), gy.ravel(), gz.ravel()])
        
        # 2. 生成物体1（立方体1：边长1，中心(1, 1, 0.5)，高度1）
        cube1_x = np.linspace(0.5, 1.5, 50)
        cube1_y = np.linspace(0.5, 1.5, 50)
        cube1_z = np.linspace(0, 1, 50)
        cx1, cy1, cz1 = np.meshgrid(cube1_x, cube1_y, cube1_z)
        cube1_points = np.column_stack([cx1.ravel(), cy1.ravel(), cz1.ravel()])
        
        # 3. 生成物体2（立方体2：边长0.8，中心(-2, -1, 0.4)，高度0.8）
        cube2_x = np.linspace(-2.4, -1.6, 40)
        cube2_y = np.linspace(-1.4, -0.6, 40)
        cube2_z = np.linspace(0, 0.8, 40)
        cx2, cy2, cz2 = np.meshgrid(cube2_x, cube2_y, cube2_z)
        cube2_points = np.column_stack([cx2.ravel(), cy2.ravel(), cz2.ravel()])
        
        # 合并所有点云
        all_points = np.vstack([ground_points, cube1_points, cube2_points])
        pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(all_points))
        pcd.paint_uniform_color([0.5, 0.5, 0.5])  # 默认灰色
        return pcd, cube1_points, cube2_points
    
    # 加载测试点云
    full_pcd, cube1_points, cube2_points = generate_test_point_cloud()
    
    # ---------------------- 测试场景1：单物体测高 ----------------------
    print("=== 场景1：单物体测高 ===")
    cube1_pcd = o3d.geometry.PointCloud(o3d.utility.Vector3dVector(cube1_points))
    cube1_pcd.paint_uniform_color([1.0, 0.3, 0.3])  # 红色
    height1, base_plane1, _ = measure_single_object_height(cube1_pcd, use_fitted_plane=True)
    print(f"物体1高度：{height1:.3f}（理论值：1.0）")
    
    # 可视化
    base_plane1.paint_uniform_color([0.3, 0.7, 1.0])  # 蓝色基准面
    o3d.visualization.draw_geometries(
        [cube1_pcd, base_plane1],
        window_name="场景1：单物体测高",
        lookat=[1, 1, 0.5]
    )
    
    # ---------------------- 测试场景2：多物体批量测高 ----------------------
    print("\n=== 场景2：多物体批量测高 ===")
    heights, object_pcds, ground_plane = measure_multi_objects_height(
        full_point_cloud=full_pcd,
        cluster_threshold=0.15,
        min_points_per_object=100
    )
    # 为每个物体设置不同颜色
    colors = [[1.0, 0.3, 0.3], [0.3, 1.0, 0.3]]  # 红、绿
    for i, (height, pcd) in enumerate(zip(heights, object_pcds)):
        pcd.paint_uniform_color(colors[i % len(colors)])
        print(f"物体{i+1}高度：{height:.3f}")
    ground_plane.paint_uniform_color([0.3, 0.7, 1.0])  # 蓝色地面
    
    # 可视化（地面+所有物体）
    o3d.visualization.draw_geometries(
        [ground_plane] + object_pcds,
        window_name="场景2：多物体批量测高",
        lookat=[0, 0, 0]
    )
    
    # ---------------------- 测试场景3：地形高程分析 ----------------------
    print("\n=== 场景3：地形高程分析 ===")
    elevation_grid, x_coords, y_coords = terrain_elevation_analysis(
        terrain_point_cloud=full_pcd,
        grid_resolution=0.5
    )
    # 打印高程网格统计
    valid_elevations = elevation_grid[~np.isnan(elevation_grid)]
    print(f"地形高程范围：{np.min(valid_elevations):.3f} ~ {np.max(valid_elevations):.3f}")
    
    # 可视化高程热力图（用matplotlib）
    import matplotlib.pyplot as plt
    plt.figure(figsize=(10, 8))
    im = plt.imshow(
        elevation_grid,
        extent=[x_coords.min(), x_coords.max(), y_coords.min(), y_coords.max()],
        origin="lower",
        cmap="jet"
    )
    plt.colorbar(im, label="高程（单位：m）")
    plt.xlabel("X坐标（m）")
    plt.ylabel("Y坐标（m）")
    plt.title("地形高程热力图")
    plt.show()

运行效果图

三、关键功能详解

1. 核心测高函数（point_to_plane_distance）

• 基于平面方程计算点到平面的垂直距离，是 3D 测高的核心；
• 平面方程 Ax + By + Cz + D = 0 可通过两种方式获取：
  • 拟合地面（fit_ground_plane，用 RANSAC 算法，抗噪性强，适合真实场景）；
  • 手动指定（如已知地面是 Z=0 平面，方程为 0x + 0y + 1z + 0 = 0）。

2. 单物体测高（场景 1）

• 适用于：目标物体点云已单独提取（如通过激光雷达框选、语义分割）；
• 两种基准面模式：
  • use_fitted_plane=True：拟合地面作为基准（更准确，不受物体倾斜影响）；
  • use_fitted_plane=False：用物体包围盒底部作为基准（简化方案，适合无地面点云）。

3. 多物体批量测高（场景 2）

• 核心流程：点云预处理 → 地面拟合与分割 → 物体聚类（DBSCAN）→ 逐物体测高；
• 关键参数：
  • cluster_threshold：聚类距离阈值（根据点云密度调整，密度高则设小）；
  • min_points_per_object：过滤小噪声点簇，避免误检测。

4. 地形高程分析（场景 3）

• 适用于：大面积地形测量（如测绘、农业、建筑场景）；
• 输出：高程网格 + 热力图，可直观展示地形起伏；
• 关键参数 grid_resolution：网格单元格大小（越小精度越高，但计算量越大）。

四、实际应用注意事项

1. 点云预处理（提升测高精度）

• 下采样 ：用 voxel_down_sample 减少点云数量，提升计算效率；
• 去噪 ：用 remove_statistical_outlier 或 remove_radius_outlier 过滤噪声点；
• 分割：确保目标物体点云与地面分离（可通过语义分割、距离筛选实现）。

2. 基准面选择

• 若场景有明确地面（如室内、户外平地），优先用 fit_ground_plane 拟合，精度更高；
• 若场景无地面（如空中物体），用包围盒底部或自定义参考点作为基准；
• 平面法向量需与 "垂直方向" 一致（如地面法向量为 Z 轴方向）。

3. 单位一致性

• 测高结果的单位与点云坐标单位一致（如点云是米制，高度单位就是米）；
• 若点云是像素坐标（如从图像深度图生成），需先转换为物理坐标。

4. 精度优化

• 增加 RANSAC 迭代次数（num_iterations）提升平面拟合精度；
• 调整聚类阈值（cluster_threshold）和内点距离阈值（distance_threshold）适配不同点云密度；
• 对物体点云取最高点的平均距离（而非单个最高点），减少噪声影响。

五、扩展功能

1. 实时测高：结合传感器（如激光雷达、深度相机）实时获取点云，重复上述流程；
2. 高度可视化：在点云渲染时，按高度映射颜色（如高度越高越红）；
3. 批量处理：遍历文件夹内所有点云文件，自动输出每个文件的物体高度；
4. 导出结果：将高程网格、物体高度保存为 CSV/JSON 文件，用于后续分析。

通过以上方案，可快速实现从点云预处理到高度测量的完整流程，适配大多数 3D 测高场景（如物体尺寸测量、地形测绘、工业检测等）。