luxonis / depthai-core

DepthAI C++ Library
MIT License
238 stars 128 forks source link

[BUG] {OAK-D-PRO-W POE Stereo Depth Estimation Is Not Correct} #631

Open samialperen opened 2 years ago

samialperen commented 2 years ago

First, let me explain my setup. I am using OAK-D-PRO-W POE to generate depth based on stereo. Here is the related part (I modified stereo_inertial_publisher.cpp in depthai-ros)

    // MonoCamera
    monoLeft->setResolution(monoResolution); // mono resolution is 720p
    monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
    monoLeft->setFps(stereo_fps);  // Stereo fps is 40
    monoRight->setResolution(monoResolution);
    monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
    monoRight->setFps(stereo_fps);

    // StereoDepth
    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_ACCURACY);
    stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
    stereo->initialConfig.setConfidenceThreshold(confidence);        // Known to be best // confidence is 90
    stereo->setRectifyEdgeFillColor(0);                              // black, to better see the cutout
    stereo->initialConfig.setLeftRightCheckThreshold(LRchecktresh);  // Known to be best
    stereo->setLeftRightCheck(lrcheck); //lr check is 1 
    stereo->setExtendedDisparity(extended); //false
    stereo->setSubpixel(subpixel); // true

I am generating point clouds based on depth using rtabmap_ros/point_cloud_xyz nodelet in ROS

    <arg name="gen_cloud"                   default="true"/> <!-- Generate Point Cloud -->
    <arg name="gen_queue_size"              default="100"/>
    <arg name="gen_approx_sync"             default="false"/> > 
    <arg name="gen_decimation"              default="4"/>
    <arg name="gen_voxel_size"              default="0.00"/>
    <arg name="gen_min_depth"               default="0.0"/>
    <arg name="gen_max_depth"               default="5.0"/>
    <arg name="gen_noise_filter_radius"     default="0.1"/> <!-- play with noise filtering more-->
    <arg name="gen_noise_filter_min_neighbors" default="10"/>
    <arg name="gen_normal_k"                default="0"/> <!-- check normal filtering-->
    <arg name="gen_normal_radius"           default="0.0"/>
    <arg name="gen_filter_nans"             default="true"/>
    <arg name="gen_roi_ratios"              default="0.0 0.0 0.0 0.0"/> <!-- this helped to get rid of noise part close to camera at top-->

    <!-- Convert Compressed Depth to Depth (Only for rosbags)-->
    <param if="$(arg use_rosbag)" name="/use_sim_time" value="true"/>
    <node if="$(arg use_rosbag)" pkg="image_transport" type="republish" name="republish_depth" args="compressedDepth in:=/stereo_inertial_publisher/stereo/image raw out:=/stereo_inertial_publisher/stereo/image"/>

    <node pkg="nodelet" type="nodelet" name="oak_nodelet"  args="manager"/>

    <!-- Create Point Cloud from Depth -->
    <node if="$(arg gen_cloud)" pkg="nodelet" type="nodelet" name="gen_cloud_from_depth" args="load rtabmap_ros/point_cloud_xyz oak_nodelet">
        <remap from="depth/image"       to="/stereo_inertial_publisher/stereo/image"/> 
        <remap from="depth/camera_info" to="/stereo_inertial_publisher/stereo/camera_info"/>
        <remap from="cloud"             to="/oak/point_cloud" />

        <param name="queue_size"                type="int"    value="$(arg gen_queue_size)"/>
        <param name="approx_sync"               type="bool"   value="$(arg gen_approx_sync)"/>
        <param name="decimation"                type="double" value="$(arg gen_decimation)"/>
        <param name="voxel_size"                type="double" value="$(arg gen_voxel_size)"/>
        <param name="min_depth"                 type="double" value="$(arg gen_min_depth)"/>
        <param name="max_depth"                 type="double" value="$(arg gen_max_depth)"/>
        <param name="noise_filter_radius"       type="double" value="$(arg gen_noise_filter_radius)"/>
        <param name="noise_filter_min_radius"   type="int" value="$(arg gen_noise_filter_min_neighbors)"/>
        <param name="normal_k"                  type="int" value="$(arg gen_normal_k)"/>
        <param name="normal_radius"             type="double" value="$(arg gen_normal_radius)"/>
        <param name="filter_nans"               type="bool" value="$(arg gen_filter_nans)"/>
        <param name="roi_ratios"                type="string" value="$(arg gen_roi_ratios)"/>
    </node>

Here is the problem. I see points in the cloud that should not be there. Here are some screenshots.

Shot 1: I do not know why it is seeing stuff significantly above the ground, but bear with me this is the least weird one. Screenshot from 2022-11-04 11-55-03

Shot 2: See the points on the right of the axes? What is it? There is nothing there :(
Screenshot from 2022-11-04 11-50-12

Shot 3: Again there are some stuff on the right that should not be there at all. (Do not worry about yellow points they are also part of point cloud. They are just clustered as obstacles (no need to worry for this bug. Think them as white too) Screenshot from 2022-11-04 11-45-57

Can someone help me out what is the issue here? I feel like there is a problem with the camera extrinsic. Do I need to calibrate it? Thanks a lot for the support! :)

SzabolcsGergely commented 2 years ago

@samialperen There are multiple issues.

  1. Stereo depth estimation highly depends on the quality of input images. In your case the images are overexposed, which leads to a loss of detail, thus bad depth estimation.
  2. OAK-D-PRO-W camera sensors are without IR-filter, because it comes with an active illuminator (dot-projector). This is not suitable for outdoor use.
  3. The buildings you are trying to measure are far away (20+ meters from what it looks like). OAK-D-PRO-W-POE is suitable for up to 10 meters, but ideally 5-6. See accuracy evaluation. This is due to 7.5 cm baseline and high FOV. The higher the FOV, the shorter the maximum perceived distance by stereo depth.

If you want depth for longer distances you need narrower FOV, higher baseline and IR-filter (for outdoor use).

samialperen commented 2 years ago

Hi @szabi-luxonis,

Thanks a lot for the response.

1-) Yes, naturally. I was wondering whether there is a way to improve image quality, too, so that we can have a better depth. 2-) Is there a version that supports outdoor use? We selected this version since we want to use it for outdoor during the night time and IR flood light coming with this version seemed appealing to us. At the time of taking these screenshots, both flood light and dot projector were on with 400mA. 3-) Actually, I am not trying to measure any of those buildings. The main case for my application is to use camera for obstacle avoidance. At the moment, I am generating point clouds up to 5 meters and I am fine with that. Those buildings are just in the background during the screenshot, but the points I am getting have nothing to do with those buildings since I am not getting any points that are far away than 5 meters.

At the moment, I can not change the camera, so I am trying to make the best out of it. What do you suggest? Let's say one for night time outdoor, and the other one for indoor outdoor environments.

Thanks a lot again for the support! :)

SzabolcsGergely commented 2 years ago

@samialperen

This is a physical limitation. There is too much light reaching the sensor, which results in oversaturation. Wide-angle lenses will let in more light, compared to narrow ones. You can use a third-party IR light filter and mount it on the front of the lens to reduce ambient light.

samialperen commented 2 years ago

I see! Thanks a lot for all the information. Highly appreciated. I will share more testing results for night time to see how it performs during the night. If it is better than day light, I will accept the physical limitation and try to find a work around for that (like buying an IR filter as you suggested)

samialperen commented 2 years ago

One of the easiest solutions I have tried is enabling manual exposure following the recent commit to depthai-core: https://github.com/luxonis/depthai-ros/commit/9ac79711a7ae247588a86c75911ddb306e03617f

Auto Exposure: autoexp

Manual exposure with exposure time 10 and ISO 100 time5iso100

Does anyone know what else I can do to make it better using CameraControl node: https://docs.luxonis.com/projects/api/en/latest/components/messages/camera_control/

samialperen commented 2 years ago

@szabi-luxonis Do you know if an external IR filter will affect performance for the night time? How can I order a filter that will not interfere with IR flood light for night vision?

samialperen commented 2 years ago

@szabi-luxonis

Although I am using HIGH_DENSITY in a situation without over exposure, still the depth cloud is not great at all. You can see the picture below. It is hard to tell whether there is a person from the depth cloud. MicrosoftTeams-image (1)

these are the stereo settings I used at that time MicrosoftTeams-image (2)

samialperen commented 2 years ago

Here is a different perspective MicrosoftTeams-image (3)