How to correctly incorporate sensor fusion for odometry in RTAB-Map SLAM Algorithm?

devin1126 commented 1 year ago

Hello,

I have built a four-wheeled differential drive robot that is equipped with an Intel Realsense D435 RGBD camera sensor, LD14 Lidar sensor, BNO055 9-DOF IMU, and a set of LM393 wheel encoders. I have been trying to perform SLAM using the RTAB-Map algorithm and I have received mixed results. Originally, I was only using RGBD visual odometry from the camera as input for the RTAB SLAM algorithm while also using the Lidar for odometry correction along with building the 2D occupancy grid. The results were pretty decent as can be seen in the screenshot of the generated point cloud + the occupancy grid. However, as is known, it is very easy to lose the odometry using visual odometry alone, so I wanted to implement readings from my other sensors in combination with the RGBD visual odometry in an extended kalman filter node (from robot_localization) for more robust odometry readings. The resulting occupancy grid + point cloud, however, did not turn out as well as the one when only visual odometry was used as can be seen from this screenshot. The launch file that initializes sensor messages, rtabmap, and the ekf can be seen below:

`

<arg name="rviz"                    default="true" />
<arg name="frame_id"                default="base_link"/>
<arg name="vis_odom_topic"          default="/vo"/>
<arg name="twist_topic"             default="/wheel_twist"/>
<arg name="imu_topic"               default="/imu/data" />
<arg name="imu_remove_gravitational_acceleration" default="true" />
<arg name="localization"            default="false"/>

<include file="$(find rtabmap_ros)/launch/rtabmap_odom.launch">
    <arg unless="$(arg localization)" name="rtabmap_args" value="--delete_db_on_start"/>
    <arg name="frame_id" value="$(arg frame_id)"/>
    <arg name="subscribe_rgbd" value="true"/>
    <arg name="subscribe_scan" value="true"/>
    <arg name="rgbd_topic"  value="/rgbd_image"/>
    <arg name="compressed"  value="true"/>
    <arg name="visual_odometry" value="true"/>
    <arg name="queue_size"  value="30"/>
    <arg name="rviz"        value="$(arg rviz)"/>
    <arg if="$(arg rviz)" name="rtabmapviz"  value="false"/>

</include>

<!--Launch static transform between base_link and camera_link-->
<node name="base_to_camera_static_publisher" pkg="tf2_ros" type="static_transform_publisher" args="0.2531 0 0.0859 0 0 0 base_link camera_link"/>

<!-- Initializing teleop node-->
<remap from="vel_cmd" to="mega_one/vel_cmd"/>
<node pkg="devbot_teleop" type="teleop_control.py" name="teleop_node" />

<!-- Initializing nodes that will provide other sources of odometry-->
<!-- Launching wheel twist node-->
<remap from="twist" to="mega_one/twist" />
<include file="$(find base_controller_twist_converter)/launch/base_controller_twist.launch">
    <!-- Set custom parameters if needed
    <param name="linear_velocity_stdev" type="double" value="" />
    <param name="angular_velocity_stdev" type="double" value="" />
    -->
</include>    
<!-- Launching imu node-->
<remap from="bno055" to="mega_two/bno055" />
<include file="$(find bno055_imu_ros)/launch/bno055_imu.launch">
    <!-- Set custom parameters if needed
    <param name="linear_velocity_stdev" type="double" value="" />
    <param name="angular_velocity_stdev" type="double" value="" />
    -->
</include>

<!-- Odometry fusion (EKF), refer to demo launch file in robot_localization for more info -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" clear_params="true" output="screen">

    <param name="frequency" value="50"/>
    <param name="sensor_timeout" value="0.1"/>
    <param name="two_d_mode" value="true"/>

    <param name="odom_frame" value="odom"/>
    <param name="base_link_frame" value="$(arg frame_id)"/>
    <param name="world_frame" value="odom"/>

    <param name="transform_time_offset" value="0.0"/>

    <param name="odom0"  value="$(arg vis_odom_topic)"/>
    <param name="twist0" value="$(arg twist_topic)"/>
    <param name="imu0" value="$(arg imu_topic)"/>

    <!-- The order of the values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
    <rosparam param="odom0_config">[true, true, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false,
                                    false, false, false]</rosparam>

    <rosparam param="twist0_config">[false, false, false,
                                    false, false, false,
                                    true, true, false,
                                    false, false, true,
                                    false, false, false]</rosparam>

    <rosparam param="imu0_config">[false, false, false,
                                    false,  false,  true,
                                    false, false, false,
                                    false,  false,  true,
                                    true,  false,  false]</rosparam>

    <param name="odom0_differential" value="false"/>
    <param name="twist0_differential" value="false"/>
    <param name="imu0_differential" value="false"/>

    <param name="odom0_relative"  value="true"/>
    <param name="twist0_relative" value="true"/>
    <param name="imu0_relative" value="true"/>

    <param name="odom0_queue_size" value="5"/>
    <param name="twist0_queue_size" value="10"/>
    <param name="imu0_queue_size" value="50"/>

    <!-- The values are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz,
       vroll, vpitch, vyaw, ax, ay, az. -->
    <rosparam param="process_noise_covariance">[0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                                              0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]</rosparam>

    <!-- The values are ordered as x, y,
    z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. -->
    <rosparam param="initial_estimate_covariance">[1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                                                    0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]</rosparam>

</node>

`

The rtabmap_odom.launch file is an edited version of the standard rtabmap.launch file with the following changes:

The rtabmap visual odometry node has the following parameters added: `

    <!-- Odom Parameters-->
    <param name="publish_null_when_lost"      type="bool"   value="false"/>
    <param name="Odom/FillInfoData"      type="string" value="true"/>
    <param name="Odom/ResetCountdown"    type="string" value="1"/>
    <param name="Vis/FeatureType"        type="string" value="6"/>
    <param name="OdomF2M/MaxSize"        type="string" value="1000"/>`

The rtabmap SLAM node has the following parameters added/changed: `

<remap from="odom" to="/odometry/filtered"/>        

<!-- RTAB-Map's parameters -->

<param name="Kp/DetectorStrategy"       type="string" value="6"/> <!-- use same features as odom -->  

<param name="RGBD/NeighborLinkRefining" type="string" value="true"/>

<param name="RGBD/ProximityBySpace"     type="string" value="true"/>

<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>

<param name="RGBD/AngularUpdate"        type="string" value="0.01"/>

<param name="RGBD/LinearUpdate"         type="string" value="0.01"/>

<param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/>

<param name="Reg/Force3DoF"             type="string" value="true"/>

<param name="Grid/Sensor"               type="string" value="false"/>  <!-- occupancy grid from lidar -->

<param name="Reg/Strategy"              type="string" value="1"/>      <!-- 1=ICP -->

<!-- ICP parameters -->

<param name="Icp/VoxelSize"                 type="string" value="0.05"/>

<param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>`

Examples of the actual error/warning messages that are appearing while I am mapping with the ekf implemented can be seen below:

[ WARN] (2023-03-18 15:35:27.761) OdometryF2M.cpp:561::computeTransform() Registration failed: "Not enough inliers 8/20 (matches=94) between -1 and 134" (guess=xyz=0.040539,-0.013540,0.053908 rpy=0.044678,0.130888,-0.003641)

[ WARN] (2023-03-18 15:35:27.761) OdometryF2M.cpp:309::computeTransform() Failed to find a transformation with the provided guess (xyz=0.040539,-0.013540,0.053908 rpy=0.044678,0.130888,-0.003641), trying again without a guess.

[ WARN] (2023-03-18 15:35:38.306) OdometryF2M.cpp:561::computeTransform() Registration failed: "Not enough inliers 6/20 (matches=101) between -1 and 170" (guess=xyz=-0.016210,-0.018582,-0.011542 rpy=0.043647,-0.017861,0.017543)

[ WARN] (2023-03-18 15:35:38.306) OdometryF2M.cpp:309::computeTransform() Failed to find a transformation with the provided guess (xyz=-0.016210,-0.018582,-0.011542 rpy=0.043647,-0.017861,0.017543), trying again without a guess.

[ WARN] (2023-03-18 15:35:38.349) OdometryF2M.cpp:571::computeTransform() Trial with no guess succeeded!

[ WARN] (2023-03-18 15:31:17.205) OptimizerTORO.cpp:151::optimize() TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter Optimizer/Strategy). Link 18 ignored...

[ WARN] (2023-03-18 15:31:17.205) OptimizerTORO.cpp:151::optimize() TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter Optimizer/Strategy). Link 20 ignored...

[ WARN] (2023-03-18 15:31:17.205) OptimizerTORO.cpp:151::optimize() TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter Optimizer/Strategy). Link 24 ignored...

[ WARN] (2023-03-18 15:35:42.914) OdometryF2M.cpp:557::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=80) between -1 and 2"

At this point, I am open to any suggestions on parameter tuning in order to obtain proper localization and odometry calculations so that the mapping can be done correctly. If anyone could give even minor advice on parameter tuning, I would truly appreciate it. I am running the noetic distro of ROS on both my remote computer and the robot itself. Thank you.

matlabbe commented 1 year ago

visual odometry is more accurate if you build rtabmap with g2o (this will get rid of the TORO warnings).

How is rgbd_image generated? With D435, use Left IR + Depth (disable IR emitter, and don't use align depth to rgb). Example IR-Depth example here http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping (don't forget to disable IR emitter)

If you want to use robot_localization, disable odom tf from rtabmap with this argument: publish_tf_odom set to false

devin1126 commented 1 year ago

Hello again,

Thank you for your time in responding. The rgbd_image is being generated from a rgbd_sync nodelet that is in a separate launch file being ran on the robot instead of the remote computer that the above launch file was ran on. Its contents will be displayed below in case any other important information can be discerned from it:

`

<!-- Launch RGBD Camera Node-->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
    <arg name="align_depth" value="true" />
</include>

<!-- Launch Rosserial Nodes for Both Arduino Mega Boards (57600 Baud Rate)-->
<node ns="mega_one" pkg="rosserial_python" type="serial_node.py" name="serial_node_one" args="/dev/ttyACM0"/>
<node ns="mega_two" pkg="rosserial_python" type="serial_node.py" name="serial_node_two" args="/dev/ttyACM1"/>

<!-- Launch LD14 Lidar Sensor Node-->
<include file="$(find ldlidar_sl_ros)/launch/ld14.launch"/>

<!-- Launch RTAB-Map RGBD Sync Nodelet-->
<arg name="rate"  default="5"/>
<arg name="approx_sync" default="false" /> 
<arg name="rgbd_sync" default="true"/>

<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
<node if="$(arg rgbd_sync)" pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync nodelet_manager" output="screen">
    <param name="compressed_rate"  type="double" value="$(arg rate)"/>
    <param name="approx_sync"      type="bool"   value="$(arg approx_sync)"/>

    <remap from="rgb/image"       to="/camera/color/image_raw"/>
    <remap from="depth/image"     to="/camera/aligned_depth_to_color/image_raw"/>
    <remap from="rgb/camera_info" to="/camera/color/camera_info"/>

    <remap from="rgbd_image"      to="rgbd_image"/>
</node>

`

Also, I looked through similar issues as the one I am describing using the D435 camera, and I saw that you suggested previously to use Left IR + Depth mode with the camera instead, so I attempted to go about using that method instead. However, I ran into an issue when I was trying to use the SLAM node that was receiving stereo images and attempting to use the LIDAR for occupancy map building. The issue is described in more detail on this ROS forum if you ever have the time to respond to it.

introlab / rtabmap_ros

How to correctly incorporate sensor fusion for odometry in RTAB-Map SLAM Algorithm? #913