Updated for rtabmap 0.10.0 (multi-cameras)

CMakeLists.txt

@@ -17,7 +17,7 @@ find_package(octomap_ros)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.9.0 REQUIRED)
+find_package(RTABMap 0.10.0 REQUIRED)
 
 #Qt stuff
 FIND_PACKAGE(Qt4 COMPONENTS QtCore QtGui REQUIRED)
@@ -44,7 +44,6 @@ add_message_files(
    Info.msg
    KeyPoint.msg
    MapData.msg
-   Graph.msg
    NodeData.msg
    Link.msg
    OdomInfo.msg

include/rtabmap_ros/MsgConversion.h

@@ -45,7 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap_ros/KeyPoint.h>
 #include <rtabmap_ros/Point2f.h>
 #include <rtabmap_ros/MapData.h>
-#include <rtabmap_ros/Graph.h>
 #include <rtabmap_ros/NodeData.h>
 #include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap_ros/Info.h>
@@ -83,24 +82,28 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
 std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
 void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
 
-void mapGraphFromROS(
-		const rtabmap_ros::Graph & msg,
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
+		std::map<int, rtabmap::Transform> & poses,
+		std::multimap<int, rtabmap::Link> & links,
+		std::map<int, rtabmap::Signature> & signatures,
+		rtabmap::Transform & mapToOdom);
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom);
-void mapGraphToROS(
+void mapDataToROS(
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links,
+		const std::map<int, rtabmap::Signature> & signatures,
+		const rtabmap::Transform & mapToOdom,
+		rtabmap_ros::MapData & msg);
+void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
-		const std::map<int, int> & mapIds,
-		const std::map<int, double> & stamps,
-		const std::map<int, std::string> & labels,
-		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::Graph & msg);
+		rtabmap_ros::MapData & msg);
 
 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg);
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg);

msg/Graph.msg

@@ -1,26 +0,0 @@
-
-Header header
-
-##
-# /map to /odom transform
-# Always identity when the graph is optimized from the latest pose.
-##
-geometry_msgs/Transform mapToOdom
-
-##
-# The nodes
-# std::map<nodeId, mapId>
-int32[] nodeIds
-int32[] mapIds
-string[] labels
-float64[] stamps
-UserData[] userDatas
-
-# std::map<nodeId, Pose>
-geometry_msgs/Pose[] poses
-
-##
-# The links
-##
-Link[] links
-

msg/MapData.msg

@@ -2,14 +2,25 @@
 Header header
 
 ##################
-# Graph stuff
+# Optimized graph
 ##################
+##
+# /map to /odom transform
+# Always identity when the graph is optimized from the latest pose.
+##
+geometry_msgs/Transform mapToOdom
 
-Graph graph
+# The poses
+int32[] posesId
+geometry_msgs/Pose[] poses
+
+# The links
+Link[] links
 
 ##################
-# Point cloud stuff
+# Graph data
 ##################
-
 NodeData[] nodes
 
+
+

msg/NodeData.msg

@@ -17,18 +17,19 @@ uint8[] image
 # use rtabmap::util3d::uncompressImage() from "rtabmap/core/util3d.h"
 uint8[] depth
 
-float32 fx
-float32 fy
-float32 cx
-float32 cy
+# Camera models
+float32[] fx
+float32[] fy
+float32[] cx
+float32[] cy
+float32 baseline
+# local transform (/base_link -> /camera_link)
+geometry_msgs/Transform[] localTransform
 
 # compressed 2D laser scan in /base_link frame
 # use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
 uint8[] laserScan
-
-# local transform (/base_link -> /camera_link)
-geometry_msgs/Transform localTransform
-
+int32 laserScanMaxPts
 
 # std::multimap<wordId, cv::Keypoint>
 # std::multimap<wordId, pcl::PointXYZ>

src/CameraNode.cpp

@@ -236,7 +236,7 @@ protected:
 		if(event->getClassName().compare("CameraEvent") == 0)
 		{
 			rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
-			const cv::Mat & image = e->data().image();
+			const cv::Mat & image = e->data().imageRaw();
 			if(!image.empty() && image.depth() == CV_8U)
 			{
 				cv_bridge::CvImage img;

src/CoreWrapper.cpp

@@ -43,10 +43,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
 
 #include <rtabmap/core/util3d_conversions.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/Memory.h>
+#include <rtabmap/core/OdometryEvent.h>
 
 #include <pcl_conversions/pcl_conversions.h>
 
@@ -68,12 +70,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 using namespace rtabmap;
 
-float max3( const float& a, const float& b, const float& c)
-{
-	float m=a>b?a:b;
-	return m>c?m:c;
-}
-
 CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 		paused_(false),
 		lastPose_(Transform::getIdentity()),
@@ -157,7 +153,6 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	infoPub_ = nh.advertise<rtabmap_ros::Info>("info", 1);
 	mapDataPub_ = nh.advertise<rtabmap_ros::MapData>("mapData", 1);
-	mapGraphPub_ = nh.advertise<rtabmap_ros::Graph>("graph", 1);
 	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 
 	// planning topics
@@ -561,8 +556,8 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
 
 		lastPose_ = odom;
 		lastPoseStamp_ = odomMsg->header.stamp;
-		float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
-		float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
+		double transVariance = uMax3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
+		double rotVariance = uMax3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
 		if(uIsFinite(rotVariance) && rotVariance > rotVariance_)
 		{
 			rotVariance_ = rotVariance;
@@ -766,23 +761,24 @@ void CoreWrapper::commonDepthCallback(
 
 	image_geometry::PinholeCameraModel model;
 	model.fromCameraInfo(*cameraInfoMsg);
-	float fx = model.fx();
-	float fy = model.fy();
-	float cx = model.cx();
-	float cy = model.cy();
+	double fx = model.fx();
+	double fy = model.fy();
+	double cx = model.cx();
+	double cy = model.cy();
 
 	process(ptrImage->header.seq,
 			scanMsg.get() != 0?scanMsg->header.stamp:ptrDepth->header.stamp,
 			ptrImage->image,
 			lastPose_,
 			odomFrameId,
-			rotVariance_>0?rotVariance_:1.0f,
-			transVariance_>0?transVariance_:1.0f,
+			rotVariance_>0?rotVariance_:1.0,
+			transVariance_>0?transVariance_:1.0,
 			ptrDepth->image,
 			fx,
 			fy,
 			cx,
 			cy,
+			0,
 			localTransform,
 			scan,
 			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
@@ -896,23 +892,25 @@ void CoreWrapper::commonStereoCallback(
 	image_geometry::StereoCameraModel model;
 	model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
 
-	float fx = model.left().fx();
-	float cx = model.left().cx();
-	float cy = model.left().cy();
-	float baseline = model.baseline();
+	double fx = model.left().fx();
+	double fy = model.left().fy();
+	double cx = model.left().cx();
+	double cy = model.left().cy();
+	double baseline = model.baseline();
 
 	process(leftImageMsg->header.seq,
 			scanMsg.get() != 0?scanMsg->header.stamp:leftImageMsg->header.stamp,
 			ptrLeftImage->image,
 			lastPose_,
 			odomFrameId,
-			rotVariance_>0?rotVariance_:1.0f,
-			transVariance_>0?transVariance_:1.0f,
+			rotVariance_>0?rotVariance_:1.0,
+			transVariance_>0?transVariance_:1.0,
 			ptrRightImage->image,
 			fx,
-			baseline,
+			fy,
 			cx,
 			cy,
+			baseline,
 			localTransform,
 			scan,
 			scanMsg.get() != 0?(int)scanMsg->ranges.size():0);
@@ -1036,13 +1034,14 @@ void CoreWrapper::process(
 		const cv::Mat & image,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		float odomRotationalVariance,
-		float odomTransitionalVariance,
+		double odomRotationalVariance,
+		double odomTransitionalVariance,
 		const cv::Mat & depthOrRightImage,
-		float fx,
-		float fyOrBaseline,
-		float cx,
-		float cy,
+		double fx,
+		double fy,
+		double cx,
+		double cy,
+		double baseline,
 		const Transform & localTransform,
 		const cv::Mat & scan,
 		int scanMaxPts)
@@ -1087,22 +1086,34 @@ void CoreWrapper::process(
 			}
 		}
 
-		SensorData data(scan,
+		SensorData data;
+		if(baseline > 0)
+		{
+			//stereo
+			data = SensorData(
+					scan,
 					scanMaxPts,
 					image.clone(),
 					imageB,
-				fx,
-				fyOrBaseline,
-				cx,
-				cy,
-				localTransform,
-				odom,
-				odomRotationalVariance,
-				odomTransitionalVariance,
+					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))
+
+		if(rtabmap_.process(data, odom, OdometryEvent::generateCovarianceMatrix(odomRotationalVariance, odomTransitionalVariance)))
 		{
 			timeRtabmap = timer.ticks();
 			mapToOdomMutex_.lock();
@@ -1451,22 +1462,15 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 	std::map<int, Signature> signatures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> constraints;
-	std::map<int, int> mapIds;
-	std::map<int, double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
 
 	if(req.graphOnly)
 	{
 		rtabmap_.getGraph(
 				poses,
 				constraints,
-				mapIds,
-				stamps,
-				labels,
-				userDatas,
 				req.optimized,
-				req.global);
+				req.global,
+				&signatures);
 	}
 	else
 	{
@@ -1474,37 +1478,22 @@ bool CoreWrapper::getMapCallback(rtabmap_ros::GetMap::Request& req, rtabmap_ros:
 				signatures,
 				poses,
 				constraints,
-				mapIds,
-				stamps,
-				labels,
-				userDatas,
 				req.optimized,
 				req.global);
 	}
 
-	if(poses.size() && poses.size() != mapIds.size())
+	if(poses.size() && poses.size() != signatures.size())
 	{
-		ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.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::mapGraphToROS(poses,
-		mapIds,
-		stamps,
-		labels,
-		userDatas,
+	rtabmap_ros::mapDataToROS(poses,
 		constraints,
+		signatures,
 		Transform::getIdentity(),
-		res.data.graph);
-
-	// add data
-	res.data.nodes.resize(signatures.size());
-	int i=0;
-	for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
-	{
-		rtabmap_ros::nodeDataToROS(iter->second, res.data.nodes[i++]);
-	}
+		res.data);
 
 	res.data.header.stamp = ros::Time::now();
 	res.data.header.frame_id = mapFrameId_;
@@ -1602,29 +1591,20 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 {
 	ROS_INFO("rtabmap: Publishing map...");
 
-	if(mapDataPub_.getNumSubscribers() ||
-		mapGraphPub_.getNumSubscribers() ||
-		!req.graphOnly)
+	if(mapDataPub_.getNumSubscribers())
 	{
-		std::map<int, Signature> signatures;
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
-		std::map<int, int> mapIds;
-		std::map<int, double> stamps;
-		std::map<int, std::string> labels;
-		std::map<int, std::vector<unsigned char> > userDatas;
+		std::map<int, Signature > signatures;
 
 		if(req.graphOnly)
 		{
 			rtabmap_.getGraph(
 					poses,
 					constraints,
-					mapIds,
-					stamps,
-					labels,
-					userDatas,
 					req.optimized,
-					req.global);
+					req.global,
+					&signatures);
 		}
 		else
 		{
@@ -1632,59 +1612,31 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 					signatures,
 					poses,
 					constraints,
-					mapIds,
-					stamps,
-					labels,
-					userDatas,
 					req.optimized,
 					req.global);
 		}
 
-		if(poses.size() && poses.size() != mapIds.size())
+		if(poses.size() && poses.size() != signatures.size())
 		{
-			ROS_ERROR("poses and map ids are not the same size!? %d vs %d", (int)poses.size(), (int)mapIds.size());
+			ROS_ERROR("poses and signatures are not the same size!? %d vs %d", (int)poses.size(), (int)signatures.size());
 		}
 
 		ros::Time now = ros::Time::now();
-		if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
-		{
-			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
-			graphMsg->header.stamp = now;
-			graphMsg->header.frame_id = mapFrameId_;
-
-			rtabmap_ros::mapGraphToROS(poses,
-				mapIds,
-				stamps,
-				labels,
-				userDatas,
-				constraints,
-				Transform::getIdentity(),
-				*graphMsg);
-
 		if(mapDataPub_.getNumSubscribers())
 		{
-				//RGB-D SLAM data
 			rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
-				msg->header = graphMsg->header;
-				msg->graph = *graphMsg;
+			msg->header.stamp = now;
+			msg->header.frame_id = mapFrameId_;
 
-				// add data
-				msg->nodes.resize(signatures.size());
-				int i=0;
-				for(std::map<int, Signature>::iterator iter = signatures.begin(); iter!=signatures.end(); ++iter)
-				{
-					rtabmap_ros::nodeDataToROS(iter->second, msg->nodes[i++]);
-				}
+			rtabmap_ros::mapDataToROS(poses,
+				constraints,
+				signatures,
+				Transform::getIdentity(),
+				*msg);
 
 			mapDataPub_.publish(msg);
 		}
 
-			if(mapGraphPub_.getNumSubscribers())
-			{
-				mapGraphPub_.publish(graphMsg);
-			}
-		}
-
 		if(!req.graphOnly)
 		{
 			std::map<int, Transform> filteredPoses;
@@ -1707,18 +1659,18 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 
 		if(labelsPub_.getNumSubscribers())
 		{
-			if(poses.size() && labels.size())
+			if(poses.size() && signatures.size())
 			{
 				visualization_msgs::MarkerArray markers;
-				for(std::map<int, std::string>::const_iterator iter=labels.begin();
-					iter!=labels.end();
+				for(std::map<int, Signature>::const_iterator iter=signatures.begin();
+					iter!=signatures.end();
 					++iter)
 				{
 					std::map<int, Transform>::const_iterator poseIter= poses.find(iter->first);
 					if(poseIter!=poses.end())
 					{
 						// Add labels
-						if(!iter->second.empty())
+						if(!iter->second.getLabel().empty())
 						{
 							visualization_msgs::Marker marker;
 							marker.header.frame_id = mapFrameId_;
@@ -1742,7 +1694,7 @@ bool CoreWrapper::publishMapCallback(rtabmap_ros::PublishMap::Request& req, rtab
 							marker.color.b = 0.0;
 
 							marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-							marker.text = iter->second;
+							marker.text = iter->second.getLabel();
 
 							markers.markers.push_back(marker);
 						}
@@ -1876,66 +1828,36 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 		infoPub_.publish(msg);
 	}
 
-	if(mapDataPub_.getNumSubscribers() || mapGraphPub_.getNumSubscribers())
-	{
-		if(stats.poses().size() == stats.getMapIds().size() &&
-		   stats.poses().size() == stats.getStamps().size() &&
-		   stats.poses().size() == stats.getLabels().size() &&
-		   stats.poses().size() == stats.getUserDatas().size())
-		{
-			rtabmap_ros::GraphPtr graphMsg(new rtabmap_ros::Graph);
-			graphMsg->header.stamp = stamp;
-			graphMsg->header.frame_id = mapFrameId_;
-
-			rtabmap_ros::mapGraphToROS(
-				stats.poses(),
-				stats.getMapIds(),
-				stats.getStamps(),
-				stats.getLabels(),
-				stats.getUserDatas(),
-				stats.constraints(),
-				stats.mapCorrection(),
-				*graphMsg);
-
 	if(mapDataPub_.getNumSubscribers())
 	{
-				//RGB-D SLAM data
 		rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
-				msg->header = graphMsg->header;
-				msg->graph = *graphMsg;
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
 
-				msg->nodes.resize(1);
-				rtabmap_ros::nodeDataToROS(stats.getSignature(), msg->nodes[0]);
+		rtabmap_ros::mapDataToROS(
+			stats.poses(),
+			stats.constraints(),
+			stats.getSignatures(),
+			stats.mapCorrection(),
+			*msg);
 
 		mapDataPub_.publish(msg);
 	}
 
-			if(mapGraphPub_.getNumSubscribers())
-			{
-				mapGraphPub_.publish(graphMsg);
-			}
-		}
-		else
-		{
-			ROS_ERROR("Poses, map ids and labels are not the same size!? %d vs %d vs %d",
-					(int)stats.poses().size(), (int)stats.getMapIds().size(), (int)stats.getLabels().size());
-		}
-	}
-
 	if(labelsPub_.getNumSubscribers())
 	{
-		if(stats.poses().size() && stats.getLabels().size())
+		if(stats.poses().size() && stats.getSignatures().size())
 		{
 			visualization_msgs::MarkerArray markers;
-			for(std::map<int, std::string>::const_iterator iter=stats.getLabels().begin();
-				iter!=stats.getLabels().end();
+			for(std::map<int, Signature>::const_iterator iter=stats.getSignatures().begin();
+				iter!=stats.getSignatures().end();
 				++iter)
 			{
 				std::map<int, Transform>::const_iterator poseIter= stats.poses().find(iter->first);
 				if(poseIter!=stats.poses().end())
 				{
 					// Add labels
-					if(!iter->second.empty())
+					if(!iter->second.getLabel().empty())
 					{
 						visualization_msgs::Marker marker;
 						marker.header.frame_id = mapFrameId_;
@@ -1959,7 +1881,7 @@ void CoreWrapper::publishStats(const ros::Time & stamp)
 						marker.color.b = 0.0;
 
 						marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-						marker.text = iter->second;
+						marker.text = iter->second.getLabel();
 
 						markers.markers.push_back(marker);
 					}

src/CoreWrapper.h

@@ -163,13 +163,14 @@ private:
 			const cv::Mat & image,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			float odomRotationalVariance = 1.0f,
-			float odomTransitionalVariance = 1.0f,
+			double odomRotationalVariance = 1.0,
+			double odomTransitionalVariance = 1.0,
 			const cv::Mat & depthOrRightImage = cv::Mat(),
-			float fx = 0.0f,
-			float fyOrBaseline = 0.0f,
-			float cx = 0.0f,
-			float cy = 0.0f,
+			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);
@@ -211,8 +212,8 @@ private:
 	bool paused_;
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
-	float rotVariance_;
-	float transVariance_;
+	double rotVariance_;
+	double transVariance_;
 	rtabmap::Transform currentMetricGoal_;
 	bool latestNodeWasReached_;
 	rtabmap::ParametersMap parameters_;
@@ -232,7 +233,6 @@ private:
 
 	ros::Publisher infoPub_;
 	ros::Publisher mapDataPub_;
-	ros::Publisher mapGraphPub_;
 	ros::Publisher labelsPub_;
 
 	//Planning stuff

src/DbPlayerNode.cpp

@@ -136,10 +136,10 @@ int main(int argc, char** argv)
 	ros::Publisher scanPub;
 	tf2_ros::TransformBroadcaster tfBroadcaster;
 
-	rtabmap::SensorData data = reader.getNextData();
-	while(ros::ok() && data.isValid())
+	rtabmap::OdometryEvent odom = reader.getNextData();
+	while(ros::ok() && odom.data().id())
 	{
-		ROS_INFO("Reading sensor data %d...", data.id());
+		ROS_INFO("Reading sensor data %d...", odom.data().id());
 
 		ros::Time time = ros::Time::now();
 
@@ -159,21 +159,30 @@ int main(int argc, char** argv)
 		camInfoB = camInfoA;
 
 		int type = -1;
-		if(!data.depth().empty() && (data.depth().type() == CV_32FC1 || data.depth().type() == CV_16UC1))
+		if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
+		{
+			if(odom.data().cameraModels().size() > 1)
+			{
+				ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
+			}
+			else
 			{
 				//depth
+				if(odom.data().cameraModels().size())
+				{
 					camInfoA.D.resize(5,0);
 
-			camInfoA.P[0] = data.fx();
-			camInfoA.K[0] = data.fx();
-			camInfoA.P[5] = data.fy();
-			camInfoA.K[4] = data.fy();
-			camInfoA.P[2] = data.cx();
-			camInfoA.K[2] = data.cx();
-			camInfoA.P[6] = data.cy();
-			camInfoA.K[5] = data.cy();
+					camInfoA.P[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.K[0] = odom.data().cameraModels()[0].fx();
+					camInfoA.P[5] = odom.data().cameraModels()[0].fy();
+					camInfoA.K[4] = odom.data().cameraModels()[0].fy();
+					camInfoA.P[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.K[2] = odom.data().cameraModels()[0].cx();
+					camInfoA.P[6] = odom.data().cameraModels()[0].cy();
+					camInfoA.K[5] = odom.data().cameraModels()[0].cy();
 
 					camInfoB = camInfoA;
+				}
 
 				type=0;
 
@@ -182,22 +191,26 @@ int main(int argc, char** argv)
 				if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
 				if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
 			}
-		else if(!data.rightImage().empty() && data.rightImage().type() == CV_8U)
+		}
+		else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
 		{
 			//stereo
+			if(odom.data().stereoCameraModel().isValid())
+			{
 				camInfoA.D.resize(8,0);
 
-			camInfoA.P[0] = data.fx();
-			camInfoA.K[0] = data.fx();
-			camInfoA.P[5] = data.fx(); // fx = fy
-			camInfoA.K[4] = data.fx(); // fx = fy
-			camInfoA.P[2] = data.cx();
-			camInfoA.K[2] = data.cx();
-			camInfoA.P[6] = data.cy();
-			camInfoA.K[5] = data.cy();
+				camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
+				camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
+				camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
+				camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
+				camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
+				camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
+				camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
+				camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
 
 				camInfoB = camInfoA;
-			camInfoB.P[3] = data.baseline()*-data.fx(); // Right_Tx = -baseline*fx
+				camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
+			}
 
 			type=1;
 
@@ -212,12 +225,12 @@ int main(int argc, char** argv)
 			if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
 		}
 
-		camInfoA.height = data.image().rows;
-		camInfoA.width = data.image().cols;
-		camInfoB.height = data.depthOrRightImage().rows;
-		camInfoB.width = data.depthOrRightImage().cols;
+		camInfoA.height = odom.data().imageRaw().rows;
+		camInfoA.width = odom.data().imageRaw().cols;
+		camInfoB.height = odom.data().depthOrRightRaw().rows;
+		camInfoB.width = odom.data().depthOrRightRaw().cols;
 
-		if(!data.laserScan().empty())
+		if(!odom.data().laserScanRaw().empty())
 		{
 			if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
 		}
@@ -226,44 +239,52 @@ int main(int argc, char** argv)
 		if(publishTf)
 		{
 			ros::Time tfExpiration = time + ros::Duration(1.0/rate);
-			if(!data.localTransform().isNull())
+
+			rtabmap::Transform localTransform;
+			if(odom.data().cameraModels().size() == 1)
+			{
+				localTransform = odom.data().cameraModels()[0].localTransform();
+			}
+			else if(odom.data().stereoCameraModel().isValid())
+			{
+				localTransform = odom.data().stereoCameraModel().left().localTransform();
+			}
+			if(!localTransform.isNull())
 			{
 				geometry_msgs::TransformStamped baseToCamera;
 				baseToCamera.child_frame_id = cameraFrameId;
 				baseToCamera.header.frame_id = frameId;
 				baseToCamera.header.stamp = tfExpiration;
-				rtabmap_ros::transformToGeometryMsg(data.localTransform(), baseToCamera.transform);
+				rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
 				tfBroadcaster.sendTransform(baseToCamera);
 			}
 
-			if(!data.pose().isNull())
+			if(!odom.pose().isNull())
 			{
 				geometry_msgs::TransformStamped odomToBase;
 				odomToBase.child_frame_id = frameId;
 				odomToBase.header.frame_id = odomFrameId;
 				odomToBase.header.stamp = tfExpiration;
-				rtabmap_ros::transformToGeometryMsg(data.pose(), odomToBase.transform);
+				rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
 				tfBroadcaster.sendTransform(odomToBase);
 			}
 		}
-		if(!data.pose().isNull())
+		if(!odom.pose().isNull())
 		{
 			if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
 
 			if(odometryPub.getNumSubscribers())
 			{
-				nav_msgs::Odometry odom;
-				odom.child_frame_id = frameId;
-				odom.header.frame_id = odomFrameId;
-				odom.header.stamp = time;
-				rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
-				odom.pose.covariance[0] = data.poseTransVariance();
-				odom.pose.covariance[7] = data.poseTransVariance();
-				odom.pose.covariance[14] = data.poseTransVariance();
-				odom.pose.covariance[21] = data.poseRotVariance();
-				odom.pose.covariance[28] = data.poseRotVariance();
-				odom.pose.covariance[35] = data.poseRotVariance();
-				odometryPub.publish(odom);
+				nav_msgs::Odometry odomMsg;
+				odomMsg.child_frame_id = frameId;
+				odomMsg.header.frame_id = odomFrameId;
+				odomMsg.header.stamp = time;
+				rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
+				UASSERT(odomMsg.pose.covariance.size() == 36 &&
+						odom.covariance().total() == 36 &&
+						odom.covariance().type() == CV_64FC1);
+				memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
+				odometryPub.publish(odomMsg);
 			}
 		}
 
@@ -290,7 +311,7 @@ int main(int argc, char** argv)
 		if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
 		{
 			cv_bridge::CvImage img;
-			if(data.image().channels() == 1)
+			if(odom.data().imageRaw().channels() == 1)
 			{
 				img.encoding = sensor_msgs::image_encodings::MONO8;
 			}
@@ -298,7 +319,7 @@ int main(int argc, char** argv)
 			{
 				img.encoding = sensor_msgs::image_encodings::BGR8;
 			}
-			img.image = data.image();
+			img.image = odom.data().imageRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -318,10 +339,10 @@ int main(int argc, char** argv)
 			}
 		}
 
-		if(depthPub.getNumSubscribers() && !data.depth().empty() && type==0)
+		if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
 		{
 			cv_bridge::CvImage img;
-			if(data.depth().type() == CV_32FC1)
+			if(odom.data().depthRaw().type() == CV_32FC1)
 			{
 				img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
 			}
@@ -329,7 +350,7 @@ int main(int argc, char** argv)
 			{
 				img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
 			}
-			img.image = data.depth();
+			img.image = odom.data().depthRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -338,11 +359,11 @@ int main(int argc, char** argv)
 			depthCamInfoPub.publish(camInfoB);
 		}
 
-		if(rightPub.getNumSubscribers() && !data.rightImage().empty() && type==1)
+		if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
 		{
 			cv_bridge::CvImage img;
 			img.encoding = sensor_msgs::image_encodings::MONO8;
-			img.image = data.rightImage();
+			img.image = odom.data().rightRaw();
 			sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
 			imageRosMsg->header.frame_id = cameraFrameId;
 			imageRosMsg->header.stamp = time;
@@ -351,9 +372,9 @@ int main(int argc, char** argv)
 			rightCamInfoPub.publish(camInfoB);
 		}
 
-		if(scanPub.getNumSubscribers() && !data.laserScan().empty())
+		if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().empty())
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(data.laserScan());
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::laserScanToPointCloud(odom.data().laserScanRaw());
 			sensor_msgs::PointCloud2 msg;
 			pcl::toROSMsg(*cloud, msg);
 			msg.header.frame_id = frameId;
@@ -369,7 +390,7 @@ int main(int argc, char** argv)
 			ros::spinOnce();
 		}
 
-		data = reader.getNextData();
+		odom = reader.getNextData();
 	}
 
 

src/GridMapAssemblerNode.cpp

@@ -99,9 +99,10 @@ public:
 		}
 
 		std::map<int, Transform> poses;
-		for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
+		UASSERT(msg->posesId.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
+			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
 		}
 
 		if(filterRadius_ > 0.0 && filterAngle_ > 0.0)

src/GuiWrapper.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap_ros/MapData.h"
 #include "rtabmap_ros/OdomInfo.h"
 #include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/core/Transform.h"
 
 #include <tf/transform_listener.h>
 
@@ -73,33 +74,54 @@ private:
 	void infoMapCallback(const rtabmap_ros::InfoConstPtr & infoMsg, const rtabmap_ros::MapDataConstPtr & mapMsg);
 
 	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeOdomInfo, bool subscribeStereo, int queueSize);
-	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg); // odom
-	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+
+	void commonDepthCallback(
 			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+	void commonStereoCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
+
+	void defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg);
+
+	// With odom msg
+	void depthCallback(
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
 			const sensor_msgs::ImageConstPtr& imageDepthMsg,
 			const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
 	void depthOdomInfoCallback(
-			const sensor_msgs::ImageConstPtr& imageMsg,
-			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
 			const sensor_msgs::ImageConstPtr& depthMsg,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
-	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+	void depthScanCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
 			const nav_msgs::OdometryConstPtr & odomMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
 		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
-						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
-						   const sensor_msgs::LaserScanConstPtr& scanMsg);
+		    const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
 
 	void stereoScanCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
 	void stereoOdomInfoCallback(
-			const nav_msgs::OdometryConstPtr & odomMsg,
 			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
 			const sensor_msgs::ImageConstPtr& leftImageMsg,
 			const sensor_msgs::ImageConstPtr& rightImageMsg,
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
@@ -111,7 +133,41 @@ private:
 			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
 			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
 
+	// with TF
+	void depthTFCallback(const sensor_msgs::ImageConstPtr& imageMsg,
+					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
+					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
+	void depthOdomInfoTFCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+			const sensor_msgs::ImageConstPtr& depthMsg,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfoMsg);
+	void depthScanTFCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& imageMsg,
+		    const sensor_msgs::ImageConstPtr& imageDepthMsg,
+		    const sensor_msgs:: CameraInfoConstPtr& camInfoMsg);
+
+	void stereoScanTFCallback(
+			const sensor_msgs::LaserScanConstPtr& scanMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoOdomInfoTFCallback(
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg,
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+	void stereoTFCallback(
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg);
+
 	void processRequestedMap(const rtabmap_ros::MapData & map);
+	rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
 
 private:
 	QApplication * app_;
@@ -121,6 +177,7 @@ private:
 
 	// odometry subscription stuffs
 	std::string frameId_;
+	std::string odomFrameId_;
 	bool waitForTransform_;
 	tf::TransformListener tfListener_;
 
@@ -145,25 +202,26 @@ private:
 			rtabmap_ros::MapData> MyInfoMapSyncPolicy;
 	message_filters::Synchronizer<MyInfoMapSyncPolicy> * infoMapSync_;
 
+	// with odom msg
 	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
+			sensor_msgs::LaserScan,
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
-			sensor_msgs::CameraInfo,
-			sensor_msgs::LaserScan> MyDepthScanSyncPolicy;
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScanSyncPolicy;
 	message_filters::Synchronizer<MyDepthScanSyncPolicy> * depthScanSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
 			nav_msgs::Odometry,
 			sensor_msgs::Image,
+			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
 			rtabmap_ros::OdomInfo,
+			nav_msgs::Odometry,
+			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo> MyDepthOdomInfoSyncPolicy;
 	message_filters::Synchronizer<MyDepthOdomInfoSyncPolicy> * depthOdomInfoSync_;
@@ -177,8 +235,8 @@ private:
 	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
 			sensor_msgs::LaserScan,
+			nav_msgs::Odometry,
 			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo,
@@ -186,13 +244,57 @@ private:
 	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
 
 	typedef message_filters::sync_policies::ApproximateTime<
-			nav_msgs::Odometry,
 			rtabmap_ros::OdomInfo,
+			nav_msgs::Odometry,
 			sensor_msgs::Image,
 			sensor_msgs::Image,
 			sensor_msgs::CameraInfo,
 			sensor_msgs::CameraInfo> MyStereoOdomInfoSyncPolicy;
 	message_filters::Synchronizer<MyStereoOdomInfoSyncPolicy> * stereoOdomInfoSync_;
+
+	// with odom TF
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::LaserScan,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthScanTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthScanTFSyncPolicy> * depthScanTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthTFSyncPolicy> * depthTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo> MyDepthOdomInfoTFSyncPolicy;
+	message_filters::Synchronizer<MyDepthOdomInfoTFSyncPolicy> * depthOdomInfoTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoTFSyncPolicy> * stereoTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::LaserScan,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoScanTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoScanTFSyncPolicy> * stereoScanTFSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			rtabmap_ros::OdomInfo,
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoOdomInfoTFSyncPolicy;
+	message_filters::Synchronizer<MyStereoOdomInfoTFSyncPolicy> * stereoOdomInfoTFSync_;
 };
 
 #endif /* GUIWRAPPER_H_ */

src/MapAssemblerNode.cpp

@@ -114,41 +114,23 @@ public:
 			int id = msg->nodes[i].id;
 			if(!uContains(rgbClouds_, id))
 			{
-				rtabmap::Transform localTransform = rtabmap_ros::transformFromGeometryMsg(msg->nodes[i].localTransform);
-				if(!localTransform.isNull())
+				rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(msg->nodes[i]);
+				if(!s.sensorData().imageCompressed().empty() &&
+				   !s.sensorData().depthOrRightCompressed().empty() &&
+				   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
 				{
 					cv::Mat image, depth;
-					float fx = msg->nodes[i].fx;
-					float fy = msg->nodes[i].fy;
-					float cx = msg->nodes[i].cx;
-					float cy = msg->nodes[i].cy;
+					s.sensorData().uncompressData(&image, &depth, 0);
 
-					//uncompress data
-					rtabmap::CompressionThread ctImage(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].image, false), true);
-					rtabmap::CompressionThread ctDepth(rtabmap_ros::compressedMatFromBytes(msg->nodes[i].depth, false), true);
-					ctImage.start();
-					ctDepth.start();
-					ctImage.join();
-					ctDepth.join();
-					image = ctImage.getUncompressedData();
-					depth = ctDepth.getUncompressedData();
 
-					if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
+					if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 					{
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-						if(depth.type() == CV_8UC1)
-						{
-							cloud = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
-						}
-						else
-						{
-							cloud = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
-						}
+						cloud = rtabmap::util3d::cloudRGBFromSensorData(
+								s.sensorData(),
+								cloudDecimation_,
+								cloudMaxDepth_);
 
-						if(cloud->size() && cloudMaxDepth_ > 0)
-						{
-							cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
-						}
 						if(cloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
 						{
 							pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloud, noiseFilterRadius_, noiseFilterMinNeighbors_);
@@ -163,9 +145,6 @@ public:
 
 						if(cloud->size())
 						{
-							cloud = util3d::transformPointCloud(cloud, localTransform);
-
-
 							rgbClouds_.insert(std::make_pair(id, cloud));
 
 							if(computeOccupancyGrid_)
@@ -213,9 +192,10 @@ public:
 
 		// filter poses
 		std::map<int, Transform> poses;
-		for(unsigned int i=0; i<msg->graph.nodeIds.size() && i<msg->graph.poses.size(); ++i)
+		UASSERT(msg->posesId.size() == msg->poses.size());
+		for(unsigned int i=0; i<msg->posesId.size(); ++i)
 		{
-			poses.insert(std::make_pair(msg->graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg->graph.poses[i])));
+			poses.insert(std::make_pair(msg->posesId[i], rtabmap_ros::transformFromPoseMsg(msg->poses[i])));
 		}
 		if(nodeFilteringAngle_ > 0.0 && nodeFilteringRadius_ > 0.0)
 		{

src/MapOptimizerNode.cpp

@@ -117,18 +117,14 @@ public:
 	{
 		// save new poses and constraints
 		// Assuming that nodes/constraints are all linked together
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.poses.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.mapIds.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.stamps.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.labels.size());
-		UASSERT(msg->graph.nodeIds.size() == msg->graph.userDatas.size());
+		UASSERT(msg->posesId.size() == msg->poses.size());
 
 		bool dataChanged = false;
 
 		std::multimap<int, Link> newConstraints;
-		for(unsigned int i=0; i<msg->graph.links.size(); ++i)
+		for(unsigned int i=0; i<msg->links.size(); ++i)
 		{
-			Link link = rtabmap_ros::linkFromROS(msg->graph.links[i]);
+			Link link = rtabmap_ros::linkFromROS(msg->links[i]);
 			newConstraints.insert(std::make_pair(link.from(), link));
 
 			bool edgeAlreadyAdded = false;
@@ -152,79 +148,48 @@ public:
 		}
 
 		std::map<int, Transform> newPoses;
-		std::map<int, int> newMapIds;
-		std::map<int, double> newStamps;
-		std::map<int, std::string> newLabels;
-		std::map<int, std::vector<unsigned char> > newUserDatas;
 		// add new odometry poses
 		for(unsigned int i=0; i<msg->nodes.size(); ++i)
 		{
 			int id = msg->nodes[i].id;
 			Transform pose = rtabmap_ros::transformFromPoseMsg(msg->nodes[i].pose);
 			newPoses.insert(std::make_pair(id, pose));
-			newMapIds.insert(std::make_pair(id, msg->nodes[i].mapId));
-			newStamps.insert(std::make_pair(id, msg->nodes[i].stamp));
-			newLabels.insert(std::make_pair(id, msg->nodes[i].label));
-			newUserDatas.insert(std::make_pair(id, msg->nodes[i].userData.data));
 
 			std::pair<std::map<int, Transform>::iterator, bool> p = cachedPoses_.insert(std::make_pair(id, pose));
 			if(!p.second && pose != cachedPoses_.at(id))
 			{
 				dataChanged = true;
 			}
-			else if(p.second)
-			{
-				cachedMapIds_.insert(std::make_pair(id, msg->nodes[i].mapId));
-				cachedStamps_.insert(std::make_pair(id, msg->nodes[i].stamp));
-				cachedLabels_.insert(std::make_pair(id, msg->nodes[i].label));
-				cachedUserDatas_.insert(std::make_pair(id, msg->nodes[i].userData.data));
-			}
 		}
 
 		if(dataChanged)
 		{
 			ROS_WARN("Graph data has changed! Reset cache...");
 			cachedPoses_ = newPoses;
-			cachedMapIds_ = newMapIds;
-			cachedStamps_ = newStamps;
-			cachedLabels_ = newLabels;
-			cachedUserDatas_ = newUserDatas;
 			cachedConstraints_ = newConstraints;
 		}
 
 		//match poses in the graph
 		std::map<int, Transform> poses;
-		std::map<int, int> mapIds;
-		std::map<int, double> stamps;
-		std::map<int, std::string> labels;
-		std::map<int, std::vector<unsigned char> > userDatas;
 		std::multimap<int, Link> constraints;
 		if(globalOptimization_)
 		{
 			poses = cachedPoses_;
-			mapIds = cachedMapIds_;
-			stamps = cachedStamps_;
-			labels = cachedLabels_;
-			userDatas = cachedUserDatas_;
 			constraints = cachedConstraints_;
 		}
 		else
 		{
 			constraints = newConstraints;
-			for(unsigned int i=0; i<msg->graph.nodeIds.size(); ++i)
+			for(unsigned int i=0; i<msg->posesId.size(); ++i)
 			{
-				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->graph.nodeIds[i]);
+				std::map<int, Transform>::iterator iter = cachedPoses_.find(msg->posesId[i]);
 				if(iter != cachedPoses_.end())
 				{
 					poses.insert(*iter);
-					mapIds.insert(*cachedMapIds_.find(iter->first));
-					stamps.insert(*cachedStamps_.find(iter->first));
-					labels.insert(*cachedLabels_.find(iter->first));
-					userDatas.insert(*cachedUserDatas_.find(iter->first));
 				}
 				else
 				{
-					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->graph.nodeIds[i]);
+					ROS_ERROR("Odometry pose of node %d not found in cache!", msg->posesId[i]);
 					return;
 				}
 			}
@@ -236,12 +201,12 @@ public:
 			UTimer timer;
 			std::map<int, Transform> optimizedPoses;
 			Transform mapCorrection = Transform::getIdentity();
+			std::multimap<int, rtabmap::Link> linksOut;
 			if(poses.size() > 1 && constraints.size() > 0)
 			{
 				graph::TOROOptimizer optimizer(iterations_, false, ignoreVariance_);
 				int fromId = optimizeFromLastNode_?poses.rbegin()->first:poses.begin()->first;
 				std::map<int, rtabmap::Transform> posesOut;
-				std::multimap<int, rtabmap::Link> linksOut;
 				optimizer.getConnectedGraph(
 						fromId,
 						poses,
@@ -264,19 +229,13 @@ public:
 				ROS_ERROR("map_optimizer: Poses=%d and edges=%d (poses must "
 					   "not be null if there are edges, and edges must be null if poses <= 1)",
 					  (int)poses.size(), (int)constraints.size());
-				mapIds.clear();
-				labels.clear();
-				stamps.clear();
-				userDatas.clear();
 			}
 
-			UASSERT(optimizedPoses.size() == mapIds.size());
-			UASSERT(optimizedPoses.size() == labels.size());
-			UASSERT(optimizedPoses.size() == stamps.size());
-			UASSERT(optimizedPoses.size() == userDatas.size());
 			rtabmap_ros::MapData outputMsg;
-			rtabmap_ros::mapGraphToROS(optimizedPoses, mapIds, stamps, labels, userDatas, std::multimap<int, rtabmap::Link>(), mapCorrection, outputMsg.graph);
-			outputMsg.graph.links = msg->graph.links;
+			rtabmap_ros::mapDataToROS(optimizedPoses,
+					linksOut,
+					mapCorrection,
+					outputMsg);
 			outputMsg.header = msg->header;
 			outputMsg.nodes = msg->nodes;
 			mapDataPub_.publish(outputMsg);
@@ -301,10 +260,6 @@ private:
 	ros::Publisher mapDataPub_;
 
 	std::map<int, Transform> cachedPoses_;
-	std::map<int, int> cachedMapIds_;
-	std::map<int, double> cachedStamps_;
-	std::map<int, std::string> cachedLabels_;
-	std::map<int, std::vector<unsigned char> > cachedUserDatas_;
 	std::multimap<int, Link> cachedConstraints_;
 
 	tf2_ros::TransformBroadcaster tfBroadcaster_;

src/MapsManager.cpp

@@ -162,7 +162,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		{
 			if(!iter->second.isNull())
 			{
-				rtabmap::Signature data;
+				rtabmap::SensorData data;
 				bool rgbDepthRequired = updateCloud && !uContains(clouds_, iter->first);
 				bool depthRequired = updateProj && !uContains(projMaps_, iter->first);
 				bool scanRequired = updateGrid && !uContains(gridMaps_, iter->first);
@@ -175,7 +175,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
 						if(findIter != signatures.end())
 						{
-							data = findIter->second;
+							data = findIter->second.sensorData();
 						}
 					}
 					else
@@ -186,55 +186,25 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 
 				if(data.id() > 0)
 				{
-					rtabmap::Transform localTransform = data.getLocalTransform();
-					if(!localTransform.isNull())
+					if(!data.imageCompressed().empty() &&
+					   !data.depthOrRightCompressed().empty() &&
+					   (data.cameraModels().size() || data.stereoCameraModel().isValid()))
 					{
 						// Which data should we decompress?
 						cv::Mat image, depth, scan;
-						data.uncompressDataConst(rgbDepthRequired?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
-						if(!depth.empty() &&
-							depth.type() == CV_8UC1 &&
-							image.empty() &&
-							!rgbDepthRequired)
-						{
-							// Stereo detected, we should uncompress left image too
-							data.uncompressDataConst(&image, 0, 0);
-						}
-						float fx = data.getFx();
-						float fy = data.getFy();
-						float cx = data.getCx();
-						float cy = data.getCy();
+						data.uncompressData(rgbDepthRequired||data.stereoCameraModel().isValid()?&image:0, rgbDepthRequired||depthRequired?&depth:0, scanRequired?&scan:0);
 
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
 						if(rgbDepthRequired)
 						{
-							if(!image.empty() &&
-								!depth.empty() &&
-								fx > 0.0f && fy > 0.0f &&
-								cx >= 0.0f && cy >= 0.0f)
+							if(!image.empty() && !depth.empty())
 							{
-								if(depth.type() == CV_8UC1)
-								{
-									cloudRGB = util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-								else
-								{
-									cloudRGB = util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-
-								if(cloudRGB->size() && cloudMaxDepth_ > 0)
-								{
-									cloudRGB = util3d::passThrough(cloudRGB, "z", 0, cloudMaxDepth_);
-								}
-								if(cloudRGB->size() && cloudVoxelSize_ > 0)
-								{
-									cloudRGB = util3d::voxelize(cloudRGB, cloudVoxelSize_);
-								}
-								if(cloudRGB->size())
-								{
-									cloudRGB = util3d::transformPointCloud(cloudRGB, localTransform);
-								}
+								cloudRGB = util3d::cloudRGBFromSensorData(
+										data,
+										cloudDecimation_,
+										cloudMaxDepth_,
+										cloudVoxelSize_);
 							}
 							else
 							{
@@ -243,55 +213,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						}
 						else if(depthRequired)
 						{
-							if(	!depth.empty() &&
-								fx > 0.0f && fy > 0.0f &&
-								cx >= 0.0f && cy >= 0.0f)
+							if(	!depth.empty())
 							{
-								if(depth.type() == CV_8UC1)
-								{
-									if(!image.empty())
-									{
-										cv::Mat leftMono;
-										if(image.channels() == 3)
-										{
-											cv::cvtColor(image, leftMono, CV_BGR2GRAY);
-										}
-										else
-										{
-											leftMono = image;
-										}
-										cloudXYZ = rtabmap::util3d::cloudFromDisparity(
-												util2d::disparityFromStereoImages(leftMono, depth),
-												cx, cy,
-												fx, fy,
-												cloudDecimation_);
-									}
-								}
-								else
-								{
-									cloudXYZ = util3d::cloudFromDepth(depth, cx, cy, fx, fy, cloudDecimation_);
-								}
-
-								if(cloudXYZ.get())
-								{
-									if(cloudXYZ->size() && cloudMaxDepth_ > 0)
-									{
-										cloudXYZ = util3d::passThrough(cloudXYZ, "z", 0, cloudMaxDepth_);
-									}
-									if(cloudXYZ->size() && gridCellSize_ > 0)
-									{
-										// use gridCellSize since this cloud is only for the projection map
-										cloudXYZ = util3d::voxelize(cloudXYZ, gridCellSize_);
-									}
-									if(cloudXYZ->size())
-									{
-										cloudXYZ = util3d::transformPointCloud(cloudXYZ, localTransform);
-									}
-								}
-								else
-								{
-									ROS_ERROR("Left stereo image was empty! (node=%d)", iter->first);
-								}
+								cloudXYZ = util3d::cloudFromSensorData(
+										data,
+										cloudDecimation_,
+										cloudMaxDepth_,
+										gridCellSize_); // use gridCellSize since this cloud is only for the projection map
 							}
 							else
 							{

src/MsgConversion.cpp

@@ -290,91 +290,91 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
 	}
 }
 
-void mapGraphFromROS(
-		const rtabmap_ros::Graph & msg,
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
+		std::map<int, rtabmap::Transform> & poses,
+		std::multimap<int, rtabmap::Link> & links,
+		std::map<int, rtabmap::Signature> & signatures,
+		rtabmap::Transform & mapToOdom)
+{
+	//optimized graph
+	mapDataFromROS(msg, poses, links, mapToOdom);
+
+	//Data
+	for(unsigned int i=0; i<msg.nodes.size(); ++i)
+	{
+		signatures.insert(std::make_pair(msg.nodes[i].id, nodeDataFromROS(msg.nodes[i])));
+	}
+}
+void mapDataFromROS(
+		const rtabmap_ros::MapData & msg,
 		std::map<int, rtabmap::Transform> & poses,
-		std::map<int, int> & mapIds,
-		std::map<int, double> & stamps,
-		std::map<int, std::string> & labels,
-		std::map<int, std::vector<unsigned char> > & userDatas,
 		std::multimap<int, rtabmap::Link> & links,
 		rtabmap::Transform & mapToOdom)
 {
-	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
-
-	UASSERT(msg.nodeIds.size() == msg.mapIds.size());
-	UASSERT(msg.nodeIds.size() == msg.poses.size());
-	UASSERT(msg.nodeIds.size() == msg.stamps.size());
-	UASSERT(msg.nodeIds.size() == msg.labels.size());
-	UASSERT(msg.nodeIds.size() == msg.userDatas.size());
-
-	for(unsigned int i=0; i<msg.nodeIds.size(); ++i)
+	//optimized graph
+	UASSERT(msg.posesId.size() == msg.poses.size());
+	for(unsigned int i=0; i<msg.posesId.size(); ++i)
 	{
-		poses.insert(std::make_pair(msg.nodeIds[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
-		mapIds.insert(std::make_pair(msg.nodeIds[i], msg.mapIds[i]));
-		stamps.insert(std::make_pair(msg.nodeIds[i], msg.stamps[i]));
-		labels.insert(std::make_pair(msg.nodeIds[i], msg.labels[i]));
-		userDatas.insert(std::make_pair(msg.nodeIds[i], msg.userDatas[i].data));
+		poses.insert(std::make_pair(msg.posesId[i], rtabmap_ros::transformFromPoseMsg(msg.poses[i])));
 	}
-
 	for(unsigned int i=0; i<msg.links.size(); ++i)
 	{
 		rtabmap::Transform t = rtabmap_ros::transformFromGeometryMsg(msg.links[i].transform);
 		links.insert(std::make_pair(msg.links[i].fromId, linkFromROS(msg.links[i])));
 	}
+	mapToOdom = transformFromGeometryMsg(msg.mapToOdom);
 }
-void mapGraphToROS(
+void mapDataToROS(
+		const std::map<int, rtabmap::Transform> & poses,
+		const std::multimap<int, rtabmap::Link> & links,
+		const std::map<int, rtabmap::Signature> & signatures,
+		const rtabmap::Transform & mapToOdom,
+		rtabmap_ros::MapData & msg)
+{
+	//Optimized graph
+	mapDataToROS(poses, links, mapToOdom, msg);
+
+	//Data
+	msg.nodes.resize(signatures.size());
+	int index=0;
+	for(std::multimap<int, rtabmap::Signature>::const_iterator iter = signatures.begin();
+		iter!=signatures.end();
+		++iter)
+	{
+		nodeDataToROS(iter->second, msg.nodes[index++]);
+	}
+}
+
+void mapDataToROS(
 		const std::map<int, rtabmap::Transform> & poses,
-		const std::map<int, int> & mapIds,
-		const std::map<int, double> & stamps,
-		const std::map<int, std::string> & labels,
-		const std::map<int, std::vector<unsigned char> > & userDatas,
 		const std::multimap<int, rtabmap::Link> & links,
 		const rtabmap::Transform & mapToOdom,
-		rtabmap_ros::Graph & msg)
+		rtabmap_ros::MapData & msg)
 {
-	UASSERT(poses.size() == 0 ||
-			(poses.size() == mapIds.size() &&
-			 poses.size() == labels.size() &&
-			 poses.size() == stamps.size() &&
-			 poses.size() == userDatas.size()));
-
-	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
-
-	msg.nodeIds.resize(poses.size());
+	//Optimized graph
+	msg.posesId.resize(poses.size());
 	msg.poses.resize(poses.size());
-	msg.mapIds.resize(poses.size());
-	msg.stamps.resize(poses.size());
-	msg.labels.resize(poses.size());
-	msg.userDatas.resize(poses.size());
 	int index = 0;
-	std::map<int, rtabmap::Transform>::const_iterator iterPoses = poses.begin();
-	std::map<int, int>::const_iterator iterMapIds = mapIds.begin();
-	std::map<int, double>::const_iterator iterStamps = stamps.begin();
-	std::map<int, std::string>::const_iterator iterLabels = labels.begin();
-	std::map<int, std::vector<unsigned char> >::const_iterator iterUserDatas = userDatas.begin();
-	while(iterPoses != poses.end())
+	for(std::map<int, rtabmap::Transform>::const_iterator iter = poses.begin();
+		iter != poses.end();
+		++iter)
 	{
-		msg.nodeIds[index] = iterPoses->first;
-		msg.mapIds[index] = iterMapIds->second;
-		msg.stamps[index] = iterStamps->second;
-		msg.labels[index] = iterLabels->second;
-		msg.userDatas[index].data = iterUserDatas->second;
-		transformToPoseMsg(iterPoses->second, msg.poses[index]);
-		++iterPoses;
-		++iterMapIds;
-		++iterStamps;
-		++iterLabels;
-		++iterUserDatas;
+		msg.posesId[index] = iter->first;
+		transformToPoseMsg(iter->second, msg.poses[index]);
 		++index;
 	}
 
 	msg.links.resize(links.size());
 	index=0;
-	for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin(); iter!=links.end(); ++iter)
+	for(std::multimap<int, rtabmap::Link>::const_iterator iter = links.begin();
+		iter!=links.end();
+		++iter)
 	{
 		linkToROS(iter->second, msg.links[index++]);
 	}
+
+	transformToGeometryMsg(mapToOdom, msg.mapToOdom);
 }
 
 rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
@@ -404,24 +404,71 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
 		ROS_ERROR("Words 2D and 3D should be the same size (%d, %d)!", (int)words.size(), (int)words3D.size());
 	}
 
-	return rtabmap::Signature(
+	rtabmap::StereoCameraModel stereoModel;
+	std::vector<rtabmap::CameraModel> models;
+	if(msg.baseline > 0.0f)
+	{
+		// stereo model
+		if(msg.fx.size() == 1 &&
+		   msg.fy.size() == 1,
+		   msg.cx.size() == 1,
+		   msg.cy.size() == 1,
+		   msg.localTransform.size() == 1)
+		{
+			stereoModel = rtabmap::StereoCameraModel(
+					msg.fx[0],
+					msg.fy[0],
+					msg.cx[0],
+					msg.cy[0],
+					msg.baseline,
+					transformFromGeometryMsg(msg.localTransform[0]));
+		}
+	}
+	else
+	{
+		// multi-cameras model
+		if(msg.fx.size() &&
+		   msg.fx.size() == msg.fy.size(),
+		   msg.fx.size() == msg.cx.size(),
+		   msg.fx.size() == msg.cy.size(),
+		   msg.fx.size() == msg.localTransform.size())
+		{
+			for(unsigned int i=0; i<msg.fx.size(); ++i)
+			{
+				models.push_back(rtabmap::CameraModel(
+						msg.fx[i],
+						msg.fy[i],
+						msg.cx[i],
+						msg.cy[i],
+						transformFromGeometryMsg(msg.localTransform[i])));
+			}
+		}
+	}
+
+	rtabmap::Signature s(
 			msg.id,
 			msg.mapId,
 			msg.weight,
 			msg.stamp,
 			msg.label,
-			words,
-			words3D,
 			transformFromPoseMsg(msg.pose),
 			msg.userData.data,
+			stereoModel.isValid()?
+				rtabmap::SensorData(
 					compressedMatFromBytes(msg.laserScan),
+					msg.laserScanMaxPts,
 					compressedMatFromBytes(msg.image),
 					compressedMatFromBytes(msg.depth),
-			msg.fx,
-			msg.fy,
-			msg.cx,
-			msg.cy,
-			transformFromGeometryMsg(msg.localTransform));
+					stereoModel):
+				rtabmap::SensorData(
+					compressedMatFromBytes(msg.laserScan),
+					msg.laserScanMaxPts,
+					compressedMatFromBytes(msg.image),
+					compressedMatFromBytes(msg.depth),
+					models));
+	s.setWords(words);
+	s.setWords3(words3D);
+	return s;
 }
 void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData & msg)
 {
@@ -433,14 +480,36 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
 	msg.label = signature.getLabel();
 	msg.userData.data = signature.getUserData();
 	transformToPoseMsg(signature.getPose(), msg.pose);
-	compressedMatToBytes(signature.getImageCompressed(), msg.image);
-	compressedMatToBytes(signature.getDepthCompressed(), msg.depth);
-	compressedMatToBytes(signature.getLaserScanCompressed(), msg.laserScan);
-	msg.fx = signature.getFx();
-	msg.fy = signature.getFy();
-	msg.cx = signature.getCx();
-	msg.cy = signature.getCy();
-	transformToGeometryMsg(signature.getLocalTransform(), msg.localTransform);
+	compressedMatToBytes(signature.sensorData().imageCompressed(), msg.image);
+	compressedMatToBytes(signature.sensorData().depthOrRightCompressed(), msg.depth);
+	compressedMatToBytes(signature.sensorData().laserScanCompressed(), msg.laserScan);
+	msg.baseline = 0;
+	if(signature.sensorData().cameraModels().size())
+	{
+		msg.fx.resize(signature.sensorData().cameraModels().size());
+		msg.fy.resize(signature.sensorData().cameraModels().size());
+		msg.cx.resize(signature.sensorData().cameraModels().size());
+		msg.cy.resize(signature.sensorData().cameraModels().size());
+		msg.localTransform.resize(signature.sensorData().cameraModels().size());
+		for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
+		{
+			msg.fx[i] = signature.sensorData().cameraModels()[i].fx();
+			msg.fy[i] = signature.sensorData().cameraModels()[i].fy();
+			msg.cx[i] = signature.sensorData().cameraModels()[i].cx();
+			msg.cy[i] = signature.sensorData().cameraModels()[i].cy();
+			transformToGeometryMsg(signature.sensorData().cameraModels()[i].localTransform(), msg.localTransform[i]);
+		}
+	}
+	else if(signature.sensorData().stereoCameraModel().isValid())
+	{
+		msg.fx.push_back(signature.sensorData().stereoCameraModel().left().fx());
+		msg.fy.push_back(signature.sensorData().stereoCameraModel().left().fy());
+		msg.cx.push_back(signature.sensorData().stereoCameraModel().left().cx());
+		msg.cy.push_back(signature.sensorData().stereoCameraModel().left().cy());
+		msg.baseline = signature.sensorData().stereoCameraModel().baseline();
+		msg.localTransform.resize(1);
+		transformToGeometryMsg(signature.sensorData().stereoCameraModel().left().localTransform(), msg.localTransform[0]);
+	}
 
 	//Features stuff...
 	msg.wordIds = uKeys(signature.getWords());

src/OdometryROS.cpp

@@ -377,7 +377,6 @@ void OdometryROS::processData(const SensorData & data, const std_msgs::Header &
 				{
 					const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
 					pcl::PointCloud<pcl::PointXYZ> cloud;
-					rtabmap::Transform t = data.localTransform();
 					for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
 					{
 						// transform to odom frame

src/RGBDOdometryNode.cpp

@@ -138,24 +138,19 @@ public:
 			{
 				image_geometry::PinholeCameraModel model;
 				model.fromCameraInfo(*cameraInfo);
-				float fx = model.fx();
-				float fy = model.fy();
-				float cx = model.cx();
-				float cy = model.cy();
+				rtabmap::CameraModel rtabmapModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						rtabmap_ros::transformFromTF(localTransform));
 				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
 				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
 
 				rtabmap::SensorData data(
 						ptrImage->image,
 						ptrDepth->image,
-						fx,
-						fy,
-						cx,
-						cy,
-						rtabmap_ros::transformFromTF(localTransform),
-						rtabmap::Transform(),
-						1.0f,
-						1.0f,
+						rtabmapModel,
 						0,
 						rtabmap_ros::timestampFromROS(image->header.stamp));
 

src/StereoOdometryNode.cpp

@@ -159,34 +159,31 @@ public:
 			{
 				image_geometry::StereoCameraModel model;
 				model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
-
-				float fx = model.left().fx();
-				float cx = model.left().cx();
-				float cy = model.left().cy();
-				float baseline = model.baseline();
-				cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
-				cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
-
-				if(baseline <= 0)
+				if(model.baseline() <= 0)
 				{
 					ROS_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
-							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", baseline);
+							  "setup where the Tx (or P(0,3)) is negative in the right camera info msg.", model.baseline());
 					return;
 				}
 
+				rtabmap::StereoCameraModel stereoModel(
+						model.left().fx(),
+						model.left().fy(),
+						model.left().cx(),
+						model.left().cy(),
+						model.baseline(),
+						rtabmap_ros::transformFromTF(localTransform));
+
+				cv_bridge::CvImageConstPtr ptrImageLeft = cv_bridge::toCvShare(imageRectLeft, "mono8");
+				cv_bridge::CvImageConstPtr ptrImageRight = cv_bridge::toCvShare(imageRectRight, "mono8");
+
 				UTimer stepTimer;
 				//
 				UDEBUG("localTransform = %s", rtabmap_ros::transformFromTF(localTransform).prettyPrint().c_str());
-				rtabmap::SensorData data(ptrImageLeft->image,
+				rtabmap::SensorData data(
+						ptrImageLeft->image,
 						ptrImageRight->image,
-						fx,
-						baseline,
-						cx,
-						cy,
-						rtabmap_ros::transformFromTF(localTransform),
-						rtabmap::Transform(),
-						1.0f,
-						1.0f,
+						stereoModel,
 						0,
 						rtabmap_ros::timestampFromROS(imageRectLeft->header.stamp));
 

src/rviz/MapCloudDisplay.cpp

@@ -252,48 +252,26 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 		if(cloud_infos_.find(id) == cloud_infos_.end())
 		{
 			// Cloud not added to RVIZ, add it!
-			rtabmap::Transform localTransform = transformFromGeometryMsg(map.nodes[i].localTransform);
-			if(!localTransform.isNull())
+			rtabmap::Signature s = rtabmap_ros::nodeDataFromROS(map.nodes[i]);
+			if(!s.sensorData().imageCompressed().empty() &&
+			   !s.sensorData().depthOrRightCompressed().empty() &&
+			   (s.sensorData().cameraModels().size() || s.sensorData().stereoCameraModel().isValid()))
 			{
 				cv::Mat image, depth;
-				float fx = map.nodes[i].fx;
-				float fy = map.nodes[i].fy;
-				float cx = map.nodes[i].cx;
-				float cy = map.nodes[i].cy;
+				s.sensorData().uncompressData(&image, &depth, 0);
 
-				//uncompress data
-				rtabmap::CompressionThread ctImage(compressedMatFromBytes(map.nodes[i].image, false), true);
-				rtabmap::CompressionThread ctDepth(compressedMatFromBytes(map.nodes[i].depth, false), true);
-				ctImage.start();
-				ctDepth.start();
-				ctImage.join();
-				ctDepth.join();
-				image = ctImage.getUncompressedData();
-				depth = ctDepth.getUncompressedData();
 
-				if(!image.empty() && !depth.empty() && fx > 0.0f && fy > 0.0f && cx >= 0.0f && cy >= 0.0f)
+				if(!s.sensorData().imageRaw().empty() && !s.sensorData().depthOrRightRaw().empty())
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-					if(depth.type() == CV_8UC1)
-					{
-						cloud = rtabmap::util3d::cloudFromStereoImages(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
-					}
-					else
-					{
-						cloud = rtabmap::util3d::cloudFromDepthRGB(image, depth, cx, cy, fx, fy, cloud_decimation_->getInt());
-					}
-					if(cloud_max_depth_->getFloat() > 0.0f)
-					{
-						cloud = rtabmap::util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());
-					}
-					if(cloud_voxel_size_->getFloat() > 0.0f)
-					{
-						cloud = rtabmap::util3d::voxelize(cloud, cloud_voxel_size_->getFloat());
-					}
+					cloud = rtabmap::util3d::cloudRGBFromSensorData(
+							s.sensorData(),
+							cloud_decimation_->getInt(),
+							cloud_max_depth_->getFloat(),
+							cloud_voxel_size_->getFloat());
 
-					cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
-
-					// do it after local transform
+					if(cloud->size())
+					{
 						if(cloud_filter_floor_height_->getFloat() > 0.0f)
 						{
 							cloud = rtabmap::util3d::passThrough(cloud, "z", cloud_filter_floor_height_->getFloat(), 999.0f);
@@ -317,12 +295,13 @@ void MapCloudDisplay::processMapData(const rtabmap_ros::MapData& map)
 				}
 			}
 		}
+	}
 
 	// Update graph
 	std::map<int, rtabmap::Transform> poses;
-	for(unsigned int i=0; i<map.graph.nodeIds.size() && i<map.graph.poses.size(); ++i)
+	for(unsigned int i=0; i<map.posesId.size() && i<map.poses.size(); ++i)
 	{
-		poses.insert(std::make_pair(map.graph.nodeIds[i], rtabmap_ros::transformFromPoseMsg(map.graph.poses[i])));
+		poses.insert(std::make_pair(map.posesId[i], rtabmap_ros::transformFromPoseMsg(map.poses[i])));
 	}
 
 	if(node_filtering_angle_->getFloat() > 0.0f && node_filtering_radius_->getFloat() > 0.0f)
@@ -503,12 +482,12 @@ void MapCloudDisplay::downloadMap()
 		else
 		{
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
 			QApplication::processEvents();
 			this->reset();
 			processMapData(getMapSrv.response.data);
 			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+					.arg(getMapSrv.response.data.poses.size()).arg(getMapSrv.response.data.nodes.size()));
 
 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}
@@ -560,10 +539,10 @@ void MapCloudDisplay::downloadGraph()
 		}
 		else
 		{
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.poses.size()));
 			QApplication::processEvents();
 			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
+			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.poses.size()));
 
 			QTimer::singleShot(1000, messageBox, SLOT(close()));
 		}

src/rviz/MapGraphDisplay.cpp

@@ -95,21 +95,17 @@ void MapGraphDisplay::destroyObjects()
 
 void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
 {
-	if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size()))
+	if(!(msg->poses.size() == msg->posesId.size()))
 	{
-		ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size.");
+		ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
 		return;
 	}
 
 	// Get links
 	std::map<int, rtabmap::Transform> poses;
-	std::map<int, int> mapIds;
-	std::map<int,  double> stamps;
-	std::map<int, std::string> labels;
-	std::map<int, std::vector<unsigned char> > userDatas;
 	std::multimap<int, rtabmap::Link> links;
 	rtabmap::Transform mapToOdom;
-	rtabmap_ros::mapGraphFromROS(msg->graph, poses, mapIds, stamps, labels, userDatas, links, mapToOdom);
+	rtabmap_ros::mapDataFromROS(*msg, poses, links, mapToOdom);
 
 	destroyObjects();