MsgConversion.h: added conversion of rtabmap_ros/Info messages from/to rtabmap::Statistics

rtabmapviz: added subscription to stereo.
Updated demo_stereo_outdoor.launch with arguments to choose between rtabmapviz and rviz
Added localPath array in rtabmap_ros/Info message
rtabmap: publishing the local path, uniformized time stamps between published topics at each iteration

include/rtabmap_ros/MsgConversion.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Signature.h>
 #include <rtabmap/core/OdometryInfo.h>
+#include <rtabmap/core/Statistics.h>
 
 #include <rtabmap_ros/Link.h>
 #include <rtabmap_ros/KeyPoint.h>
@@ -47,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/Graph.h>
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
+#include <rtabmap_ros/Info.h>
 
 namespace rtabmap_ros {
 
@@ -63,6 +65,9 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg);
 void compressedMatToBytes(const cv::Mat & compressed, std::vector<unsigned char> & bytes);
 cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool copy = true);
 
+void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat);
+void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info);
+
 rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg);
 void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg);
 

launch/demo/demo_stereo_outdoor.launch

@@ -14,6 +14,10 @@
       $ rosbag play -.-clock stereo_oudoorA.bag
    -->
    
+   <!-- Choose visualization -->
+   <arg name="rviz" default="true" />
+   <arg name="rtabmapviz" default="false" />
+    
    <param name="use_sim_time" type="bool" value="True"/>
    
    <!-- Just to uncompress images for stereo_image_rect -->
@@ -51,6 +55,7 @@
       <param name="OdomBow/NNDR" type="string" value="0.8"/>
       <param name="GFTT/MaxCorners" type="string" value="500"/>
       <param name="GFTT/MinDistance" type="string" value="5"/>
+      <param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
    </node>
      
    <group ns="rtabmap">   
@@ -94,15 +99,30 @@
       <!-- Optimizing outside rtabmap node makes it able to optimize always the global map -->
       <node pkg="rtabmap_ros" type="map_optimizer" name="map_optimizer"/>
       
-      <node pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
+      <node if="$(arg rviz)" pkg="rtabmap_ros" type="map_assembler" name="map_assembler">
          <param name="occupancy_grid" type="bool" value="true"/>
          <remap from="mapData" to="mapData_optimized"/>
          <remap from="grid_projection_map" to="/map"/>
       </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_stereo" type="bool" value="true"/>
+         <param name="subscribe_odom_info" type="bool" value="true"/>
+         <param name="queue_size" type="int" value="10"/>
+         <param name="frame_id" type="string" value="base_footprint"/>
+         <remap from="left/image_rect" to="/stereo_camera/left/image_rect_color"/>
+         <remap from="right/image_rect" to="/stereo_camera/right/image_rect"/>
+         <remap from="left/camera_info" to="/stereo_camera/left/camera_info_throttle"/>
+         <remap from="right/camera_info" to="/stereo_camera/right/camera_info_throttle"/>
+         <remap from="odom_info" to="/odom_info"/>
+         <remap from="odom" to="/odometry"/>
+         <remap from="mapData" to="mapData_optimized"/>
+      </node>
+         
    </group>
   
-   <!-- RVIZ --> 
-   <node pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>
+   <!-- Visualisation RVIZ --> 
+   <node if="$(arg rviz)" pkg="rviz" type="rviz" name="rviz" args="-d $(find rtabmap_ros)/launch/config/demo_stereo_outdoor.rviz"/>
 
 </launch>

msg/Info.msg

@@ -33,3 +33,6 @@ int32[] weightsValues
 # std::map<std::string, float> stats
 string[] statsKeys
 float32[] statsValues
+
+# std::vector<int> localPath
+int32[] localPath

src/CoreWrapper.cpp

@@ -134,10 +134,9 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	// planning topics
 	goalNodeSub_ = nh.subscribe("goal_node", 1, &CoreWrapper::goalNodeCallback, this);
 	nextMetricGoalPub_ = nh.advertise<geometry_msgs::PoseStamped>("goal_pose", 1);
-	nextMetricGoalIdPub_ = nh.advertise<std_msgs::Int32>("goal_pose_id", 1);
 	goalReachedPub_ = nh.advertise<std_msgs::Empty>("goal_reached", 1);
-	pathPub_ = nh.advertise<nav_msgs::Path>("path", 1);
-	pathIdsPub_ = nh.advertise<std_msgs::Int32MultiArray>("path_ids", 1);
+	globalPathPub_ = nh.advertise<nav_msgs::Path>("global_path", 1);
+	localPathPub_ = nh.advertise<nav_msgs::Path>("local_path", 1);
 
 	ros::Publisher nextMetricGoal_;
 	ros::Publisher goalReached_;
@@ -438,7 +437,7 @@ void CoreWrapper::defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 			else
 			{
 				const Statistics & stats = rtabmap_.getStatistics();
-				this->publishStats(stats);
+				this->publishStats(stats, ros::Time::now());
 			}
 		}
 		else if(!rtabmap_.isIDsGenerated())
@@ -929,9 +928,45 @@ void CoreWrapper::process(
 			mapToOdomMutex_.unlock();
 
 			const Statistics & stats = rtabmap_.getStatistics();
-			this->publishStats(stats);
+			ros::Time timeNow = ros::Time::now();
+			this->publishStats(stats, timeNow);
 
-			this->updateGoal();
+			// update goal if planning is enabled
+			if(!currentMetricGoal_.isNull())
+			{
+				if(rtabmap_.getPath().size() == 0)
+				{
+					// Goal reached
+					ROS_INFO("Planning: Publishing goal reached!");
+					if(goalReachedPub_.getNumSubscribers())
+					{
+						goalReachedPub_.publish(std_msgs::Empty());
+					}
+					currentMetricGoal_.setNull();
+				}
+				else
+				{
+					Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
+					if(!updatedGoalPose.isNull())
+					{
+						// detect if the goal has changed or local map
+						// has changed so much that current goal drifted
+						if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
+						{
+							currentMetricGoal_ = updatedGoalPose;
+
+							publishGoal(timeNow);
+						}
+
+						// publish local path
+						publishLocalPath(timeNow);
+					}
+					else
+					{
+						ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
+					}
+				}
+			}
 		}
 	}
 	else if(!rtabmap_.isIDsGenerated())
@@ -968,34 +1003,29 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
 		{
 			ROS_INFO("Planning: Path successfully created (size=%d)", (int)poses.size());
 			ros::Time now = ros::Time::now();
-			if(pathPub_.getNumSubscribers())
+
+			// Global path
+			if(globalPathPub_.getNumSubscribers())
 			{
 				nav_msgs::Path path;
 				path.header.frame_id = mapFrameId_;
 				path.header.stamp = now;
 				path.poses.resize(poses.size());
 				int oi = 0;
+				std::stringstream stream;
 				for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 				{
 					path.poses[oi].header = path.header;
 					rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
 					++oi;
+					stream << iter->first << " ";
 				}
-				pathPub_.publish(path);
-			}
-			if(pathIdsPub_.getNumSubscribers())
-			{
-				std_msgs::Int32MultiArray array;
-				array.data.resize(poses.size());
-				int oi = 0;
-				for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-				{
-					array.data[oi++] = iter->first;
-				}
-				pathIdsPub_.publish(array);
+				ROS_INFO("Publishing global path: [%s]", stream.str().c_str());
+				globalPathPub_.publish(path);
 			}
 
-			publishGoal();
+			publishGoal(now);
+			publishLocalPath(now);
 		}
 	}
 	else
@@ -1005,62 +1035,6 @@ void CoreWrapper::goalNodeCallback(const std_msgs::Int32ConstPtr & msg)
 	}
 }
 
-void CoreWrapper::updateGoal()
-{
-	if(!currentMetricGoal_.isNull())
-	{
-		if(rtabmap_.getPath().size() == 0)
-		{
-			// Goal reached
-			ROS_INFO("Planning: Publishing goal reached!");
-			if(goalReachedPub_.getNumSubscribers())
-			{
-				goalReachedPub_.publish(std_msgs::Empty());
-			}
-			currentMetricGoal_.setNull();
-		}
-		else
-		{
-			Transform updatedGoalPose = rtabmap_.getPose(rtabmap_.getPathGoalId());
-			if(!updatedGoalPose.isNull())
-			{
-				// detect if the goal has changed or local map
-				// has changed so much that current goal drifted
-				if(currentMetricGoal_.getDistance(updatedGoalPose) > rtabmap_.getGoalReachedRadius()/2.0f)
-				{
-					currentMetricGoal_ = updatedGoalPose;
-
-					publishGoal();
-				}
-			}
-			else
-			{
-				ROS_ERROR("Planning: Pose of node %d not found!? Cannot send a metric goal...", rtabmap_.getPathGoalId());
-			}
-		}
-	}
-}
-
-void CoreWrapper::publishGoal()
-{
-	ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
-			rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
-	if(nextMetricGoalPub_.getNumSubscribers())
-	{
-		geometry_msgs::PoseStamped goalMsg;
-		goalMsg.header.frame_id = mapFrameId_;
-		goalMsg.header.stamp = ros::Time::now();
-		rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
-		nextMetricGoalPub_.publish(goalMsg);
-	}
-	if(nextMetricGoalIdPub_.getNumSubscribers())
-	{
-		std_msgs::Int32 msg;
-		msg.data = rtabmap_.getPathGoalId();
-		nextMetricGoalIdPub_.publish(msg);
-	}
-}
-
 bool CoreWrapper::updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -1304,39 +1278,16 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 	return true;
 }
 
-void CoreWrapper::publishStats(const Statistics & stats)
+void CoreWrapper::publishStats(const Statistics & stats, const ros::Time & stamp)
 {
-	ros::Time timeNow = ros::Time::now();
 	if(infoPub_.getNumSubscribers())
 	{
 		//ROS_INFO("Sending RtabmapInfo msg (last_id=%d)...", stat.refImageId());
 		rtabmap_ros::InfoPtr msg(new rtabmap_ros::Info);
-		msg->header.stamp = timeNow;
+		msg->header.stamp = stamp;
 		msg->header.frame_id = mapFrameId_;
 
-		msg->refId = stats.refImageId();
-		msg->loopClosureId = stats.loopClosureId();
-		msg->localLoopClosureId = stats.localLoopClosureId();
-
-		rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), msg->loopClosureTransform);
-
-		// Detailed info
-		if(stats.extended())
-		{
-			//Posterior, likelihood, childCount
-			msg->posteriorKeys = uKeys(stats.posterior());
-			msg->posteriorValues = uValues(stats.posterior());
-			msg->likelihoodKeys = uKeys(stats.likelihood());
-			msg->likelihoodValues = uValues(stats.likelihood());
-			msg->rawLikelihoodKeys = uKeys(stats.rawLikelihood());
-			msg->rawLikelihoodValues = uValues(stats.rawLikelihood());
-			msg->weightsKeys = uKeys(stats.weights());
-			msg->weightsValues = uValues(stats.weights());
-
-			// Statistics data
-			msg->statsKeys = uKeys(stats.data());
-			msg->statsValues = uValues(stats.data());
-		}
+		rtabmap_ros::infoToROS(stats, *msg);
 		infoPub_.publish(msg);
 	}
 
@@ -1345,7 +1296,7 @@ void CoreWrapper::publishStats(const Statistics & stats)
 		if(stats.poses().size() == 0 || stats.poses().size() == stats.getMapIds().size())
 		{
 			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
-			graphMsg->header.stamp = timeNow;
+			graphMsg->header.stamp = stamp;
 			graphMsg->header.frame_id = mapFrameId_;
 
 			rtabmap_ros::mapGraphToROS(
@@ -1380,6 +1331,53 @@ void CoreWrapper::publishStats(const Statistics & stats)
 	}
 }
 
+void CoreWrapper::publishGoal(const ros::Time & stamp)
+{
+	if(!currentMetricGoal_.isNull())
+	{
+		ROS_INFO("Planning: Publishing next goal: Location %d pose=%s",
+				rtabmap_.getPathGoalId(), currentMetricGoal_.prettyPrint().c_str());
+		if(nextMetricGoalPub_.getNumSubscribers())
+		{
+			geometry_msgs::PoseStamped goalMsg;
+			goalMsg.header.frame_id = mapFrameId_;
+			goalMsg.header.stamp = ros::Time::now();
+			rtabmap_ros::transformToPoseMsg(currentMetricGoal_, goalMsg.pose);
+			ROS_INFO("Publishing next goal: %d", rtabmap_.getPathGoalId());
+			nextMetricGoalPub_.publish(goalMsg);
+		}
+	}
+}
+
+void CoreWrapper::publishLocalPath(const ros::Time & stamp)
+{
+	if(rtabmap_.getPath().size())
+	{
+		std::list<std::pair<int, Transform> > poses = rtabmap_.getPathNextPoses();
+		if(poses.size())
+		{
+			if(localPathPub_.getNumSubscribers())
+			{
+				nav_msgs::Path path;
+				path.header.frame_id = mapFrameId_;
+				path.header.stamp = stamp;
+				path.poses.resize(poses.size());
+				int oi = 0;
+				std::stringstream stream;
+				for(std::list<std::pair<int, Transform> >::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					path.poses[oi].header = path.header;
+					rtabmap_ros::transformToPoseMsg(iter->second, path.poses[oi].pose);
+					++oi;
+					stream << iter->first << " ";
+				}
+				ROS_INFO("Publishing local path: [%s]", stream.str().c_str());
+				localPathPub_.publish(path);
+			}
+		}
+	}
+}
+
 /**
  * exclusive callbacks:
  *     image

src/CoreWrapper.h

@@ -94,8 +94,7 @@ private:
 						   const sensor_msgs::LaserScanConstPtr& scanMsg,
 						   const nav_msgs::OdometryConstPtr & odomMsg);
 	void goalNodeCallback(const std_msgs::Int32ConstPtr & msg);
-	void updateGoal();
-	void publishGoal();
+	void updateGoal(const ros::Time & stamp);
 
 	void process(
 			int id,
@@ -126,7 +125,9 @@ private:
 
 	void publishLoop(double tfDelay);
 
-	void publishStats(const rtabmap::Statistics & stats);
+	void publishStats(const rtabmap::Statistics & stats, const ros::Time & stamp);
+	void publishGoal(const ros::Time & stamp);
+	void publishLocalPath(const ros::Time & stamp);
 
 private:
 	rtabmap::Rtabmap rtabmap_;
@@ -152,10 +153,9 @@ private:
 	//Planning stuff
 	ros::Subscriber goalNodeSub_;
 	ros::Publisher nextMetricGoalPub_;
-	ros::Publisher nextMetricGoalIdPub_;
 	ros::Publisher goalReachedPub_;
-	ros::Publisher pathPub_;
-	ros::Publisher pathIdsPub_;
+	ros::Publisher globalPathPub_;
+	ros::Publisher localPathPub_;
 
 	// for loop closure detection only
 	image_transport::Subscriber defaultSub_;

src/GuiWrapper.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <cv_bridge/cv_bridge.h>
 #include <std_srvs/Empty.h>
 #include <std_msgs/Empty.h>
+#include <sensor_msgs/image_encodings.h>
 
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -39,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/highgui/highgui.hpp>
 
 #include <image_geometry/pinhole_camera_model.h>
+#include <image_geometry/stereo_camera_model.h>
 
 #include <rtabmap/gui/MainWindow.h>
 #include <rtabmap/core/RtabmapEvent.h>
@@ -64,7 +66,10 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 		cameraNodeName_(""),
 		depthScanSync_(0),
 		depthSync_(0),
-		depthOdomInfoSync_(0)
+		depthOdomInfoSync_(0),
+		stereoSync_(0),
+		stereoScanSync_(0),
+		stereoOdomInfoSync_(0)
 {
 	ros::NodeHandle nh;
 	app_ = new QApplication(argc, argv);
@@ -101,15 +106,17 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
 	bool subscribeLaserScan = false;
 	bool subscribeDepth = false;
 	bool subscribeOdomInfo = false;
+	bool subscribeStereo = false;
 	int queueSize = 10;
 	pnh.param("frame_id", frameId_, frameId_);
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
 	pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
+	pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
 	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, queueSize);
+	this->setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeOdomInfo, subscribeStereo, queueSize);
 
 	UEventsManager::addHandler(this);
 	UEventsManager::addHandler(mainWindow_);
@@ -134,6 +141,18 @@ GuiWrapper::~GuiWrapper()
 	{
 		delete depthOdomInfoSync_;
 	}
+	if(stereoSync_)
+	{
+		delete stereoSync_;
+	}
+	if(stereoScanSync_)
+	{
+		delete stereoScanSync_;
+	}
+	if(stereoOdomInfoSync_)
+	{
+		delete stereoOdomInfoSync_;
+	}
 	delete infoMapSync_;
 	delete mainWindow_;
 	delete app_;
@@ -153,50 +172,12 @@ void GuiWrapper::infoMapCallback(
 	// Map from ROS struct to rtabmap struct
 	rtabmap::Statistics stat;
 
-	stat.setExtended(true); // Extended
+	// Info
+	rtabmap_ros::infoFromROS(*infoMsg, stat);
 
-	stat.setRefImageId(infoMsg->refId);
-	stat.setLoopClosureId(infoMsg->loopClosureId);
-	stat.setLocalLoopClosureId(infoMsg->localLoopClosureId);
-
-	//Posterior, likelihood, childCount
-	std::map<int, float> mapIntFloat;
-	for(unsigned int i=0; i<infoMsg->posteriorKeys.size() && i<infoMsg->posteriorValues.size(); ++i)
-	{
-		mapIntFloat.insert(std::pair<int, float>(infoMsg->posteriorKeys.at(i), infoMsg->posteriorValues.at(i)));
-	}
-	stat.setPosterior(mapIntFloat);
-	mapIntFloat.clear();
-	for(unsigned int i=0; i<infoMsg->likelihoodKeys.size() && i<infoMsg->likelihoodValues.size(); ++i)
-	{
-		mapIntFloat.insert(std::pair<int, float>(infoMsg->likelihoodKeys.at(i), infoMsg->likelihoodValues.at(i)));
-	}
-	stat.setLikelihood(mapIntFloat);
-	mapIntFloat.clear();
-	for(unsigned int i=0; i<infoMsg->rawLikelihoodKeys.size() && i<infoMsg->rawLikelihoodValues.size(); ++i)
-	{
-		mapIntFloat.insert(std::pair<int, float>(infoMsg->rawLikelihoodKeys.at(i), infoMsg->rawLikelihoodValues.at(i)));
-	}
-	stat.setRawLikelihood(mapIntFloat);
-	std::map<int, int> mapIntInt;
-	for(unsigned int i=0; i<infoMsg->weightsKeys.size() && i<infoMsg->weightsValues.size(); ++i)
-	{
-		mapIntInt.insert(std::pair<int, int>(infoMsg->weightsKeys.at(i), infoMsg->weightsValues.at(i)));
-	}
-	stat.setWeights(mapIntInt);
-
-	// Statistics data
-	for(unsigned int i=0; i<infoMsg->statsKeys.size() && i<infoMsg->statsValues.size(); i++)
-	{
-		stat.addStatistic(infoMsg->statsKeys.at(i), infoMsg->statsValues.at(i));
-	}
-
-	stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(infoMsg->loopClosureTransform));
-
-	//RGB-D SLAM data
-
-	Transform mapToOdom;
-	std::map<int, Transform> poses;
+	// MapData
+	rtabmap::Transform mapToOdom;
+	std::map<int, rtabmap::Transform> poses;
 	std::map<int, int> mapIds;
 	std::multimap<int, Link> links;
 
@@ -559,61 +540,346 @@ void GuiWrapper::depthScanCallback(
 	this->post(new OdometryEvent(image));
 }
 
+void GuiWrapper::stereoScanCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		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)
+{
+	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+	{
+		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
+		return;
+	}
+
+	// TF ready?
+	Transform localTransform;
+	sensor_msgs::PointCloud2 scanOut;
+	try
+	{
+		//transform laser to point cloud and to frameId_
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
+		localTransform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	pcl::PointCloud<pcl::PointXYZ> pclScan;
+	pcl::fromROSMsg(scanOut, pclScan);
+	cv::Mat scan = util3d::laserScanFromPointCloud(pclScan);
+
+	cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
+	}
+	else
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
+	}
+	ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
+
+	image_geometry::StereoCameraModel model;
+	model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
+
+	float fx = model.left().fx();
+	float cx = model.left().cx();
+	float cy = model.left().cy();
+	float baseline = model.baseline();
+
+	rtabmap::SensorData image(
+			scan,
+			ptrLeftImage->image.clone(),
+			ptrRightImage->image.clone(),
+			fx,
+			baseline,
+			cx,
+			cy,
+			localTransform,
+			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
+	this->post(new OdometryEvent(image));
+}
+
+void GuiWrapper::stereoOdomInfoCallback(
+		const nav_msgs::OdometryConstPtr & odomMsg,
+		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)
+{
+	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+	{
+		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
+		return;
+	}
+
+	// TF ready?
+	Transform localTransform;
+	try
+	{
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
+		localTransform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
+	}
+	else
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
+	}
+	ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
+
+	image_geometry::StereoCameraModel model;
+	model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
+
+	float fx = model.left().fx();
+	float cx = model.left().cx();
+	float cy = model.left().cy();
+	float baseline = model.baseline();
+
+	rtabmap::SensorData image(
+			ptrLeftImage->image.clone(),
+			ptrRightImage->image.clone(),
+			fx,
+			baseline,
+			cx,
+			cy,
+			localTransform,
+			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
+	OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
+	this->post(new OdometryEvent(image, info));
+}
+
+void GuiWrapper::stereoCallback(
+		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)
+{
+	if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+		!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+		rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
+	{
+		ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
+		return;
+	}
+
+	// TF ready?
+	Transform localTransform;
+	try
+	{
+		if(waitForTransform_)
+		{
+			if(!tfListener_.waitForTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, ros::Duration(1)))
+			{
+				ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), leftImageMsg->header.frame_id.c_str());
+				return;
+			}
+		}
+
+		tf::StampedTransform tmp;
+		tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
+		localTransform = rtabmap_ros::transformFromTF(tmp);
+	}
+	catch(tf::TransformException & ex)
+	{
+		ROS_WARN("%s",ex.what());
+		return;
+	}
+
+	cv_bridge::CvImageConstPtr ptrLeftImage, ptrRightImage;
+	if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
+	   leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "mono8");
+	}
+	else
+	{
+		ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
+	}
+	ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
+
+	image_geometry::StereoCameraModel model;
+	model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
+
+	float fx = model.left().fx();
+	float cx = model.left().cx();
+	float cy = model.left().cy();
+	float baseline = model.baseline();
+
+	rtabmap::SensorData image(
+			ptrLeftImage->image.clone(),
+			ptrRightImage->image.clone(),
+			fx,
+			baseline,
+			cx,
+			cy,
+			localTransform,
+			rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
+			odomMsg->pose.covariance[0],
+			odomMsg->header.seq);
+	this->post(new OdometryEvent(image));
+}
+
 void GuiWrapper::setupCallbacks(
 		bool subscribeDepth,
 		bool subscribeLaserScan,
 		bool subscribeOdomInfo,
+		bool subscribeStereo,
 		int queueSize)
 {
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
-	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(subscribeDepth && subscribeLaserScan)
+	if(subscribeDepth && subscribeStereo)
 	{
-		ROS_INFO("Registering Depth+LaserScan callback...");
-		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-		odomSub_.subscribe(nh, "odom", 1);
-		scanSub_.subscribe(nh, "scan", 1);
-		depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
-		depthScanSync_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+		ROS_WARN("\"subscribe_depth\" already true, ignoring \"subscribe_stereo\".");
 	}
-	else if(subscribeDepth && !subscribeLaserScan && subscribeOdomInfo)
+	if(!subscribeDepth && !subscribeStereo && subscribeLaserScan)
 	{
-		ROS_INFO("Registering Depth callback...");
-		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-		odomSub_.subscribe(nh, "odom", 1);
-		odomInfoSub_.subscribe(nh, "odom_info", 1);
-		depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
-		depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
+		ROS_WARN("Cannot subscribe to laser scan without depth or stereo subscription...");
 	}
-	else if(subscribeDepth && !subscribeLaserScan)
+
+	if(subscribeDepth)
 	{
-		ROS_INFO("Registering Depth callback...");
+		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);
+
+		odomSub_.subscribe(nh, "odom", 1);
 		imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
 		imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
 		cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+
+		if(subscribeLaserScan)
+		{
+			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_->registerCallback(boost::bind(&GuiWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+		}
+		else if(subscribeOdomInfo)
+		{
+			ROS_INFO("Registering Depth callback + OdomInfo...");
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			depthOdomInfoSync_ = new message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy>(MyDepthOdomInfoSyncPolicy(queueSize), imageSub_, odomSub_, odomInfoSub_, imageDepthSub_, cameraInfoSub_);
+			depthOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::depthOdomInfoCallback, this, _1, _2, _3, _4, _5));
+		}
+		else
+		{
+			ROS_INFO("Registering Depth callback...");
+			depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+			depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+		}
+	}
+	else if(subscribeStereo)
+	{
+		ros::NodeHandle left_nh(nh, "left");
+		ros::NodeHandle right_nh(nh, "right");
+		ros::NodeHandle left_pnh(pnh, "left");
+		ros::NodeHandle right_pnh(pnh, "right");
+		image_transport::ImageTransport left_it(left_nh);
+		image_transport::ImageTransport right_it(right_nh);
+		image_transport::TransportHints hintsLeft("raw", ros::TransportHints(), left_pnh);
+		image_transport::TransportHints hintsRight("raw", ros::TransportHints(), right_pnh);
+
+		imageRectLeft_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
+		imageRectRight_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
+		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
 		odomSub_.subscribe(nh, "odom", 1);
-		depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
-		depthSync_->registerCallback(boost::bind(&GuiWrapper::depthCallback, this, _1, _2, _3, _4));
+
+		if(subscribeLaserScan)
+		{
+			ROS_INFO("Registering Stereo callback + LaserScan...");
+			scanSub_.subscribe(nh, "scan", 1);
+			stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), odomSub_, scanSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			stereoScanSync_->registerCallback(boost::bind(&GuiWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
+		}
+		else if(subscribeOdomInfo)
+		{
+			ROS_INFO("Registering Stereo callback + OdomInfo...");
+			odomInfoSub_.subscribe(nh, "odom_info", 1);
+			stereoOdomInfoSync_ = new message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy>(MyStereoOdomInfoSyncPolicy(queueSize), odomSub_, odomInfoSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			stereoOdomInfoSync_->registerCallback(boost::bind(&GuiWrapper::stereoOdomInfoCallback, this, _1, _2, _3, _4, _5, _6));
+		}
+		else
+		{
+			ROS_INFO("Registering Stereo callback...");
+			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));
+		}
 	}
 	else // default odom only
 	{
-		if(!subscribeDepth && subscribeLaserScan)
-		{
-			ROS_WARN("Cannot subscribe to laser scan without depth subscription...");
-		}
 		ROS_INFO("Registering default callback (\"odom\" only)...");
 		defaultSub_ = nh.subscribe("odom", 1, &GuiWrapper::defaultCallback, this);
 	}

src/GuiWrapper.h

@@ -72,7 +72,7 @@ protected:
 private:
 	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
 
-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, int queueSize);
+	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,
@@ -90,6 +90,27 @@ private:
 						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
 						   const sensor_msgs::LaserScanConstPtr& scanMsg);
 
+	void stereoScanCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			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 stereoOdomInfoCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			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 stereoCallback(
+			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 processRequestedMap(const rtabmap_ros::MapData & map);
 
 private:
@@ -113,6 +134,11 @@ private:
 	message_filters::Subscriber<rtabmap_ros::OdomInfo> odomInfoSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
 	typedef message_filters::sync_policies::ExactTime<
 			rtabmap_ros::Info,
 			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
@@ -140,6 +166,32 @@ private:
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
 	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoSyncPolicy;
+	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			nav_msgs::Odometry,
+			sensor_msgs::LaserScan,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScanSyncPolicy;
+	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			nav_msgs::Odometry,
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
+	message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
 };
 
 #endif /* GUIWRAPPER_H_ */

src/MsgConversion.cpp

@@ -124,6 +124,80 @@ cv::Mat compressedMatFromBytes(const std::vector<unsigned char> & bytes, bool co
 	return out;
 }
 
+void infoFromROS(const rtabmap_ros::Info & info, rtabmap::Statistics & stat)
+{
+	stat.setExtended(true); // Extended
+
+	// rtabmap_ros::Info
+	stat.setRefImageId(info.refId);
+	stat.setLoopClosureId(info.loopClosureId);
+	stat.setLocalLoopClosureId(info.localLoopClosureId);
+
+	stat.setLoopClosureTransform(rtabmap_ros::transformFromGeometryMsg(info.loopClosureTransform));
+
+	//Posterior, likelihood, childCount
+	std::map<int, float> mapIntFloat;
+	for(unsigned int i=0; i<info.posteriorKeys.size() && i<info.posteriorValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(info.posteriorKeys.at(i), info.posteriorValues.at(i)));
+	}
+	stat.setPosterior(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<info.likelihoodKeys.size() && i<info.likelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(info.likelihoodKeys.at(i), info.likelihoodValues.at(i)));
+	}
+	stat.setLikelihood(mapIntFloat);
+	mapIntFloat.clear();
+	for(unsigned int i=0; i<info.rawLikelihoodKeys.size() && i<info.rawLikelihoodValues.size(); ++i)
+	{
+		mapIntFloat.insert(std::pair<int, float>(info.rawLikelihoodKeys.at(i), info.rawLikelihoodValues.at(i)));
+	}
+	stat.setRawLikelihood(mapIntFloat);
+	std::map<int, int> mapIntInt;
+	for(unsigned int i=0; i<info.weightsKeys.size() && i<info.weightsValues.size(); ++i)
+	{
+		mapIntInt.insert(std::pair<int, int>(info.weightsKeys.at(i), info.weightsValues.at(i)));
+	}
+	stat.setWeights(mapIntInt);
+
+	stat.setLocalPath(info.localPath);
+
+	// Statistics data
+	for(unsigned int i=0; i<info.statsKeys.size() && i<info.statsValues.size(); i++)
+	{
+		stat.addStatistic(info.statsKeys.at(i), info.statsValues.at(i));
+	}
+}
+
+void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
+{
+	info.refId = stats.refImageId();
+	info.loopClosureId = stats.loopClosureId();
+	info.localLoopClosureId = stats.localLoopClosureId();
+
+	rtabmap_ros::transformToGeometryMsg(stats.loopClosureTransform(), info.loopClosureTransform);
+
+	// Detailed info
+	if(stats.extended())
+	{
+		//Posterior, likelihood, childCount
+		info.posteriorKeys = uKeys(stats.posterior());
+		info.posteriorValues = uValues(stats.posterior());
+		info.likelihoodKeys = uKeys(stats.likelihood());
+		info.likelihoodValues = uValues(stats.likelihood());
+		info.rawLikelihoodKeys = uKeys(stats.rawLikelihood());
+		info.rawLikelihoodValues = uValues(stats.rawLikelihood());
+		info.weightsKeys = uKeys(stats.weights());
+		info.weightsValues = uValues(stats.weights());
+		info.localPath = stats.localPath();
+
+		// Statistics data
+		info.statsKeys = uKeys(stats.data());
+		info.statsValues = uValues(stats.data());
+	}
+}
+
 rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
 {
 	return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.variance);