3D mapping cloud map for localization and 2D grid map for global costmap

[Princemjp]

Hi Matthieu,

 i am trying to make my rtabmap with velodyne setup map an entire area and use the 2D grid map for the global costmap

however using default parameters i get a map like this



this is what i want my 3D cloud map to look like by localizing using all available points of reference such as the ceiling, however my 2D grid map is basically all black, which i need to edit the grid map manually for global costmap however i will be mapping large areas for my robot and i do not want to edit the map manually for such a large map

so i set my grid/MaxObstacleHeight to 0.5 meter and i get this



this is slightly better as my grid map doesnt look at the ceiling anymore but the grid map still sees dynamic obstacle such as myself when i am walking along with the robot while teleopping it, and also my 3D cloud map is missing the entire top part of the map

i set my grid/MaxGroundHeight to 0.0 meter as my sensor is 2 meters above the ground so now it will not see me as an obstacle and i got this



this is basically what i want my grid map to look like so that i can use this for my global costmap, however now my 3D cloud map is just a small line which i do not want as i want the most accurate localization as possible as my robot will be moving in a big area and if it has very little information it might lose localization easily.

so i saw another post where you set Grid/NormalsSegmentation to false and using this i got this



this is the best so far, with my 3D cloud map having a decent amount of data and my 2D grid map doesnt have that many black spots for me to edit out for global costmap. However i am still not getting the full data for my 3D cloud map as grid/MaxObstacleHeight limits the creation of the 3D cloud map in the vertical space. Do you have any way such that i could solve this issue? I was thinking of mainly creating the full 3D cloud map and then the 2D grid map only takes obstacles using MaxGroundHeight and MaxObstacleHeight as reference

[Princemjp]

this is my launch file

<launch>

    <!--
    Hand-held 3D lidar mapping example using only a Velodyne PUCK (no camera).
    Prerequisities: rtabmap should be built with libpointmatcher
    Example:
     $ roslaunch rtabmap_ros test_velodyne.launch
     $ rosrun rviz rviz -f map
     $ Show TF and /rtabmap/cloud_map topics
    -->

    <arg name="rtabmapviz"    default="false"/>
    <arg name="rviz"          default="true"/>
    <arg name="localization"  default="false"/>
    <arg name="use_imu"       default="false"/> <!-- Assuming IMU fixed to lidar with /velodyne -> /imu_link TF -->
    <arg name="imu_topic"     default="/imu/data"/>
    <arg name="scan_20_hz"    default="false"/> <!-- If we launch the velodyne with "rpm:=1200" argument -->
    <arg name="use_sim_time"  default="false"/>
    <param if="$(arg use_sim_time)" name="use_sim_time" value="true"/>

    <arg name="frame_id" default="velodyne"/>


    <include file="$(find velodyne_pointcloud)/launch/VLP16_points.launch">
       <arg     if="$(arg scan_20_hz)" name="rpm" value="1200"/>
       <arg unless="$(arg scan_20_hz)" name="rpm" value="600"/>
    </include>




  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0 0.0 0.0 0.0 base_footprint /base_link 10" />

  <node pkg="tf" type="static_transform_publisher" name="base_link_to_velodyne" args="0 0 1.8 0.0 0.0 0.0 /base_link velodyne 10" />



    <!-- IMU orientation estimation and publish tf accordingly to os1_sensor frame -->
    <node if="$(arg use_imu)" pkg="rtabmap_ros" type="imu_to_tf" name="imu_to_tf">
      <remap from="imu/data" to="$(arg imu_topic)"/>
      <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="map_optimizer" name="map_optimizer" output="screen">
        <param name="slam_2d"        type="bool" value="true"/>
      </node>

      <node pkg="rtabmap_ros" type="icp_odometry" name="icp_odometry" output="screen">
        <remap from="scan_cloud" to="/velodyne_points"/>
        <param name="frame_id"        type="string" value="base_footprint"/>
        <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"/>

        <remap if="$(arg use_imu)" from="imu" to="$(arg imu_topic)"/>
        <param if="$(arg use_imu)" name="guess_frame_id"   type="string" value="$(arg frame_id)_stabilized"/>
        <param if="$(arg use_imu)" name="wait_imu_to_init" type="bool" value="true"/>

        <!-- ICP parameters -->
  <param name="publish_tf"              type="bool"   value="true"/>
  <param name="tf_delay"                type="double"   value="0.02"/>
        <param name="Icp/PointToPlane"        type="string" value="true"/>
        <param name="Icp/Iterations"          type="string" value="10"/>
        <param name="Icp/VoxelSize"           type="string" value="0.2"/>
        <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/PM"                  type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"      type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio" type="string" value="0.01"/>

        <!-- Odom parameters -->
        <param name="Odom/ScanKeyFrameThr"       type="string" value="0.9"/>
        <param name="Odom/Strategy"              type="string" value="0"/>
        <param name="OdomF2M/ScanSubtractRadius" type="string" value="0.2"/>
        <param name="OdomF2M/ScanMaxSize"        type="string" value="15000"/>
      </node>

      <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d">
  <param name="use_action_for_goal"     type="bool"   value="true"/>
  <remap from="move_base" to="/move_base"/>
  <param name="tf_delay"                type="double"   value="0.02"/>
        <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="false"/>
  <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)"/>

        <remap from="scan_cloud" to="assembled_cloud"/>

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

        <param 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/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="Grid/FromDepth"                 type="string" value="false"/>
        <param name="Grid/MaxObstacleHeight"         type="string" value="0.6"/>
        <param name="Grid/MaxGroundHeight"           type="string" value="0.0"/>
        <param name="Grid/NormalsSegmentation"       type="string" value="false"/>
        <param name="Grid/CellSize"                  type="string" value="0.1"/>
        <param name="Grid/RangeMax"                  type="string" value="50"/>
        <param name="Grid/ClusterRadius"             type="string" value="1"/>
        <param name="Grid/GroundIsObstacle"          type="string" value="false"/>

        <!-- ICP parameters -->
        <param name="Icp/VoxelSize"                  type="string" value="0.2"/>
        <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/PM"                         type="string" value="true"/>
        <param name="Icp/PMOutlierRatio"             type="string" value="0.7"/>
        <param name="Icp/CorrespondenceRatio"        type="string" value="0.4"/>
      </node>

      <node if="$(arg 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="false"/>
        <remap from="scan_cloud" to="/velodyne_points"/>
      </node>

      <node if="$(arg rviz)" name="rviz" pkg="rviz" type="rviz" 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="false"/>
        <remap from="scan_cloud" to="/velodyne_points"/>
      </node>

      <node pkg="nodelet" type="nodelet" name="point_cloud_assembler" args="standalone rtabmap_ros/point_cloud_assembler" output="screen">
        <remap from="cloud"           to="/velodyne_points"/>
        <remap from="odom"            to="odom"/>
        <param     if="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="20" />
        <param unless="$(arg scan_20_hz)" name="max_clouds"      type="int"    value="10" />
        <param name="fixed_frame_id"  type="string" value="" />
      </node>
  </group>

</launch>

[matlabbe]

Hi,

To visualize the whole point cloud independently of the grid map, you can use MapCloud plugin and check "Cloud from scan".

cheers,
Mathieu

[Princemjp]

Hi matthieu,

So am i right to say that even if i set my grid obstacles from 0-0.2 meters and my rtabmap/cloud_map topic is only showing a small portion of the pointcloud, rtabmap is still using the full pointcloud generated for localization?

If this is the case, then i that would be really helpful

[matlabbe]

Yes, it will keep the original cloud for localization.

[Princemjp]

Hi matthieu,

Thanks for all the help and i apologise again if im asking too much as im new to all things ros

The odometry finally used by rtabmap published on /rtabmap/odom is the odometry from icpodometry which is done purely by lidar point matching correct?

My wheel encoders are publishing odometry on the topic /odom do i just need to remap odom to /odom on rtabmap node if i want to combine wheel odometry with my rtabmap?

Also i have an imu that is publishing on /imu_data on imu_link which the transform is just a static transform from velodyne --> imu_link what should i do with this launch file if i wanted to include the imu, because im not sure what the fixed_frame_id --> frame_stabilised and base_frame --> frame_id and guess_frame_id is meant to be


My transforms are currently

Map --> Odom --> base_footprint --> base_link --> velodyne --> imu_link

Map --> odom --> base_footprint (provided by rtabmap)

Base_footprint --> base_link --> velodyne --> imu_link (provided by static transforms)

Velodyne is off the ground by 1.8meters

Because when i do mapping of large areas my map would drift in the Z axis which causes the map to be not aligned so i was hoping adding the wheel odometry and imu will help with this, my current fix is enabling slam_2D but certain areas there are slopes and elevation so i am unable to keep using a 2D map

Or will adding a realsense D435i to this velodyne setup be good?

[matlabbe]

Hi,

you are using the launch file here: https://github.com/introlab/rtabmap_ros/blob/master/launch/tests/test_velodyne.launch

In this one, we can optionally add imu data to help with scan matching. /rtabmap/odom is lidar point matching. The guess frame from imu is used for scan matching (when used).  This sets the current rotation of the latest scan, then ICP is done.

With use_imu:=true, you would have (with frame_id=base_footprint):
Map --> Odom --> base_footprint_stabilized --> base_footprint --> base_link --> velodyne --> imu_link

To optimize on Z axis against gravity, you should subscribe to imu topic and add Optimizer/GravitySigma=0.3 to rtabmap node:

<node pkg="rtabmap_ros" type="rtabmap" ...
  <param name="Optimizer/GravitySigma" type="string" value="0.3"/>
  <remap from="imu"                    to="$(arg imu_topic)"/>
</node>

I updated the original launch file for reference: https://github.com/introlab/rtabmap_ros/commit/962ef3b312c86ddcb85769a4f60226999fd806f4

If your imu gives only gyro and accelerometer, you can use a madwwick filter to compute the quaternion for rtabmap.

cheers,
Mathieu