How to correctly incorporate sensor fusion for odometry in RTAB-Map SLAM Algorithm?
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:
<!-- This launch assumes that you have already started you preferred RGB-D sensor and your IMU. TF between frame_id and the sensors should already be set too. -->
<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 --> <!-- 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 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 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.
[ INFO] [1679168127.787621315]: rtabmap (27): Rate=1.00s, Limit=0.000s, Conversion=0.0009s, RTAB-Map=0.0740s, Maps update=0.0040s pub=0.0001s (local map=4, WM=4)
WARN 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 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 OdometryF2M.cpp:571::computeTransform() Trial with no guess succeeded!
WARN 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 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 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] [1679168142.628500127]: Odometry automatically reset to latest computed pose!
[ INFO] [1679168142.628977971]: Odom: quality=8, std dev=0.000000m|0.000000rad, update time=0.296383s
[ INFO] [1679168142.665094478]: Odom: quality=0, std dev=99.995000m|99.995000rad, update time=0.034518s
WARN OdometryF2M.cpp:557::computeTransform() Registration failed: "Not enough inliers 0/20 (matches=80) between -1 and 2"
[ WARN] [1679168142.915116408]: Odometry lost! Odometry will be reset after next 1 consecutive unsuccessful odometry updates...
[ WARN] [1679168143.115914792]: Could not get transform from vo to baselink after 0.200000 seconds (for stamp=1679168142.506509)! Error="canTransform: targetframe vo does not exist.. canTransform returned after 0.20073 timeout was 0.2.".
[ WARN] [1679168143.115975277]: Odometry automatically reset to latest computed pose!
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.
Asked by Devin1126 on 2023-03-18 23:38:35 UTC
Answers
I have figured out the issue to the problem I described above. The vis_odom_topic in the launch file /vo is actually supposed to be /rtabmap/vo because the visual odometry node in the rtabmap launch file launches with the namespace /rtabmap. The built map is much clearer now.
Asked by Devin1126 on 2023-03-25 11:26:48 UTC
Comments
This may not help but try using the robot_pose_ekf package it transforms odom->base_footprint. and maybe try setting visual_odometry to false
Asked by bribri123 on 2023-03-19 12:18:46 UTC