Rtabmap octree centers remain after loop closure dtected

[acp]

Dear people,

I  do have a:

ZED camera
Jetson TX2
Pixhawk 4 mini

Mounted on a drone.

Then

I have a launch file that contains the following two packages, among others:

 rtabmap.launch

 octomap_server with cloud_in = /rtabmap/cloud_map

I do have a problem, when I run the rtabmap.launch  and I plot the /octomap_point_cloud_centers in RVIZ.

The problem is that when the rtabmap finds a loop closure and corrects itself, the octrees do not correct and  do not disappear,   I mean they remind as you can see in the two pictures below.


How can I solve this issue?



Any help is appreciated, thank you  

[matlabbe]

Hi,

On loop closure, the octomap should be cleared when using octomap_server. Instead of using octomap_server, did you try using /rtabmap/octomap_bin or /rtabmap/octomap_full topics directly? Those octomaps are updated on loop closures (without having to clear the full octomap).

For convenience, you can also show octomap from rtabmap in PointCloud2 format, see /rtabmap/octomap_obstacles.

cheers,
Mathieu

[acp]

,

matlabbe wrote

On loop closure, the octomap should be cleared when using octomap_server.

Well it seems to be that the octomap is not clear in loop closure when using octomap server.

This is the setup of the octomap_server

matlabbe wrote

 Instead of using octomap_server, did you try using /rtabmap/octomap_bin or /rtabmap/octomap_full topics directly? Those octomaps are updated on loop closures (without having to clear the full octomap).

The question here is;

How can I use /rtabmap/octomap_bin or /rtabmap/octomap_full  since I need /octomap_point_cloud_centers?

And also In RVIZ i only see:


     /rtabmap/octomap_empty_space,   /rtabmap/octomap_grid,  /rtabmap/octomap_ground, /rtabmap/octomap_occupied_space,  /rtabmap/octomap_obstacles 

However using rostopid hz I get the data of the topics: /rtabmap/octomap_binary and /rtabmap/octomap_full at the average rate: 0.482

matlabbe wrote

For convenience, you can also show octomap from rtabmap in PointCloud2 format, see /rtabmap/octomap_obstacles.

Thank you and looking forward to your replay.

[matlabbe]

There is a service "reset" on octomap_server node. We could call it when rtabmap detects a loop closure.

On rtabmap side, I think /rtabmap/octomap_occupied_space may be the same than /octomap_point_cloud_centers. If "Grid/RayTracing" is enabled, /rtabmap/octomap_empty_space would contain the empty cells as points.

[acp]

Hi Mathieu

Thank you for your replay.

matlabbe wrote

There is a service "reset" on octomap_server node. We could call it when rtabmap detects a loop closure.

I have tried the previous and the /octomap_point_cloud_centers is not reseted, but for instance the /occupied_cells_vis_array is reseted when there is a loop closure and the reset service is called.

matlabbe wrote

On rtabmap side, I think /rtabmap/octomap_occupied_space may be the same than /octomap_point_cloud_centers. If "Grid/RayTracing" is enabled, /rtabmap/octomap_empty_space would contain the empty cells as points.

As it can be seen in the following picture, the white cubes correspond to /octomap_point_cloud_centers topic  whereas the colour cubes correspond to /rtabmap/octomap_occupied_space topic. It seems to be they are not the same.




Just asking:

1.-  How can this problem of clear the /octomap_point_cloud_centers topic  in a a loop closure  be solved?


2.-How can I spread the points in the rtabmap/cloud_map, in other words, how can I have an sparse 3D map? So far have done the following:

In rtabmap.launch file:

param name="Grid/CellSize" type="string" value="0.12"

If I want to increase the value number, I can not see the rtabmap/cloud_map in RVIZ

[matlabbe]

If we clear the octomap from octomap_server, it will likely miss some data afterwards. What are the parameters of octomap_server? The best would be to have a rosbag with rtabmap and octomap_server examples working with it.

Setting Grid/CellSize will decrease resolution of the cloud_map topic. For the octomap clouds from rtabmap, it i possible to set octomap_tree_depth parameter to adjust at which resolution (16 is maximum resolution, 15 is 1/2 resolution and so on) the resulting occupied cloud will be generated.

cheers,
Mathieu

[acp]

Hi Mathieu


Here you are a ,bag file I have run it with the scrip placed below.  I hope you can load it and able to run it and see how the octomap does not delete what is remain after the update of the rtabmap.

I am looking forward to your replay

https://www.dropbox.com/s/zponjnbnyj2jnxv/fityard.bag?dl=0


<?xml version="1.0"?>
<launch>


 
<arg name="camera_frame"              default="zed_camera_center" />

<node pkg="static_transform_mux" type="static_transform_mux" name="static_transform"  output="screen"/>

<group ns="/zed/">
    <include file="$(find rtabmap_ros)/launch/rtabmap.launch">
       <arg name="rtabmap_args"        value="--delete_db_on_start --Optimizer/GravitySigma 0.3" />
      <arg name="rgb_topic"               value="/zed/zed_node/rgb/image_rect_color" />
      <arg name="depth_topic"             value="/zed/depth/depth_filtered" />
      <arg name="camera_info_topic"       value="/zed/zed_node/rgb/camera_info" />
      <arg name="depth_camera_info_topic" value="/zed/zed_node/depth/camera_info" />
      <arg name="frame_id"                      value="$(arg camera_frame)" />
      <arg name="odom_frame_id"                 value="odom" />
      <arg name="approx_sync"                   value="false" />
      <arg name="visual_odometry"               value="false" />
      <arg name="rtabmapviz"                    value="false" />
      <arg name="rviz"                          value="false" />
      <arg name="odom_topic"                    value="/zed/zed_node/odom" />
      <arg name="imu_topic"                     value="/mavros/imu/data"/> 
      <(param) name="Optimizer/GravitySigma "     type="string"  value="0.3"/>
     
     
      <(param) name="Grid/NormalsSegmentation" type="bool" value="false"/>
      <(param) name =" Grid/MaxGroundHeight" type="double" value="0.05"/>
      <(param) name="Optimizer/Strategy"  type="string" value="1"/>  
      <(param) name="RGBD/OptimizeStrategy" type="string" value="1"/>
    </include>
</group>


  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
        <(param) name="resolution" value="0.3" />
        <(param) name="frame_id" type="string" value="/map" />
        <(param) name="max_sensor_range" value="7.0" />
        <(param) name="latch" value="false" />
        <(param) name="pointcloud_max_z" value="200" />
        <(param) name="pointcloud_min_z" value="-200" />
      <(param) name="filter_ground" value="false" />
    <(param) name="base_frame_id" value="$(arg camera_frame)" />
        <remap from="cloud_in" to="/zed/rtabmap/cloud_map" />
  </node>
</launch>

[matlabbe]

Hi,

no need to replay, octomap_server_node has to be manually cleared after a loop closure to clear all obstacles. To be automatic and be more efficient, consider using octomap output of rtabmap directly. See octomap available topic outputs from rtabmap node.

Based on your octomap configuration, the same settings in rtabmap would be:

<arg name="rtabmap_args"  value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Grid/3D true --Grid/RangeMax 7 --Grid/CellSize 0.3 --Grid/GroundIsObstacle true" />

rtabmap will automatically update the full octomap on loop closure (warning, this can take some time to do depending on the Grid configuration, see table 10 of this paper).

cheers,
Mathieu

[acp]

Hi Mathieu

Thank you for the answer.

matlabbe wrote

Based on your octomap configuration, the same settings in rtabmap would be:

<arg name="rtabmap_args"  value="--delete_db_on_start --Optimizer/GravitySigma 0.3 --Grid/3D true --Grid/RangeMax 7 --Grid/CellSize 0.3 --Grid/GroundIsObstacle true" />

rtabmap will automatically update the full octomap on loop closure (warning, this can take some time to do depending on the Grid configuration, see table 10 of this paper).

I have done what you have suggested, and the octomap map updates quite smoothly using /rtabmap/octomap_occupied_space topic. Even the optimization and the path estimation are much more accurate as you can see it the figure.


What parameter you think improved the mapping, perhaps the --Grid/GroundIsObstacle true What do you think?

I just have a question?

Is it possible to fuse video positioning system with zed_odom to feed rtabmap_ros? So probably I could get better  path  estimation.

What do you think?

I am looking forward to your replay.

[matlabbe]

Hi,

Not sure what made it better, those parameters (Grid/) affect only the mapping, not localization. --Grid/GroundIsObstacle true just says that ground will not be segmented, everything the camera sees will be an obstacle (thus saving some computation time as no segmentation needs to be done).

What do you mean by video positioning system? Do you have an example?

[acp]

matlabbe wrote

Hi,

Not sure what made it better, those parameters (Grid/) affect only the mapping, not localization.   just says that ground will not be segmented, everything the camera sees will be an obstacle (thus saving some computation time as no segmentation needs to be done).

What do you mean by video positioning system? Do you have an example?

Well, I am not sure either what have improved the map, probably both --Grid/3D true and  --Grid/GroundIsObstacle true.


1.-  I was told that video positioning system (VPS) could improve the position of the drone.

             *  I do not have any example,  just what I have seen searching in google.


             *  As far I do understand, this technique is quite new and it uses the camera and google maps for localization. Whether it can be use in Ubuntu-zed-rtabmap I have no clue.

             *  So far I have not found any usable information.


2.- By the way, do you know how can I improve the /zed_node/odom or in other words, how to improve the position of the drone to be feed into rtabmap_ros

3.- Just wondering whether there exists a relocation module and  in case of yes, is it  possible to use the relocation module with ZED in order to improve the /zed/odom topic

4.- I have another question: when I ran rtabmap.launch I get the following warning:

           * We are receiving imu data (buffer=1), but cannot interpolate imu transform at time 1591344362.222727 (closest is at 4.191690). IMU won't be added to graph.

Do you have a clue  about it?



Thank you and looking forward to your replay

[matlabbe]

Hi,

1. That VPS is mostly a visual localization system. The idea is to create a visual map of an environment (slam), then in localization mode you can localize with a camera. VPS is a fancy name for SLAM and localization.

2. You can look for vio approaches. Check msckf_vio or VINS-Fusion projects and see if you have a compatible camera.

3. RTAB-Map does relocalization (localization mode). It won't help zed odometry, it will just update /map -> /odom frame.

4. IMU stamps seem not using host time. If you want to use IMU, its stamp should be host time, not sensor time.

[acp]

Hi Mathieu

Thank you for your replay.

matlabbe wrote

2. You can look for vio approaches. Check msckf_vio or VINS-Fusion projects and see if you have a compatible camera.

Do you have some examples?

matlabbe wrote

4. IMU stamps seem not using host time. If you want to use IMU, its stamp should be host time, not sensor time.

How can I change from sensor time to host time, and the question is:  if it is recommendable to do it.


I have one more question.


I have installed rtabmap from source, then when I run RVIZ I get the following two ERRORS

[ERROR]  Client [/start_octomap_server] wants topic /zed/rtabmap/info to have datatype/md5sum [octomap_point_centers_filter/Info/f9cc2e215f6cae05161c74d102ffc592], but our version has [rtabmap_ros/Info/dd8620be5f7bd87dac1670b11dc3540d]. Dropping connection.


AND


[ERROR] [1593064047.076232248]: PluginlibFactory: The plugin for class 'rviz_plugin_tutorials/Imu' failed to load.  Error: According to the loaded plugin descriptions the class rviz_plugin_tutorials/Imu with base class type rviz::Display does not exist. Declared types are  rtabmap_ros/Info rtabmap_ros/MapCloud rtabmap_ros/MapGraph rviz/Axes rviz/Camera rviz/DepthCloud rviz/Effort rviz/FluidPressure rviz/Grid rviz/GridCells rviz/Illuminance rviz/Image rviz/InteractiveMarkers rviz/LaserScan rviz/Map rviz/Marker rviz/MarkerArray rviz/Odometry rviz/Path rviz/PointCloud rviz/PointCloud2 rviz/PointStamped rviz/Polygon rviz/Pose rviz/PoseArray rviz/PoseWithCovariance rviz/Range rviz/RelativeHumidity rviz/RobotModel rviz/TF rviz/Temperature rviz/WrenchStamped



Do you have a clue how to solve them.



Thank you

[matlabbe]

I have one example with VINS-Fusion and a D435i. rtabmap should be built with VINS-Fusion support for this example.
https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_d435i_vio.launch

More config for other setups here: https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/tree/master/config
I don't think they have a zedm or ZED2 example.

If you want to synchronize with other sensors, yes. If many sensors are not on the same clock, it will be difficult to synchronize them. Note that is IMU and images are taken from the same camera and only topics that need to be synchronized, there should be no problems to use sensor time.

For the rviz errors, it seems the wrong topic has been associated with the wrong plugin.

For imu plugin error, rviz_plugin_tutorials/Imu may not be in your workspace.

[acp]

Hi Mathieu


Thank you for your reply, however I do have few doubts or questions

matlabbe wrote

I have one example with VINS-Fusion and a D435i. rtabmap should be built with VINS-Fusion support for this example.
https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_d435i_vio.launch

I have successfully installed Vins-Fusion  on JetsonTX2 and able to run the rosbag MH_01_easy.bag.

   Stereo cameras + IMU
    roslaunch vins vins_rviz.launch
    rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
    (optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-
    Fusion/config/euroc/euroc_stereo_imu_config.yaml
    rosbag play  MH_01_easy.bag


However,  taking a look at your example test_d435i_vio.launch I would like to try it with the ZED stereo camera and the IMU from pixhawk 4 mini.

1.- I can see that you run Vins-Fusion  differently  from the Vins-Fusion.

    ---Vins-Fusion uses roslaunch vins vins_rviz.launch, whereas  test_d435i_vio.launch uses --OdomVINS/ConfigPath $(find vins)/../config/realsense_d435i/realsense_stereo_imu_config.yaml

2.-  I can also see that Vins-Fusion  path estimation is not feed into rtabmap.launch, instead, I guess stereo odometry is used.

3.- How to generate the zed_stereo_imu_config.yaml?

4.- How can build rtabmap   with VINS-Fusion support ?

matlabbe wrote

More config for other setups here: https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/tree/master/config
I don't think they have a zedm or ZED2 example.

If you want to synchronize with other sensors, yes. If many sensors are not on the same clock, it will be difficult to synchronize them. Note that is IMU and images are taken from the same camera and only topics that need to be synchronized, there should be no problems to use sensor time.

For the rviz errors, it seems the wrong topic has been associated with the wrong plugin.

For imu plugin error, rviz_plugin_tutorials/Imu may not be in your workspace.

In my case,

5.- The images  are  generated from  the ZED camera, whereas IMU is generated from Pixhawk 4 mini, I guess they do not have the same clock, right? If this my case can I still use Vins-Fusion?

6.- Reading at  Vins-Fusion   

Run with your devices
VIO is not only a software algorithm, it heavily relies on hardware quality. For beginners, we recommend you to run VIO with professional equipment, which contains global shutter cameras and hardware synchronization.

   What do they mean with professional equipment and global shutter?


I do have also another question :) I have posted as a single message in the forum :)


I have a question, I am lately using the topic  /rtabmap/octomap_occupied_space rom rtabmap_ros instead of /octomap_point_cloud_centers from the octomap_server. However, the resolution can be changed in the octomap_server according to the following:

  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
      <(param) name="resolution" value="0.5" />
      <remap from="cloud_in" to="/zed/rtabmap/cloud_map" />
  </node>

So, when I plot the voxels in RVIZ I can see there is a separation of 0.5 between each other.

The question here how can I do something similar with  /rtabmap/octomap_occupied_space




Thank you and I am looking  at your replay.