Different behavior of icp_odometry node on real and simulated robot.

Posted by Divelix on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Different-behavior-of-icp-odometry-node-on-real-and-simulated-robot-tp9037.html

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.