基于ToF相机深度数据生成RGB点云的异常问题求助

我尝试用ToF相机的深度数据生成点云，并将RGB图像叠加到点云上。已通过OpenCV对ToF幅度图像与RGB图像做立体标定，校正映射保存为.xml文件并在程序中加载。目前代码能输出点云，但存在两个异常：

• RGB图像未完整显示在点云上
• 点云呈现逐渐缩小的锥形效果

相关代码 #

void openCVViewer::displayPCLRGBData2(float xData[], float yData[], float depth[], int frameWidth, cv::Mat img)
{
    // Convert ToF image data to cv::Mats to rectify
    cv::Mat depthMap(640, 480, CV_32FC1, depth); //= arrToMat(depth, frameWidth);
    
    // Load maps for rectification
    cv::Mat rmapx, rmapy, lmapx, lmapy, ToFCamMat;
    cv::FileStorage rgbFile;
    rgbFile.open(savePath1 + "RGBStereo.xml", cv::FileStorage::READ);
    if (!rgbFile.isOpened())
    {
        std::cerr << "Failed to find RGBStereo map" << std::endl;
    }
    rgbFile["mapx"] >> rmapx;
    rgbFile["mapy"] >> rmapy;
    rgbFile.release();
    cv::FileStorage tofFile;
    tofFile.open(savePath1 + "ToFStereo.xml", cv::FileStorage::READ);
    if (!tofFile.isOpened())
    {
        std::cerr << "Failed to find ToFStereo map" << std::endl;
    }
    tofFile["mapx"] >> lmapx;
    tofFile["mapy"] >> lmapy;
    tofFile.release();
    
    cv::FileStorage ToFIn;
    ToFIn.open(savePath1 + "ToFIntrinsic.xml", cv::FileStorage::READ);
    //ToFIn.open(savePath1 + "RGBIntrinsic.xml", cv::FileStorage::READ);
    if(!ToFIn.isOpened()){
        std::cerr << "Failed to finde ToFIntrinsic.xml" << std::endl;
    }
    ToFIn["K"] >> ToFCamMat;
    ToFIn.release();

    float cx = ToFCamMat.at<float>(0,2);
    float cy = ToFCamMat.at<float>(1,2);
    float fx = ToFCamMat.at<float>(0,0);
    float fy = ToFCamMat.at<float>(1,1);
    std:: cout << "cx: " << cx << " cy: " << cy << std::endl;

    cv::Mat xcoor(480,640, CV_32F, xData);
    cv::Mat ycoor(480,640, CV_32F, yData);

    // Rectify images
    cv::Mat recy, recx, recImg, recDep;
    cv::remap(depthMap, recDep, lmapx, lmapy, cv::INTER_LINEAR);
    cv::remap(img, recImg, rmapx, rmapy, cv::INTER_LINEAR);
    cv::remap(xcoor, recx, lmapx, lmapy, cv::INTER_LINEAR);
    cv::remap(ycoor, recy, lmapx, lmapy, cv::INTER_LINEAR);
    
    float* depthArr = new float[recDep.cols * recDep.rows];
    std::memcpy(depthArr, recDep.ptr<float>(0), (recDep.cols * recDep.rows) * sizeof(float));

    // Convert color
    cv::cvtColor(recImg, recImg, cv::COLOR_BGR2RGB);

    // Create the aligned point cloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr alignedPointCloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    std::cout << "img rows: " << recImg.rows << " img cols: " << recImg.cols << std::endl;
    int size = (recImg.rows * recImg.cols);
    // Loop through each point in the point cloud
    for (size_t i = 0; i < size; ++i) {
        // Retrieve the corresponding pixel coordinates in the corrected RGB image
        int x = static_cast<int>(lmapx.at<float>(i));
        int y = static_cast<int>(lmapy.at<float>(i));

        // Skip points with invalid pixel coordinates
        if (x < 0 || x >= recImg.cols || y < 0 || y >= recImg.rows)
            continue;

        // Retrieve the RGB color at the pixel coordinates from the corrected RGB image
        cv::Vec3b color = recImg.at<cv::Vec3b>(y, x);

        // Calculate the x and y coordinates based on the pixel and depth values
        float px = (x - cx) * depthArr[i] / fx;
        float py = (y - cy) * depthArr[i] / fy;

        // Create a PCL point and assign the coordinates and color
        pcl::PointXYZRGB alignedPoint;
        alignedPoint.x = px;
        alignedPoint.y = py;
        alignedPoint.z = depthArr[i];
        alignedPoint.r = color[2];
        alignedPoint.g = color[1];
        alignedPoint.b = color[0];

        // Add the aligned point to the aligned point cloud
        alignedPointCloud->push_back(alignedPoint);
    }
    // Visualize the point cloud
    pcl::visualization::CloudViewer viewer("Point Cloud Viewer");
    viewer.showCloud(alignedPointCloud);
    while (!viewer.wasStopped()) {
        // Do nothing
    }
}

问题分析与修复方案 #

1. 点云锥形变形的原因 #

代码中计算3D坐标时使用的是原始ToF相机内参，但深度图已经过remap校正，校正后的图像对应的是立体标定输出的校正后内参，而非原始内参，这会导致坐标计算错误，出现锥形变形。

修复：
从立体标定的ToFStereo.xml中读取校正后的投影矩阵P，从中提取校正后的内参：

// 在读取lmapx、lmapy后添加
cv::Mat ToFP;
tofFile["P"] >> ToFP;
float fx = ToFP.at<float>(0,0);
float fy = ToFP.at<float>(1,1);
float cx = ToFP.at<float>(0,2);
float cy = ToFP.at<float>(1,2);

2. RGB图像未完整显示的原因 #

循环中直接用i索引lmapx获取RGB像素坐标，混淆了校正前后的图像索引逻辑。立体校正后，ToF和RGB图像已经行对齐，同一位置的(y,x)像素是一一对应的，无需再通过原始映射表查找。

修复：
改为按行列遍历校正后的图像，直接用当前像素的(y,x)匹配RGB图像：

// 替换原有的for循环
for (int y = 0; y < recDep.rows; ++y) {
    for (int x = 0; x < recDep.cols; ++x) {
        float depth_val = recDep.at<float>(y, x);
        // 过滤无效深度值
        if (depth_val <= 0) continue;

        // 校正后图像行对齐，直接用(y,x)取RGB像素
        if (x < 0 || x >= recImg.cols || y < 0 || y >= recImg.rows)
            continue;
        
        cv::Vec3b color = recImg.at<cv::Vec3b>(y, x);

        // 用校正后的内参计算3D坐标
        float px = (x - cx) * depth_val / fx;
        float py = (y - cy) * depth_val / fy;

        pcl::PointXYZRGB alignedPoint;
        alignedPoint.x = px;
        alignedPoint.y = py;
        alignedPoint.z = depth_val;
        alignedPoint.r = color[2];
        alignedPoint.g = color[1];
        alignedPoint.b = color[0];

        alignedPointCloud->push_back(alignedPoint);
    }
}

3. 其他潜在问题修复 #

• 内存泄漏：原代码中new的depthArr未释放，建议在使用完后添加delete[] depthArr;，或改用std::vector<float>管理内存。
• 无效深度过滤：添加对深度值的有效性判断，避免生成错误的3D点。

内容的提问来源于stack exchange，提问作者TheProbPro