Different behavior of icp_odometry node on real and simulated robot.

[Divelix]

Hi, Matheu,

I'm trying to mimic a robot with Gazebo simulation and use "libgazebo_ros_openni_kinect.so" camera plugin to get same topics as I get from L515 on real robot. When I launch the same launch on it, I get these warnings from icp_odometry node:
[ WARN] (2022-03-24 18:38:22.630) util3d_filtering.cpp:241::commonFiltering() Voxel filter is applied, but normal parameters are not set and input scan has normals. The returned scan has no normals.
What normal parameters is it talking about? (There are no such warnings on real robot)

Here are my RTAB-Map setup:

<!-- ICP Odometry -->
<node name="icp_odometry" pkg="rtabmap_ros" type="icp_odometry" output="screen">
    <remap from="scan_cloud" to="/cloud"/>
    <remap from="odom"       to="/icp_odom"/>

    <param name="frame_id"                    type="string" value="base_footprint"/>
    <param name="odom_frame_id"               type="string" value="icp_odom"/>
    <param name="guess_frame_id"              type="string" value="wheel_odom"/>
    <param name="scan_voxel_size"             type="string" value="0.05"/>
    <param name="scan_normal_k"               type="string" value="5"/>
    
    <param name="Icp/RangeMax"                  type="string" value="0"/>
    <param name="Icp/Epsilon"                   type="string" value="0.001"/>
    <param name="Icp/PointToPlane"              type="string" value="true"/>
    <param name="Icp/MaxCorrespondenceDistance" type="string" value="0.1"/>
    <param name="Icp/PointToPlaneRadius"        type="string" value="0"/>
    <param name="Icp/Strategy"                  type="string" value="0"/>
    <param name="Icp/OutlierRatio"              type="string" value="0.85"/>
    <param name="Icp/CorrespondenceRatio"       type="string" value="0.1"/>
    <param name="Odom/Strategy"                 type="string" value="0"/>
    <param name="Odom/GuessMotion"              type="string" value="true"/>
    <param name="Odom/ResetCountdown"           type="string" value="0"/>
    <param name="Odom/ScanKeyFrameThr"          type="string" value="0.9"/>
</node>

<!-- RTAB-Map -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="-d">
    <remap from="rgbd_image" to="/rgbd_image"/>
    <remap from="scan_cloud" to="/cloud"/>
    <remap from="odom"       to="/icp_odom"/>
    <remap from="grid_map"   to="/map"/>
    <remap from="move_base"  to="/move_base"/>

    <param name="database_path"        type="string" value="$(arg rtabmap_db_path)"/>
    <param name="frame_id"             type="string" value="base_footprint"/>
    <param name="subscribe_rgbd"       type="bool"   value="true"/>
    <param name="subscribe_rgb"        type="bool"   value="false"/>
    <param name="subscribe_depth"      type="bool"   value="false"/>
    <param name="subscribe_scan_cloud" type="bool"   value="true"/>
    <param name="subscribe_scan"       type="bool"   value="false"/>
    <param name="approx_sync"          type="bool"   value="true"/>
    <param name="queue_size"           type="int"    value="10"/>
    <param name="use_action_for_goal"  type="bool"   value="true"/>

    <param name="RGBD/AngularUpdate"             type="string" value="0.01"/>
    <param name="RGBD/LinearUpdate"              type="string" value="0.01"/>
    <param name="RGBD/OptimizeFromGraphEnd"      type="string" value="false"/>
    <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/>
    <param name="RGBD/OptimizeMaxError"          type="double" value="3"/>
    <param name="RGBD/NeighborLinkRefining"      type="bool"   value="true"/>
    <param name="RGBD/ProximityBySpace"          type="bool"   value="true"/>
    <param name="Grid/Sensor"                    type="int"    value="1"/>
    <param name="Reg/Force3DoF"                  type="bool"   value="true"/>
    <param name="Reg/Strategy"                   type="int"    value="2"/>
    <param name="Rtabmap/LoopThr"                type="string" value="2.0"/>
    <param name="ICP/Strategy"                   type="int"    value="1"/>
</node>

Also, as you can see, I use different "ICP/Strategy" for icp_odometry and rtabmap. I do this because when I use libpointmatcher for icp_odometry, it don't publish tf between wheel_odom and icp_odom (I use wheel odometry as guess for icp odometry) so I have 2 tf trees: map->/icp_odom and wheel_odom->base_footprint and rtabmap can't use it. Do you have any idea why it happens? Btw, on robot everything works fine with libpointmatcher.

[matlabbe]

Hi,

Your warning happens because the input cloud has normals, and you filter them without recomputing them. Probably on the real robot, there are no normals in the point cloud. You can ignore the warning. Looking at your parameters, normals are recomputed though, so the warning should not happen. Which version of rtabmap_ros do you have? It is like Icp/VoxelSize is not set to zero here (should be in 0.20.18). You could add --udebug in icp_odometry arguments to get the full debug log.

Based on this launch file, you should have only 1 TF trree:
/map -> icp_odom -> wheel_odom -> base_footprint

assuming wheel_odom->base_footprint TF is published by your simulated robot.