Pointclouds get dropped

[oskarmue]

Thank you for your great work with this package, it looks really promising! I recorded a 3d Pointcloud map of our environment beforhand. And now I'd like to lokalize our robot in said map. Therefore I wanted to use the ynamic_robot_localization_system_6dof.launch launchfile. But when starting it, after the Map is loaded and global pose estimate is published, all the new Pointclouds get dropped, because:

[ WARN] [1660297010.079226327]: Dropping pointcloud because tf between base_link and odom isn't available Failed to find match for field 'rgb'. Failed to find match for field 'normal_x'. Failed to find match for field 'normal_y'. Failed to find match for field 'normal_z'. Failed to find match for field 'curvature'.

Is that a Problem because of the pointcloud type I use? I am using a robosense Lidar and can decide between XYZIRT and XYZI type. So there is no option for rgb... . Am I still able to use this package somehow?

[carlosmccosta]

Hello,

If you do not have odometry, then you need to set the odom_frame_id to be the frame as your base_link_frame_id.

Have a nice day :)

[oskarmue]

Hey, thank you, that solved the first error :) now I get these pcl errors. Is there some configuration I have to do? [pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?

[carlosmccosta]

Hello,

That error seems to be from sac_model_registration.h#L248, in which the covariance matrix is computed with centroid.hpp#L508.

That class is used in drl in cloud_matcher.hpp#L65 if the max_number_of_ransac_iterations > 0, which is the default for recovery_3d.yaml#L12 (you can set it to 0 to disable the usage of ransac within ICP) and also in sample_consensus_prerejective.hpp#L542

drl removes NaN points from the point clouds when it receives sensor data (localization.hpp#L1953) and after normal estimation (localization.hpp#L2875).

As such, it is odd that the pcl function computeMeanAndCovarianceMatrix is generating a covariance matrix with NaN values.

Keep in mind that the dynamic_robot_localization_system_6dof.launch does not have the initial pose estimation module loaded by default and as such, it expects the end user to specify the initial pose either in that launch file dynamic_robot_localization_system_6dof.launch#L23 or given in a topic dynamic_robot_localization_system.launch#L4

Alternatively, you can enable the initial pose module by commenting the line with the empty.yaml (dynamic_robot_localization_system_6dof.launch#L44) and uncommenting the line below (dynamic_robot_localization_system_6dof.launch#L45)

The 3d reference point cloud can be provided in reference_pointcloud_filename or in reference_pointcloud_topic.

I tested the 6 DoF tracking with the ETHZ ASL dataset using their high complexity environments rosbags. One of the tests can be started with ethzasl_kinect_dataset_high_complexity_slow_fly_movement.launch which should have results similar to this video

If your sensor data has a scale that is very different (objects much smaller or larger than the ones present in the video above), you may need to adjust the search radius / k / correspondence distance values of the files: initial_pose_estimation_3d.yaml, cluttered_environments_dynamic_large_map_3d.yaml and recovery_3d.yaml

Have a nice day :)