Not getting valid depth from stereo pointcloud data



  • I'm trying to work back on why our collision prevention does not work as expected, and I noticed that we're not getting back a valid depth from stereo point cloud.

    I've saved some more images from DFS server, that I've attached here. The weird thing is that I can see disparity points in the raw image, but they all seem to get filtered out, leaving little to no features to measure. I'm wondering how normal that is, what I'm expected to see in disparity images, and whether my environment is in good condition for this experiment.

    I can follow up with the ROS pointcloud recording for your reference, but they're basically all inf or -inf values as such:

    4684.26830914,-inf,inf,10000.0

    Thanks in advance

    Stereo right:
    stereo_right.jpg

    Stereo left:
    stereo_left.jpg

    Disparity raw:
    disparity_raw.jpg

    Disparity scaled after blur:
    disparity_scaled_after_blur.jpg

    Disparity filtered after blur:
    disparity_filtered_after_blur.jpg


  • Dev Team

    Hi Lynn,

    It's normal for the DFS pointcloud to be very sparse with our default settings. As you can see in the filtered images, there's very little usable data in environments with flat white walls. We find that the filters help remove a lot of the noise that you can see in the raw disparity image, but it doesn't usually look quite so sparse. My two recommendations for you would be to

    A) Refocus and recalibrate your stereo cameras. It looks like your right stereo image is slightly out of focus, which won't help, and any small changes to the orientation of the cameras with regards to each other will lead to significant issues with dfs, they'll need a strong calibration. The lenses themselves tend not to change over time at all, so you can likely skip the intrinsics portion of the stereo cal process and skip straight to the extrinsics.

    B) Play with the filter settings in /etc/modalai/voxl-dfs-server.conf to your liking. By default, we apply a 17-pixel median filter and then a second 7-pixel median filter, which is a pretty heavy filter process and is intended to produce low-res/low-noise/obstacle avoidance friendly images, not high-res disparity images. You can remove the second filter entirely and/or lower either of the values to have something closer to the original image based on your use case.



  • Hi Alex,

    Thank you for your response! I tried 2 things:

    1. I attempted to calibrate the stereo extrinsics, and got the following error:
    voxl-calibrate-camera stereo -s 9x6 -l 0.029 -e
    Sampling overlapping images now
    Enter an empty line to take a sample or "stop" to finish sampling
    
    Taking Frame
    
    Taking Frame
    stop
    Stopping sampling
    Pulling existing intrinsics from: /data/modalai/opencv_stereo_intrinsics.yml
    
    Matrix L
    [497.7006450187309, 0, 295.8446445528053;
     0, 496.3585448760351, 231.712917372481;
     0, 0, 1]
    Distortion L
    [-0.1865369023976547;
     0.1016650425512336;
     0.0001161942987197076;
     0.004382065643093006;
     0]
    Matrix R
    [490.0228530745731, 0, 373.8964558150473;
     0, 489.1449020535995, 247.3517837095496;
     0, 0, 1]
    Distortion R
    [-0.2086578047150433;
     0.151676525578168;
     -0.001749483285398663;
     -0.00269335419013983;
     0]
    
    Calibrating Extrinsics
    2 frames will be processed
    R
    [0.9913194969599594, 0.03829151497302103, -0.1257752552298109;
     -0.04342180569409582, 0.9983222939882923, -0.038303317300436;
     0.1240975492800618, 0.04343221393188008, 0.9913190410032776]
    T
    [-0.1452994050346482;
     -0.007246953931032494;
     -0.0001531663907812685]
    Re-projection error reported by stereoCalibrate: 5.086737
    Extrinsics Calibration Failed
    Max reprojection error:  0.5
    
    Exiting Cleanly
    

    Here are my intrinsics:

    %YAML:1.0
    ---
    M1: !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [ 4.9770064501873094e+02, 0., 2.9584464455280528e+02, 0.,
           4.9635854487603513e+02, 2.3171291737248100e+02, 0., 0., 1. ]
    D1: !!opencv-matrix
       rows: 5
       cols: 1
       dt: d
       data: [ -1.8653690239765469e-01, 1.0166504255123363e-01,
           1.1619429871970763e-04, 4.3820656430930060e-03, 0. ]
    M2: !!opencv-matrix
       rows: 3
       cols: 3
       dt: d
       data: [ 4.9002285307457311e+02, 0., 3.7389645581504732e+02, 0.,
           4.8914490205359948e+02, 2.4735178370954961e+02, 0., 0., 1. ]
    D2: !!opencv-matrix
       rows: 5
       cols: 1
       dt: d
       data: [ -2.0865780471504333e-01, 1.5167652557816802e-01,
           -1.7494832853986628e-03, -2.6933541901398303e-03, 0. ]
    distortion_model: plumb_bob
    width: 640.
    height: 480.
    
    1. I also played with the dfs configs some more and looked at the depth pointcloud again, and I know this might be fixed when calibration is done, but it seems very wrong as there are way too many points close to the drone.

    Here's my updated dfs config:

    {
    	"skip_n_frames":	5,
    	"n_disparity_levels":	24,
    	"sobel_cutoff":	5,
    	"first_median_filter_size":	7,
    	"en_second_median_filter":	false,
    	"second_median_filter_size":	7
    }
    

    Here's what I'm seeing:
    DFS points.png

    Any thoughts on this?

    Thanks!



  • Update

    I was able to recalibrate the cameras, but the proximity data still seems off. Here's what I'm seeing:

    DFS Cloud and proximity.png

    Note that the white pointcloud is the DFS disparity pointcloud. The red points are the points that were considered the closest to the drone when calculating the closest obstacle in voa_manager. I'm simply doing the following:

    // calculate distance in cm to send to px4
    double x = T_point_wrt_level.d[0];
    double y = T_point_wrt_level.d[1];
    double z = T_point_wrt_stereo.d[2];
    

    .....

    // record if this point is closer than previously detected obstacle in this bin
    if(dist_cm < distances[bin]) {
    	 distances[bin] = dist_cm;
    
    	if(send_linescan) {
    		point_cloud_points[bin] = x;
    		point_cloud_points[bin+1] = y;
    		point_cloud_points[bin+2] = z;
    	}
    }
    

    And then sending the point_cloud_points data over a new pipe that gets recognised automatically by voxl_mpa_to_ros

    Any thoughts?


Log in to reply