ros-pkg: updated with new RTAB-Map stereo features

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1862 f169173b-cf89-36c8-b27e-44dbe73f0c83

src/CoreWrapper.cpp

@@ -75,12 +75,27 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 
 	bool subscribeLaserScan = false;
 	bool subscribeDepth = true;
+	bool subscribeStereo = false;
 	int queueSize = 10;
 	double tfDelay = 0.05; // 20 Hz
 
 	// ROS related parameters (private)
 	pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
 	pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
+	pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
+	if(subscribeDepth && subscribeStereo)
+	{
+		UWARN("Parameters subscribe_depth and subscribe_stereo cannot be true at the same time. Parameter subscribe_depth is set to false.");
+		subscribeDepth = false;
+	}
+	if(subscribeLaserScan)
+	{
+		if(!subscribeDepth && !subscribeStereo)
+		{
+			ROS_WARN("When subscribing to laser scan, you should subscribe to depth or stereo too. Subscribing to depth by default...");
+			subscribeDepth = true;
+		}
+	}
 
 	pnh.param("config_path", configPath_, configPath_);
 
@@ -187,10 +202,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	if(isRGBD)
 	{
 		// RGBD SLAM
-		if(!subscribeDepth)
+		if(!subscribeDepth && !subscribeStereo)
 		{
-			ROS_WARN("ROS param subscribe_depth is false, but RTAB-Map "
-					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth "
+			ROS_WARN("ROS param subscribe_depth and subscribe_stereo are false, but RTAB-Map "
+					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth or subscribe_stereo "
 					  "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
 					  "detection on images-only.");
 		}
@@ -198,10 +213,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	else
 	{
 		// loop closure detection (images-only)
-		if(subscribeDepth || subscribeLaserScan)
+		if(subscribeDepth || subscribeLaserScan || subscribeStereo)
 		{
-			ROS_WARN("ROS param subscribe_depth or subscribe_laserScan is true, but RTAB-Map "
-					  "parameter \"RGBD/Enabled\" is false! Please set subscribe_depth and subscribe_laserScan "
+			ROS_WARN("ROS param subscribe_depth, subscribe_laserScan or subscribe_stereo is true, but RTAB-Map "
+					  "parameter \"RGBD/Enabled\" is false! Please set subscribe_depth, subscribe_laserScan and subscribe_stereo "
 					  "to false to use rtabmap node for loop closure detection on images-only, or set \"RGBD/Enabled\" to true "
 					  "for RGB-D SLAM.");
 		}
@@ -227,7 +242,7 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	getMapDataSrv_ = nh.advertiseService("get_map", &CoreWrapper::getMapCallback, this);
 	publishMapDataSrv_ = nh.advertiseService("publish_map", &CoreWrapper::publishMapCallback, this);
 
-	setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
+	setupCallbacks(subscribeDepth, subscribeLaserScan, subscribeStereo, queueSize);
 
 	transformThread_ = new boost::thread(boost::bind(&CoreWrapper::publishLoop, this, tfDelay));
 }
@@ -382,9 +397,9 @@ void CoreWrapper::depthCallback(
 		if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 			 imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-			 imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
-			(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
-			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
+			 imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))
 		{
 			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
 			return;
@@ -460,9 +475,9 @@ void CoreWrapper::depthScanCallback(
 		if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
 				imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) &&
-			(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 ||
-			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0))
+				imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			!(depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1) == 0 ||
+			 depthMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0))
 		{
 			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 and image_depth=32FC1,16UC1");
 			return;
@@ -526,14 +541,186 @@ void CoreWrapper::depthScanCallback(
 	}
 }
 
+void CoreWrapper::stereoCallback(
+		const sensor_msgs::ImageConstPtr& leftImageMsg,
+	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+	   const nav_msgs::OdometryConstPtr & odomMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		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
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		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(*leftCamInfoMsg, *rightCamInfoMsg);
+
+		float fx = model.left().fx();
+		float cx = model.left().cx();
+		float cy = model.left().cy();
+		float baseline = model.baseline();
+
+		process(leftImageMsg->header.seq,
+				ptrLeftImage->image,
+				odom,
+				odomMsg->header.frame_id,
+				ptrRightImage->image,
+				fx,
+				baseline,
+				cx,
+				cy,
+				localTransform,
+				cv::Mat());
+	}
+}
+
+void CoreWrapper::stereoScanCallback(
+		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 nav_msgs::OdometryConstPtr & odomMsg)
+{
+	if(!paused_)
+	{
+		if(rate_>0.0f)
+		{
+			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
+			{
+				return;
+			}
+		}
+		time_ = ros::Time::now();
+
+		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
+		{
+			tf::StampedTransform tmp;
+			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
+			tfListener_.lookupTransform(frameId_, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, tmp);
+			localTransform = transformFromTF(tmp);
+		}
+		catch(tf::TransformException & ex)
+		{
+			ROS_WARN("%s",ex.what());
+			return;
+		}
+
+		//transform in frameId_ frame
+		sensor_msgs::PointCloud2 scanOut;
+		laser_geometry::LaserProjection projection;
+		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
+		pcl::PointCloud<pcl::PointXYZ> pclScan;
+		pcl::fromROSMsg(scanOut, pclScan);
+		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
+
+		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
+
+		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(*leftCamInfoMsg, *rightCamInfoMsg);
+
+		float fx = model.left().fx();
+		float cx = model.left().cx();
+		float cy = model.left().cy();
+		float baseline = model.baseline();
+
+		process(leftImageMsg->header.seq,
+				ptrLeftImage->image,
+				odom,
+				odomMsg->header.frame_id,
+				ptrRightImage->image,
+				fx,
+				baseline,
+				cx,
+				cy,
+				localTransform,
+				scan);
+	}
+}
+
 void CoreWrapper::process(
 		int id,
 		const cv::Mat & image,
 		const Transform & odom,
 		const std::string & odomFrameId,
-		const cv::Mat & depth,
+		const cv::Mat & depthOrRightImage,
 		float fx,
-		float fy,
+		float fyOrBaseline,
 		float cx,
 		float cy,
 		const Transform & localTransform,
@@ -542,37 +729,47 @@ void CoreWrapper::process(
 	UTimer timer;
 	if(rtabmap_.isIDsGenerated() || id > 0)
 	{
-		cv::Mat depth16;
-		if(!depth.empty() && depth.type() != CV_16UC1)
+		cv::Mat imageB;
+		if(!depthOrRightImage.empty())
 		{
-			if(depth.type() == CV_32FC1)
+			if(depthOrRightImage.type() == CV_8UC1)
 			{
-				//convert to 16 bits
-				depth16 = util3d::cvtDepthFromFloat(depth);
-				static bool shown = false;
-				if(!shown)
+				//right image
+				imageB = depthOrRightImage.clone();
+			}
+			else if(depthOrRightImage.type() != CV_16UC1)
+			{
+				// depth float
+				if(depthOrRightImage.type() == CV_32FC1)
 				{
-					ROS_WARN("Use depth image with \"unsigned short\" type to "
-							 "avoid conversion. This message is only printed once...");
-					shown = true;
+					//convert to 16 bits
+					imageB = util3d::cvtDepthFromFloat(depthOrRightImage);
+					static bool shown = false;
+					if(!shown)
+					{
+						ROS_WARN("Use depth image with \"unsigned short\" type to "
+								 "avoid conversion. This message is only printed once...");
+						shown = true;
+					}
+				}
+				else
+				{
+					ROS_ERROR("Depth image must be of type \"unsigned short\"!");
+					return;
 				}
 			}
 			else
 			{
-				ROS_ERROR("Depth image must be of type \"unsigned short\"!");
-				return;
+				// depth short
+				imageB = depthOrRightImage.clone();
 			}
 		}
-		else
-		{
-			depth16 = depth.clone();
-		}
 
 		SensorData data(image.clone(),
-				depth16,
+				imageB,
 				scan,
 				fx,
-				fy,
+				fyOrBaseline,
 				cx,
 				cy,
 				odom,
@@ -1248,52 +1445,80 @@ void CoreWrapper::publishStats(const Statistics & stats)
 void CoreWrapper::setupCallbacks(
 		bool subscribeDepth,
 		bool subscribeLaserScan,
+		bool subscribeStereo,
 		int queueSize)
 {
-	if(subscribeLaserScan)
-	{
-		if(!subscribeDepth)
-		{
-			ROS_WARN("When subscribing to laser scan, you should subscribe to depth too. Subscribing to depth...");
-			subscribeDepth = true;
-		}
-	}
-
 	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)
 	{
-		ROS_INFO("Registering Depth+LaserScan 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);
+
 		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(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+
+		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(&CoreWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+		}
+		else //!subscribeLaserScan
+		{
+			ROS_INFO("Registering Depth callback...");
+			depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+			depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+		}
 	}
-	else if(subscribeDepth && !subscribeLaserScan)
+	else if(subscribeStereo)
 	{
-		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);
+		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(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
+
+		if(subscribeLaserScan)
+		{
+			ROS_INFO("Registering Stereo+LaserScan callback...");
+			scanSub_.subscribe(nh, "scan", 1);
+			stereoScanSync_ = new message_filters::Synchronizer<MyStereoScanSyncPolicy>(MyStereoScanSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, scanSub_, odomSub_);
+			stereoScanSync_->registerCallback(boost::bind(&CoreWrapper::stereoScanCallback, this, _1, _2, _3, _4, _5, _6));
+		}
+		else //!subscribeLaserScan
+		{
+			ROS_INFO("Registering Stereo callback...");
+			stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomSub_);
+			stereoSync_->registerCallback(boost::bind(&CoreWrapper::stereoCallback, this, _1, _2, _3, _4, _5));
+		}
 	}
 	else
 	{
-		ROS_INFO("Registering default callback...");
+		ROS_INFO("Registering image-only callback...");
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
 		defaultSub_ = rgb_it.subscribe("image", 1, &CoreWrapper::defaultCallback, this);
 	}
 }

src/CoreWrapper.h

@@ -68,7 +68,7 @@ public:
 	virtual ~CoreWrapper();
 
 private:
-	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, int queueSize);
+	void setupCallbacks(bool subscribeDepth, bool subscribeLaserScan, bool subscribeStereo, int queueSize);
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 	void depthCallback(const sensor_msgs::ImageConstPtr& imageMsg,
 					   const nav_msgs::OdometryConstPtr & odomMsg,
@@ -79,15 +79,26 @@ private:
 						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
 						   const sensor_msgs::CameraInfoConstPtr& camInfoMsg,
 						   const sensor_msgs::LaserScanConstPtr& scanMsg);
+	void stereoCallback(const sensor_msgs::ImageConstPtr& leftImageMsg,
+					   	   const sensor_msgs::ImageConstPtr& rightImageMsg,
+					   	   const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
+					   	   const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
+					   	   const nav_msgs::OdometryConstPtr & odomMsg);
+	void stereoScanCallback(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 nav_msgs::OdometryConstPtr & odomMsg);
 
 	void process(
 			int id,
 			const cv::Mat & image,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			const cv::Mat & depth = cv::Mat(),
+			const cv::Mat & depthOrRightImage = cv::Mat(),
 			float fx = 0.0f,
-			float fy = 0.0f,
+			float fyOrBaseline = 0.0f,
 			float cx = 0.0f,
 			float cy = 0.0f,
 			const rtabmap::Transform & localTransform = rtabmap::Transform(),
@@ -126,12 +137,20 @@ private:
 	ros::Publisher infoPubEx_;
 	ros::Publisher mapData_;
 
+	// for loop closure detection only
 	image_transport::Subscriber defaultSub_;
+
+	//for depth callback
 	image_transport::SubscriberFilter imageSub_;
 	image_transport::SubscriberFilter imageDepthSub_;
-	image_transport::SubscriberFilter imageSubRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubRight_;
+
+	//stereo callback
+	image_transport::SubscriberFilter imageRectLeft_;
+	image_transport::SubscriberFilter imageRectRight_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
+
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 
@@ -150,6 +169,23 @@ private:
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
 
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::LaserScan,
+			nav_msgs::Odometry> MyStereoScanSyncPolicy;
+	message_filters::Synchronizer<MyStereoScanSyncPolicy> * stereoScanSync_;
+
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			nav_msgs::Odometry> MyStereoSyncPolicy;
+	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
+
 	tf::TransformBroadcaster tfBroadcaster_;
 	tf::TransformListener tfListener_;
 

src/MapAssemblerNode.cpp

@@ -130,7 +130,15 @@ public:
 
 					if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
 					{
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
+						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+						if(depth.type() == CV_8UC1)
+						{
+							cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
+						}
+						else
+						{
+							cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloudDecimation_);
+						}
 
 						if(cloudMaxDepth_ > 0)
 						{

src/OdometryROS.cpp

@@ -61,7 +61,6 @@ OdometryROS::OdometryROS(int argc, char * argv[]) :
 	odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
 	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
-	//odomMatches_ = nh.advertise<sensor_msgs::Image>("odom_matches", 1);
 
 	ros::NodeHandle pnh("~");
 

src/StereoOdometryNode.cpp

@@ -60,22 +60,11 @@ public:
 	StereoOdometry(int argc, char * argv[]) :
 		OdometryROS(argc, argv),
 		feature2D_(0),
-		depthPatchSize_(1),
-		generateDepth_(false),
-		stereoFlowWinSize_(21),
-		stereoFlowIterations_(30),
-		stereoFlowEpsilon_(0.01),
-		stereoFlowMaxLevel_(3),
-		stereoSubPixWinSize_(5),
-		stereoSubPixIterations_(20),
-		stereoSubPixEps_(0.03),
 		approxSync_(0),
 		exactSync_(0)
 	{
 		ros::NodeHandle nh;
 
-		odomDepth_ = nh.advertise<sensor_msgs::Image>("odom_depth", 1);
-
 		ros::NodeHandle pnh("~");
 
 		bool approxSync = false;
@@ -84,27 +73,9 @@ public:
 		pnh.param("queue_size", queueSize, queueSize);
 		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
-		pnh.param("generate_depth", generateDepth_, generateDepth_);
-		pnh.param("depth_patch_size", depthPatchSize_, depthPatchSize_);
-		ROS_INFO("Generate depth = %s", generateDepth_?"true":"false");
-
-		pnh.param("flow_win_size", stereoFlowWinSize_, stereoFlowWinSize_);
-		pnh.param("flow_iterations", stereoFlowIterations_, stereoFlowIterations_);
-		pnh.param("flow_epsilon", stereoFlowEpsilon_, stereoFlowEpsilon_);
-		pnh.param("flow_max_level", stereoFlowMaxLevel_, stereoFlowMaxLevel_);
-
-		pnh.param("subpix_win_size", stereoSubPixWinSize_, stereoSubPixWinSize_);
-		pnh.param("subpix_iterations", stereoSubPixIterations_, stereoSubPixIterations_);
-		pnh.param("subpix_eps", stereoSubPixEps_, stereoSubPixEps_);
-
-		UASSERT_MSG(!this->isOdometryBOW() || (this->isOdometryBOW() && generateDepth_),
-				"Odom/Strategy=0 (OdometryBOW) requires depth generation (generate_depth=true).");
-
-		UASSERT(depthPatchSize_ >= 0);
-
 		//Keypoint detector
 		ParametersMap::const_iterator iter;
-		Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
+		Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
 		if((iter=this->parameters().find(Parameters::kOdomFeatureType())) != this->parameters().end())
 		{
 			detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
@@ -195,173 +166,28 @@ public:
 				model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
 
 				float fx = model.left().fx();
-				float fy = model.left().fy();
 				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");
 
-				cv::Mat depthOrRightImage;
-				std::vector<cv::KeyPoint> kptsLeft, kptsRight;
-				cv::Mat descLeft, descRight;
 				UTimer stepTimer;
-
-				if(!generateDepth_)
-				{
-					// copy right image in depth
-					depthOrRightImage = ptrImageRight->image;
-				}
-				else
-				{
-					//generate depth
-					depthOrRightImage = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
-
-					std::vector<cv::Point2f> cornersLeft, cornersRight;
-
-					cv::Rect roi = Feature2D::computeRoi(ptrImageLeft->image, roiRatios_);
-					kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image, 0, roi);
-					UDEBUG("time generate left kpts=%fs", stepTimer.ticks());
-
-					if(!kptsLeft.size())
-					{
-						ROS_WARN("No left keypoints extracted!");
-						return;
-					}
-
-					int stereoFeaturesAdded = 0;
-					int stereoFeaturesMatched = 0;
-					int stereoFeaturesExtracted = 0;
-
-					cv::KeyPoint::convert(kptsLeft, cornersLeft);
-
-					if(stereoSubPixWinSize_ > 0 && stereoSubPixIterations_ > 0)
-					{
-						cv::cornerSubPix( ptrImageLeft->image, cornersLeft,
-								cv::Size( stereoSubPixWinSize_, stereoSubPixWinSize_ ),
-								cv::Size( -1, -1 ),
-								cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, stereoSubPixIterations_, stereoSubPixEps_ ) );
-						UDEBUG("time subpix left kpts=%fs", stepTimer.ticks());
-					}
-
-					std::vector<unsigned char> status;
-					std::vector<float> err;
-
-					UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-					cv::calcOpticalFlowPyrLK(
-							ptrImageLeft->image,
-							ptrImageRight->image,
-							cornersLeft,
-							cornersRight,
-							status,
-							err,
-							cv::Size(stereoFlowWinSize_, stereoFlowWinSize_), stereoFlowMaxLevel_,
-							cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, stereoFlowIterations_, stereoFlowEpsilon_),
-							cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
-					UDEBUG("cv::calcOpticalFlowPyrLK() end");
-					UDEBUG("time optical flow=%fs", stepTimer.ticks());
-
-					std::vector<cv::KeyPoint> kptsLeftFiltered(kptsLeft.size());
-					int oi = 0;
-					for(int i=0; i<status.size(); ++i)
-					{
-						if(status[i] &&
-							uIsInBounds(cornersLeft[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
-							uIsInBounds(cornersLeft[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f) &&
-							uIsInBounds(cornersRight[i].x, 0.0f, float(depthOrRightImage.cols)-1.0f) &&
-							uIsInBounds(cornersRight[i].y, 0.0f, float(depthOrRightImage.rows)-1.0f))
-						{
-							float disparity = cornersLeft[i].x - cornersRight[i].x;
-
-							if(disparity >= 0)
-							{
-								float d = model.getZ(disparity);
-								if(d>0)
-								{
-									bool depthAdded = false;
-									int u = int(cornersLeft[i].x+0.5f);
-									int v = int(cornersLeft[i].y+0.5f);
-									for(int j=-depthPatchSize_; j<=depthPatchSize_; ++j)
-									{
-										for(int k=-depthPatchSize_; k<=depthPatchSize_; ++k)
-										{
-											if(uIsInBounds(u+j, 0, depthOrRightImage.cols-1) &&
-											   uIsInBounds(v+k, 0, depthOrRightImage.rows-1))
-											{
-												depthOrRightImage.at<float>(v+j, u+k) = d;
-												depthAdded = true;
-											}
-										}
-									}
-									if(depthAdded)
-									{
-										kptsLeftFiltered[oi] = kptsLeft[i];
-										kptsLeftFiltered[oi].pt = cornersLeft[i];
-										++oi;
-									}
-								}
-							}
-							++stereoFeaturesMatched;
-						}
-					}
-					stereoFeaturesAdded = oi;
-					stereoFeaturesExtracted = kptsLeft.size();
-
-					UDEBUG("stereoFeaturesExtracted=%d", stereoFeaturesExtracted);
-					UDEBUG("stereoFeaturesMatched=%d", stereoFeaturesMatched);
-					UDEBUG("stereoFeaturesAdded=%d", stereoFeaturesAdded);
-
-					kptsLeftFiltered.resize(oi);
-					kptsLeft = kptsLeftFiltered;
-
-					if(!kptsLeft.size())
-					{
-						ROS_WARN("No left keypoints extracted!");
-						return;
-					}
-
-					// For OdometryBOW, we must generate descriptors
-					int odomStrategy = Parameters::defaultOdomStrategy();
-					Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
-					if(odomStrategy == 0)
-					{
-						descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
-						UDEBUG("time generate left descriptors=%fs, remaining kpts=%d", stepTimer.ticks(), (int)kptsLeft.size());
-						if(!kptsLeft.size())
-						{
-							ROS_WARN("No left descriptors extracted!");
-							return;
-						}
-					}
-				}
-
 				//
 				UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
-				UDEBUG("kptsLeft=%d descLeft=%d", (int)kptsLeft.size(), descLeft.rows);
 				rtabmap::SensorData data(ptrImageLeft->image,
-						depthOrRightImage,
+						ptrImageRight->image,
 						fx,
-						generateDepth_?fy:baseline,
+						baseline,
 						cx,
 						cy,
 						rtabmap::Transform(),
 						rtabmap::transformFromTF(localTransform));
-				data.setFeatures(kptsLeft, descLeft);
 				quality=0;
 
 				this->processData(data, imageRectLeft->header, quality);
 				UDEBUG("time odometry->process()=%fs", stepTimer.ticks());
 
-				if(generateDepth_ && odomDepth_.getNumSubscribers())
-				{
-					cv_bridge::CvImage img;
-					img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-					img.image = depthOrRightImage;
-					sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
-					rosMsg->header= imageRectLeft->header;
-					odomDepth_.publish(rosMsg);
-				}
-
 				//ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added/matched/extracted %d/%d/%d",
 				//	quality, (ros::WallTime::now()-time).toSec(),
 				//	stereoFeaturesAdded, stereoFeaturesMatched, stereoFeaturesExtracted);
@@ -379,22 +205,6 @@ private:
 	Feature2D * feature2D_;
 	std::string roiRatios_;
 
-	// ROS parameters
-	int depthPatchSize_;
-
-	bool generateDepth_;
-
-	int stereoFlowWinSize_;
-	int stereoFlowIterations_;
-	double stereoFlowEpsilon_;
-	int stereoFlowMaxLevel_;
-
-	int stereoSubPixWinSize_;
-	int stereoSubPixIterations_;
-	double stereoSubPixEps_;
-
-	ros::Publisher odomDepth_;
-
 	image_transport::SubscriberFilter imageRectLeft_;
 	image_transport::SubscriberFilter imageRectRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;

src/rviz/MapCloudDisplay.cpp

@@ -308,8 +308,15 @@ void MapCloudDisplay::processMapData(const rtabmap::MapData& map)
 
 				if(!image.empty() && !depth.empty() && depthFx > 0.0f && depthFy > 0.0f && depthCx >= 0.0f && depthCy >= 0.0f)
 				{
-					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
-
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+					if(depth.type() == CV_8UC1)
+					{
+						cloud = util3d::cloudFromStereoImages(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
+					}
+					else
+					{
+						cloud = util3d::cloudFromDepthRGB(image, depth, depthCx, depthCy, depthFx, depthFy, cloud_decimation_->getInt());
+					}
 					if(cloud_max_depth_->getFloat() > 0.0f)
 					{
 						cloud = util3d::passThrough(cloud, "z", 0, cloud_max_depth_->getFloat());