来自深度图的OpenCV PointCloud

我正在尝试用我的Minoru3D立体相机生成点 Cloud ，但它不起作用 . 生成的点肯定不正确 .
相机已正确校准 . 我纠正的图像（rec1，rec2），视差图（disp）和深度图（深度）：

和点 Cloud ：

我使用reprojectImageTo3D创建深度图像，然后读取每个像素的点 .

完整代码：

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/contrib/contrib.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "libcam.h"
#include <stdio.h>
#include <fstream>

using namespace cv;
using namespace std;

int main(int argc, char** argv) {

    // The camera properties
    int w = 640;
    int h = 480;
    int fps = 20;

    // The images, which are proceeded
    Mat img1, img2;
    // The grayscale versions of the images
    Mat gray1, gray2;
    // The disparity and depth map
    Mat disparity;
    Mat depth(Size(h, w), CV_32FC3);
    // The iplImage versions of the images used to get the images from the camera
    IplImage *iplImg1 = cvCreateImage(cvSize(w, h), 8, 3), *iplImg2 = cvCreateImage(cvSize(w, h), 8, 3);

    // The cameras
    Camera cam1("/dev/video0", w, h, fps), cam2("/dev/video1", w, h, fps);

    // Load the camera model
    Mat CM1, CM2, D1, D2, R, T, E, F, R1, R2, P1, P2, Q;
    FileStorage fs("data/stereocalib.yml", FileStorage::READ);
    fs["CM1"] >> CM1;
    fs["CM2"] >> CM2;
    fs["D1"] >> D1;
    fs["D2"] >> D2;
    fs["R"] >> R;
    fs["T"] >> T;
    fs["E"] >> E;
    fs["F"] >> F;
    fs["R1"] >> R1;
    fs["R2"] >> R2;
    fs["P1"] >> P1;
    fs["P2"] >> P2;
    fs["Q"] >> Q;
    fs.release();

    Mat map1x = Mat(h, w, CV_32F);
    Mat map1y = Mat(h, w, CV_32F);
    Mat map2x = Mat(h, w, CV_32F);
    Mat map2y = Mat(h, w, CV_32F);
    initUndistortRectifyMap(CM1, D1, R1, P1, cvSize(w, h), CV_32FC1, map1x, map1y);
    initUndistortRectifyMap(CM2, D2, R2, P2, cvSize(w, h), CV_32FC1, map2x, map2y);

    // The rectified images
    Mat imgU1 = Mat(img1.size(), img1.type()), imgU2 = Mat(img2.size(), img2.type());

    Ptr<StereoBM> sbm = createStereoBM(16 * 10, 5);

    while (true) {

        // Get the images from the cameras
        cam1.Update();
        cam2.Update();
        cam1.toIplImage(iplImg1);
        cam2.toIplImage(iplImg2);
        img1 = cvarrToMat(iplImg1);
        img2 = cvarrToMat(iplImg2);

        // Rectify
        remap(img1, imgU1, map1x, map1y, INTER_LINEAR, BORDER_CONSTANT, Scalar());
        remap(img2, imgU2, map2x, map2y, INTER_LINEAR, BORDER_CONSTANT, Scalar());

        // Convert the rectified images to grayscale images
        cvtColor(imgU1, gray1, CV_BGR2GRAY);
        cvtColor(imgU2, gray2, CV_BGR2GRAY);

        // Create disparity map
        sbm->compute(gray1, gray2, disparity);

        // Create depth map
        reprojectImageTo3D(disparity, depth, Q, true, CV_32F);

        //imshow("img1", img1);
        //imshow("img2", img2);
        imshow("rec1", gray1);
        imshow("rec2", gray2);
        imshow("disp", disparity);
        imshow("depth", depth);

        int key = waitKey(10);
        if (key == 'q') {
            break;
        }
        else if (key == 's') {

            stringstream output;
            for (int x = 0; x < img1.rows; x++) {
                for (int y = 0; y < img1.cols; y++) {
                    Point3f p = depth.at<Point3f>(x, y);
                    if(p.z >= 10000) continue;  // Filter errors
                    output << p.x << "," << p.y << "," << p.z << endl;
                }
            }

            ofstream outputFile("points");
            outputFile << output.str();
            outputFile.close();

            cout << "saved" << endl;
        }
        else if (key == 'p') {
            waitKey(0);
        }
    }
    destroyAllWindows();

    return 0;
}