Stereo camera point cloud using viz in opencv

Question

I am trying to create a point cloud with a zed stereo camera. So I wrote some simple code and visualized it with viz, one of opencv modules. But xyz seems to come out well, but no result. What is the problem?

{   
    double baseLine = 120.0;
    double covergence = 0.00285;
    double FX = 700;
    double FY = 700;
    double CX = 320;
    double CY = 240;
    double K1 = -0.15;
    double K2 = 0.0;
    double P1 = 0.0;
    double P2 = 0.0;

    cv::Matx33d K = cv::Matx33d(FX, 0, CX, 0, FY, CY, 0, 0, 1);
    cv::Matx41d distCoeffs = cv::Matx41d(K1, K2, P1, P2);
    cv::Matx44d Q = cv::Matx44d(    
        1.0, 0.0, 0.0, -CX,
        0.0, 1.0, 0.0, -CY,
        0.0, 0.0, 0.0, FX,
        0.0, 0.0, -1.0 / baseLine, (CX - CX) / baseLine
    );
    //// SGBM
    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 16 * 5, 9);

    // param
    int sgbmWinSize = 3;
    int numberOfDisparities = 16 * 6;
    int cn = 3;

    // filter
    cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter;
    wls_filter = cv::ximgproc::createDisparityWLSFilter(sgbm);
    cv::Ptr<cv::StereoMatcher> sm = cv::ximgproc::createRightMatcher(sgbm);
    // param
    double lambda = 8000.0;
    double sigma = 1.5;
    double vis_multi = 1.0;

    // init
    sgbm->setPreFilterCap(63);
    sgbm->setBlockSize(sgbmWinSize);
    sgbm->setP1(8 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setP2(32 * cn*sgbmWinSize*sgbmWinSize);
    sgbm->setMode(cv::StereoSGBM::MODE_SGBM_3WAY);

    //// viz
    cv::viz::Viz3d window("Coordinate Frame");
    window.showWidget("Coordinate Widget", cv::viz::WCoordinateSystem());

    //main loop
    while (!window.wasStopped())
    {
        cv::Mat tmpImg;
        cap.read(tmpImg);
        leftImg = tmpImg(cv::Rect(0, 0, tmpImg.cols / 2, tmpImg.rows));
        rightImg = tmpImg(cv::Rect(tmpImg.cols / 2, 0, tmpImg.cols / 2, tmpImg.rows));
        cv::Mat tmp1 = leftImg.clone();
        cv::Mat tmp2 = rightImg.clone();

        sgbm->compute(tmp1, tmp2, disparity16S);
        sm->compute(tmp2, tmp1, img16Sr);

        cv::Mat showDisparity;
        disparity16S.convertTo(showDisparity, CV_8UC1, 255 / (numberOfDisparities*16.));

        printf("disparity16S: %s %d x %d\n", type2str(disparity16S.type()).c_str(), disparity16S.rows, disparity16S.cols);
        cv::imshow("disparity", showDisparity);

        wls_filter->setLambda(lambda);
        wls_filter->setSigmaColor(sigma);
        wls_filter->filter(disparity16S, tmp1, filteredDisparity, img16Sr);

        cv::Mat showFilteredDisparity;
        filteredDisparity.convertTo(showFilteredDisparity, CV_8U, 255 / (numberOfDisparities*16.));

        printf("filteredDisparity: %s %d x %d\n", type2str(filteredDisparity.type()).c_str(), filteredDisparity.rows, filteredDisparity.cols);
        cv::imshow("Filtered Disparity", showFilteredDisparity);

        cv::Mat xyz, xyzt;
        // output : 3-channel floating-point image of the same size as disparity
        cv::reprojectImageTo3D(filteredDisparity, xyz, Q, true);
        printf("xyz: %s %d x %d\n", type2str(xyz.type()).c_str(), xyz.rows, xyz.cols);

        cv::Mat showXYZ;
        xyz.convertTo(showXYZ, CV_8UC3, 255 / (numberOfDisparities*8.));
        cv::imshow("XYZ", showXYZ);

        viz::WCloud cw(xyz, viz::Color::white());
        cw.setRenderingProperty(cv::viz::POINT_SIZE, 2);
        window.showWidget("Cloud Widget", cw);
        window.spinOnce(30, true);

    }
   //
}

Answer 1

I know I'm answering after a very long time, Let this be useful to people who come across this problem.

You have used OpenCV's reprojectTo3D() function to get 3D points and you have handled the missing values. The values like infinity and NaN causes issues when you display using viz. So you just filter out the infinity values and NaN values.

I used OpenCV's forEach function to filter these values

xyz.forEach<Vec3f>(
[](Vec3f& val, const int *pos)
{
    if(isnan(val[0]) || isinf(val[0]))
        val = Vec3f();
});