ICP covariance calculation failure.

[nethdeco]

Hi, thank you publicly sharing your work.

I have tested your default data sets and they have been working fine. Then I have been trying to integrate the system with one of our robots which is a 2 wheel differential driven robot with two casters with a Velodyne VLP-16 and an IMU running at 20Hz. We were testing in a warehouse which is a n50m X 30m environment.

But this time I get an error ""Failed to find eigen values when computing ICP covariance. " which seems the localization is failing. Is there any pointers to fix this issue.

The rosbag file can be downloaded from here

sensors.yaml file is as follows.

velodyne: name: velodyne parent: arch position: x: -0.17 y: 0.0 z: 0.50 orientation: r: 0.0 p: 0.0 y: 3.14 rate: 10 topic_name: velodyne_points vn100: name: vn100 parent: arch topic_name: imu/data position: x: 0.20 y: 0.00 z: 0.23 orientation: r: 0 p: 0 y: 1.571 rate: 20

localization parameters are as follows.

localization: registration_method: gicp compute_icp_covariance: true icp_covariance_method: 1 icp_max_covariance: 0.01 compute_icp_observability: false tf_epsilon: 0.00001 corr_dist: 0.5 iterations: 25 transform_thresholding: true max_translation: 1.0 max_rotation: 1.0 enable_timing_output: false normal_search_radius: 10 recompute_covariance_local_map: false recompute_covariance_scan: false

robot description

<xacro:include filename="$(find sensor_description)/urdf/sensors/VLP-16.urdf.xacro" />

<xacro:VLP-16
parent="${sensor_props['velodyne']['parent']}"
name="${sensor_props['velodyne']['name']}"
topic="${sensor_props['velodyne']['topic_name']}"
hz="${sensor_props['velodyne']['rate']}"
samples="440"
noise="0.016"
gpu="true">

</xacro:VLP-16>

<xacro:include filename="$(find sensor_description)/urdf/sensors/imu_vn100.urdf.xacro"/>
<xacro:vn100
robot_namespace="$(arg robot_namespace)"
name="${sensor_props['vn100']['name']}"
topic_name="${sensor_props['vn100']['topic_name']}"
hz="${sensor_props['vn100']['rate']}"
parent="imu_link">

</xacro:vn100>

Lo_settings has been updated to match the frames of the datalogs.

`frame_id:
fixed: $(arg robot_namespace)/map
base: $(arg robot_namespace)/base_link
imu: imu_link
bd_odometry: odom
artifacts_in_global: true

frame_conversions:
b_convert_imu_to_base_link_frame: true`

[BenjaminMorrell]

Hi @nethdeco - thanks for testing out LOCUS.

The covariance is not strictly needed in LOCUS, rather it is required when combined with LAMP where we do global pose-graph optimization. https://github.com/NeBula-Autonomy/LAMP

Hence for a short-term solution, change the parameter compute_icp_covariance to false and it will skip that computation.

I don't think that error you are seeing will actually stop locus, it will just return zeros in the covariance, but we need to look more closely.

Let me know if that short term fix I mention is enough for now, otherwise we'll push harder to go into the issue in more details.

Cheers,

Ben

[nethdeco]

Thank you for the solution posted. Sorry for the bit delayed reply.

I have tested what you suggested. I was able to get a result with 3 ---> odometry integration. In order to get the result I had to set b_integrate_interpolated_odom & b_pub_odom_on_timer to false.
The map is good, but there are overlapping sections. I think having accurate IMU data should fix this.

During the execution i also got following messages.

Dropped 39 scans over 5s --> drop rate is 7.8 scans/s
itrTime Points to buffer end

If seems the node is dropping frames.
Velodyne frame of my data set is set as velodyne. Should this be set to /robot/velodyne as done on the test data ?

Somehow Still i could not integrate the IMU data.
At the beginning there is an error claiming it could not find imu_link
imu_link" passed to lookupTransform argument target_frame does not exist.
But it also says Received imu_msg is correctly expressed in imu frame

[femust]

Dropped 39 scans over 5s --> drop rate is 7.8 scans/s
itrTime Points to buffer end

this may be connected to the fact that the step of Locus (in the LidarCallback function) has some problems with the optimization of two scans or the map (more likely). We have this flag b_enable_computation_time_profiling you can set it True and record the corresponding topics to see what takes the most time, how to solve it? Reduce the optimization step, number of iterations or actually we should use icp covariance calculation to not perform this calculation (this will be for future work).

Somehow Still i could not integrate the IMU data.
At the beginning there is an error claiming it could not find imu_link
imu_link" passed to lookupTransform argument target_frame does not exist.
But it also says Received imu_msg is correctly expressed in imu frame

maybe something in your robot description, try to run

rosrun rqt_tf_tree rqt_tf_tree

if imu is not expressed in base_link then set

b_convert_imu_to_base_link_frame: true

in lo_frames.yaml