Ask Your Question
0

Getting point cloud from disparity

asked 2015-04-19 08:30:34 -0600

Aram gravatar image

updated 2015-04-20 17:21:44 -0600

I tried different ways of view point cloud but in all cases I get black screen. What I am doing wrong? Am I generating point cloud the right way? Or am I wrong visualizing point cloud?

My source images are: enter image description here enter image description here

The disparity looks like: enter image description here

This is my code:

#include "opencv2/core/core.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "opencv2/contrib/contrib.hpp"
#include <cstdio>
#include <iostream>
#include <fstream>
#include "pcl/common/common_headers.h"
#include "pcl/io/io.h"
#include "pcl/visualization//pcl_visualizer.h"
#include <boost/thread/thread.hpp>
#include <pcl/io/pcd_io.h>
#include "pcl/point_cloud.h"
#include "pcl/visualization/cloud_viewer.h"

using namespace cv;
using namespace std;
using namespace pcl;


ofstream out("points.txt");

boost::shared_ptr<pcl::visualization::PCLVisualizer> createVisualizer (pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud)
{
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
  viewer->addPointCloud<pcl::PointXYZRGB> (cloud, rgb, "reconstruction");
  //viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "reconstruction");
  viewer->addCoordinateSystem ( 1.0 );
  viewer->initCameraParameters ();
  return (viewer);
}

int main()
{
    Mat img1, img2;
    img1 = imread("I1.png");
    img2 = imread("I2.png");

    Mat g1,g2, disp, disp8;

    cvtColor(img1, g1, CV_BGR2GRAY);
    cvtColor(img2, g2, CV_BGR2GRAY);

    int sadSize = 3;
    StereoSGBM sbm;
    sbm.SADWindowSize = sadSize;
    sbm.numberOfDisparities = 144;//144; 128
    sbm.preFilterCap = 10; //63
    sbm.minDisparity = 0; //-39; 0
    sbm.uniquenessRatio = 10;
    sbm.speckleWindowSize = 100;
    sbm.speckleRange = 32;
    sbm.disp12MaxDiff = 1;
    sbm.fullDP = true;
    sbm.P1 = sadSize*sadSize*4;
    sbm.P2 = sadSize*sadSize*32;
    sbm(g1, g2, disp);

    normalize(disp, disp8, 0, 255, CV_MINMAX, CV_8U);

    Mat dispSGBMscale; 
    disp.convertTo(dispSGBMscale,CV_32F, 1./16); 

    double cm1[3][3] = {{8.941981e+02, 0.000000e+00, 6.601406e+02}, {0.000000e+00, 8.927151e+02, 2.611004e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};
    double cm2[3][3] = {{8.800704e+02, 0.000000e+00, 6.635881e+02 }, {0.000000e+00, 8.798504e+02, 2.690108e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};
    double d1[1][5] = {{ -3.695739e-01, 1.726456e-01, -1.281525e-03, 1.188796e-03, -4.284730e-02}};
    double d2[1][5] = {{-3.753454e-01, 1.843265e-01, -1.307069e-03, 2.190397e-03, -4.989103e-02}};

    Mat CM1 (3,3, CV_64FC1, cm1);
    Mat CM2 (3,3, CV_64FC1, cm2);
    Mat D1(1,5, CV_64FC1, d1);
    Mat D2(1,5, CV_64FC1, d2);

    double r[3][3] = {{9.998381e-01, 1.610234e-02, 8.033237e-03},{-1.588968e-02, 9.995390e-01, -2.586908e-02 },{-8.446087e-03, 2.573724e-02, 9.996331e-01}};
    double t[3][4] = {{ -5.706425e-01}, {8.447320e-03}, {1.235975e-02}};

    Mat R (3,3, CV_64FC1, r);
    Mat T (3,1, CV_64FC1, t);

    //Mat   R, T;
    Mat R1, R2, T1, T2, Q, P1, P2;

    stereoRectify(CM1, D1, CM2, D2, img1.size(), R, T, R1, R2, P1, P2, Q);

    Mat points, points1;
    reprojectImageTo3D(disp8, points, Q, true);
    imshow("points", points);
    cvtColor(points, points1, CV_BGR2GRAY);
    imshow("points1", points1);

    imwrite("disparity.jpg", disp8);
    imwrite("points1.jpg", points1);
    imwrite("points.jpg", points);


    for(int i=0; i<points.rows; ++i)
    {
        Point3f* point = points ...
(more)
edit retag flag offensive close merge delete

Comments

mostly, the object is just 'behind you'. try to press 'h' in the pclviewer, there is some option to 'jump' to the object.

also, you'll probably have to invert the y-axis (by multiplying with -1) , y goes up in 3d space, but down in opencv's 2d images

berak gravatar imageberak ( 2015-04-19 09:04:41 -0600 )edit

@berak: I press "h" but didn't find some helpfull option. I also tried multiplying point.y with -1, but it doesn't help too.

Aram gravatar imageAram ( 2015-04-19 09:51:55 -0600 )edit

1 answer

Sort by ยป oldest newest most voted
2

answered 2015-04-19 14:53:48 -0600

Eduardo gravatar image

updated 2015-04-19 19:04:02 -0600

I tried your program. I think there is a problem when you initialize the size of cm1 and cm2 (it should be [3][3]):

double cm1[3][1] = {{8.941981e+02, 0.000000e+00, 6.601406e+02}, {0.000000e+00, 8.927151e+02,
     2.611004e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};
double cm2[3][2] = {{8.800704e+02, 0.000000e+00, 6.635881e+02 }, {0.000000e+00, 8.798504e+02,
     2.690108e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};

It tried to visualize your result with Meshlab (rename the file to .xyz and remove the header) as I don't have for the moment PCL:

xyz2

For

reprojectImageTo3D

I think that it is better to use the raw disparity instead of the normalize one. The result:

xyz

I don't think there is a problem when you reconstruct the 3D the point cloud.

Edit:

When I load the point cloud (your pcd file renamed in xyz and without the header part) into Meshlab, I get this:

xyz3

and I have to move around to get the view of the first image (rotation on two axis + zoom).

Also, to obtain the point cloud in the second image, I had to manually threshold the value:

if(points.at<Vec3f>(i,j)[2] < 10) { 
  //Write
}

I use this tutorial to get better visualization.

The code I used:

#include <iostream>
#include <fstream>

#include <opencv2/core/core.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;
using namespace std;


int main()
{
    Mat img1, img2;
    img1 = imread("left.png", CV_LOAD_IMAGE_GRAYSCALE);
    img2 = imread("right.png", CV_LOAD_IMAGE_GRAYSCALE);

    double cm1[3][3] = {{8.941981e+02, 0.000000e+00, 6.601406e+02}, {0.000000e+00, 8.927151e+02, 2.611004e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};
    double cm2[3][3] = {{8.800704e+02, 0.000000e+00, 6.635881e+02 }, {0.000000e+00, 8.798504e+02, 2.690108e+02}, {0.000000e+00, 0.000000e+00, 1.000000e+00}};
    double d1[1][5] = {{ -3.695739e-01, 1.726456e-01, -1.281525e-03, 1.188796e-03, -4.284730e-02}};
    double d2[1][5] = {{-3.753454e-01, 1.843265e-01, -1.307069e-03, 2.190397e-03, -4.989103e-02}};

    Mat CM1 (3,3, CV_64FC1, cm1);
    Mat CM2 (3,3, CV_64FC1, cm2);
    Mat D1(1,5, CV_64FC1, d1);
    Mat D2(1,5, CV_64FC1, d2);

    cout << "Calibration matrix left:\n" << CM1 << endl;
    cout << "Distorstion matrix left:\n" << D1 << endl;
    cout << "Calibration matrix right:\n" << CM2 << endl;
    cout << "Distorstion matrix right:\n" << D2 << endl;

    double r[3][3] = {{9.998381e-01, 1.610234e-02, 8.033237e-03},{-1.588968e-02, 9.995390e-01, -2.586908e-02 },{-8.446087e-03, 2.573724e-02, 9.996331e-01}};
    double t[3][4] = {{ -5.706425e-01}, {8.447320e-03}, {1.235975e-02}};

    Mat R (3,3, CV_64FC1, r);
    Mat T (3,1, CV_64FC1, t);

    //Mat   R, T;
    Mat R1, R2, T1, T2, Q, P1, P2;

    stereoRectify(CM1, D1, CM2, D2, img1.size(), R, T, R1, R2, P1, P2, Q);

    Mat map11, map12, map21, map22;
    Size img_size = img1.size();
    initUndistortRectifyMap(CM1, D1, R1, P1, img_size, CV_16SC2 ...
(more)
edit flag offensive delete link more

Comments

Thank you for answer. cm1 and cm2 are (3,3) I think it's problem of copy or smth else. I'll try with MeshLab now. But why I can't see with point cloud viewer?

Aram gravatar imageAram ( 2015-04-19 15:59:39 -0600 )edit

I'm running my code and view results with MeshLab but I did'nt get the same result as you.

Aram gravatar imageAram ( 2015-04-19 17:22:58 -0600 )edit

Thank you for the answer :) The result is so bad because of matrix Q?

Aram gravatar imageAram ( 2015-04-20 17:17:57 -0600 )edit

What needs improvement here ?

JohnnyJacob gravatar imageJohnnyJacob ( 2017-02-17 10:09:06 -0600 )edit

Question Tools

4 followers

Stats

Asked: 2015-04-19 08:30:34 -0600

Seen: 10,306 times

Last updated: Apr 20 '15