用cpp写一个将xyz点云文件转为有序点云，根据输入像素坐标形成矩形锚框切割为新点云的函数

假设xyz点云文件格式为每行三个数字，分别是x、y、z坐标，以空格或制表符分隔。

首先，我们需要读入xyz点云文件，并把点云存储到一个vector中：

#include <iostream>
#include <fstream>
#include <vector>

struct Point3D {
    float x, y, z;
};

std::vector<Point3D> readPointCloud(const std::string& filename) {
    std::vector<Point3D> pointCloud;

    std::ifstream input(filename);
    if (!input.is_open()) {
        std::cerr << "Failed to open file: " << filename << std::endl;
        return pointCloud;
    }

    float x, y, z;
    while (input >> x >> y >> z) {
        pointCloud.push_back({x, y, z});
    }

    input.close();
    return pointCloud;
}

接下来，我们需要实现一个函数，将输入像素坐标形成矩形锚框，并根据锚框切割点云。假设输入的锚框信息为左上角坐标(x1, y1)，右下角坐标(x2, y2)，则可以按如下方式实现：

std::vector<Point3D> cropPointCloud(const std::vector<Point3D>& pointCloud, int x1, int y1, int x2, int y2) {
    // 确保锚框位置合法
    if (x1 < 0 || y1 < 0 || x2 >= IMAGE_WIDTH || y2 >= IMAGE_HEIGHT || x1 > x2 || y1 > y2) {
        std::cerr << "Invalid anchor box: (" << x1 << ", " << y1 << ") - (" << x2 << ", " << y2 << ")" << std::endl;
        return pointCloud;
    }

    // 计算锚框对应的点云索引范围
    size_t start = y1 * IMAGE_WIDTH + x1;
    size_t end = y2 * IMAGE_WIDTH + x2;

    // 切割点云
    return std::vector<Point3D>(pointCloud.begin() + start, pointCloud.begin() + end + 1);
}

完整代码如下：

#include <iostream>
#include <fstream>
#include <vector>

const int IMAGE_WIDTH = 640;
const int IMAGE_HEIGHT = 480;

struct Point3D {
    float x, y, z;
};

std::vector<Point3D> readPointCloud(const std::string& filename) {
    std::vector<Point3D> pointCloud;

    std::ifstream input(filename);
    if (!input.is_open()) {
        std::cerr << "Failed to open file: " << filename << std::endl;
        return pointCloud;
    }

    float x, y, z;
    while (input >> x >> y >> z) {
        pointCloud.push_back({x, y, z});
    }

    input.close();
    return pointCloud;
}

std::vector<Point3D> cropPointCloud(const std::vector<Point3D>& pointCloud, int x1, int y1, int x2, int y2) {
    // 确保锚框位置合法
    if (x1 < 0 || y1 < 0 || x2 >= IMAGE_WIDTH || y2 >= IMAGE_HEIGHT || x1 > x2 || y1 > y2) {
        std::cerr << "Invalid anchor box: (" << x1 << ", " << y1 << ") - (" << x2 << ", " << y2 << ")" << std::endl;
        return pointCloud;
    }

    // 计算锚框对应的点云索引范围
    size_t start = y1 * IMAGE_WIDTH + x1;
    size_t end = y2 * IMAGE_WIDTH + x2;

    // 切割点云
    return std::vector<Point3D>(pointCloud.begin() + start, pointCloud.begin() + end + 1);
}

int main() {
    std::vector<Point3D> pointCloud = readPointCloud("point_cloud.xyz");
    std::vector<Point3D> croppedPointCloud = cropPointCloud(pointCloud, 100, 100, 200, 200);
    std::cout << "Original point cloud size: " << pointCloud.size() << std::endl;
    std::cout << "Cropped point cloud size: " << croppedPointCloud.size() << std::endl;

    return 0;
}