c++ - OpenCV PointCloud from Depth map -

c++ - OpenCV PointCloud from Depth map -

i trying generate point cloud minoru3d stereo camera, not work. generated points not correct. photographic camera correctly calibrated. rectified images(rec1, rec2), disparity map(disp) , depth map(depth):

and pointcloud:

i create depth image reprojectimageto3d , read points @ every pixel. total 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) { // photographic camera properties int w = 640; int h = 480; int fps = 20; // images, proceeded mat img1, img2; // grayscale versions of images mat gray1, gray2; // disparity , depth map mat disparity; mat depth(size(h, w), cv_32fc3); // iplimage versions of images used images photographic camera iplimage *iplimg1 = cvcreateimage(cvsize(w, h), 8, 3), *iplimg2 = cvcreateimage(cvsize(w, h), 8, 3); // cameras photographic camera cam1("/dev/video0", w, h, fps), cam2("/dev/video1", w, h, fps); // load photographic 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); // rectified images mat imgu1 = mat(img1.size(), img1.type()), imgu2 = mat(img2.size(), img2.type()); ptr<stereobm> sbm = createstereobm(16 * 10, 5); while (true) { // images 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 rectified images 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; (int x = 0; x < img1.rows; x++) { (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(); homecoming 0; }

c++ opencv 3d stereo-3d