Calculating depth map from stereo images on M350

[uzgit]

I have some questions regarding the stereovision sample in samples/sample_c++/module_sample/perception/test_perception_entry.cpp

1. The camera directions, e.g. DJI_PERCEPTION_RECTIFY_FRONT are named such that they are implied to be rectified streams. Can you please confirm if they are in fact rectified?
2. Do you have code samples for creating a depth map from the pairs of stereo images? I have successfully visualized the disparity and created a depth map using an OpenCV sample, but it appears that the units are incorrect in my calculation (see below).

static void *DjiTest_StereoImagesDisplayTask(void *arg)
{
    T_DjiOsalHandler *osalHandler = DjiPlatform_GetOsalHandler();
    auto *pack = (T_DjiTestStereoImagePacket *) arg;
    char nameStr[32] = {0};
    char fpsStr[20] = "FPS: ";
    int fps = 0;
    double timePrev[USER_PERCEPTION_DIRECTION_NUM] = {0};
    double timeNow[USER_PERCEPTION_DIRECTION_NUM] = {0};
    double timeFess[USER_PERCEPTION_DIRECTION_NUM] = {0};
    int count[USER_PERCEPTION_DIRECTION_NUM] = {1};
    char showFpsString[USER_PERCEPTION_DIRECTION_NUM][FPS_STRING_LEN] = {0};
    int i = 0;

    cv::Mat display;
    cv::Mat image_l;
    cv::Mat image_r;
    cv::Mat disparity_left;
    cv::Mat disparity_right;
    cv::Mat filtered_disparity;
    cv::Mat disparity;
    cv::Mat depth;

    cv::Mat cameraMatrix1 = (cv::Mat_<double>(3, 3) <<
                             camera_parameters.leftIntrinsics[0], camera_parameters.leftIntrinsics[1], camera_parameters.leftIntrinsics[2],
                             camera_parameters.leftIntrinsics[3], camera_parameters.leftIntrinsics[4], camera_parameters.leftIntrinsics[5],
                             camera_parameters.leftIntrinsics[6], camera_parameters.leftIntrinsics[7], camera_parameters.leftIntrinsics[8]);

    cv::Mat cameraMatrix2 = (cv::Mat_<double>(3, 3) <<
                             camera_parameters.rightIntrinsics[0], camera_parameters.rightIntrinsics[1], camera_parameters.rightIntrinsics[2],
                             camera_parameters.rightIntrinsics[3], camera_parameters.rightIntrinsics[4], camera_parameters.rightIntrinsics[5],
                             camera_parameters.rightIntrinsics[6], camera_parameters.rightIntrinsics[7], camera_parameters.rightIntrinsics[8]);

    // Convert the rotation matrix to CV_64F
    cv::Mat R = (cv::Mat_<double>(3,3) <<
                 static_cast<double>(camera_parameters.rotationLeftInRight[0]), static_cast<double>(camera_parameters.rotationLeftInRight[1]), static_cast<double>(camera_parameters.rotationLeftInRight[2]),
                 static_cast<double>(camera_parameters.rotationLeftInRight[3]), static_cast<double>(camera_parameters.rotationLeftInRight[4]), static_cast<double>(camera_parameters.rotationLeftInRight[5]),
                 static_cast<double>(camera_parameters.rotationLeftInRight[6]), static_cast<double>(camera_parameters.rotationLeftInRight[7]), static_cast<double>(camera_parameters.rotationLeftInRight[8]));

    // Convert the translation vector to CV_64F
    cv::Mat T = (cv::Mat_<double>(3,1) <<
                 static_cast<double>(camera_parameters.translationLeftInRight[0]), static_cast<double>(camera_parameters.translationLeftInRight[1]), static_cast<double>(camera_parameters.translationLeftInRight[2]));

    // Distortion coefficients should also be CV_64F
    cv::Mat distCoeffs1 = cv::Mat::zeros(5, 1, CV_64F); // Zero distortion for left camera
    cv::Mat distCoeffs2 = cv::Mat::zeros(5, 1, CV_64F); // Zero distortion for right camera

    cv::Size image_size = cv::Size(640, 480); // Adjust based on your image resolution

    cv::Mat R1, R2, P1, P2, Q;
    std::cout << "before stereo rectification: " << std::endl;

    // Output the input parameters
    std::cout << "cameraMatrix1 (" << type_string(cameraMatrix1) << "):\n" << cameraMatrix1 << std::endl;
    std::cout << "distCoeffs1 (" << type_string(distCoeffs1) << "):\n" << distCoeffs1 << std::endl;
    std::cout << "cameraMatrix2 (" << type_string(cameraMatrix2) << "):\n" << cameraMatrix2 << std::endl;
    std::cout << "distCoeffs2 (" << type_string(distCoeffs2) << "):\n" << distCoeffs2 << std::endl;
    std::cout << "R (Rotation Matrix) (" << type_string(R) << "):\n" << R << std::endl;
    std::cout << "T (Translation Vector) (" << type_string(T) << "):\n" << T << std::endl;
    std::cout << "image_size: " << image_size.width << "x" << image_size.height << std::endl;

    cv::stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
                      image_size, R, T, R1, R2, P1, P2, Q,
                      cv::CALIB_ZERO_DISPARITY, -1, image_size);

    std::cout << "Output parameters from stereoRectify:" << std::endl;
    std::cout << "R1 (Rectification Transform 1) (" << type_string(R1) << "):\n" << R1 << std::endl;
    std::cout << "R2 (Rectification Transform 2) (" << type_string(R2) << "):\n" << R2 << std::endl;
    std::cout << "P1 (Projection Matrix 1) (" << type_string(P1) << "):\n" << P1 << std::endl;
    std::cout << "P2 (Projection Matrix 2) (" << type_string(P2) << "):\n" << P2 << std::endl;
    std::cout << "Q (" << type_string(Q) << "):\n" << Q << std::endl;
    std::cout << "after stereo rectification: " << std::endl;

    auto last_execution_time = std::chrono::high_resolution_clock::now();
    auto now = std::chrono::high_resolution_clock::now();
    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_execution_time);

    while (true)
    {
        osalHandler->TaskSleepMs(1);

#ifdef OPEN_CV_INSTALLED
        /*! Get data here */
        osalHandler->MutexLock(pack->mutex);
        if (!pack->gotData)
        {
            osalHandler->MutexUnlock(pack->mutex);
            continue;
        }

        cv::Mat image = cv::Mat(pack->info.rawInfo.height, pack->info.rawInfo.width, CV_8U);
        int copySize = pack->info.rawInfo.height * pack->info.rawInfo.width;
        if (pack->imageRawBuffer)
        {
            memcpy(image.data, pack->imageRawBuffer, copySize);
            osalHandler->Free(pack->imageRawBuffer);
            pack->imageRawBuffer = NULL;
        }

        for (i = 0; i < sizeof(positionName) / sizeof(T_DjiTestPerceptionCameraPositionName); ++i)
        {
            if (positionName[i].cameraPosition == pack->info.dataType)
            {
                if( i % 2 == 0 )
                {
                    image_l = image.clone();
                }
                else
                {
                    image_r = image.clone();
                }

                sprintf(nameStr, "Image position: %s", positionName[i].name);
                break;
            }
        }

        pack->gotData = false;
        osalHandler->MutexUnlock(pack->mutex);

        if( ! image_l.empty() && ! image_r.empty() )
        {
            now = std::chrono::high_resolution_clock::now();
            duration = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_execution_time);

            if( duration.count() > 100 )
            {
                last_execution_time = now;

                cv::Mat left_for_matcher, right_for_matcher;
                cv::Mat left_disp, right_disp;
                cv::Mat filtered_disp;
                left_for_matcher = image_l.clone();
                right_for_matcher = image_r.clone();

                int max_disp = 128;
                int wsize = 19;

                cv::Ptr<cv::StereoBM> left_matcher = cv::StereoBM::create(max_disp, wsize);
                cv::Ptr<cv::StereoMatcher> right_matcher = cv::ximgproc::createRightMatcher(left_matcher);
                cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls_filter;
//  cv::Ptr<cv::ximgproc::DisparityWLSFilter> wlsFilter = cv::ximgproc::createDisparityWLSFilter(stereoBM);

//              Ptr<StereoSGBM> left_matcher  = StereoSGBM::create(0,max_disp,wsize);
//              left_matcher->setP1(24*wsize*wsize);
//              left_matcher->setP2(96*wsize*wsize);
//              left_matcher->setPreFilterCap(63);
//              left_matcher->setMode(StereoSGBM::MODE_SGBM_3WAY);
//              wls_filter = createDisparityWLSFilter(left_matcher);
//              Ptr<StereoMatcher> right_matcher = createRightMatcher(left_matcher);

                left_matcher-> compute(left_for_matcher,  right_for_matcher, left_disp);
                right_matcher->compute(right_for_matcher, left_for_matcher,  right_disp);

                Mat conf_map;
                conf_map.create(image_l.size(), CV_8U);
                conf_map = Scalar(255);
                double lambda = 8000;
                double sigma  = 1.5;
                wls_filter = cv::ximgproc::createDisparityWLSFilter(left_matcher);
//              wls_filter->setLambda(lambda);
//                  wls_filter->setSigmaColor(sigma);
                wls_filter->filter(left_disp, image_l, filtered_disp, right_disp);
//              wlsFilter->filter(disparity_left, down_l, filtered_disparity, -1 * disparity_right); // -16 ???

                conf_map = wls_filter->getConfidenceMap();

                double fbs_spatial = 16;
                double fbs_luma    = 8.0;
                double fbs_chroma  = 8.0;
                double fbs_lambda   = 0.5;
                cv::Mat solved_disp;
                fastBilateralSolverFilter(image_l, left_disp, conf_map/255.0f, solved_disp, fbs_spatial, fbs_luma, fbs_chroma, fbs_lambda);

                cv::Mat depth;
                cv::reprojectImageTo3D(solved_disp, depth, Q, true, CV_32F);

                std::vector<cv::Mat> channels(3);
                cv::split(depth, channels); // Split the 3 channels into separate single-channel images
                cv::Mat z_channel = channels[2];
                z_channel.setTo(0, z_channel == -std::numeric_limits<float>::infinity());
                z_channel.setTo(0, z_channel ==  std::numeric_limits<float>::infinity());
                z_channel.setTo(0, z_channel !=  z_channel);

            // Get the center point of the image
            int centerX = z_channel.cols / 2;
            int centerY = z_channel.rows / 2;

            // Retrieve the depth value at the center point
            float centerDepthValue = z_channel.at<float>(centerY, centerX);
            std::cout << "Depth value at center (" << centerX << ", " << centerY << "): " << centerDepthValue << " meters" << std::endl;

                double minVal, maxVal;
                cv::minMaxLoc(z_channel, &minVal, &maxVal);
                std::cout << "z_channel Min: " << minVal << " Max: " << maxVal << std::endl;

                cv::Mat visualization;
                //cv::normalize(z_channel, visualization, 0, 255, cv::NORM_MINMAX, CV_8U);
//              cv::normalize(z_channel, visualization, 0, 255, cv::NORM_MINMAX, CV_32F);
//              visualization.convertTo(visualization, CV_8U);

//              getDisparityVis(filtered_disp, visualization, 50);
                getDisparityVis(solved_disp, visualization, 5);
//              getDisparityVis(z_channel, visualization, 5);

                cv::Mat visualization_rgb;
                cv::cvtColor(visualization, visualization_rgb, cv::COLOR_GRAY2RGB);
                            // Draw a circle at the center point
            cv::circle(visualization_rgb, cv::Point(centerX, centerY), 5, cv::Scalar(0, 255, 0), -1);

            // Label the depth value
            std::string depthLabel = "Depth: " + std::to_string(centerDepthValue) + "m";
            cv::putText(visualization_rgb, depthLabel, cv::Point(centerX + 10, centerY - 10), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0), 1);

                //getDisparityVis(z_channel, visualization, 1);
                cv::imshow("visualization", visualization_rgb);
            }

            image_l.release();
            image_r.release();
            disparity.release();
            disparity_right.release();
            filtered_disparity.release();
            depth.release();
        }

        cv::waitKey(1);
#else
        osalHandler->TaskSleepMs(1000);
        USER_LOG_WARN("Please install opencv to run this stereo image display sample.");
#endif
    }
}

[dji-dev]

Agent comment from Leon in Zendesk ticket #116330:

Dear developer,

Hello, thank you for contacting DJI.

They are corrected. Do you want to obtain depth information? For depth information, we provide a specific process.

Common business steps: RAW image -> camera internal parameters (dedistortion to obtain correction image) -> calculate disparity map -> use camera external parameters (calculate depth map)

You can refer to the early OSDK Sample: https://github.com/dji-sdk/Onboard-SDK/blob/master/sample/platform/linux/advanced- sensing/stereo_vision_depth_perception_sample/stereo_vision_depth_perception_sample.cpp

Thank you for your support of DJI products! Wish you all the best!

Best Regards, DJI SDK Technical Support

°°°