Scanning with manipulator

[kevin_s]

Hello,

I am in the process of using a UR5 with L515/D455 in order to scan a section of a room. The robot is stationary and has the camera_link at the end, which is specified in the launch file.

My parameters and remappings are as follows:

    parameters=[{
          'frame_id':'camera_link',
          'subscribe_depth':True,
          'subscribe_odom_info':False,
          'approx_sync':True,
          'queue_size':1000,
          'wait_for_transform':0.3}]

    remappings=[
          ('rgb/image', '/camera/color/image_raw'),
          ('rgb/camera_info', '/camera/color/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')]

and I am currently launching these nodes.

        Node(
            package='rtabmap_slam', executable='rtabmap', output='screen',
            parameters=parameters,
            remappings=remappings,
            arguments=['-d']),

        Node(
            package='rtabmap_viz', executable='rtabmap_viz', output='screen',
            parameters=parameters,
            remappings=remappings)

Are my mappings correct regarding the frame_id?
If all of the transforms are accounted for, would I benefit from launching a rgbd odometry node for minor corrections?
I see that the IMU topic is used in the slam node, does it do anything besides helping odometry?
I sometimes see that the pointcloud wall thickness is quite big, is there settings that could improve this?

[matlabbe]

Hi,

If the robot doesn't move and you have TF available for the motion between base frame (e..g, base_link) and camera frame, I would use frame_id as the base frame and set odom_frame_id as the same base frame (I assume you don't need odometry if the arm is already publishing the pose of the end effector on which the camera is fixed). To accumulate the point clouds, you can set RGBD/LinearUpdate to 0. I would also disable loop closure detection with Kp/MaxFeature=-1 and I would not make rtabmap subscribe to imu, you can remap to something else to not use it.

Example 1:

'frame_id': 'base_link',
'odom_frame_id': 'base_link',
'RGBD/LinearUpdate': '0',
'Kp/MaxFeatures': '-1', # loop closure detection disabled

Example 2, in case you want to keep track of the poses of the camera in the map:

'frame_id': 'camera_link',
'odom_frame_id': 'base_link',
'Kp/MaxFeatures': '-1', # loop closure detection disabled

queue_size should not be that large (default 10), otherwise you have clock/timestamp issues.

For the thick walls, are you referring to D455 cloud or L515? I would expect not too thick wall with L515, unless the TF published by your UR5 robot are wrong, or extrinsic between the camera frame and the end effector frame of the robot arm is not correctly calibrated.

cheers,
Mathieu

[kevin_s]

Hey,

Thanks for the reply.

I did the tf based on a measurement in cad. However, i did try to use the calibration method using aruco nodes and the opencv hand in eye calibration method. I did calibrate it with about 30 different poses, with an aruco marker that is 24,3cm x 24,3cm, do you reckon this could be improved with more poses? I am still experiencing miss-alignment of frames with the l515 with both the calibrated tf's and the measured tf's. With the more accurate pointcloud data and slight miss-alignment, the meshing process turns out to be worse than with the d455.

With the d455, the wall thickness, masks the miss-alignment and I think this is better for meshing with poisson.

Would you recommend using the lidar localization instead, since I am having these issues?

EDIT:
I have tested more and this is now my config:

    parameters=[{
          'frame_id':'camera_link', # What frame it should track odom from (the loop closure graphics)
          'subscribe_scan_cloud':True, # Works well with subscribe_scan_cloud
          'subscribe_depth':False, # Saves the cache, but gives bad data
          'subscribe_odom_info':False,
          'approx_sync':True,
          'queue_size':10, # Was 10000, should be 10
          'Kp/MaxFeatures': '-1',
          'RGBD/LinearUpdate': '0',
          'Icp/VoxelSize' : '0.05',
          'Icp/MaxCorrespondenceDistance' : '0.1',
          'wait_for_transform':0.1}]

    remappings=[
          ('scan_cloud', '/camera/depth/color/points'),
          ('rgb/image', '/camera/color/image_raw'),
          ('rgb/camera_info', '/camera/color/camera_info'),
          ('depth/image', '/camera/depth/image_rect_raw')]

and I get these amazing results which are really flat and consistent, I think it is because it is using ICP from the point cloud loaded with subscribe_scan_cloud:


The only downside is that I can't seem to save the map using the GUI because it does not have any point cloud data in the chache. I get this error:


with this error in the command line:


However, if I set subscribe_depth to True, then it will save the pointclouds in the cache, but the result is not the same, as seen below.


Is there a way to save the pointclouds in the cache, but get the same results as when the subscribe_depth is set to false?

EDIT 2:

I got it working properly, the problem was the remapping, i changed the depth image to the aligned depth image and now it is smooth sailing. here is my remappings now.

    remappings=[
          ('scan_cloud', '/camera/depth/color/points'),
          ('depth/image', '/camera/aligned_depth_to_color/image_raw'),
          ('rgb/image', '/camera/color/image_raw'),
          ('rgb/camera_info', '/camera/color/camera_info')]


 It works perfectly. Thanks again for the first reply, but I do still wonder if it runs ICP or something of the sort behind the scenes?

[matlabbe]

If you don't see any links other than the blue links, there are no loop closures or proximity links added. I see you disabled visual loop closure detection with Kp/MaxFeatures=-1, but if you subscribe to scan_cloud, it may still do proximity detection with the scans.

When exporting point cloud, if there is no depth image recorded, you can check/uncheck the first option in Export dialog to use the scan instead of depth image.

[kevin_s]

Okay,

thank you.