OpenCV PointCloud from Depth map - c++

Question

I am trying to generate a point cloud with my Minoru3D stereo camera, but it does not work.
The generated points are definitely not correct.
The camera is correctly calibrated.
My rectified images(rec1, rec2), the disparity map(disp) and the depth map(depth):
And the pointcloud:
I create the depth image with reprojectImageTo3D and then read the points at every pixel.
Full Code:
#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;
}