【Lidar】基于Python格网法计算点云体积（eg.树木体积）

# -*- coding: utf-8 -*-
"""
@Time ： 2023/9/3 14:37
@Auth ： RS迷途小书童
@File ：Grid method for volume calculation.py
@IDE ：PyCharm
@Purpose：格网法计算点云体积并展示刨面图
"""
import open3d as o3d
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.colors import LinearSegmentedColormap


def grid_method(point_cloud, grid_size):
    points = np.asarray(point_cloud.points)
    # 将点云数据转换为数组
    points_x = points[:, 0]  # 获取x的数组
    points_y = points[:, 1]  # 获取y的数组
    cols_x = int(np.ceil((np.max(points_x) - np.min(points_x)) / grid_size))+1  # 计算栅格的列数
    cols_y = int(np.ceil((np.max(points_y) - np.min(points_y)) / grid_size))+1  # 计算栅格的行数
    start_x = np.min(points_x)  # 获取x的最小值作为格网的起始x
    start_y = np.min(points_y)  # 获取y的最小值作为格网的起始y
    result = 0  # 初始化体积为0
    array_show = np.zeros((cols_x, cols_y))  # 创建用来显示的数组
    for col_x in range(0, cols_x):  # 遍历格网的列
        end_x = np.min(points_x) + grid_size * (col_x+1)
        # 获取当前格网的范围
        for col_y in range(0, cols_y):  # 遍历格网的行
            end_y = np.min(points_y) + grid_size * (col_y+1)
            # 获取当前格网的范围
            mask = (points[:, 0] >= start_x) & (points[:, 0] <= end_x) & (points[:, 1] >= start_y) & (points[:, 1] <= end_y)
            # 通过格网范围筛选出落在当前网格内的点
            if np.sum(mask) > 0:
                # 如果当前网格内有点，则记录最大高度
                max_z = np.max(points[mask, 2])
                min_z = np.min(points[mask, 2])
                height = max_z - min_z  # 计算格网内的高度插差值
                # print("Max_z:",max_z)
                # print("Min_z:",min_z)
                # print("Height:",height)
                if height <= 0.0005:
                    # 记录高度差非常小的格网，原因：格网范围过小，最高点和最低点没有落在格网内
                    print("当前起始点：", start_x, start_y)
                    print("当前终止点：", end_x, end_y)
                    print("当前格网内的点云为：")
                    print(np.array([points[mask, 0], points[mask, 1], points[mask, 2]]))
                result += height * grid_size ** 2  # 将格网的体积累加
                array_show[col_x, col_y] = height
            start_y = end_y
        start_x = end_x
    return result, array_show


if __name__ == "__main__":
    pcd_path = "彭俊喜/1 - Cloud.pcd"  # 读取点云文件
    pcd = o3d.io.read_point_cloud(pcd_path)
    Grid_size = 0.1  # 设置栅格大小（根据实际情况调整）
    volume, array = grid_method(pcd, Grid_size)  # 计算树冠体积
    print("树冠体积:", volume)
    fig, ax = plt.subplots()  # 创建图形和轴对象
    cmap = LinearSegmentedColormap.from_list('white_to_red', ['white', (0.8, 0.5, 0.2), 'yellow', 'green'])
    im = ax.imshow(array, cmap=cmap)  # 绘制图像'coolwarm'
    cbar = fig.colorbar(im, ax=ax, orientation='vertical')  # 添加颜色拉伸的图例
    cbar.set_label('Colorbar Label')
    plt.savefig(r"G:\Anaconda\ProjectYOLO\yolov5-7.0\Point Cloud/1.png")  # 将图形窗口中的内容保存到指定的路径
    plt.show()  # 显示图形