Hesai QT128 with Zed2i and external fused Odometry

[matlabbe]

I agree that RGBD/ProximityAngle=0 doesn't make sense for 180 deg lidar, you could put it back to default value. For the number of neighbors, it could be increased to increase the chance of overlap on proximity detection.

[aninath93]

Hi Mathieu,

Thank you Mathieu for the last post, Apologies, had to delete my post as my link was expired. After changing parameters for quick pass through, data looks more better, I just had to keep the Grid/MinGroundHeight to 0.02 as I could have issue to detect peeled labels or any other object laying around. I have attached some pictures and database with upgraded parameters if you like to glance it with my robot picture. On the LIDAR side, currently its 3D printed bracket so it sagged down, did not even realize that it had deflection of about 2.68 degrees, compensated in URDF side but will make a metal bracket soon for this.

Latest Database

I see that rtabmap is detecting Apriltags with external detection and occupancy grids look really good, I have couple questions,

1. Can I use Marker/Priors (so the tag location does not get altered while detecting in map frame and it stays constant ? as I am not using detection inside rtabmap so just curious how can I make this work )
Currently Optimizer/PriorsIgnored = true



2.How could the pose be exported with label ids to know the exact location of that label ?

[matlabbe]

In rtabmap-databaseViewer, the labels are indicated along current node info (main sliders). You can export the poses with IDs with RGB-D format + ID. On ROS, you can call "/rtabmap/list_labels" service to get the node associated with a label:

rosservice call /rtabmap/list_labels 
ids: [1, 49]
labels: 
  - map0
  - test

If you are subscribing to "/rtabmap/mapGraph" data, you could quickly get the pose of the label.

For "Marker/Priors", it can be used when we know the world coordinate of a tag. As soon as the tag will be detected, the whole map will be transformed in tag coordinates. See also this post for an example: https://github.com/introlab/rtabmap_ros/issues/867

The map looks a lot better now:

[aninath93]

Hi Mathieu,

Thank you for the last reply, I appreciate you.

I have three more questions:

1. I was going through your paper Memory Management . Can you please shed some light in terms of how to test this and optimize. (As I will have people and forklift traffic in the map environment after initial mapping phase being dynamic one)

For sending goal poses, to Nav2 planner do we have to use action goal from Rtabmap or is that action lib just an convenience as currently I am using custom BT tree, also apart from mapdata is there any feedback which needs to be connected from Nav2 or other way around ?

2. For Multi Robot Localization and navigation, what would be ideal way to do this  (map will be same across the board of robots), I intend to save map and rtabmap database on server and have robots load and offload instances from there on as needed so the map keeps getting updated for all the robots at same time unless this approach is wrong and each robot needs to have its own database and map file.

a. Namespace of tf2 and all underlying nodes with same shared map frame across ?
OR
b. Namespace tf2, nodes and map frame too so each robot has its own map ?

3. This is something I have noticed, sometimes when I start rtabmap nodes in general I see the Rtabmap time is far away from ROS time which causes it complain about tf2 side, to fix this if I restart my PTP server for LIDAR purposes instead of free spinning, it works fine ? Is there a cross conflict (ptp4l) ?

Also I get error as parse error, Rtabap:cpp 658 for Marker/Priors
'Marker/Priors','1 1.39 -10.06 0.46 0.14 0.00 3.04|2 -0.74 -12.42 0.56 0.00 0.00 2.76|3 3.46 -0.04 0.49 0.06 0.00 -1.58|4 -2.05 -4.46 0.49 2.30 0.07 0.17',

 

Can you please suggest a good camera in your experience, as ZedX i am using does not support for Jetpack 6 yet, thinking to use D457 realsense, but open to your thoughts.

[matlabbe]

aninath93 wrote

1. I was going through your paper Memory Management . Can you please shed some light in terms of how to test this and optimize. (As I will have people and forklift traffic in the map environment after initial mapping phase being dynamic one)

I cannot go over the details of the paper in a post like this, it would take me too much time. Note that memory management implications in most real-world applications (e.g., doing slam in semi-online/offline way and then localization in autonomous navigation) may make the application more complex than it really needs (as shown in the paper, downstream packages like behavior tree or navigation should be aware that the full map won't be always available at any time). If there is a performance issue, I would try to look at the Timing statistics first and remove stuff that you don't need, or do some parameter tuning to use less resources. Some questions to think about if map should be updated over time: does the SLAM need to be ran online?, or the robot could record data during operation and do an offline update aftwards? Should dynamic obstacles be integrated in the SLAM map, or they could be integrated/updated in a downstream global costmap instead?

aninath93 wrote

For sending goal poses, to Nav2 planner do we have to use action goal from Rtabmap or is that action lib just an convenience as currently I am using custom BT tree, also apart from mapdata is there any feedback which needs to be connected from Nav2 or other way around ?

You don't have to use the action goal of rtabmap, unless you want to go to a particular "place" (i.e., a "label" set in the map) or "node" in the map, for which rtabmap will translate for convenience into a pose to send to nav2. We must use it when memory management is enabled (so that nodes are correctly retrieved on navigation, as explained in the paper). nav2 and rtabmap would be connected only by TF and the occupancy grid map.

aninath93 wrote

2. For Multi Robot Localization and navigation, what would be ideal way to do this  (map will be same across the board of robots), I intend to save map and rtabmap database on server and have robots load and offload instances from there on as needed so the map keeps getting updated for all the robots at same time unless this approach is wrong and each robot needs to have its own database and map file.

a. Namespace of tf2 and all underlying nodes with same shared map frame across ?
OR
b. Namespace tf2, nodes and map frame too so each robot has its own map ?

Each robot would need their own instance of rtabmap, which means a copy of the shared map between the robots. You would only need to carefully setup namespace and tf prefix if the robots can see each other through ROS, if they don't, the same launch files could be launched on each robot without any changes.

aninath93 wrote

3. This is something I have noticed, sometimes when I start rtabmap nodes in general I see the Rtabmap time is far away from ROS time which causes it complain about tf2 side, to fix this if I restart my PTP server for LIDAR purposes instead of free spinning, it works fine ? Is there a cross conflict (ptp4l) ?

"Rtabmap time" you are referring is probably timestamp in the received topics. In your case, maybe the ptp connection of the lidar drifts for some reasons from the computer time, making the timestamp in lidar topics not in sync anymore with host time. That would be an issue of inputs given to rtabmap, but not rtabmap itself.

aninath93 wrote

Also I get error as parse error, Rtabap:cpp 658 for Marker/Priors
'Marker/Priors','1 1.39 -10.06 0.46 0.14 0.00 3.04|2 -0.74 -12.42 0.56 0.00 0.00 2.76|3 3.46 -0.04 0.49 0.06 0.00 -1.58|4 -2.05 -4.46 0.49 2.30 0.07 0.17',

They look pretty close to me. To be closer to the priors set, you can adjust (beware that if your real measurements have some error, setting variance too low may make the optimizer failing):

Param: Marker/PriorsVarianceAngular = "0.001"              [Angular variance to set on marker priors.]
Param: Marker/PriorsVarianceLinear = "0.001"               [Linear variance to set on marker priors.]

aninath93 wrote

Can you please suggest a good camera in your experience, as ZedX i am using does not support for Jetpack 6 yet, thinking to use D457 realsense, but open to your thoughts.

A stereo camera with large FOV and global shutter would work best. If you go D450, I'll suggest to use stereo IR mode with ir emitter disabled for VO.

cheers,
Mathieu

[aninath93]

Hi Mathieu,

Thank you for your response, I appreciate you.

Is there a RTABMAP tool to plot timing stats ? Currently I am also trying to build RT kernel patch on jetson, maybe that will help too.

Also is this imu_to_tf2 ported over to ROS2 side, as I do not see any documentation of wiki page of rtabmap_util  ? (If so for deskewing, this high frame tf would be better right than robot localization output of wheels + IMU ? and use this ICP_odom + wheels for rtabmap_slam.

[matlabbe]

If you use rtabmap_viz, open Statictics panel and you will get many timing statistics. Those statistics (except for odometry tab) are saved in the database to be analyzed afterwards in rtabmap-databaseViewer using the same Statistics panel.

wheels+IMU TF could be used for deskewing directly, as long it is published faster than lidar topic. It is sure the higher the frequency is, the more accurate the deskewing will be.

I updated the ros wiki to include imu_to_tf nodelet.

cheers,
Mathieu

[aninath93]

Hi Mathieu,

Thank you for updating the wiki and your reply, I deleted my last post because I figured the issue with external tags was, camera was seeing 2 tags in mapping.
Currently my setup is in lab and will be porting over to assembly area but still the robot jumps a lot when the apriltags are detected and its still not right in the sense, when I activate the Marker/Priors the logger complains about parsing "1" and also robot jumps to an unknow location sometimes outside map. I am attaching my database and launch file. Can you please advise.

Latest Database

[matlabbe]

The Marker/Priors parameter was incorrectly read from the python file. You may try adding quotes:

'Marker/Priors','"1 1.03 -10.08 0.52 0.11 0.03 2.97|2 -1.29 -12.44 0.56 -0.01 0.0 2.94|3 3.52 -0.13 0.52 -0.13 0.02 -2.10"',

Note also that often that with tags like this we would want to ignore detections too far from the target because the farther you are, the worst the orientation estimation is. Here one tag added to map has bad orientation, which would lead to large localization jumps:


The other ones look okay:


Note also there were some changes 2 months ago about localization with tags in 3DoF mode. See discussion here: https://github.com/introlab/rtabmap_ros/issues/1057#issuecomment-1813879350

[aninath93]

Hi Mathieu,

Thank you for replying, I went through the post and tried but had no luck, the robot flips still. I tried using 3 degrees of odometry in mapping and 6 degrees in localization but did not succeed. Can you please see attached database unless I need to have matrix of 2 x 2 of them to improve localization or am I doing something wrong here.

I am currently sitting on master branch, on ROS2 Galactic distro just to add.

Updated

Currently I am using external Apriltags_ros library, with this config below,  



You mentioned Z up for 2d mode, do you mean tag orientation being 180 degrees rotated ? Currently above I store world location of tag in the Marker/Priors string. Also just to let you know that I also tried 6DoF and result was robot flew to ceiling.

[matlabbe]

Can you also share a rosbag of the localization session? From the database point of view, it looked fine (I didn't check the new one, but previous one was not too bad), so without seeing actual tag detction constraints during localization, it is difficult to see why the robot jumps.

[aninath93]

Hi Mathieu,

I can send you the localization session by Monday morning, is there a specific topic you are looking for in rosbag ? (tag_detections ? global_pose ? )

[matlabbe]

I think tf, tf_static, tag_detections, not sure global_pose is but you can add it, lidar point cloud could be enough.

[aninath93]

Hi Mathieu,

Please see attached link, I could not record pointcloud as cloud assembler node kept dropping connection with LIDAR, so far I got /tf, /tf_static, /global_pose, /apriltag/detections and assembled_cloud.

Database_rviz-video_launch-file_ros-bag'

Also, I just tried 'g20' with same tags but did not see this issue of jumps or issue in convergence, I have also attached gtsam folder which I am using from source as galactic does not have binaries (I was on 4.2 version unless there is specific branch.
g20 files

Can you please suggest any other parameters based on my launch file in your experience, I have not got to warehouse stage where I can start tuning Memory parameter but I will look into your paper too once I prove it in this lab completely.

[matlabbe]

The tf tree doesn't have a single root, but three? (open image in new tab for full resolution)


The base_link frame is not linked to slam_*** tree which contains the TF transform of the apriltag detected. I don't want to spend time to fix the rosbag, but if you can share another one where the TF tree is fixed, I can look again.

The apriltag detections look kinda noisy. Can you try with Force3DoF=false? and see if it happens again, as it would be easier to compare with the database (without having the tag rotated upwards in database).

[aninath93]

Hi Mathieu,

That's strange on my TF but I can send you another one next week, but I did try Force3dof to false and the outcome was same with GTSAM.

Did you see this link Rosbag where I used g20 as the graph optimization and I did not have that issue as compared to using GTSAM with Force3Dof.

g20 files and database

[matlabbe]

So g2o + Force3DoF=false is fine, can you confirm the following:

• g2o + Force3DoF=false -> fine
• g2o + Force3DoF=true -> ???
• GTSAM + Force3DoF=false -> ???
• GTSAM + Force3DoF=true -> bad

[aninath93]

Hi Mathieu,

Apologies I should have mentioned this combination in my previous post,

g2o + Force3DoF=false -> I need to try this, I can try this early next week and let you know.
g2o + Force3DoF=true -> fine
GTSAM + Force3DoF=false -> bad
GTSAM + Force3DoF=true -> bad

[aninath93]

Hi Mathieu,

Final update, I have attached link with 3dof and 6dof, where both work fine in g20 optimizer. Hope these database help on the why GTSAM side they were failing in 3dof and 6dof unless there is a very specific version I have to be on but based on my previous response I was on 4.2 version.
3dof_6dof_mapping_launch_file_apriltags_config

g2o + Force3DoF=false ->fine
g2o + Force3DoF=true -> fine
GTSAM + Force3DoF=false -> bad
GTSAM + Force3DoF=true -> bad

Also one more thing I have noticed is with g20 while robot is navigating, when it sees tag the robot takes noticeable time to optimize map, is that going to be related to Mem/thr timing where I need to start at ?

Also I would appreciate if you can please let me know for any optimizations in my launch file. I appreciate your time as always.

[aninath93]

Hi Mathieu,

I have a question as I was going through your Long term memory management paper as I have one issue that when the tags are detected the map gets optimized but takes a split sec causing Nav2 to act weird if in middle of navigation.
I am not doing continuous SLAM, mapping separately the warehouse and then localizing in the same environment based on tags at home position and other pickup locations, what is missing in my setup ?

database with memory

            'Mem/NotLinkedNodesKept', 'false',
            'Mem/STMSize', '30',
            'Mem/LaserScanNormalK', '20',
            'Mem/IncrementalMemory','false',
            'Mem/InitWMWithAllNodes','true',
            'Mem/RehearsalSimilarity','0.35', (this was based on Memory/Reh sim time weight)
            'Rtabmap/TimeThr','0.7',
            'RGBD/OptimizeFromGraphEnd','true',

The robot jumps a lot with map constantly shifting, so I removed the TimeThr and Optimizefromgraphend, leaving the Mem/RehearsalSimilarity in place and results were OK but still feel it could be better in terms of optimizing as robot goes either under the floor or ceiling for a split sec but thats enough on Nav2 side.