Added multi-cameras demo: demo_two_kinects.launch

launch/demo/demo_two_kinects.launch

@@ -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>

src/CoreWrapper.cpp

@@ -45,7 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UMath.h>
 
-#include <rtabmap/core/util3d_conversions.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/OdometryEvent.h>
@@ -83,12 +83,15 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		databasePath_(UDirectory::homeDir()+"/.ros/"+rtabmap::Parameters::getDefaultDatabaseName()),
 		waitForTransform_(false),
 		useActionForGoal_(false),
+		genScan_(false),
+		genScanMaxDepth_(4.0),
 		mapToOdom_(rtabmap::Transform::getIdentity()),
 		depthSync_(0),
 		depthScanSync_(0),
 		stereoScanSync_(0),
 		stereoApproxSync_(0),
 		stereoExactSync_(0),
+		depth2Sync_(0),
 		depthTFSync_(0),
 		depthScanTFSync_(0),
 		stereoScanTFSync_(0),
@@ -105,15 +108,16 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	bool subscribeLaserScan = false;
 	bool subscribeDepth = true;
 	bool subscribeStereo = false;
+	int depthCameras = 1;
 	int queueSize = 10;
 	bool publishTf = true;
 	double tfDelay = 0.05; // 20 Hz
 	bool stereoApproxSync = false;
 
 	// ROS related parameters (private)
-	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+	pnh.param("subscribe_depth",     subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
-	pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
+	pnh.param("subscribe_stereo",    subscribeStereo, subscribeStereo);
 	if(subscribeDepth && subscribeStereo)
 	{
 		UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
@@ -128,19 +132,27 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		}
 	}
 
-	pnh.param("config_path", configPath_, configPath_);
+	pnh.param("config_path",         configPath_, configPath_);
-	pnh.param("database_path", databasePath_, databasePath_);
+	pnh.param("database_path",       databasePath_, databasePath_);
 
-	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("frame_id",            frameId_, frameId_);
-	pnh.param("map_frame_id", mapFrameId_, mapFrameId_);
+	pnh.param("map_frame_id",        mapFrameId_, mapFrameId_);
-	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
+	pnh.param("odom_frame_id",       odomFrameId_, odomFrameId_); // set to use odom from TF
-	pnh.param("queue_size", queueSize, queueSize);
+	pnh.param("depth_cameras",       depthCameras, depthCameras);
-	pnh.param("stereo_approx_sync", stereoApproxSync, stereoApproxSync);
+	pnh.param("queue_size",          queueSize, queueSize);
+	pnh.param("stereo_approx_sync",  stereoApproxSync, stereoApproxSync);
 
-	pnh.param("publish_tf", publishTf, publishTf);
+	pnh.param("publish_tf",          publishTf, publishTf);
-	pnh.param("tf_delay", tfDelay, tfDelay);
+	pnh.param("tf_delay",            tfDelay, tfDelay);
-	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
+	pnh.param("wait_for_transform",  waitForTransform_, waitForTransform_);
 	pnh.param("use_action_for_goal", useActionForGoal_, useActionForGoal_);
+	pnh.param("gen_scan",            genScan_, genScan_);
+	pnh.param("gen_scan_max_depth",  genScanMaxDepth_, genScanMaxDepth_);
+
+	if(depthCameras <= 0 && subscribeDepth)
+	{
+		depthCameras = 1;
+	}
 
 	ROS_INFO("rtabmap: frame_id = %s", frameId_.c_str());
 	if(!odomFrameId_.empty())
@@ -150,6 +162,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	ROS_INFO("rtabmap: map_frame_id = %s", mapFrameId_.c_str());
 	ROS_INFO("rtabmap: queue_size = %d", queueSize);
 	ROS_INFO("rtabmap: tf_delay = %f", tfDelay);
+	ROS_INFO("rtabmap: depth_cameras = %d", depthCameras);
 
 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
@@ -352,7 +365,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	octomapFullSrv_ = nh.advertiseService("octomap_full", &CoreWrapper::octomapFullCallback, this);
 #endif
 
-	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync);
+	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize, stereoApproxSync, depthCameras);
 
 	int optimizeIterations = 0;
 	Parameters::parse(parameters_, Parameters::kRGBDOptimizeIterations(), optimizeIterations);
@@ -385,6 +398,8 @@ CoreWrapper::~CoreWrapper()
 		delete stereoApproxSync_;
 	if(stereoExactSync_)
 		delete stereoExactSync_;
+	if(depth2Sync_)
+		delete depth2Sync_;
 	if(depthTFSync_)
 		delete depthTFSync_;
 	if(depthScanTFSync_)
@@ -396,6 +411,22 @@ CoreWrapper::~CoreWrapper()
 	if(stereoExactTFSync_)
 		delete stereoExactTFSync_;
 
+	for(unsigned int i=0; i<imageSubs_.size(); ++i)
+	{
+		delete imageSubs_[i];
+	}
+	imageSubs_.clear();
+	for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
+	{
+		delete imageDepthSubs_[i];
+	}
+	imageDepthSubs_.clear();
+	for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
+	{
+		delete cameraInfoSubs_[i];
+	}
+	cameraInfoSubs_.clear();
+
 	this->saveParameters(configPath_);
 
 	ros::NodeHandle nh;
@@ -667,17 +698,24 @@ void CoreWrapper::commonDepthCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg)
 {
-	if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
-			imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
-			imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+	std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
-			imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+	imageMsgs.push_back(imageMsg);
-		!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+	depthMsgs.push_back(depthMsg);
-		 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+	cameraInfoMsgs.push_back(cameraInfoMsg);
-		 depthMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
+	commonDepthCallback(odomFrameId, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
-	{
+}
-		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
+void CoreWrapper::commonDepthCallback(
-		return;
+		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)
+{
+	UASSERT(imageMsgs.size()>0 &&
+			imageMsgs.size() == depthMsgs.size() &&
+			imageMsgs.size() == cameraInfoMsgs.size());
 
 	//for sync transform
 	Transform odomT = getTransform(odomFrameId, frameId_, lastPoseStamp_);
@@ -687,22 +725,121 @@ void CoreWrapper::commonDepthCallback(
 				odomFrameId.c_str(), frameId_.c_str());
 	}
 
-	Transform localTransform = getTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp);
+	int imageWidth = imageMsgs[0]->width;
-	if(localTransform.isNull())
+	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)
 	{
-		return;
+		if(!(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-	}
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-	// sync with odometry stamp
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-	if(lastPoseStamp_ != depthMsg->header.stamp)
+			 imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-	{
+			!(depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
-		if(!odomT.isNull())
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
+			 depthMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0))
 		{
-			Transform sensorT = getTransform(odomFrameId, frameId_, depthMsg->header.stamp);
+			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1,mono16");
-			if(sensorT.isNull())
+			return;
+		}
+		UASSERT(imageMsgs[i]->width == imageWidth && imageMsgs[i]->height == imageHeight);
+		UASSERT(depthMsgs[i]->width == imageWidth && depthMsgs[i]->height == imageHeight);
+
+		Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
+		if(localTransform.isNull())
+		{
+			return;
+		}
+		// sync with odometry stamp
+		if(lastPoseStamp_ != depthMsgs[i]->header.stamp)
+		{
+			if(!odomT.isNull())
 			{
-				return;
+				Transform sensorT = getTransform(odomFrameId, frameId_, depthMsgs[i]->header.stamp);
+				if(sensorT.isNull())
+				{
+					return;
+				}
+				localTransform = odomT.inverse() * sensorT * localTransform;
 			}
-			localTransform = odomT.inverse() * sensorT * localTransform;
+		}
+
+		cv_bridge::CvImageConstPtr ptrImage;
+		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 = 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(*cameraInfoMsgs[i]);
+		cameraModels.push_back(rtabmap::CameraModel(
+				model.fx(),
+				model.fy(),
+				model.cx(),
+				model.cy(),
+				localTransform));
+
+		if(scanMsg.get() == 0 && genScan_)
+		{
+			scanCloud += util3d::laserScanFromDepthImage(
+					subDepth,
+					model.fx(),
+					model.fy(),
+					model.cx(),
+					model.cy(),
+					genScanMaxDepth_,
+					localTransform);
 		}
 	}
 
@@ -746,42 +883,25 @@ void CoreWrapper::commonDepthCallback(
 		}
 		scan = util3d::laserScanFromPointCloud(*pclScan);
 	}
-
+	else if(scanCloud.size())
-	cv_bridge::CvImageConstPtr ptrImage;
-	if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-	   imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 	{
-		ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
+		scan = util3d::laserScanFromPointCloud(scanCloud);
 	}
-	else
-	{
-		ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-	}
-	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-	image_geometry::PinholeCameraModel model;
+	ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:depthMsgs[0]->header.stamp;
-	model.fromCameraInfo(*cameraInfoMsg);
-	double fx = model.fx();
-	double fy = model.fy();
-	double cx = model.cx();
-	double cy = model.cy();
 
-	process(ptrImage->header.seq,
+	process(stamp,
-			scanMsg.get() != 0?scanMsg->header.stamp:ptrDepth->header.stamp,
+			SensorData(scan,
-			ptrImage->image,
+					scanMsg.get() != 0?(int)scanMsg->ranges.size():0,
+					rgb,
+					depth,
+					cameraModels,
+					imageMsgs[0]->header.seq,
+					rtabmap_ros::timestampFromROS(stamp)),
 			lastPose_,
 			odomFrameId,
 			rotVariance_>0?rotVariance_:1.0,
-			transVariance_>0?transVariance_:1.0,
+			transVariance_>0?transVariance_:1.0);
-			ptrDepth->image,
-			fx,
-			fy,
-			cx,
-			cy,
-			0,
-			localTransform,
-			scan,
-			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
 	rotVariance_ = 0;
 	transVariance_ = 0;
 }
@@ -891,29 +1011,28 @@ void CoreWrapper::commonStereoCallback(
 
 	image_geometry::StereoCameraModel model;
 	model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
+	rtabmap::StereoCameraModel stereoModel(
+			model.left().fx(),
+			model.left().fy(),
+			model.left().cx(),
+			model.left().cy(),
+			model.baseline(),
+			localTransform);
 
-	double fx = model.left().fx();
+	ros::Time stamp = scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp;
-	double fy = model.left().fy();
+	process(stamp,
-	double cx = model.left().cx();
+			SensorData(scan,
-	double cy = model.left().cy();
+					scanMsg.get() != 0?(int)scanMsg->ranges.size():0,
-	double baseline = model.baseline();
+					ptrLeftImage->image,
-
+					ptrRightImage->image,
-	process(leftImageMsg->header.seq,
+					stereoModel,
-			scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp,
+					leftImageMsg->header.seq,
-			ptrLeftImage->image,
+					rtabmap_ros::timestampFromROS(stamp)),
 			lastPose_,
 			odomFrameId,
 			rotVariance_>0?rotVariance_:1.0,
-			transVariance_>0?transVariance_:1.0,
+			transVariance_>0?transVariance_:1.0);
-			ptrRightImage->image,
+
-			fx,
-			fy,
-			cx,
-			cy,
-			baseline,
-			localTransform,
-			scan,
-			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
 	rotVariance_ = 0;
 	transVariance_ = 0;
 }
@@ -975,6 +1094,34 @@ void CoreWrapper::stereoScanCallback(
 	commonStereoCallback(odomMsg->header.frame_id, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scanMsg);
 }
 
+void CoreWrapper::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)
+{
+	if(!commonOdomUpdate(odomMsg))
+	{
+		return;
+	}
+
+	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
+	imageMsgs.push_back(image1Msg);
+	imageMsgs.push_back(image2Msg);
+	depthMsgs.push_back(depth1Msg);
+	depthMsgs.push_back(depth2Msg);
+	cameraInfoMsgs.push_back(cameraInfo1Msg);
+	cameraInfoMsgs.push_back(cameraInfo2Msg);
+
+	sensor_msgs::LaserScanConstPtr scanMsg; // Null
+	commonDepthCallback(odomMsg->header.frame_id, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg);
+}
+
 
 void CoreWrapper::depthTFCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
@@ -1029,90 +1176,17 @@ void CoreWrapper::stereoScanTFCallback(
 }
 
 void CoreWrapper::process(
-		int id,
 		const ros::Time & stamp,
-		const cv::Mat & image,
+		const SensorData & data,
 		const Transform & odom,
 		const std::string & odomFrameId,
 		double odomRotationalVariance,
-		double odomTransitionalVariance,
+		double odomTransitionalVariance)
-		const cv::Mat & depthOrRightImage,
-		double fx,
-		double fy,
-		double cx,
-		double cy,
-		double baseline,
-		const Transform & localTransform,
-		const cv::Mat & scan,
-		int scanMaxPts)
 {
 	UTimer timer;
-	if(rtabmap_.isIDsGenerated() || id > 0)
+	if(rtabmap_.isIDsGenerated() || data.id() > 0)
 	{
 		double timeRtabmap = 0.0;
-		cv::Mat imageB;
-		if(!depthOrRightImage.empty())
-		{
-			if(depthOrRightImage.type() == CV_8UC1)
-			{
-				//right image
-				imageB = depthOrRightImage.clone();
-			}
-			else if(depthOrRightImage.type() != CV_16UC1)
-			{
-				// depth float
-				if(depthOrRightImage.type() == CV_32FC1)
-				{
-					//convert to 16 bits
-					imageB = util3d::cvtDepthFromFloat(depthOrRightImage);
-					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;
-					}
-				}
-				else
-				{
-					ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-					return;
-				}
-			}
-			else
-			{
-				// depth short
-				imageB = depthOrRightImage.clone();
-			}
-		}
-
-		SensorData data;
-		if(baseline > 0)
-		{
-			//stereo
-			data = SensorData(
-					scan,
-					scanMaxPts,
-					image.clone(),
-					imageB,
-					StereoCameraModel(fx, fy, cx, cy, baseline, localTransform),
-					id,
-					rtabmap_ros::timestampFromROS(stamp));
-		}
-		else
-		{
-			//depth
-			data = SensorData(
-					scan,
-					scanMaxPts,
-					image.clone(),
-					imageB,
-					CameraModel(fx, fy, cx, cy, localTransform),
-					id,
-					rtabmap_ros::timestampFromROS(stamp));
-		}
-
-
 		if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
 		{
 			timeRtabmap = timer.ticks();
@@ -1482,12 +1556,6 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				req.global);
 	}
 
-	if(poses.size() && poses.size() != signatures.size())
-	{
-		ROS_ERROR("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
-		return false;
-	}
-
 	//RGB-D SLAM data
 	rtabmap_ros::mapDataToROS(poses,
 		constraints,
@@ -2109,25 +2177,45 @@ void CoreWrapper::setupCallbacks(
 		bool subscribeLaserScan,
 		bool subscribeStereo,
 		int queueSize,
-		bool stereoApproxSync)
+		bool stereoApproxSync,
+		int depthCameras)
 {
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
 
 	if(subscribeDepth)
 	{
-		ros::NodeHandle rgb_nh(nh, "rgb");
+		UASSERT(depthCameras >= 1 && depthCameras <= 2);
-		ros::NodeHandle depth_nh(nh, "depth");
+		UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
-		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);
 
-		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageSubs_.resize(depthCameras);
-		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		imageDepthSubs_.resize(depthCameras);
-		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		cameraInfoSubs_.resize(depthCameras);
+		for(int i=0; i<depthCameras; ++i)
+		{
+			std::string rgbPrefix = "rgb";
+			std::string depthPrefix = "depth";
+			if(depthCameras>1)
+			{
+				rgbPrefix += uNumber2Str(i);
+				depthPrefix += uNumber2Str(i);
+			}
+			ros::NodeHandle rgb_nh(nh, rgbPrefix);
+			ros::NodeHandle depth_nh(nh, depthPrefix);
+			ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+			ros::NodeHandle depth_pnh(pnh, depthPrefix);
+			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);
+
+			imageSubs_[i] = new image_transport::SubscriberFilter;
+			imageDepthSubs_[i] = new image_transport::SubscriberFilter;
+			cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
+			imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
+		}
 
 		if(odomFrameId_.empty())
 		{
@@ -2136,29 +2224,67 @@ void CoreWrapper::setupCallbacks(
 			{
 				ROS_INFO("Registering Depth+LaserScan callback...");
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
+						MyDepthScanSyncPolicy(queueSize),
+						*imageSubs_[0],
+						odomSub_,
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0],
+						scanSub_);
 				depthScanSync_->registerCallback(boost::bind(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
 			else //!subscribeLaserScan
 			{
-				ROS_INFO("Registering Depth callback...");
+				if(depthCameras > 1)
-				depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+				{
-				depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+					ROS_INFO("Registering Depth2 callback...");
+					depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
+							MyDepth2SyncPolicy(queueSize),
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							*imageSubs_[1],
+							*imageDepthSubs_[1],
+							*cameraInfoSubs_[1]);
+					depth2Sync_->registerCallback(boost::bind(&CoreWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
-						odomSub_.getTopic().c_str());
+							imageSubs_[1]->getTopic().c_str(),
+							imageDepthSubs_[1]->getTopic().c_str(),
+							cameraInfoSubs_[1]->getTopic().c_str(),
+							odomSub_.getTopic().c_str());
+				}
+				else
+				{
+					ROS_INFO("Registering Depth callback...");
+					depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
+							MyDepthSyncPolicy(queueSize),
+							*imageSubs_[0],
+							odomSub_,
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+							ros::this_node::getName().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
+							odomSub_.getTopic().c_str());
+				}
 			}
 		}
 		else
@@ -2167,26 +2293,35 @@ void CoreWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(MyDepthScanTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
+						MyDepthScanTFSyncPolicy(queueSize),
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0],
+						scanSub_);
 				depthScanTFSync_->registerCallback(boost::bind(&CoreWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
 			else //!subscribeLaserScan
 			{
-				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(MyDepthTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
+						MyDepthTFSyncPolicy(queueSize),
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
 				depthTFSync_->registerCallback(boost::bind(&CoreWrapper::depthTFCallback, this, _1, _2, _3));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str());
+						cameraInfoSubs_[0]->getTopic().c_str());
 			}
 		}
 	}
@@ -2212,7 +2347,14 @@ void CoreWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_, odomSub_);
+				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
+						MyStereoScanSyncPolicy(queueSize),
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_,
+						scanSub_,
+						odomSub_);
 				stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -2229,13 +2371,25 @@ void CoreWrapper::setupCallbacks(
 				if(stereoApproxSync)
 				{
 					ROS_INFO("Registering Stereo Approx callback...");
-					stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(MyStereoApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
+					stereoApproxSync_ = new message_filters::Synchronizer<MyStereoApproxSyncPolicy>(
+							MyStereoApproxSyncPolicy(queueSize),
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_,
+							odomSub_);
 					stereoApproxSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
 				}
 				else
 				{
 					ROS_INFO("Registering Stereo Exact callback...");
-					stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(MyStereoExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
+					stereoExactSync_ = new message_filters::Synchronizer<MyStereoExactSyncPolicy>(
+							MyStereoExactSyncPolicy(queueSize),
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_,
+							odomSub_);
 					stereoExactSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
 				}
 
@@ -2255,7 +2409,13 @@ void CoreWrapper::setupCallbacks(
 			{
 				ROS_INFO("Registering Stereo+LaserScan+OdomTF callback...");
 				scanSub_.subscribe(nh, "scan", 1);
-				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(MyStereoScanTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_);
+				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
+						MyStereoScanTFSyncPolicy(queueSize),
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_,
+						scanSub_);
 				stereoScanTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -2271,13 +2431,23 @@ void CoreWrapper::setupCallbacks(
 				if(stereoApproxSync)
 				{
 					ROS_INFO("Registering Stereo+OdomTF Approx callback...");
-					stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(MyStereoApproxTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+					stereoApproxTFSync_ = new message_filters::Synchronizer<MyStereoApproxTFSyncPolicy>(
+							MyStereoApproxTFSyncPolicy(queueSize),
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_);
 					stereoApproxTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
 				}
 				else
 				{
 					ROS_INFO("Registering Stereo+OdomTF Exact callback...");
-					stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(MyStereoExactTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+					stereoExactTFSync_ = new message_filters::Synchronizer<MyStereoExactTFSyncPolicy>(
+							MyStereoExactTFSyncPolicy(queueSize),
+							imageRectLeft_,
+							imageRectRight_,
+							cameraInfoLeft_,
+							cameraInfoRight_);
 					stereoExactTFSync_->registerCallback(boost::bind(&CoreWrapper::stereoTFCallback, this, _1, _2, _3, _4));
 				}
 

src/CoreWrapper.h

@@ -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::ImageConstPtr& depthMsg,
-				const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
+				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(
@@ -158,22 +178,12 @@ private:
 	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 = "",
 			double odomRotationalVariance = 1.0,
-			double odomTransitionalVariance = 1.0,
+			double odomTransitionalVariance = 1.0);
-			const cv::Mat & depthOrRightImage = cv::Mat(),
-			double fx = 0.0,
-			double fy = 0.0,
-			double cx = 0.0,
-			double cy = 0.0,
-			double baseline = 0.0,
-			const rtabmap::Transform & localTransform = rtabmap::Transform(),
-			const cv::Mat & scan = cv::Mat(),
-			int scanMaxPts = 0);
 
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -225,6 +235,8 @@ private:
 	std::string databasePath_;
 	bool waitForTransform_;
 	bool useActionForGoal_;
+	bool genScan_;
+	double genScanMaxDepth_;
 
 	rtabmap::Transform mapToOdom_;
 	boost::mutex mapToOdomMutex_;
@@ -247,9 +259,9 @@ private:
 	image_transport::Subscriber defaultSub_;
 
 	//for depth callback
-	image_transport::SubscriberFilter imageSub_;
+	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	image_transport::SubscriberFilter imageDepthSub_;
+	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	std::vector<message_filters::Subscriber<sensor_msgs::CameraInfo>*> cameraInfoSubs_;
 
 	//stereo callback
 	image_transport::SubscriberFilter imageRectLeft_;
@@ -300,6 +312,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,

src/DbPlayerNode.cpp

@@ -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;

src/GuiWrapper.cpp

@@ -47,7 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/ParamEvent.h>
 #include <rtabmap/core/OdometryEvent.h>
-#include <rtabmap/core/util3d_conversions.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/UTimer.h>
 
@@ -78,7 +78,9 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		depthOdomInfoSync_(0),
 		stereoSync_(0),
 		stereoScanSync_(0),
-		stereoOdomInfoSync_(0)
+		stereoOdomInfoSync_(0),
+		depth2Sync_(0),
+		depthOdomInfo2Sync_(0)
 {
 	ros::NodeHandle nh;
 	app_ = new QApplication(argc, argv);
@@ -117,52 +119,72 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	bool subscribeOdomInfo = false;
 	bool subscribeStereo = false;
 	int queueSize = 10;
+	int depthCameras = 1;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
 	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 	pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
+	pnh.param("depth_cameras", depthCameras, depthCameras);
 	pnh.param("queue_size", queueSize, queueSize);
 	pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
 	pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
-	this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, subscribeStereo, queueSize);
+	this->setupCallbacks(
+			subscribeDepth,
+			subscribeLaserScan,
+			subscribeOdomInfo,
+			subscribeStereo,
+			queueSize,
+			depthCameras);
 
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
 
 	infoTopic_.subscribe(nh, "info", 1);
 	mapDataTopic_.subscribe(nh, "mapData", 1);
-	infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(MyInfoMapSyncPolicy(queueSize), infoTopic_, mapDataTopic_);
+	infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
+			MyInfoMapSyncPolicy(queueSize),
+			infoTopic_,
+			mapDataTopic_);
 	infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, _1, _2));
 }
 
 GuiWrapper::~GuiWrapper()
 {
 	if(depthSync_)
-	{
 		delete depthSync_;
-	}
+	if(depth2Sync_)
+		delete depth2Sync_;
 	if(depthScanSync_)
-	{
 		delete depthScanSync_;
-	}
 	if(depthOdomInfoSync_)
-	{
 		delete depthOdomInfoSync_;
-	}
+	if(depthOdomInfo2Sync_)
+		delete depthOdomInfo2Sync_;
 	if(stereoSync_)
-	{
 		delete stereoSync_;
-	}
 	if(stereoScanSync_)
-	{
 		delete stereoScanSync_;
-	}
 	if(stereoOdomInfoSync_)
-	{
 		delete stereoOdomInfoSync_;
+
+	for(unsigned int i=0; i<imageSubs_.size(); ++i)
+	{
+		delete imageSubs_[i];
 	}
+	imageSubs_.clear();
+	for(unsigned int i=0; i<imageDepthSubs_.size(); ++i)
+	{
+		delete imageDepthSubs_[i];
+	}
+	imageDepthSubs_.clear();
+	for(unsigned int i=0; i<cameraInfoSubs_.size(); ++i)
+	{
+		delete cameraInfoSubs_[i];
+	}
+	cameraInfoSubs_.clear();
+
 	delete infoMapSync_;
 	delete mainWindow_;
 	delete app_;
@@ -373,6 +395,23 @@ void GuiWrapper::commonDepthCallback(
 		const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
+{
+	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
+	imageMsgs.push_back(imageMsg);
+	depthMsgs.push_back(depthMsg);
+	cameraInfoMsgs.push_back(cameraInfoMsg);
+	commonDepthCallback(odomMsg, imageMsgs, depthMsgs, cameraInfoMsgs, scanMsg, odomInfoMsg);
+}
+
+void GuiWrapper::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)
 {
 	if(UTimer::now() - lastOdomInfoUpdateTime_ > 0.1 &&
 	   !mainWindow_->isProcessingOdometry() &&
@@ -380,19 +419,9 @@ void GuiWrapper::commonDepthCallback(
 	{
 		lastOdomInfoUpdateTime_ = UTimer::now();
 
-		if(!(imageMsg.get() == 0 ||
+		UASSERT(imageMsgs.size()>0 &&
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				imageMsgs.size() == depthMsgs.size() &&
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				imageMsgs.size() == cameraInfoMsgs.size());
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-			!(depthMsg.get() == 0 ||
-				 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
-				 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0 ||
-				 depthMsg->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;
-		}
 
 		std_msgs::Header odomHeader;
 		if(odomMsg.get())
@@ -405,17 +434,17 @@ void GuiWrapper::commonDepthCallback(
 			{
 				odomHeader = scanMsg->header;
 			}
-			else if(cameraInfoMsg.get())
+			else if(cameraInfoMsgs.size() && cameraInfoMsgs[0].get())
 			{
-				odomHeader = cameraInfoMsg->header;
+				odomHeader = cameraInfoMsgs[0]->header;
 			}
-			else if(depthMsg.get())
+			else if(depthMsgs.size() && depthMsgs[0].get())
 			{
-				odomHeader = depthMsg->header;
+				odomHeader = depthMsgs[0]->header;
 			}
-			else if(imageMsg.get())
+			else if(imageMsgs.size() && imageMsgs[0].get())
 			{
-				odomHeader = imageMsg->header;
+				odomHeader = imageMsgs[0]->header;
 			}
 			odomHeader.frame_id = odomFrameId_;
 		}
@@ -447,51 +476,110 @@ void GuiWrapper::commonDepthCallback(
 			return;
 		}
 
-		CameraModel cameraModel;
+		int imageWidth = imageMsgs[0]->width;
-		if(cameraInfoMsg.get())
+		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)
 		{
-			Transform localTransform = getTransform(frameId_, cameraInfoMsg->header.frame_id, cameraInfoMsg->header.stamp);
+			if(!(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);
+
+			Transform localTransform = getTransform(frameId_, depthMsgs[i]->header.frame_id, depthMsgs[i]->header.stamp);
 			if(localTransform.isNull())
 			{
 				return;
 			}
 			// sync with odometry stamp
-			if(odomHeader.stamp != cameraInfoMsg->header.stamp)
+			if(odomHeader.stamp != depthMsgs[i]->header.stamp)
 			{
-				Transform sensorT = getTransform(odomHeader.frame_id, frameId_, cameraInfoMsg->header.stamp);
+				if(!odomT.isNull())
-				if(sensorT.isNull())
 				{
-					return;
+					Transform sensorT = getTransform(odomHeader.frame_id, frameId_, depthMsgs[i]->header.stamp);
+					if(sensorT.isNull())
+					{
+						return;
+					}
+					localTransform = odomT.inverse() * sensorT * localTransform;
 				}
-				localTransform = odomT.inverse() * sensorT * localTransform;
 			}
 
-			image_geometry::PinholeCameraModel model;
+			cv_bridge::CvImageConstPtr ptrImage;
-			model.fromCameraInfo(*cameraInfoMsg);
+			if(imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-			if(!localTransform.isNull())
+			   imageMsgs[i]->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
 			{
-				cameraModel = CameraModel(model.fx(), model.fy(), model.cx(), model.cy(), localTransform);
+				ptrImage = cv_bridge::toCvShare(imageMsgs[i], "mono8");
-			}
-		}
-
-		cv::Mat rgb;
-		if(imageMsg.get())
-		{
-			if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-			   imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-			{
-				rgb = cv_bridge::toCvCopy(imageMsg, "mono8")->image;
 			}
 			else
 			{
-				rgb = cv_bridge::toCvCopy(imageMsg, "bgr8")->image;
+				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 = 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;
+				}
 			}
-		}
 
-		cv::Mat depth;
+			// initialize
-		if(depthMsg.get())
+			if(rgb.empty())
-		{
+			{
-			depth = cv_bridge::toCvCopy(depthMsg)->image;
+				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(*cameraInfoMsgs[i]);
+			cameraModels.push_back(rtabmap::CameraModel(
+					model.fx(),
+					model.fy(),
+					model.cx(),
+					model.cy(),
+					localTransform));
 		}
 
 		cv::Mat scan;
@@ -540,7 +628,7 @@ void GuiWrapper::commonDepthCallback(
 					scanMsg.get()?(int)scanMsg->ranges.size():0,
 					rgb,
 					depth,
-					cameraModel,
+					cameraModels,
 					odomHeader.seq,
 					rtabmap_ros::timestampFromROS(odomHeader.stamp)),
 			odomT,
@@ -754,6 +842,34 @@ void GuiWrapper::depthCallback(
 			rtabmap_ros::OdomInfoConstPtr());
 }
 
+void GuiWrapper::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)
+{
+	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
+	imageMsgs.push_back(image1Msg);
+	imageMsgs.push_back(image2Msg);
+	depthMsgs.push_back(depth1Msg);
+	depthMsgs.push_back(depth2Msg);
+	cameraInfoMsgs.push_back(cameraInfo1Msg);
+	cameraInfoMsgs.push_back(cameraInfo2Msg);
+
+	commonDepthCallback(
+			odomMsg,
+			imageMsgs,
+			depthMsgs,
+			cameraInfoMsgs,
+			sensor_msgs::LaserScanConstPtr(),
+			rtabmap_ros::OdomInfoConstPtr());
+}
+
 void GuiWrapper::depthOdomInfoCallback(
 		const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -770,6 +886,35 @@ void GuiWrapper::depthOdomInfoCallback(
 			odomInfoMsg);
 }
 
+void GuiWrapper::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)
+{
+	std::vector<sensor_msgs::ImageConstPtr> imageMsgs;
+	std::vector<sensor_msgs::ImageConstPtr> depthMsgs;
+	std::vector<sensor_msgs::CameraInfoConstPtr> cameraInfoMsgs;
+	imageMsgs.push_back(image1Msg);
+	imageMsgs.push_back(image2Msg);
+	depthMsgs.push_back(depth1Msg);
+	depthMsgs.push_back(depth2Msg);
+	cameraInfoMsgs.push_back(cameraInfo1Msg);
+	cameraInfoMsgs.push_back(cameraInfo2Msg);
+
+	commonDepthCallback(
+			odomMsg,
+			imageMsgs,
+			depthMsgs,
+			cameraInfoMsgs,
+			sensor_msgs::LaserScanConstPtr(),
+			odomInfoMsg);
+}
+
 void GuiWrapper::depthScanCallback(
 		const sensor_msgs::LaserScanConstPtr& scanMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -939,7 +1084,8 @@ void GuiWrapper::setupCallbacks(
 		bool subscribeLaserScan,
 		bool subscribeOdomInfo,
 		bool subscribeStereo,
-		int queueSize)
+		int queueSize,
+		int depthCameras)
 {
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
@@ -952,21 +1098,49 @@ void GuiWrapper::setupCallbacks(
 	{
 		ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
 	}
+	if(depthCameras <= 0)
+	{
+		depthCameras = 1;
+	}
+	if(depthCameras > 2)
+	{
+		ROS_WARN("Cannot subscribe to more than 2 cameras yet...");
+		depthCameras = 2;
+	}
 
 	if(subscribeDepth)
 	{
-		ros::NodeHandle rgb_nh(nh, "rgb");
+		UASSERT(depthCameras >= 1 && depthCameras <= 2);
-		ros::NodeHandle depth_nh(nh, "depth");
+		UASSERT_MSG(depthCameras == 1 || !(subscribeLaserScan || !odomFrameId_.empty()), "Not yet supported!");
-		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);
 
-		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		imageSubs_.resize(depthCameras);
-		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		imageDepthSubs_.resize(depthCameras);
-		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+		cameraInfoSubs_.resize(depthCameras);
+		for(int i=0; i<depthCameras; ++i)
+		{
+			std::string rgbPrefix = "rgb";
+			std::string depthPrefix = "depth";
+			if(depthCameras>1)
+			{
+				rgbPrefix += uNumber2Str(i);
+				depthPrefix += uNumber2Str(i);
+			}
+			ros::NodeHandle rgb_nh(nh, rgbPrefix);
+			ros::NodeHandle depth_nh(nh, depthPrefix);
+			ros::NodeHandle rgb_pnh(pnh, rgbPrefix);
+			ros::NodeHandle depth_pnh(pnh, depthPrefix);
+			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);
+
+			imageSubs_[i] = new image_transport::SubscriberFilter;
+			imageDepthSubs_[i] = new image_transport::SubscriberFilter;
+			cameraInfoSubs_[i] = new message_filters::Subscriber<sensor_msgs::CameraInfo>;
+			imageSubs_[i]->subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+			imageDepthSubs_[i]->subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			cameraInfoSubs_[i]->subscribe(rgb_nh, "camera_info", 1);
+		}
 
 		if(odomFrameId_.empty())
 		{
@@ -974,42 +1148,113 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), scanSub_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(
+						MyDepthScanSyncPolicy(queueSize),
+						scanSub_,
+						odomSub_,
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
 				depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
 						odomSub_.getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
-				depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), odomInfoSub_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+				if(depthCameras > 1)
-				depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
+				{
+					depthOdomInfo2Sync_ = new message_filters::Synchronizer<MyDepthOdomInfo2SyncPolicy>(
+							MyDepthOdomInfo2SyncPolicy(queueSize),
+							odomInfoSub_,
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							*imageSubs_[1],
+							*imageDepthSubs_[1],
+							*cameraInfoSubs_[1]);
+					depthOdomInfo2Sync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfo2Callback, this, _1, _2, _3, _4, _5, _6, _7, _8));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
-						odomSub_.getTopic().c_str(),
+							imageSubs_[1]->getTopic().c_str(),
-						odomInfoSub_.getTopic().c_str());
+							imageDepthSubs_[1]->getTopic().c_str(),
+							cameraInfoSubs_[1]->getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
+				else
+				{
+					depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(
+							MyDepthOdomInfoSyncPolicy(queueSize),
+							odomInfoSub_,
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
+
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
+							ros::this_node::getName().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
+							odomSub_.getTopic().c_str(),
+							odomInfoSub_.getTopic().c_str());
+				}
 			}
 			else
 			{
-				depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+				if(depthCameras > 1)
-				depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+				{
+					depth2Sync_ = new message_filters::Synchronizer<MyDepth2SyncPolicy>(
+							MyDepth2SyncPolicy(queueSize),
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0],
+							*imageSubs_[1],
+							*imageDepthSubs_[1],
+							*cameraInfoSubs_[1]);
+					depth2Sync_->registerCallback(boost::bind(&GuiWrapper::depth2Callback, this, _1, _2, _3, _4, _5, _6, _7));
 
-				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s\n   %s,\n   %s,\n   %s",
-						ros::this_node::getName().c_str(),
+							ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
-						odomSub_.getTopic().c_str());
+							imageSubs_[1]->getTopic().c_str(),
+							imageDepthSubs_[1]->getTopic().c_str(),
+							cameraInfoSubs_[1]->getTopic().c_str(),
+							odomSub_.getTopic().c_str());
+				}
+				else
+				{
+					depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(
+							MyDepthSyncPolicy(queueSize),
+							odomSub_,
+							*imageSubs_[0],
+							*imageDepthSubs_[0],
+							*cameraInfoSubs_[0]);
+					depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+
+					ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
+							ros::this_node::getName().c_str(),
+							imageSubs_[0]->getTopic().c_str(),
+							imageDepthSubs_[0]->getTopic().c_str(),
+							cameraInfoSubs_[0]->getTopic().c_str(),
+							odomSub_.getTopic().c_str());
+				}
 			}
 		}
 		else
@@ -1018,39 +1263,53 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(MyDepthScanTFSyncPolicy(queueSize), scanSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthScanTFSync_ = new message_filters::Synchronizer<MyDepthScanTFSyncPolicy>(
+						MyDepthScanTFSyncPolicy(queueSize),
+						scanSub_,
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
 				depthScanTFSync_->registerCallback(boost::bind(&GuiWrapper::depthScanTFCallback, this, _1, _2, _3, _4));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
 						scanSub_.getTopic().c_str());
 			}
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
-				depthOdomInfoTFSync_ = new message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy>(MyDepthOdomInfoTFSyncPolicy(queueSize), odomInfoSub_, imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthOdomInfoTFSync_ = new message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy>(
+						MyDepthOdomInfoTFSyncPolicy(queueSize),
+						odomInfoSub_,
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
 				depthOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoTFCallback, this, _1, _2, _3, _4));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str(),
+						cameraInfoSubs_[0]->getTopic().c_str(),
 						odomInfoSub_.getTopic().c_str());
 			}
 			else
 			{
-				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(MyDepthTFSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthTFSync_ = new message_filters::Synchronizer<MyDepthTFSyncPolicy>(
+						MyDepthTFSyncPolicy(queueSize),
+						*imageSubs_[0],
+						*imageDepthSubs_[0],
+						*cameraInfoSubs_[0]);
 				depthTFSync_->registerCallback(boost::bind(&GuiWrapper::depthTFCallback, this, _1, _2, _3));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
 						ros::this_node::getName().c_str(),
-						imageSub_.getTopic().c_str(),
+						imageSubs_[0]->getTopic().c_str(),
-						imageDepthSub_.getTopic().c_str(),
+						imageDepthSubs_[0]->getTopic().c_str(),
-						cameraInfoSub_.getTopic().c_str());
+						cameraInfoSubs_[0]->getTopic().c_str());
 			}
 		}
 	}
@@ -1076,7 +1335,14 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), scanSub_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(
+						MyStereoScanSyncPolicy(queueSize),
+						scanSub_,
+						odomSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -1091,7 +1357,14 @@ void GuiWrapper::setupCallbacks(
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
-				stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(MyStereoOdomInfoSyncPolicy(queueSize), odomInfoSub_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(
+						MyStereoOdomInfoSyncPolicy(queueSize),
+						odomInfoSub_,
+						odomSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -1105,7 +1378,13 @@ void GuiWrapper::setupCallbacks(
 			}
 			else
 			{
-				stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(
+						MyStereoSyncPolicy(queueSize),
+						odomSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoSync_->registerCallback(boost::bind(&GuiWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -1123,7 +1402,13 @@ void GuiWrapper::setupCallbacks(
 			if(subscribeLaserScan)
 			{
 				scanSub_.subscribe(nh, "scan", 1);
-				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(MyStereoScanTFSyncPolicy(queueSize), scanSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoScanTFSync_ = new message_filters::Synchronizer<MyStereoScanTFSyncPolicy>(
+						MyStereoScanTFSyncPolicy(queueSize),
+						scanSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoScanTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanTFCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -1137,7 +1422,13 @@ void GuiWrapper::setupCallbacks(
 			else if(subscribeOdomInfo)
 			{
 				odomInfoSub_.subscribe(nh, "odom_info", 1);
-				stereoOdomInfoTFSync_ = new message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy>(MyStereoOdomInfoTFSyncPolicy(queueSize), odomInfoSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoOdomInfoTFSync_ = new message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy>(
+						MyStereoOdomInfoTFSyncPolicy(queueSize),
+						odomInfoSub_,
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoOdomInfoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoTFCallback, this, _1, _2, _3, _4, _5));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s,\n   %s",
@@ -1150,7 +1441,12 @@ void GuiWrapper::setupCallbacks(
 			}
 			else
 			{
-				stereoTFSync_ = new message_filters::Synchronizer<MyStereoTFSyncPolicy>(MyStereoTFSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+				stereoTFSync_ = new message_filters::Synchronizer<MyStereoTFSyncPolicy>(
+						MyStereoTFSyncPolicy(queueSize),
+						imageRectLeft_,
+						imageRectRight_,
+						cameraInfoLeft_,
+						cameraInfoRight_);
 				stereoTFSync_->registerCallback(boost::bind(&GuiWrapper::stereoTFCallback, this, _1, _2, _3, _4));
 
 				ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s,\n   %s",

src/GuiWrapper.h

@@ -73,7 +73,13 @@ 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 setupCallbacks(
+			bool subscribeDepth,
+			bool subscribeLaserScan,
+			bool subscribeOdomInfo,
+			bool subscribeStereo,
+			int queueSize,
+			int depthCameras);
 
 	void commonDepthCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
@@ -82,6 +88,13 @@ private:
 			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,
@@ -99,12 +112,29 @@ private:
 			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 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,
@@ -185,9 +215,9 @@ private:
 	message_filters::Subscriber<rtabmap_ros::MapData> mapDataTopic_;
 
 	ros::Subscriber defaultSub_; // odometry only
-	image_transport::SubscriberFilter imageSub_;
+	std::vector<image_transport::SubscriberFilter*> imageSubs_;
-	image_transport::SubscriberFilter imageDepthSub_;
+	std::vector<image_transport::SubscriberFilter*> imageDepthSubs_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	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_;
@@ -252,6 +282,27 @@ private:
 			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,

src/MapAssemblerNode.cpp

@@ -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>

src/RGBDOdometryNode.cpp

@@ -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");
+		if(depthCameras == 2)
-		ros::NodeHandle depth_nh(nh, "depth");
+		{
-		ros::NodeHandle rgb_pnh(pnh, "rgb");
+			ros::NodeHandle rgb0_nh(nh, "rgb0");
-		ros::NodeHandle depth_pnh(pnh, "depth");
+			ros::NodeHandle depth0_nh(nh, "depth0");
-		image_transport::ImageTransport rgb_it(rgb_nh);
+			ros::NodeHandle rgb0_pnh(pnh, "rgb0");
-		image_transport::ImageTransport depth_it(depth_nh);
+			ros::NodeHandle depth0_pnh(pnh, "depth0");
-		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+			image_transport::ImageTransport rgb0_it(rgb0_nh);
-		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+			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_mono_sub_.subscribe(rgb0_it, rgb0_nh.resolveName("image"), 1, hintsRgb0);
-		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+			image_depth_sub_.subscribe(depth0_it, depth0_nh.resolveName("image"), 1, hintsDepth0);
-		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+			info_sub_.subscribe(rgb0_nh, "camera_info", 1);
 
-		ROS_INFO("\n%s subscribed to:\n   %s,\n   %s,\n   %s",
+			ros::NodeHandle rgb1_nh(nh, "rgb1");
-				ros::this_node::getName().c_str(),
+			ros::NodeHandle depth1_nh(nh, "depth1");
-				image_mono_sub_.getTopic().c_str(),
+			ros::NodeHandle rgb1_pnh(pnh, "rgb1");
-				image_depth_sub_.getTopic().c_str(),
+			ros::NodeHandle depth1_pnh(pnh, "depth1");
-				info_sub_.getTopic().c_str());
+			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_);
+			image_mono2_sub_.subscribe(rgb1_it, rgb1_nh.resolveName("image"), 1, hintsRgb1);
-		sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+			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)
 	{
@@ -159,12 +229,154 @@ public:
 		}
 	}
 
+	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);
+
+			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::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);
+
+				tf::StampedTransform localTransform;
+				try
+				{
+					if(this->waitForTransform())
+					{
+						if(!this->tfListener().waitForTransform(this->frameId(), imageMsgs[i]->header.frame_id, imageMsgs[i]->header.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, imageMsgs[i]->header.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::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(image->header.stamp));
+
+			this->processData(data, image->header);
+		}
+	}
+
 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[])