Problem fusing LiDAR and depth (hector_slam)
Posted by utrecht on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-fusing-LiDAR-and-depth-hector-slam-tp4115.html
Hey! I've followed your tutorials and I've managed to get a good clear image with odometry with depth alone (using an R200). However, I added a LIDAR now (RPLiDAR A2) and there are errors when trying to match the scans with the pointclouds. When RTABMap first initializes the map actually looks quite nice with plenty details, as seen below:
However, when the drone moves around slightly, the scan and 3D cloud start jumping around and become mismatched. The map then gets broken, as seen below:
I think this has something to do with the static transform publishers in my code, but I just don't see where they mess it up (note, the RPLiDAR node initializes with a static transform publisher from base link to laser with 0 0 0 3.14159 0 0). Here is my TF tree:
Notice the transform publisher types. I chose to not use robot_state_publisher as I felt that it was unnecessary. Do correct me if I am wrong. Below is my custom launch file for RTABMap, with code borrowed from the demo_hector_mapping launch file(apologies for the length):
Do you have an idea what I can change in my code? Also, I need the Z values as well. Using RTABMap I can easily extract them, but since odometry is provided by hector_slam, RTABMap ceases to output odometry and I can only rely on the topic /poseupdate from hector_slam to provide me with odometry values in the XY plane. I've removed the Force3DoF parameter from the launch file but I think it does not matter. Yet If I put the visual odometry argument to be true I still don't get anything :(. Any ideas? Mathieu please save me!
URL: http://official-rtab-map-forum.206.s1.nabble.com/Problem-fusing-LiDAR-and-depth-hector-slam-tp4115.html
Hey! I've followed your tutorials and I've managed to get a good clear image with odometry with depth alone (using an R200). However, I added a LIDAR now (RPLiDAR A2) and there are errors when trying to match the scans with the pointclouds. When RTABMap first initializes the map actually looks quite nice with plenty details, as seen below:
However, when the drone moves around slightly, the scan and 3D cloud start jumping around and become mismatched. The map then gets broken, as seen below:
I think this has something to do with the static transform publishers in my code, but I just don't see where they mess it up (note, the RPLiDAR node initializes with a static transform publisher from base link to laser with 0 0 0 3.14159 0 0). Here is my TF tree:
Notice the transform publisher types. I chose to not use robot_state_publisher as I felt that it was unnecessary. Do correct me if I am wrong. Below is my custom launch file for RTABMap, with code borrowed from the demo_hector_mapping launch file(apologies for the length):
<?xml version="1.0"?>
<launch>
<arg name="tf_prefix" default="$(optenv ROS_NAMESPACE)"/>
<arg name="x" default="0.0"/>
<arg name="y" default="0.0"/>
<arg name="z" default="0.0"/>
<arg name="use_ground_truth_for_tf" default="true" />
<arg name="use_ground_truth_for_control" default="true" />
<arg name="world_frame" default="/world"/>
<arg name="base_link_frame" default="$(arg tf_prefix)/base_link"/>
<param name="tf_prefix" type="string" value="$(arg tf_prefix)"/> <!-- required for hector_pose_estimation and message_to_tf -->
<param name="base_link_frame" type="string" value="$(arg base_link_frame)" />
<param name="base_stabilized_frame" type="string" value="$(arg tf_prefix)/base_stabilized" />
<param name="base_footprint_frame" type="string" value="$(arg tf_prefix)/base_footprint" />
<param name="world_frame" type="string" value="$(arg world_frame)" />
<!-- publish state and tf -->
<node name="ground_truth_to_tf" pkg="message_to_tf" type="message_to_tf" output="screen">
<param name="odometry_topic" value="ground_truth/state" />
<param name="frame_id" value="$(arg world_frame)" />
<param name="tf_prefix" value="$(arg tf_prefix)" if="$(arg use_ground_truth_for_tf)" />
<param name="tf_prefix" value="$(arg tf_prefix)/ground_truth" unless="$(arg use_ground_truth_for_tf)" />
</node>
<node pkg="tf" type="static_transform_publisher" name="world_to_map"
args="0.0 0.0 0.0 0.0 0.0 0.0 world map 100" />
<node pkg="tf" type="static_transform_publisher" name="scanmatcher_to_base_footprint"
args="0.0 0.0 0.0 0.0 0.0 0.0 scanmatcher_frame base_footprint 100" />
<node pkg="laser_filters" type="scan_to_scan_filter_chain" name="laser_filter">
<rosparam>
scan_filter_chain:
- name: range
type: LaserScanRangeFilter
params:
lower_threshold: 0.25
upper_threshold: .inf
</rosparam>
<remap from="scan" to="/scan" />
<remap from="scan_filtered" to="/scan_filtered" />
</node>
<!-- Odometry from laser scans -->
<!-- We use Hector mapping to generate odometry for us -->
<node pkg="hector_mapping" type="hector_mapping" name="hector_mapping">
<!-- Frame names -->
<param name="map_frame" value="/map" />
<param name="base_frame" value="/base_footprint" />
<param name="odom_frame" value="/scanmatcher_frame" />
<!-- Tf use -->
<param name="pub_map_odom_transform" value="false"/>
<param name="pub_map_scanmatch_transform" value="true"/>
<param name="pub_odometry" value="true"/>
<!-- Map size / start point -->
<param name="map_resolution" value="0.050"/>
<param name="map_size" value="2048"/>
<param name="map_multi_res_levels" value="2" />
<!-- Map update parameters -->
<param name="map_update_angle_thresh" value="0.06" />
<!-- Advertising config -->
<param name="scan_topic" value="/scan_filtered"/>
</node>
<arg name="stereo" default="false" />
<!-- Choose visualization -->
<arg name="rtabmapviz" default="false" />
<arg name="rviz" default="true" />
<!-- Localization-only mode -->
<arg name="localization" default="false"/>
<!-- sim time for convenience, if playing a rosbag -->
<arg name="use_sim_time" default="false"/>
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<!-- Corresponding config files -->
<arg name="cfg" default="" /> <!-- To change RTAB-Map's parameters, set the path of config file (*.ini) generated by the standalone app -->
<arg name="gui_cfg" default="~/.ros/rtabmap_gui.ini" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<arg name="frame_id" default="base_footprint"/> <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
<arg name="odom_frame_id" default="/scanmatcher_frame"/> <!-- If set, TF is used to get odometry instead of the topic -->
<arg name="map_frame_id" default="map"/>
<arg name="ground_truth_frame_id" default=""/> <!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default=""/> <!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="namespace" default="rtabmap"/>
<arg name="database_path" default="~/.ros/rtabmap.db"/>
<arg name="queue_size" default="10"/>
<arg name="wait_for_transform" default="0.2"/>
<arg name="args" default=""/> <!-- delete_db_on_start, udebug -->
<arg name="rtabmap_args" default="--delete_db_on_start"/> <!-- deprecated, use "args" argument -->
<arg name="launch_prefix" default=""/> <!-- for debugging purpose, it fills launch-prefix tag of the nodes -->
<arg name="output" default="screen"/> <!--Control node output (screen or log)-->
<!-- if timestamps of the input topics are synchronized using approximate or exact time policy-->
<arg if="$(arg stereo)" name="approx_sync" default="false"/>
<arg unless="$(arg stereo)" name="approx_sync" default="true"/>
<!-- RGB-D related topics -->
<arg name="rgb_topic" default="/camera/rgb/image_rect_color" />
<arg name="depth_topic" default="/camera/depth_registered/sw_registered/image_rect_raw" />
<arg name="camera_info_topic" default="/camera/rgb/camera_info" />
<arg name="image_raw_topic" default="/camera/depth/points/image_raw" />
<arg name="cloud_topic" default="/camera/depth/points" />
<arg name="image_topic" default="/camera/depth/image" />
<arg name="_approx" default="false" />
<arg name="_fill_holes_size" default="2" />
<!-- stereo related topics -->
<arg name="stereo_namespace" default="/stereo_camera"/>
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect_color" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" /> <!-- using grayscale image for efficiency -->
<arg name="left_camera_info_topic" default="$(arg stereo_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg stereo_namespace)/right/camera_info" />
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="subscribe_rgbd" default="false"/>
<arg name="rgbd_topic" default="/camera/rgbd_image" />
<arg name="compressed" default="true"/> <!-- If you want to subscribe to compressed image topics -->
<arg name="rgb_image_transport" default="compressed"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="depth_image_transport" default="compressedDepth"/> <!-- Common types: compressed, theora (see "rosrun image_transport list_transports") -->
<arg name="subscribe_scan" default="true"/>
<arg name="scan_topic" default="/scan_filtered"/>
<arg name="subscribe_scan_cloud" default="false"/>
<arg name="scan_cloud_topic" default="/scan_cloud"/>
<arg name="scan_normal_k" default="0"/>
<arg name="visual_odometry" default="false"/> <!-- Launch rtabmap visual odometry node -->
<arg name="odom_topic" default="/scanmatcher_frame"/> <!-- Odometry topic used if visual_odometry is false -->
<arg name="vo_frame_id" default="odom"/>
<arg name="odom_tf_angular_variance" default="1"/> <!-- If TF is used to get odometry, this is the default angular variance -->
<arg name="odom_tf_linear_variance" default="1"/> <!-- If TF is used to get odometry, this is the default linear variance -->
<arg name="odom_args" default="$(arg rtabmap_args)"/>
<arg name="odom_sensor_sync" default="false"/>
<arg name="subscribe_user_data" default="false"/> <!-- user data synchronized subscription -->
<arg name="user_data_topic" default="/user_data"/>
<arg name="user_data_async_topic" default="/user_data_async" /> <!-- user data async subscription (rate should be lower than map update rate) -->
<!-- These arguments should not be modified directly, see referred topics without "_relay" suffix above -->
<arg if="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)_relay"/>
<arg unless="$(arg compressed)" name="rgb_topic_relay" default="$(arg rgb_topic)"/>
<arg if="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)_relay"/>
<arg unless="$(arg compressed)" name="depth_topic_relay" default="$(arg depth_topic)"/>
<arg if="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="left_image_topic_relay" default="$(arg left_image_topic)"/>
<arg if="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)_relay"/>
<arg unless="$(arg compressed)" name="right_image_topic_relay" default="$(arg right_image_topic)"/>
<node pkg="rtabmap_ros" type="pointcloud_to_depthimage" name="pointcloud_to_depthimage" args="$(arg cloud_topic) $(arg camera_info_topic) $(arg image_raw_topic) $(arg image_topic) $(arg _approx) $(arg _fill_holes_size)" />
<!-- Nodes -->
<group ns="$(arg namespace)">
<!-- RGB-D Odometry -->
<group unless="$(arg stereo)">
<node if="$(arg compressed)" name="republish_rgb" type="republish" pkg="image_transport" args="$(arg rgb_image_transport) in:=$(arg rgb_topic) raw out:=$(arg rgb_topic_relay)" />
<node if="$(arg compressed)" name="republish_depth" type="republish" pkg="image_transport" args="$(arg depth_image_transport) in:=$(arg depth_topic) raw out:=$(arg depth_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<param name="Vis/MinInliers" value="10"/>
<param name="Odom/ResetCountdown" value="1" />
<!-- 0: 3D - 3D, 1: 3D-2D -->
<param name="Odom/EstimationType" value="1"/>
<!-- Minimum Eucliedean distance in Mode 0 (3D-3D) -->
<param name="Odom/InlinerDistance" value="0.2" />
<!-- Reprojection error in Mode 1 (3D->2D) -->
<param name="Odom/PnPReprojError" value="5.0" />
<!-- Maximum depth -->
<param name="Odom/MaxDepth" value="10.0" />
<param name="GFFT/QualityLevel" value="0.001" />
<param name="GFFT/MinDistance" value="3" />
<param name="Kp/DetectorStrategy" value="6" />
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
</node>
</group>
<!-- Stereo Odometry -->
<group if="$(arg stereo)">
<node if="$(arg compressed)" name="republish_left" type="republish" pkg="image_transport" args="compressed in:=$(arg left_image_topic) raw out:=$(arg left_image_topic_relay)" />
<node if="$(arg compressed)" name="republish_right" type="republish" pkg="image_transport" args="compressed in:=$(arg right_image_topic) raw out:=$(arg right_image_topic_relay)" />
<node if="$(arg visual_odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="$(arg output)" args="$(arg odom_args)" launch-prefix="$(arg launch_prefix)">
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg vo_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
</node>
</group>
<!-- Visual SLAM (robot side) -->
<!-- args: "delete_db_on_start" and "udebug" -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="$(arg output)" args="$(arg rtabmap_args)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param unless="$(arg stereo)" name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_user_data" type="bool" value="$(arg subscribe_user_data)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="map_frame_id" type="string" value="$(arg map_frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="ground_truth_frame_id" type="string" value="$(arg ground_truth_frame_id)"/>
<param name="ground_truth_base_frame_id" type="string" value="$(arg ground_truth_base_frame_id)"/>
<param name="odom_tf_angular_variance" type="double" value="$(arg odom_tf_angular_variance)"/>
<param name="odom_tf_linear_variance" type="double" value="$(arg odom_tf_linear_variance)"/>
<param name="odom_sensor_sync" type="bool" value="$(arg odom_sensor_sync)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="database_path" type="string" value="$(arg database_path)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="config_path" type="string" value="$(arg cfg)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="scan_normal_k" type="int" value="$(arg scan_normal_k)"/>
<param name="Grid/FromDepth" type="string" value="true"/>
<param name="RGBD/OptimizeMaxError" value="0.2" />
<param name="Reg/Strategy" type="string" value="2"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.25"/>
<param name="Vis/MaxDepth" type="string" value="10.0"/>
<param name="Vis/InlierDistance" type="string" value="0.1"/>
<param name="RGBD/AngularUpdate" type="string" value="0.1"/>
<param name="RGBD/LinearUpdate" type="string" value="0.1"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap from="user_data" to="$(arg user_data_topic)"/>
<remap from="user_data_async" to="$(arg user_data_async_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
<!-- localization mode -->
<param if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
<param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
<param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(arg gui_cfg)" output="$(arg output)" launch-prefix="$(arg launch_prefix)">
<param if="$(arg stereo)" name="subscribe_depth" type="bool" value="false"/>
<param unless="$(arg stereo)" name="subscribe_depth" type="bool" value="true"/>
<param unless="$(arg stereo)" name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_stereo" type="bool" value="$(arg stereo)"/>
<param name="subscribe_scan" type="bool" value="$(arg subscribe_scan)"/>
<param name="subscribe_scan_cloud" type="bool" value="$(arg subscribe_scan_cloud)"/>
<param name="subscribe_odom_info" type="bool" value="$(arg visual_odometry)"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="$(arg odom_frame_id)"/>
<param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="rgbd_image" to="$(arg rgbd_topic)"/>
<remap from="left/image_rect" to="$(arg left_image_topic_relay)"/>
<remap from="right/image_rect" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="scan_cloud" to="$(arg scan_cloud_topic)"/>
<remap unless="$(arg visual_odometry)" from="odom" to="$(arg odom_topic)"/>
</node>
</group>
<!-- Visualization RVIZ -->
<node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
<node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="points_xyzrgb" args="standalone rtabmap_ros/point_cloud_xyzrgb">
<remap from="left/image" to="$(arg left_image_topic_relay)"/>
<remap from="right/image" to="$(arg right_image_topic_relay)"/>
<remap from="left/camera_info" to="$(arg left_camera_info_topic)"/>
<remap from="right/camera_info" to="$(arg right_camera_info_topic)"/>
<remap from="rgb/image" to="$(arg rgb_topic_relay)"/>
<remap from="depth/image" to="$(arg depth_topic_relay)"/>
<remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
<remap from="cloud" to="voxel_cloud" />
<param name="decimation" type="double" value="2"/>
<param name="voxel_size" type="double" value="0.02"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
<node pkg="tf" type="static_transform_publisher" name="footprint_to_base_link"
args="0.0 0.0 0.0 0.0 0.0 0.0 base_footprint base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_link_to_camera"
args="0.0 0.0 0.0 0.0 0.0 0.0 base_link camera_link 100" />
</launch>Do you have an idea what I can change in my code? Also, I need the Z values as well. Using RTABMap I can easily extract them, but since odometry is provided by hector_slam, RTABMap ceases to output odometry and I can only rely on the topic /poseupdate from hector_slam to provide me with odometry values in the XY plane. I've removed the Force3DoF parameter from the launch file but I think it does not matter. Yet If I put the visual odometry argument to be true I still don't get anything :(. Any ideas? Mathieu please save me!
| Free forum by Nabble | Edit this page |