Point Cloud miss-alignment while rotation

[wienans]

Hey,
i installed mcl_3dl couple of months ago with apt install (so assuming i installed 0.5.1 but i am not sure). Everything was working fine. While rotation the velodyne pointclount aligns well with the map.
Now i updated the mcl_3dl with apt to 0.5.3 and while rotation the velodyne pointcloud rotates in respect to the map, which hints that position estimation is not correct. The position is only corrected over time to align the pointcloud again.
After this i installed Version 0.5.1 from source to double check if it was working with earlier versions like intended. And with 0.5.1 build from source it worked again like before updating the package. So i think there is something happening between this two builds which might be contra productive for localization performance.

I didn't tested 0.5.2.

[wienans]

Okay let me correct a bit. with older version it wasn't perfect too. But i didn't noticed it so much. After increasing size of points in rviz it became noticeable. So this miss-alignment got worse with Version 0.5.3 and 0.5.1 had some of miss-alignment too.

[at-wat]

@wienans is it possible to provide a bag file and launch file? I don't have velodyne, so it will make it easier to investigate.

[wienans]

Hey i don't think it is about velodyne, because there is no big difference to other point cloud data. And also maybe it is something with my config files. I tried to understand the different parameters but with less documentation of the parameters it is very hard.
Here you find data.tar.xz which includes launch bag config and .pcd file:
https://www.file-upload.net/download-14508289/data.tar.xz.html

[wienans]

Hey @at-wat ,
i think i found a other problem which causes lag in the estimation in respect to other packages.
Next to mcl_3dl i used robot_localization which received the mcl_3dl position and fuses potential other data. I found that over time the time stamp of mcl_3dl /amcl_pose starts lagging behind the robot_localization estimation. And the lag increases over time. (The provided bag file is to short to see the big lag happening)
I shortly looked into the code and found that you added a buffer for the 'cloud' topic from 100. Because the package couldn't process the 20 Hz point cloud fast enough the messages wait in the buffer and get processed after finishing the current message. Because at the time point the second message gets processed it is already old (newer messages are available) a lag of mcl_3dl position estimation emerges. I reduced the buffer to 1 (using only the newest message for processing) and the lag was gone. Didn't checked if this also helps for the above described problem.
sub_cloud_ = mcl_3dl_compat::subscribe( nh_, "cloud", pnh_, "cloud", 1, &MCL3dlNode::cbCloud, this);

[at-wat]

It seems better to make the buffer size settable by node parameter.

[wienans]

Yes i think thats a good idea. Would be nice if you also increase your documentation of the parameters. :)

[HappySamuel]

Hi @wienans

For solving the pointcloud miss-alignment issue while rotating the lidar, we just need to set the queue_size of sub_cloud_ to 1 ?
Anything else need to adjust to avoid this miss-alignment issue?

Best,
Samuel

[wienans]

Hi @HappySamuel i switched to a other package for localization so I didn’t follow up on this. Reducing the quesize to 1 will defiantly help as no lag will be accumulated but I didn’t test regarding rotational error. Now I use hdl_localization package

[HappySamuel]

Hi @wienans

How does hdl_localization package do when comparing with mcl_3dl? Is it a lot better?

Best,
Samuel

[wienans]

@HappySamuel
ist very Good but also computational costly. But I think this shouldn’t be discussed here

best
Sven