def crop_pointcloud(data_crop, x_o, y_o, x_i, y_i, R_o, R_i, z_critical): range_z = 400; K_o = R_o ** 2 / range_z K_i = R_i ** 2 / range_z for z in range(range_z): r_o = np.sqrt(z * K_o) data_layer = data_crop[:, :, z] d_o = np.sqrt(x_o ** 2 + y_o ** 2) d_i = np.sqrt(x_i ** 2 + y_i ** 2) if z < z_critical: r_i = 0 else: r_i = np.sqrt(z * K_i) data_crop[:, :, z] = np.where((d_o > r_o) | (d_i <= r_i), 0, data_layer) return data_crop 转Eigen和c++代码

下面是crop_pointcloud函数的Eigen/C++代码实现：

#include <Eigen/Core>
#include <cmath>

Eigen::ArrayXXd crop_pointcloud(Eigen::ArrayXXd data_crop, double x_o, double y_o, double x_i, double y_i, double R_o, double R_i, int z_critical)
{
    const int range_z = 400;
    const double K_o = std::pow(R_o, 2) / range_z;
    const double K_i = std::pow(R_i, 2) / range_z;

    for (int z = 0; z < range_z; ++z)
    {
        const double r_o = std::sqrt(z * K_o);
        Eigen::ArrayXXd data_layer = data_crop.slice(z);
        const double d_o = std::sqrt(std::pow(x_o, 2) + std::pow(y_o, 2));
        const double d_i = std::sqrt(std::pow(x_i, 2) + std::pow(y_i, 2));

        double r_i = 0;
        if (z >= z_critical)
        {
            r_i = std::sqrt(z * K_i);
        }

        data_crop.slice(z) = (d_o > r_o || d_i <= r_i).select(0, data_layer);
    }

    return data_crop;
}

请注意，这里的Eigen数组是使用slice()方法进行切片来实现三维数组的效果。