Difference in odometry topic of rgbdicp_odometry and final output.

Posted by Mikor on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Difference-in-odometry-topic-of-rgbdicp-odometry-and-final-output-tp8014.html

Hi Mathieu,

I am trying to incorporate in a launch file the rgbdicp_odometry node. So far this is my launch file.

<launch>
    <arg name="rtabmapviz"    default="true"/>	
    <arg name="frame_id" default="imu_link"/>
    <arg name="scan_cloud_max_points"  default="131072"/>
    

    <param name="use_sim_time" value="true"/>
	
	<arg name="common_params" value="--RGBD/CreateOccupancyGrid false \
      --Rtabmap/DetectionRate 2 \
      --Odom/ScanKeyFrameThr 0.9 \
      --OdomF2M/ScanMaxSize 25000  \
      --OdomF2M/ScanSubtractRadius 0.5   \
      --Icp/PM true \
      --Icp/VoxelSize 0.5   \
      --Icp/MaxTranslation 5   \
      --Icp/MaxCorrespondenceDistance 1.5 \
      --Icp/PMOutlierRatio 0.8 \
      --Icp/Iterations 10 \
      --Icp/PointToPlane true \
      --Icp/PMMatcherKnn 3 \
      --Icp/PMMatcherEpsilon 0 \
      --Icp/Epsilon 0.0001 \
      --Icp/PointToPlaneK 10 \
      --Icp/PointToPlaneRadius 0 \
      --Icp/CorrespondenceRatio 0.01 \
      --Odom/GuessSmoothingDelay 1"/>
	
	<node pkg="tf2_ros" type="static_transform_publisher" name="odom_to_guess" args="0 0 0 0 0 0 1 odom odom_guess"/> 
	<node pkg="tf2_ros" type="static_transform_publisher" name="imu_to_camera" args="-0.01 0.028 0 0.9999598 -0.0033599 -0.0021211 -0.0080401 imu_link camera_link"/> 
	<node pkg="tf2_ros" type="static_transform_publisher" name="imu_to_velodyne" args="0 0.15 0.075 0 0 0 1 imu_link velodyne"/> 

	<group ns = "rtabmap">
	
		 <node 	pkg="rtabmap_ros" type="rgbdicp_odometry" name="rgbdicp_odometry" output="screen" args=" $(arg common_params)">
			<param name="frame_id"             type="string" value="$(arg frame_id)"/> 
        		<remap from="scan_cloud"        to="/velodyne_points"/>
		        <remap from="depth/image"       to="/camera/aligned_depth_to_color/image_raw"/>
        		<remap from="rgb/camera_info"   to="/camera/color/camera_info"/>
        		<remap from="rgb/image"         to="/camera/color/image_raw"/>
        		<param name="scan_normal_k"   type="int" value="10"/>
        		<param name="subscribe_scan_cloud" type="bool" value="true"/>
			<param name="subscribe_depth" type="bool" value="true"/>
			<param name="approx_sync"          type="bool"   value="true"/>
			
			<param name="Vis/MinInliers" type="string" value="10"/>
			<param name="Vis/FeatureType" type="string" value="0"/>
			<param name="Kp/NndrRatio" type="string" value="0.05"/>
			<param name="Kp/MaxFeatures" type="string" value="500"/>
			<param name="Vis/CorNNDR" type="string" value="5"/>
			<param name="RGBD/OptimizeMaxError" type="string" value="5.0"/>

	        </node>
		
		<node pkg="rtabmap_ros" type="rtabmap" name="rtabmap" output="screen" args="-d $(arg common_params)">
		    <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="true"/>
			<param name="scan_cloud_max_points"       type="int"    value="$(arg scan_cloud_max_points)"/>
			<remap from="scan_cloud" to="/velodyne_points"/>
		</node>
		
		<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="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="true"/>
			<remap from="scan_cloud" to="/velodyne_points"/>
		</node>
		
	</group>
</launch>

By calling rosnode info /rtabmap/odom and visualizing it on rviz it seems that it is working as intended.
Here is a screen shot of how the poses are computed.
\

Although this seems fairly reasonably  accurate the output at the gui is the following:


And the pointcloud is very messy.


Can you help me understand what's going on ?