def crop_pointcloud(data_crop, x_o, y_o, x_i, y_i, R_o, R_i, z_critical): 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 转c++ Eigen

以下是crop_pointcloud函数的C++ Eigen库实现：

#include <cmath>
#include <Eigen/Dense>
#include <vector>

using namespace Eigen;
using namespace std;

vector<MatrixXd> crop_pointcloud(vector<MatrixXd> data_crop, float x_o, float y_o, float x_i, float y_i, float R_o, float R_i, int z_critical) {
    int range_z = data_crop[0].cols();
    float K_o = pow(R_o, 2) / range_z;
    float K_i = pow(R_i, 2) / range_z;
    for (int z = 0; z < range_z; ++z) {
        float r_o = sqrt(z * K_o);
        MatrixXd data_layer = data_crop[z];
        float d_o = sqrt(pow(x_o, 2) + pow(y_o, 2));
        float d_i = sqrt(pow(x_i, 2) + pow(y_i, 2));
        float r_i = z < z_critical ? 0 : sqrt(z * K_i);
        data_crop[z] = (d_o > r_o || d_i <= r_i).select(MatrixXd::Zero(data_layer.rows(), data_layer.cols()), data_layer);
    }
    return data_crop;
}