Incorrect ros2 pointcloud

simonvintecc

Hi,

I use the depthai-ros package (installed from binaries on ros2 humble) to connect with my OAK-D-PoE. RGB and depth images all look fine but there is something wrong with the pointcloud.

As you can see there are a lot of blank spaces between the lines of points. When i measure the distance from an object to the camera, it does not correspond to the distance measured by the camera.

I think it is something to do with the conversion from int to float so I tried adding a ConvertMetric from the depth_image_proc but this doesn't help.

Any help on this is welcome! Thanks!

Launch file:

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import LoadComposableNodes
from launch_ros.descriptions import ComposableNode

def launch_setup(context, *args, **kwargs): 
   params_file = LaunchConfiguration("params_file") 
   depthai_prefix = get_package_share_directory("depthai_ros_driver") 
   name = LaunchConfiguration('name').perform(context) 

   return [ 
      IncludeLaunchDescription( 
         PythonLaunchDescriptionSource( 
            os.path.join(depthai_prefix, 'launch', 'camera.launch.py')), 
         launch_arguments={"name": name, 
                           "params_file": params_file, 
                           "parent_frame": LaunchConfiguration("parent_frame"), 
                           "cam_pos_x": LaunchConfiguration("cam_pos_x"), 
                           "cam_pos_y": LaunchConfiguration("cam_pos_y"), 
                           "cam_pos_z": LaunchConfiguration("cam_pos_z"), 
                           "cam_roll": LaunchConfiguration("cam_roll"), 
                           "cam_pitch": LaunchConfiguration("cam_pitch"), 
                           "cam_yaw": LaunchConfiguration("cam_yaw"), 
                           "use_rviz": LaunchConfiguration("use_rviz") }.items()), 
      LoadComposableNodes( 
         target_container=name+"_container", 
         composable_node_descriptions=[ 
            ComposableNode( 
               package='depth_image_proc', 
               plugin='depth_image_proc::ConvertMetricNode', 
               name='convert_metric_node', 
               remappings=[('image_raw', name+'/stereo/image_raw'),
                           ('camera_info', name+'/stereo/camera_info'), 
                           ('image', name+'/stereo/image') 
                           ]), 
            ComposableNode( 
               package='depth_image_proc', 
               plugin='depth_image_proc::PointCloudXyziNode', 
               name='point_cloud_xyzi', 
               remappings=[('depth/image_rect', name+'/stereo/image'), 
                           ('intensity/image_rect', name+'/right/image_rect'), 
                           ('intensity/camera_info', name+'/stereo/camera_info'), 
                           ('points', name+'/points') 
                           ]), 
         ], 
      ), 
   ]

def generate_launch_description(): 
   depthai_prefix = get_package_share_directory("depthai_ros_driver") 
   declared_arguments = [ 
      DeclareLaunchArgument("name", default_value="oak"), 
      DeclareLaunchArgument("parent_frame", default_value="oak-d-base-frame"), 
      DeclareLaunchArgument("cam_pos_x", default_value="0.0"), 
      DeclareLaunchArgument("cam_pos_y", default_value="0.0"), 
      DeclareLaunchArgument("cam_pos_z", default_value="0.0"), 
      DeclareLaunchArgument("cam_roll", default_value="0.0"), 
      DeclareLaunchArgument("cam_pitch", default_value="0.0"), 
      DeclareLaunchArgument("cam_yaw", default_value="0.0"), 
      DeclareLaunchArgument("params_file", default_value=os.path.join(depthai_prefix, 'config', 'pcl.yaml')), 
      DeclareLaunchArgument("use_rviz", default_value="False"), 
   ] 
   
   return LaunchDescription( 
      declared_arguments + [OpaqueFunction(function=launch_setup)] 
   )

jakaskerl

cc @Luxonis-Adam

Luxonis-Adam

Hi, the empty spaces visible between points are due to disparity calculation, you can enable subpixel mode in parameters to get better results, although at the cost of bandwidth (this cannot be run with low_bandwidth mode). Regarding distance errors, what is the difference between measurements and camera data?

simonvintecc

Hi, thanks for the response.

I already use the pcl.yaml config file from the depthai_ros package where the subpixel parameter is set to True. So i guess that subpixel mode is enabled this way?

For a close object at 4.5m, the measured distance by the sensor is 5m, an object further away at 9m is measured by the sensor as 13.5m

jakaskerl

Hi simonvintecc

simonvintecc For a close object at 4.5m, the measured distance by the sensor is 5m, an object further away at 9m is measured by the sensor as 13.5m

10% error at 5m is terrible depth. Can you check with c++ or python API (https://docs.luxonis.com/projects/api/en/latest/samples/SpatialDetection/spatial_location_calculator/). Maybe the camera need reacalibration.

Thanks,
Jaka

simonvintecc

Hi Jaka,

Using the spatial_location_calculator the sensor measures the distance as 5.5m:

From the pointcloud, the object is detected at roughly the same distance as measured with the spatial_location_calculator. In the real world, the object is at 4.5m distance from the sensor.

After I did a new calibration the measured distance was better:

The pointcloud measurement now also matches the real world distance.

Thanks for the help!