c++ - 在opencv中使用viz的立体相机点云

Question

我正在尝试使用 zed 立体相机创建点云。因此，我编写了一些简单的代码，并使用 opencv 模块之一 viz 将其可视化。但是xyz好像出来的不错，但是没有结果。有什么问题？

{   
    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

我知道我在很长一段时间后才回答，让这对遇到这个问题的人有用。

您已使用 OpenCV 的 reprojectTo3D() 函数获取 3D 点并且已处理缺失值。当您使用可视化项显示时，无穷大和 NaN 等值会导致问题。因此，您只需过滤掉无穷大值和 NaN 值。

我使用 OpenCV 的 forEach 函数来过滤这些值

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