Official RTAB-Map Forum

Lidar SLAM with loop closure

Classic

List

Threaded

19 messages Options

denzle

Lidar SLAM with loop closure

This post was updated on .

Hi,

So I've had great success using the ouster os0-32 and the intel realsense sensors separately for mapping purposes. But over larger areas the lidar does drift and with the depth sensors i'm using april tags to help with odometry and loop closures. I'm keen to test out stereo/lidar fusion so i can obtain loop closure whilst mapping with the lidar and pick up on the tags. I have had a brief play around trying to set up both sets of sensors but haven't spent too much time on it.

Has anyone successfully setup the ouster and realsense together? Or can anyone recommend a decent camera to use in such a setup and what changes were made to their launch file.

Cheers
Dan

denzle

Re: Lidar SLAM with loop closure

Just on a side note i have used the Zed cameras but having found the TX2 and Xavier just absolute pains in the back side to use I'm now using the Asus PN50 so don't have an Nvidia GPU to effectively run the Zed. And having tried to use the Zed as a USB camera it was a bit lacking so gave up on that avenue.

matlabbe

Re: Lidar SLAM with loop closure

Administrator

Hi,

the zed open capture library can help you to get Zed data (with imu) with a computer without CUDA. Note sure if a ros wrapper exists for the open capture library though. I integrated it inside rtabmap standalone.

For rtabmap node, you would have to pre-sync zed images and camera_info with rtabmap_ros/stereo_sync, then enable subscribe_scan_cloud to input the lidar. I didn't have time yet to add the examples to this page http://wiki.ros.org/rtabmap_ros/Tutorials/SetupOnYourRobot, but there is a launch file to play with different configurations of RGB-D / Velodyne / Wheel odometry with a simulated Husky: https://github.com/introlab/rtabmap_ros/blob/master/launch/demo/demo_husky.launch

cheers,
Mathieu

denzle

Re: Lidar SLAM with loop closure

Hi Mathieu,

So after much stressing and many failed test launch files i have finally got something close to my aim for loop closure. Although much is down to the new ouster launch file you recently added (so thanks for that as odometry is much more accurate and its sorted out the point cloud jumping around erratically with the ptp params you added).

Currently i have the camera able to pick up tags but I'm not sure if they should be showing up on Rtabviz or if the camera frame should be present rather than having to use rviz. Would i need set visual odometry as well?

I am sorry to ask you to have a look over my launch file as i'm sure you're very busy, but could you offer advice of where i may be going wrong or what may need to be changed.

Many thanks
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  -->
        
   <node pkg="tf2_ros" type="static_transform_publisher" name="os_sensor_to_d415_link" args="0 0 0 0 0 0 1 os_sensor d415_link" />

    
    <!-- Required: -->
    <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="approx_sync"     default="true"/>
    <arg name="frame_id"        default="os_sensor"/>
    <arg name="use_rtabmapviz"  default="true"/>
    <arg name="use_rviz"        default="true"/>
    <arg name="gui_cfg"         default="~/.ros/rtabmap_gui.ini" />
    <arg name="rviz_cfg"        default="$(find rtabmap_ros)/launch/config/rgbd.rviz" /> 
    <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="false"/> <!-- 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"/>
    
    
        <!-- Camera: -->
  <arg name="use_d415"               default="true"/>
  <arg name="device_type_camera2"    default="d415"/>  <!-- Note: using regular expression. match D435, D435i, D415... -->
  <arg name="serial_no_camera2"      default="002422061074"/>
  <arg name="camera2"                default="d415"/>
  <arg name="tf_prefix_camera2"      default="$(arg camera2)"/>
  
  <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=""/>
  
   <!-- RGB-D related topics -->
   <arg name="camera_frame_id"         default="/$(arg camera2)_color_optical_frame"/>
   <arg name="rgb_topic"             value="/$(arg camera2)/color/image_raw"/>
   <arg name="depth_topic"           value="/$(arg camera2)/aligned_depth_to_color/image_raw"/>
   <arg name="camera_info_topic"     value="/$(arg camera2)/color/camera_info"/>
   <arg name="depth_camera_info_topic" default="$(arg camera_info_topic)" />
   <arg name="rgbd_topic"              default="rgbd_image" />



   <group if="$(arg use_d415)" ns="$(arg camera2)">
   <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
   <node pkg="nodelet" type="nodelet" name="rgbd_sync" args="load rtabmap_ros/rgbd_sync nodelet_manager">
     <remap from="rgb/image"       to="color/image_raw"/>
     <remap from="depth/image"     to="aligned_depth_to_color/image_raw"/>
     <remap from="rgb/camera_info" to="color/camera_info"/>
    <param name="approx_sync"      type="bool" value="$(arg approx_sync)"/>
   </node>
  </group>

    <!-- 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> 

    <group ns="rtabmap">
      <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="scan_cloud_max_points" value="327680"/>
        <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="20000"/>      
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">	  
        <param name="frame_id"             type="string" value="$(arg frame_id)"/>  
        <param name="subscribe_depth"      type="bool" value="false"/>
        <param name="subscribe_rgb"        type="bool" value="false"/>
        <param name="subscribe_scan_cloud" type="bool" value="true"/>
        <param name="approx_sync"          type="bool" value="$(arg approx_sync)"/>
        <param name="scan_cloud_max_points" value="327680"/>
        <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"/>
     
        <!-- 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/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 -->
	<param name="Vis/FeatureType" type="string" value="6"/> <!--suppress warning,want to be same as KP/detectorstrategy -->
    
        <!-- 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="true"/>
        <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/Strategy"                         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 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="" />
      </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="subscribe_rgb"       type="bool"   value="false"/>
        <param name="subscribe_depth"     type="bool"   value="false"/>
        <param name="subscribe_rgbd"      type="bool"   value="false"/>
        <remap from="scan_cloud" to="/os_cloud_node/points"/>
      </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)" />

    <param name="camera_frame" type="str" value="$(arg camera_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>

SUMMARY
========

CLEAR PARAMETERS
* /apriltag_ros_continuous_node/

PARAMETERS
* /apriltag_ros_continuous_node/camera_frame: /d415_color_optic...
* /apriltag_ros_continuous_node/publish_tag_detections_image: True
* /apriltag_ros_continuous_node/publish_tf: True
* /apriltag_ros_continuous_node/standalone_tags: [{'id': 0, 'size'...
* /apriltag_ros_continuous_node/tag_blur: 0.0
* /apriltag_ros_continuous_node/tag_border: 1
* /apriltag_ros_continuous_node/tag_bundles: [{'name': 'tag_bu...
* /apriltag_ros_continuous_node/tag_debug: 0
* /apriltag_ros_continuous_node/tag_decimate: 1.0
* /apriltag_ros_continuous_node/tag_family: tag36h11
* /apriltag_ros_continuous_node/tag_refine_decode: 0
* /apriltag_ros_continuous_node/tag_refine_edges: 1
* /apriltag_ros_continuous_node/tag_refine_pose: 0
* /apriltag_ros_continuous_node/tag_threads: 2
* /d415/rgbd_sync/approx_sync: True
* /imu_filter/publish_tf: False
* /imu_filter/use_mag: False
* /imu_filter/world_frame: enu
* /imu_to_tf/base_frame_id: os_sensor
* /imu_to_tf/fixed_frame_id: os_sensor_stabilized
* /rosdistro: noetic
* /rosversion: 1.15.11
* /rtabmap/icp_odometry/Icp/CorrespondenceRatio: 0.01
* /rtabmap/icp_odometry/Icp/DownsamplingStep: 1
* /rtabmap/icp_odometry/Icp/Epsilon: 0.001
* /rtabmap/icp_odometry/Icp/Iterations: 30
* /rtabmap/icp_odometry/Icp/MaxCorrespondenceDistance: 1
* /rtabmap/icp_odometry/Icp/MaxTranslation: 2
* /rtabmap/icp_odometry/Icp/OutlierRatio: 0.1
* /rtabmap/icp_odometry/Icp/PointToPlane: true
* /rtabmap/icp_odometry/Icp/PointToPlaneK: 20
* /rtabmap/icp_odometry/Icp/PointToPlaneRadius: 0
* /rtabmap/icp_odometry/Icp/Strategy: true
* /rtabmap/icp_odometry/Icp/VoxelSize: 0.15
* /rtabmap/icp_odometry/Odom/ScanKeyFrameThr: 0.95
* /rtabmap/icp_odometry/Odom/Strategy: 0
* /rtabmap/icp_odometry/OdomF2M/ScanMaxSize: 20000
* /rtabmap/icp_odometry/OdomF2M/ScanSubtractRadius: 0.15
* /rtabmap/icp_odometry/expected_update_rate: 25.0
* /rtabmap/icp_odometry/frame_id: os_sensor
* /rtabmap/icp_odometry/guess_frame_id: os_sensor_stabilized
* /rtabmap/icp_odometry/odom_frame_id: odom
* /rtabmap/icp_odometry/scan_cloud_max_points: 327680
* /rtabmap/icp_odometry/wait_imu_to_init: True
* /rtabmap/point_cloud_assembler/assembling_time: 1.0
* /rtabmap/point_cloud_assembler/fixed_frame_id:
* /rtabmap/rtabmap/Grid/CellSize: 0.1
* /rtabmap/rtabmap/Grid/ClusterRadius: 1
* /rtabmap/rtabmap/Grid/FromDepth: false
* /rtabmap/rtabmap/Grid/GroundIsObstacle: true
* /rtabmap/rtabmap/Grid/RangeMax: 20
* /rtabmap/rtabmap/Icp/CorrespondenceRatio: 0.2
* /rtabmap/rtabmap/Icp/Epsilon: 0.001
* /rtabmap/rtabmap/Icp/Iterations: 10
* /rtabmap/rtabmap/Icp/MaxCorrespondenceDistance: 1
* /rtabmap/rtabmap/Icp/MaxTranslation: 3
* /rtabmap/rtabmap/Icp/PM: true
* /rtabmap/rtabmap/Icp/PMOutlierRatio: 0.7
* /rtabmap/rtabmap/Icp/PointToPlane: true
* /rtabmap/rtabmap/Icp/PointToPlaneK: 20
* /rtabmap/rtabmap/Icp/PointToPlaneRadius: 0
* /rtabmap/rtabmap/Icp/VoxelSize: 0.15
* /rtabmap/rtabmap/Kp/DetectorStrategy: 6
* /rtabmap/rtabmap/Mem/NotLinkedNodesKept: false
* /rtabmap/rtabmap/Mem/STMSize: 30
* /rtabmap/rtabmap/Optimizer/GravitySigma: 0.5
* /rtabmap/rtabmap/Optimizer/Strategy: 1
* /rtabmap/rtabmap/RGBD/AngularUpdate: 0.05
* /rtabmap/rtabmap/RGBD/LinearUpdate: 0.05
* /rtabmap/rtabmap/RGBD/LocalRadius: 2
* /rtabmap/rtabmap/RGBD/NeighborLinkRefining: false
* /rtabmap/rtabmap/RGBD/ProximityBySpace: true
* /rtabmap/rtabmap/RGBD/ProximityMaxGraphDepth: 0
* /rtabmap/rtabmap/RGBD/ProximityPathMaxNeighbors: 1
* /rtabmap/rtabmap/Reg/Strategy: 1
* /rtabmap/rtabmap/Rtabmap/DetectionRate: 0
* /rtabmap/rtabmap/Vis/FeatureType: 6
* /rtabmap/rtabmap/approx_sync: True
* /rtabmap/rtabmap/frame_id: os_sensor
* /rtabmap/rtabmap/scan_cloud_max_points: 327680
* /rtabmap/rtabmap/subscribe_depth: False
* /rtabmap/rtabmap/subscribe_rgb: False
* /rtabmap/rtabmap/subscribe_scan_cloud: True
* /rtabmap/rtabmapviz/approx_sync: True
* /rtabmap/rtabmapviz/frame_id: os_sensor
* /rtabmap/rtabmapviz/odom_frame_id: odom
* /rtabmap/rtabmapviz/subscribe_depth: False
* /rtabmap/rtabmapviz/subscribe_odom_info: True
* /rtabmap/rtabmapviz/subscribe_rgb: False
* /rtabmap/rtabmapviz/subscribe_rgbd: False
* /rtabmap/rtabmapviz/subscribe_scan_cloud: True

NODES
/
apriltag_ros_continuous_node (apriltag_ros/apriltag_ros_continuous_node)
imu_filter (nodelet/nodelet)
imu_nodelet_manager (nodelet/nodelet)
imu_to_tf (nodelet/nodelet)
os_sensor_to_d415_link (tf2_ros/static_transform_publisher)
rviz (rviz/rviz)
/d415/
nodelet_manager (nodelet/nodelet)
rgbd_sync (nodelet/nodelet)
/rtabmap/
icp_odometry (rtabmap_ros/icp_odometry)
point_cloud_assembler (rtabmap_ros/point_cloud_assembler)
rtabmap (rtabmap_ros/rtabmap)
rtabmapviz (rtabmap_ros/rtabmapviz)

ROS_MASTER_URI=http://localhost:11311

process[os_sensor_to_d415_link-1]: started with pid [141111]
process[d415/nodelet_manager-2]: started with pid [141112]
process[d415/rgbd_sync-3]: started with pid [141113]
process[imu_nodelet_manager-4]: started with pid [141115]
process[imu_filter-5]: started with pid [141120]
process[imu_to_tf-6]: started with pid [141121]
process[rtabmap/icp_odometry-7]: started with pid [141130]
process[rtabmap/rtabmap-8]: started with pid [141137]
process[rtabmap/point_cloud_assembler-9]: started with pid [141152]
process[rtabmap/rtabmapviz-10]: started with pid [141163]
process[apriltag_ros_continuous_node-11]: started with pid [141175]
process[rviz-12]: started with pid [141177]
[ INFO] [1622154587.177620239]: Initializing nodelet with 8 worker threads.
[ INFO] [1622154587.180684698]: Initializing nodelet with 8 worker threads.
[ INFO] [1622154587.263856157]: Loaded tag config: 0, size: 0.121, frame_name: TAG_0
[ INFO] [1622154587.263999153]: Loaded tag config: 1, size: 0.121, frame_name: TAG_1
[ INFO] [1622154587.264047002]: Loaded tag config: 2, size: 0.121, frame_name: TAG_2
[ INFO] [1622154587.264135387]: Loaded tag config: 137, size: 0.121, frame_name: TAG_137
[ INFO] [1622154587.264179429]: Loaded tag config: 184, size: 0.121, frame_name: TAG_184
[ INFO] [1622154587.264777894]: Loading tag bundle 'tag_bundle1'
[ INFO] [1622154587.264944093]: 0) id: 1, size: 0.062, p = [0,0,0], q = [1,0,0,0]
[ INFO] [1622154587.264997623]: 1) id: 2, size: 0.062, p = [0.077,0,0], q = [1,0,0,0]
[ INFO] [1622154587.265048378]: 2) id: 25, size: 0.062, p = [0,-0.077,0], q = [1,0,0,0]
[ INFO] [1622154587.265104252]: 3) id: 26, size: 0.062, p = [0.077,-0.077,0], q = [1,0,0,0]
[ INFO] [1622154587.265154856]: 4) id: 49, size: 0.062, p = [0,-0.154,0], q = [1,0,0,0]
[ INFO] [1622154587.265202916]: 5) id: 50, size: 0.062, p = [0.077,-0.154,0], q = [1,0,0,0]
[ WARN] [1622154587.265695503]: remove_duplicates parameter not provided. Defaulting to true
[ INFO] [1622154587.387657208]: Starting node...
[ INFO] [1622154587.413452565]: Initializing nodelet with 8 worker threads.
[ INFO] [1622154587.441852404]: Initializing nodelet with 8 worker threads.
[ INFO] [1622154587.499530779]: Odometry: frame_id = os_sensor
[ INFO] [1622154587.499559562]: Odometry: odom_frame_id = odom
[ INFO] [1622154587.499569090]: Odometry: publish_tf = true
[ INFO] [1622154587.499577265]: Odometry: wait_for_transform = true
[ INFO] [1622154587.499594227]: Odometry: wait_for_transform_duration = 0.100000
[ INFO] [1622154587.499622119]: Odometry: initial_pose = xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000
[ INFO] [1622154587.499631656]: Odometry: ground_truth_frame_id =
[ INFO] [1622154587.499639251]: Odometry: ground_truth_base_frame_id = os_sensor
[ INFO] [1622154587.499646464]: Odometry: config_path =
[ INFO] [1622154587.499653477]: Odometry: publish_null_when_lost = true
[ INFO] [1622154587.499660610]: Odometry: guess_frame_id = os_sensor_stabilized
[ INFO] [1622154587.499668024]: Odometry: guess_min_translation = 0.000000
[ INFO] [1622154587.499675608]: Odometry: guess_min_rotation = 0.000000
[ INFO] [1622154587.499682892]: Odometry: guess_min_time = 0.000000
[ INFO] [1622154587.499693161]: Odometry: expected_update_rate = 25.000000 Hz
[ INFO] [1622154587.499700495]: Odometry: max_update_rate = 0.000000 Hz
[ INFO] [1622154587.499707658]: Odometry: wait_imu_to_init = true
[ INFO] [1622154587.499733125]: Odometry: stereoParams_=0 visParams_=0 icpParams_=1
[ INFO] [1622154587.500166824]: Starting node...
[ INFO] [1622154587.502766627]: Setting odometry parameter "Icp/CorrespondenceRatio"="0.01"
[ INFO] [1622154587.503039876]: Setting odometry parameter "Icp/DownsamplingStep"="1"
[ INFO] [1622154587.503303317]: Setting odometry parameter "Icp/Epsilon"="0.001"
[ INFO] [1622154587.504162157]: Setting odometry parameter "Icp/Iterations"="30"
[ INFO] [1622154587.504396875]: Setting odometry parameter "Icp/MaxCorrespondenceDistance"="1"
[ INFO] [1622154587.505268448]: Setting odometry parameter "Icp/MaxTranslation"="2"
[ INFO] [1622154587.505582333]: Setting odometry parameter "Icp/OutlierRatio"="0.1"
[ INFO] [1622154587.509742764]: Setting odometry parameter "Icp/PointToPlane"="true"
[ INFO] [1622154587.511053736]: Setting odometry parameter "Icp/PointToPlaneK"="20"
[ INFO] [1622154587.513088707]: Setting odometry parameter "Icp/PointToPlaneRadius"="0"
[ INFO] [1622154587.516132087]: Setting odometry parameter "Icp/Strategy"="true"
[ INFO] [1622154587.516565685]: Setting odometry parameter "Icp/VoxelSize"="0.15"
[ INFO] [1622154587.536243826]: /rtabmap/point_cloud_assembler: queue_size=5
[ INFO] [1622154587.536324396]: /rtabmap/point_cloud_assembler: fixed_frame_id=
[ INFO] [1622154587.536349502]: /rtabmap/point_cloud_assembler: frame_id=
[ INFO] [1622154587.536619145]: /rtabmap/point_cloud_assembler: max_clouds=0
[ INFO] [1622154587.536694024]: /rtabmap/point_cloud_assembler: assembling_time=1.000000s
[ INFO] [1622154587.536774684]: /rtabmap/point_cloud_assembler: skip_clouds=0
[ INFO] [1622154587.536834386]: /rtabmap/point_cloud_assembler: circular_buffer=false
[ INFO] [1622154587.536880441]: /rtabmap/point_cloud_assembler: wait_for_transform_duration=0.100000
[ INFO] [1622154587.536913202]: /rtabmap/point_cloud_assembler: range_min=0.000000
[ INFO] [1622154587.537002088]: /rtabmap/point_cloud_assembler: range_max=0.000000
[ INFO] [1622154587.537062230]: /rtabmap/point_cloud_assembler: voxel_size=0.000000m
[ INFO] [1622154587.537108456]: /rtabmap/point_cloud_assembler: noise_radius=0.000000m
[ INFO] [1622154587.537166715]: /rtabmap/point_cloud_assembler: noise_min_neighbors=5
[ INFO] [1622154587.569512234]:
/rtabmap/point_cloud_assembler subscribed to (exact sync):
/os_cloud_node/points,
/rtabmap/odom
[ INFO] [1622154587.573658879]: Setting odometry parameter "Odom/ScanKeyFrameThr"="0.95"
[ INFO] [1622154587.574182835]: Setting odometry parameter "Odom/Strategy"="0"
[ INFO] [1622154587.581614831]: Setting odometry parameter "OdomF2M/ScanMaxSize"="20000"
[ INFO] [1622154587.585845182]: Setting odometry parameter "OdomF2M/ScanSubtractRadius"="0.15"
[ INFO] [1622154587.612405224]: /rtabmap/rtabmap(maps): map_filter_radius = 0.000000
[ INFO] [1622154587.612449616]: /rtabmap/rtabmap(maps): map_filter_angle = 30.000000
[ INFO] [1622154587.612465656]: /rtabmap/rtabmap(maps): map_cleanup = true
[ INFO] [1622154587.612482468]: /rtabmap/rtabmap(maps): map_always_update = false
[ INFO] [1622154587.612498968]: /rtabmap/rtabmap(maps): map_empty_ray_tracing = true
[ INFO] [1622154587.612515509]: /rtabmap/rtabmap(maps): cloud_output_voxelized = true
[ INFO] [1622154587.612531659]: /rtabmap/rtabmap(maps): cloud_subtract_filtering = false
[ INFO] [1622154587.612548961]: /rtabmap/rtabmap(maps): cloud_subtract_filtering_min_neighbors = 2
[ INFO] [1622154587.625916135]: /rtabmap/rtabmap(maps): octomap_tree_depth = 16
[ INFO] [1622154587.690375973]: rtabmap: frame_id = os_sensor
[ INFO] [1622154587.690419875]: rtabmap: map_frame_id = map
[ INFO] [1622154587.690446685]: rtabmap: use_action_for_goal = false
[ INFO] [1622154587.690469608]: rtabmap: tf_delay = 0.050000
[ INFO] [1622154587.690489836]: rtabmap: tf_tolerance = 0.100000
[ INFO] [1622154587.690514802]: rtabmap: odom_sensor_sync = false
[ INFO] [1622154587.691037626]: rtabmap: gen_scan = false
[ INFO] [1622154587.691061200]: rtabmap: gen_depth = false
[ INFO] [1622154587.692605828]: rtabmapviz: Using configuration from "/home/dan/.ros/rtabmapGUI.ini"
[ INFO] [1622154587.692830987]: rtabmap: scan_cloud_max_points = 327680
[ INFO] [1622154587.810145715]: Setting RTAB-Map parameter "Grid/CellSize"="0.1"
[ INFO] [1622154587.810672315]: Setting RTAB-Map parameter "Grid/ClusterRadius"="1"
[ WARN] [1622154587.819272658]: Odometry: Parameter name changed: "Icp/PM" -> "Icp/Strategy". Please update your launch file accordingly. Value "true" is still set to the new parameter name.
[ WARN] [1622154587.821183157]: Odometry: Parameter name changed: "Icp/PMOutlierRatio" -> "Icp/OutlierRatio". Please update your launch file accordingly. Value "0.1" is still set to the new parameter name.
[ INFO] [1622154587.825448934]: Setting RTAB-Map parameter "Grid/FromDepth"="false"
[ INFO] [1622154587.826171761]: Setting RTAB-Map parameter "Grid/GroundIsObstacle"="true"
[ INFO] [1622154587.860052902]: Setting RTAB-Map parameter "Grid/RangeMax"="20"
[ WARN] [1622154587.887603038]: IcpOdometry: Transferring value 0.15 of "Icp/VoxelSize" to ros parameter "scan_voxel_size" for convenience. "Icp/VoxelSize" is set to 0.
[ WARN] [1622154587.888084284]: IcpOdometry: Transferring value 20 of "Icp/PointToPlaneK" to ros parameter "scan_normal_k" for convenience.
[ WARN] (2021-05-27 23:29:47.888) OdometryF2M.cpp:159::OdometryF2M() OdomF2M/BundleAdjustment=1 cannot be used with registration not done only with images (Reg/Strategy=1), disabling bundle adjustment.
[ INFO] [1622154587.891382379]: Setting RTAB-Map parameter "Icp/CorrespondenceRatio"="0.2"
[ INFO] [1622154587.893635165]: Setting RTAB-Map parameter "Icp/Epsilon"="0.001"
[ INFO] [1622154587.896090329]: Setting RTAB-Map parameter "Icp/Iterations"="10"
[ INFO] [1622154587.896878518]: Setting RTAB-Map parameter "Icp/MaxCorrespondenceDistance"="1"
[ INFO] [1622154587.899196937]: odometry: Subscribing to IMU topic /os_cloud_node/imu/data
[ INFO] [1622154587.899509139]: Setting RTAB-Map parameter "Icp/MaxTranslation"="3"
[ INFO] [1622154587.904735956]: IcpOdometry: scan_cloud_max_points = 327680
[ INFO] [1622154587.904785248]: IcpOdometry: scan_downsampling_step = 1
[ INFO] [1622154587.904808862]: IcpOdometry: scan_range_min = 0.000000 m
[ INFO] [1622154587.904854457]: IcpOdometry: scan_range_max = 0.000000 m
[ INFO] [1622154587.904896625]: IcpOdometry: scan_voxel_size = 0.150000 m
[ INFO] [1622154587.904919217]: IcpOdometry: scan_normal_k = 20
[ INFO] [1622154587.904953922]: IcpOdometry: scan_normal_radius = 0.000000 m
[ INFO] [1622154587.904972286]: IcpOdometry: scan_normal_ground_up = 0.000000
[ INFO] [1622154587.911833830]: Setting RTAB-Map parameter "Icp/PointToPlane"="true"
[ INFO] [1622154587.914464220]: Setting RTAB-Map parameter "Icp/PointToPlaneK"="20"
[ INFO] [1622154587.917693977]: Setting RTAB-Map parameter "Icp/PointToPlaneRadius"="0"
[ INFO] [1622154587.920840569]: Setting RTAB-Map parameter "Icp/VoxelSize"="0.15"
[ INFO] [1622154587.937471623]: Setting RTAB-Map parameter "Kp/DetectorStrategy"="6"
[ INFO] [1622154587.975618801]: Setting RTAB-Map parameter "Mem/NotLinkedNodesKept"="false"
[ INFO] [1622154587.982652625]: Setting RTAB-Map parameter "Mem/STMSize"="30"
[ INFO] [1622154587.994437681]: Setting RTAB-Map parameter "Optimizer/GravitySigma"="0.5"
[ INFO] [1622154587.996878047]: Setting RTAB-Map parameter "Optimizer/Strategy"="1"
[ INFO] [1622154588.002149307]: Setting RTAB-Map parameter "RGBD/AngularUpdate"="0.05"
[ INFO] [1622154588.007282430]: Setting RTAB-Map parameter "RGBD/LinearUpdate"="0.05"
[ INFO] [1622154588.009863950]: Setting RTAB-Map parameter "RGBD/LocalRadius"="2"
[ INFO] [1622154588.017741285]: Setting RTAB-Map parameter "RGBD/NeighborLinkRefining"="false"
[ INFO] [1622154588.023738158]: Setting RTAB-Map parameter "RGBD/ProximityBySpace"="true"
[ INFO] [1622154588.024499616]: Setting RTAB-Map parameter "RGBD/ProximityMaxGraphDepth"="0"
[ INFO] [1622154588.026428479]: Setting RTAB-Map parameter "RGBD/ProximityPathMaxNeighbors"="1"
[ INFO] [1622154588.029286464]: Setting RTAB-Map parameter "Reg/Strategy"="1"
[ INFO] [1622154588.030635958]: Setting RTAB-Map parameter "Rtabmap/DetectionRate"="0"
[ INFO] [1622154588.097520584]: Setting RTAB-Map parameter "Vis/FeatureType"="6"
[ WARN] [1622154588.099310770]: IcpOdometry: "scan_cloud_max_points" is set to 327680 but input cloud is not dense and has a size of 32768 (1024x32), setting to this later size.
[ WARN] (2021-05-27 23:29:48.104) Odometry.cpp:311::process() Updated initial pose from xyz=0.000000,0.000000,0.000000 rpy=0.000000,-0.000000,0.000000 to xyz=0.000000,0.000000,0.000000 rpy=0.029307,-0.593729,-0.040529 with IMU orientation
[ WARN] (2021-05-27 23:29:48.104) Transform.cpp:517::getTransform() No transform found for stamp 1622154588.034613! Earliest is 1622154588.087633
[ INFO] [1622154588.105445869]: Odom: ratio=0.000000, std dev=99.995000m|99.995000rad, update time=0.000692s
[ WARN] [1622154588.142757731]: Rtabmap: Parameter name changed: "Icp/PM" -> "Icp/Strategy". Please update your launch file accordingly. Value "true" is still set to the new parameter name.
[ WARN] [1622154588.144735285]: Rtabmap: Parameter name changed: "Icp/PMOutlierRatio" -> "Icp/OutlierRatio". Please update your launch file accordingly. Value "0.7" is still set to the new parameter name.
[ WARN] (2021-05-27 23:29:48.160) Transform.cpp:517::getTransform() No transform found for stamp 1622154588.084641! Earliest is 1622154588.087633
[ INFO] [1622154588.170148179]: Odom: ratio=0.973121, std dev=0.004842m|0.001531rad, update time=0.010052s
[ INFO] [1622154588.217444382]: Odom: ratio=0.962703, std dev=0.004863m|0.001538rad, update time=0.010833s
[ INFO] [1622154588.262793317]: Odom: ratio=0.976074, std dev=0.004837m|0.001530rad, update time=0.010978s
[ INFO] [1622154588.308926423]: RTAB-Map detection rate = 0.000000 Hz
[ INFO] [1622154588.309070111]: rtabmap: Deleted database "/home/dan/.ros/rtabmap.db" (--delete_db_on_start or -d are set).
[ INFO] [1622154588.309095638]: rtabmap: Using database from "/home/dan/.ros/rtabmap.db" (0 MB).
[ INFO] [1622154588.309842339]: Odom: ratio=0.980885, std dev=0.005023m|0.001588rad, update time=0.008335s
[ WARN] (2021-05-27 23:29:48.319) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ WARN] (2021-05-27 23:29:48.320) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ WARN] (2021-05-27 23:29:48.324) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ INFO] [1622154588.367599251]: Odom: ratio=0.970141, std dev=0.005005m|0.001583rad, update time=0.009202s
[ INFO] [1622154588.415769702]: Odom: ratio=0.979235, std dev=0.004949m|0.001565rad, update time=0.007296s
[ INFO] [1622154588.468553228]: Odom: ratio=0.979902, std dev=0.004876m|0.001542rad, update time=0.008483s
[ WARN] (2021-05-27 23:29:48.483) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
[ INFO] [1622154588.485600156]: rtabmap: Database version = "0.20.11".
[ INFO] [1622154588.485668343]: rtabmap: SLAM mode (Mem/IncrementalMemory=true)
[ INFO] [1622154588.514029691]: /rtabmap/rtabmap: subscribe_depth = false
[ INFO] [1622154588.514119147]: /rtabmap/rtabmap: subscribe_rgb = false
[ INFO] [1622154588.514144585]: /rtabmap/rtabmap: subscribe_stereo = false
[ INFO] [1622154588.514168379]: /rtabmap/rtabmap: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1622154588.514198705]: /rtabmap/rtabmap: subscribe_odom_info = false
[ INFO] [1622154588.514221638]: /rtabmap/rtabmap: subscribe_user_data = false
[ INFO] [1622154588.514244942]: /rtabmap/rtabmap: subscribe_scan = false
[ INFO] [1622154588.514268265]: /rtabmap/rtabmap: subscribe_scan_cloud = true
[ INFO] [1622154588.514290546]: /rtabmap/rtabmap: subscribe_scan_descriptor = false
[ INFO] [1622154588.514314691]: /rtabmap/rtabmap: queue_size = 10
[ INFO] [1622154588.514337274]: /rtabmap/rtabmap: approx_sync = true
[ INFO] [1622154588.514425328]: Setup scan callback
[ INFO] [1622154588.515885608]: Odom: ratio=0.968427, std dev=0.005166m|0.001634rad, update time=0.010854s
[ INFO] [1622154588.522116387]:
/rtabmap/rtabmap subscribed to (approx sync):
/rtabmap/odom \
/rtabmap/assembled_cloud
[ WARN] [1622154588.522180466]: There is no image subscription, bag-of-words loop closure detection will be disabled...
[ WARN] [1622154588.522234326]: Setting Kp/MaxFeatures=-1 (bag-of-words disabled)
[ WARN] (2021-05-27 23:29:48.522) Features2d.cpp:561::create() BRIEF, FREAK and DAISY features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
libpng warning: iCCP: known incorrect sRGB profile
[ INFO] [1622154588.568050421]: Odom: ratio=0.972648, std dev=0.004934m|0.001560rad, update time=0.018402s
[ INFO] [1622154588.611994048]: Odom: ratio=0.979825, std dev=0.004889m|0.001546rad, update time=0.011261s
[ INFO] [1622154588.663795895]: rtabmap 0.20.11 started...
[ INFO] [1622154588.666417128]: Odom: ratio=0.975068, std dev=0.005241m|0.001657rad, update time=0.014690s
[ INFO] [1622154588.716093546]: Odom: ratio=0.979637, std dev=0.004980m|0.001575rad, update time=0.009566s
[ INFO] [1622154588.766323875]: rtabmapviz: Reading parameters from the ROS server...
[ INFO] [1622154588.767873302]: Odom: ratio=0.980937, std dev=0.004975m|0.001573rad, update time=0.009463s
[ INFO] [1622154588.825297493]: Odom: ratio=0.982571, std dev=0.005131m|0.001623rad, update time=0.022376s
[ INFO] [1622154588.864029981]: Odom: ratio=0.974781, std dev=0.004996m|0.001580rad, update time=0.009635s
[ INFO] [1622154588.923431316]: Odom: ratio=0.981481, std dev=0.004989m|0.001578rad, update time=0.019884s
[ INFO] [1622154588.964096786]: Odom: ratio=0.974303, std dev=0.004886m|0.001545rad, update time=0.009550s
[ INFO] [1622154589.012680978]: Odom: ratio=0.975490, std dev=0.004776m|0.001510rad, update time=0.008021s
[ INFO] [1622154589.062924041]: rtabmapviz: Parameters read = 349
[ INFO] [1622154589.062958716]: rtabmapviz: Parameters successfully read.
[ INFO] [1622154589.067337474]: Odom: ratio=0.975784, std dev=0.004953m|0.001566rad, update time=0.006871s
[ INFO] [1622154589.119511373]: Odom: ratio=0.975530, std dev=0.004735m|0.001497rad, update time=0.010999s
[ INFO] [1622154589.161548198]: Odom: ratio=0.979292, std dev=0.004927m|0.001558rad, update time=0.006362s
[ INFO] [1622154589.214127023]: Odom: ratio=0.973513, std dev=0.004907m|0.001552rad, update time=0.010912s
[ INFO] [1622154589.266009610]: Odom: ratio=0.972055, std dev=0.005174m|0.001636rad, update time=0.011738s
[ INFO] [1622154589.319580312]: Odom: ratio=0.973180, std dev=0.004848m|0.001533rad, update time=0.010601s
[ INFO] [1622154589.369237363]: /rtabmap/rtabmapviz: subscribe_depth = false
[ INFO] [1622154589.369267830]: /rtabmap/rtabmapviz: subscribe_rgb = false
[ INFO] [1622154589.369276216]: /rtabmap/rtabmapviz: subscribe_stereo = false
[ INFO] [1622154589.369299449]: /rtabmap/rtabmapviz: subscribe_rgbd = false (rgbd_cameras=1)
[ INFO] [1622154589.369306091]: /rtabmap/rtabmapviz: subscribe_odom_info = true
[ INFO] [1622154589.369322612]: /rtabmap/rtabmapviz: subscribe_user_data = false
[ INFO] [1622154589.369330216]: /rtabmap/rtabmapviz: subscribe_scan = false
[ INFO] [1622154589.369338512]: /rtabmap/rtabmapviz: subscribe_scan_cloud = true
[ INFO] [1622154589.369346076]: /rtabmap/rtabmapviz: subscribe_scan_descriptor = false
[ INFO] [1622154589.369353780]: /rtabmap/rtabmapviz: queue_size = 10
[ INFO] [1622154589.369362517]: /rtabmap/rtabmapviz: approx_sync = true
[ INFO] [1622154589.369406629]: Setup scan callback
[ INFO] [1622154589.370985681]: Odom: ratio=0.978190, std dev=0.004943m|0.001563rad, update time=0.008868s
[ INFO] [1622154589.372437094]:
/rtabmap/rtabmapviz subscribed to (approx sync):
/os_cloud_node/points \
/rtabmap/odom_info
[ INFO] [1622154589.372580932]: rtabmapviz started.
[ INFO] [1622154589.429056296]: Odom: ratio=0.977049, std dev=0.005176m|0.001637rad, update time=0.019174s
[ INFO] [1622154589.470694528]: Odom: ratio=0.976516, std dev=0.004929m|0.001559rad, update time=0.011734s
[ INFO] [1622154589.529006213]: Odom: ratio=0.973799, std dev=0.004909m|0.001552rad, update time=0.019547s
[ INFO] [1622154589.569012044]: Odom: ratio=0.975996, std dev=0.004875m|0.001542rad, update time=0.017478s
[ INFO] [1622154589.614063705]: Odom: ratio=0.975490, std dev=0.004911m|0.001553rad, update time=0.008343s
[ INFO] [1622154589.662077575]: Odom: ratio=0.975570, std dev=0.004987m|0.001577rad, update time=0.007600s
[ INFO] [1622154589.726001846]: Odom: ratio=0.977448, std dev=0.004948m|0.001565rad, update time=0.021868s
[ INFO] [1622154589.770482453]: Odom: ratio=0.967812, std dev=0.005020m|0.001587rad, update time=0.015083s
[ INFO] [1622154589.813416740]: Odom: ratio=0.980906, std dev=0.005032m|0.001591rad, update time=0.007112s
[ INFO] [1622154589.867796830]: Odom: ratio=0.973399, std dev=0.005039m|0.001594rad, update time=0.014096s
[ INFO] [1622154589.918483930]: Odom: ratio=0.976282, std dev=0.004974m|0.001573rad, update time=0.010948s
[ INFO] [1622154589.969405297]: Odom: ratio=0.977448, std dev=0.004874m|0.001541rad, update time=0.008710s
[FATAL] (2021-05-27 23:29:50.001) MsgConversion.cpp:2195::convertScan3dMsg() Condition (scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height) not met! [data=34603008 row_step=1081344 height=1]
terminate called after throwing an instance of 'UException'
what(): [FATAL] (2021-05-27 23:29:50.001) MsgConversion.cpp:2195::convertScan3dMsg() Condition (scan3dMsg.data.size() == scan3dMsg.row_step*scan3dMsg.height) not met! [data=34603008 row_step=1081344 height=1]
[ INFO] [1622154590.020932612]: Odom: ratio=0.963348, std dev=0.005139m|0.001625rad, update time=0.009999s
[ INFO] [1622154590.065775945]: Odom: ratio=0.969730, std dev=0.005200m|0.001645rad, update time=0.006349s
[ INFO] [1622154590.123377627]: Odom: ratio=0.974512, std dev=0.005369m|0.001698rad, update time=0.018996s
[ INFO] [1622154590.161834673]: Odom: ratio=0.972885, std dev=0.005161m|0.001632rad, update time=0.008088s
[ INFO] [1622154590.212860564]: Odom: ratio=0.969598, std dev=0.005044m|0.001595rad, update time=0.011122s
[ INFO] [1622154590.264260642]: Odom: ratio=0.973956, std dev=0.005252m|0.001661rad, update time=0.010298s
[rtabmap/rtabmap-8] process has died [pid 141137, exit code -6, cmd /home/dan/catkin_ws/devel/lib/rtabmap_ros/rtabmap -d scan_cloud:=assembled_cloud imu:=/os_cloud_node/imu/data __name:=rtabmap __log:=/home/dan/.ros/log/a0365fd2-bf27-11eb-a7ca-57629d3c5fcb/rtabmap-rtabmap-8.log].
log file: /home/dan/.ros/log/a0365fd2-bf27-11eb-a7ca-57629d3c5fcb/rtabmap-rtabmap-8*.log
[ INFO] [1622154590.320630069]: Odom: ratio=0.971647, std dev=0.005206m|0.001646rad, update time=0.013118s
[ INFO] [1622154590.374222001]: Odom: ratio=0.966830, std dev=0.005115m|0.001617rad, update time=0.015954s
[ INFO] [1622154590.418617940]: Odom: ratio=0.968091, std dev=0.005109m|0.001616rad, update time=0.008908s
[ INFO] [1622154590.473458809]: Odom: ratio=0.969846, std dev=0.005160m|0.001632rad, update time=0.013763s
[ INFO] [1622154590.524588183]: Odom: ratio=0.970636, std dev=0.005149m|0.001628rad, update time=0.022895s
[ INFO] [1622154590.567025033]: Odom: ratio=0.957504, std dev=0.004959m|0.001568rad, update time=0.015617s
[ INFO] [1622154590.622036138]: Odom: ratio=0.974415, std dev=0.005340m|0.001689rad, update time=0.018483s
[ INFO] [1622154590.664231799]: Odom: ratio=0.965928, std dev=0.005125m|0.001621rad, update time=0.009049s
[ INFO] [1622154590.716262232]: Odom: ratio=0.965517, std dev=0.005022m|0.001588rad, update time=0.013089s
[ INFO] [1622154590.768551066]: Odom: ratio=0.965479, std dev=0.005302m|0.001677rad, update time=0.012671s

matlabbe

Re: Lidar SLAM with loop closure

Administrator

This post was updated on .

Hi,

When a tag is detected, in rtabmapviz a small coordinate frame will appear in the 3D Map view. In GraphView, you would see dark green links representing the landmark added to graph. When re-detecting a tag, the loop closure detection view would turn Orange with "Landmark match #" title (see https://github.com/introlab/rtabmap/issues/377#issuecomment-482765725 and http://official-rtab-map-forum.206.s1.nabble.com/Best-Practices-for-Quick-Automatic-Localization-tp6881p6915.html). You didn't remap explicitly the tag topic to rtabmap node, make sure it is connected to apriltag_ros output topic ("rosnode info /rtabmap/rtabmap | grep tag" or rqt_graph).

For the ASSERT, which rtabmap version do you have? A similar bug was fixed (on March 28, 2021) by this commit when using point_cloud_assembler (a bug inside PCL when appending a cloud to another one).

cheers,
Mathieu

denzle

Re: Lidar SLAM with loop closure

Many thanks for your reply it is greatly appreciated.

I'm using the latest version of rtabmap. i had to do a fresh install yesterday due to a corrupt SSD.

Please forgive my following bonehead/simpleton question but using grep i get the following:

dan@PN50:~$ rosnode info /rtabmap/rtabmap | grep tag
 * /rtabmap/tag_detections [unknown type]
    * to: /apriltag_ros_continuous_node (http://PN50:35167/)

From reading the link you provided and your advice i would need to add something similar to below under the rtabmap node?

<param name="tag_detections"   type="bool"   value="true"/>

<remap from="/rtabmap/tag_detections" to="/apriltag_ros_continuous_node" />

Regards
Dan

denzle

Re: Lidar SLAM with loop closure

Sorry, I've sort of made some progress. Rtabmapviz is recognising the tag now

^Cdan@PN50:~$ rosnode info /rtabmap/rtabmap | grep tag
 * /rtabmap/tag_detections [apriltag_ros/AprilTagDetectionArray]
    * to: /apriltag_ros_continuous_node (http://PN50:37801/)
 * topic: /rtabmap/tag_detections
    * to: /apriltag_ros_continuous_node (http://PN50:37801/)

 <param name="apriltag_ros_continuous_node"        type="bool"   value="true"/>

matlabbe

Re: Lidar SLAM with loop closure

Administrator

Hi,

In the rqt_graph, you can hide /tf topics, which are linked to almost every nodes. Not sure if the second frame you are seeing is a tag. It could be the origin frame. The rosnode info was just to see to which tag topics rtabmap is subscribing to "/rtabmap/tag_detections". You can do also "rosnode info /apriltag_ros_continuous_node" to see the topic name, then remap if needed to connect the two nodes (which should be seen in rqt_graph).

denzle

Re: Lidar SLAM with loop closure

Many thanks for your reply.

The second frame is indeed a tag it is somewhat off as i haven't properly mounted the camera and lidar together yet whilst i try and get everything working properly.

I have re-read through the links you supplied making some changes to the launch file. Currently rtabmapviz is recognising the tags albeit a bit off from its true location. I've added some pics below of rtabmapviz and rqt_graph having made some changes.

From the errors the problem appears to stem from rgbd_sync (although i may be wrong) as setting subscribe_rgbd in rtabmap node just blacks out the screen only working if set to true in rtabmapviz node. Setting rgbd_sync in rtabmapviz node removes the laser scan feed only showing the point cloud which isn't too much of a problem. Whilst setting no subscribe nodes returns the laser scan feed to the point cloud whilst still recognising the tag as a landmark. but receive:

[ INFO] [1622591199.428706352]: Odom: ratio=0.766439, std dev=0.024672m|0.007802rad, update time=0.052901s
[ WARN] (2021-06-02 00:46:39.435) Rtabmap.cpp:3049::process() Rejecting all added loop closures (1, first is 123 <-> 13) in this iteration because a wrong loop closure has been detected after graph optimization, resulting in a maximum graph error ratio of 3.064820 (edge 39->40, type=0, abs error=0.081044 m, stddev=0.026443). The maximum error ratio parameter "RGBD/OptimizeMaxError" is 3.000000 of std deviation.
[ WARN] (2021-06-02 00:46:39.435) Rtabmap.cpp:3098::process() Loop closure 123->13 rejected!
[ INFO] [1622591199.436998227]: rtabmap (123): Rate=1.00s, Limit=0.000s, Conversion=0.0017s, RTAB-Map=0.0723s, Maps update=0.0005s pub=0.0000s (local map=49, WM=47)

I've tried multiple configurations within the launch file but i'm still unable to get a live camera feed into rtabmapviz.

denzle

Re: Lidar SLAM with loop closure

Having had more time troubleshooting i've found that the rbgd_sync problem related to having the D415 launch nodelet outside of the launch file. With it now included rtabmap and the realsense manager are subscribing properly. Also the rgbd_sync nodelet needs to be within the rtabmap group. Then setting approx sync has kicked everything in.

Now my next steps are to ensure the point cloud is made up from the lidar rather than the camera and have the camera solely for tag detection. Hopefully i'll be able to make some more progress after work by re-incorporating the changes mentioned in the links provided.

Cheers
Dan

matlabbe

Re: Lidar SLAM with loop closure

Administrator

Hi,

in Preferences->3D rendering panel, you can uncheck the "Show 3D clouds" checkboxes.

cheers,
Mathieu

antoniomontana

Re: Lidar SLAM with loop closure

In reply to this post by denzle

Hi

I am willing to integrate the same thing as you have already discussed.
3D mapping using rtabmap / ouster sensor for pointcloud /realsense 455 just for detecting apriltags and improve localization.

Do you have a success launch ?

Thanks in advance

Antonis

denzle

Re: Lidar SLAM with loop closure

This post was updated on .

Hi Antonis,

I have my one that I originally managed to get working I can add it for you in the next day or so once I'm back on my Linux computer. It should be similar to the one posted above albeit with the D415 (or in your case D455) launch nodelet included and moving the rgbd_sync nodelet as per above. Although I haven't used it in a long time and it may need updating to include the more recent changes made to the ouster nodes.

Dan

antoniomontana

Re: Lidar SLAM with loop closure

Hi and thanks for your answer .

Any help would be really appreciated. Did you manage to produce the cloud only from ouster and have a good mapping and localization; Merging both clouds from realsense and ouster could improve things with rtabmap ; I am afraid putting cloud from camera in rtabmap would slow down the processes as I don't have any GPU on the PC that runs rtabmap.

Using Jackal Clearpath
Ouster os1-32
Realsense 455
Testing room around 200 m^2

Best regards
Antonis

denzle

Re: Lidar SLAM with loop closure

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>

sameh

Re: Lidar SLAM with loop closure

In reply to this post by denzle

Hello Denzle,
I am trying to use rtabmap to create a 3d map using ouster os1-16 and D435i, can you please provide me with the final launch file?

denzle

Re: Lidar SLAM with loop closure

Hi Sameh,

This was the one I managed to use with the camera. You'd need to adjust some of the inputs to fit your own camera but it should work. Unless any recent changes have been made by mathieu to the nodes.

Dan

sameh

Re: Lidar SLAM with loop closure

In reply to this post by antoniomontana

Hi Antonio,
I am trying to use both ouster lidar and d435i, did you find a working launch file?

sameh

Re: Lidar SLAM with loop closure

In reply to this post by denzle

which camera did you used?