#!/usr/bin/env python
# coding=utf-8
import numpy as np
import math
import multiprocessing as mp  # 添加多进程（并行计算）库
import pyransac3d as pyrsc
import zelas.Multispectral as p1  # 添加自己的库
import plotly.graph_objects as go  # 添加其他显示库
import zelas.voxel as vl  # 添加体素类
import zelas.ransac as rs
import zelas.Ellipse as ep
from ellipse import LsqEllipse  # 添加椭圆拟合算法
from tqdm import tqdm
import open3d as o3d
from scipy.spatial import ConvexHull
import cv2 as cv


def random_partition(n, n_data):
    """return n random rows of data and the other len(data) - n rows"""
    all_idxs = np.arange(n_data)  # 获取n_data下标索引
    np.random.shuffle(all_idxs)  # 打乱下标索引
    idxs1 = all_idxs[:n]
    idxs2 = all_idxs[n:]
    return idxs1, idxs2


class CircleLeastSquareModel:
    # 最小二乘 fitting circle
    def __init__(self):
        pass

    def fit(self, data):
        A = []
        B = []
        for d in data:
            A.append([-d[0], -d[1], -1])
            B.append(d[0] ** 2 + d[1] ** 2)
        A_matrix = np.array(A)
        B_matrix = np.array(B)
        C_matrix = A_matrix.T.dot(A_matrix)
        result = np.linalg.inv(C_matrix).dot(A_matrix.T.dot(B_matrix))
        model = CircleLeastSquareModel()
        model.a = result[0]
        model.b = result[1]
        model.c = result[2]
        return model  # 返回最小平方和向量

    # get error 为每个数据点和拟合后的结果之间的误差的平方，换其他模型也可以用
    def get_error(self, data, model):
        err_per_point = []
        for d in data:
            B = d[0] ** 2 + d[1] ** 2
            B_fit = model.a * d[0] + model.b * d[1] + model.c
            err_per_point.append((B + B_fit) ** 2)  # sum squared error per row
        return np.array(err_per_point)


def ransac(data, model, n, k, t, d, debug=False, return_all=False):
    """
    参考:http://scipy.github.io/old-wiki/pages/Cookbook/RANSAC
    伪代码:http://en.wikipedia.org/w/index.php?title=RANSAC&oldid=116358182
    输入:
        data - 样本点
        model - 假设模型:事先自己确定
        n - 生成模型所需的最少样本点
        k - 最大迭代次数
        t - 阈值:作为判断点满足模型的条件
        d - 拟合较好时,需要的样本点最少的个数,当做阈值看待
    输出:
        bestfit - 最优拟合解（返回nil,如果未找到）

    iterations = 0
    bestfit = nil #后面更新
    besterr = something really large #后期更新besterr = thiserr
    while iterations < k
    {
        maybeinliers = 从样本中随机选取n个,不一定全是局内点,甚至全部为局外点
        maybemodel = n个maybeinliers 拟合出来的可能符合要求的模型
        alsoinliers = emptyset #满足误差要求的样本点,开始置空
        for (每一个不是maybeinliers的样本点)
        {
            if 满足maybemodel即error < t
                将点加入alsoinliers
        }
        if (alsoinliers样本点数目 > d)
        {
            %有了较好的模型,测试模型符合度
            bettermodel = 利用所有的maybeinliers 和 alsoinliers 重新生成更好的模型
            thiserr = 所有的maybeinliers 和 alsoinliers 样本点的误差度量
            if thiserr < besterr
            {
                bestfit = bettermodel
                besterr = thiserr
            }
        }
        iterations++
    }
    return bestfit
    """
    iterations = 0
    bestfit = None
    besterr = np.inf  # 设置默认值
    best_inlier_idxs = None
    while iterations < k:
        maybe_idxs, test_idxs = random_partition(n, data.shape[0])
        maybe_inliers = data[maybe_idxs, :]  # 获取size(maybe_idxs)行数据(Xi,Yi)
        test_points = data[test_idxs]  # 若干行(Xi,Yi)数据点
        maybemodel = model.fit(maybe_inliers)  # 拟合模型
        test_err = model.get_error(test_points, maybemodel)  # 计算误差:平方和最小
        also_idxs = test_idxs[test_err < t]
        also_inliers = data[also_idxs, :]
        if debug:
            print('test_err.min()', test_err.min())
            print('test_err.max()', test_err.max())
            print('numpy.mean(test_err)', np.mean(test_err))
            print('iteration %d:len(alsoinliers) = %d' % (iterations, len(also_inliers)))
        if len(also_inliers > d):
            betterdata = np.concatenate((maybe_inliers, also_inliers))  # 样本连接
            bettermodel = model.fit(betterdata)
            better_errs = model.get_error(betterdata, bettermodel)
            thiserr = np.mean(better_errs)  # 平均误差作为新的误差
            if thiserr < besterr:
                bestfit = bettermodel
                besterr = thiserr
                best_inlier_idxs = np.concatenate((maybe_idxs, also_idxs))  # 更新局内点,将新点加入
        iterations += 1
        if bestfit is None:
            raise ValueError("did't meet fit acceptance criteria")
        if return_all:
            return bestfit, {'inliers': best_inlier_idxs}
        else:
            return bestfit


def c_refit(x, z, c, num_cpu=mp.cpu_count()):
    '进行第三次圆拟合'
    name_c = np.unique(c)  # 盾构环名
    model = CircleLeastSquareModel()  # 类的实例化:用最小二乘生成已知模型
    # 并行计算加速算法准备
    # num_cpu = mp.cpu_count()  # 电脑线程数
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    num_all = len(name_c)  # 圆环数
    xzrc = np.empty([num_all, 4])  # 新建一个存储每个圆环的容器
    tik = cut_down(num_all)
    tik_b = 0  # 分块输出计时器
    # 并行计算分块运行
    multi_res = [pool.apply_async(c_seam_block_3, args=(x, z, c, model, name_c, tik[i], tik[i + 1])) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    for res in multi_res:
        xzrc[tik[tik_b]:tik[tik_b + 1], :] = res.get()  # 将每个进程得到的结果分block的发送到容器当中
        tik_b += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    return xzrc


def fit_circle(x, z, c, num_cpu=mp.cpu_count(), t=0.5):
    '''
    双次圆拟合的单次圆拟合函数（需要执行两次）
    :param x: 点云x坐标
    :param z: 点云z坐标
    :param c: 每个点云所属的圆环名
    :param num_cpu: 开启多线程的CPU核心数
    :param t: ransac拟合参数
    :return: xzrc：拟合每个圆环的圆心和半径
    '''
    c_un = np.unique(c)  # 圆环名精简
    num_all = len(c_un)  # 圆环数
    model = CircleLeastSquareModel()  # 圆拟合模型建立
    # 并行计算加速准备
    # num_cpu = mp.cpu_count()  # 电脑线程数
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    xzrc = np.empty([num_all, 4])  # 新建一个存储每个圆环的容器
    tik = cut_down(num_all)  # 分块器
    tik_b = 0  # 分块输出计时器
    # 并行计算每个圆环
    multi_res = [pool.apply_async(c_seam_block, args=(x, z, c, model, c_un, tik[i], tik[i + 1], t)) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    for res in multi_res:
        xzrc[tik[tik_b]:tik[tik_b + 1], :] = res.get()  # 将每个进程得到的结果分block的发送到容器当中
        tik_b += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    print('平均圆心', np.mean(xzrc[:, 0]), np.mean([xzrc[:, 1]]), '平均半径', np.mean(xzrc[:, 2]))
    return xzrc


def c_seam(x, z, c):
    """
    将盾构隧道点云拟合出每个圆环（参数）
    :param x: 点云x坐标数组
    :param z: 点云z坐标数组
    :param c: 每个点云所属圆环名
    :return: 每个圆环的圆心和半径
    """
    # x0 = x
    # z0 = z  # 备份初值
    # c0 = c
    name_c = np.unique(c)  # 盾构环名
    model = CircleLeastSquareModel()  # 类的实例化:用最小二乘生成已知模型
    '''
    plt.figure(figsize=(4, 4))  # 设置显示窗口形状
    plt.plot(x, z)  # 显示初始点云
    plt.show()
    '''
    # 求平均值
    mean_z = np.mean(z)
    # mean_x = np.mean(x)
    # 确定初值
    # r0 = (np.max(x)-np.min(x)) / 2  # 确定初始半径
    # x0 = np.min(x)+r0  # 确定初始圆心坐标
    # z0 = mean_z + std_z/4
    # 寻找点云z轴出现最多的那一区间并剔除所在区间内的点云，留下剩下的
    # plt.hist(z, bins=50)
    # plt.show()
    # 将符合条件的数组全部提取出来(大于z的平均值)
    idz = (z >= mean_z)
    x = x[idz]
    z = z[idz]
    c = c[idz]
    data = np.vstack([x, z]).T
    result = model.fit(data)
    x_0 = result.a * -0.5
    z_0 = result.b * -0.5
    r_0 = 0.5 * math.sqrt(result.a ** 2 + result.b ** 2 - 4 * result.c)
    # circle_0 = Circle(xy=(x_0, z_0), radius=r_0, alpha=0.5, fill=False, label="least square ransac fit circle")
    # plt.gcf().gca().add_artist(circle_0)
    d_dis = 0.15
    dis = np.abs((x - x_0) ** 2 + (z - z_0) ** 2 - r_0 ** 2)
    id_r0 = dis < d_dis
    x = x[id_r0]
    z = z[id_r0]
    c = c[id_r0]
    '''
    idx = (x >= mean_x)
    x = x[idx]
    z = z[idx]
    c = c[idx]
    '''
    # plt.plot(x, z)
    # plt.show()  # 显示窗口
    # 并行计算加速算法准备
    num_cpu = mp.cpu_count()  # 电脑线程数
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    num_all = len(name_c)  # 圆环数
    xzrc = np.empty([num_all, 4])  # 新建一个存储每个圆环的容器
    tik = cut_down(num_all)
    tik_b = 0  # 分块输出计时器
    # 并行计算分块运行
    multi_res = [pool.apply_async(c_seam_block, args=(x, z, c, model, name_c, tik[i], tik[i + 1])) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    for res in multi_res:
        xzrc[tik[tik_b]:tik[tik_b + 1], :] = res.get()  # 将每个进程得到的结果分block的发送到容器当中
        tik_b += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    '''
    xzrc = np.empty([num_all, 4])  # 新建一个存储每个圆环的容器
    j = 0
    for c1 in name_c:
        # x0_c1 = x0[c0 == c1]
        # z0_c1 = z0[c0 == c1]
        x_c1 = x[c == c1]
        z_c1 = z[c == c1]  # 提取单环的全部点云二维坐标
        data1 = np.vstack([x_c1, z_c1]).T
        # run RANSAC 算法
        # model = CircleLeastSquareModel()  # 类的实例化:用最小二乘生成已知模型
        ransac_fit, ransac_data = ransac(data1, model, 50, 2000, 0.5, 100, debug=False, return_all=True)  # ransac迭代
        x1 = ransac_fit.a * -0.5
        z1 = ransac_fit.b * -0.5
        r1 = 0.5 * math.sqrt(ransac_fit.a ** 2 + ransac_fit.b ** 2 - 4 * ransac_fit.c)
        # circle3 = Circle(xy=(x1, z1), radius=r1, alpha=0.5, fill=False, label="least square ransac fit circle")
        # plt.gcf().gca().add_artist(circle3)
        # plt.plot(x0_c1, z0_c1)
        # plt.show()
        # 存储数据
        xzrc[j, 0] = x1
        xzrc[j, 1] = z1
        xzrc[j, 2] = r1
        xzrc[j, 3] = c1
        j += 1
    '''
    return xzrc


# 环缝提取_分块
def c_seam_block(x, z, c, model, name_c, begin_, over_, t=0.13):
    num_ = over_ - begin_  # 计算每个块的圆环数量
    xzrc = np.empty([num_, 4])
    j = 0  # 循环计数器
    for c1 in name_c[begin_:over_]:
        x_c1 = x[c == c1]
        z_c1 = z[c == c1]  # 提取单环的全部点云二维坐标
        data1 = np.vstack([x_c1, z_c1]).T
        ransac_fit, ransac_data = ransac(data1, model, 50, 2000, t, 300, debug=False, return_all=True)  # ransac迭代
        'n:拟合模型所需的最小数据值数 k:算法中允许的最大迭代次数 t:用于确定数据点何时适合模型的阈值 d:断言模型很好地符合数据所需的接近数据值的数量'
        x1 = ransac_fit.a * -0.5
        z1 = ransac_fit.b * -0.5
        r1 = 0.5 * math.sqrt(ransac_fit.a ** 2 + ransac_fit.b ** 2 - 4 * ransac_fit.c)
        xzrc[j, 0] = x1
        xzrc[j, 1] = z1
        xzrc[j, 2] = r1
        xzrc[j, 3] = c1
        j += 1
    # print('每块最后一个值为', name_c[begin_:over_])
    print('已完成第', begin_, '至第', over_, '的圆环')
    return xzrc


# 环缝提取_分块
def c_seam_block_3(x, z, c, model, name_c, begin_, over_):
    num_ = over_ - begin_  # 计算每个块的圆环数量
    xzrc = np.empty([num_, 4])
    j = 0  # 循环计数器
    for c1 in name_c[begin_:over_]:
        x_c1 = x[c == c1]
        z_c1 = z[c == c1]  # 提取单环的全部点云二维坐标
        data1 = np.vstack([x_c1, z_c1]).T
        ransac_fit, ransac_data = ransac(data1, model, 500, 2000, 0.2, 1000, debug=False, return_all=True)  # ransac迭代
        x1 = ransac_fit.a * -0.5
        z1 = ransac_fit.b * -0.5
        r1 = 0.5 * math.sqrt(ransac_fit.a ** 2 + ransac_fit.b ** 2 - 4 * ransac_fit.c)
        xzrc[j, 0] = x1
        xzrc[j, 1] = z1
        xzrc[j, 2] = r1
        xzrc[j, 3] = c1
        j += 1
    # print('每块最后一个值为', name_c[begin_:over_])
    print('已完成第', begin_, '至第', over_, '的圆环')
    return xzrc


# 建立断点
def cut_down(num, Piece=mp.cpu_count()):
    tik = []
    if num <= Piece:
        tik.append(0)
        print('点云数量过少，不能分块')
    else:
        n_pool = math.ceil(num / Piece)  # 每个池处理的最大点云数量
        print('每个block的tik位置为', n_pool)
        for i in range(0, Piece):
            tik.append(i * n_pool)
    tik.append(num)
    return tik  # 输出每个断点位置


# 点云分割算法(非并行)
def cut_points(xzrc, xyzi, dr=0.15):
    # 准备工作
    inxyzc = []  # 新建符合条件的点云容器
    outxyzc = []  # 新建不符合条件的点云容器
    line_y = []  # 圆心的y的容器
    # 提取圆环当中符合条件的点云
    for i in xzrc[:, -1]:
        xyz_c = xyzi[xyzi[:, -1] == i, :]  # 提取当前的圆环点云
        line_y.append(np.mean(xyzi[xyzi[:, -1] == i, 1]))  # 提取圆心y坐标
        r_c = xzrc[xzrc[:, -1] == i, 2]  # 提取当前圆环半径
        for j in range(len(xyz_c)):
            R2 = (xzrc[xzrc[:, -1] == i, 0] - xyz_c[j, 0]) ** 2 + (xzrc[xzrc[:, -1] == i, 1] - xyz_c[j, 2]) ** 2
            d_r = np.abs(R2 - r_c ** 2)
            if d_r < dr:
                inxyzc.append(xyz_c[j, :])
            else:
                outxyzc.append(xyz_c[j, :])
    xyzc_in = np.array(inxyzc)  # 添加点云分割后的点云
    line_y = np.array(line_y)  # 顺便返回点云圆心y坐标
    xyzc_out = np.array(outxyzc)  # 添加点云分割后不符合的点云
    return xyzc_in, line_y, xyzc_out


def get_CenterDis(xzrc, xyzi, cpu_count=mp.cpu_count()):
    '求圆心距（并行）'
    n_points = len(xyzi)  # 点云数量
    dis = np.empty([n_points])  # 总距离容器
    tik = cut_down(n_points, cpu_count)  # 分块起始点
    # 并行计算准备
    pool = mp.Pool(processes=cpu_count)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(cut_points_block_new, args=(xzrc, xyzi, tik[i], tik[i + 1])) for i in
                 range(cpu_count)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        dis[tik[tik_]:tik[tik_ + 1]] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    return dis


def cut_point_ep(points, xzlwp, cpu_count=mp.cpu_count(), dr=0.15):
    '点云分割椭圆算法'
    n_points = len(points)  # 点云数量
    dis = np.empty([n_points])  # 总距离容器
    tik = cut_down(n_points, cpu_count)  # 分块起始点
    # 并行计算准备
    pool = mp.Pool(processes=cpu_count)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(cut_points_block_ep, args=(points, xzlwp, tik[i], tik[i + 1])) for i in
                 range(cpu_count)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        dis[tik[tik_]:tik[tik_ + 1]] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    dis_mean = np.mean(dis)
    dis_std = np.std(dis)
    print('总距离均值', dis_mean, '总距离方差', dis_std)
    xyz_in = points[dis <= dr, :]
    xyz_out = points[dis > dr, :]
    return xyz_in, xyz_out


def cut_points_block_ep(points, xzlwp, a, b):
    '点云分割椭圆分块算法'
    dis = np.empty([b - a])  # 点到圆心的距离
    c = np.unique(points[:, -1])
    for i in range(b - a):
        xyz_ = points[a + i, :3]  # 当前点的空间位置
        c_i = points[a + i, -1]  # 当前点圆环名  ，-1
        xzlwp_ = xzlwp[c == c_i, :]  # 圆心位置
        xzlwp_ = xzlwp_.flatten()
        # xzlwp_ = np.reshape(xzlwp_, [-1])
        # arg_Ellipse = np.array([center[0], center[1], np.max([width, height]), np.min([width, height]), phi])  # 合并椭圆参数
        dis_ = ep.dis_ellipse(points[a + i, 0], points[a + i, 2], xzlwp_)  # 求点到椭圆的距离
        # dis_0 = dis_0.reshape(-1, 1)  # 便于查看
        dis[i] = np.nan_to_num(dis_, nan=1)  # 将空值转换为1
    return dis


# 点云分割算法优化
def cut_point(xzrc, xyzi, dr=0.15, cpu_count=mp.cpu_count(), xyz_c=None, d_r=None):
    n_points = len(xyzi)  # 点云数量
    dis = np.empty([n_points])  # 总距离容器
    tik = cut_down(n_points, cpu_count)  # 分块起始点
    # 并行计算准备
    pool = mp.Pool(processes=cpu_count)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(cut_points_block_new, args=(xzrc, xyzi[:, :5], tik[i], tik[i + 1])) for i in
                 range(cpu_count)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        dis[tik[tik_]:tik[tik_ + 1]] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    # 统计距离均值和标准差
    dis_mean = np.mean(dis)
    dis_std = np.std(dis)
    print('总距离均值', dis_mean, '总距离方差', dis_std)
    # 根据距离进行点云分割+为纵缝提取做基础
    if xyz_c is None and d_r is None:
        xyz_in = xyzi[dis <= dr, :]
        xyz_out = xyzi[dis > dr, :]
    elif xyz_c is not None and d_r is None:
        xyz_in = xyz_c[dis <= dr, :]
        xyz_out = xyz_c[dis > dr, :]
    elif xyz_c is None and d_r is not None:
        item = np.where(np.logical_and(dis > d_r, dis < dr))
        xyz_in = xyzi[item, :]
        xyz_in = xyz_in[0, :, :]
        xyz_out = None
    # RMSE
    dis = dis[dis <= dr]
    RMSE = np.sqrt(np.sum(dis ** 2) / len(dis))
    print('RMSE:', RMSE)
    return xyz_in, xyz_out


# 点云分割算法优化(分块）
def cut_points_block_new(xzrc, xyzi, a, b):
    dis = np.empty([b - a])  # 点到圆心的距离
    c = xzrc[:, -1]
    for i in range(b - a):
        xyz_ = xyzi[a + i, :3]  # 当前点的空间位置
        c_i = xyzi[a + i, -1]  # 当前点圆环名  ，默认为-1列
        xzr_ = xzrc[c == c_i, :3]  # 圆心位置
        R2 = (xyz_[0] - xzr_[0, 0]) ** 2 + (xyz_[2] - xzr_[0, 1]) ** 2  # 点到圆心的距离
        dis[i] = np.abs(R2 - xzr_[0, -1] ** 2)
        # dis[i] = R2 - xzr_[0, -1] ** 2
    return dis


# 拟合直线算法
def fit_line_3d(xyz):
    line = pyrsc.Line()  # 创建直线拟合的类
    # A:直线的斜率，B：直线的截距，inliers：内点索引，
    # thresh：内点的距离阈值
    # maxIteration：RANSAC算法的拟合次数
    A, B, inliers = line.fit(xyz, thresh=0.05, maxIteration=500)
    line_xyz = np.vstack([A, B]).T  # 将直线设置为3行2列的表示方法
    return line_xyz


# 环缝提取算法
def discern_CS(xyzic):
    i = xyzic[:, -2]
    i_new = p1.normalization(i, 255)  # 强度值离散化，强度值直方图均衡化
    # 强度值累加
    c = np.unique(xyzic[:, -1])  # 返回圆环名的容器
    i_allC = np.zeros(len(c))  # 存储总强度值容器
    tik_c = 0
    for i in c:
        i_c = i_new[xyzic[:, -1] == i]  # 提取当前圆环xyzi值
        i_allC[tik_c] = np.sum(i_c)  # 单环强度值求和
        tik_c += 1  # 计数器递增
    # 缩放
    V = 255  # 缩放差
    i_allC_new = p1.normalization(i_allC, V)  # 和再次进行直方图均衡化
    R = 0.30  # 惩罚参数
    N = 40  # 左右迭代参与环数
    T = V * R  # 惩罚值
    # 判断环缝位置
    c_in = []  # 环缝容器
    # xyz_c_in = []
    for i in range(N, len(c) - N):
        Gl = np.sum(i_allC_new[(i - (N + 1)):i]) / (N - 2)  # 当前圆环左侧强度值均值
        Gr = np.sum(i_allC_new[(i + 1):(i + N)]) / (N - 2)  # 当前圆环右侧强度值均值
        Dl = Gl - i_allC_new[i]
        Dr = Gr - i_allC_new[i]  # 求左右差值
        if Dl >= T and Dr >= T:  # 如果超过惩罚阈值
            c_in.append(i)
            # xyz_c_in.append(xyzic[xyzic[:, -1] == c[i], :3])
    print('环缝圆环名为', c[c_in])
    # 折线图先显示一下
    # line1 = go.Scatter(x=c, y=i_allC_new, name='强度值趋势')  # 建立折线
    # line2 = go.Scatter(x=c, y=i_allC_new)  # 建立折线
    # fig = go.Figure([line1, line2])  # 建立窗口
    # fig.show()  # 显示
    '输出'
    xyz = xyzic[np.isin(xyzic[:, -1], c[c_in]), :3]  # 返回xyzic[:, -1]中有c[c_in]的行数
    '显示'
    line = go.Scatter(x=c, y=i_allC_new, name='强度值和趋势')
    # fig = go.Figure(line)
    # fig.show()
    print('离散化前最高强度值和为', np.max(i_allC), '离散化前最低强度值和为', np.min(i_allC))
    return xyz, c[c_in], line


# 基于半径的环缝提取算法
def radius_CS(rc):
    num = len(rc)
    c_new, index = np.unique(rc[:, 1], return_index=True)
    r_new = rc[index, 0]  # 按照顺序变化的半径趋势
    r_255 = p1.normalization(r_new, 255)  # 离散化后的半径比值
    line = go.Scatter(x=c_new, y=r_255, name='拟合半径趋势')
    # fig = go.Figure(line)
    # fig.show()
    mean_r_255 = np.mean(r_255)  # 半径的平均比值
    N = 40  # 左右迭代参与环数
    T = 12  # 惩罚值
    c_in = []  # 环缝容器
    for i in range(N, num - N):
        if r_255[i] > mean_r_255:
            Gl = np.sum(r_255[(i - (N + 1)):i]) / (N - 2)  # 当前圆环左侧半径值均值
            Gr = np.sum(r_255[(i + 1):(i + N)]) / (N - 2)  # 当前圆环右侧半径值均值
            Dl = r_255[i] - Gl
            Dr = r_255[i] - Gr  # 求左右差值
            if Dl >= T and Dr >= T:  # 如果超过惩罚阈值
                c_in.append(i)
    print('环缝圆环名为(基于半径)', rc[c_in, 1])
    print('离散化前最大半径为', np.max(r_new), '离散化前最小半径为', np.min(r_new))
    return rc[c_in, 1], line


# 基于每个圆环的平均强度值提取环缝
def meanIc_CS(xyzic, xzrc, num_cpu=mp.cpu_count()):
    # 圆环分割
    xzc_p = np.c_[xyzic[:, 0], xyzic[:, 2], xyzic[:, -1]]  # 圆柱面数据
    xzc_c = np.c_[xzrc[:, 0], xzrc[:, 1], xzrc[:, -1]]  # 圆心数据
    belong = Split_ring_mp(xzc_p, xzc_c, n=10000)  # 将圆平均分成多少份(并行计算)
    print('已完成切蛋糕')
    # 摘除非圆柱上的点
    belong_order = np.unique(belong)  # 从小到大升序排序
    num_belong = len(belong_order)  # 点云有多少份
    points_3500 = xyzic[belong < -3500, :]
    points_1500 = xyzic[belong > -1500, :]  # 摘除非圆柱上的点
    points_new = np.vstack([points_3500, points_1500])  # 圆柱上新的点
    points_new[:, -2] = p1.normalization(points_new[:, -2], 255)  # 强度值离散化
    # xyzic[:, -2] = p1.normalization(xyzic[:, -2], 255)  # 强度值离散化
    # 计算每个环的平均强度值
    i_c = np.empty(len(xzrc))
    # 并行计算准备
    num_C = len(xzrc)
    c_un = np.unique(xzrc[:, -1])  # 圆环从小到大排列
    tik = cut_down(num_C, num_cpu)  # 分块起止点
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(find_cImean_block, args=(points_new, c_un, tik[i], tik[i + 1])) for i in  # points_new
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        i_c[tik[tik_]:tik[tik_ + 1]] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    # 后期整理
    i_c = p1.normalization(i_c, 255)  # 均值离散化
    i_c_mean = np.mean(i_c)  # 平均强度值均值
    i_c_std = np.std(i_c)  # 平均强度值方差
    print('均值', i_c_mean, '标准差', i_c_std)
    # 基于强度值求环缝
    c_ = []  # 环缝的存储名
    c_id = []  # 环缝的存储下标器
    N = 60  # 左右环的收集平均值的间隔
    dis_max = i_c_mean - i_c_std * 3  # 最大差值
    j = N  # 环名下标计数器
    for i in range(N, len(i_c) - N):
        if i_c[i] <= i_c_mean - i_c_std * 2 and i_c[i] <= i_c[i - 1] and i_c[i] <= i_c[i + 1]:
            Il = np.mean(i_c[(i - N):i])
            Ir = np.mean(i_c[(i + 1):(i + 1 + N)])  # 求左右50邻域平均强度值
            if Il - i_c[i] >= dis_max and Ir - i_c[i] >= dis_max:
                c_.append(i)
                c_id.append(j)
        j += 1
    # c_name = c_un[np.array(c_)]  # 粗估环缝名
    # 精简环缝值
    c_nei = 70  # 确定左右邻域搜索数
    id_c_ry = np.empty(len(c_id))
    k = 0
    for i in c_id:
        c_i_70 = i_c[(i - c_nei):(i + c_nei + 1)]  # 找到环缝的左右60邻域
        id_i_120 = np.argmin(c_i_70)  # 找到邻域的最小值
        id_c_ry[k] = id_i_120 + i - c_nei  # 找到局部最小值的准确下标
        k += 1
    c_reduce = np.unique(id_c_ry)  # 精简环缝下标
    c_reduce = c_reduce.astype(int)
    print('环缝id', c_reduce)
    c_name_reduce = c_un[c_reduce]  # 细挑后的环缝名
    print('环缝名', c_name_reduce)
    # c_name_reduce = c_name_reduce.astype(int)
    return c_name_reduce, c_reduce


# 分块求每个环的平均强度值
def find_cImean_block(points_new, c_un, tik0, tik1):
    i_c_ = np.empty(tik1 - tik0)
    j = 0
    for i in c_un[tik0:tik1]:
        i_c_[j] = np.mean(points_new[points_new[:, -1] == i, -2])
        j += 1
    return i_c_


# 圆环y轴提取算法
def find_cy(y, c):
    c_name = np.unique(c)  # 返回圆环名
    y_c = np.empty(len(c_name))  # 存储圆心容器
    n = 0
    for i in c_name:
        y_c[n] = np.mean(y[c == i])  # 求当前圆环的平均y值
        n += 1
    return y_c


# 求圆环表面积（输入y值和半径）
def find_LateralArea(y, r):
    long = np.max(y) - np.min(y)  # 求侧面积长
    r_mean = np.mean(r)  # 求底半径
    width = r_mean * 3.14159 * 2  # 求侧面积宽
    LateralArea = long * width  # 求侧面积
    return LateralArea


# 精简环缝名
def reduce_c(c):
    c_ = np.unique(c)  # 将圆环名从小到大排列并删除重复圆环名
    c_new = [c_[0]]  # 初始圆环
    for i in range(len(c) - 1):
        if c_[i + 1] - c_[i] > 10:
            c_new.append(c_[i + 1])  # 留下精简后的圆环
    c_new = np.array(c_new)
    print(c_new)
    return c_new


# 求体素体元边长
def find_Larea(y, r, v=2.0):
    area = find_LateralArea(y, r)  # 求侧面积
    density = p1.num_area(len(y), area)  # 求密度
    pixel_ = 1 / np.sqrt(density) * v  # 求体素网格边长
    print('默认的体素边长为', pixel_)
    return pixel_


# 计算每个点到圆心的距离(非并行计算)
def dis_tcac(xyzic, xzrc):
    num = len(xyzic)
    dis_all = np.empty(num, dtype=np.float16)
    for i in range(num):
        xi = xyzic[i, 0]
        zi = xyzic[i, 2]
        index = xzrc[:, -1] == xyzic[i, -1]
        ri = xzrc[index, -2]
        length = np.sqrt((xi - xzrc[index, 0]) ** 2 + (zi - xzrc[index, 1]) ** 2)
        # dis_all[i] = length - ri  # 求点云距离面上的距离
        dis_all[i] = length  # 求点云距离圆心的距离
    # dis_all = p1.normalization(dis_all, 255)
    return dis_all


# 将圆环分成n份(点云以及圆环)
def Split_ring(xzc_point, xzc_c, n=10160):
    # 求每个点的反正切
    num = len(xzc_point)  # 点云数量
    d_y = np.empty([num])  # 存储点云y截距容器
    d_x = np.empty([num])  # 存储点云x截距容器
    belong = np.empty([num])  # 存储点云标签值容器
    j = 0  # 迭代计数器
    for i in xzc_point:
        ci = i[-1]
        xz_c = xzc_c[xzc_c[:, -1] == ci, :2]
        # k[j] = (i[1] - xz_c[0, 1])/(i[0] - xz_c[0, 0])
        d_y[j] = (i[1] - xz_c[0, 1])
        d_x[j] = (i[0] - xz_c[0, 0])
        # print(k[j])
        j += 1
    # angle = np.arctan(k)  # 求反正切值
    angle = np.arctan2(d_y, d_x)  # 求反正切值
    # 将角度分解
    max_a = np.pi * 2  # 最大角度
    single_a = max_a / n  # 求每个块的过渡
    for j in range(num):
        belong[j] = np.floor(angle[j] / single_a)  # 点云归属标签赋值
        # print(belong[j])
    return belong


# 将圆环分成n份(点云以及圆环)并行计算
def Split_ring_mp(xzc_point, xzc_c, n=10160, num_cpu=mp.cpu_count()):
    # 求每个点的反正切
    num = len(xzc_point)  # 点云数量
    d_yx = np.empty([num, 2])  # 存储点云截距的容器
    belong = np.empty([num])  # 存储点云标签值容器
    # 并行计算准备
    tik = cut_down(num, num_cpu)  # 分块起止点
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(find_arctan_block, args=(xzc_point, xzc_c, tik[i], tik[i + 1])) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        d_yx[tik[tik_]:tik[tik_ + 1], :] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    angle = np.arctan2(d_yx[:, 0], d_yx[:, 1])  # 求反正切值
    # 将角度分解
    single_a = np.pi * 2 / n  # 求每个块的过渡
    # 并行计算点云归属块
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    multi_res = [pool.apply_async(find_belong_block, args=(angle, single_a, tik[i], tik[i + 1])) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        belong[tik[tik_]:tik[tik_ + 1]] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    return belong


# 并行计算每个点的反正切
def find_arctan_block(xzc_point, xzc_c, tik0, tik1):
    d_yx_ = np.empty([tik1 - tik0, 2])
    j = 0  # 计数器
    for i in range(tik0, tik1):
        ci = xzc_point[i, -1]
        xz_c = xzc_c[xzc_c[:, -1] == ci, :2]
        d_yx_[j, 0] = (xzc_point[i, 1] - xz_c[0, 1])
        d_yx_[j, 1] = (xzc_point[i, 0] - xz_c[0, 0])
        j += 1
    print('已完成', tik0, '-', tik1, '的反正切')
    return d_yx_


# 计算每个点云属于哪个环块
def find_belong_block(angle, single_a, tik0, tik1):
    belong_ = np.empty(tik1 - tik0)
    j = 0  # 计数器
    for i in range(tik0, tik1):
        belong_[j] = np.floor(angle[i] / single_a)  # 点云归属标签赋值
        j += 1
    print('已完成', tik0, '-', tik1, '的环块归属')
    return belong_


# 纵缝提取(输入点云和环缝名及拟合参数)
def find_LJ_old(xyzic, c_in_, xzrc):
    '旧函数，已停止使用'
    xyz_LJ = []  # 存储纵缝容器
    num_c_in_ = len(c_in_)  # 环缝数量
    for i in range(num_c_in_ - 1):
        # 求出当前圆环的数据
        print('正在处理第', i + 1, '个环块')
        ci_xyz0 = xyzic[c_in_[i] <= xyzic[:, -1], :]
        ci_xyzi = ci_xyz0[ci_xyz0[:, -1] <= c_in_[i + 1], :]  # 当前两环缝之间的所有点云
        del ci_xyz0
        dis_all = dis_tcac(ci_xyzi, xzrc)  # 求当前环所有点云的距离
        '体素化'
        r = np.mean(xzrc[:, 2])
        # pixel_ = find_Larea(ci_xyzi[:, 1], r, v=1)  # 求体素体元边长
        pixel_ = find_Larea(ci_xyzi[:, 1], r, v=1.5)  # 求体素体元边长
        ci_voxel = vl.voxel(ci_xyzi[:, :3], pixel=pixel_)  # 建立体素
        del pixel_
        voxel_i = ci_voxel.P2V(ci_xyzi[:, 3])  # 连接点云强度值
        voxel_dis = ci_voxel.P2V(dis_all)  # 连接点云与圆心距离
        '种子点搜索'
        # 符合条件的点云
        ci_xyzi_begin_index = [ci_xyzi[:, -1] == (c_in_[i] + 5)]  # 符合种子点的开启条件
        ci_xyzi_begin = ci_xyzi[ci_xyzi_begin_index, :]  # 种子点云
        num_begin = len(ci_xyzi_begin_index)  # 种子点云数量
        '纵缝搜索迭代'
        # 种子体元入栈
        for i in range(num_begin):
            if voxel_i[ci_xyzi_begin[i, 0], ci_xyzi_begin[i, 1], ci_xyzi_begin[i, 1]] < 1:  # 未完成：范围暂时未确定
                pass
        # 滤波
        '''
        '将点云进行分块'
        xzc_p = np.c_[ci_xyzi[:, 0], ci_xyzi[:, 1], ci_xyzi[:, -1]]  # 点云需要的数据
        xzc_c = np.c_[xzrc[:, 0], xzrc[:, 1], xzrc[:, -1]]  # 圆环需要的数据
        belong = Split_ring(xzc_p, xzc_c, n)  # 将当前点云环分成n份
        belong_n = np.unique(belong)  # 从小到大升序排序
        num_belong = len(belong_n)  # 点云有多少份
        dis_all = dis_tcac(ci_xyzi, xzrc)  # 求当前环所有点云的距离
        
        # 计算每份点云的平均距离
        dis_c = np.empty([num_belong])
        k = 0
        for j in belong_n:
            dis_c[k] = np.mean(dis_all[belong == j])  # 平均距离赋值
            k += 1
        '判断是否为纵缝'
        i_all = []  # 记录所有符合条件的块
        n = 5  # 邻域参数
        dis_d = 0.02  # 差值阈值
        for l in range(n, num_belong - n):
            if dis_c[l] >= dis_c[l - 1] and dis_c[l] >= dis_c[l + 1] and dis_c[l] >= (np.mean(xzrc[:, 2]) + np.std(xzrc[:, 2]) * 10) and dis_c[l] - dis_c[l - n] >= 0.008 and dis_c[l] - dis_c[l + n] >= 0.008:
                # 求左右邻域平均值  存在求值问题
                Cl = np.mean(dis_c[(l - n * 4):l])
                Cr = np.mean(dis_c[(l + 1):(l + 1 + n * 4)])
                if dis_c[l] - Cl >= dis_d and dis_c[l] - Cr >= dis_d:
                    i_all.append(l)
        i_all = np.array(i_all)  # 数组化
        if len(i_all) > 0:
            print(len(i_all))
            index_p = belong_n[i_all]  # 符合条件的归属  IndexError: arrays used as indices must be of integer (or boolean) type
            index_pp = np.isin(belong, index_p)  # 符合归属的点下标
            xyz_LJ = np.append(xyz_LJ, ci_xyzi[index_pp, :])
        '''

    return xyz_LJ


def popStack(fit_i_id_un):  # 输入种子点体素位置
    first = fit_i_id_un[0, :]  # 提出来第一个
    other = fit_i_id_un[1:, :]  # 把剩下的留下
    return first, other


def bl_ABCD2(xyz, c, ρθ, n=0.02):
    '用曲率判断断面点云属于哪类模板'
    '合并数据'
    xyzcρθ = np.c_[xyz, c, ρθ]  # 合并数据
    F1 = None  # 提取第一个缝的容器
    '分别寻找A,B,C,D环块默认第一纵缝角度区间点云'
    # A环块
    F_a1 = xyzcρθ[xyzcρθ[:, -1] >= 2965, :]
    F_a1 = F_a1[F_a1[:, -1] <= 2975, :]
    # B环块
    F_b1 = xyzcρθ[xyzcρθ[:, -1] >= 3390, :]
    F_b1 = F_b1[F_b1[:, -1] <= 3400, :]
    # C环块
    F_c1 = xyzcρθ[xyzcρθ[:, -1] >= 2535, :]
    F_c1 = F_c1[F_c1[:, -1] <= 2545, :]
    # D环块
    F_d1 = xyzcρθ[xyzcρθ[:, -1] >= 3810, :]
    F_d1 = F_d1[F_d1[:, -1] <= 3820, :]
    '掐头去尾'
    F_a1 = F_a1[F_a1[:, 1] <= (np.max(F_a1[:, 1]) - n), :]
    F_a1 = F_a1[F_a1[:, 1] >= (np.min(F_a1[:, 1]) + n), :]
    F_b1 = F_b1[F_b1[:, 1] <= (np.max(F_b1[:, 1]) - n), :]
    F_b1 = F_b1[F_b1[:, 1] >= (np.min(F_b1[:, 1]) + n), :]
    F_c1 = F_c1[F_c1[:, 1] <= (np.max(F_c1[:, 1]) - n), :]
    F_c1 = F_c1[F_c1[:, 1] >= (np.min(F_c1[:, 1]) + n), :]
    F_d1 = F_d1[F_d1[:, -2] <= (np.max(F_d1[:, 1]) - n), :]
    F_d1 = F_d1[F_d1[:, 1] >= (np.min(F_d1[:, 1]) + n), :]
    'A B C D环块体元数量'
    num_F_a1 = len(F_a1)
    num_F_b1 = len(F_b1)
    num_F_c1 = len(F_c1)
    num_F_d1 = len(F_d1)
    '分别计算ABCD环块符合曲率阈值的体元数量'
    m = 0.05  # 曲率阈值
    # 计算数量
    num_a1 = len(F_a1[F_a1[:, 3] > m, :])
    num_b1 = len(F_b1[F_b1[:, 3] > m, :])
    num_c1 = len(F_c1[F_c1[:, 3] > m, :])
    num_d1 = len(F_d1[F_d1[:, 3] > m, :])  # 曲率大于等于0.05的体素数量
    p = np.array([num_a1 / num_F_a1, num_b1 / num_F_b1, num_c1 / num_F_c1, num_d1 / num_F_d1])  # 计算比率
    print(p)  # 打印比率值
    '判断环块模板'
    belong_ = np.argmax(p)  # 环块归属
    # 只需要下标，不需要返回别的
    belong_ = np.argmax(p)  # 环块归属
    if belong_ == 0:
        print('此环块判断属于A模板')
        F1 = F_a1[:, :3]
    elif belong_ == 1:
        print('此环块判断属于B模板')
        F1 = F_b1[:, :3]
    elif belong_ == 2:
        print('此环块判断属于C模板')
        F1 = F_c1[:, :3]
    elif belong_ == 3:
        print('此环块判断属于D模板')
        F1 = F_d1[:, :3]
    return belong_, F1


def bl_ABCD(voxel_Y_points_local, i, n=5):
    '通过曲率值判断环块属于哪类模板'
    '输入曲率赋值后的体素'
    poyi_ = np.c_[voxel_Y_points_local, i]
    # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\poyi'+str()+, poyi_)
    F1 = None
    '分别寻找A,B,C环块默认第一纵缝区间点云'
    '''
    # A环块
    F_a1 = poyi_[poyi_[:, 1] >= 2090, :]
    F_a1 = F_a1[F_a1[:, 1] <= 2105, :]
    # B环块
    F_b1 = poyi_[poyi_[:, 1] >= 2390, :]
    F_b1 = F_b1[F_b1[:, 1] <= 2405, :]
    # C环块
    F_c1 = poyi_[poyi_[:, 1] >= 1790, :]
    F_c1 = F_c1[F_c1[:, 1] <= 1805, :]
    # D环块
    F_d1 = poyi_[poyi_[:, 1] >= 2690, :]
    F_d1 = F_d1[F_d1[:, 1] <= 2705, :]
    '''
    # A环块
    F_a1 = poyi_[poyi_[:, 1] >= 2965, :]
    F_a1 = F_a1[F_a1[:, 1] <= 2975, :]
    # B环块
    F_b1 = poyi_[poyi_[:, 1] >= 3390, :]
    F_b1 = F_b1[F_b1[:, 1] <= 3400, :]
    # C环块
    F_c1 = poyi_[poyi_[:, 1] >= 2535, :]
    F_c1 = F_c1[F_c1[:, 1] <= 2545, :]
    # D环块
    F_d1 = poyi_[poyi_[:, 1] >= 3810, :]
    F_d1 = F_d1[F_d1[:, 1] <= 3820, :]
    '通过Y轴坐标掐头去尾'
    F_a1 = F_a1[F_a1[:, -2] <= (np.max(F_a1[:, -2]) - n), :]
    F_a1 = F_a1[F_a1[:, -2] >= (np.min(F_a1[:, -2]) + n), :]
    F_b1 = F_b1[F_b1[:, -2] <= (np.max(F_b1[:, -2]) - n), :]
    F_b1 = F_b1[F_b1[:, -2] >= (np.min(F_b1[:, -2]) + n), :]
    F_c1 = F_c1[F_c1[:, -2] <= (np.max(F_c1[:, -2]) - n), :]
    F_c1 = F_c1[F_c1[:, -2] >= (np.min(F_c1[:, -2]) + n), :]
    F_d1 = F_d1[F_d1[:, -2] <= (np.max(F_d1[:, -2]) - n), :]
    F_d1 = F_d1[F_d1[:, -2] >= (np.min(F_d1[:, -2]) + n), :]
    'A B C环块体元数量'
    num_F_a1 = len(F_a1)
    num_F_b1 = len(F_b1)
    num_F_c1 = len(F_c1)
    num_F_d1 = len(F_d1)
    '分别计算ABC环块符合曲率阈值的体元数量'
    m = 0.05  # 曲率阈值 0.04
    num_a1 = len(F_a1[F_a1[:, -1] > m, :])
    num_b1 = len(F_b1[F_b1[:, -1] > m, :])
    num_c1 = len(F_c1[F_c1[:, -1] > m, :])
    num_d1 = len(F_d1[F_d1[:, -1] > m, :])  # 曲率大于等于0.05的体素数量
    p = np.array([num_a1 / num_F_a1, num_b1 / num_F_b1, num_c1 / num_F_c1, num_d1 / num_F_d1])  # 计算比值
    print(p)
    '判断环块模板'
    belong_ = np.argmax(p)  # 环块归属
    if belong_ == 0:
        print('此环块判断属于A模板')
        F1 = F_a1[:, :3]
    elif belong_ == 1:
        print('此环块判断属于B模板')
        F1 = F_b1[:, :3]
    elif belong_ == 2:
        print('此环块判断属于C模板')
        F1 = F_c1[:, :3]
    elif belong_ == 3:
        print('此环块判断属于D模板')
        F1 = F_d1[:, :3]
    # return belong_, F1, poyi_
    return belong_, F1


def find_LJ_FV(xyzicc, RB_S, RB_E, xzrcy):
    '2022.11.5非体素化极坐标纵缝提取版'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    type_RingBlock = np.empty(num_c)  # 环块所属模板类型容器
    θg = np.array([[[415, 425], [3700, 3765], [3880, 3945]], [[1690, 1700], [2425, 2490], [2605, 2670]], [[1685, 1695], [3270, 3335], [3450, 3515]], [[410, 420], [2845, 2905], [3025, 3090]]])
    # 开始循环
    F_all = {}  # 存储缝的词典
    for i in range(num_c):
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzicc_ = xyzicc[xyzicc[:, -2] < RB_E[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -2] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\ShieldRing-'+str(i)+'.txt',xyzicc_,fmt='%.05f')
        '计算当前中轴线（非体素化使用）'
        xzrcy_ = xzrcy[xzrcy[:, -2] < RB_E[i + 1], :]
        xzrcy_ = xzrcy_[xzrcy_[:, -2] > RB_S[i + 1], :]  # 当前环块的空间圆心点
        # 拟合中轴线
        line_ = np.vstack([xzrcy_[:, 0], xzrcy_[:, -1], xzrcy_[:, 1]]).T  # 圆心位置容器
        KB_line_ = fit_line_3d(line_)  # ransac空间直线拟合
        # 找到拟合后的新圆心
        n = 0  # 计数器
        xyzc_new = np.empty([len(xzrcy_[:, -1]), 4])
        xyzc_new[:, 1] = xzrcy_[:, -1]
        xyzc_new[:, -1] = xzrcy_[:, -2]
        for j in xzrcy_[:, -1]:
            k_ = (j - KB_line_[1, 1]) / KB_line_[1, 0]  # 求k
            x_ = k_ * KB_line_[0, 0] + KB_line_[0, 1]  # 求X
            z_ = k_ * KB_line_[2, 0] + KB_line_[2, 1]  # 求Z
            xyzc_new[n, 0] = x_
            xyzc_new[n, 2] = z_
            n += 1
        ρθ_ = p1.XYZ2ρθ_mp(xyzicc_[:, :-1], xyzc_new, n=4250)  # 合并圆柱位置和点云属性
        # 判断环块类型
        type_RingBlock[i], Fa_ = bl_ABCD2(xyzicc_[:, :3], xyzicc_[:, -1], ρθ_)  # 返回F1缝点云以及环块所属
        F_all[str(i) + 'a'] = Fa_  # 顺便提取A缝
        '寻找其他纵缝'
        xyzcρθ_ = np.c_[xyzicc_[:, :3], xyzicc_[:, -1], ρθ_]
        Fb_, Fc_, Fd_ = findF_others2(xyzcρθ_, θg[int(type_RingBlock[i])])
        F_all[str(i) + 'b'] = Fb_[:, :3]
        F_all[str(i) + 'c'] = Fc_[:, :3]
        F_all[str(i) + 'd'] = Fd_[:, :3]
    return type_RingBlock, F_all


def find_LJ_un(xyzicc, RB_S, RB_E, xzrcy):
    '2022.11.22纵缝提取普适性版'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    F_all = {}  # 存储缝的词典
    m = 0.004 * 6  # 斜缝搜索提取最高圆心距的差值 4
    θ23_all = np.arange(2425, 3946)  # 所有斜缝经历的角度区间
    k = 0.024  # 斜缝的斜率的绝对值（y/角度值）
    for i in range(num_c):  # 按完整环块进行循环
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzicc_ = xyzicc[xyzicc[:, -2] < RB_E[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -2] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        np.set_printoptions(precision=5)  # 设置小数位置为5位
        '计算当前中轴线（非体素化使用）'
        xzrcy_ = xzrcy[xzrcy[:, -2] < RB_E[i + 1], :]
        xzrcy_ = xzrcy_[xzrcy_[:, -2] > RB_S[i + 1], :]  # 当前环块的空间圆心点
        # 拟合中轴线
        line_ = np.vstack([xzrcy_[:, 0], xzrcy_[:, -1], xzrcy_[:, 1]]).T  # 圆心位置容器
        KB_line_ = fit_line_3d(line_)  # ransac空间直线拟合
        # 找到拟合后的新圆心
        n = 0  # 计数器
        xyzc_new = np.empty([len(xzrcy_[:, -1]), 4])
        xyzc_new[:, 1] = xzrcy_[:, -1]
        xyzc_new[:, -1] = xzrcy_[:, -2]
        for j in xzrcy_[:, -1]:
            k_ = (j - KB_line_[1, 1]) / KB_line_[1, 0]  # 求k
            x_ = k_ * KB_line_[0, 0] + KB_line_[0, 1]  # 求X
            z_ = k_ * KB_line_[2, 0] + KB_line_[2, 1]  # 求Z
            xyzc_new[n, 0] = x_
            xyzc_new[n, 2] = z_
            n += 1
        ρθ_ = p1.XYZ2ρθ_mp(xyzicc_[:, :-1], xyzc_new, n=4250)  # 合并圆柱位置和点云属性
        # θ_un_ = np.unique(ρθ_[:, 1])  # 对角度值进行精简
        xyziccρθ_ = np.c_[xyzicc_, ρθ_]  # 点云属性集中
        del xyzicc_, ρθ_
        # output_ = xyziccρθ_[xyziccρθ_[:, -2] >= (np.max(xyziccρθ_[:, -2])-m), :6]
        # output_ = output_[output_[:, -1] >= 0.05, :]
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\p-m'+str(i)+'.txt', output_)
        # 寻找纵缝（一）
        '计算每个角度区间大于0.05的概率'
        θ_tik_ = np.arange(2960, 3820 + 1, 10)  # 将角度每10分一个
        p_θ_ = np.zeros(len(θ_tik_) - 1)  # 存储
        ρ_mean_θ_ = np.zeros(len(θ_tik_) - 1)  # 长度
        for l in range(len(θ_tik_) - 1):
            l_ = xyziccρθ_[xyziccρθ_[:, -1] <= θ_tik_[l + 1], :]
            l_ = l_[l_[:, -1] >= θ_tik_[l], :]  # 提取当前角度区间点云
            l_c_ = l_[l_[:, -3] > 0.05, :]  # 提取大于0.05的曲率
            if len(l_) > 0:
                p_θ_[l] = len(l_c_) / len(l_)  # 符合曲率条件的比值
                ρ_mean_θ_[l] = np.mean(l_c_[:, -2])
        index_02 = np.argwhere(p_θ_ >= 0.15)  # 首先遍历比值大于0.2的区间段
        print(index_02)
        index_max_ρ = np.argmax(ρ_mean_θ_[index_02])  # 找到最大圆心距的下标
        print(index_max_ρ)
        # 输出正确的纵缝点云
        index_Ture = index_02[index_max_ρ]
        print(index_Ture)
        l_ = xyziccρθ_[xyziccρθ_[:, -1] <= θ_tik_[index_Ture + 1], :]
        l_ = l_[l_[:, -1] >= θ_tik_[index_Ture], :]  # 提取当前角度区间点云
        LJ_1_ = l_[l_[:, -3] >= 0.05, :5]  # 提取大于0.05的曲率
        F_all[str(i) + 'a'] = LJ_1_  # 将缝存储到容器中
        # 寻找纵缝（二、三）
        '准备工作'
        y_min_ = np.min(xyziccρθ_[:, 1])
        y_max_ = np.max(xyziccρθ_[:, 1])  # 求y的开始和结束
        LJ_2_, LJ_3_, p_lr = find_LJ23(xyziccρθ_, y_max_, y_min_, θ23_all, m, k)  # 通过并行计算求出两条斜缝
        F_all[str(i) + 'b'] = LJ_2_
        F_all[str(i) + 'c'] = LJ_3_
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\p_all_lr.txt', p_lr)

        # 寻找第四个缝

        '''
        '限制纵缝搜索区间'
        θ_tik_ = np.arange(2960, 3820+1, 10)
        p_θ_ = np.zeros(len(θ_tik_)-1)  # 长度
        for l in range(len(θ_tik_)-1):
            l_ = xyziccρθ_[xyziccρθ_[:, -1] <= θ_tik_[l+1], :]
            l_ = l_[l_[:, -1] >= θ_tik_[l], :]  # 提取当前角度区间点云
            l_c_ = l_[l_[:, -3] > 0.05, :]  # 提取大于0.05的曲率
            if len(l_) > 0:
                p_θ_[l] = len(l_c_)/len(l_)  # 符合曲率条件的比值
        # 画一条折线图
        # line = go.Scatter(x=np.arange(len(θ_tik_)-1), y=p_θ_)
        # fig = go.Figure(line)
        # fig.show()
        id_max_ = np.argmax(p_θ_)
        # 输出峰值
        l_ = xyziccρθ_[xyziccρθ_[:, -1] <= θ_tik_[id_max_ + 1], :]
        l_ = l_[l_[:, -1] >= θ_tik_[id_max_], :]  # 提取当前角度区间点云
        LJ_1_ = l_[l_[:, -3] > 0.05, :5]  # 提取大于0.05的曲率
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\LJ1_'+str(i)+'.txt', LJ_1_)
        '''
    return F_all


def find_LJ_Curve(xyzic, RB_S, RB_E, xzrcy, n=3000):
    '2023.6.26纵缝提取深度剩余法'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    F_all = {}  # 存储缝的词典
    xyzc = np.c_[xzrcy[:, 0], xzrcy[:, -1], xzrcy[:, 1], xzrcy[:, -2]]  # 每个圆环的
    r_mean = np.mean(xzrcy[:, -3])  # 平均半径
    cl = r_mean * 2 * np.pi  # 周长
    pixel = cl / n  # 体素平面半径
    k = -6  # 斜缝搜索斜率
    # 开始循环盾构环
    for i in range(num_c):
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzic_ = xyzic[xyzic[:, -1] < RB_E[i + 1], :]
        xyzic_ = xyzic_[xyzic_[:, -1] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\ShieldRing-' + str(i) + '.txt', xyzic_, fmt='%.05f')
        # ρθ_ = XYZ2ρθ_mp(xyzic_, xyzc, n, xzrcy[:, -3])  # 合并圆柱位置和点云属性  # 加一个求半径的
        ρθy_ = XYZ2ρθy_mp(xyzic_, xyzc, n, xzrcy[:, -3])  # 合并圆柱位置和点云属性
        xyzicρθy_ = np.c_[xyzic_, ρθy_]  # 点云属性集中
        # 进一步剔除其他非必要点
        xyzicρθy_ = xyzicρθy_[xyzicρθy_[:, -3] >= (np.max(xyzicρθy_[:, -3]) - 2.5 * pixel), :]
        xyzicρθy_ = xyzicρθy_[xyzicρθy_[:, 1] >= (np.min(xyzicρθy_[:, 1]) + 10 * pixel), :]
        xyzicρθy_ = xyzicρθy_[xyzicρθy_[:, 1] <= (np.max(xyzicρθy_[:, 1]) - 10 * pixel), :]
        xyzicρθy_[:,-1] -= np.min(xyzicρθy_[:, -1])
        # np.savetxt('E:\\MyCodeWorkPlace\\Python\\Shield tunnel Deformation monitoring\\xyzicρθy_.txt',xyzicρθy_,fmt='%.05f')
        # ρθY = vl.XYZ2ρθY(xyzicρθ_[:, :5], xzrcy[:, :-1], n=n)  # 点极坐标  （后期需要和以上算法进行合并）
        PV = vl.voxel_ρθY(xyzicρθy_[:,-3:], pixel=[pixel, 1, pixel])  # 建立体素
        PV.P2V(xyzicρθy_[:, 3])  # 有值体素赋值
        # PV.watch_voxel(r_mean,r_mean,n)
        '在给定框架内，找到所有起点的角度值，然后给一个斜率模板直线'
        θ_all = np.unique(PV.points_local_un[:, 1])  # 第一个环的有值体素弧度值容器
        dis_all_rl = np.zeros([θ_all.shape[0], 2])  # 存储距离容器
        j = 0  # 计数器
        for l in θ_all:
            b_r = 248 - k * l  # 确定截距值
            b_l = k * l  # 确定截距值
            dis_r = p1.get_distance_point2line(PV.points_local_un[:, 1:], [k, b_r])  # 体素在这条线上所有的平面距离
            dis_l = p1.get_distance_point2line(PV.points_local_un[:, 1:], [-k, b_l])  # 体素在这条线上所有的平面距离
            dis_all_rl[j, 0] = PV.points_local_un[dis_r <= 3, 1:].shape[0]  # 距离不超过3的体素数量
            dis_all_rl[j, 1] = PV.points_local_un[dis_l <= 3, 1:].shape[0]  # 距离不超过3的体素数量
            j += 1  # 刷新计数器
        index_r = np.argmax(dis_all_rl[:, 0])
        index_l = np.argmax(dis_all_rl[:, 1])  # 最大值位置
        b_r = 248 - k * θ_all[index_r]  # 确定截距值
        b_l = k * θ_all[index_l]  # 右侧截距值
        dis_r = p1.get_distance_point2line(PV.points_local_un[:, 1:], [k, b_r])  # 体素在这条线上所有的平面距离
        dis_l = p1.get_distance_point2line(PV.points_local_un[:, 1:], [-k, b_l])  # 体素在这条线上所有的平面距离
        out_r = PV.points_local_un[dis_r <= 3, :]
        out_l = PV.points_local_un[dis_l <= 3, :]  # 输出体素
        F_all[str(i) + 'c'] = xyzicρθy_[PV.V2P(out_r), :5]
        F_all[str(i) + 'd'] = xyzicρθy_[PV.V2P(out_l), :5]
    return F_all


def find_LJ_MAX(xyzicc, RB_S, RB_E, xzrcy, n=4250):
    '2023.4.10纵缝提取普适性版'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    F_all = {}  # 存储缝的词典
    xyzc = np.c_[xzrcy[:, 0], xzrcy[:, -1], xzrcy[:, 1], xzrcy[:, -2]]
    r = xzrcy[:, -3]  # 半径
    θ1 = np.arange(int(2425 / 4250 * n), int(3946 / 4250 * n + 1))  # 纵缝1区间点云
    θ2 = np.arange(int(410 / 4250 * n), int(1700 / 4250 * n + 1))  # 纵缝2区间点云 410-1700
    k = 0.024 * n / 4250  # 默认斜率
    for i in range(num_c):
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzicc_ = xyzicc[xyzicc[:, -2] < RB_E[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -2] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        ρθ_ = XYZ2ρθ_mp(xyzicc_[:, :-1], xyzc, n, r)  # 合并圆柱位置和点云属性  # 加一个求半径的
        xyziccρθ_ = np.c_[xyzicc_, ρθ_]  # 点云属性集中
        # xyziccρθ_ρ_ = xyziccρθ_[xyziccρθ_[:, -2] >= 0, :]  # 在隧道上的点云
        del xyzicc_, ρθ_
        # 寻找纵缝1
        num_θ1all = np.zeros(len(θ1))
        pp = np.zeros(len(θ2))
        for j in range(len(θ1)):
            xyziccρθ_p_o = xyziccρθ_[xyziccρθ_[:, -1] == θ1[j], :]
            p_o_c = xyziccρθ_p_o[xyziccρθ_p_o[:, -3] >= 0.05, :]
            num_θ1all[j] = len(p_o_c)
            # pp[j] = len(p_o_c)/len(xyziccρθ_p_o)
            # num_θ1all[j] =len(xyziccρθ_p_o)
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\23.4.20\\o-p_0a.txt', pp, fmt='%.05f')
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\曲率概率.txt', pp)
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\最大圆心距.txt', max_p)
        id1_max = np.argmax(num_θ1all)
        out_a = xyziccρθ_[xyziccρθ_[:, -1] == θ1[id1_max], :6]
        than_005 = out_a[out_a[:, -1] >= 0.05]
        max_x = np.max(than_005[:, 0])
        min_x = np.min(than_005[:, 0])
        index = np.where(np.logical_and(out_a[:, 0] >= min_x, out_a[:, 0] <= max_x))
        out_a = out_a[index, :]  # 剩余点云
        F_all[str(i) + 'a'] = out_a[0, :, :]
        print('a的缝的位置为', min_x, '-', max_x)
        # 寻找纵缝2
        num_θ2all = np.zeros(len(θ2))
        for j in range(len(θ2)):
            xyziccρθ_p_o = xyziccρθ_[xyziccρθ_[:, -1] == θ2[j], :]
            p_o_c = xyziccρθ_p_o[xyziccρθ_p_o[:, -3] >= 0.05, :]
            num_θ2all[j] = len(p_o_c)
            if len(xyziccρθ_p_o) > 0:
                pp[j] = len(p_o_c) / len(xyziccρθ_p_o)
        np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\23.4.20\\to-p_0b.txt', np.c_[θ2, pp], fmt='%.05f')
        id2_max = np.argmax(num_θ2all)
        out_b = xyziccρθ_[xyziccρθ_[:, -1] == θ2[id2_max], :6]
        than_005 = out_b[out_b[:, -1] >= 0.05]
        max_x = np.max(than_005[:, 0])
        min_x = np.min(than_005[:, 0])
        index = np.where(np.logical_and(out_b[:, 0] >= min_x, out_b[:, 0] <= max_x))
        out_b = out_b[index, :]  # 剩余点云
        F_all[str(i) + 'b'] = out_b[0, :, :]
        print('b的缝的位置为', min_x, '-', max_x)
        # 寻找纵缝34  点的旋转算法
        θ3 = np.arange(int(2425 / 4250 * n), int(3765 / 4250 * n + 1))  # 纵缝3区间点云

    return F_all


def θy2θY(θy, k):
    '二维点集坐标转为新二维点集坐标'
    dy = np.max(θy[:, 1]) - np.min(θy[:, 1])
    а = np.arctan2(k)  # 角度值
    sink = np.sin(а)  # 正弦值
    y_new = dy / sink  # 输出新的长度

    return


def find_LJ_MAX_c(xyzic, RB_S, RB_E, xzrcy, n=4250):
    '计算每个Piece中点到圆心的距离，然后求距离数组的方差'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    F_all = {}  # 存储缝的词典
    xyzc = np.c_[xzrcy[:, 0], xzrcy[:, -1], xzrcy[:, 1], xzrcy[:, -2]]
    r = xzrcy[:, -3]  # 半径
    θ1 = np.arange(int(2425 / 4250 * n), int(3946 / 4250 * n + 1))  # 纵缝1区间点云
    θ2 = np.arange(int(410 / 4250 * n), int(1700 / 4250 * n + 1))  # 纵缝2区间点云 410-1700
    for i in range(num_c):
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzicc_ = xyzic[xyzic[:, -1] < RB_E[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -1] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        ρθ_ = XYZ2ρθ_mp(xyzicc_, xyzc, n, r)  # 合并圆柱位置和点云属性  # 加一个求半径的
        xyziccρθ_ = np.c_[xyzicc_, ρθ_]  # 点云属性集中
        # 寻找纵缝1
        num_θ1all = np.zeros(len(θ1))
        for j in range(len(θ1)):
            xyziccρθ_p_o = xyziccρθ_[xyziccρθ_[:, -1] == θ1[j], :]
            num_θ1all[j] = np.var(xyziccρθ_p_o[:, -2])

        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\23.4.20\\o-p_'+str(i)+'a.txt', np.c_[θ1, num_θ1all], fmt='%.05f')
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\曲率概率.txt', pp)
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\最大圆心距.txt', max_p)
        '''
        num_un = np.unique(num_θ1all)
        print(np.mean(num_un))
        print(np.std(num_un))
        '''
        num_θ1all = np.nan_to_num(num_θ1all)
        index_ = num_θ1all >= 0.000083
        out_a = xyziccρθ_[np.isin(xyziccρθ_[:, -1], θ1[index_]), :6]
        '''
        than_005 = out_a[out_a[:, -1] >= 0.05]
        max_x = np.max(than_005[:, 0])
        min_x = np.min(than_005[:, 0])
        index = np.where(np.logical_and(out_a[:, 0] >= min_x, out_a[:, 0] <= max_x))
        out_a = out_a[index, :]  # 剩余点云
        '''
        F_all[str(i) + 'a'] = out_a
        # print('a的缝的位置为', min_x, '-', max_x)
        # 寻找纵缝2
        num_θ2all = np.zeros(len(θ2))
        for j in range(len(θ2)):
            xyziccρθ_p_o = xyziccρθ_[xyziccρθ_[:, -1] == θ2[j], :]
            num_θ2all[j] = np.var(xyziccρθ_p_o[:, -2])
        # np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\23.4.20\\o-p_' + str(i) + 'b.txt', np.c_[θ2, num_θ2all], fmt='%.05f')
        num_θ2all = np.nan_to_num(num_θ2all)
        index_ = num_θ2all >= 0.000083
        out_b = xyziccρθ_[np.isin(xyziccρθ_[:, -1], θ2[index_]), :6]
        '''
        than_005 = out_b[out_b[:, -1] >= 0.05]
        max_x = np.max(than_005[:, 0])
        min_x = np.min(than_005[:, 0])
        index = np.where(np.logical_and(out_b[:, 0] >= min_x, out_b[:, 0] <= max_x))
        out_b = out_b[index, :]  # 剩余点云
        '''
        F_all[str(i) + 'b'] = out_b
    return F_all


def XYZ2ρθ_mp(xyzic, xyzc, n, r, num_cpu=mp.cpu_count()):
    '点云笛卡尔转极坐标（p修改版，并行版）'
    num = len(xyzic)  # 点云数量
    tik = cut_down(num)  # 并行计算分块
    ρθ = np.empty([num, 2])  # 新建一个存储点云ρθ的容器
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    # 开始进行并行计算
    multi_res = [pool.apply_async(get_ρθ_block, args=(xyzic, xyzc, tik[i], tik[i + 1], n, r)) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        ρθ[tik[tik_]:tik[tik_ + 1], :] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    # 归零化
    min_ρθ = ρθ.min(0)  # 求最小值
    # ρθ[:, 0] -= min_ρθ[0]
    ρθ[:, 1] -= min_ρθ[1]
    return ρθ

def XYZ2ρθy_mp(xyzic, xyzc, n, r, num_cpu=mp.cpu_count()):
    '点云笛卡尔转极坐标（p修改版，并行版）'
    num = len(xyzic)  # 点云数量
    tik = cut_down(num)  # 并行计算分块
    ρθy = np.empty([num, 3])  # 新建一个存储点云ρθ的容器
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    # 开始进行并行计算
    multi_res = [pool.apply_async(get_ρθ_block, args=(xyzic, xyzc, tik[i], tik[i + 1], n, r)) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        ρθy[tik[tik_]:tik[tik_ + 1], :-1] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    # 转移y坐标
    ρθy[:, 2] = xyzic[:,1]
    # 归零化
    min_ρθy = ρθy.min(0)  # 求最小值
    # ρθ[:, 0] -= min_ρθ[0]
    ρθy[:, 1] -= min_ρθy[1]
    ρθy[:, 2] -= min_ρθy[2]
    return ρθy

def get_ρθ_block(xyzic, xyzc, Ts, Te, n, r):
    '分块只求ρθ的函数'
    # 准备工作
    single_a = np.pi * 2 / n  # 求每个块的过渡
    ρθ_ = np.empty([Te - Ts, 2])
    j = 0  # 计数器
    for i in range(Ts, Te):
        # 求圆心距
        xyz_ = xyzic[i, :3]  # 当前点的空间位置
        c_i = xyzic[i, -1]  # 当前点圆环名
        c_xyz_ = xyzc[xyzc[:, -1] == c_i, :3]  # 圆心位置
        R2 = (xyz_[0] - c_xyz_[0, 0]) ** 2 + (xyz_[2] - c_xyz_[0, 2]) ** 2  # 点到圆心的距离
        r_ = r[xyzc[:, -1] == c_i]  # 半径大小
        ρθ_[j, 0] = np.sqrt(R2) - r_  # 点云到中轴线的距离
        # 求角度
        angle_ = np.arctan2((xyzic[i, 2] - c_xyz_[0, 2]), (xyzic[i, 0] - c_xyz_[0, 0]))  # 求反正切值
        ρθ_[j, 1] = np.floor(angle_ / single_a)  # 点云归属标签赋值
        j += 1  # 计数器更新
    print('极坐标属性添加已完成', Te / len(xyzic) * 100, '%')
    return ρθ_


def find_LJ23(xyziccρθ, y_max, y_min, θ23_all, m, k, num_cpu=mp.cpu_count()):
    '通过并行计算求出两条斜缝'
    # 准备工作
    num_θ23 = len(θ23_all)
    tik = cut_down(num_θ23, num_cpu)  # 总循环分块
    print('使用cpu的核为', len(tik))
    p_all_lr = np.empty([num_θ23, 2])  # 建立斜直线大于0.05的比例数组(左,右)
    # p_max_lr = np.empty([num_θ23, 2])  # 建立最大圆心距统计数组
    # p_mean_lr = np.empty([num_θ23, 2])  # 建立平均圆心距统计数组
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    # 开始进行并行计算
    multi_res = [pool.apply_async(find_LJ23_block, args=(xyziccρθ, y_max, y_min, k, tik[i], tik[i + 1], θ23_all)) for i in
                 range(num_cpu)]  # 将每个block需要处理的点云区间发送到每个进程当中
    tik_ = 0  # 计数器
    for res in multi_res:
        p_all_lr[tik[tik_]:tik[tik_ + 1], :] = res.get()
        tik_ += 1
    pool.close()  # 禁止进程池再接收任务
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算
    # 后续处理
    print('最大左比例', np.max(p_all_lr[:, 0]))
    print('最大右比例', np.max(p_all_lr[:, 1]))  # 检查
    np.savetxt('E:\\2023硕士研究上\\0：盾构隧道论文\\.txt', p_all_lr)
    '输出左缝'
    id_l = np.argmax(p_all_lr[:, 0])  # 找到最大的下标
    o_id_l_ = θ23_all[id_l]  # 角度值
    b_l_ = y_min - k * o_id_l_  # 求当前直线的b截距
    # 确认搜索的点云范围(o_min-o_max)
    o_max_l_ = (y_max - b_l_) / k
    ps_c_l_ = xyziccρθ[xyziccρθ[:, -1] >= o_id_l_, :]
    ps_c_l_ = ps_c_l_[ps_c_l_[:, -1] <= o_max_l_, :]
    oy_l_ = np.c_[ps_c_l_[:, -1], ps_c_l_[:, 1]]
    dis_l_ = p1.get_distance_point2line(oy_l_, [k, b_l_])
    LJ2_dis_ = ps_c_l_[dis_l_ <= 0.10, :]
    LJ2 = LJ2_dis_[LJ2_dis_[:, -2] >= (np.max(LJ2_dis_[:, -2]) - m), :5]
    '输出右缝'
    id_r = np.argmax(p_all_lr[:, 1])  # 找到最大的下标
    o_id_r_ = θ23_all[id_r]  # 角度值
    b_r_ = y_min - (-1 * k) * o_id_r_  # 求当前直线的b截距
    # 确认搜索的点云范围(c_min-c_max)
    o_max_r_ = (y_max - b_r_) / (-1 * k)
    ps_c_r_ = xyziccρθ[xyziccρθ[:, -1] <= o_id_r_, :]
    ps_c_r_ = ps_c_r_[ps_c_r_[:, -1] >= o_max_r_, :]
    oy_r_ = np.c_[ps_c_r_[:, -1], ps_c_r_[:, 1]]
    dis_r_ = p1.get_distance_point2line(oy_r_, [(-1 * k), b_r_])
    LJ3_dis_ = ps_c_r_[dis_r_ <= 0.10, :]
    LJ3 = LJ3_dis_[LJ3_dis_[:, -2] >= (np.max(LJ3_dis_[:, -2]) - m), :5]
    '''
    '显示折线图'
    line1 = go.Scatter(x=θ23_all, y=p_all_lr[:, 0], name='左缝比例变化趋势')
    line2 = go.Scatter(x=θ23_all, y=p_max_lr[:, 0], name='左缝最高圆心距变化趋势')
    line3 = go.Scatter(x=θ23_all, y=p_mean_lr[:, 0], name='左缝平均圆心距变化趋势')
    line4 = go.Scatter(x=θ23_all, y=p_all_lr[:, 1], name='右缝比例变化趋势')
    line5 = go.Scatter(x=θ23_all, y=p_max_lr[:, 1], name='右缝最高圆心距变化趋势')
    line6 = go.Scatter(x=θ23_all, y=p_mean_lr[:, 1], name='右缝平均圆心距变化趋势')
    # 显示
    fig = go.Figure([line1, line2, line3, line4, line5, line6])
    fig.show()
    '''
    return LJ2, LJ3, p_all_lr


def find_LJ23_block(xyziccρθ, y_max, y_min, k, tik0, tik1, θ23_all):
    '求斜缝的分块函数'
    # 准备工作
    p_block_lr = np.empty([tik1 - tik0, 2])  # 分块存储容器
    # p_max_lr = np.empty([tik1-tik0, 2])  # 建立最大圆心距统计数组
    # p_mean_lr = np.empty([tik1-tik0, 2])  # 建立平均圆心距统计数组
    # p_all_lr = np.empty([tik1-tik0, 6])
    j = 0  # 计数器
    for i in θ23_all[tik0:tik1]:  # 单块循环
        '计算左斜缝'
        b_l_ = y_min - k * i  # 求当前直线的b截距
        # 确认搜索的点云范围(o_min-o_max)
        o_max_l_ = (y_max - b_l_) / k  # 求搜索区间最大角度值
        ps_l_c_ = xyziccρθ[xyziccρθ[:, -1] >= i, :]
        ps_l_c_ = ps_l_c_[ps_l_c_[:, -1] <= o_max_l_, :]  # 满足角度范围的点云
        oy_l_ = np.c_[ps_l_c_[:, -1], ps_l_c_[:, 1]]  # 每个点云的角度值和y值
        dis_l_ = p1.get_distance_point2line(oy_l_, [k, b_l_])  # 求距离
        ps_dis_l_ = ps_l_c_[dis_l_ <= 0.10, :]  # 满足距离小于0.1的点云
        ps_dis_c_l_ = ps_dis_l_[ps_dis_l_[:, -3] >= 0.05, :]  # 曲率大于等于0.05的点云
        p_block_lr[j, 0] = len(ps_dis_c_l_) / len(ps_dis_l_)  # 比例
        # p_all_lr[j, 1] = np.nanmax(ps_dis_c_l_[:, -2])
        # p_all_lr[j, 2] = np.mean(ps_dis_c_l_[:, -2])
        '搜索右斜缝'
        b_r_ = y_min - (-1 * k) * i  # 求当前直线的b截距
        o_max_r_ = (y_max - b_r_) / (k * -1)  # 求搜索区间最大角度值
        ps_r_c_ = xyziccρθ[xyziccρθ[:, -1] >= o_max_r_, :]
        ps_r_c_ = ps_r_c_[ps_r_c_[:, -1] <= i, :]  # 满足角度范围的点云
        oy_r_ = np.c_[ps_r_c_[:, -1], ps_r_c_[:, 1]]  # 每个点云的角度值和y值
        dis_r_ = p1.get_distance_point2line(oy_r_, [k * -1, b_r_])  # 求距离
        ps_dis_r_ = ps_r_c_[dis_r_ <= 0.10, :]  # 满足距离小于0.1的点云
        ps_dis_c_r_ = ps_dis_r_[ps_dis_r_[:, -3] >= 0.05, :]  # 曲率大于等于0.05的点云
        p_block_lr[j, 1] = len(ps_dis_c_r_) / len(ps_dis_r_)  # 比例
        # p_all_lr[j, 4] = np.nanmax(ps_dis_c_r_[:, -2])
        # p_all_lr[j, 5] = np.mean(ps_dis_c_r_[:, -2])
        j += 1
    # print('已完成', θ23_all[tik0], '-', θ23_all[tik1], '角度值')
    return p_block_lr


def find_LJ_P(xyzicc, RB_S, RB_E, xzrc):
    '2022.10.3纵缝提取改进版'
    # 准备工作
    num_c = len(RB_S) - 2  # 参与计算的环块个数 = all-2
    type_RingBlock = np.empty(num_c)  # 环块所属模板类型容器
    θg = np.array([[[415, 425], [3700, 3765], [3880, 3945]], [[1690, 1700], [2425, 2490], [2605, 2670]], [[1685, 1695], [3270, 3335], [3450, 3515]], [[410, 420], [2845, 2905], [3025, 3090]]])
    # 开始循环
    F_all = {}  # 存储缝的词典
    # V_θY = {}  # 存储纵缝体素θY起始范围和分辨率
    P_F_all = []  # 非标签纵缝存储器
    for i in range(num_c):
        print('正在处理第', i + 1, '个环块')
        # 遍历到当前环块
        xyzicc_ = xyzicc[xyzicc[:, -2] < RB_E[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -2] > RB_S[i + 1], :]  # 循环到当前环块的所有点云
        '''
        '计算当前中轴线（非体素化使用）'
        xzrcy_ = xzrcy[xzrcy[:, -2] < RB_E[i + 1], :]
        xzrcy_ = xzrcy_[xzrcy_[:, -2] > RB_S[i + 1], :]  # 当前环块的空间圆心点
        # 拟合中轴线
        line_ = np.vstack([xzrcy_[:, 0], xzrcy_[:, -1], xzrcy_[:, 1]]).T  # 圆心位置容器
        KB_line_ = fit_line_3d(line_)  # ransac空间直线拟合
        # 找到拟合后的新圆心
        n = 0  # 计数器
        xyzc_new = np.empty([len(xzrcy_[:, -1]), 4])
        xyzc_new[:, 1] = xzrcy_[:, -1]
        xyzc_new[:, -1] = xzrcy_[:, -2]
        for j in xzrcy_[:, -1]:
            k_ = (j-KB_line_[1, 1])/KB_line_[1, 0]  # 求k
            x_ = k_ * KB_line_[0, 0] + KB_line_[0, 1]  # 求X
            z_ = k_ * KB_line_[2, 0] + KB_line_[2, 1]  # 求Z
            xyzc_new[n, 0] = x_
            xyzc_new[n, 2] = z_
            n += 1
        '体素化'
        # 体素化前的标准化处理
        '''
        ρθY_ = p1.XYZ2ρθY_mp(xyzicc_[:, :-1], xzrc, n=4250)  # 合并圆柱位置和点云属性  # 角度分解已发生改变
        # 极坐标体素化并赋值
        v_ρθY_ = vl.voxel_ρθY(ρθY_)
        voxel_Y_c = v_ρθY_.P2Vm(xyzicc_[:, -1])  # 体素赋值（体素赋曲率）
        # v_ρθY_.watchVoxel_plt()  # 显示体素
        # V_θY[str(i) + 'a'] = np.array([[,],[,]])
        # 输出
        # np.savetxt('E:\\PointCloudSourceFile\\txt\\Shield\\ρθY_'+str(i)+'.txt', v_ρθY_.points_local)
        # 将现有存储点云的大数组与角度数组进行合并
        xyziccθY_ = np.c_[xyzicc_, v_ρθY_.points_local[:, 1:]]
        # 判断环块类型
        type_RingBlock[i], v_F_a_ = bl_ABCD(v_ρθY_.points_local, xyzicc_[:, -1])  # 通过曲率判断
        # 单独处理F1
        p_Fa_id = v_ρθY_.V2P(v_F_a_)  # 返回符合条件A1的点云下标
        p_Fa_b = xyziccθY_[p_Fa_id, :]  # 返回体素对应的点云
        p_Fa_ = p_Fa_b[p_Fa_b[:, 5] > 0.05, :]
        F_all[str(i) + 'a'] = p_Fa_
        '''
        
        if len(P_F_all) == 0:
            P_F_all = p_Fa_
        else:
            P_F_all = np.vstack((P_F_all, p_Fa_))
        
        '''
        del p_Fa_, p_Fa_b
        # 寻找其他纵缝
        v_F_b_, v_F_c_, v_F_d_ = findF_others(voxel_Y_c, v_ρθY_.points_local_un, θg[int(type_RingBlock[i])])
        # 提取B纵缝
        p_Fb_id = v_ρθY_.V2P(v_F_b_)  # 返回符合条件A的点云下标
        p_b_ = xyziccθY_[p_Fb_id, :]
        F_all[str(i) + 'b'] = p_b_[p_b_[:, 5] > 0.05, :]
        # P_F_all = np.vstack((P_F_all, p_b_[p_b_[:, 5] > 0.05, :]))
        del v_F_b_, p_b_
        # 提取C纵缝
        p_Fc_id = v_ρθY_.V2P(v_F_c_)  # 返回符合条件A的点云下标
        p_c_ = xyziccθY_[p_Fc_id, :]
        F_all[str(i) + 'c'] = p_c_
        # P_F_all = np.vstack((P_F_all, p_c_))
        del v_F_c_, p_c_
        # 提取D纵缝
        p_Fd_id = v_ρθY_.V2P(v_F_d_)  # 返回符合条件A的点云下标
        p_d_ = xyziccθY_[p_Fd_id, :]
        F_all[str(i) + 'd'] = p_d_
        # P_F_all = np.vstack((P_F_all, p_d_))
        del v_F_d_, p_d_

    return type_RingBlock, F_all


def find_LJ(xyzicc, C_name, xzrc):
    '2022.6.27 纵缝提取函数(基于曲率)'
    # 准备工作
    num_c_id = len(C_name)  # 环缝数量
    type_RingBlock = np.empty(num_c_id - 1)  # 环块所属模板类型容器
    P_a = []
    P_b = []
    P_c = []
    P_F1 = []  # 存储各环块缝的容器
    # 开始循环
    for i in range(num_c_id - 1):
        print('正在处理第', i + 1, '个环块')
        xyzicc_ = xyzicc[xyzicc[:, -2] < C_name[i + 1], :]
        xyzicc_ = xyzicc_[xyzicc_[:, -2] > C_name[i], :]  # 循环到当前环块的所有点云
        # 体素化前的标准化处理
        ρθY_ = p1.XYZ2ρθY(xyzicc_[:, :-1], xzrc)  # 合并圆柱位置和点云属性
        # 极坐标体素化并赋值
        v_ρθY_ = vl.voxel_ρθY(ρθY_)
        voxel_Y_c = v_ρθY_.P2V(xyzicc_[:, -1])  # 体素赋值（体素赋曲率）
        # 判断环块类型
        belong_, F1 = bl_ABCD(v_ρθY_.points_local, xyzicc_[:, -1])  # 通过曲率判断
        # 单独处理F1
        p_F1_id = v_ρθY_.V2P(F1)  # 返回符合条件A1的点云下标
        p_F1_b = xyzicc_[p_F1_id, :]
        p_F1_ = p_F1_b[p_F1_b[:, -1] > 0.04, :]
        if len(P_F1) == 0:
            P_F1 = p_F1_
        else:
            P_F1 = np.append(P_F1, p_F1_, axis=0)  # 将所有F1缝点云全部保存
        if belong_ == 0:
            F_a = findF_a(voxel_Y_c, v_ρθY_.points_local_un)  # 返回A区域纵缝
            p_Fa_id = v_ρθY_.V2P(F_a)  # 返回符合条件A的点云下标
            p_a_ = xyzicc_[p_Fa_id, :]
            if len(P_a) == 0:
                P_a = p_a_
            else:
                P_a = np.append(P_a, p_a_, axis=0)  # 将所有A缝点云全部保存
        elif belong_ == 1:
            F_b = findF_b(voxel_Y_c, v_ρθY_.points_local_un)  # 返回B区域纵缝1
            p_Fb_id = v_ρθY_.V2P(F_b)  # 返回符合条件A1的点云下标
            p_b_ = xyzicc_[p_Fb_id, :]
            if len(P_b) == 0:
                P_b = p_b_
            else:
                P_b = np.append(P_b, p_b_, axis=0)  # 将所有B1点云全部保存
        elif belong_ == 2:
            F_c = findF_c(voxel_Y_c, v_ρθY_.points_local_un)  # 返回B区域纵缝1
            p_Fc_id = v_ρθY_.V2P(F_c)  # 返回符合条件A1的点云下标
            p_c_ = xyzicc_[p_Fc_id, :]
            if len(P_c) == 0:
                P_c = p_c_
            else:
                P_c = np.append(P_c, p_c_, axis=0)  # 将所有B1点云全部保存
        elif belong_ == 3:  # 如果环块所属模板为D
            pass
        type_RingBlock[i] = belong_  # 模板类型传递

    if len(np.unique(type_RingBlock)) == 3:
        P_F = np.vstack((P_F1, P_a, P_b, P_c))  # 合并所有缝点云
    else:
        P_F = np.vstack((P_F1, P_a, P_b))  # 合并所有缝点云
    return type_RingBlock, P_F


def findF_a(v_c, v_p_un):
    '寻找模板A的纵缝点云'
    # 输入（强度值体素，缝A1体元位置）
    # 未 高斯混合模型（2类）（离心距，强度值）
    # 未 采用26邻域搜索符合强度值的点云
    ag = np.array([[2605, 2660], [2735, 2785], [285, 305]])  # A类234纵缝所在角度区间
    m = 5  # 高度差阈值
    v_Fa2_id = []  # 容器
    v_Fa3_id = []
    # A4
    v_Fa4_before = v_p_un[v_p_un[:, 1] < ag[2, 1], :]
    v_Fa4_before = v_Fa4_before[v_Fa4_before[:, 1] > ag[2, 0], :]  # 符合A4角度的体素
    v_Fa4_b_Y = v_Fa4_before[v_Fa4_before[:, -1] <= (np.max(v_Fa4_before[:, -1]) - 15), :]
    v_Fa4_b_Y = v_Fa4_b_Y[v_Fa4_b_Y[:, -1] >= (np.min(v_Fa4_before[:, -1]) + 15), :]  # 掐头去尾
    P_max_A4 = np.max(v_Fa4_b_Y[:, 0])  # 求非缝最高P
    del v_Fa4_b_Y
    v_Fa4 = v_Fa4_before[v_Fa4_before[:, 0] > (P_max_A4 - m), :]
    del v_Fa4_before
    # A2
    v_Fa2_before = v_p_un[v_p_un[:, 1] <= ag[0, 1], :]
    v_Fa2_before = v_Fa2_before[v_Fa2_before[:, 1] >= ag[0, 0], :]  # 符合A2角度的体素
    v_Fa2_b_Y = v_Fa2_before[v_Fa2_before[:, -1] <= (np.max(v_Fa2_before[:, -1]) - 15), :]
    v_Fa2_b_Y = v_Fa2_b_Y[v_Fa2_b_Y[:, -1] >= (np.min(v_Fa2_before[:, -1]) + 15), :]  # 掐头去尾
    del v_Fa2_before
    # 大于曲率0.05的体元
    for i in range(len(v_Fa2_b_Y)):
        if v_c[int(v_Fa2_b_Y[i, 0]), int(v_Fa2_b_Y[i, 1]), int(v_Fa2_b_Y[i, 2])] > 0.05:
            v_Fa2_id.append(i)
    v_Fa2 = v_Fa2_b_Y[v_Fa2_id, :]
    v_Fa2 = Reduced_diagonal_seam(v_Fa2, -1, v_Fa2_b_Y)  # 去噪
    del v_Fa2_b_Y
    '''
    P_max_a2 = np.max(v_Fa2_b_Y[:, 0])  # 求非缝最高P
    del v_Fa2_before
    v_Fa2 = v_Fa2_b_Y[v_Fa2_b_Y[:, 0] >= (P_max_a2-4), :]
    v_Fa2 = Reduced_diagonal_seam(v_Fa2, -1)
    '''
    # A3
    v_Fa3_before = v_p_un[v_p_un[:, 1] <= ag[1, 1], :]
    v_Fa3_before = v_Fa3_before[v_Fa3_before[:, 1] >= ag[1, 0], :]  # 符合A2角度的体素
    v_Fa3_b_Y = v_Fa3_before[v_Fa3_before[:, -1] <= (np.max(v_Fa3_before[:, -1]) - 15), :]
    v_Fa3_b_Y = v_Fa3_b_Y[v_Fa3_b_Y[:, -1] >= (np.min(v_Fa3_before[:, -1]) + 15), :]  # 掐头去尾
    del v_Fa3_before
    for i in range(len(v_Fa3_b_Y)):
        if v_c[int(v_Fa3_b_Y[i, 0]), int(v_Fa3_b_Y[i, 1]), int(v_Fa3_b_Y[i, 2])] > 0.05:
            v_Fa3_id.append(i)
    v_Fa3 = v_Fa3_b_Y[v_Fa3_id, :]
    v_Fa3 = Reduced_diagonal_seam(v_Fa3, 1, v_Fa3_b_Y)
    del v_Fa3_b_Y
    '''
    P_max_a3 = np.max(v_Fa3_b_Y[:, 0])  # 求非缝最高P
    del v_Fa3_before
    v_Fa3 = v_Fa3_b_Y[v_Fa3_b_Y[:, 0] >= (P_max_a3-4), :]
    v_Fa3 = Reduced_diagonal_seam(v_Fa3, 1)
    '''
    # 合并所有缝
    v_Fa = np.vstack((v_Fa4, v_Fa2, v_Fa3))
    return v_Fa


def findF_b(v_c, v_p_un):
    '寻找模板B的纵缝点云'
    bg = np.array([[1193, 1200], [1710, 1755], [1838, 1885]])  # B类234纵缝所在角度区间
    m = 5  # 高度差阈值
    v_Fb3_id = []
    v_Fb4_id = []
    # B1
    # B2
    v_Fb2_before = v_p_un[v_p_un[:, 1] <= bg[0, 1], :]
    v_Fb2_before = v_Fb2_before[v_Fb2_before[:, 1] >= bg[0, 0], :]  # 符合b2角度的体素
    v_Fb2_b_Y = v_Fb2_before[v_Fb2_before[:, -1] <= (np.max(v_Fb2_before[:, -1]) - 15), :]
    v_Fb2_b_Y = v_Fb2_b_Y[v_Fb2_b_Y[:, -1] >= (np.min(v_Fb2_before[:, -1]) + 15), :]
    P_max = np.max(v_Fb2_b_Y[:, 0])  # 求非缝最高P
    v_Fb2 = v_Fb2_before[v_Fb2_before[:, 0] > (P_max - m), :]
    del v_Fb2_before
    # B3
    v_Fb3_before = v_p_un[v_p_un[:, 1] <= bg[1, 1], :]
    v_Fb3_before = v_Fb3_before[v_Fb3_before[:, 1] >= bg[1, 0], :]  # 符合A2角度的体素
    v_Fb3_b_Y = v_Fb3_before[v_Fb3_before[:, -1] <= (np.max(v_Fb3_before[:, -1]) - 15), :]
    v_Fb3_b_Y = v_Fb3_b_Y[v_Fb3_b_Y[:, -1] >= (np.min(v_Fb3_before[:, -1]) + 15), :]
    del v_Fb3_before
    # P_max_B3 = np.max(v_Fb3_b_Y[:, 0])  # 求非缝最高P
    # del v_Fb3_before
    # v_Fb3 = v_Fb3_b_Y[v_Fb3_b_Y[:, 0] >= (P_max_B3-4), :]
    for i in range(len(v_Fb3_b_Y)):
        if v_c[int(v_Fb3_b_Y[i, 0]), int(v_Fb3_b_Y[i, 1]), int(v_Fb3_b_Y[i, 2])] >= 0.05:
            v_Fb3_id.append(i)
    v_Fb3 = v_Fb3_b_Y[v_Fb3_id, :]
    del v_Fb3_b_Y
    v_Fb3 = Reduced_diagonal_seam(v_Fb3)
    # B4
    v_Fb4_before = v_p_un[v_p_un[:, 1] <= bg[2, 1], :]
    v_Fb4_before = v_Fb4_before[v_Fb4_before[:, 1] >= bg[2, 0], :]  # 符合A2角度的体素
    v_Fb4_b_Y = v_Fb4_before[v_Fb4_before[:, -1] <= (np.max(v_Fb4_before[:, -1]) - 15), :]
    v_Fb4_b_Y = v_Fb4_b_Y[v_Fb4_b_Y[:, -1] >= (np.min(v_Fb4_before[:, -1]) + 15), :]
    del v_Fb4_before
    # P_max_B4 = np.max(v_Fb4_b_Y[:, 0])  # 求非缝最高P
    # del v_Fb4_before
    # v_Fb4 = v_Fb4_b_Y[v_Fb4_b_Y[:, 0] >= (P_max_B4-4), :]
    for i in range(len(v_Fb4_b_Y)):
        if v_c[int(v_Fb4_b_Y[i, 0]), int(v_Fb4_b_Y[i, 1]), int(v_Fb4_b_Y[i, 2])] >= 0.05:
            v_Fb4_id.append(i)
    v_Fb4 = v_Fb4_b_Y[v_Fb4_id, :]
    del v_Fb4_b_Y
    v_Fb4 = Reduced_diagonal_seam(v_Fb4)
    # 合并所有缝
    v_Fb = np.vstack((v_Fb2, v_Fb3, v_Fb4))  # 合并所有缝
    return v_Fb


def findF_c(v_c, v_p_un):
    '寻找模板C的纵缝点云'
    cg = np.array([[1195, 1200], [2310, 2357], [2438, 2484]])  # C类234纵缝所在角度区间
    v_Fc3_id = []
    v_Fc4_id = []
    # C1
    # C2
    v_Fc2_before = v_p_un[v_p_un[:, 1] <= cg[0, 1], :]
    v_Fc2_before = v_Fc2_before[v_Fc2_before[:, 1] >= cg[0, 0], :]  # 符合b2角度的体素
    v_Fc2_b_Y = v_Fc2_before[v_Fc2_before[:, -1] <= (np.max(v_Fc2_before[:, -1]) - 15), :]
    v_Fc2_b_Y = v_Fc2_b_Y[v_Fc2_b_Y[:, -1] >= (np.min(v_Fc2_before[:, -1]) + 15), :]
    P_max = np.max(v_Fc2_b_Y[:, 0])  # 求非缝最高P
    v_Fc2 = v_Fc2_before[v_Fc2_before[:, 0] > (P_max - 5), :]
    del v_Fc2_before
    # C3
    v_Fc3_before = v_p_un[v_p_un[:, 1] <= cg[1, 1], :]
    v_Fc3_before = v_Fc3_before[v_Fc3_before[:, 1] >= cg[1, 0], :]  # 符合A2角度的体素
    v_Fc3_b_Y = v_Fc3_before[v_Fc3_before[:, -1] <= (np.max(v_Fc3_before[:, -1]) - 15), :]
    v_Fc3_b_Y = v_Fc3_b_Y[v_Fc3_b_Y[:, -1] >= (np.min(v_Fc3_before[:, -1]) + 15), :]
    del v_Fc3_before
    # P_max_c3 = np.max(v_Fc3_b_Y[:, 0])  # 求非缝最高P
    # del v_Fc3_before
    # v_Fc3 = v_Fc3_b_Y[v_Fc3_b_Y[:, 0] >= (P_max_c3-4), :]
    for i in range(len(v_Fc3_b_Y)):
        if v_c[int(v_Fc3_b_Y[i, 0]), int(v_Fc3_b_Y[i, 1]), int(v_Fc3_b_Y[i, 2])] > 0.05:
            v_Fc3_id.append(i)
    v_Fc3 = v_Fc3_b_Y[v_Fc3_id, :]
    v_Fc3 = Reduced_diagonal_seam(v_Fc3, -1, v_Fc3_b_Y)
    del v_Fc3_b_Y
    # C4
    v_Fc4_before = v_p_un[v_p_un[:, 1] <= cg[2, 1], :]
    v_Fc4_before = v_Fc4_before[v_Fc4_before[:, 1] >= cg[2, 0], :]  # 符合A2角度的体素
    v_Fc4_b_Y = v_Fc4_before[v_Fc4_before[:, -1] <= (np.max(v_Fc4_before[:, -1]) - 15), :]
    v_Fc4_b_Y = v_Fc4_b_Y[v_Fc4_b_Y[:, -1] >= (np.min(v_Fc4_before[:, -1]) + 15), :]
    del v_Fc4_before
    # P_max_c4 = np.max(v_Fc4_b_Y[:, 0])  # 求非缝最高P
    # del v_Fc4_before
    # v_Fc4 = v_Fc4_b_Y[v_Fc4_b_Y[:, 0] >= (P_max_c4-4), :]
    for i in range(len(v_Fc4_b_Y)):
        if v_c[int(v_Fc4_b_Y[i, 0]), int(v_Fc4_b_Y[i, 1]), int(v_Fc4_b_Y[i, 2])] > 0.05:
            v_Fc4_id.append(i)
    v_Fc4 = v_Fc4_b_Y[v_Fc4_id, :]
    v_Fc4 = Reduced_diagonal_seam(v_Fc4, 1, v_Fc4_b_Y)
    del v_Fc4_b_Y
    # 合并所有缝
    v_Fc = np.vstack((v_Fc2, v_Fc3, v_Fc4))  # 合并所有缝
    return v_Fc


def findF_others2(xyzcρθ, θg):
    '''
    寻找其他的纵缝点云(非体素版)
    :param xyzc: 点云坐标+曲率
    :param ρθ: 点云圆心距和角度值
    :return:
    '''
    m = 5 * 0.004  # 高度差阈值
    m_ = 4 * 0.004  # 斜缝高度差阈值
    c_ = 0.05  # 曲率阈值
    # xyzcρθ = np.c_[xyzc, ρθ]  # 合并数组
    'B缝'
    Fb_ = xyzcρθ[xyzcρθ[:, -1] <= θg[0, 1], :]
    Fb_ = Fb_[Fb_[:, -1] >= θg[0, 0], :]
    ρ_max = np.max(Fb_[:, -2])
    Fb_ = Fb_[Fb_[:, -2] >= (ρ_max - m), :]
    'C缝'
    Fc_ = xyzcρθ[xyzcρθ[:, -1] <= θg[1, 1], :]
    Fc_ = Fc_[Fc_[:, -1] >= θg[1, 0], :]
    ρ_max = np.max(Fc_[:, -2])
    Fc_ = Fc_[Fc_[:, -2] >= (ρ_max - m_), :]
    Fc_ = Fc_[Fc_[:, 3] >= c_, :]
    'D缝'
    Fd_ = xyzcρθ[xyzcρθ[:, -1] <= θg[2, 1], :]
    Fd_ = Fd_[Fd_[:, -1] >= θg[2, 0], :]
    ρ_max = np.max(Fd_[:, -2])
    Fd_ = Fd_[Fd_[:, -2] >= (ρ_max - m_), :]
    Fd_ = Fd_[Fd_[:, 3] >= c_, :]
    return Fb_, Fc_, Fd_


def findF_others(v_c, v_p_un, θg):
    '寻找其他的纵缝点云'
    m = 5  # 高度差阈值
    c_ = 0.05
    # b
    v_Fb_before = v_p_un[v_p_un[:, 1] <= θg[0, 1], :]
    v_Fb_before = v_Fb_before[v_Fb_before[:, 1] >= θg[0, 0], :]  # 符合b角度的体素
    P_max = np.max(v_Fb_before[:, 0])  # 求非缝最高P
    v_Fb_ = v_Fb_before[v_Fb_before[:, 0] > (P_max - m), :]
    del v_Fb_before
    # c
    v_Fc_before = v_p_un[v_p_un[:, 1] <= θg[1, 1], :]
    v_Fc_before = v_Fc_before[v_Fc_before[:, 1] >= θg[1, 0], :]  # 符合c角度的体素
    Pc_max = np.max(v_Fc_before[:, 0])
    v_Fc_before = v_Fc_before[v_Fc_before[:, 0] >= (Pc_max - m + 1), :]
    # 大于曲率0.05的体元
    v_Fc_id = []
    for i in range(len(v_Fc_before)):
        if v_c[int(v_Fc_before[i, 0]), int(v_Fc_before[i, 1]), int(v_Fc_before[i, 2])] > c_:
            v_Fc_id.append(i)
    v_Fc_ = v_Fc_before[v_Fc_id, :]
    # v_Fc_ = RemovalOfLongitudinalSeamAbnormality(v_Fc_)  # 剔除异常值
    del v_Fc_before
    # d
    v_Fd_before = v_p_un[v_p_un[:, 1] <= θg[2, 1], :]
    v_Fd_before = v_Fd_before[v_Fd_before[:, 1] >= θg[2, 0], :]  # 符合A2角度的体素
    Pd_max = np.max(v_Fd_before[:, 0])
    v_Fd_before = v_Fd_before[v_Fd_before[:, 0] >= (Pd_max - m + 1), :]
    v_Fd_id = []
    for i in range(len(v_Fd_before)):
        if v_c[int(v_Fd_before[i, 0]), int(v_Fd_before[i, 1]), int(v_Fd_before[i, 2])] > c_:
            v_Fd_id.append(i)
    v_Fd_ = v_Fd_before[v_Fd_id, :]
    # v_Fd_ = RemovalOfLongitudinalSeamAbnormality(v_Fd_)  # 剔除异常值
    return v_Fb_, v_Fc_, v_Fd_


def Reduced_diagonal_seam(v_xf, Complexity=0, v_before=None):
    "斜缝去噪(体素条件下)"
    # Complexity=0,1,-1 0:不需要判断斜率，1：斜率大于0，-1：斜缝小于0。复杂情况下需要 原体素
    global k, b
    'k值去噪'
    # 求得斜缝四至
    Y_max = np.max(v_xf[:, -1])
    Y_min = np.min(v_xf[:, -1])
    id_Y_max = np.argmax(v_xf[:, -1])
    id_Y_min = np.argmin(v_xf[:, -1])
    O_Y_max = v_xf[id_Y_max, 1]
    O_Y_min = v_xf[id_Y_min, 1]
    O_max = np.max(v_xf[:, 1])
    O_min = np.min(v_xf[:, 1])
    # 求O的平均数
    O_m = np.around(np.mean(v_xf[:, 1]))
    Y_O_m = np.mean(v_xf[v_xf[:, 1] == O_m, -1])
    # 求斜率和截距
    if Complexity == 0:
        k = (Y_max - Y_O_m) / (O_Y_max - O_m)  # 最高点
        b = Y_O_m - k * O_m
        # print('无干扰的直线K值', k)
    elif Complexity == 1:
        k = (Y_max - Y_min) / (O_max - O_min)
        b = Y_max - k * O_max
    elif Complexity == -1:
        k = (Y_max - Y_min) / (O_min - O_max)
        b = Y_max - k * O_min
    # 计算每个体元到线的距离
    print('斜率为', k)
    dis = np.abs(k * v_xf[:, 1] + b - v_xf[:, -1])  # 距离容器
    d_dis = 22  # 阈值  15
    # 去噪
    v_xf_new = v_xf[dis <= d_dis, :]
    '二次处理'
    # 只是为了要最大的P值
    if Complexity != 0:
        # 求Y中位数
        Y_m = np.floor(np.median(v_xf_new[:, -1]))
        O_Y_m = np.mean(v_xf_new[v_xf_new[:, -1] == Y_m, 1])
        # if Complexity > 0 and np.abs(k-5.4) > 2:
        if Complexity > 0:
            k = 5.4
        # elif Complexity < 0 and np.abs(k+5.4) > 2:
        elif Complexity < 0:
            k = -5.4
        b = Y_m - k * O_Y_m
        # 符合P范围的体元
        P_max = np.max(v_xf_new[:, 0])
        v_xf_new = v_before[v_before[:, 0] <= P_max, :]
        v_xf_new = v_xf_new[v_xf_new[:, 0] >= (P_max - 5), :]
        dis = np.abs(k * v_xf_new[:, 1] + b - v_xf_new[:, -1])  # 距离容器
        v_xf_new = v_xf_new[dis <= d_dis, :]
    return v_xf_new


def RemovalOfLongitudinalSeamAbnormality(v_xf):
    '2022.10.19剔除异常纵缝体素'
    # 找到斜缝的角度值左右值体素平面位置
    θ_max = np.max(v_xf[:, 1])
    θ_min = np.min(v_xf[:, 1])
    id_θ_max = np.argmax(v_xf[:, 1])
    id_θ_min = np.argmin(v_xf[:, 1])
    Y_θ_max = v_xf[id_θ_max, -1]
    Y_θ_min = v_xf[id_θ_min, -1]
    # 求直线的k,b
    k = (Y_θ_max - Y_θ_min) / (θ_max - θ_min)
    print('斜率为', k)
    b = Y_θ_max - k * θ_max
    # 求体素到直线的距离
    d_dis = 15  # 边长倍数阈值
    dis = np.abs(k * v_xf[:, 1] + b - v_xf[:, -1])  # 距离容器
    v_xf_new = v_xf[dis <= d_dis, :]
    return v_xf_new


def Line_completion(F_all):
    "点云线补全算法（算法多余，现已停止使用）"
    for i in F_all.keys():
        F_ = F_all.get(i)  # 每一个标签的点云
        Y_all_un = np.unique(F_[:, -1])  # 求所有Y的下标分布
        num_Y = len(Y_all_un)  # Y的遍历长度
        num_mean = int(len(F_) / num_Y)  # 求每个Y的平均点云数量
        print('此纵缝的平均Y单位点数量为', num_mean)
        # 求Y递增数列
        Y_arg = np.arange(np.min(Y_all_un), np.max(Y_all_un) + 1)
        # 使用Ransac直线拟合，建立O与Y之间的关系式
        k_, b_ = rs.fit_2Dline_ransac(F_[:, -2:], sigma=0.15)
        print('拟合直线的斜率为', k_, '拟合直线的半径为', b_)
        if k_ == 0:
            θ_ = np.around(np.mean(F_[:, -2]))  # 四舍五入
            θ_un_ = np.ones(num_Y) * θ_
        else:
            θ_un_ = np.unique(np.around((Y_arg - b_) / k))  # 四舍五入、
        # 设置点云起始位置以及分辨率

        # 按照Y值进行遍历，找到中轴是否存在点云，若是，则不需要补全，若不是，则需要补全
        l = 0  # 递增遍历
        for j in range(Y_all_un):
            Y_have_ = F_[F_[:, -1] == j, :]
            θ_have_ = Y_have_[Y_have_[:, -2] == θ_un_[l]]
            l += 1
            if len(θ_have_) == 0:  # 判断需要补全
                pass
        # 遍历Y，以公式查看O的范围，然后根据点的多少确定是否进行补点（最好建立一个O的左右范围点数量横切面直方图）
        # 进行补点
        # 返回到F_all
    return F_all


def Fitellipse(xyzic, xmax=3, xmin=-2.9, ymax=4.5, ymin=0.5):
    '椭圆拟合'
    '1.限制坐标范围'
    xyzic_0 = xyzic[xyzic[:, 0] > xmin, :]
    xyzic_0 = xyzic_0[xyzic_0[:, 0] < xmax, :]
    xyzic_0 = xyzic_0[xyzic_0[:, 2] < ymax, :]
    xyzic_0 = xyzic_0[xyzic_0[:, 2] > ymin, :]
    '2.整体拟合'
    xz_0 = np.c_[xyzic_0[:, 0], xyzic_0[:, 2]]  # 对拟合点进行降维
    reg = LsqEllipse().fit(xz_0)  # 输入二维点
    center, width, height, phi = reg.as_parameters()  # 求解
    print(f'center: {center[0]:.3f}, {center[1]:.3f}')  # 椭圆点中心
    print(f'width: {width:.3f}')  # 椭圆长半轴
    print(f'height: {height:.3f}')  # 椭圆短半轴
    print(f'phi: {phi:.3f}')  # 椭圆旋转角
    '3.求点到椭圆的距离'
    arg_Ellipse = np.array([center[0], center[1], np.max([width, height]), np.min([width, height]), phi])  # 合并椭圆参数
    distances = ep.dis_ellipse(xyzic[:, 0], xyzic[:, 2], arg_Ellipse)  # 求点到椭圆的距离
    distances = np.nan_to_num(distances, nan=0)  # 将空值转换为0
    '4.求均值和标准差'
    dis_mean = np.mean(distances)
    dis_std = np.std(distances)
    print(dis_std, dis_mean)
    m = -0.25  # 标准差倍数
    threshold = dis_mean + dis_std * m  # 阈值
    print(threshold)
    xyzic_0 = xyzic[distances < threshold, :]
    '5.第二次拟合'
    index, outdex, xzlwp, dis = fit_Ellipse_starmap(xyzic_0[:, 0], xyzic_0[:, 2], xyzic_0[:, -1], xyzic)  # 求每个圆环的参数
    xyzic_0 = xyzic[index, :]
    return

def fit_Ellipse_starmap(x, z, c, points,num_cpu=mp.cpu_count(), dr=0.05):
    '异步并行计算拟合椭圆 Parallel computing for fitting ellipses'
    c_un = np.unique(c)  # 所有序列数
    num_c = len(c_un)  # 圆环数
    xzlwp = np.empty([num_c, 5])  # 新建一个存储每个椭圆参数的容器
    dis_all = np.zeros(len(points))  # 距离容器
    # 并行计算加速准备 #Accelerated preparation for parallel computing
    pool = mp.Pool(processes=num_cpu)  # 开启多进程池，数量为cpu
    # tik = cut_down(num_c)  # 分块器
    # tik2 = p0.cut_down(len(points))  # 分块器
    j = 0  # 分块输出计时器
    # 并行计算每个圆环 Parallel computing for each torus
    # 并行计算
    multi_res = pool.starmap_async(fit_Ellipse_single, ((x[c==c_un[i]], z[c==c_un[i]], c_un[i], points) for i in
                 tqdm(range(num_c),desc='分配任务拟合单个椭圆参数',unit='个cross-section',total=num_c)))
    # multi_res = [pool.starmap_async(fit_Ellipse_single, (x[c==c[i]].reshape(1,-1), z[c==c[i]].reshape(1,-1), c_un[i], points)) for i in
    #              tqdm(range(num_c))]  # 将每个block需要处理的点云区间发送到每个进程当中
    for res in tqdm(multi_res.get(), total=num_c, desc='导出单个椭圆参数', unit='个cross-section'):
    # for res in tqdm(multi_res):
        # xzlwp[tik[tik_b]:tik[tik_b + 1], :] = res.get()  # 将每个进程得到的结果分block的发送到容器当中
        # res_block = res.get()
        dis_all += res[0]
        # dis_all += res_block[0]  # 距离累加
        xzlwp[j, :] = res[1]  # 拟合值回归
        j += 1
    pool.close()  # 禁止进程池再接收任务  #Prohibit process pools from receiving tasks again
    pool.join()  # 当所有进程全部计算完毕后，退出并行计算  #After all processes have completed their calculations, exit parallel computing
    # 下标
    index = dis_all <= dr
    outdex = dis_all > dr
    # RMSE
    dis_index = dis_all[dis_all <= dr]  # 在椭圆上的距离集合
    RMSE = np.sqrt(np.sum(dis_index**2)/len(dis_index))
    print('RMSE:', RMSE)
    print('保留点比例：',len(dis_index)/len(points))
    return index, outdex, xzlwp, dis_all

def fit_Ellipse_single(x, z, c_, points):
    '单环求椭圆参数 Calculating Ellipse Parameters by Blocks'
    # num_ = e-b
    # c_un = np.unique(c)
    dis_ = np.zeros(len(points))
    # dis_ = np.zeros(num_)
    xzlwp = np.empty([5])
    # j = 0  # 循环计数器 cycle counter
    # for c1 in c_un[b:e]:
    # x_c1 = x[c == c1]
    # z_c1 = z[c == c1]  # 提取单环的全部点云二维坐标  Extract all point cloud 2D coordinates of a single ring
    xz_ = np.vstack([x, z]).T
    reg = LsqEllipse().fit(xz_)  # 输入二维点  Enter 2D points
    center, width, height, phi = reg.as_parameters()  # 求解 solve
    xzlwp[0] = center[0]
    xzlwp[1] = center[1]
    xzlwp[2] = np.max([width, height])
    xzlwp[3] = np.min([width, height])
    xzlwp[4] = phi
    dis_[points[:, 4] == c_] = ep.dis_ellipse(points[points[:, 4]==c_, 0], points[points[:, 4]==c_, 2], xzlwp)
    dis_ = np.nan_to_num(dis_, nan=0)  # 若距离为nan，则转换为1
    return dis_,xzlwp

def get_dis_θ(xz,xyzic):
    '求单截面点云相邻点的角度差240530,by:Mengyao Gao'
    # num = len(xyzic)  # 点云数量
    angle = np.arctan2(xyzic[:, 0]-xz[0], xyzic[:, 2]-xz[1]) * 180 / np.pi  # 点云求角度值
    diff = np.diff(angle)  # 计算相邻角度差值
    # plt.hist(diff, bins=3600)
    # plt.show()
    print('mean',np.mean(diff))
    print('std',np.std(diff))
    condition = (np.abs(diff) >= 0.03) & (np.abs(diff) <= 0.08)
    new_xyzic = xyzic[1:][condition]
    return new_xyzic,diff[condition]  # 符合条件的点云，相邻点角度差值

def get_wrong(xzlwp_l,xzlwp_r,xyzic_l,xyzic_r,n=5000):
    '基于实际变化的全局错台量分析20240530'
    x0_m = np.mean([xzlwp_l[0], xzlwp_r[0]])
    z0_m = np.mean([xzlwp_l[2], xzlwp_r[2]])  # 圆心轴
    angle_l = np.arctan2(xyzic_l[:, 0] - x0_m, xyzic_l[:, 2] - z0_m) * 180 / np.pi  # 点云求角度值
    angle_r = np.arctan2(xyzic_r[:, 0] - x0_m, xyzic_r[:, 2] - z0_m) * 180 / np.pi  # 点云求角度值
    single_a = np.pi * 2 / n  # 求每个块的过渡
    belong_l = np.floor(angle_l / single_a)
    belong_r = np.floor(angle_r / single_a)  # 属于区间
    b_l_un = np.unique(belong_l)
    b_r_un = np.unique(belong_r)  # 排序
    b_common = np.intersect1d(b_l_un, b_r_un)  # 共同区间
    print('共同区间数量',len(b_common))
    w_all = np.zeros([len(b_common)])  # 求错容器
    j = 0
    for i in b_common:
        xyz_l_ = np.mean(xyzic_l[belong_l == i, :3], axis=0)
        # print(xyz_l_)
        xyz_r_ = np.mean(xyzic_r[belong_r == i, :3], axis=0)
        # print(xyz_r_)
        w_all[j] = np.linalg.norm(np.array([xyz_l_[0],xyz_l_[2]])-np.array([xyz_r_[0],xyz_r_[2]]))
        print(w_all[j])
        j += 1
    # w_all = w_all[w_all <= 0.01]
    return np.max(w_all)

def conv_hull(points: np.ndarray):
    """
    生成凸包 参考文档：https://blog.csdn.net/io569417668/article/details/106274172
    :param points: 待生成凸包的点集
    :return: 索引 list
    """
    # pcl = array_to_pointcloud(points)
    # hull, lst = pcl.compute_convex_hull()
    hull = ConvexHull(points)
    lst = hull.vertices
    return lst

# 这里会返回列表类型
def load_data_txt(path):
    file = open(path, 'r')
    data = file.read().split('\n')
    lst = _data_trans(data)
    return lst


def array_to_pointcloud(np_array):
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(np_array)
    return pcd


def _data_trans(data):
    lst = []
    for num in data:
        num_list = num.split()
        lst.append([eval(i) for i in num_list])
    lst.pop()
    return lst

def in_convex_polyhedron(points_set: np.ndarray, test_points: np.ndarray):
    """
    检测点(集)是否在三维凸包内
    :param points_set: 凸包，需要对分区的点进行凸包生成 具体见conv_hull函数
    :param test_points: 需要检测的点 可以是多个点
    :return: bool类型
    """
    assert type(points_set) == np.ndarray
    assert type(test_points) == np.ndarray
    bol = np.zeros(test_points.shape[0], dtype=bool)
    ori_set = points_set
    ori_edge_index = conv_hull(ori_set)
    ori_edge_index = np.sort(np.unique(ori_edge_index))
    for i in tqdm(range(test_points.shape[0])):
        new_set = np.concatenate((points_set, test_points[i, np.newaxis]), axis=0)
        new_edge_index = conv_hull(new_set)
        new_edge_index = np.sort(np.unique(new_edge_index))
        bol[i] = (new_edge_index.tolist() == ori_edge_index.tolist())
    return bol

def trans_2D(data):
    '三维数据转二维数据'
    data_x = data[:, 0]
    data_y = data[:, 1]
    data_z = data[:, 2]
    data_i = data[:, 3]

    R = 2.75
    data_2D = np.empty((len(data), 4))

    for i in range(len(data)):
        xi = data_x[i]
        yi = data_y[i]
        zi = data_z[i]
        ii = data_i[i]
        if xi > 0:
            arf = np.arccos(zi / np.power((xi * xi + zi * zi), 0.5))
            new_x = arf * R
            data_2D[i, :] = [new_x, yi, R, ii]
        else:
            arf = np.arccos(zi / np.power((xi * xi + zi * zi), 0.5))
            new_x = -1 * arf * R
            data_2D[i, :] = [new_x, yi, R, ii]
    return data_2D


# 2D重采样(转成图像)
def resample(data, path):
    x = data[:, 0]
    y = data[:, 1]
    intensity = data[:, 3]
    pixel = 0.006

    max_x = np.max(x)
    min_x = np.max(x)
    max_y = np.max(y)
    min_y = np.min(y)

    length = int(math.ceil((max_x - min_x) / pixel))
    width = int(math.ceil((max_y - min_y) / pixel))
    image = np.empty([width + 1, length + 1])

    for i in range(len(data)):
        xi = np.ceil((x - min_x) / pixel).astype(int)
        yi = np.ceil((y - min_y) / pixel).astype(int)
        image[yi, xi] = intensity[i]

    image.astype(np.float32)
    image_ = cv.imwrite(path, image)
    return image_