Getting point cloud with ZED2 doesn't work

I try to get point cloud using my ZED2. The cloud has a lot of noise. My code is:


#include <sl/Camera.hpp>
#include <chrono>

using namespace std;
using namespace sl;




int main(int argc, char** argv) {
    Camera zed;
    POSITIONAL_TRACKING_STATE tracking_state = POSITIONAL_TRACKING_STATE::OFF;
    // Set configuration parameters for the ZED
    InitParameters init_parameters;
    init_parameters.depth_mode = DEPTH_MODE::ULTRA;
    init_parameters.coordinate_system = COORDINATE_SYSTEM::RIGHT_HANDED_Y_UP; 
    init_parameters.camera_resolution = sl::RESOLUTION::HD1080;
    init_parameters.depth_maximum_distance = 2;
    init_parameters.depth_minimum_distance = 0.2;
    init_parameters.coordinate_units = sl::UNIT::METER;
    
    PositionalTrackingParameters track;
    track.enable_imu_fusion = true;
    // Open the camera
    auto returned_state = zed.open(init_parameters);
    zed.enablePositionalTracking(track);
    if (returned_state != ERROR_CODE::SUCCESS) {
        return -1;
    }

    auto camera_config = zed.getCameraInformation().camera_configuration;


    RuntimeParameters runParameters;
    // Setting the depth confidence parameters
    runParameters.confidence_threshold = 50;
    runParameters.texture_confidence_threshold = 100;
    runParameters.sensing_mode = SENSING_MODE::FILL;

    // Allocation of 4 channels of float on GPU
    Mat point_cloud(camera_config.resolution, MAT_TYPE::F32_C4, MEM::GPU);
    auto first_time_counter = std::chrono::high_resolution_clock::now();
    // Main Loop
    std::cout << "Start" << std::endl;
    while (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - first_time_counter).count() < 4000) {
        // Check that a new image is successfully acquired
        if (zed.grab(runParameters) == ERROR_CODE::SUCCESS) {
            // retrieve the current 3D coloread point cloud in GPU
            zed.retrieveMeasure(point_cloud, MEASURE::XYZRGBA, MEM::GPU);
        }
    }
    // free allocated memory before closing the ZED
    point_cloud.write("point_cloud.ply", MEM::GPU);
    point_cloud.free();
    std::cout << "end" << std::endl;
    // close the ZED
    zed.close();

    return 0;
}

Why is it happening?

Hello and thank you for reaching us out,

A few ideas :

  • The point cloud at each frame can be noisy. Most algorithms (like our positional tracking, SLAM, etc.) will use a mesh built from several frames.
  • The light can also have a huge impact, darkness will not be your friend (That does not seem to be the case here though)
  • You can use NEURAL mode to reduce noise.
  • You probably want to use the standard sensing mode. FILL is made for game engine and augmented reality, which cannot handle having holes in meshes.
  • What happens when you tune the confidences thresholds ?

Best regards

Antoine Lassagne
Senior Developer - ZED SDK
Stereolabs Support

Hello. I decreased the confidences thresholds and the cloud began to look like it had its edges cut off, when i increased the coefficient a new artifact appeared. I set depth maximum distance for 3 meters and it helps me to avoid big emissions. Also I change DEPT MODE and it become better. But its still can be artifacts in cloud and it depends on initial camera position. Cloud outliers can still appear if scene selection is somehow unsuccessful.
Cloud with old settings:

Cloud with depth maximum distance setting to 3 meter:

Cloud with depth maximum distance setting to 3 meter and confidences threshold to 60:

Cloud with depth maximum distance setting to 3 meter and confidences threshold to 40:

So, my best result with depth maximum distance setting to 3 meter and confidences threshold to 50, but it still have outliers. Scene that i was mapping looks like:

Hello again,

Your point cloud seems a lot better. There are still a few things you could try, like checking if the depth_stabilization is ON.
Also, you’ll have a much better result if you merge the point clouds from several frames. We do that in the spatial mapping example : https://github.com/stereolabs/zed-examples/tree/master/spatial%20mapping/advanced%20point%20cloud%20mapping

Regards

Antoine Lassagne
Senior Developer - ZED SDK
Stereolabs Support

Ok, I’ll try to deal with code
Thank you