Re: Lidar SLAM with loop closure
Posted by
denzle on
Aug 04, 2022; 7:31pm
URL: http://official-rtab-map-forum.206.s1.nabble.com/Lidar-SLAM-with-loop-closure-tp7846p9142.html
Hi Antonis,
Below is the launch I've tested today that works for me. You will have to launch the ouster launch first then run the Rtab launch. You may need to make some changes to suit your own needs, such as the serial numbers and hostname etc.
Hope this helps
Cheers
Dan
<?xml version="1.0"?>
<launch>
<!--
Hand-held 3D lidar mapping example using only a Ouster GEN2 (no camera).
Prerequisities: rtabmap should be built with libpointmatcher
Example:
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX
$ rosrun rviz rviz -f map
RVIZ: Show TF and /rtabmap/cloud_map topics
ISSUE: You may have to reset odometry after receiving the first cloud if the map looks tilted. The problem seems
coming from the first cloud sent by os_cloud_node, which may be poorly synchronized with IMU data.
PTP mode (synchronize timestamp with host computer time)
* Install:
$ sudo apt install linuxptp httpie
$ printf "[global]\ntx_timestamp_timeout 10\n" >> ~/os.conf
* Running:
(replace "XXXXXXXXXXXX" by your ouster serial, as well as XXX by its IP address)
(replace "eth0" by the network interface used to communicate with ouster)
$ http PUT http://os-XXXXXXXXXXXX.local/api/v1/time/ptp/profile <<< '"default-relaxed"'
$ sudo ptp4l -i eth0 -m -f ~/os.conf -S
$ roslaunch rtabmap_ros test_ouster_gen2.launch sensor_hostname:=os-XXXXXXXXXXXX.local udp_dest:=192.168.1.XXX ptp:=true
-->
<!-- TF: x y z z° y° x° -->
<!--node pkg="tf2_ros" type="static_transform_publisher" name="odom_to_base_link" args="0 0 0 0 0 0 1 odom base_link " /-->
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_os_sensor" args="0 0 0 0 0 0 1 base_link os_sensor " />
<node pkg="tf2_ros" type="static_transform_publisher" name="base_link_to_camera_link" args="0 0 0 0 0 0 1 base_link camera_link " />
<!-- Ouster: -->
<arg name="sensor_hostname" default="os-122105000261.local"/>
<arg name="udp_dest" default="169.254.255.255"/>
<arg name="use_sim_time" default="false"/>
<arg name="rtabmapviz" default="true"/>
<arg name="scan_20_hz" default="true"/>
<arg name="voxel_size" default="0.15"/> <!-- indoor: 0.1 to 0.3, outdoor: 0.3 to 0.5 -->
<arg name="assemble" default="false"/>
<arg name="ptp" default="true"/> <!-- See comments in header to start before launching the launch -->
<arg name="distortion_correction" default="true"/> <!-- Requires this pull request: https://github.com/ouster-lidar/ouster_example/pull/245 -->
<param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>
<arg name="frame_id" default="base_link"/>
<arg name="use_rtabmapviz" default="true"/>
<arg name="use_rviz" default="true"/>
<arg name="gui_cfg" default="" />
<arg name="rviz_cfg" default="$(find rtabmap_ros)/launch/config/rgbd.rviz" />
<!-- Camera: -->
<arg name="use_d415" default="true"/>
<arg name="device_type_camera" default="D415"/> <!-- Note: using regular expression. match D435, D435i, D415... -->
<arg name="serial_no_camera" default="002422061074"/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix_camera" default="$(arg camera)"/>
<arg name="initial_reset" default="false"/>
<arg name="enable_fisheye" default="false"/>
<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="clip_distance" default="-2"/>
<arg name="calib_odom_file" default=""/>
<arg name="output" default="screen"/>
<arg name="approx_sync" default="true"/>
<arg name="queue_size" default="100"/>
<!-- Already synchronized RGB-D related topic, with rtabmap_ros/rgbd_sync nodelet -->
<arg name="approx_rgbd_sync" default="true"/> <!-- false=exact synchronization -->
<arg name="rgbd_sync" default="true"/> <!-- pre-sync rgb_topic, depth_topic, camera_info_topic -->
<arg name="subscribe_rgbd" default="$(arg rgbd_sync)"/>
<arg name="subscribe_rgb" default="false"/>
<arg name="subscribe_depth" default="false"/>
<arg name="subscribe_scan" default="false"/>
<arg name="scan_topic" default="/$(arg camera)/aligned_depth_to_color/image_raw"/>
<arg name="subscribe_scan_cloud" default="true"/>
<arg name="scan_cloud_topic" default="/os1_cloud_node/points"/>
<arg name="scan_cloud_max_points" default="327680"/>
<arg name="scan_cloud_filtered" default="false"/>
<arg name="scan_normal_k" default="0"/>
<arg name="visual_odometry" default="false"/>
<arg name="icp_odometry" default="true"/>
<arg name="args" default=""/>
<arg name="rtabmap_args" default="$(arg args)"/>
<arg name="launch_prefix" default=""/>
<arg name="odom_topic" default="odom"/> <!-- Odometry topic name -->
<arg name="vo_frame_id" default="$(arg odom_topic)"/> <!-- Visual/Icp odometry frame ID for TF -->
<arg name="tag_linear_variance" default="0.0001" />
<arg name="tag_angular_variance" default="9999" />
<arg name="tag_topic" default="/tag_detections" /> <!-- apriltags async subscription -->
<!-- RGB-D related topics -->
<arg name="camera_frame_id" default="base_link"/>
<arg name="rgb_topic" value="/$(arg camera)/color/image_raw"/>
<arg name="depth_topic" value="/$(arg camera)/aligned_depth_to_color/image_raw"/>
<arg name="camera_info_topic" value="/$(arg camera)/color/camera_info"/>
<arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
<arg name="rgbd_topic" default="rgbd_image" />
<!--remap from="rgbd_image" to="/camera/rgbd_image"/-->
<group ns="$(arg camera)">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="device_type" value="$(arg device_type_camera)"/>
<arg name="serial_no" value="$(arg serial_no_camera)"/>
<arg name="tf_prefix" value="$(arg tf_prefix_camera)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="align_depth" value="true"/>
<arg name="filters" value="pointcloud"/>
<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="clip_distance" value="$(arg clip_distance)"/>
</include>
</group>
<group ns="rtabmap">
<node pkg="nodelet" type="nodelet" name="rgbd_sync" args="standalone rtabmap_ros/rgbd_sync camera_nodelet_manager" output="screen">
<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="rgbd_image" to="$(arg rgbd_topic)"/> <!-- output -->
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
</node>
<!-- IMU orientation estimation and publish tf accordingly to os_sensor frame -->
<node pkg="nodelet" type="nodelet" name="imu_nodelet_manager" args="manager">
<remap from="imu/data_raw" to="/os_cloud_node/imu"/>
<remap from="imu/data" to="/os_cloud_node/imu/data"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_filter" args="load imu_filter_madgwick/ImuFilterNodelet imu_nodelet_manager">
<param name="use_mag" value="false"/>
<param name="world_frame" value="enu"/>
<param name="publish_tf" value="false"/>
</node>
<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="load rtabmap_ros/imu_to_tf imu_nodelet_manager">
<remap from="imu/data" to="/os_cloud_node/imu/data"/>
<param name="fixed_frame_id" value="$(arg frame_id)_stabilized"/>
<param name="base_frame_id" value="$(arg frame_id)"/>
</node>
<node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param if="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="25"/>
<param unless="$(arg scan_20_hz)" name="expected_update_rate" type="double" value="15"/>
<param name="guess_frame_id" type="string" value="$(arg frame_id)_stabilized"/>
<param name="wait_imu_to_init" type="bool" value="true"/>
<!-- ICP parameters -->
<param name="Icp/PointToPlane" type="string" value="true"/>
<param name="Icp/Iterations" type="string" value="30"/>
<param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
<param name="Icp/DownsamplingStep" type="string" value="1"/> <!-- cannot be increased with ring-like lidar -->
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/MaxTranslation" type="string" value="2"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/Strategy" type="string" value="true"/>
<param name="Icp/OutlierRatio" type="string" value="0.1"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>
<!-- Odom parameters -->
<param name="Odom/ScanKeyFrameThr" type="string" value="0.95"/>
<param name="Odom/Strategy" type="string" value="0"/>
<param name="OdomF2M/ScanSubtractRadius" type="string" value="$(arg voxel_size)"/>
<param name="OdomF2M/ScanMaxSize" type="string" value="15000"/>
</node>
<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
<remap from="tag_detections" to="$(arg tag_topic)" />
<remap if="$(arg assemble)" from="scan_cloud" to="assembled_cloud"/>
<remap unless="$(arg assemble)" from="scan_cloud" to="/os_cloud_node/points"/>
<remap from="imu" to="/os_cloud_node/imu/data"/>
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="apriltag_ros_continuous_node" type="bool" value="true"/>
<!-- RTAB-Map's parameters -->
<param if="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="0"/> <!-- already set 1 Hz in point_cloud_assembler -->
<param unless="$(arg assemble)" name="Rtabmap/DetectionRate" type="string" value="1"/>
<param name="RGBD/NeighborLinkRefining" type="string" value="false"/>
<param name="RGBD/ProximityBySpace" type="string" value="true"/>
<param name="RGBD/ProximityMaxGraphDepth" type="string" value="0"/>
<param name="RGBD/ProximityPathMaxNeighbors" type="string" value="1"/>
<param name="RGBD/LocalRadius" type="string" value="2"/>
<param name="RGBD/AngularUpdate" type="string" value="0.05"/>
<param name="RGBD/LinearUpdate" type="string" value="0.05"/>
<param name="Mem/NotLinkedNodesKept" type="string" value="false"/>
<param name="Mem/STMSize" type="string" value="30"/>
<!-- param name="Mem/LaserScanVoxelSize" type="string" value="0.1"/ -->
<!-- param name="Mem/LaserScanNormalK" type="string" value="10"/ -->
<!-- param name="Mem/LaserScanRadius" type="string" value="0"/ -->
<param name="Reg/Strategy" type="string" value="1"/>
<param name="Optimizer/GravitySigma" type="string" value="0.5"/>
<param name="Optimizer/Strategy" type="string" value="1"/>
<param name="Grid/CellSize" type="string" value="0.1"/>
<param name="Grid/RangeMax" type="string" value="20"/>
<param name="Grid/ClusterRadius" type="string" value="1"/>
<param name="Grid/GroundIsObstacle" type="string" value="true"/>
<param name="Grid/FromDepth" type="string" value="false"/>
<!--param name="Kp/MaxFeatures" type="string" value="-1"/-->
<param name="Kp/DetectorStrategy" type="string" value="6"/> <!--0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF(default) 7=BRISK 8=GFTT/ORB 9=KAZE-->
<param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy default set as 6-->
<!-- ICP parameters -->
<param name="Icp/VoxelSize" type="string" value="$(arg voxel_size)"/>
<param name="Icp/PointToPlaneK" type="string" value="20"/>
<param name="Icp/PointToPlaneRadius" type="string" value="0"/>
<param name="Icp/PointToPlane" type="string" value="false"/>
<param name="Icp/Iterations" type="string" value="10"/>
<param name="Icp/Epsilon" type="string" value="0.001"/>
<param name="Icp/MaxTranslation" type="string" value="3"/>
<param name="Icp/MaxCorrespondenceDistance" type="string" value="1"/>
<param name="Icp/PM" type="string" value="true"/>
<param name="Icp/PMOutlierRatio" type="string" value="0.7"/>
<param name="Icp/CorrespondenceRatio" type="string" value="0.2"/>
</node>
<node if="$(arg use_rtabmapviz)" name="rtabmapviz" pkg="rtabmap_ros" type="rtabmapviz" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="odom_frame_id" type="string" value="odom"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="subscribe_scan_cloud" type="bool" value="true"/>
<param name="approx_sync" type="bool" value="$(arg approx_sync)"/>
<param name="queue_size" type="int" value="$(arg queue_size)"/>
<param name="subscribe_rgb" type="bool" value="$(arg subscribe_rgb)"/>
<param name="subscribe_depth" type="bool" value="$(arg subscribe_depth)"/>
<param name="subscribe_rgbd" type="bool" value="$(arg subscribe_rgbd)"/>
<remap from="scan_cloud" to="/os_cloud_node/points"/>
<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="rgbd_image" to="$(arg rgbd_topic)"/> <!-- output -->
<remap from="tag_detections" to="$(arg tag_topic)" />
<param name="apriltag_ros_continuous_node" type="bool" value="true"/>
</node>
<node if="$(arg assemble)" pkg="rtabmap_ros" type="point_cloud_assembler" name="point_cloud_assembler" output="screen">
<remap from="cloud" to="/os_cloud_node/points"/>
<remap from="odom" to="odom"/>
<param name="assembling_time" type="double" value="1" />
<param name="fixed_frame_id" type="string" value="$(arg frame_id)_stabilized" />
</node>
</group>
<!-- Set parameters -->
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tag_settings.yaml" ns="apriltag_ros_continuous_node" />
<rosparam command="load" file="$(find rtabmap_ros)/launch/tests/tags.yaml" ns="apriltag_ros_continuous_node" />
<node pkg="apriltag_ros" type="apriltag_ros_continuous_node" name="apriltag_ros_continuous_node" clear_params="true" output="screen">
<remap from="image_rect" to="$(arg rgb_topic)" />
<remap from="camera_info" to="$(arg camera_info_topic)" />
<remap from="tag_detections" to="$(arg tag_topic)" />
<param name="camera_frame" type="str" value="$(arg frame_id)" />
<param name="publish_tag_detections_image" type="bool" value="true" /> <!-- default: false -->
</node>
<node if="$(arg use_rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(arg rviz_cfg)"/>
</launch>