Merge pull request #23 from introlab/devel

Merging devel to master
2026-10-05 17:27:46 +08:00 · 2015-07-19 18:59:07 -04:00
parent 48658d4e55 4694c7ffe4
commit a80f60ed12
28 changed files with 2685 additions and 1592 deletions
@@ -17,7 +17,7 @@ find_package(octomap_ros)

 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.9.0 REQUIRED)
+find_package(RTABMap 0.10.1 REQUIRED)

 find_package(OpenCV REQUIRED)

@@ -46,11 +46,9 @@ add_message_files(
   Info.msg
   KeyPoint.msg
   MapData.msg
-   Graph.msg
   NodeData.msg
   Link.msg
   OdomInfo.msg
-   UserData.msg
   Point2f.msg
 )

@@ -7,8 +7,6 @@ gen = ParameterGenerator()

 gen.add("device_id", int_t, 0, "Camera device ID", 0, 0, 7)
 gen.add("frame_rate", double_t, 0, "Frame rate", 15.0, 0.0, 100.0)
-gen.add("width", int_t, 0, "Width", 640, 0, 1920)
-gen.add("height", int_t, 0, "Image height", 480, 0, 1080)
 gen.add("video_or_images_path", str_t, 0, "Video or images directory path", "")
 gen.add("pause", bool_t, 0, "Pause", False)

@@ -45,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/KeyPoint.h>
 #include <rtabmap_ros/Point2f.h>
 #include <rtabmap_ros/MapData.h>
-#include <rtabmap_ros/Graph.h>
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/Info.h>
@@ -83,24 +82,28 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);

-void mapGraphFromROS(
-		const rtabmap_ros::Graph & msg,
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
+		std::map<int, rtabmap::Transform> & poses,
+		std::multimap<int, rtabmap::Link> & links,
+		std::map<int, rtabmap::Signature> & signatures,
+		rtabmap::Transform & mapToOdom);
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
-void mapGraphToROS(
+void mapDataToROS(
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links,
+		const std::map<int, rtabmap::Signature> & signatures,
+		const rtabmap::Transform & mapToOdom,
+		rtabmap_ros::MapData & msg);
+void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
-		const std::map<int, int> & mapIds,
-		const std::map<int, double> & stamps,
-		const std::map<int, std::string> & labels,
-		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::Graph & msg);
+		rtabmap_ros::MapData & msg);

 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);
@@ -0,0 +1,139 @@
+
+<launch>
+   
+  <!-- Multi-cameras demo  with 2 Kinects -->
+   
+  <!-- Cameras -->
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera1" />
+    <arg name="device_id" value="#1" />
+  </include>
+  <include file="$(find freenect_launch)/launch/freenect.launch">
+    <arg name="depth_registration" value="True" />
+    <arg name="camera" value="camera2" />
+    <arg name="device_id" value="#2" />
+  </include>
+
+  <!-- Frames: Kinects are placed at 90 degrees -->
+  <node pkg="tf" type="static_transform_publisher" name="base_to_camera1_tf"
+      args="0.0 0.0 0.0 0.0 0.0 0.0 /base_link /camera1_link 100" />
+  <node pkg="tf" type="static_transform_publisher" name="base_to_camera2_tf"
+      args="-0.1325 -0.1975 0.0 -1.570796327 0.0 0.0 /base_link /camera2_link 100" />
+   
+   <!-- Choose visualization -->
+   <arg name="rviz"       default="false" />
+   <arg name="rtabmapviz" default="true" /> 
+   
+   <!-- ODOMETRY MAIN ARGUMENTS: 
+        -"strategy"        : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
+        -"feature"         : Feature type: 0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK
+        -"nn"              : Nearest neighbor strategy : 0=Linear, 1=FLANN_KDTREE, 2=FLANN_LSH, 3=BRUTEFORCE 
+                             Set to 1 for float descriptor like SIFT/SURF                  
+                             Set to 3 for binary descriptor like ORB/FREAK/BRIEF/BRISK  
+        -"max_depth"       : Maximum features depth (m)  
+        -"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" />
+   <arg name="nn"              default="3" />
+   <arg name="max_depth"       default="4.0" />
+   <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" />
+   <arg name="wait_for_transform"  default="true" />
+        
+  <group ns="rtabmap">
+  
+    <!-- Odometry -->
+    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
+      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
+      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
+      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
+      
+      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
+      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
+      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+	  
+	  <param name="frame_id"                 type="string" value="base_link"/>
+	  <param name="depth_cameras"            type="int"    value="2"/>
+	  <param name="wait_for_transform"       type="bool"   value="$(arg wait_for_transform)"/>
+	  <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)"/>
+	  <param name="Odom/MaxDepth"            type="string" value="$(arg max_depth)"/>  
+	  <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) -->
+    <!-- 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="depth_cameras"    type="int"    value="2"/>
+	  <param name="frame_id"         type="string" value="base_link"/>
+	  <param name="gen_scan"         type="bool"   value="true"/>
+	  <param name="wait_for_transform" type="bool"   value="$(arg wait_for_transform)"/>
+	
+      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
+      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
+      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
+     
+      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
+      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
+      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+
+      <param name="LccBow/MinInliers"     type="string" value="10"/>
+      <param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
+    </node>
+  
+    <!-- Visualisation RTAB-Map -->
+    <node if="$(arg rtabmapviz)" 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_odom_info" type="bool"   value="$(arg odom_info_data)"/>
+      <param name="frame_id"            type="string" value="base_link"/>
+      <param name="depth_cameras"       type="int"    value="2"/>
+      <param name="wait_for_transform"  type="bool"   value="$(arg wait_for_transform)"/>
+    
+      <remap from="rgb0/image"       to="/camera1/rgb/image_rect_color"/>
+      <remap from="depth0/image"     to="/camera1/depth_registered/image_raw"/>
+      <remap from="rgb0/camera_info" to="/camera1/rgb/camera_info"/>
+     
+      <remap from="rgb1/image"       to="/camera2/rgb/image_rect_color"/>
+      <remap from="depth1/image"     to="/camera2/depth_registered/image_raw"/>
+      <remap from="rgb1/camera_info" to="/camera2/rgb/camera_info"/>
+    </node>
+  
+  </group>
+  
+  <!-- Visualization RVIZ -->
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/rgbd.rviz"/>
+  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
+  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
+    <remap from="rgb/image_in"       to="camera1/rgb/image_rect_color"/>
+    <remap from="depth/image_in"     to="camera1/depth_registered/image_raw"/>
+    <remap from="rgb/camera_info_in" to="camera1/rgb/camera_info"/>
+    <remap from="odom_in"            to="rtabmap/odom"/>
+    
+    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
+    <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"/>
+  </node>
+  <node if="$(arg rviz)" 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"/>
+    <remap from="depth/image"     to="data_odom_sync/depth"/>
+    <remap from="rgb/camera_info" to="data_odom_sync/camera_info"/>
+    <remap from="cloud"           to="voxel_cloud" />
+
+    <param name="voxel_size" type="double" value="0.01"/>
+  </node>
+
+</launch>
@@ -6,11 +6,18 @@
   <!--           See "delete_db_on_start" option below... -->
  
   <!-- Choose visualization -->
-   <arg name="rviz" default="false" />
-   <arg name="rtabmapviz" default="true" /> 
+   <arg name="rviz"                    default="false" />
+   <arg name="rtabmapviz"              default="true" /> 
  
   <!-- Fixed frame id, you may set "base_link" or "base_footprint" if they are published -->
-   <arg name="frame_id" default="camera_link"/>
+   <arg name="frame_id"                default="camera_link"/>
+   
+   <!-- Image topics input -->
+   <arg name="rgb_topic"               default="/camera/rgb/image_rect_color" />
+   <arg name="depth_registered_topic"  default="/camera/depth_registered/image_raw" />
+   <arg name="camera_info_topic"       default="/camera/rgb/camera_info" />
+   
+   <arg name="namespace"               default="rtabmap"/>
  
   <!-- ODOMETRY MAIN ARGUMENTS: 
        -"strategy"        : Strategy: 0=BOW (bag-of-words) 1=Optical Flow
@@ -24,43 +31,43 @@
        -"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" />
-   <arg name="nn" default="3" />
-   <arg name="max_depth" default="4.0" />
-   <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" />
+   <arg name="strategy"            default="0" />
+   <arg name="feature"             default="6" />
+   <arg name="nn"                  default="3" />
+   <arg name="max_depth"           default="4.0" />
+   <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" />
        
-  <group ns="rtabmap">
+  <group ns="$(arg namespace)">
  
    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="rgbd_odometry" name="rgbd_odometry" output="screen">
-      <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/rgb/camera_info"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
 	  
-	  <param name="frame_id" type="string" value="$(arg frame_id)"/>
-	  <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)"/>
-	  <param name="Odom/MaxDepth" type="string" value="$(arg max_depth)"/>  
-	  <param name="Odom/MinInliers" type="string" value="$(arg min_inliers)"/> 
-	  <param name="Odom/InlierDistance" type="string" value="$(arg inlier_distance)"/>       
+	  <param name="frame_id"                 type="string" value="$(arg frame_id)"/>
+	  <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)"/>
+	  <param name="Odom/MaxDepth"            type="string" value="$(arg max_depth)"/>  
+	  <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)"/>   
+      <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="frame_id" type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth" type="bool"   value="true"/>
+	  <param name="frame_id"        type="string" value="$(arg frame_id)"/>
 	
-      <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/rgb/camera_info"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>

      <param name="LccBow/MinInliers" type="string" value="10"/>
      <param name="LccBow/InlierDistance" type="string" value="$(arg inlier_distance)"/>
@@ -68,13 +75,13 @@
  
    <!-- Visualisation RTAB-Map -->
    <node if="$(arg rtabmapviz)" 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_odom_info" type="bool" value="$(arg odom_info_data)"/>
-      <param name="frame_id" type="string" value="$(arg frame_id)"/>
+      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_odom_info" type="bool"   value="$(arg odom_info_data)"/>
+      <param name="frame_id"            type="string" value="$(arg frame_id)"/>
    
-      <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/rgb/camera_info"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg depth_registered_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg camera_info_topic)"/>
    </node>
  
  </group>
@@ -84,9 +91,9 @@
  <!-- sync cloud with odometry and voxelize the point cloud (for fast visualization in rviz) -->
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
  <node if="$(arg rviz)" pkg="nodelet" type="nodelet" name="data_odom_sync" args="load rtabmap_ros/data_odom_sync standalone_nodelet">
-    <remap from="rgb/image_in"       to="camera/rgb/image_rect_color"/>
-    <remap from="depth/image_in"     to="camera/depth_registered/image_raw"/>
-    <remap from="rgb/camera_info_in" to="camera/rgb/camera_info"/>
+    <remap from="rgb/image_in"        to="$(arg rgb_topic)"/>
+    <remap from="depth/image_in"      to="$(arg depth_registered_topic)"/>
+    <remap from="rgb/camera_info_in"  to="$(arg camera_info_topic)"/>
    <remap from="odom_in"             to="rtabmap/odom"/>
    
    <remap from="rgb/image_out"       to="data_odom_sync/image"/>
@@ -1,26 +0,0 @@
-
-Header header
-
-##
-# /map to /odom transform
-# Always identity when the graph is optimized from the latest pose.
-##
-geometry_msgs/Transform mapToOdom
-
-##
-# The nodes
-# std::map<nodeId, mapId>
-int32[] nodeIds
-int32[] mapIds
-string[] labels
-float64[] stamps
-UserData[] userDatas
-
-# std::map<nodeId, Pose>
-geometry_msgs/Pose[] poses
-
-##
-# The links
-##
-Link[] links
-
@@ -2,14 +2,25 @@
 Header header

 ##################
-# Graph stuff
+# Optimized graph
 ##################
+##
+# /map to /odom transform
+# Always identity when the graph is optimized from the latest pose.
+##
+geometry_msgs/Transform mapToOdom

-Graph graph
+# The poses
+int32[] posesId
+geometry_msgs/Pose[] poses
+
+# The links
+Link[] links

 ##################
-# Point cloud stuff
+# Graph data
 ##################
-
 NodeData[] nodes

+
+
@@ -4,7 +4,6 @@ int32 mapId
 int32 weight
 float64 stamp
 string label
-UserData userData

 # Pose from odometry not corrected
 geometry_msgs/Pose pose
@@ -17,18 +16,23 @@ uint8[] image
 # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
 uint8[] depth

-float32 fx
-float32 fy
-float32 cx
-float32 cy
+# Camera models
+float32[] fx
+float32[] fy
+float32[] cx
+float32[] cy
+float32 baseline
+# local transform (/base_link -> /camera_link)
+geometry_msgs/Transform[] localTransform

 # compressed 2D laser scan in /base_link frame
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
 uint8[] laserScan
+int32 laserScanMaxPts

-# local transform (/base_link -> /camera_link)
-geometry_msgs/Transform localTransform
-
+# compressed user data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] userData

 # std::multimap<wordId, cv::Keypoint>
 # std::multimap<wordId, pcl::PointXYZ>
@@ -30,7 +30,11 @@ int32 inliers
 float32 variance
 int32 features
 int32 localMapSize
-float32 time
+float32 timeEstimation
+float32 timeParticleFiltering
+float32 stamp
+float32 interval
+float32 distanceTravelled

 int32 type

@@ -43,3 +47,6 @@ Point2f[] refCorners
 Point2f[] newCorners
 int32[] cornerInliers

+geometry_msgs/Transform transform
+geometry_msgs/Transform transformFiltered
+
@@ -1,2 +0,0 @@
-
-uint8[] data
@@ -33,7 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/image_transport.h>
 #include <std_srvs/Empty.h>

-#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/CameraRGB.h>
 #include <rtabmap/core/CameraThread.h>
 #include <rtabmap/core/CameraEvent.h>

@@ -55,6 +55,7 @@ public:
 				  unsigned int imageWidth = 0,
 				  unsigned int imageHeight = 0) :
 		cameraThread_(0),
+		camera_(0),
 		frameId_("camera")
 	{
 		ros::NodeHandle nh;
@@ -80,7 +81,7 @@ public:
 	{
 		if(cameraThread_)
 		{
-			return cameraThread_->init();
+			return cameraThread_->camera()->init();
 		}
 		return false;
 	}
@@ -113,10 +114,10 @@ public:
 		return true;
 	}

-	void setParameters(int deviceId, double frameRate, int width, int height, const std::string & path, bool pause)
+	void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
 	{
-		ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f w/h=%d/%d pause=%s",
-				deviceId, path.c_str(), frameRate, width, height, pause?"true":"false");
+		ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
+				deviceId, path.c_str(), frameRate, pause?"true":"false");
 		if(cameraThread_)
 		{
 			rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
@@ -128,7 +129,6 @@ public:
 				if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
 				{
 					imagesCam->setImageRate(frameRate);
-					imagesCam->setImageSize(width, height);
 					if(pause && !cameraThread_->isPaused())
 					{
 						cameraThread_->join(true);
@@ -150,7 +150,6 @@ public:
 				{
 					// video
 					videoCam->setImageRate(frameRate);
-					videoCam->setImageSize(width, height);
 					if(pause && !cameraThread_->isPaused())
 					{
 						cameraThread_->join(true);
@@ -165,25 +164,14 @@ public:
 						videoCam->getUsbDevice() == deviceId)
 				{
 					// usb device
-					unsigned int w;
-					unsigned int h;
-					videoCam->getImageSize(w, h);
-					if((int)w == width && (int)h == height)
+					videoCam->setImageRate(frameRate);
+					if(pause && !cameraThread_->isPaused())
 					{
-						videoCam->setImageRate(frameRate);
-						if(pause && !cameraThread_->isPaused())
-						{
-							cameraThread_->join(true);
-						}
-						else if(!pause && cameraThread_->isPaused())
-						{
-							cameraThread_->start();
-						}
+						cameraThread_->join(true);
 					}
-					else
+					else if(!pause && cameraThread_->isPaused())
 					{
-						delete cameraThread_;
-						cameraThread_ = 0;
+						cameraThread_->start();
 					}
 				}
 				else
@@ -205,12 +193,12 @@ public:
 			if(!path.empty() && UDirectory::exists(path))
 			{
 				//images
-				camera_ = new rtabmap::CameraImages(path, 1, false, frameRate, width, height);
+				camera_ = new rtabmap::CameraImages(path, 1, false, frameRate);
 			}
 			else if(!path.empty() && UFile::exists(path))
 			{
 				//video
-				camera_ = new rtabmap::CameraVideo(path, frameRate, width, height);
+				camera_ = new rtabmap::CameraVideo(path, frameRate);
 			}
 			else
 			{
@@ -219,7 +207,7 @@ public:
 					ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
 				}
 				//usb device
-				camera_ = new rtabmap::CameraVideo(deviceId, frameRate, width, height);
+				camera_ = new rtabmap::CameraVideo(deviceId, frameRate);
 			}
 			cameraThread_ = new rtabmap::CameraThread(camera_);
 			init();
@@ -236,7 +224,7 @@ protected:
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
-			const cv::Mat & image = e->data().image();
+			const cv::Mat & image = e->data().imageRaw();
 			if(!image.empty() && image.depth() == CV_8U)
 			{
 				cv_bridge::CvImage img;
@@ -271,7 +259,7 @@ void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
 {
 	if(camera)
 	{
-		camera->setParameters(config.device_id, config.frame_rate, config.width, config.height, config.video_or_images_path, config.pause);
+		camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
 	}
 }

@@ -83,7 +83,13 @@ public:
 	virtual ~CoreWrapper();

 private:
-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize, bool stereoApproxSync);
+	void setupCallbacks(
+			bool subscribeDepth,
+			bool subscribeLaserScan,
+			bool subscribeStereo,
+			int queueSize,
+			bool stereoApproxSync,
+			int depthCameras);
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom

 	bool commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg);
@@ -93,8 +99,14 @@ private:
 	void commonDepthCallback(
 				const std::string & odomFrameId,
 				const sensor_msgs::ImageConstPtr& imageMsg,
-				const sensor_msgs::ImageConstPtr& imageDepthMsg,
-				const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+				const sensor_msgs::ImageConstPtr& depthMsg,
+				const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+				const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void commonDepthCallback(
+				const std::string & odomFrameId,
+				const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
+				const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
+				const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
 				const sensor_msgs::LaserScanConstPtr& scanMsg);
 	void commonStereoCallback(
 				const std::string & odomFrameId,
@@ -129,6 +141,14 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
 			const nav_msgs::OdometryConstPtr & odomMsg);
+	void depth2Callback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& image1Msg,
+			const sensor_msgs::ImageConstPtr& imageDepth1Msg,
+			const sensor_msgs::CameraInfoConstPtr& camInfo1Msg,
+			const sensor_msgs::ImageConstPtr& image2Msg,
+			const sensor_msgs::ImageConstPtr& imageDept2hMsg,
+			const sensor_msgs::CameraInfoConstPtr& camInfo2Msg);

 	// without odom, when TF is used for odom
 	void depthTFCallback(
@@ -154,25 +174,15 @@ private:

 	void goalCommonCallback(const std::vector<std::pair<int, rtabmap::Transform> > & poses);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
-	void goalGlobalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 	void updateGoal(const ros::Time & stamp);

 	void process(
-			int id,
 			const ros::Time & stamp,
-			const cv::Mat & image,
+			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			float odomRotationalVariance = 1.0f,
-			float odomTransitionalVariance = 1.0f,
-			const cv::Mat & depthOrRightImage = cv::Mat(),
-			float fx = 0.0f,
-			float fyOrBaseline = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f,
-			const rtabmap::Transform & localTransform = rtabmap::Transform(),
-			const cv::Mat & scan = cv::Mat(),
-			int scanMaxPts = 0);
+			double odomRotationalVariance = 1.0,
+			double odomTransitionalVariance = 1.0);

 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -187,6 +197,7 @@ private:
 	bool getGridMapCallback(nav_msgs::GetMap::Request  &req, nav_msgs::GetMap::Response &res);
 	bool publishMapCallback(rtabmap_ros::PublishMap::Request&, rtabmap_ros::PublishMap::Response&);
 	bool setGoalCallback(rtabmap_ros::SetGoal::Request& req, rtabmap_ros::SetGoal::Response& res);
+	bool cancelGoalCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Response& res);
 	bool setLabelCallback(rtabmap_ros::SetLabel::Request& req, rtabmap_ros::SetLabel::Response& res);
 	bool listLabelsCallback(rtabmap_ros::ListLabels::Request& req, rtabmap_ros::ListLabels::Response& res);
 #ifdef WITH_OCTOMAP
@@ -211,8 +222,8 @@ private:
 	bool paused_;
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
-	float rotVariance_;
-	float transVariance_;
+	double rotVariance_;
+	double transVariance_;
 	rtabmap::Transform currentMetricGoal_;
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;
@@ -224,6 +235,8 @@ private:
 	std::string databasePath_;
 	bool waitForTransform_;
 	bool useActionForGoal_;
+	bool genScan_;
+	double genScanMaxDepth_;

 	rtabmap::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;
@@ -232,12 +245,10 @@ private:

 	ros::Publisher infoPub_;
 	ros::Publisher mapDataPub_;
-	ros::Publisher mapGraphPub_;
 	ros::Publisher labelsPub_;

 	//Planning stuff
 	ros::Subscriber goalSub_;
-	ros::Subscriber goalGlobalSub_;
 	ros::Publisher nextMetricGoalPub_;
 	ros::Publisher goalReachedPub_;
 	ros::Publisher globalPathPub_;
@@ -247,9 +258,9 @@ private:
 	image_transport::Subscriber defaultSub_;

 	//for depth callback
-	image_transport::SubscriberFilter imageSub_;
-	image_transport::SubscriberFilter imageDepthSub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	std::vector<image_transport::SubscriberFilter*> imageSubs_;
+	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
+	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;

 	//stereo callback
 	image_transport::SubscriberFilter imageRectLeft_;
@@ -300,6 +311,16 @@ private:
 			nav_msgs::Odometry> MyStereoExactSyncPolicy;
 	message_filters::Synchronizer<MyStereoExactSyncPolicy> * stereoExactSync_;

+	typedef message_filters::sync_policies::ApproximateTime<
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
+	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
+
 	// without odom, when TF is used for odom
 	typedef message_filters::sync_policies::ApproximateTime<
 			sensor_msgs::Image,
@@ -352,6 +373,7 @@ private:
 	ros::ServiceServer getGridMapSrv_;
 	ros::ServiceServer publishMapDataSrv_;
 	ros::ServiceServer setGoalSrv_;
+	ros::ServiceServer cancelGoalSrv_;
 	ros::ServiceServer setLabelSrv_;
 	ros::ServiceServer listLabelsSrv_;
 #ifdef WITH_OCTOMAP
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_srvs/Empty.h>
 #include <rtabmap_ros/MsgConversion.h>
 #include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/core/util3d_conversions.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/DBReader.h>

 bool paused = false;
@@ -136,10 +136,10 @@ int main(int argc, char** argv)
 	ros::Publisher scanPub;
 	tf2_ros::TransformBroadcaster tfBroadcaster;

-	rtabmap::SensorData data = reader.getNextData();
-	while(ros::ok() && data.isValid())
+	rtabmap::OdometryEvent odom = reader.getNextData();
+	while(ros::ok() && odom.data().id())
 	{
-		ROS_INFO("Reading sensor data %d...", data.id());
+		ROS_INFO("Reading sensor data %d...", odom.data().id());

 		ros::Time time = ros::Time::now();

@@ -159,45 +159,58 @@ int main(int argc, char** argv)
 		camInfoB = camInfoA;

 		int type = -1;
-		if(!data.depth().empty() && (data.depth().type() == CV_32FC1 || data.depth().type() == CV_16UC1))
+		if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
 		{
-			//depth
-			camInfoA.D.resize(5,0);
+			if(odom.data().cameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
+			{
+				//depth
+				if(odom.data().cameraModels().size())
+				{
+					camInfoA.D.resize(5,0);

-			camInfoA.P[0] = data.fx();
-			camInfoA.K[0] = data.fx();
-			camInfoA.P[5] = data.fy();
-			camInfoA.K[4] = data.fy();
-			camInfoA.P[2] = data.cx();
-			camInfoA.K[2] = data.cx();
-			camInfoA.P[6] = data.cy();
-			camInfoA.K[5] = data.cy();
+					camInfoA.P[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.K[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.P[5] = odom.data().cameraModels()[0].fy();
+					camInfoA.K[4] = odom.data().cameraModels()[0].fy();
+					camInfoA.P[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.K[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.P[6] = odom.data().cameraModels()[0].cy();
+					camInfoA.K[5] = odom.data().cameraModels()[0].cy();

-			camInfoB = camInfoA;
+					camInfoB = camInfoA;
+				}

-			type=0;
+				type=0;

-			if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
-			if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
-			if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
-			if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
+				if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
+				if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
+				if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
+				if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
+			}
 		}
-		else if(!data.rightImage().empty() && data.rightImage().type() == CV_8U)
+		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
 		{
 			//stereo
-			camInfoA.D.resize(8,0);
+			if(odom.data().stereoCameraModel().isValid())
+			{
+				camInfoA.D.resize(8,0);

-			camInfoA.P[0] = data.fx();
-			camInfoA.K[0] = data.fx();
-			camInfoA.P[5] = data.fx(); // fx = fy
-			camInfoA.K[4] = data.fx(); // fx = fy
-			camInfoA.P[2] = data.cx();
-			camInfoA.K[2] = data.cx();
-			camInfoA.P[6] = data.cy();
-			camInfoA.K[5] = data.cy();
+				camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
+				camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
+				camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
+				camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
+				camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
+				camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
+				camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
+				camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();

-			camInfoB = camInfoA;
-			camInfoB.P[3] = data.baseline()*-data.fx(); // Right_Tx = -baseline*fx
+				camInfoB = camInfoA;
+				camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
+			}

 			type=1;

@@ -212,12 +225,12 @@ int main(int argc, char** argv)
 			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
 		}

-		camInfoA.height = data.image().rows;
-		camInfoA.width = data.image().cols;
-		camInfoB.height = data.depthOrRightImage().rows;
-		camInfoB.width = data.depthOrRightImage().cols;
+		camInfoA.height = odom.data().imageRaw().rows;
+		camInfoA.width = odom.data().imageRaw().cols;
+		camInfoB.height = odom.data().depthOrRightRaw().rows;
+		camInfoB.width = odom.data().depthOrRightRaw().cols;

-		if(!data.laserScan().empty())
+		if(!odom.data().laserScanRaw().empty())
 		{
 			if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
 		}
@@ -226,44 +239,52 @@ int main(int argc, char** argv)
 		if(publishTf)
 		{
 			ros::Time tfExpiration = time + ros::Duration(1.0/rate);
-			if(!data.localTransform().isNull())
+
+			rtabmap::Transform localTransform;
+			if(odom.data().cameraModels().size() == 1)
+			{
+				localTransform = odom.data().cameraModels()[0].localTransform();
+			}
+			else if(odom.data().stereoCameraModel().isValid())
+			{
+				localTransform = odom.data().stereoCameraModel().left().localTransform();
+			}
+			if(!localTransform.isNull())
 			{
 				geometry_msgs::TransformStamped baseToCamera;
 				baseToCamera.child_frame_id = cameraFrameId;
 				baseToCamera.header.frame_id = frameId;
 				baseToCamera.header.stamp = tfExpiration;
-				rtabmap_ros::transformToGeometryMsg(data.localTransform(), baseToCamera.transform);
+				rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
 				tfBroadcaster.sendTransform(baseToCamera);
 			}

-			if(!data.pose().isNull())
+			if(!odom.pose().isNull())
 			{
 				geometry_msgs::TransformStamped odomToBase;
 				odomToBase.child_frame_id = frameId;
 				odomToBase.header.frame_id = odomFrameId;
 				odomToBase.header.stamp = tfExpiration;
-				rtabmap_ros::transformToGeometryMsg(data.pose(), odomToBase.transform);
+				rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
 				tfBroadcaster.sendTransform(odomToBase);
 			}
 		}
-		if(!data.pose().isNull())
+		if(!odom.pose().isNull())
 		{
 			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);

 			if(odometryPub.getNumSubscribers())
 			{
-				nav_msgs::Odometry odom;
-				odom.child_frame_id = frameId;
-				odom.header.frame_id = odomFrameId;
-				odom.header.stamp = time;
-				rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
-				odom.pose.covariance[0] = data.poseTransVariance();
-				odom.pose.covariance[7] = data.poseTransVariance();
-				odom.pose.covariance[14] = data.poseTransVariance();
-				odom.pose.covariance[21] = data.poseRotVariance();
-				odom.pose.covariance[28] = data.poseRotVariance();
-				odom.pose.covariance[35] = data.poseRotVariance();
-				odometryPub.publish(odom);
+				nav_msgs::Odometry odomMsg;
+				odomMsg.child_frame_id = frameId;
+				odomMsg.header.frame_id = odomFrameId;
+				odomMsg.header.stamp = time;
+				rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
+				UASSERT(odomMsg.pose.covariance.size() == 36 &&
+						odom.covariance().total() == 36 &&
+						odom.covariance().type() == CV_64FC1);
+				memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
+				odometryPub.publish(odomMsg);
 			}
 		}

@@ -290,7 +311,7 @@ int main(int argc, char** argv)
 		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
 		{
 			cv_bridge::CvImage img;
-			if(data.image().channels() == 1)
+			if(odom.data().imageRaw().channels() == 1)
 			{
 				img.encoding = sensor_msgs::image_encodings::MONO8;
 			}
@@ -298,7 +319,7 @@ int main(int argc, char** argv)
 			{
 				img.encoding = sensor_msgs::image_encodings::BGR8;
 			}
-			img.image = data.image();
+			img.image = odom.data().imageRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -318,10 +339,10 @@ int main(int argc, char** argv)
 			}
 		}

-		if(depthPub.getNumSubscribers() && !data.depth().empty() && type==0)
+		if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
 		{
 			cv_bridge::CvImage img;
-			if(data.depth().type() == CV_32FC1)
+			if(odom.data().depthRaw().type() == CV_32FC1)
 			{
 				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
 			}
@@ -329,7 +350,7 @@ int main(int argc, char** argv)
 			{
 				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
 			}
-			img.image = data.depth();
+			img.image = odom.data().depthRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -338,11 +359,11 @@ int main(int argc, char** argv)
 			depthCamInfoPub.publish(camInfoB);
 		}

-		if(rightPub.getNumSubscribers() && !data.rightImage().empty() && type==1)
+		if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
 		{
 			cv_bridge::CvImage img;
 			img.encoding = sensor_msgs::image_encodings::MONO8;
-			img.image = data.rightImage();
+			img.image = odom.data().rightRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -351,9 +372,9 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}

-		if(scanPub.getNumSubscribers() && !data.laserScan().empty())
+		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan());
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(odom.data().laserScanRaw());
 			sensor_msgs::PointCloud2 msg;
 			pcl::toROSMsg(*cloud, msg);
 			msg.header.frame_id = frameId;
@@ -369,7 +390,7 @@ int main(int argc, char** argv)
 			ros::spinOnce();
 		}

-		data = reader.getNextData();
+		odom = reader.getNextData();
 	}


@@ -99,9 +99,10 @@ public:
 		}

 		std::map<int, Transform> poses;
-		for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
+		UASSERT(msg->posesId.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
+			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
 		}

 		if(filterRadius_ > 0.0 && filterAngle_ > 0.0)
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/OdomInfo.h"
 #include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/core/Transform.h"

 #include <tf/transform_listener.h>

@@ -72,34 +73,85 @@ protected:
 private:
 	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);

-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, bool subscribeStereo, 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,
+	void setupCallbacks(
+			bool subscribeDepth,
+			bool subscribeLaserScan,
+			bool subscribeOdomInfo,
+			bool subscribeStereo,
+			int queueSize,
+			int depthCameras);
+
+	void commonDepthCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	void commonDepthCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const std::vector<sensor_msgs::ImageConstPtr> & imageMsgs,
+			const std::vector<sensor_msgs::ImageConstPtr> & depthMsgs,
+			const std::vector<sensor_msgs::CameraInfoConstPtr> & cameraInfoMsgs,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	void commonStereoCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
+
+	// With odom msg
+	void depthCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& imageDepthMsg,
+			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depth2Callback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& image1Msg,
+			const sensor_msgs::ImageConstPtr& depth1Msg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
+			const sensor_msgs::ImageConstPtr& image2Msg,
+			const sensor_msgs::ImageConstPtr& depth2Msg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
+	void depthOdomInfoCallback(
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
 			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,
-						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-						   const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void depthOdomInfo2Callback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& image1Msg,
+			const sensor_msgs::ImageConstPtr& depth1Msg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo1Msg,
+			const sensor_msgs::ImageConstPtr& image2Msg,
+			const sensor_msgs::ImageConstPtr& depth2Msg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo2Msg);
+	void depthScanCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
+		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);

 	void stereoScanCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
 	void stereoOdomInfoCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
@@ -111,7 +163,41 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);

+	// with TF
+	void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depthOdomInfoTFCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
+	void depthScanTFCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
+		    const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
+
+	void stereoScanTFCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoOdomInfoTFCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoTFCallback(
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+
 	void processRequestedMap(const rtabmap_ros::MapData & map);
+	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;

 private:
 	QApplication * app_;
@@ -121,16 +207,18 @@ private:

 	// odometry subscription stuffs
 	std::string frameId_;
+	std::string odomFrameId_;
 	bool waitForTransform_;
 	tf::TransformListener tfListener_;

 	message_filters::Subscriber<rtabmap_ros::Info> infoTopic_;
 	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
+	ros::Subscriber globalPathTopic_;

 	ros::Subscriber defaultSub_; // odometry only
-	image_transport::SubscriberFilter imageSub_;
-	image_transport::SubscriberFilter imageDepthSub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	std::vector<image_transport::SubscriberFilter*> imageSubs_;
+	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
+	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>* > cameraInfoSubs_;
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
@@ -145,25 +233,26 @@ private:
 			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
 	message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;

+	// with odom msg
 	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
+			sensor_msgs::LaserScan,
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;

 	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
+			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,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
 	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
@@ -177,8 +266,8 @@ private:
 	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;

 	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
 			sensor_msgs::LaserScan,
+			nav_msgs::Odometry,
 			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo,
@@ -186,13 +275,78 @@ private:
 	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;

 	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
 			rtabmap_ros::OdomInfo,
+			nav_msgs::Odometry,
 			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo,
 			sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
 	message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepth2SyncPolicy;
+	message_filters::Synchronizer<MyDepth2SyncPolicy> * depth2Sync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthOdomInfo2SyncPolicy;
+	message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy> * depthOdomInfo2Sync_;
+
+	// with odom TF
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::LaserScan,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::LaserScan,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
 };

 #endif /* GUIWRAPPER_H_ */
@@ -32,7 +32,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
-#include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/Compression.h>
 #include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/ULogger.h>
@@ -114,41 +113,23 @@ public:
 			int id = msg->nodes[i].id;
 			if(!uContains(rgbClouds_, id))
 			{
-				rtabmap::Transform localTransform = rtabmap_ros::transformFromGeometryMsg(msg->nodes[i].localTransform);
-				if(!localTransform.isNull())
+				rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
+				if(!s.sensorData().imageCompressed().empty() &&
+				   !s.sensorData().depthOrRightCompressed().empty() &&
+				   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
 				{
 					cv::Mat image, depth;
-					float fx = msg->nodes[i].fx;
-					float fy = msg->nodes[i].fy;
-					float cx = msg->nodes[i].cx;
-					float cy = msg->nodes[i].cy;
+					s.sensorData().uncompressData(&image, &depth, 0);

-					//uncompress data
-					rtabmap::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
-					rtabmap::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
-					ctImage.start();
-					ctDepth.start();
-					ctImage.join();
-					ctDepth.join();
-					image = ctImage.getUncompressedData();
-					depth = ctDepth.getUncompressedData();

-					if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
+					if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 					{
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-						if(depth.type() == CV_8UC1)
-						{
-							cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
-						}
-						else
-						{
-							cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
-						}
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(
+								s.sensorData(),
+								cloudDecimation_,
+								cloudMaxDepth_);

-						if(cloud->size() && cloudMaxDepth_ > 0)
-						{
-							cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
-						}
 						if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
 						{
 							pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
@@ -163,9 +144,6 @@ public:

 						if(cloud->size())
 						{
-							cloud = util3d::transformPointCloud(cloud, localTransform);
-
-
 							rgbClouds_.insert(std::make_pair(id, cloud));

 							if(computeOccupancyGrid_)
@@ -213,9 +191,10 @@ public:

 		// filter poses
 		std::map<int, Transform> poses;
-		for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
+		UASSERT(msg->posesId.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
+			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
 		}
 		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
 		{
@@ -117,18 +117,14 @@ public:
 	{
 		// save new poses and constraints
 		// Assuming that nodes/constraints are all linked together
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.stamps.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.userDatas.size());
+		UASSERT(msg->posesId.size() == msg->poses.size());

 		bool dataChanged = false;

 		std::multimap<int, Link> newConstraints;
-		for(unsigned int i=0; i<msg->graph.links.size(); ++i)
+		for(unsigned int i=0; i<msg->links.size(); ++i)
 		{
-			Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
+			Link link = rtabmap_ros::linkFromROS(msg->links[i]);
 			newConstraints.insert(std::make_pair(link.from(), link));

 			bool edgeAlreadyAdded = false;
@@ -152,79 +148,48 @@ public:
 		}

 		std::map<int, Transform> newPoses;
-		std::map<int, int> newMapIds;
-		std::map<int, double> newStamps;
-		std::map<int, std::string> newLabels;
-		std::map<int, std::vector<unsigned char> > newUserDatas;
 		// add new odometry poses
 		for(unsigned int i=0; i<msg->nodes.size(); ++i)
 		{
 			int id = msg->nodes[i].id;
 			Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
 			newPoses.insert(std::make_pair(id, pose));
-			newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
-			newStamps.insert(std::make_pair(id, msg->nodes[i].stamp));
-			newLabels.insert(std::make_pair(id, msg->nodes[i].label));
-			newUserDatas.insert(std::make_pair(id, msg->nodes[i].userData.data));

 			std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
 			if(!p.second && pose != cachedPoses_.at(id))
 			{
 				dataChanged = true;
 			}
-			else if(p.second)
-			{
-				cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
-				cachedStamps_.insert(std::make_pair(id, msg->nodes[i].stamp));
-				cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
-				cachedUserDatas_.insert(std::make_pair(id, msg->nodes[i].userData.data));
-			}
 		}

 		if(dataChanged)
 		{
 			ROS_WARN("Graph data has changed! Reset cache...");
 			cachedPoses_ = newPoses;
-			cachedMapIds_ = newMapIds;
-			cachedStamps_ = newStamps;
-			cachedLabels_ = newLabels;
-			cachedUserDatas_ = newUserDatas;
 			cachedConstraints_ = newConstraints;
 		}

 		//match poses in the graph
 		std::map<int, Transform> poses;
-		std::map<int, int> mapIds;
-		std::map<int, double> stamps;
-		std::map<int, std::string> labels;
-		std::map<int, std::vector<unsigned char> > userDatas;
 		std::multimap<int, Link> constraints;
 		if(globalOptimization_)
 		{
 			poses = cachedPoses_;
-			mapIds = cachedMapIds_;
-			stamps = cachedStamps_;
-			labels = cachedLabels_;
-			userDatas = cachedUserDatas_;
 			constraints = cachedConstraints_;
 		}
 		else
 		{
 			constraints = newConstraints;
-			for(unsigned int i=0; i<msg->graph.nodeIds.size(); ++i)
+			for(unsigned int i=0; i<msg->posesId.size(); ++i)
 			{
-				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.nodeIds[i]);
+				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->posesId[i]);
 				if(iter != cachedPoses_.end())
 				{
 					poses.insert(*iter);
-					mapIds.insert(*cachedMapIds_.find(iter->first));
-					stamps.insert(*cachedStamps_.find(iter->first));
-					labels.insert(*cachedLabels_.find(iter->first));
-					userDatas.insert(*cachedUserDatas_.find(iter->first));
 				}
 				else
 				{
-					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.nodeIds[i]);
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->posesId[i]);
 					return;
 				}
 			}
@@ -236,12 +201,12 @@ public:
 			UTimer timer;
 			std::map<int, Transform> optimizedPoses;
 			Transform mapCorrection = Transform::getIdentity();
+			std::multimap<int, rtabmap::Link> linksOut;
 			if(poses.size() > 1 && constraints.size() > 0)
 			{
 				graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
 				int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
 				std::map<int, rtabmap::Transform> posesOut;
-				std::multimap<int, rtabmap::Link> linksOut;
 				optimizer.getConnectedGraph(
 						fromId,
 						poses,
@@ -264,19 +229,13 @@ public:
 				ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
 					   "not be null if there are edges, and edges must be null if poses <= 1)",
 					  (int)poses.size(), (int)constraints.size());
-				mapIds.clear();
-				labels.clear();
-				stamps.clear();
-				userDatas.clear();
 			}

-			UASSERT(optimizedPoses.size() == mapIds.size());
-			UASSERT(optimizedPoses.size() == labels.size());
-			UASSERT(optimizedPoses.size() == stamps.size());
-			UASSERT(optimizedPoses.size() == userDatas.size());
 			rtabmap_ros::MapData outputMsg;
-			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, stamps, labels, userDatas, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
-			outputMsg.graph.links = msg->graph.links;
+			rtabmap_ros::mapDataToROS(optimizedPoses,
+					linksOut,
+					mapCorrection,
+					outputMsg);
 			outputMsg.header = msg->header;
 			outputMsg.nodes = msg->nodes;
 			mapDataPub_.publish(outputMsg);
@@ -301,10 +260,6 @@ private:
 	ros::Publisher mapDataPub_;

 	std::map<int, Transform> cachedPoses_;
-	std::map<int, int> cachedMapIds_;
-	std::map<int, double> cachedStamps_;
-	std::map<int, std::string> cachedLabels_;
-	std::map<int, std::vector<unsigned char> > cachedUserDatas_;
 	std::multimap<int, Link> cachedConstraints_;

 	tf2_ros::TransformBroadcaster tfBroadcaster_;
@@ -71,7 +71,7 @@ MapsManager::MapsManager() :
 	// common map stuff
 	pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
 	pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
-	pnh.param("map_mapsManager_cleanup", mapCacheCleanup_, mapCacheCleanup_);
+	pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);

 	// mapping topics
 	cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
@@ -162,7 +162,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		{
 			if(!iter->second.isNull())
 			{
-				rtabmap::Signature data;
+				rtabmap::SensorData data;
 				bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
 				bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
 				bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
@@ -175,7 +175,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
 						if(findIter != signatures.end())
 						{
-							data = findIter->second;
+							data = findIter->second.sensorData();
 						}
 					}
 					else
@@ -186,55 +186,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(

 				if(data.id() > 0)
 				{
-					rtabmap::Transform localTransform = data.getLocalTransform();
-					if(!localTransform.isNull())
+					if(!data.imageCompressed().empty() &&
+					   !data.depthOrRightCompressed().empty() &&
+					   (data.cameraModels().size() || data.stereoCameraModel().isValid()))
 					{
 						// Which data should we decompress?
 						cv::Mat image, depth, scan;
-						data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
-						if(!depth.empty() &&
-							depth.type() == CV_8UC1 &&
-							image.empty() &&
-							!rgbDepthRequired)
-						{
-							// Stereo detected, we should uncompress left image too
-							data.uncompressDataConst(&image, 0, 0);
-						}
-						float fx = data.getFx();
-						float fy = data.getFy();
-						float cx = data.getCx();
-						float cy = data.getCy();
+						data.uncompressData(rgbDepthRequired||data.stereoCameraModel().isValid()?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);

 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
 						if(rgbDepthRequired)
 						{
-							if(!image.empty() &&
-								!depth.empty() &&
-								fx > 0.0f && fy > 0.0f &&
-								cx >= 0.0f && cy >= 0.0f)
+							if(!image.empty() && !depth.empty())
 							{
-								if(depth.type() == CV_8UC1)
-								{
-									cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-								else
-								{
-									cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-
-								if(cloudRGB->size() && cloudMaxDepth_ > 0)
-								{
-									cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
-								}
-								if(cloudRGB->size() && cloudVoxelSize_ > 0)
-								{
-									cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
-								}
-								if(cloudRGB->size())
-								{
-									cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
-								}
+								cloudRGB = util3d::cloudRGBFromSensorData(
+										data,
+										cloudDecimation_,
+										cloudMaxDepth_,
+										cloudVoxelSize_);
 							}
 							else
 							{
@@ -243,55 +213,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						}
 						else if(depthRequired)
 						{
-							if(	!depth.empty() &&
-								fx > 0.0f && fy > 0.0f &&
-								cx >= 0.0f && cy >= 0.0f)
+							if(	!depth.empty())
 							{
-								if(depth.type() == CV_8UC1)
-								{
-									if(!image.empty())
-									{
-										cv::Mat leftMono;
-										if(image.channels() == 3)
-										{
-											cv::cvtColor(image, leftMono, CV_BGR2GRAY);
-										}
-										else
-										{
-											leftMono = image;
-										}
-										cloudXYZ = rtabmap::util3d::cloudFromDisparity(
-												util2d::disparityFromStereoImages(leftMono, depth),
-												cx, cy,
-												fx, fy,
-												cloudDecimation_);
-									}
-								}
-								else
-								{
-									cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-
-								if(cloudXYZ.get())
-								{
-									if(cloudXYZ->size() && cloudMaxDepth_ > 0)
-									{
-										cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
-									}
-									if(cloudXYZ->size() && gridCellSize_ > 0)
-									{
-										// use gridCellSize since this cloud is only for the projection map
-										cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
-									}
-									if(cloudXYZ->size())
-									{
-										cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
-									}
-								}
-								else
-								{
-									ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
-								}
+								cloudXYZ = util3d::cloudFromSensorData(
+										data,
+										cloudDecimation_,
+										cloudMaxDepth_,
+										gridCellSize_); // use gridCellSize since this cloud is only for the projection map
 							}
 							else
 							{
@@ -290,91 +290,91 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
 	}
 }

-void mapGraphFromROS(
-		const rtabmap_ros::Graph & msg,
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
+		std::map<int, rtabmap::Transform> & poses,
+		std::multimap<int, rtabmap::Link> & links,
+		std::map<int, rtabmap::Signature> & signatures,
+		rtabmap::Transform & mapToOdom)
+{
+	//optimized graph
+	mapDataFromROS(msg, poses, links, mapToOdom);
+
+	//Data
+	for(unsigned int i=0; i<msg.nodes.size(); ++i)
+	{
+		signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
+	}
+}
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom)
 {
-	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
-
-	UASSERT(msg.nodeIds.size() == msg.mapIds.size());
-	UASSERT(msg.nodeIds.size() == msg.poses.size());
-	UASSERT(msg.nodeIds.size() == msg.stamps.size());
-	UASSERT(msg.nodeIds.size() == msg.labels.size());
-	UASSERT(msg.nodeIds.size() == msg.userDatas.size());
-
-	for(unsigned int i=0; i<msg.nodeIds.size(); ++i)
+	//optimized graph
+	UASSERT(msg.posesId.size() == msg.poses.size());
+	for(unsigned int i=0; i<msg.posesId.size(); ++i)
 	{
-		poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
-		mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
-		stamps.insert(std::make_pair(msg.nodeIds[i], msg.stamps[i]));
-		labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
-		userDatas.insert(std::make_pair(msg.nodeIds[i], msg.userDatas[i].data));
+		poses.insert(std::make_pair(msg.posesId[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
 	}
-
 	for(unsigned int i=0; i<msg.links.size(); ++i)
 	{
 		rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform);
 		links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i])));
 	}
+	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
 }
-void mapGraphToROS(
+void mapDataToROS(
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links,
+		const std::map<int, rtabmap::Signature> & signatures,
+		const rtabmap::Transform & mapToOdom,
+		rtabmap_ros::MapData & msg)
+{
+	//Optimized graph
+	mapDataToROS(poses, links, mapToOdom, msg);
+
+	//Data
+	msg.nodes.resize(signatures.size());
+	int index=0;
+	for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
+		iter!=signatures.end();
+		++iter)
+	{
+		nodeDataToROS(iter->second, msg.nodes[index++]);
+	}
+}
+
+void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
-		const std::map<int, int> & mapIds,
-		const std::map<int, double> & stamps,
-		const std::map<int, std::string> & labels,
-		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::Graph & msg)
+		rtabmap_ros::MapData & msg)
 {
-	UASSERT(poses.size() == 0 ||
-			(poses.size() == mapIds.size() &&
-			 poses.size() == labels.size() &&
-			 poses.size() == stamps.size() &&
-			 poses.size() == userDatas.size()));
-
-	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
-
-	msg.nodeIds.resize(poses.size());
+	//Optimized graph
+	msg.posesId.resize(poses.size());
 	msg.poses.resize(poses.size());
-	msg.mapIds.resize(poses.size());
-	msg.stamps.resize(poses.size());
-	msg.labels.resize(poses.size());
-	msg.userDatas.resize(poses.size());
 	int index = 0;
-	std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
-	std::map<int, int>::const_iterator iterMapIds = mapIds.begin();
-	std::map<int, double>::const_iterator iterStamps = stamps.begin();
-	std::map<int, std::string>::const_iterator iterLabels = labels.begin();
-	std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = userDatas.begin();
-	while(iterPoses != poses.end())
+	for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
+		iter != poses.end();
+		++iter)
 	{
-		msg.nodeIds[index] = iterPoses->first;
-		msg.mapIds[index] = iterMapIds->second;
-		msg.stamps[index] = iterStamps->second;
-		msg.labels[index] = iterLabels->second;
-		msg.userDatas[index].data = iterUserDatas->second;
-		transformToPoseMsg(iterPoses->second, msg.poses[index]);
-		++iterPoses;
-		++iterMapIds;
-		++iterStamps;
-		++iterLabels;
-		++iterUserDatas;
+		msg.posesId[index] = iter->first;
+		transformToPoseMsg(iter->second, msg.poses[index]);
 		++index;
 	}

 	msg.links.resize(links.size());
 	index=0;
-	for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+	for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
+		iter!=links.end();
+		++iter)
 	{
 		linkToROS(iter->second, msg.links[index++]);
 	}
+
+	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
 }

 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
@@ -404,24 +404,76 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
 	}

-	return rtabmap::Signature(
+	rtabmap::StereoCameraModel stereoModel;
+	std::vector<rtabmap::CameraModel> models;
+	if(msg.baseline > 0.0f)
+	{
+		// stereo model
+		if(msg.fx.size() == 1 &&
+		   msg.fy.size() == 1,
+		   msg.cx.size() == 1,
+		   msg.cy.size() == 1,
+		   msg.localTransform.size() == 1)
+		{
+			stereoModel = rtabmap::StereoCameraModel(
+					msg.fx[0],
+					msg.fy[0],
+					msg.cx[0],
+					msg.cy[0],
+					msg.baseline,
+					transformFromGeometryMsg(msg.localTransform[0]));
+		}
+	}
+	else
+	{
+		// multi-cameras model
+		if(msg.fx.size() &&
+		   msg.fx.size() == msg.fy.size(),
+		   msg.fx.size() == msg.cx.size(),
+		   msg.fx.size() == msg.cy.size(),
+		   msg.fx.size() == msg.localTransform.size())
+		{
+			for(unsigned int i=0; i<msg.fx.size(); ++i)
+			{
+				models.push_back(rtabmap::CameraModel(
+						msg.fx[i],
+						msg.fy[i],
+						msg.cx[i],
+						msg.cy[i],
+						transformFromGeometryMsg(msg.localTransform[i])));
+			}
+		}
+	}
+
+	rtabmap::Signature s(
 			msg.id,
 			msg.mapId,
 			msg.weight,
 			msg.stamp,
 			msg.label,
-			words,
-			words3D,
 			transformFromPoseMsg(msg.pose),
-			msg.userData.data,
-			compressedMatFromBytes(msg.laserScan),
-			compressedMatFromBytes(msg.image),
-			compressedMatFromBytes(msg.depth),
-			msg.fx,
-			msg.fy,
-			msg.cx,
-			msg.cy,
-			transformFromGeometryMsg(msg.localTransform));
+			stereoModel.isValid()?
+				rtabmap::SensorData(
+					compressedMatFromBytes(msg.laserScan),
+					msg.laserScanMaxPts,
+					compressedMatFromBytes(msg.image),
+					compressedMatFromBytes(msg.depth),
+					stereoModel,
+					msg.id,
+					msg.stamp,
+					compressedMatFromBytes(msg.userData)):
+				rtabmap::SensorData(
+					compressedMatFromBytes(msg.laserScan),
+					msg.laserScanMaxPts,
+					compressedMatFromBytes(msg.image),
+					compressedMatFromBytes(msg.depth),
+					models,
+					msg.id,
+					msg.stamp,
+					compressedMatFromBytes(msg.userData)));
+	s.setWords(words);
+	s.setWords3(words3D);
+	return s;
 }
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
 {
@@ -431,16 +483,38 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	msg.weight = signature.getWeight();
 	msg.stamp = signature.getStamp();
 	msg.label = signature.getLabel();
-	msg.userData.data = signature.getUserData();
 	transformToPoseMsg(signature.getPose(), msg.pose);
-	compressedMatToBytes(signature.getImageCompressed(), msg.image);
-	compressedMatToBytes(signature.getDepthCompressed(), msg.depth);
-	compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan);
-	msg.fx = signature.getFx();
-	msg.fy = signature.getFy();
-	msg.cx = signature.getCx();
-	msg.cy = signature.getCy();
-	transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform);
+	compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
+	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
+	compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
+	compressedMatToBytes(signature.sensorData().userDataCompressed(), msg.userData);
+	msg.baseline = 0;
+	if(signature.sensorData().cameraModels().size())
+	{
+		msg.fx.resize(signature.sensorData().cameraModels().size());
+		msg.fy.resize(signature.sensorData().cameraModels().size());
+		msg.cx.resize(signature.sensorData().cameraModels().size());
+		msg.cy.resize(signature.sensorData().cameraModels().size());
+		msg.localTransform.resize(signature.sensorData().cameraModels().size());
+		for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
+		{
+			msg.fx[i] = signature.sensorData().cameraModels()[i].fx();
+			msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
+			msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
+			msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
+			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
+		}
+	}
+	else if(signature.sensorData().stereoCameraModel().isValid())
+	{
+		msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
+		msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
+		msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
+		msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
+		msg.baseline = signature.sensorData().stereoCameraModel().baseline();
+		msg.localTransform.resize(1);
+		transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
+	}

 	//Features stuff...
 	msg.wordIds = uKeys(signature.getWords());
@@ -482,8 +556,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.features = msg.features;
 	info.inliers = msg.inliers;
 	info.localMapSize = msg.localMapSize;
-	info.time = msg.time;
+	info.timeEstimation = msg.timeEstimation;
 	info.variance = msg.variance;
+	info.timeParticleFiltering =  msg.timeParticleFiltering;
+	info.stamp = msg.stamp;
+	info.interval = msg.interval;
+	info.distanceTravelled = msg.distanceTravelled;

 	info.type = msg.type;

@@ -500,6 +578,9 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.newCorners = points2fFromROS(msg.newCorners);
 	info.cornerInliers = msg.cornerInliers;

+	info.transform = transformFromGeometryMsg(msg.transform);
+	info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
+
 	return info;
 }

@@ -510,8 +591,13 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.features = info.features;
 	msg.inliers = info.inliers;
 	msg.localMapSize = info.localMapSize;
-	msg.time = info.time;
+	msg.timeEstimation = info.timeEstimation;
 	msg.variance = info.variance;
+	msg.timeParticleFiltering =  info.timeParticleFiltering;
+	msg.stamp = info.stamp;
+	msg.interval = info.interval;
+	msg.distanceTravelled = info.distanceTravelled;
+

 	msg.type = info.type;

@@ -525,6 +611,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	points2fToROS(info.newCorners, msg.newCorners);
 	msg.cornerInliers = info.cornerInliers;

+	transformToGeometryMsg(info.transform, msg.transform);
+	transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
+
 }

 }
@@ -72,12 +72,31 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :

 	Transform initialPose = Transform::getIdentity();
 	std::string initialPoseStr;
+	std::string tfPrefix;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
 	pnh.param("publish_tf", publishTf_, publishTf_);
+	pnh.param("tf_prefix", tfPrefix, tfPrefix);
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("initial_pose", initialPoseStr, initialPoseStr); // "x y z roll pitch yaw"
 	pnh.param("ground_truth_frame_id", groundTruthFrameId_, groundTruthFrameId_);
+
+	if(!tfPrefix.empty())
+	{
+		if(!frameId_.empty())
+		{
+			frameId_ = tfPrefix + "/" + frameId_;
+		}
+		if(!odomFrameId_.empty())
+		{
+			odomFrameId_ = tfPrefix + "/" + odomFrameId_;
+		}
+		if(!groundTruthFrameId_.empty())
+		{
+			groundTruthFrameId_ = tfPrefix + "/" + groundTruthFrameId_;
+		}
+	}
+
 	if(initialPoseStr.size())
 	{
 		std::vector<std::string> values = uListToVector(uSplit(initialPoseStr, ' '));
@@ -157,6 +176,7 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 	oldParameterNames.push_back("Odom/LocalHistory");
 	oldParameterNames.push_back("Odom/NearestNeighbor");
 	oldParameterNames.push_back("Odom/NNDR");
+	oldParameterNames.push_back("GFTT/MaxCorners");
 	for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
 	{
 		std::string vStr;
@@ -192,6 +212,12 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 						Parameters::kOdomBowNNDR().c_str());
 				parameters_.at(Parameters::kOdomBowNNDR())= vStr;
 			}
+			else if(iter->compare("GFTT/MaxCorners") == 0)
+			{
+				ROS_WARN("Parameter GFTT/MaxCorners doesn't exist anymore, use %s. Please update your launch file accordingly.",
+						Parameters::kOdomMaxFeatures().c_str());
+				parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
+			}
 		}
 	}

@@ -276,7 +302,7 @@ void OdometryROS::processArguments(int argc, char * argv[])
 	}
 }

-void OdometryROS::processData(const SensorData & data, const std_msgs::Header & header)
+void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 {
 	if(odometry_->getPose().isNull() &&
 	   !groundTruthFrameId_.empty())
@@ -286,13 +312,13 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 		{
 			if(this->waitForTransform())
 			{
-				if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, header.stamp, ros::Duration(1)))
+				if(!this->tfListener().waitForTransform(groundTruthFrameId_, frameId_, stamp, ros::Duration(1)))
 				{
 					ROS_WARN("Could not get transform from %s to %s after 1 second!", groundTruthFrameId_.c_str(), frameId_.c_str());
 					return;
 				}
 			}
-			this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, header.stamp, initialPose);
+			this->tfListener().lookupTransform(groundTruthFrameId_, frameId_, stamp, initialPose);
 		}
 		catch(tf::TransformException & ex)
 		{
@@ -319,7 +345,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 		geometry_msgs::TransformStamped poseMsg;
 		poseMsg.child_frame_id = frameId_;
 		poseMsg.header.frame_id = odomFrameId_;
-		poseMsg.header.stamp = header.stamp;
+		poseMsg.header.stamp = stamp;
 		rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);

 		if(publishTf_)
@@ -331,7 +357,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 		{
 			//next, we'll publish the odometry message over ROS
 			nav_msgs::Odometry odom;
-			odom.header.stamp = header.stamp; // use corresponding time stamp to image
+			odom.header.stamp = stamp; // use corresponding time stamp to image
 			odom.header.frame_id = odomFrameId_;
 			odom.child_frame_id = frameId_;

@@ -363,7 +389,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 			}
 			sensor_msgs::PointCloud2 cloudMsg;
 			pcl::toROSMsg(cloud, cloudMsg);
-			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
 			cloudMsg.header.frame_id = odomFrameId_;
 			odomLocalMap_.publish(cloudMsg);
 		}
@@ -377,7 +403,6 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 				{
 					const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
 					pcl::PointCloud<pcl::PointXYZ> cloud;
-					rtabmap::Transform t = data.localTransform();
 					for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
 					{
 						// transform to odom frame
@@ -387,7 +412,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &

 					sensor_msgs::PointCloud2 cloudMsg;
 					pcl::toROSMsg(cloud, cloudMsg);
-					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
@@ -402,7 +427,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 					cloudTransformed = util3d::transformPointCloud(cloud, pose);
 					sensor_msgs::PointCloud2 cloudMsg;
 					pcl::toROSMsg(*cloudTransformed, cloudMsg);
-					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
 					cloudMsg.header.frame_id = odomFrameId_;
 					odomLastFrame_.publish(cloudMsg);
 				}
@@ -415,7 +440,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &

 		//send null pose to notify that odometry is lost
 		nav_msgs::Odometry odom;
-		odom.header.stamp = header.stamp; // use corresponding time stamp to image
+		odom.header.stamp = stamp; // use corresponding time stamp to image
 		odom.header.frame_id = odomFrameId_;
 		odom.child_frame_id = frameId_;

@@ -427,7 +452,7 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 	{
 		rtabmap_ros::OdomInfo infoMsg;
 		odomInfoToROS(info, infoMsg);
-		infoMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+		infoMsg.header.stamp = stamp; // use corresponding time stamp to image
 		infoMsg.header.frame_id = odomFrameId_;
 		odomInfoPub_.publish(infoMsg);
 	}
@@ -53,7 +53,7 @@ public:
 	~OdometryROS();

 	void processArguments(int argc, char * argv[]);
-	void processData(const rtabmap::SensorData & data, const std_msgs::Header & header);
+	void processData(const rtabmap::SensorData & data, const ros::Time & stamp);

 	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetToPose(rtabmap_ros::ResetPose::Request&, rtabmap_ros::ResetPose::Response&);
@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include "rtabmap_ros/MsgConversion.h"

+#include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/ULogger.h>

 using namespace rtabmap;
@@ -51,43 +52,112 @@ class RGBDOdometry : public rtabmap_ros::OdometryROS
 public:
 	RGBDOdometry(int argc, char * argv[]) :
 		rtabmap_ros::OdometryROS(argc, argv),
-		sync_(0)
+		sync_(0),
+		sync2_(0)
 	{
 		ros::NodeHandle nh;
 		ros::NodeHandle pnh("~");

 		int queueSize = 5;
+		int depthCameras = 1;
 		pnh.param("queue_size", queueSize, queueSize);
+		pnh.param("depth_cameras", depthCameras, depthCameras);
+		if(depthCameras <= 0)
+		{
+			depthCameras = 1;
+		}
+		if(depthCameras > 2)
+		{
+			ROS_FATAL("Only 2 cameras maximum supported yet.");
+		}

-		ros::NodeHandle rgb_nh(nh, "rgb");
-		ros::NodeHandle depth_nh(nh, "depth");
-		ros::NodeHandle rgb_pnh(pnh, "rgb");
-		ros::NodeHandle depth_pnh(pnh, "depth");
-		image_transport::ImageTransport rgb_it(rgb_nh);
-		image_transport::ImageTransport depth_it(depth_nh);
-		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+		if(depthCameras == 2)
+		{
+			ros::NodeHandle rgb0_nh(nh, "rgb0");
+			ros::NodeHandle depth0_nh(nh, "depth0");
+			ros::NodeHandle rgb0_pnh(pnh, "rgb0");
+			ros::NodeHandle depth0_pnh(pnh, "depth0");
+			image_transport::ImageTransport rgb0_it(rgb0_nh);
+			image_transport::ImageTransport depth0_it(depth0_nh);
+			image_transport::TransportHints hintsRgb0("raw", ros::TransportHints(), rgb0_pnh);
+			image_transport::TransportHints hintsDepth0("raw", ros::TransportHints(), depth0_pnh);

-		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+			image_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
+			image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
+			info_sub_.subscribe(rgb0_nh, "camera_info", 1);

-		ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
-				ros::this_node::getName().c_str(),
-				image_mono_sub_.getTopic().c_str(),
-				image_depth_sub_.getTopic().c_str(),
-				info_sub_.getTopic().c_str());
+			ros::NodeHandle rgb1_nh(nh, "rgb1");
+			ros::NodeHandle depth1_nh(nh, "depth1");
+			ros::NodeHandle rgb1_pnh(pnh, "rgb1");
+			ros::NodeHandle depth1_pnh(pnh, "depth1");
+			image_transport::ImageTransport rgb1_it(rgb1_nh);
+			image_transport::ImageTransport depth1_it(depth1_nh);
+			image_transport::TransportHints hintsRgb1("raw", ros::TransportHints(), rgb1_pnh);
+			image_transport::TransportHints hintsDepth1("raw", ros::TransportHints(), depth1_pnh);

-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
-		sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
+			image_depth2_sub_.subscribe(depth1_it, depth1_nh.resolveName("image"), 1, hintsDepth1);
+			info2_sub_.subscribe(rgb1_nh, "camera_info", 1);
+
+			ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ros::this_node::getName().c_str(),
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str(),
+					image_mono2_sub_.getTopic().c_str(),
+					image_depth2_sub_.getTopic().c_str(),
+					info2_sub_.getTopic().c_str());
+
+			sync2_ = new message_filters::Synchronizer<MySync2Policy>(
+					MySync2Policy(queueSize),
+					image_mono_sub_,
+					image_depth_sub_,
+					info_sub_,
+					image_mono2_sub_,
+					image_depth2_sub_,
+					info2_sub_);
+			sync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2, _3, _4, _5, _6));
+		}
+		else
+		{
+			ros::NodeHandle rgb_nh(nh, "rgb");
+			ros::NodeHandle depth_nh(nh, "depth");
+			ros::NodeHandle rgb_pnh(pnh, "rgb");
+			ros::NodeHandle depth_pnh(pnh, "depth");
+			image_transport::ImageTransport rgb_it(rgb_nh);
+			image_transport::ImageTransport depth_it(depth_nh);
+			image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+			image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+			image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+			ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
+					ros::this_node::getName().c_str(),
+					image_mono_sub_.getTopic().c_str(),
+					image_depth_sub_.getTopic().c_str(),
+					info_sub_.getTopic().c_str());
+
+			sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
+			sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+		}
 	}

 	~RGBDOdometry()
 	{
-		delete sync_;
+		if(sync_)
+		{
+			delete sync_;
+		}
+		if(sync2_)
+		{
+			delete sync2_;
+		}
 	}

-	void callback(const sensor_msgs::ImageConstPtr& image,
+	void callback(
+			const sensor_msgs::ImageConstPtr& image,
 			const sensor_msgs::ImageConstPtr& depth,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
 	{
@@ -118,18 +188,20 @@ public:
 				}
 			}

+			ros::Time stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
+
 			tf::StampedTransform localTransform;
 			try
 			{
 				if(this->waitForTransform())
 				{
-					if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, image->header.stamp, ros::Duration(1)))
+					if(!this->tfListener().waitForTransform(this->frameId(), image->header.frame_id, stamp, ros::Duration(1)))
 					{
 						ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), image->header.frame_id.c_str());
 						return;
 					}
 				}
-				this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, image->header.stamp, localTransform);
+				this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, stamp, localTransform);
 			}
 			catch(tf::TransformException & ex)
 			{
@@ -141,38 +213,192 @@ public:
 			{
 				image_geometry::PinholeCameraModel model;
 				model.fromCameraInfo(*cameraInfo);
-				float fx = model.fx();
-				float fy = model.fy();
-				float cx = model.cx();
-				float cy = model.cy();
+				rtabmap::CameraModel rtabmapModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						rtabmap_ros::transformFromTF(localTransform));
 				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
 				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);

 				rtabmap::SensorData data(
 						ptrImage->image,
 						ptrDepth->image,
-						fx,
-						fy,
-						cx,
-						cy,
-						rtabmap_ros::transformFromTF(localTransform),
-						rtabmap::Transform(),
-						1.0f,
-						1.0f,
+						rtabmapModel,
 						0,
-						rtabmap_ros::timestampFromROS(image->header.stamp));
+						rtabmap_ros::timestampFromROS(stamp));

-				this->processData(data, image->header);
+				this->processData(data, stamp);
 			}
 		}
 	}

+	void callback2(
+			const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo,
+			const sensor_msgs::ImageConstPtr& image2,
+			const sensor_msgs::ImageConstPtr& depth2,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo2)
+	{
+		if(!this->isPaused())
+		{
+			std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+			std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+			std::vector<sensor_msgs::CameraInfoConstPtr> infoMsgs;
+			imageMsgs.push_back(image);
+			imageMsgs.push_back(image2);
+			depthMsgs.push_back(depth);
+			depthMsgs.push_back(depth2);
+			infoMsgs.push_back(cameraInfo);
+			infoMsgs.push_back(cameraInfo2);
+
+			ros::Time higherStamp;
+			int imageWidth = imageMsgs[0]->width;
+			int imageHeight = imageMsgs[0]->height;
+			int cameraCount = imageMsgs.size();
+			cv::Mat rgb;
+			cv::Mat depth;
+			pcl::PointCloud<pcl::PointXYZ> scanCloud;
+			std::vector<CameraModel> cameraModels;
+			for(unsigned int i=0; i<imageMsgs.size(); ++i)
+			{
+				if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+					 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+					!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+					 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+				{
+					ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+					return;
+				}
+				UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
+				UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
+
+				ros::Time stamp = imageMsgs[i]->header.stamp>depthMsgs[i]->header.stamp?imageMsgs[i]->header.stamp:depthMsgs[i]->header.stamp;
+
+				if(i == 0)
+				{
+					higherStamp = stamp;
+				}
+				else if(stamp > higherStamp)
+				{
+					higherStamp = stamp;
+				}
+
+				tf::StampedTransform localTransform;
+				try
+				{
+					if(this->waitForTransform())
+					{
+						if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, ros::Duration(1)))
+						{
+							ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageMsgs[i]->header.frame_id.c_str());
+							return;
+						}
+					}
+					this->tfListener().lookupTransform(this->frameId(), imageMsgs[i]->header.frame_id, stamp, localTransform);
+				}
+				catch(tf::TransformException & ex)
+				{
+					ROS_WARN("%s",ex.what());
+					return;
+				}
+
+				cv_bridge::CvImageConstPtr ptrImage;
+				if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0)
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i]);
+				}
+				else if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+				   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
+				}
+				else
+				{
+					ptrImage = cv_bridge::toCvShare(imageMsgs[i], "bgr8");
+				}
+				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsgs[i]);
+				cv::Mat subDepth = ptrDepth->image;
+				if(subDepth.type() == CV_32FC1)
+				{
+					subDepth = rtabmap::util3d::cvtDepthFromFloat(subDepth);
+					static bool shown = false;
+					if(!shown)
+					{
+						ROS_WARN("Use depth image with \"unsigned short\" type to "
+								 "avoid conversion. This message is only printed once...");
+						shown = true;
+					}
+				}
+
+				// initialize
+				if(rgb.empty())
+				{
+					rgb = cv::Mat(imageHeight, imageWidth*cameraCount, ptrImage->image.type());
+				}
+				if(depth.empty())
+				{
+					depth = cv::Mat(imageHeight, imageWidth*cameraCount, subDepth.type());
+				}
+
+				if(ptrImage->image.type() == rgb.type())
+				{
+					ptrImage->image.copyTo(cv::Mat(rgb, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+				}
+				else
+				{
+					ROS_ERROR("Some RGB images are not the same type!");
+					return;
+				}
+
+				if(subDepth.type() == depth.type())
+				{
+					subDepth.copyTo(cv::Mat(depth, cv::Rect(i*imageWidth, 0, imageWidth, imageHeight)));
+				}
+				else
+				{
+					ROS_ERROR("Some Depth images are not the same type!");
+					return;
+				}
+
+				image_geometry::PinholeCameraModel model;
+				model.fromCameraInfo(*infoMsgs[i]);
+				cameraModels.push_back(rtabmap::CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						rtabmap_ros::transformFromTF(localTransform)));
+			}
+
+			rtabmap::SensorData data(
+					rgb,
+					depth,
+					cameraModels,
+					0,
+					rtabmap_ros::timestampFromROS(higherStamp));
+
+			this->processData(data, higherStamp);
+		}
+	}
+
 private:
 	image_transport::SubscriberFilter image_mono_sub_;
 	image_transport::SubscriberFilter image_depth_sub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	image_transport::SubscriberFilter image_mono2_sub_;
+	image_transport::SubscriberFilter image_depth2_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info2_sub_;
 	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
 	message_filters::Synchronizer<MySyncPolicy> * sync_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySync2Policy;
+	message_filters::Synchronizer<MySync2Policy> * sync2_;
 };

 int main(int argc, char *argv[])
@@ -132,19 +132,21 @@ public:
 				return;
 			}

+			ros::Time stamp = imageRectLeft->header.stamp>imageRectRight->header.stamp?imageRectLeft->header.stamp:imageRectRight->header.stamp;
+
 			tf::StampedTransform localTransform;
 			try
 			{
 				if(this->waitForTransform())
 				{
-					if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, ros::Duration(1)))
+					if(!this->tfListener().waitForTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, ros::Duration(1)))
 					{
 						ROS_WARN("Could not get transform from %s to %s after 1 second!", this->frameId().c_str(), imageRectLeft->header.frame_id.c_str());
 						return;
 					}
 				}

-				this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
+				this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, stamp, localTransform);
 			}
 			catch(tf::TransformException & ex)
 			{
@@ -159,38 +161,35 @@ public:
 			{
 				image_geometry::StereoCameraModel model;
 				model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
-
-				float fx = model.left().fx();
-				float cx = model.left().cx();
-				float cy = model.left().cy();
-				float baseline = model.baseline();
-				cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
-				cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
-
-				if(baseline <= 0)
+				if(model.baseline() <= 0)
 				{
 					ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
-							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", baseline);
+							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
 					return;
 				}

+				rtabmap::StereoCameraModel stereoModel(
+						model.left().fx(),
+						model.left().fy(),
+						model.left().cx(),
+						model.left().cy(),
+						model.baseline(),
+						rtabmap_ros::transformFromTF(localTransform));
+
+				cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
+				cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
+
 				UTimer stepTimer;
 				//
 				UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
-				rtabmap::SensorData data(ptrImageLeft->image,
+				rtabmap::SensorData data(
+						ptrImageLeft->image,
 						ptrImageRight->image,
-						fx,
-						baseline,
-						cx,
-						cy,
-						rtabmap_ros::transformFromTF(localTransform),
-						rtabmap::Transform(),
-						1.0f,
-						1.0f,
+						stereoModel,
 						0,
-						rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp));
+						rtabmap_ros::timestampFromROS(stamp));

-				this->processData(data, imageRectLeft->header);
+				this->processData(data, stamp);
 			}
 			else
 			{
@@ -171,7 +171,7 @@ private:

 		if(cloudPub_.getNumSubscribers())
 		{
-			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0?"":"mono8");
+			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);

 			image_geometry::PinholeCameraModel model;
@@ -252,66 +252,45 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 		if(cloud_infos_.find(id) == cloud_infos_.end())
 		{
 			// Cloud not added to RVIZ, add it!
-			rtabmap::Transform localTransform = transformFromGeometryMsg(map.nodes[i].localTransform);
-			if(!localTransform.isNull())
+			rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
+			if(!s.sensorData().imageCompressed().empty() &&
+			   !s.sensorData().depthOrRightCompressed().empty() &&
+			   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
 			{
 				cv::Mat image, depth;
-				float fx = map.nodes[i].fx;
-				float fy = map.nodes[i].fy;
-				float cx = map.nodes[i].cx;
-				float cy = map.nodes[i].cy;
+				s.sensorData().uncompressData(&image, &depth, 0);

-				//uncompress data
-				rtabmap::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
-				rtabmap::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
-				ctImage.start();
-				ctDepth.start();
-				ctImage.join();
-				ctDepth.join();
-				image = ctImage.getUncompressedData();
-				depth = ctDepth.getUncompressedData();

-				if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
+				if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					if(depth.type() == CV_8UC1)
-					{
-						cloud = rtabmap::util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
-					}
-					else
-					{
-						cloud = rtabmap::util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
-					}
-					if(cloud_max_depth_->getFloat() > 0.0f)
-					{
-						cloud = rtabmap::util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());
-					}
-					if(cloud_voxel_size_->getFloat() > 0.0f)
-					{
-						cloud = rtabmap::util3d::voxelize(cloud, cloud_voxel_size_->getFloat());
-					}
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+							cloud_decimation_->getInt(),
+							cloud_max_depth_->getFloat(),
+							cloud_voxel_size_->getFloat());

-					cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
-
-					// do it after local transform
-					if(cloud_filter_floor_height_->getFloat() > 0.0f)
+					if(cloud->size())
 					{
-						cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
-					}
+						if(cloud_filter_floor_height_->getFloat() > 0.0f)
+						{
+							cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
+						}

-					sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
-					pcl::toROSMsg(*cloud, *cloudMsg);
-					cloudMsg->header = map.header;
+						sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+						pcl::toROSMsg(*cloud, *cloudMsg);
+						cloudMsg->header = map.header;

-					CloudInfoPtr info(new CloudInfo);
-					info->message_ = cloudMsg;
-					info->pose_ = rtabmap::Transform::getIdentity();
-					info->id_ = id;
+						CloudInfoPtr info(new CloudInfo);
+						info->message_ = cloudMsg;
+						info->pose_ = rtabmap::Transform::getIdentity();
+						info->id_ = id;

-					if (transformCloud(info, true))
-					{
-						boost::mutex::scoped_lock lock(new_clouds_mutex_);
-						new_cloud_infos_.insert(std::make_pair(id, info));
+						if (transformCloud(info, true))
+						{
+							boost::mutex::scoped_lock lock(new_clouds_mutex_);
+							new_cloud_infos_.insert(std::make_pair(id, info));
+						}
 					}
 				}
 			}
@@ -320,9 +299,9 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)

 	// Update graph
 	std::map<int, rtabmap::Transform> poses;
-	for(unsigned int i=0; i<map.graph.nodeIds.size() && i<map.graph.poses.size(); ++i)
+	for(unsigned int i=0; i<map.posesId.size() && i<map.poses.size(); ++i)
 	{
-		poses.insert(std::make_pair(map.graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
+		poses.insert(std::make_pair(map.posesId[i], rtabmap_ros::transformFromPoseMsg(map.poses[i])));
 	}

 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -503,12 +482,12 @@ void MapCloudDisplay::downloadMap()
 		else
 		{
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
 			QApplication::processEvents();
 			this->reset();
 			processMapData(getMapSrv.response.data);
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));

 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}
@@ -560,10 +539,10 @@ void MapCloudDisplay::downloadGraph()
 		}
 		else
 		{
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
 			QApplication::processEvents();
 			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));

 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}
@@ -95,21 +95,17 @@ void MapGraphDisplay::destroyObjects()

 void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
 {
-	if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size()))
+	if(!(msg->poses.size() == msg->posesId.size()))
 	{
-		ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size.");
+		ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
 		return;
 	}

 	// Get links
 	std::map<int, rtabmap::Transform> poses;
-	std::map<int, int> mapIds;
-	std::map<int,  double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
 	std::multimap<int, rtabmap::Link> links;
 	rtabmap::Transform mapToOdom;
-	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom);
+	rtabmap_ros::mapDataFromROS(*msg, poses, links, mapToOdom);

 	destroyObjects();