Using rtabmap visual slam using zed2i camera

[impressivetoken]

Hello!

I have been working on slam for outdoor environment using zed2i camera and the zed-ros-examples github repo (used to be known as zed-wrapper but now has been bifurgated to zed-ros-wrapper and zed-ros-examples)

Under zed-ros-examples i have come across the zed-rtabmap-example folder that can be used for doing the above task mentioned.

i have been getting okayish results so far.




I'm able to plot the map pretty satisfactorily but one of the problems ive been facing is that the it overdoes its turns i.e., if i try taking a 90 degree turn, it shows it takes a turn which is greater than 90 degrees which kinda messes up my map.

i want my map to be near perfect cause i will be using it for navigation of a relatively bigger vehicle and there can be no room for errors. Unfortunately i dont have any pictures of that happening but I hope i'm making sense so that you can understand it well.

One more problem that im facing is that while localization, even though it is comparing the right frame to the real time frame, it shows "loop closure hypothesis rejected" way too many times which makes the vehicle loose its track and i recieve not very accurate results.

i'm very new to this right now so please bare with me.

Below is my zed_rtabmap.launch file
<launch>
    <arg name="svo_file"             default="" /> 
    <arg name="stream"               default="" /> 

    <arg name="zed_node_name"        default="zed_node" />
    <arg name="camera_model"         default="zed2" />
    <arg name="publish_urdf"         default="true" />

    <arg name="camera_name"          default="zed" />

    <arg name="base_frame"           default="base_link" />

    <group ns="$(arg camera_name)">
       
        <include file="$(find zed_wrapper)/launch/include/zed_camera.launch.xml">
            <arg name="camera_name"         value="$(arg camera_name)" />
            <arg name="svo_file"            value="$(arg svo_file)" />
            <arg name="stream"              value="$(arg stream)" />
            <arg name="node_name"           value="$(arg zed_node_name)" />
            <arg name="camera_model"        value="$(arg camera_model)" />
            <arg name="base_frame"          value="$(arg base_frame)" />
            <arg name="publish_urdf"        value="$(arg publish_urdf)" />
        </include>   
 
       
        <arg name="custom_rtabmap_launch_file" default="$(find zed_rtabmap_example)/launch/include/sl_rtabmap.launch.xml"/>
        <include file="$(arg custom_rtabmap_launch_file)">
            <arg name="odom_topic"              default="$(arg zed_node_name)/odom" />
            <arg name="rgb_topic"               default="$(arg zed_node_name)/rgb/image_rect_color" />
            <arg name="depth_topic"             default="$(arg zed_node_name)/depth/depth_registered" />
            <arg name="camera_info_topic"       default="$(arg zed_node_name)/rgb/camera_info" />
            <arg name="depth_camera_info_topic" default="$(arg zed_node_name)/depth/camera_info" />
        </include>   

       
        <node name="rviz" pkg="rviz" type="rviz" args="-d $(find zed_rtabmap_example)/rviz/zed-rtabmap.rviz" output="screen" /> 
    </group>
</launch>

below is my sl_rtabmap.launch.xml file



<launch>
   
    <arg name="localization"            default="false"/>

    <arg     if="$(arg localization)" name="args" default=""/>
    <arg unless="$(arg localization)" name="args" default="--delete_db_on_start"/>

   
    <arg name="rgb_topic"               default="/zed/zed_node/rgb/image_rect_color" />
    <arg name="depth_topic"             default="/zed/zed_node/depth/depth_registered" />
    <arg name="camera_info_topic"       default="/zed/zed_node/rgb/camera_info" />
    <arg name="depth_camera_info_topic" default="/zed/zed_node/depth/camera_info" />

    <arg name="odom_topic"              default="/zed/zed_node/odom" />   
   

   
   

   
    <node name="rtabmap_viz" pkg="rtabmap_viz" type="rtabmap_viz" output="screen" args="" launch-prefix="">   
        <remap from="rgb/image"         to="$(arg rgb_topic)"/>
        <remap from="depth/image"       to="$(arg depth_topic)"/>
        <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>

        <remap from="grid_map"          to="map" />
        <remap from="odom"              to="$(arg odom_topic)"/>
    </node> 


   
    <node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg args)" launch-prefix="">
        <rosparam command="load" file="$(find zed_rtabmap_example)/params/rtabmap.yaml" />
   
        <remap from="rgb/image"         to="$(arg rgb_topic)"/>
        <remap from="depth/image"       to="$(arg depth_topic)"/>
        <remap from="rgb/camera_info"   to="$(arg camera_info_topic)"/>

        <remap from="grid_map"          to="map" />
        <remap from="odom"              to="$(arg odom_topic)"/>

       
       
       
       
    </node>
</launch>


below is my rtabmap.yaml file
# RTAB-Map configuration

subscribe_depth:        true
subscribe_rgbd:         false
subscribe_stereo:       false
subscribe_stereo:       false
subscribe_scan:         false
subscribe_scan_cloud:   false
subscribe_user_data:    false
subscribe_odom_info:    false

database_path:      "~/.ros/rtabmap.db"
config_path:        "~/.ros/rtabmap.cfg"

frame_id:       "base_link"
map_frame_id:   "map"
odom_frame_id:  "odom"                   # odometry from odom msg to have covariance - Remapped by launch file
odom_tf_angular_variance:  0.001     # If TF is used to get odometry, this is the default angular variance
odom_tf_linear_variance:   0.001     # If TF is used to get odometry, this is the default linear variance
tf_delay:       0.02
publish_tf:     false                # Set to false if fusing different poses (map->odom) with UKF

odom_sensor_sync:               true
wait_for_transform_duration:    0.2
approx_sync:                    true

queue_size: 10

scan_normal_k:  0

Grid:
    3D:                     true
    FlatObstacleDetected:   true
    MapFrameProjection:     false
    GroundIsObstacle:       false
    PreVoxelFiltering:      true
    RayTracing:             true
    FromDepth:              true
    NormalsSegmentation:    false
    CellSize:               0.05
    ClusterRadius:          0.1
    MinClusterSize:         3
    DepthDecimation:        1
    DepthRoiRatios:         [0.0, 0.0, 0.0, 0.0]
    FootprintHeight:        2.0
    FootprintLength:        0.18
    FootprintWidth:         0.18
    MaxGroundAngle:         30.0
    MinGroundHeight:        -0.5
    MaxGroundHeight:        -0.4
    MaxObstacleHeight:      0.1
    NoiseFilteringMinNeighbors: 5
    NoiseFilteringRadius:   0.1
    NormalK:                20
    RangeMin:               0.7
    RangeMax:               3.0

GridGlobal:
    Eroded:                 false       # Erode obstacle cells
    FootprintRadius:        0.18        # Footprint radius (m) used to clear all obstacles under the graph
    FullUpdate:             true        # When the graph is changed, the whole map will be reconstructed instead of moving individually each cells of the map. Also, data added to cache won't be released after updating the map. This process is longer but more robust to drift that would erase some parts of the map when it should not
    MaxNodes:               0           # Maximum nodes assembled in the map starting from the last node (0=unlimited)
    MinSize:                1.0        # Minimum map size (m)
    OccupancyThr:           0.55        # Occupancy threshold (value between 0 and 1)
    ProbClampingMax:        0.971       # Probability clamping maximum (value between 0 and 1)
    ProbClampingMin:        0.1192      # Probability clamping minimum (value between 0 and 1)
    ProbHit:                0.7         # Probability of a hit (value between 0.5 and 1)
    ProbMiss:               0.4         # Probability of a miss (value between 0 and 0.5)
    UpdateError:            0.01        # Graph changed detection error (m). Update map only if poses in new optimized graph have moved more than this value


I am also facing huge z axis drifts while mapping. Is there any way i can rectify that as well?

I would also like to know how to go ahead from here if i'm making any mistakes.
please let me know if there are any advices and any changes that can be made as it would be of great great help.

Thanking you in advance!

[matlabbe]

I see you are using ZED's odometry, I think it should be more accurate than that. Maybe the camera is not correctly calibrated? For stereo camera, it is super important that the calibration is good, otherwise motion estimation is drifting a lot more.

For the "loop closure hypothesis rejected", what is the full message? why is it rejected?

[impressivetoken]

Hey!

Thanks for replying,

(i)I have calibrated the camera again and will try mapping the layout one more time.

(ii) For loop closure hypothesis part, it just shows loop closure hypothesis rejected with its respective frame id it is comparing it to in rtabmap(with the red background instead of green when it detects a loop closure).

The thing I wanted to know was why is it showing me loop closure hypothesis rejected even though it is comparing it with the right frame id with not many feature mismatches.

Is there any way I can change my code to get rid of this problem?

(iii) Can I also know how to use rtabmap instead of zed's odometry?

[matlabbe]

Hi,

(ii) You would see a warning in terminal showing why it has been rejected. Sometimes it is because graph deformation error is above RGBD/OptimizeMaxError, which could be increased (default 3) if you confirm that these loop closures should be accepted.

(iii) You may have to disable odom from zed to avoid conflicting TF for same odom frame, but afterwards it could be something like this:

  <node pkg="rtabmap_odom" type="stereo_odometry" name="stereo_odometry" output="screen">
      <remap from="left/image_rect" to="$(arg zed_node_name)/left/image_rect_color"/>
      <remap from="right/image_rect" to="$(arg zed_node_name)/right/image_rect_color"/>
      <remap from="left/camera_info" to="$(arg zed_node_name)/left/camera_info"/>
      <remap from="right/camera_info" to="$(arg zed_node_name)/right/camera_info"/>
      <remap from="odom" to="odom"/>

      <param name="frame_id" type="string" value="$(arg base_frame)"/>
    </node>

[impressivetoken]

Hi again,

(ii)You were right, that is exactly what is showing up in the terminal(RGBD/OptimizeMaxError)


How to Fix this?
Also by how much should I increase the value(RGBD/OptimizeMaxError) to?


Question:
1. Could you please let me know what all things need to be done for setting up RTABMAP in order to use it for navigation using ZED2i camera? I would appreciate any sources that you could kindly provide! I have been through - http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping and other ros-wiki pages related to it. I am not able to get any localization values whatsoever in only localization mode.

[matlabbe]

It seems you are using a config file to set rtabmap parameters, you may add:

RGBD:
    OptimizeMaxError: 10

EDIT

Could you please let me know what all things need to be done for setting up RTABMAP in order to use it for navigation using ZED2i camera?

http://wiki.ros.org/rtabmap_ros/Tutorials/MappingAndNavigationOnTurtlebot

[impressivetoken]

Hi again,

Thanks it worked!

But the problem i am facing is as i am increasing the OptimizeMaxError, the error ratio also increases.

For example:
    (i)When OptimizeMaxError was default, the error ratio would go up to 3.5-4 as already observed in the picture above.

   (ii)When it was set to "10", the error ratio would increase to 47 and at times would go up to 66 as well


I'm sorry for the glare in the photos.

   (iii)I was just exploring with its values just to see how far it can go up. One thing I have noticed is that the higher the value I set of OptimizeMaxError the more error ratio would go up.


I also noticed another warning showing up in the terminal, i dont know if it will be of any help or not.


Also thank you for the link you sent me in the end!

[matlabbe]

I am suspicious of the rejected 180 deg loop closures if they are really good. I would need to reproduce your experiment to see if there ia a bug with the visual proximity detections. I'll to reproduce it in the next days.

[matlabbe]

For some reasons I though this post was a followup of this one: https://github.com/introlab/rtabmap_ros/issues/1034

I would need the database to debug more your issue.

[impressivetoken]

Hello again,

Thanks for the reply

I would appreciate it if you could tell me the necessary things that you would require.
I will try my level best to provide them to you.

Cheers,
Raj

[impressivetoken]

Hey there,

Just wanted to give you a quick small update,

Just to remove any doubt , I had a chance of changing my zed2i camera
I reproduced the same experiment and it gave me good results,


i dont know why this can happen,
any inputs that you can give?

Cheers,
Raj

[matlabbe]

Hi,

Difficult to tell without seeing the raw data.

[impressivetoken]

Can you please elaborate on what you mean by raw data

[matlabbe]

A rosbag recording of the inputs of rtabmap.