Updated OdomInfo msg to include inliers/outliers data, updated default launch files

include/rtabmap_ros/MsgConversion.h

@@ -69,6 +69,9 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg);
 cv::KeyPoint keypointFromROS(const rtabmap_ros::KeyPoint & msg);
 void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg);
 
+std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg);
+void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg);
+
 void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,

launch/config/rgbd_gui.ini

@@ -13,8 +13,8 @@ General\beep=false
 General\keypointsOpacity=16
 General\voxelSize=0
 General\decimation=16
-MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\x1\a\0\0\x2\x92\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\x1\0\0\0(\0\0\x2\x92\0\0\x1\xe5\0\xff\xff\xff\0\0\0\x1\0\0\x3\xf3\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1\r\0\0\x1\x12\0\0\0q\0\xff\xff\xff\xfc\0\0\x2%\0\0\x2\xdb\0\0\0\xe6\x1\0\0\x1d\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
-MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\x64\0\0\0\xa1\0\0\x6s\0\0\x3\x94\0\0\x1l\0\0\0\xbd\0\0\x6k\0\0\x3\x8c\0\0\0\0\0\0)
+MainWindow\state="@ByteArray(\0\0\0\xff\0\0\0\0\xfd\0\0\0\x3\0\0\0\0\0\0\0\x80\0\0\x2\x92\xfc\x2\0\0\0\x1\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0s\0t\0\x61\0t\0s\0V\0\x32\0\0\0\0(\0\0\x2\x92\0\0\0\xe0\0\xff\xff\xff\0\0\0\x1\0\0\x5\0\0\0\x2\x92\xfc\x2\0\0\0\x2\xfc\0\0\0(\0\0\x2\x92\0\0\x1\x46\0\xff\xff\xff\xfc\x1\0\0\0\x2\xfc\0\0\0\0\0\0\x1\\\0\0\0q\0\xff\xff\xff\xfc\x2\0\0\0\x2\xfb\0\0\0&\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0o\0\x64\0o\0m\0\x65\0t\0r\0y\x1\0\0\0(\0\0\x1\x1b\0\0\0g\0\xff\xff\xff\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0i\0m\0\x61\0g\0\x65\0V\0i\0\x65\0w\x1\0\0\x1I\0\0\x1q\0\0\0\xd9\0\xff\xff\xff\xfc\0\0\x1\x62\0\0\x3\x9e\0\0\0\xc8\0\xff\xff\xff\xfa\0\0\0\x1\x2\0\0\0\x2\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0g\0r\0\x61\0p\0h\0V\0i\0\x65\0w\0\x65\0r\0\0\0\0\0\xff\xff\xff\xff\0\0\0g\0\xff\xff\xff\xfb\0\0\0,\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0l\0o\0u\0\x64\0V\0i\0\x65\0w\0\x65\0r\x1\0\0\0(\0\0\x2\x92\0\0\0\xe1\0\xff\xff\xff\xfb\0\0\0\x38\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0o\0o\0p\0\x43\0l\0o\0s\0u\0r\0\x65\0V\0i\0\x65\0w\0\x65\0r\0\0\0\x1I\0\0\0\xf7\0\0\0\xf7\0\xff\xff\xff\0\0\0\x3\0\0\x5\0\0\0\0\x9a\xfc\x1\0\0\0\x5\xfb\0\0\0(\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0p\0o\0s\0t\0\x65\0r\0i\0o\0r\0\0\0\0\0\0\0\x5\0\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0*\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0$\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0\x63\0o\0n\0s\0o\0l\0\x65\0\0\0\0\0\0\0\x5\0\0\0\0g\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0r\0\x61\0w\0l\0i\0k\0\x65\0l\0i\0h\0o\0o\0\x64\0\0\0\0\0\xff\xff\xff\xff\0\0\0\x87\0\xff\xff\xff\xfb\0\0\0\x30\0\x64\0o\0\x63\0k\0W\0i\0\x64\0g\0\x65\0t\0_\0m\0\x61\0p\0V\0i\0s\0i\0\x62\0i\0l\0i\0t\0y\0\0\0\0\0\xff\xff\xff\xff\0\0\0`\0\xff\xff\xff\0\0\0\0\0\0\x2\x92\0\0\0\x4\0\0\0\x4\0\0\0\b\0\0\0\b\xfc\0\0\0\x1\0\0\0\x2\0\0\0\x1\0\0\0\xe\0t\0o\0o\0l\0\x42\0\x61\0r\x1\0\0\0\0\xff\xff\xff\xff\0\0\0\0\0\0\0\0)"
+MainWindow\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1V\0\0\0\xa1\0\0\x6\x65\0\0\x3\x94\0\0\x1^\0\0\0\xbd\0\0\x6]\0\0\x3\x8c\0\0\0\0\0\0)
 General\showClouds0=true
 General\voxelSize0=0
 General\decimation0=4
@@ -39,9 +39,9 @@ General\decimation2=1
 General\maxDepth2=4
 General\showScans2=true
 General\meshing0=false
-General\cloudFiltering=false
-General\cloudFilteringRadius=0.5
-General\cloudFilteringAngle=30
+General\cloudFiltering=true
+General\cloudFilteringRadius=0.2
+General\cloudFilteringAngle=20
 General\meshNormalKSearch0=20
 General\meshGP3Radius0=0.04
 General\meshSmoothing0=false
@@ -62,3 +62,51 @@ General\gridMapFillEmptySpace=true
 General\gridMapOccupancyFrom3DCloud=false
 General\gridMapFillEmptyRadius=0
 General\gridMapOpacity=0.75
+General\posteriorGraphView=true
+General\showGraphs=true
+PreferencesDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\x1\xde\0\0\0n\0\0\x5\xe8\0\0\x3\x82\0\0\0\0\0\0)
+AboutDialog\geometry=@ByteArray(\x1\xd9\xd0\xcb\0\x1\0\0\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0\0\x18\0\0\x2_\0\0\x1\xf2\0\0\0\0\0\0)
+widget_cloudViewer\camera_pose=@Variant(\0\0\0T\xbf\xf0\0\x13\0\0\0\0\xbe\xb6\0\0\0\0\0\0>\xa4\0\0\0\0\0\0)
+widget_cloudViewer\camera_focal=@Variant(\0\0\0T>\xa4\0\0\0\0\0\0\xbe\xa1\0\0\0\0\0\0>t\0\0\0\0\0\0)
+widget_cloudViewer\camera_up=@Variant(\0\0\0T\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0\0?\xf0\0\0\0\0\0\0)
+widget_cloudViewer\grid=false
+widget_cloudViewer\grid_cell_count=50
+widget_cloudViewer\grid_cell_size=@Variant(\0\0\0\x87?\x80\0\0)
+widget_cloudViewer\trajectory_shown=true
+widget_cloudViewer\trajectory_size=100
+widget_cloudViewer\camera_target_locked=false
+widget_cloudViewer\camera_target_follow=true
+widget_cloudViewer\camera_free=false
+widget_cloudViewer\camera_lockZ=true
+widget_cloudViewer\bg_color=@Variant(\0\0\0\x43\x1\xff\xff\0\0\0\0\0\0\0\0)
+imageView_source\image_shown=true
+imageView_source\depth_shown=false
+imageView_source\features_shown=true
+imageView_source\lines_shown=true
+imageView_loopClosure\image_shown=true
+imageView_loopClosure\depth_shown=false
+imageView_loopClosure\features_shown=true
+imageView_loopClosure\lines_shown=true
+imageView_odometry\image_shown=true
+imageView_odometry\depth_shown=false
+imageView_odometry\features_shown=true
+imageView_odometry\lines_shown=true
+ExportCloudsDialog\assemble=true
+ExportCloudsDialog\assemble_voxel=0.005
+ExportCloudsDialog\regenerate=true
+ExportCloudsDialog\regenerate_decimation=1
+ExportCloudsDialog\regenerate_voxel=0.005
+ExportCloudsDialog\regenerate_max_depth=4
+ExportCloudsDialog\binary=true
+ExportCloudsDialog\mls=false
+ExportCloudsDialog\mls_radius=0.04
+ExportCloudsDialog\mesh=false
+ExportCloudsDialog\mesh_k=20
+ExportCloudsDialog\mesh_radius=0.04
+PostProcessingDialog\detect_more_lc=true
+PostProcessingDialog\cluster_radius=0.3
+PostProcessingDialog\cluster_angle=30
+PostProcessingDialog\iterations=1
+PostProcessingDialog\reextract_features=false
+PostProcessingDialog\refine_neigbors=false
+PostProcessingDialog\refine_lc=false

launch/rgbd_localization.launch

@@ -13,6 +13,7 @@
         -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
         -"inlier_distance" : RANSAC maximum inliers distance (m)  
         -"local_map"       : Local map size: number of unique features to keep track 
+        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
     -->
    <arg name="strategy" default="0" />
    <arg name="feature" default="6" />
@@ -21,6 +22,7 @@
    <arg name="min_inliers" default="20" />
    <arg name="inlier_distance" default="0.02" />
    <arg name="local_map" default="1000" />
+   <arg name="odom_info_data" default="true" />
    
   <!-- TF FRAMES -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" 
@@ -34,8 +36,6 @@
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
       
-      <param name="frame_id" type="string" value="base_link"/>
-      
       <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
 	  <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>  
 	  <param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
@@ -43,6 +43,7 @@
 	  <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> 
 	  <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
       <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>
+      <param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>   
     </node>
   
     <!-- Visual SLAM (robot side) -->
@@ -54,29 +55,22 @@
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
-	
-      <param name="queue_size" type="int" value="30"/>
       
       <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
       <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
-      <param name="Rtabmap/DetectionRate" type="string" value="1"/>     <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
       <param name="Mem/STMSize" type="string" value="1"/>               <!-- 1 location in short-term memory -->
       <param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- false = Localization mode-->
       <param name="LccBow/MinInliers" type="string" value="10"/>        <!-- Minimum inliers to accept a loop closure -->
-      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
     </node>
   
     <!-- Visualisation (client side) -->
     <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="false"/>
-      <param name="queue_size" type="int" value="30"/>
+      <param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
     
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
     </node>
   
   </group>

launch/rgbd_localization_rviz.launch

@@ -34,8 +34,6 @@
       <remap from="depth/image"     to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
       
-      <param name="frame_id" type="string" value="base_link"/>
-      
       <param name="Odom/Strategy" type="string" value="$(arg strategy)"/> 
 	  <param name="Odom/FeatureType" type="string" value="$(arg feature)"/>  
 	  <param name="OdomBow/NNType" type="string" value="$(arg nn)"/>
@@ -49,22 +47,16 @@
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="">
       <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="false"/>
 	
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
-	
-      <param name="queue_size" type="int" value="30"/>
       
       <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
       <param name="Rtabmap/DatabasePath" type="string" value="~/.ros/rtabmap.db"/>  <!-- Database used for localization -->
-      <param name="Rtabmap/DetectionRate" type="string" value="1"/>     <!-- Don't need to do relocation very often! Though better results if the same rate as when mapping. -->
       <param name="Mem/STMSize" type="string" value="1"/>               <!-- 1 location in short-term memory -->
       <param name="Mem/IncrementalMemory" type="string" value="false"/>  <!-- false = Localization mode-->
       <param name="LccBow/MinInliers" type="string" value="10"/>        <!-- Minimum inliers to accept a loop closure -->
-      <param name="RGBD/OptimizeFromGraphEnd" type="string" value="false"/> 
     </node>
     
   </group>
@@ -83,8 +75,6 @@
     <remap from="depth/image_out"     to="data_odom_sync/depth"/>
     <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
     <remap from="odom_out"            to="odom_sync"/>
-    
-    <param name="queue_size" type="int" value="30"/>
   </node>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
     <remap from="rgb/image"       to="data_odom_sync/image"/>
@@ -92,7 +82,6 @@
     <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
 
-    <param name="queue_size" type="int" value="10"/>
     <param name="voxel_size" type="double" value="0.01"/>
   </node>
 

launch/rgbd_mapping.launch

@@ -15,6 +15,7 @@
         -"min_inliers"     : Minimum visual correspondences to accept a transformation (m)  
         -"inlier_distance" : RANSAC maximum inliers distance (m)  
         -"local_map"       : Local map size: number of unique features to keep track 
+        -"odom_info_data"  : Fill odometry info messages with inliers/outliers data.
     -->
    <arg name="strategy" default="0" />
    <arg name="feature" default="6" />
@@ -23,6 +24,7 @@
    <arg name="min_inliers" default="20" />
    <arg name="inlier_distance" default="0.02" />
    <arg name="local_map" default="1000" />
+   <arg name="odom_info_data" default="true" />
    
   <!-- TF FRAMES -->
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera_tf" 
@@ -43,38 +45,30 @@
 	  <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> 
 	  <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
       <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/> 
-   
-      <param name="frame_id" type="string" value="base_link"/>
+      <param name="Odom/FillInfoData" type="string" value="$(arg odom_info_data)"/>   
     </node>
   
     <!-- Visual SLAM (robot side) -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="false"/>
 	
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
 
-      <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
       <param name="LccBow/MinInliers" type="string" value="10"/>
       <param name="LccBow/InlierDistance" type="string" value="0.02"/>
-      	
-      <param name="queue_size" type="int" value="30"/>
     </node>
   
     <!-- Visualisation (client side) -->
     <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
       <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="false"/>
-      <param name="queue_size" type="int" value="30"/>
+      <param name="subscribe_odom_info" type="bool" value="$(arg odom_info_data)"/>
     
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
     </node>
   
   </group>

launch/rgbd_mapping_rviz.launch

@@ -44,26 +44,19 @@
 	  <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> 
 	  <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
       <param name="OdomBow/LocalHistorySize" type="string" value="$(arg local_map)"/>  
-   
-      <param name="frame_id" type="string" value="base_link"/>
     </node>
     
     <!-- Visual SLAM -->
     <!-- args: "delete_db_on_start" and "udebug" -->
     <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
       <param name="subscribe_depth" type="bool" value="true"/>
-      <param name="subscribe_laserScan" type="bool" value="false"/>
 	
       <remap from="rgb/image" to="/camera/rgb/image_rect_color"/>
       <remap from="depth/image" to="/camera/depth_registered/image_raw"/>
       <remap from="rgb/camera_info" to="/camera/depth_registered/camera_info"/>
-      <remap from="odom" to="odom"/>
       
-      <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
       <param name="LccBow/MinInliers" type="string" value="10"/>
       <param name="LccBow/InlierDistance" type="string" value="0.02"/>
-
-      <param name="queue_size" type="int" value="30"/>
     </node>
         
   </group>
@@ -82,8 +75,6 @@
     <remap from="depth/image_out"     to="data_odom_sync/depth"/>
     <remap from="rgb/camera_info_out" to="data_odom_sync/camera_info"/>
     <remap from="odom_out"            to="odom_sync"/>
-    
-    <param name="queue_size" type="int" value="30"/>
   </node>
   <node pkg="nodelet" type="nodelet" name="points_xyzrgb" args="load rtabmap_ros/point_cloud_xyzrgb standalone_nodelet">
     <remap from="rgb/image"       to="data_odom_sync/image"/>
@@ -91,7 +82,6 @@
     <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
     <remap from="cloud"           to="voxel_cloud" />
 
-    <param name="queue_size" type="int" value="10"/>
     <param name="voxel_size" type="double" value="0.01"/>
   </node>
 

msg/OdomInfo.msg

@@ -10,6 +10,18 @@ Header header
 #    int features;
 #    int localMapSize;
 #    float time;
+#
+#    int type; // 0=BOW, 1=Optical Flow, 2=ICP
+#
+#    // BOW odometry
+#    std::multimap<int, cv::KeyPoint> words;
+#    std::vector<int> wordMatches;
+#    std::vector<int> wordInliers;
+#
+#    // Optical Flow odometry
+#    std::vector<cv::KeyPoint> refCorners;
+#    std::vector<cv::KeyPoint> newCorners;
+#    std::vector<int> cornerInliers;
 #}
 
 bool lost
@@ -19,3 +31,15 @@ float32 variance
 int32 features
 int32 localMapSize
 float32 time
+
+int32 type
+
+int32[] wordsKeys
+KeyPoint[] wordsValues
+int32[] wordMatches
+int32[] wordInliers
+
+KeyPoint[] refCorners
+KeyPoint[] newCorners
+int32[] cornerInliers
+

src/GuiWrapper.cpp

@@ -61,7 +61,10 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		mainWindow_(0),
 		frameId_("base_link"),
 		waitForTransform_(false),
-		cameraNodeName_("")
+		cameraNodeName_(""),
+		depthScanSync_(0),
+		depthSync_(0),
+		depthOdomInfoSync_(0)
 {
 	ros::NodeHandle nh;
 	app_ = new QApplication(argc, argv);
@@ -97,14 +100,16 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	// To receive odometry events
 	bool subscribeLaserScan = false;
 	bool subscribeDepth = false;
+	bool subscribeOdomInfo = false;
 	int queueSize = 10;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 	pnh.param("queue_size", queueSize, queueSize);
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
-	this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
+	this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, queueSize);
 
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
@@ -117,6 +122,19 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 
 GuiWrapper::~GuiWrapper()
 {
+	if(depthSync_)
+	{
+		delete depthSync_;
+	}
+	if(depthScanSync_)
+	{
+		delete depthScanSync_;
+	}
+	if(depthOdomInfoSync_)
+	{
+		delete depthOdomInfoSync_;
+	}
+	delete infoMapSync_;
 	delete mainWindow_;
 	delete app_;
 }
@@ -364,7 +382,7 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 {
 	Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
-	rtabmap::SensorData data;
+	rtabmap::SensorData data(cv::Mat(), odomMsg->header.seq);
 	data.setPose(odom, odomMsg->pose.covariance[0]);
 	this->post(new OdometryEvent(data));
 }
@@ -417,10 +435,66 @@ void GuiWrapper::depthCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0]);
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }
 
+void GuiWrapper::depthOdomInfoCallback(
+		const sensor_msgs::ImageConstPtr& imageMsg,
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+		const sensor_msgs::ImageConstPtr& depthMsg,
+		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg)
+{
+	// TF ready?
+	Transform localTransform;
+	try
+	{
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
+		localTransform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
+	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
+
+	image_geometry::PinholeCameraModel model;
+	model.fromCameraInfo(*cameraInfoMsg);
+	float fx = model.fx();
+	float fy = model.fy();
+	float cx = model.cx();
+	float cy = model.cy();
+
+	rtabmap::SensorData image(
+			ptrImage->image.clone(),
+			ptrDepth->image.clone(),
+			fx,
+			fy,
+			cx,
+			cy,
+			localTransform,
+			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
+	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
+	this->post(new OdometryEvent(image, info));
+}
+
 void GuiWrapper::depthScanCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -480,13 +554,15 @@ void GuiWrapper::depthScanCallback(
 			cy,
 			localTransform,
 			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
-			odomMsg->pose.covariance[0]);
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
 	this->post(new OdometryEvent(image));
 }
 
 void GuiWrapper::setupCallbacks(
 		bool subscribeDepth,
 		bool subscribeLaserScan,
+		bool subscribeOdomInfo,
 		int queueSize)
 {
 	ros::NodeHandle nh; // public
@@ -511,6 +587,17 @@ void GuiWrapper::setupCallbacks(
 		depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
 		depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
 	}
+	else if(subscribeDepth && !subscribeLaserScan && subscribeOdomInfo)
+	{
+		ROS_INFO("Registering Depth callback...");
+		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		odomSub_.subscribe(nh, "odom", 1);
+		odomInfoSub_.subscribe(nh, "odom_info", 1);
+		depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
+		depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
+	}
 	else if(subscribeDepth && !subscribeLaserScan)
 	{
 		ROS_INFO("Registering Depth callback...");

src/GuiWrapper.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ros/ros.h>
 #include "rtabmap_ros/Info.h"
 #include "rtabmap_ros/MapData.h"
+#include "rtabmap_ros/OdomInfo.h"
 #include "rtabmap/utilite/UEventsHandler.h"
 
 #include <tf/transform_listener.h>
@@ -71,12 +72,18 @@ protected:
 private:
 	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
 
-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, int queueSize);
 	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
 	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
 					   const nav_msgs::OdometryConstPtr & odomMsg,
 					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
 					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depthOdomInfoCallback(
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
 	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
 						   const nav_msgs::OdometryConstPtr & odomMsg,
 						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
@@ -103,6 +110,7 @@ private:
 	image_transport::SubscriberFilter imageDepthSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 
 	typedef message_filters::sync_policies::ExactTime<
@@ -124,6 +132,14 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			nav_msgs::Odometry,
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
 };
 
 #endif /* GUIWRAPPER_H_ */

src/MsgConversion.cpp

@@ -154,6 +154,25 @@ void keypointToROS(const cv::KeyPoint & kpt, rtabmap_ros::KeyPoint & msg)
 	msg.size = kpt.size;
 }
 
+std::vector<cv::KeyPoint> keypointsFromROS(const std::vector<rtabmap_ros::KeyPoint> & msg)
+{
+	std::vector<cv::KeyPoint> v(msg.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		v[i] = keypointFromROS(msg[i]);
+	}
+	return v;
+}
+
+void keypointsToROS(const std::vector<cv::KeyPoint> & kpts, std::vector<rtabmap_ros::KeyPoint> & msg)
+{
+	msg.resize(kpts.size());
+	for(unsigned int i=0; i<msg.size(); ++i)
+	{
+		keypointToROS(kpts[i], msg[i]);
+	}
+}
+
 void mapGraphFromROS(
 		const rtabmap_ros::Graph & msg,
 		std::map<int, rtabmap::Transform> & poses,
@@ -314,6 +333,22 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.localMapSize = msg.localMapSize;
 	info.time = msg.time;
 	info.variance = msg.variance;
+
+	info.type = msg.type;
+
+	UASSERT(msg.wordsKeys.size() == msg.wordsValues.size());
+	for(unsigned int i=0; i<msg.wordsKeys.size(); ++i)
+	{
+		info.words.insert(std::make_pair(msg.wordsKeys[i], keypointFromROS(msg.wordsValues[i])));
+	}
+
+	info.wordMatches = msg.wordMatches;
+	info.wordInliers = msg.wordInliers;
+
+	info.refCorners = keypointsFromROS(msg.refCorners);
+	info.newCorners = keypointsFromROS(msg.newCorners);
+	info.cornerInliers = msg.cornerInliers;
+
 	return info;
 }
 
@@ -326,6 +361,19 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localMapSize = info.localMapSize;
 	msg.time = info.time;
 	msg.variance = info.variance;
+
+	msg.type = info.type;
+
+	msg.wordsKeys = uKeys(info.words);
+	keypointsToROS(uValues(info.words), msg.wordsValues);
+
+	msg.wordMatches = info.wordMatches;
+	msg.wordInliers = info.wordInliers;
+
+	keypointsToROS(info.refCorners, msg.refCorners);
+	keypointsToROS(info.newCorners, msg.newCorners);
+	msg.cornerInliers = info.cornerInliers;
+
 }
 
 }

src/PreferencesDialogROS.cpp

@@ -46,7 +46,6 @@ PreferencesDialogROS::PreferencesDialogROS(const QString & configFile) :
 
 PreferencesDialogROS::~PreferencesDialogROS()
 {
-	ROS_INFO("rtabmapviz: GUI settings are saved to \"%s\"", configFile_.toStdString().c_str());
 }
 
 QString PreferencesDialogROS::getIniFilePath() const