ros-pkg:

-visual_odometry node is now called rgbd_odometry
-added stereo_odometry node with test launch files
-updated launch files accordingly to modified parameter names, default values or new parameters

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

src/CoreNode.cpp

@@ -69,8 +69,7 @@ int main(int argc, char** argv)
 				// hide specific parameters
 				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
 				{
-					if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
-					   uSplit(iter->first, '/').front().compare("OdomICP") == 0)
+					if(iter->first.find("Odom") == 0)
 					{
 						parameters.erase(iter++);
 					}

src/CoreWrapper.cpp

@@ -394,20 +394,22 @@ void CoreWrapper::depthCallback(
 		}
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthFx = cameraInfoMsg->K[0];
-		float depthFy = cameraInfoMsg->K[4];
-		float depthCx = cameraInfoMsg->K[2];
-		float depthCy = cameraInfoMsg->K[5];
+		image_geometry::PinholeCameraModel model;
+		model.fromCameraInfo(*cameraInfoMsg);
+		float fx = model.fx();
+		float fy = model.fy();
+		float cx = model.cx();
+		float cy = model.cy();
 
 		process(ptrImage->header.seq,
 				ptrImage->image,
 				odom,
 				odomMsg->header.frame_id,
 				ptrDepth->image,
-				depthFx,
-				depthFy,
-				depthCx,
-				depthCy,
+				fx,
+				fy,
+				cx,
+				cy,
 				localTransform,
 				cv::Mat());
 	}
@@ -479,20 +481,22 @@ void CoreWrapper::depthScanCallback(
 		}
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthFx = cameraInfoMsg->K[0];
-		float depthFy = cameraInfoMsg->K[4];
-		float depthCx = cameraInfoMsg->K[2];
-		float depthCy = cameraInfoMsg->K[5];
+		image_geometry::PinholeCameraModel model;
+		model.fromCameraInfo(*cameraInfoMsg);
+		float fx = model.fx();
+		float fy = model.fy();
+		float cx = model.cx();
+		float cy = model.cy();
 
 		process(ptrImage->header.seq,
 				ptrImage->image,
 				odom,
 				odomMsg->header.frame_id,
 				ptrDepth->image,
-				depthFx,
-				depthFy,
-				depthCx,
-				depthCy,
+				fx,
+				fy,
+				cx,
+				cy,
 				localTransform,
 				scan);
 	}
@@ -504,10 +508,10 @@ void CoreWrapper::process(
 		const Transform & odom,
 		const std::string & odomFrameId,
 		const cv::Mat & depth,
-		float depthFx,
-		float depthFy,
-		float depthCx,
-		float depthCy,
+		float fx,
+		float fy,
+		float cx,
+		float cy,
 		const Transform & localTransform,
 		const cv::Mat & scan)
 {
@@ -543,10 +547,10 @@ void CoreWrapper::process(
 		SensorData data(image.clone(),
 				depth16,
 				scan,
-				depthFx,
-				depthFy,
-				depthCx,
-				depthCy,
+				fx,
+				fy,
+				cx,
+				cy,
 				odom,
 				localTransform,
 				id);

src/CoreWrapper.h

@@ -86,10 +86,10 @@ private:
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
 			const cv::Mat & depth = cv::Mat(),
-			float depthFx = 0.0f,
-			float depthFy = 0.0f,
-			float depthCx = 0.0f,
-			float depthCy = 0.0f,
+			float fx = 0.0f,
+			float fy = 0.0f,
+			float cx = 0.0f,
+			float cy = 0.0f,
 			const rtabmap::Transform & localTransform = rtabmap::Transform(),
 			const cv::Mat & scan = cv::Mat());
 

src/DataRecorderNode.cpp

@@ -42,10 +42,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
 #include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/sync_policies/exact_time.h>
 
 #include <image_transport/image_transport.h>
 #include <image_transport/subscriber_filter.h>
 
+#include <image_geometry/stereo_camera_model.h>
+
 #include "rtabmap/MsgConversion.h"
 
 #include <pcl_ros/transforms.h>
@@ -77,15 +80,18 @@ public:
 		bool subscribeOdometry = false;
 		bool subscribeLaserScan = false;
 		bool subscribeDepth = false;
+		bool subscribeStereo = false;
 		int queueSize = 10;
+		bool showGUI = true;
 		pnh.param("subscribe_odometry", subscribeOdometry, subscribeOdometry);
 		pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
+		pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo);
 		pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("output_file_name", fileName_, fileName_);
 		pnh.param("frame_id", frameId_, frameId_);
 
-		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeLaserScan, queueSize);
+		setupCallbacks(subscribeOdometry, subscribeDepth, subscribeStereo, subscribeLaserScan, queueSize);
 	}
 	bool init()
 	{
@@ -108,70 +114,105 @@ private:
 	void setupCallbacks(
 			bool subscribeOdom,
 			bool subscribeDepth,
+			bool subscribeStereo,
 			bool subscribeLaserScan,
 			int queueSize)
 	{
-		ros::NodeHandle nh; // public
-		ros::NodeHandle pnh("~"); // private
-		ros::NodeHandle rgb_nh(nh, "rgb");
-		ros::NodeHandle depth_nh(nh, "depth");
-		ros::NodeHandle rgb_pnh(pnh, "rgb");
-		ros::NodeHandle depth_pnh(pnh, "depth");
-		image_transport::ImageTransport rgb_it(rgb_nh);
-		image_transport::ImageTransport depth_it(depth_nh);
-		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+		if(subscribeStereo)
+		{
+			ros::NodeHandle nh; // public
+			ros::NodeHandle pnh("~"); // private
+			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);
 
-		if(subscribeOdom && subscribeDepth && subscribeLaserScan)
-		{
-			ROS_INFO("Registering Depth+LaserScan callback...");
-			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-			imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-			cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-			odomSub_.subscribe(nh, "odom", 1);
-			scanSub_.subscribe(nh, "scan", 1);
-			depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
-			depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
-		}
-		else if(subscribeOdom && subscribeDepth && !subscribeLaserScan)
-		{
-			ROS_INFO("Registering Depth callback...");
-			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-			imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-			cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
-			odomSub_.subscribe(nh, "odom", 1);
-			depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
-			depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
-		}
-		else if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
-		{
-			ROS_INFO("Registering LaserScan callback...");
-			imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-			odomSub_.subscribe(nh, "odom", 1);
-			scanSub_.subscribe(nh, "scan", 1);
-			scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
-			scanSync_->registerCallback(boost::bind(&DataRecorderWrapper::scanCallback, this, _1, _2, _3));
-		}
-		else if(!subscribeOdom && subscribeDepth)
-		{
-			ROS_INFO("Registering to depth without odometry 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);
-			depthImageSync_ = new message_filters::Synchronizer<MyDepthImageSyncPolicy>(MyDepthImageSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
-			depthImageSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
+			imageSub_.subscribe(left_it, left_nh.resolveName("image_rect"), 1, hintsLeft);
+			imageRightSub_.subscribe(right_it, right_nh.resolveName("image_rect"), 1, hintsRight);
+			cameraInfoSub_.subscribe(left_nh, "camera_info", 1);
+			cameraInfoRightSub_.subscribe(right_nh, "camera_info", 1);
+			if(subscribeOdom)
+			{
+				ROS_INFO("Registering Stero+Odom callback...");
+				odomSub_.subscribe(nh, "odom", 1);
+				stereoOdomSync_ = new message_filters::Synchronizer<MyStereoOdomSyncPolicy>(MyStereoOdomSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_, odomSub_);
+				stereoOdomSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoOdomCallback, this, _1, _2, _3, _4, _5));
+			}
+			else
+			{
+				ROS_INFO("Registering Stero callback...");
+				stereoSync_ = new message_filters::Synchronizer<MyStereoSyncPolicy>(MyStereoSyncPolicy(queueSize), imageSub_, imageRightSub_, cameraInfoSub_, cameraInfoRightSub_);
+				stereoSync_->registerCallback(boost::bind(&DataRecorderWrapper::stereoCallback, this, _1, _2, _3, _4));
+			}
 		}
 		else
 		{
-			ROS_INFO("Registering default callback...");
-			defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
+			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(subscribeOdom && subscribeDepth && subscribeLaserScan)
+			{
+				ROS_INFO("Registering Depth+LaserScan callback...");
+				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+				imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+				cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+				odomSub_.subscribe(nh, "odom", 1);
+				scanSub_.subscribe(nh, "scan", 1);
+				depthScanSync_ = new message_filters::Synchronizer<MyDepthScanSyncPolicy>(MyDepthScanSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_, scanSub_);
+				depthScanSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthScanCallback, this, _1, _2, _3, _4, _5));
+			}
+			else if(subscribeOdom && subscribeDepth && !subscribeLaserScan)
+			{
+				ROS_INFO("Registering Depth callback...");
+				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+				imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+				cameraInfoSub_.subscribe(rgb_nh, "camera_info", 1);
+				odomSub_.subscribe(nh, "odom", 1);
+				depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
+				depthSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthCallback, this, _1, _2, _3, _4));
+			}
+			else if(subscribeOdom && !subscribeDepth && subscribeLaserScan)
+			{
+				ROS_INFO("Registering LaserScan callback...");
+				imageSub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+				odomSub_.subscribe(nh, "odom", 1);
+				scanSub_.subscribe(nh, "scan", 1);
+				scanSync_ = new message_filters::Synchronizer<MyScanSyncPolicy>(MyScanSyncPolicy(queueSize), imageSub_, odomSub_, scanSub_);
+				scanSync_->registerCallback(boost::bind(&DataRecorderWrapper::scanCallback, this, _1, _2, _3));
+			}
+			else if(!subscribeOdom && subscribeDepth)
+			{
+				ROS_INFO("Registering to depth without odometry 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);
+				depthImageSync_ = new message_filters::Synchronizer<MyDepthImageSyncPolicy>(MyDepthImageSyncPolicy(queueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
+				depthImageSync_->registerCallback(boost::bind(&DataRecorderWrapper::depthImageCallback, this, _1, _2, _3));
+			}
+			else
+			{
+				ROS_INFO("Registering default callback...");
+				defaultSub_ = rgb_it.subscribe("image", 1, &DataRecorderWrapper::defaultCallback, this);
+			}
 		}
 	}
 
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 	{
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		rtabmap::SensorData image(
+		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			cv::Mat(),
 			cv::Mat(),
@@ -181,7 +222,7 @@ private:
 			0.0f,
 			Transform(),
 			Transform());
-		recorder_.addData(image);
+		recorder_.addData(data);
 	}
 
 	void depthImageCallback(
@@ -206,10 +247,12 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthFx = cameraInfoMsg->K[0];
-		float depthFy = cameraInfoMsg->K[4];
-		float depthCx = cameraInfoMsg->K[2];
-		float depthCy = cameraInfoMsg->K[5];
+		image_geometry::PinholeCameraModel model;
+		model.fromCameraInfo(*cameraInfoMsg);
+		float fx = model.fx();
+		float fy = model.fy();
+		float cx = model.cx();
+		float cy = model.cy();
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -234,20 +277,20 @@ private:
 		}
 		else
 		{
-			depth16 = ptrDepth->image;
+			depth16 = ptrDepth->image.clone();
 		}
 
-		rtabmap::SensorData image(
+		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
-			depthFx,
-			depthFy,
-			depthCx,
-			depthCy,
+			fx,
+			fy,
+			cx,
+			cy,
 			Transform(),
 			localTransform);
-		recorder_.addData(image);
+		recorder_.addData(data);
 	}
 
 	void depthCallback(
@@ -275,10 +318,12 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthFx = cameraInfoMsg->K[0];
-		float depthFy = cameraInfoMsg->K[4];
-		float depthCx = cameraInfoMsg->K[2];
-		float depthCy = cameraInfoMsg->K[5];
+		image_geometry::PinholeCameraModel model;
+		model.fromCameraInfo(*cameraInfoMsg);
+		float fx = model.fx();
+		float fy = model.fy();
+		float cx = model.cx();
+		float cy = model.cy();
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -303,20 +348,20 @@ private:
 		}
 		else
 		{
-			depth16 = ptrDepth->image;
+			depth16 = ptrDepth->image.clone();
 		}
 
-		rtabmap::SensorData image(
+		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
-			depthFx,
-			depthFy,
-			depthCx,
-			depthCy,
+			fx,
+			fy,
+			cx,
+			cy,
 			odom,
 			localTransform);
-		recorder_.addData(image);
+		recorder_.addData(data);
 	}
 
 	void scanCallback(
@@ -348,7 +393,7 @@ private:
 
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 
-		rtabmap::SensorData image(
+		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			cv::Mat(),
 			scan,
@@ -358,7 +403,7 @@ private:
 			0.0f,
 			odom,
 			Transform());
-		recorder_.addData(image);
+		recorder_.addData(data);
 
 	}
 
@@ -397,10 +442,12 @@ private:
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 		cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-		float depthFx = cameraInfoMsg->K[0];
-		float depthFy = cameraInfoMsg->K[4];
-		float depthCx = cameraInfoMsg->K[2];
-		float depthCy = cameraInfoMsg->K[5];
+		image_geometry::PinholeCameraModel model;
+		model.fromCameraInfo(*cameraInfoMsg);
+		float fx = model.fx();
+		float fy = model.fy();
+		float cx = model.cx();
+		float cy = model.cy();
 
 		cv::Mat depth16;
 		if(ptrDepth->image.type() != CV_16UC1)
@@ -425,20 +472,109 @@ private:
 		}
 		else
 		{
-			depth16 = ptrDepth->image;
+			depth16 = ptrDepth->image.clone();
 		}
 
-		rtabmap::SensorData image(
+		rtabmap::SensorData data(
 			ptrImage->image.clone(),
 			depth16,
 			scan,
-			depthFx,
-			depthFy,
-			depthCx,
-			depthCy,
+			fx,
+			fy,
+			cx,
+			cy,
 			odom,
 			localTransform);
-		recorder_.addData(image);
+		recorder_.addData(data);
+	}
+
+	void stereoOdomCallback(
+				const sensor_msgs::ImageConstPtr& leftImageMsg,
+				const sensor_msgs::ImageConstPtr& rightImageMsg,
+				const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+				const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg,
+				const nav_msgs::OdometryConstPtr & odomMsg)
+	{
+		// 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 = cv_bridge::toCvShare(leftImageMsg, "bgr8");
+		cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
+
+		image_geometry::StereoCameraModel model;
+		model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
+		float fx = model.right().fx();
+		float baseline = model.baseline();
+		float cx = model.right().cx();
+		float cy = model.right().cy();
+
+		rtabmap::SensorData data(
+				ptrLeftImage->image.clone(),
+				ptrRightImage->image.clone(),
+				cv::Mat(),
+				fx,
+				baseline,
+				cx,
+				cy,
+				odom,
+				localTransform);
+		recorder_.addData(data);
+	}
+
+	void stereoCallback(
+			const sensor_msgs::ImageConstPtr& leftImageMsg,
+			const sensor_msgs::ImageConstPtr& rightImageMsg,
+			const sensor_msgs::CameraInfoConstPtr& leftCameraInfoMsg,
+			const sensor_msgs::CameraInfoConstPtr& rightCameraInfoMsg)
+	{
+		// 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;
+		}
+
+		cv_bridge::CvImageConstPtr ptrLeftImage = cv_bridge::toCvShare(leftImageMsg, "bgr8");
+		cv_bridge::CvImageConstPtr ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
+
+		image_geometry::StereoCameraModel model;
+		model.fromCameraInfo(*leftCameraInfoMsg, *rightCameraInfoMsg);
+		float fx = model.right().fx();
+		float baseline = model.baseline();
+		float cx = model.right().cx();
+		float cy = model.right().cy();
+
+		rtabmap::SensorData data(
+				ptrLeftImage->image.clone(),
+				ptrRightImage->image.clone(),
+				cv::Mat(),
+				fx,
+				baseline,
+				cx,
+				cy,
+				Transform(),
+				localTransform);
+		recorder_.addData(data);
 	}
 
 private:
@@ -449,7 +585,9 @@ private:
 	image_transport::Subscriber defaultSub_;
 	image_transport::SubscriberFilter imageSub_;
 	image_transport::SubscriberFilter imageDepthSub_;
+	image_transport::SubscriberFilter imageRightSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRightSub_;
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 
@@ -481,6 +619,23 @@ private:
 			sensor_msgs::CameraInfo> MyDepthImageSyncPolicy;
 	message_filters::Synchronizer<MyDepthImageSyncPolicy> * depthImageSync_;
 
+	//stereo with odometry
+	typedef message_filters::sync_policies::ApproximateTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo,
+			nav_msgs::Odometry> MyStereoOdomSyncPolicy;
+	message_filters::Synchronizer<MyStereoOdomSyncPolicy> * stereoOdomSync_;
+
+	//stereo without odometry
+	typedef message_filters::sync_policies::ExactTime<
+			sensor_msgs::Image,
+			sensor_msgs::Image,
+			sensor_msgs::CameraInfo,
+			sensor_msgs::CameraInfo> MyStereoSyncPolicy;
+	message_filters::Synchronizer<MyStereoSyncPolicy> * stereoSync_;
+
 	tf::TransformListener tfListener_;
 };
 

src/GuiWrapper.cpp

@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <opencv2/highgui/highgui.hpp>
 
+#include <image_geometry/pinhole_camera_model.h>
+
 #include <rtabmap/gui/MainWindow.h>
 #include <rtabmap/core/RtabmapEvent.h>
 #include <rtabmap/core/Parameters.h>
@@ -581,18 +583,20 @@ void GuiWrapper::depthCallback(
 	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-	float depthFx = cameraInfoMsg->K[0];
-	float depthFy = cameraInfoMsg->K[4];
-	float depthCx = cameraInfoMsg->K[2];
-	float depthCy = cameraInfoMsg->K[5];
+	image_geometry::PinholeCameraModel model;
+	model.fromCameraInfo(*cameraInfoMsg);
+	float fx = model.fx();
+	float fy = model.fy();
+	float cx = model.cx();
+	float cy = model.cy();
 
 	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
-			depthFx,
-			depthFy,
-			depthCx,
-			depthCy,
+			fx,
+			fy,
+			cx,
+			cy,
 			odom,
 			localTransform);
 	this->post(new OdometryEvent(image));
@@ -675,19 +679,21 @@ void GuiWrapper::depthScanCallback(
 	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 	cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depthMsg);
 
-	float depthFx = cameraInfoMsg->K[0];
-	float depthFy = cameraInfoMsg->K[4];
-	float depthCx = cameraInfoMsg->K[2];
-	float depthCy = cameraInfoMsg->K[5];
+	image_geometry::PinholeCameraModel model;
+	model.fromCameraInfo(*cameraInfoMsg);
+	float fx = model.fx();
+	float fy = model.fy();
+	float cx = model.cx();
+	float cy = model.cy();
 
 	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
 			scan,
-			depthFx,
-			depthFy,
-			depthCx,
-			depthCy,
+			fx,
+			fy,
+			cx,
+			cy,
 			odom,
 			localTransform);
 	this->post(new OdometryEvent(image));

src/OdometryROS.cpp

@@ -0,0 +1,412 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "OdometryROS.h"
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <nav_msgs/Odometry.h>
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+#include <rtabmap/core/Odometry.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/Signature.h>
+#include "rtabmap/MsgConversion.h"
+#include "rtabmap/utilite/UConversion.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UStl.h"
+
+namespace rtabmap {
+
+OdometryROS::OdometryROS(int argc, char * argv[]) :
+	odometry_(0),
+	frameId_("base_link"),
+	odomFrameId_("odom"),
+	publishTf_(true),
+	paused_(false)
+{
+	this->processArguments(argc, argv);
+
+	ros::NodeHandle nh;
+
+	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("~");
+
+	pnh.param("frame_id", frameId_, frameId_);
+	pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
+	pnh.param("publish_tf", publishTf_, publishTf_);
+
+	//parameters
+	rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+	for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(uStrContains(group, "Odom") ||
+			group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			parameters_.insert(*iter);
+		}
+	}
+
+	for(rtabmap::ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+	{
+		std::string vStr;
+		bool vBool;
+		int vInt;
+		double vDouble;
+		if(pnh.getParam(iter->first, vStr))
+		{
+			ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
+			iter->second = vStr;
+		}
+		else if(pnh.getParam(iter->first, vBool))
+		{
+			ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
+			iter->second = uBool2Str(vBool);
+		}
+		else if(pnh.getParam(iter->first, vInt))
+		{
+			ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
+			iter->second = uNumber2Str(vInt);
+
+			if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
+			{
+				ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
+				iter->second = uNumber2Str(8);
+			}
+		}
+		else if(pnh.getParam(iter->first, vDouble))
+		{
+			ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
+			iter->second = uNumber2Str(vDouble);
+		}
+	}
+
+	// Backward compatibility
+	std::list<std::string> oldParameterNames;
+	oldParameterNames.push_back("Odom/Type");
+	oldParameterNames.push_back("Odom/MaxWords");
+	oldParameterNames.push_back("Odom/WordsRatio");
+	oldParameterNames.push_back("Odom/LocalHistory");
+	oldParameterNames.push_back("Odom/NearestNeighbor");
+	oldParameterNames.push_back("Odom/NNDR");
+	for(std::list<std::string>::iterator iter=oldParameterNames.begin(); iter!=oldParameterNames.end(); ++iter)
+	{
+		std::string vStr;
+		if(pnh.getParam(*iter, vStr))
+		{
+			if(iter->compare("Odom/Type") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/Type -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomFeatureType().c_str());
+				parameters_.at(Parameters::kOdomFeatureType())= vStr;
+			}
+			else if(iter->compare("Odom/MaxWords") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/MaxWords -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomMaxFeatures().c_str());
+				parameters_.at(Parameters::kOdomMaxFeatures())= vStr;
+			}
+			else if(iter->compare("Odom/LocalHistory") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/LocalHistory -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomBowLocalHistorySize().c_str());
+				parameters_.at(Parameters::kOdomBowLocalHistorySize())= vStr;
+			}
+			else if(iter->compare("Odom/NearestNeighbor") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/NearestNeighbor -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomBowNNType().c_str());
+				parameters_.at(Parameters::kOdomBowNNType())= vStr;
+			}
+			else if(iter->compare("Odom/WordsRatio") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/WordsRatio -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomFeaturesRatio().c_str());
+				parameters_.at(Parameters::kOdomFeaturesRatio())= vStr;
+			}
+			else if(iter->compare("Odom/NNDR") == 0)
+			{
+				ROS_WARN("Parameter name changed: Odom/NNDR -> %s. Please update your launch file accordingly.",
+						Parameters::kOdomBowNNDR().c_str());
+				parameters_.at(Parameters::kOdomBowNNDR())= vStr;
+			}
+		}
+	}
+
+	int odomStrategy = 0; // BOW
+	Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
+	if(odomStrategy == 1)
+	{
+		ROS_INFO("Using OdometryOpticalFlow");
+		odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
+	}
+	else
+	{
+		ROS_INFO("Using OdometryBOW");
+		odometry_ = new rtabmap::OdometryBOW(parameters_);
+	}
+
+	resetSrv_ = nh.advertiseService("reset_odom", &OdometryROS::reset, this);
+	pauseSrv_ = nh.advertiseService("pause_odom", &OdometryROS::pause, this);
+	resumeSrv_ = nh.advertiseService("resume_odom", &OdometryROS::resume, this);
+}
+
+OdometryROS::~OdometryROS()
+{
+	ros::NodeHandle pnh("~");
+	for(ParametersMap::iterator iter=parameters_.begin(); iter!=parameters_.end(); ++iter)
+	{
+		pnh.deleteParam(iter->first);
+	}
+
+	delete odometry_;
+}
+
+void OdometryROS::processArguments(int argc, char * argv[])
+{
+	for(int i=1;i<argc;++i)
+	{
+		if(strcmp(argv[i], "--params") == 0)
+		{
+			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
+			rtabmap::ParametersMap parametersOdom;
+			if(strcmp(argv[i], "--params") == 0)
+			{
+				// show specific parameters
+				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+				{
+					if(iter->first.find("Odom") == 0 ||
+						uSplit(iter->first, '/').front().compare("SURF") == 0 ||
+						uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
+						uSplit(iter->first, '/').front().compare("ORB") == 0 ||
+						uSplit(iter->first, '/').front().compare("FAST") == 0 ||
+						uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
+						uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
+						uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
+						uSplit(iter->first, '/').front().compare("BRISK") == 0)
+					{
+						parametersOdom.insert(*iter);
+					}
+				}
+			}
+
+			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
+			{
+				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
+				std::cout <<
+						str <<
+						std::setw(60 - str.size()) <<
+						" [" <<
+						rtabmap::Parameters::getDescription(iter->first).c_str() <<
+						"]" <<
+						std::endl;
+			}
+			ROS_WARN("Node will now exit after showing default odometry parameters because "
+					 "argument \"--params\" is detected!");
+			exit(0);
+		}
+	}
+}
+
+Transform OdometryROS::processData(SensorData & data, const std_msgs::Header & header, int & quality)
+{
+	rtabmap::Transform pose = odometry_->process(data, &quality);
+	if(!pose.isNull())
+	{
+		//*********************
+		// Update odometry
+		//*********************
+		tf::Transform poseTF;
+		rtabmap::transformToTF(pose, poseTF);
+
+		if(publishTf_)
+		{
+			tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, header.stamp, odomFrameId_, frameId_));
+		}
+
+		if(odomPub_.getNumSubscribers())
+		{
+			//next, we'll publish the odometry message over ROS
+			nav_msgs::Odometry odom;
+			odom.header.stamp = header.stamp; // use corresponding time stamp to image
+			odom.header.frame_id = odomFrameId_;
+			odom.child_frame_id = frameId_;
+
+			//set the position
+			odom.pose.pose.position.x = poseTF.getOrigin().x();
+			odom.pose.pose.position.y = poseTF.getOrigin().y();
+			odom.pose.pose.position.z = poseTF.getOrigin().z();
+			tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
+
+			//publish the message
+			odomPub_.publish(odom);
+		}
+
+		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
+		{
+			const std::multimap<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
+			pcl::PointCloud<pcl::PointXYZ> cloud;
+			for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
+			{
+				cloud.push_back(iter->second);
+			}
+			sensor_msgs::PointCloud2 cloudMsg;
+			pcl::toROSMsg(cloud, cloudMsg);
+			cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+			cloudMsg.header.frame_id = odomFrameId_;
+			odomLocalMap_.publish(cloudMsg);
+		}
+
+		if(odomLastFrame_.getNumSubscribers())
+		{
+			if(dynamic_cast<OdometryBOW*>(odometry_))
+			{
+				const rtabmap::Signature * s  = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
+				if(s)
+				{
+					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
+						pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
+						cloud.push_back(pt);
+					}
+
+					sensor_msgs::PointCloud2 cloudMsg;
+					pcl::toROSMsg(cloud, cloudMsg);
+					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+					cloudMsg.header.frame_id = odomFrameId_;
+					odomLastFrame_.publish(cloudMsg);
+				}
+			}
+			else
+			{
+				//Optical flow
+				const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
+				if(cloud->size())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
+					cloudTransformed = util3d::transformPointCloud(cloud, pose);
+					sensor_msgs::PointCloud2 cloudMsg;
+					pcl::toROSMsg(*cloudTransformed, cloudMsg);
+					cloudMsg.header.stamp = header.stamp; // use corresponding time stamp to image
+					cloudMsg.header.frame_id = odomFrameId_;
+					odomLastFrame_.publish(cloudMsg);
+				}
+			}
+		}
+	}
+	else
+	{
+		//ROS_WARN("Odometry lost!");
+
+		//send null pose to notify that odometry is lost
+		nav_msgs::Odometry odom;
+		odom.header.stamp = header.stamp; // use corresponding time stamp to image
+		odom.header.frame_id = odomFrameId_;
+		odom.child_frame_id = frameId_;
+
+		//publish the message
+		odomPub_.publish(odom);
+	}
+
+	if(odomMatches_.getNumSubscribers())
+	{
+		if(dynamic_cast<OdometryOpticalFlow*>(odometry_))
+		{
+			cv_bridge::CvImage img;
+			img.encoding = sensor_msgs::image_encodings::BGR8;
+			img.image = ((OdometryOpticalFlow*)odometry_)->imgMatches_;
+			sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
+			rosMsg->header= header;
+			odomMatches_.publish(rosMsg);
+		}
+	}
+
+	return pose;
+}
+
+bool OdometryROS::isOdometryBOW() const
+{
+	return dynamic_cast<OdometryBOW*>(odometry_) != 0;
+}
+
+bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	ROS_INFO("visual_odometry: reset odom!");
+	odometry_->reset();
+	return true;
+}
+
+bool OdometryROS::pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(paused_)
+	{
+		ROS_WARN("visual_odometry: Already paused!");
+	}
+	else
+	{
+		paused_ = true;
+		ROS_INFO("visual_odometry: paused!");
+	}
+	return true;
+}
+
+bool OdometryROS::resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+	if(!paused_)
+	{
+		ROS_WARN("visual_odometry: Already running!");
+	}
+	else
+	{
+		paused_ = false;
+		ROS_INFO("visual_odometry: resumed!");
+	}
+	return true;
+}
+
+}

src/OdometryROS.h

@@ -0,0 +1,90 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef ODOMETRYROS_H_
+#define ODOMETRYROS_H_
+
+#include <ros/ros.h>
+
+#include <tf/tf.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_listener.h>
+
+#include <std_srvs/Empty.h>
+#include <std_msgs/Header.h>
+
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Parameters.h>
+
+namespace rtabmap {
+class Odometry;
+
+class OdometryROS
+{
+public:
+	OdometryROS(int argc, char * argv[]);
+	~OdometryROS();
+
+	void processArguments(int argc, char * argv[]);
+	Transform processData(SensorData & data, const std_msgs::Header & header, int & quality);
+
+	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
+
+	const std::string & frameId() const {return frameId_;}
+	const std::string & odomFrameId() const {return odomFrameId_;}
+	const ParametersMap & parameters() const {return parameters_;}
+	const tf::TransformListener & tfListener() const {return tfListener_;}
+	bool isPaused() const {return paused_;}
+	bool isOdometryBOW() const;
+
+private:
+	rtabmap::Odometry * odometry_;
+
+	// parameters
+	std::string frameId_;
+	std::string odomFrameId_;
+	bool publishTf_;
+	ParametersMap parameters_;
+
+	ros::Publisher odomPub_;
+	ros::Publisher odomLocalMap_;
+	ros::Publisher odomLastFrame_;
+	ros::Publisher odomMatches_;
+	ros::ServiceServer resetSrv_;
+	ros::ServiceServer pauseSrv_;
+	ros::ServiceServer resumeSrv_;
+	tf::TransformBroadcaster tfBroadcaster_;
+	tf::TransformListener tfListener_;
+
+	bool paused_;
+};
+
+}
+
+#endif

src/RGBDOdometryNode.cpp

@@ -0,0 +1,169 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "OdometryROS.h"
+
+#include <message_filters/subscriber.h>
+#include <message_filters/time_synchronizer.h>
+#include <message_filters/sync_policies/approximate_time.h>
+
+#include <image_transport/image_transport.h>
+#include <image_transport/subscriber_filter.h>
+
+#include <image_geometry/stereo_camera_model.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <cv_bridge/cv_bridge.h>
+
+#include "rtabmap/MsgConversion.h"
+
+#include <rtabmap/utilite/ULogger.h>
+
+using namespace rtabmap;
+
+class RGBDOdometry : public OdometryROS
+{
+public:
+	RGBDOdometry(int argc, char * argv[]) :
+		OdometryROS(argc, argv),
+		sync_(0)
+	{
+		ros::NodeHandle nh;
+		ros::NodeHandle pnh("~");
+
+		int queueSize = 5;
+		pnh.param("queue_size", queueSize, queueSize);
+
+		ros::NodeHandle rgb_nh(nh, "rgb");
+		ros::NodeHandle depth_nh(nh, "depth");
+		ros::NodeHandle rgb_pnh(pnh, "rgb");
+		ros::NodeHandle depth_pnh(pnh, "depth");
+		image_transport::ImageTransport rgb_it(rgb_nh);
+		image_transport::ImageTransport depth_it(depth_nh);
+		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
+		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
+
+		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
+		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
+		info_sub_.subscribe(rgb_nh, "camera_info", 1);
+
+		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
+		sync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
+	}
+
+	~RGBDOdometry()
+	{
+		delete sync_;
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+			const sensor_msgs::ImageConstPtr& depth,
+			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
+	{
+		if(!this->isPaused())
+		{
+			if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
+				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
+			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
+				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
+			{
+				ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
+				return;
+			}
+			else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
+			{
+				static bool warned = false;
+				if(!warned)
+				{
+					ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
+							 "will be processed anyway but with a conversion. This warning is only be printed once...");
+					warned = true;
+				}
+			}
+
+			tf::StampedTransform localTransform;
+			try
+			{
+				this->tfListener().lookupTransform(this->frameId(), image->header.frame_id, image->header.stamp, localTransform);
+			}
+			catch(tf::TransformException & ex)
+			{
+				ROS_WARN("%s",ex.what());
+				return;
+			}
+
+			ros::WallTime time = ros::WallTime::now();
+
+			int quality = -1;
+			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
+			{
+				image_geometry::PinholeCameraModel model;
+				model.fromCameraInfo(*cameraInfo);
+				float fx = model.fx();
+				float fy = model.fy();
+				float cx = model.cx();
+				float cy = model.cy();
+				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::Transform(),
+						rtabmap::transformFromTF(localTransform));
+				quality=0;
+
+				this->processData(data, image->header, quality);
+			}
+			ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
+		}
+	}
+
+private:
+	image_transport::SubscriberFilter image_mono_sub_;
+	image_transport::SubscriberFilter image_depth_sub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
+	message_filters::Synchronizer<MySyncPolicy> * sync_;
+};
+
+int main(int argc, char *argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	ULogger::setLevel(ULogger::kWarning);
+	ros::init(argc, argv, "rgbd_odometry");
+
+	RGBDOdometry odom(argc, argv);
+	ros::spin();
+	return 0;
+}

src/StereoOdometryNode.cpp

@@ -25,13 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#include <ros/ros.h>
-
-#include <tf/tf.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
-
-#include <std_srvs/Empty.h>
+#include "OdometryROS.h"
 
 #include <message_filters/subscriber.h>
 #include <message_filters/time_synchronizer.h>
@@ -42,153 +36,83 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <nav_msgs/Odometry.h>
 
 #include <image_geometry/stereo_camera_model.h>
 
-#include <pcl_conversions/pcl_conversions.h>
-
 #include <cv_bridge/cv_bridge.h>
 
-#include <rtabmap/core/Odometry.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/Memory.h>
-#include <rtabmap/core/Signature.h>
+#include <opencv2/video/tracking.hpp>
+
 #include "rtabmap/MsgConversion.h"
-#include "rtabmap/utilite/UConversion.h"
-#include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UStl.h"
-#include "rtabmap/utilite/UTimer.h"
+
+#include <rtabmap/core/Features2d.h>
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UMath.h>
 
 using namespace rtabmap;
 
-class StereoOdometry
+class StereoOdometry : public OdometryROS
 {
 public:
-	StereoOdometry() :
-		odometry_(0),
+	StereoOdometry(int argc, char * argv[]) :
+		OdometryROS(argc, argv),
 		feature2D_(0),
-		frameId_("base_link"),
-		odomFrameId_("odom"),
-		publishTf_(true),
-		minDisparity_(0.0),
-		maxDisparity_(128.0),
-		k_(10),
-		winSize_(5),
-		sync_(0),
-		paused_(false)
+		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;
 
-		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
-		odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
-		odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
 		odomDepth_ = nh.advertise<sensor_msgs::Image>("odom_depth", 1);
 
 		ros::NodeHandle pnh("~");
 
+		bool approxSync = false;
 		int queueSize = 5;
-		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
-		pnh.param("publish_tf", publishTf_, publishTf_);
+		pnh.param("approx_sync", approxSync, approxSync);
 		pnh.param("queue_size", queueSize, queueSize);
-		pnh.param("min_disparity", minDisparity_, minDisparity_);
-		pnh.param("max_disparity", maxDisparity_, maxDisparity_);
-		pnh.param("k", k_, k_);
-		pnh.param("window_size", winSize_, winSize_);
+		ROS_INFO("Approximate time sync = %s", approxSync?"true":"false");
 
-		//parameters
-		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-		rtabmap::ParametersMap parametersOdom;
-		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			std::string group = uSplit(iter->first, '/').front();
-			if(group.compare("Odom") == 0 ||
-				group.compare("SURF") == 0 ||
-				group.compare("SIFT") == 0 ||
-				group.compare("ORB") == 0 ||
-				group.compare("FAST") == 0 ||
-				group.compare("FREAK") == 0 ||
-				group.compare("BRIEF") == 0 ||
-				group.compare("GFTT") == 0 ||
-				group.compare("BRISK") == 0)
-			{
-				parametersOdom.insert(*iter);
-			}
-		}
+		pnh.param("generate_depth", generateDepth_, generateDepth_);
+		pnh.param("depth_patch_size", depthPatchSize_, depthPatchSize_);
+		ROS_INFO("Generate depth = %s", generateDepth_?"true":"false");
 
-		for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
-		{
-			std::string vStr;
-			bool vBool;
-			int vInt;
-			double vDouble;
-			if(pnh.getParam(iter->first, vStr))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
-				iter->second = vStr;
-			}
-			else if(pnh.getParam(iter->first, vBool))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
-				iter->second = uBool2Str(vBool);
-			}
-			else if(pnh.getParam(iter->first, vInt))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
-				iter->second = uNumber2Str(vInt);
+		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_);
 
-				if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
-				{
-					ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
-					iter->second = uNumber2Str(8);
-				}
-			}
-			else if(pnh.getParam(iter->first, vDouble))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
-				iter->second = uNumber2Str(vDouble);
-			}
-		}
+		pnh.param("subpix_win_size", stereoSubPixWinSize_, stereoSubPixWinSize_);
+		pnh.param("subpix_iterations", stereoSubPixIterations_, stereoSubPixIterations_);
+		pnh.param("subpix_eps", stereoSubPixEps_, stereoSubPixEps_);
 
-		odometry_ = new rtabmap::OdometryBOW(parametersOdom);
+		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;
-		if((iter=parametersOdom.find(Parameters::kOdomType())) != parametersOdom.end())
+		if((iter=this->parameters().find(Parameters::kOdomFeatureType())) != this->parameters().end())
 		{
 			detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
 		}
-		switch(detectorStrategy)
-		{
-		case Feature2D::kFeatureSift:
-			feature2D_ = new SIFT(parametersOdom);
-			break;
-		case Feature2D::kFeatureFastBrief:
-			feature2D_ = new FAST_BRIEF(parametersOdom);
-			break;
-		case Feature2D::kFeatureFastFreak:
-			feature2D_ = new FAST_FREAK(parametersOdom);
-			break;
-		case Feature2D::kFeatureOrb:
-			feature2D_ = new ORB(parametersOdom);
-			break;
-		case Feature2D::kFeatureGfttFreak:
-			feature2D_ = new GFTT_FREAK(parametersOdom);
-			break;
-		case Feature2D::kFeatureGfttBrief:
-			feature2D_ = new GFTT_BRIEF(parametersOdom);
-			break;
-		case Feature2D::kFeatureBrisk:
-			feature2D_ = new BRISK(parametersOdom);
-			break;
-		case Feature2D::kFeatureSurf:
-		default:
-			feature2D_ = new SURF(parametersOdom);
-			break;
-		}
+		feature2D_ = Feature2D::create(detectorStrategy, this->parameters());
+
+		roiRatios_ = Parameters::defaultOdomRoiRatios();
+		Parameters::parse(this->parameters(), Parameters::kOdomRoiRatios(), roiRatios_);
 
 		ros::NodeHandle left_nh(nh, "left");
 		ros::NodeHandle right_nh(nh, "right");
@@ -204,25 +128,28 @@ public:
 		cameraInfoLeft_.subscribe(left_nh, "camera_info", 1);
 		cameraInfoRight_.subscribe(right_nh, "camera_info", 1);
 
-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
-		sync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
-
-		resetSrv_ = nh.advertiseService("reset_odom", &StereoOdometry::reset, this);
-		pauseSrv_ = nh.advertiseService("pause_odom", &StereoOdometry::pause, this);
-		resumeSrv_ = nh.advertiseService("resume_odom", &StereoOdometry::resume, this);
+		if(approxSync)
+		{
+			approxSync_ = new message_filters::Synchronizer<MyApproxSyncPolicy>(MyApproxSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			approxSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
+		else
+		{
+			exactSync_ = new message_filters::Synchronizer<MyExactSyncPolicy>(MyExactSyncPolicy(queueSize), imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_);
+			exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, _1, _2, _3, _4));
+		}
 	}
 
 	~StereoOdometry()
 	{
-		ros::NodeHandle pnh("~");
-		ParametersMap parameters = Parameters::getDefaultParameters();
-		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+		if(approxSync_)
 		{
-			pnh.deleteParam(iter->first);
+			delete approxSync_;
+		}
+		if(exactSync_)
+		{
+			delete exactSync_;
 		}
-
-		delete sync_;
-		delete odometry_;
 		delete feature2D_;
 	}
 
@@ -232,7 +159,7 @@ public:
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
 			const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
 	{
-		if(!paused_)
+		if(!this->isPaused())
 		{
 			if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
 				 imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
@@ -250,7 +177,8 @@ public:
 			tf::StampedTransform localTransform;
 			try
 			{
-				tfListener_.lookupTransform(frameId_, imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
+				UDEBUG("");
+				this->tfListener().lookupTransform(this->frameId(), imageRectLeft->header.frame_id, imageRectLeft->header.stamp, localTransform);
 			}
 			catch(tf::TransformException & ex)
 			{
@@ -263,229 +191,182 @@ public:
 			int quality = -1;
 			if(imageRectLeft->data.size() && imageRectRight->data.size())
 			{
-				float depthFx = cameraInfoLeft->K[0];
-				float depthFy = cameraInfoLeft->K[4];
-				float depthCx = cameraInfoLeft->K[2];
-				float depthCy = cameraInfoLeft->K[5];
+				image_geometry::StereoCameraModel model;
+				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");
 
-				//generate depth
-				cv::Mat depth = cv::Mat::zeros(ptrImageLeft->image.rows, ptrImageLeft->image.cols, CV_32FC1);
-
+				cv::Mat depthOrRightImage;
 				std::vector<cv::KeyPoint> kptsLeft, kptsRight;
-				std::vector<cv::Point2f> cornersLeft, cornersRight;
 				cv::Mat descLeft, descRight;
-				kptsLeft = feature2D_->generateKeypoints(ptrImageLeft->image);
-				if(kptsLeft.size())
+				UTimer stepTimer;
+
+				if(!generateDepth_)
 				{
-					descLeft = feature2D_->generateDescriptors(ptrImageLeft->image, kptsLeft);
-
-					kptsRight = feature2D_->generateKeypoints(ptrImageRight->image);
-					if(kptsRight.size())
-					{
-						descRight = feature2D_->generateDescriptors(ptrImageRight->image, kptsRight);
-
-						if(kptsLeft.size() && kptsRight.size())
-						{
-							cornersLeft.resize(kptsLeft.size());
-							cornersRight.resize(kptsRight.size());
-							for(unsigned int i=0; i<kptsLeft.size() || i<kptsRight.size(); ++i)
-							{
-								if(i<kptsLeft.size())
-								{
-									cornersLeft[i] = kptsLeft[i].pt;
-								}
-								if(i<kptsRight.size())
-								{
-									cornersRight[i] = kptsRight[i].pt;
-								}
-							}
-							UTimer time;
-							cv::cornerSubPix( ptrImageLeft->image, cornersLeft, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
-										  cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
-
-							cv::cornerSubPix( ptrImageRight->image, cornersRight, cv::Size( winSize_, winSize_ ), cv::Size( -1, -1 ),
-										  cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03 ) );
-							UDEBUG("time subpix = %fs", time.ticks());
-						}
-					}
-				}
-
-
-				if(kptsLeft.size() >= odometry_->getMinInliers() && kptsRight.size() >= odometry_->getMinInliers())
-				{
-					std::vector<std::vector<cv::DMatch> > matches;
-					cv::BFMatcher matcher(descLeft.depth()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2);
-					int k = std::min((int)kptsLeft.size(), k_);
-					k = std::min((int)kptsRight.size(), k);
-					matcher.knnMatch(descLeft, descRight, matches, k);
-					int added = 0;
-
-					if(matches.size())
-					{
-						image_geometry::StereoCameraModel model;
-						model.fromCameraInfo(*cameraInfoLeft, *cameraInfoRight);
-
-						int addedFirst = 0;
-						for(int i=0; i< matches.size(); ++i)
-						{
-							// add only those on same Y
-							for(unsigned int j=0; j<matches[i].size(); ++j)
-							{
-								float disparity = cornersLeft[matches[i].at(j).queryIdx].x - cornersRight[matches[i].at(j).trainIdx].x;
-
-								if((int)disparity >= minDisparity_ && (int)disparity <= maxDisparity_)
-								{
-									float d = model.getZ(disparity);
-									if(	d>0 &&
-										cornersLeft[matches[i].at(j).queryIdx].y >= cornersRight[matches[i].at(j).trainIdx].y - 3.0f &&
-										cornersLeft[matches[i].at(j).queryIdx].y <= cornersRight[matches[i].at(j).trainIdx].y + 3.0f)
-									{
-										kptsLeft[matches[i].at(j).queryIdx].pt = cornersLeft[matches[i].at(j).queryIdx];
-										depth.at<float>(int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f), int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f)) = d;
-										/*ROS_INFO("Add%d Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f, depth=%f",
-												j,
-												int(kptsLeft[matches[i].at(j).queryIdx].pt.x+0.5f),
-												int(kptsLeft[matches[i].at(j).queryIdx].pt.y+0.5f),
-												int(kptsRight[matches[i].at(j).trainIdx].pt.x+0.5f),
-												int(kptsRight[matches[i].at(j).trainIdx].pt.y+0.5f),
-												i, matches[i].at(j).distance, disparity, d);*/
-										if(j == 0)
-										{
-											++addedFirst;
-										}
-										++added;
-										break;
-									}
-									else
-									{
-										/*ROS_INFO("--- Left(%d, %d) Right(%d, %d) distance %d = %f disp=%f depth=%f",
-												int(kptsLeft[matches[i].queryIdx].pt.x+0.5f),
-												int(kptsLeft[matches[i].queryIdx].pt.y+0.5f),
-												int(kptsRight[matches[i].trainIdx].pt.x+0.5f),
-												int(kptsRight[matches[i].trainIdx].pt.y+0.5f),
-												i, matches[i].distance, disparity, d);*/
-									}
-								}
-							}
-						}
-						UDEBUG("addedFirst = %d/%d", addedFirst, added);
-
-						//
-						UDEBUG("localTransform = %s", rtabmap::transformFromTF(localTransform).prettyPrint().c_str());
-						rtabmap::SensorData data(ptrImageLeft->image,
-								depth,
-								depthFx,
-								depthFy,
-								depthCx,
-								depthCy,
-								rtabmap::Transform(),
-								rtabmap::transformFromTF(localTransform));
-						data.setFeatures(kptsLeft, descLeft);
-						quality=0;
-						rtabmap::Transform pose = odometry_->process(data, &quality);
-						if(!pose.isNull())
-						{
-							//*********************
-							// Update odometry
-							//*********************
-							tf::Transform poseTF;
-							rtabmap::transformToTF(pose, poseTF);
-
-							if(publishTf_)
-							{
-								tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, imageRectLeft->header.stamp, odomFrameId_, frameId_));
-							}
-
-							if(odomPub_.getNumSubscribers())
-							{
-								//next, we'll publish the odometry message over ROS
-								nav_msgs::Odometry odom;
-								odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
-								odom.header.frame_id = odomFrameId_;
-								odom.child_frame_id = frameId_;
-
-								//set the position
-								odom.pose.pose.position.x = poseTF.getOrigin().x();
-								odom.pose.pose.position.y = poseTF.getOrigin().y();
-								odom.pose.pose.position.z = poseTF.getOrigin().z();
-								tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
-
-								//publish the message
-								odomPub_.publish(odom);
-							}
-
-							if(odomLocalMapPub_.getNumSubscribers())
-							{
-								const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
-								pcl::PointCloud<pcl::PointXYZ> cloud;
-								for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
-								{
-									cloud.push_back(iter->second);
-								}
-								sensor_msgs::PointCloud2 cloudMsg;
-								pcl::toROSMsg(cloud, cloudMsg);
-								cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
-								cloudMsg.header.frame_id = odomFrameId_;
-								odomLocalMapPub_.publish(cloudMsg);
-							}
-
-							if(odomLastFrame_.getNumSubscribers())
-							{
-								const rtabmap::Signature * s  = odometry_->getMemory()->getLastWorkingSignature();
-								if(s)
-								{
-									const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
-									pcl::PointCloud<pcl::PointXYZ> cloud;
-									rtabmap::Transform t = rtabmap::transformFromTF(localTransform);
-									for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
-									{
-										// transform to odom frame
-										pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
-										cloud.push_back(pt);
-									}
-
-									sensor_msgs::PointCloud2 cloudMsg;
-									pcl::toROSMsg(cloud, cloudMsg);
-									cloudMsg.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
-									cloudMsg.header.frame_id = odomFrameId_;
-									odomLastFrame_.publish(cloudMsg);
-								}
-							}
-							if(odomDepth_.getNumSubscribers())
-							{
-								cv_bridge::CvImage img;
-								img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
-								img.image = depth;
-								sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
-								rosMsg->header= imageRectLeft->header;
-								odomDepth_.publish(rosMsg);
-							}
-						}
-						else
-						{
-							//ROS_WARN("Odometry lost!");
-
-							//send null pose to notify that odometry is lost
-							nav_msgs::Odometry odom;
-							odom.header.stamp = imageRectLeft->header.stamp; // use corresponding time stamp to image
-							odom.header.frame_id = odomFrameId_;
-							odom.child_frame_id = frameId_;
-
-							//publish the message
-							odomPub_.publish(odom);
-						}
-					}
-					ROS_INFO("Odom: quality=%d, update time=%fs, stereo matches: added %d/%d",
-						quality, (ros::WallTime::now()-time).toSec(),
-						added, (int)matches.size());
+					// copy right image in depth
+					depthOrRightImage = ptrImageRight->image;
 				}
 				else
 				{
-					ROS_WARN("Odom: no keypoints extracted!");
+					//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,
+						fx,
+						generateDepth_?fy: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);
+				ROS_INFO("Odom: quality=%d, update time=%fs",
+						quality, (ros::WallTime::now()-time).toSec());
 			}
 			else
 			{
@@ -494,72 +375,34 @@ public:
 		}
 	}
 
-	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		ROS_INFO("visual_odometry: reset odom!");
-		odometry_->reset();
-		return true;
-	}
-
-	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		if(paused_)
-		{
-			ROS_WARN("visual_odometry: Already paused!");
-		}
-		else
-		{
-			paused_ = true;
-			ROS_INFO("visual_odometry: paused!");
-		}
-		return true;
-	}
-
-	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		if(!paused_)
-		{
-			ROS_WARN("visual_odometry: Already running!");
-		}
-		else
-		{
-			paused_ = false;
-			ROS_INFO("visual_odometry: resumed!");
-		}
-		return true;
-	}
-
 private:
-	rtabmap::OdometryBOW * odometry_;
-	rtabmap::Feature2D * feature2D_;
+	Feature2D * feature2D_;
+	std::string roiRatios_;
 
-	// parameters
-	std::string frameId_;
-	std::string odomFrameId_;
-	bool publishTf_;
-	int minDisparity_;
-	int maxDisparity_;
-	int k_;
-	int winSize_;
+	// ROS parameters
+	int depthPatchSize_;
+
+	bool generateDepth_;
+
+	int stereoFlowWinSize_;
+	int stereoFlowIterations_;
+	double stereoFlowEpsilon_;
+	int stereoFlowMaxLevel_;
+
+	int stereoSubPixWinSize_;
+	int stereoSubPixIterations_;
+	double stereoSubPixEps_;
 
-	ros::Publisher odomPub_;
-	ros::Publisher odomLocalMapPub_;
-	ros::Publisher odomLastFrame_;
 	ros::Publisher odomDepth_;
-	ros::ServiceServer resetSrv_;
-	ros::ServiceServer pauseSrv_;
-	ros::ServiceServer resumeSrv_;
-	tf::TransformBroadcaster tfBroadcaster_;
-	tf::TransformListener tfListener_;
 
 	image_transport::SubscriberFilter imageRectLeft_;
 	image_transport::SubscriberFilter imageRectRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoLeft_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoRight_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
-
-	bool paused_;
+	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyApproxSyncPolicy;
+	message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
+	message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
 };
 
 int main(int argc, char *argv[])
@@ -567,52 +410,9 @@ int main(int argc, char *argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 	//pcl::console::setVerbosityLevel(pcl::console::L_DEBUG);
-	ros::init(argc, argv, "visual_odometry");
+	ros::init(argc, argv, "stereo_odometry");
 
-	for(int i=1;i<argc;++i)
-	{
-		if(strcmp(argv[i], "--params") == 0)
-		{
-			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-			rtabmap::ParametersMap parametersOdom;
-			if(strcmp(argv[i], "--params") == 0)
-			{
-				// show specific parameters
-				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-				{
-					if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
-						uSplit(iter->first, '/').front().compare("SURF") == 0 ||
-						uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
-						uSplit(iter->first, '/').front().compare("ORB") == 0 ||
-						uSplit(iter->first, '/').front().compare("FAST") == 0 ||
-						uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
-						uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
-						uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
-						uSplit(iter->first, '/').front().compare("BRISK") == 0)
-					{
-						parametersOdom.insert(*iter);
-					}
-				}
-			}
-
-			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
-			{
-				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
-				std::cout <<
-						str <<
-						std::setw(60 - str.size()) <<
-						" [" <<
-						rtabmap::Parameters::getDescription(iter->first).c_str() <<
-						"]" <<
-						std::endl;
-			}
-			ROS_WARN("Node will now exit after showing default odometry parameters because "
-					 "argument \"--params\" is detected!");
-			exit(0);
-		}
-	}
-
-	StereoOdometry vOdom;
+	StereoOdometry odom(argc, argv);
 	ros::spin();
 	return 0;
 }

src/VisualOdometryDEPRECATED.cpp

@@ -0,0 +1,37 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rtabmap/utilite/ULogger.h>
+
+int main(int argc, char *argv[])
+{
+	ULogger::setType(ULogger::kTypeConsole);
+	UFATAL("visual_odometry node is deprecated, use rgbd_odometry instead!");
+	return 0;
+}
+
+

src/VisualOdometryNode.cpp

@@ -1,439 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include <ros/ros.h>
-
-#include <tf/tf.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_listener.h>
-
-#include <std_srvs/Empty.h>
-
-#include <message_filters/subscriber.h>
-#include <message_filters/time_synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-
-#include <image_transport/image_transport.h>
-#include <image_transport/subscriber_filter.h>
-
-#include <sensor_msgs/Image.h>
-#include <sensor_msgs/image_encodings.h>
-#include <sensor_msgs/PointCloud2.h>
-#include <nav_msgs/Odometry.h>
-
-#include <pcl_conversions/pcl_conversions.h>
-
-#include <cv_bridge/cv_bridge.h>
-
-#include <rtabmap/core/Odometry.h>
-#include <rtabmap/core/util3d.h>
-#include <rtabmap/core/Memory.h>
-#include <rtabmap/core/Signature.h>
-#include "rtabmap/MsgConversion.h"
-#include "rtabmap/utilite/UConversion.h"
-#include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UStl.h"
-
-using namespace rtabmap;
-
-class VisualOdometry
-{
-public:
-	VisualOdometry() :
-		odometry_(0),
-		frameId_("base_link"),
-		odomFrameId_("odom"),
-		publishTf_(true),
-		sync_(0),
-		paused_(false)
-	{
-		ros::NodeHandle nh;
-
-		odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
-		odomLocalMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
-		odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
-
-		ros::NodeHandle pnh("~");
-
-		int queueSize = 5;
-		pnh.param("frame_id", frameId_, frameId_);
-		pnh.param("odom_frame_id", odomFrameId_, odomFrameId_);
-		pnh.param("publish_tf", publishTf_, publishTf_);
-		pnh.param("queue_size", queueSize, queueSize);
-
-		//parameters
-		rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-		rtabmap::ParametersMap parametersOdom;
-		for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			std::string group = uSplit(iter->first, '/').front();
-			if(group.compare("Odom") == 0 ||
-				group.compare("SURF") == 0 ||
-				group.compare("SIFT") == 0 ||
-				group.compare("ORB") == 0 ||
-				group.compare("FAST") == 0 ||
-				group.compare("FREAK") == 0 ||
-				group.compare("BRIEF") == 0 ||
-				group.compare("GFTT") == 0 ||
-				group.compare("BRISK") == 0)
-			{
-				parametersOdom.insert(*iter);
-			}
-		}
-
-		for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
-		{
-			std::string vStr;
-			bool vBool;
-			int vInt;
-			double vDouble;
-			if(pnh.getParam(iter->first, vStr))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), vStr.c_str());
-				iter->second = vStr;
-			}
-			else if(pnh.getParam(iter->first, vBool))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uBool2Str(vBool).c_str());
-				iter->second = uBool2Str(vBool);
-			}
-			else if(pnh.getParam(iter->first, vInt))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vInt).c_str());
-				iter->second = uNumber2Str(vInt);
-
-				if(iter->first.compare(Parameters::kOdomMinInliers()) == 0 && vInt < 8)
-				{
-					ROS_WARN("Parameter min_inliers must be >= 8, setting to 8...");
-					iter->second = uNumber2Str(8);
-				}
-			}
-			else if(pnh.getParam(iter->first, vDouble))
-			{
-				ROS_INFO("Setting odometry parameter \"%s\"=\"%s\"", iter->first.c_str(), uNumber2Str(vDouble).c_str());
-				iter->second = uNumber2Str(vDouble);
-			}
-		}
-
-		odometry_ = new rtabmap::OdometryBOW(parametersOdom);
-
-		ros::NodeHandle rgb_nh(nh, "rgb");
-		ros::NodeHandle depth_nh(nh, "depth");
-		ros::NodeHandle rgb_pnh(pnh, "rgb");
-		ros::NodeHandle depth_pnh(pnh, "depth");
-		image_transport::ImageTransport rgb_it(rgb_nh);
-		image_transport::ImageTransport depth_it(depth_nh);
-		image_transport::TransportHints hintsRgb("raw", ros::TransportHints(), rgb_pnh);
-		image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
-
-		image_mono_sub_.subscribe(rgb_it, rgb_nh.resolveName("image"), 1, hintsRgb);
-		image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
-		info_sub_.subscribe(rgb_nh, "camera_info", 1);
-
-		sync_ = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(queueSize), image_mono_sub_, image_depth_sub_, info_sub_);
-		sync_->registerCallback(boost::bind(&VisualOdometry::callback, this, _1, _2, _3));
-
-		resetSrv_ = nh.advertiseService("reset_odom", &VisualOdometry::reset, this);
-		pauseSrv_ = nh.advertiseService("pause_odom", &VisualOdometry::pause, this);
-		resumeSrv_ = nh.advertiseService("resume_odom", &VisualOdometry::resume, this);
-	}
-
-	~VisualOdometry()
-	{
-		ros::NodeHandle pnh("~");
-		ParametersMap parameters = Parameters::getDefaultParameters();
-		for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-		{
-			pnh.deleteParam(iter->first);
-		}
-
-		delete sync_;
-		delete odometry_;
-	}
-
-	void callback(const sensor_msgs::ImageConstPtr& image,
-			const sensor_msgs::ImageConstPtr& depth,
-			const sensor_msgs::CameraInfoConstPtr& cameraInfo)
-	{
-		if(!paused_)
-		{
-			if(!(image->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				 image->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
-			   !(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)==0 ||
-				 depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0))
-			{
-				ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8 (mono8 recommended) and image_depth=16UC1");
-				return;
-			}
-			else if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)==0)
-			{
-				static bool warned = false;
-				if(!warned)
-				{
-					ROS_WARN("Input depth type is 32FC1, please use type 16UC1 for depth. The depth images "
-							 "will be processed anyway but with a conversion. This warning is only be printed once...");
-					warned = true;
-				}
-			}
-
-			tf::StampedTransform localTransform;
-			try
-			{
-				tfListener_.lookupTransform(frameId_, image->header.frame_id, image->header.stamp, localTransform);
-			}
-			catch(tf::TransformException & ex)
-			{
-				ROS_WARN("%s",ex.what());
-				return;
-			}
-
-			ros::WallTime time = ros::WallTime::now();
-
-			int quality = -1;
-			if(image->data.size() && depth->data.size() && cameraInfo->K[4] != 0)
-			{
-				float depthFx = cameraInfo->K[0];
-				float depthFy = cameraInfo->K[4];
-				float depthCx = cameraInfo->K[2];
-				float depthCy = cameraInfo->K[5];
-				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
-				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
-
-				rtabmap::SensorData data(ptrImage->image,
-						ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
-						depthFx,
-						depthFy,
-						depthCx,
-						depthCy,
-						rtabmap::Transform(),
-						rtabmap::transformFromTF(localTransform));
-				quality=0;
-				rtabmap::Transform pose = odometry_->process(data, &quality);
-				if(!pose.isNull())
-				{
-					//*********************
-					// Update odometry
-					//*********************
-					tf::Transform poseTF;
-					rtabmap::transformToTF(pose, poseTF);
-
-					if(publishTf_)
-					{
-						tfBroadcaster_.sendTransform( tf::StampedTransform (poseTF, image->header.stamp, odomFrameId_, frameId_));
-					}
-
-					if(odomPub_.getNumSubscribers())
-					{
-						//next, we'll publish the odometry message over ROS
-						nav_msgs::Odometry odom;
-						odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
-						odom.header.frame_id = odomFrameId_;
-						odom.child_frame_id = frameId_;
-
-						//set the position
-						odom.pose.pose.position.x = poseTF.getOrigin().x();
-						odom.pose.pose.position.y = poseTF.getOrigin().y();
-						odom.pose.pose.position.z = poseTF.getOrigin().z();
-						tf::quaternionTFToMsg(poseTF.getRotation().normalized(), odom.pose.pose.orientation);
-
-						//publish the message
-						odomPub_.publish(odom);
-					}
-
-					if(odomLocalMapPub_.getNumSubscribers())
-					{
-						const std::multimap<int, pcl::PointXYZ> & map = odometry_->getLocalMap();
-						pcl::PointCloud<pcl::PointXYZ> cloud;
-						for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
-						{
-							cloud.push_back(iter->second);
-						}
-						sensor_msgs::PointCloud2 cloudMsg;
-						pcl::toROSMsg(cloud, cloudMsg);
-						cloudMsg.header.stamp = image->header.stamp; // use corresponding time stamp to image
-						cloudMsg.header.frame_id = odomFrameId_;
-						odomLocalMapPub_.publish(cloudMsg);
-					}
-
-					if(odomLastFrame_.getNumSubscribers())
-					{
-						const rtabmap::Signature * s  = odometry_->getMemory()->getLastWorkingSignature();
-						if(s)
-						{
-							const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
-							pcl::PointCloud<pcl::PointXYZ> cloud;
-							rtabmap::Transform t = rtabmap::transformFromTF(localTransform);
-							for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
-							{
-								// transform to odom frame
-								pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
-								cloud.push_back(pt);
-							}
-
-							sensor_msgs::PointCloud2 cloudMsg;
-							pcl::toROSMsg(cloud, cloudMsg);
-							cloudMsg.header.stamp = image->header.stamp; // use corresponding time stamp to image
-							cloudMsg.header.frame_id = odomFrameId_;
-							odomLastFrame_.publish(cloudMsg);
-						}
-					}
-				}
-				else
-				{
-					//ROS_WARN("Odometry lost!");
-
-					//send null pose to notify that odometry is lost
-					nav_msgs::Odometry odom;
-					odom.header.stamp = image->header.stamp; // use corresponding time stamp to image
-					odom.header.frame_id = odomFrameId_;
-					odom.child_frame_id = frameId_;
-
-					//publish the message
-					odomPub_.publish(odom);
-				}
-			}
-
-			ROS_INFO("Odom: quality=%d, update time=%fs", quality, (ros::WallTime::now()-time).toSec());
-		}
-	}
-
-	bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		ROS_INFO("visual_odometry: reset odom!");
-		odometry_->reset();
-		return true;
-	}
-
-	bool pause(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		if(paused_)
-		{
-			ROS_WARN("visual_odometry: Already paused!");
-		}
-		else
-		{
-			paused_ = true;
-			ROS_INFO("visual_odometry: paused!");
-		}
-		return true;
-	}
-
-	bool resume(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
-	{
-		if(!paused_)
-		{
-			ROS_WARN("visual_odometry: Already running!");
-		}
-		else
-		{
-			paused_ = false;
-			ROS_INFO("visual_odometry: resumed!");
-		}
-		return true;
-	}
-
-private:
-	rtabmap::OdometryBOW * odometry_;
-
-	// parameters
-	std::string frameId_;
-	std::string odomFrameId_;
-	bool publishTf_;
-
-	ros::Publisher odomPub_;
-	ros::Publisher odomLocalMapPub_;
-	ros::Publisher odomLastFrame_;
-	ros::ServiceServer resetSrv_;
-	ros::ServiceServer pauseSrv_;
-	ros::ServiceServer resumeSrv_;
-	tf::TransformBroadcaster tfBroadcaster_;
-	tf::TransformListener tfListener_;
-
-	image_transport::SubscriberFilter image_mono_sub_;
-	image_transport::SubscriberFilter image_depth_sub_;
-	message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub_;
-	typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
-	message_filters::Synchronizer<MySyncPolicy> * sync_;
-
-	bool paused_;
-};
-
-int main(int argc, char *argv[])
-{
-	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kWarning);
-	ros::init(argc, argv, "visual_odometry");
-
-	for(int i=1;i<argc;++i)
-	{
-		if(strcmp(argv[i], "--params") == 0)
-		{
-			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
-			rtabmap::ParametersMap parametersOdom;
-			if(strcmp(argv[i], "--params") == 0)
-			{
-				// show specific parameters
-				for(rtabmap::ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-				{
-					if(uSplit(iter->first, '/').front().compare("Odom") == 0 ||
-						uSplit(iter->first, '/').front().compare("SURF") == 0 ||
-						uSplit(iter->first, '/').front().compare("SIFT") == 0 ||
-						uSplit(iter->first, '/').front().compare("ORB") == 0 ||
-						uSplit(iter->first, '/').front().compare("FAST") == 0 ||
-						uSplit(iter->first, '/').front().compare("FREAK") == 0 ||
-						uSplit(iter->first, '/').front().compare("BRIEF") == 0 ||
-						uSplit(iter->first, '/').front().compare("GFTT") == 0 ||
-						uSplit(iter->first, '/').front().compare("BRISK") == 0)
-					{
-						parametersOdom.insert(*iter);
-					}
-				}
-			}
-
-			for(rtabmap::ParametersMap::iterator iter=parametersOdom.begin(); iter!=parametersOdom.end(); ++iter)
-			{
-				std::string str = "Param: " + iter->first + " = \"" + iter->second + "\"";
-				std::cout <<
-						str <<
-						std::setw(60 - str.size()) <<
-						" [" <<
-						rtabmap::Parameters::getDescription(iter->first).c_str() <<
-						"]" <<
-						std::endl;
-			}
-			ROS_WARN("Node will now exit after showing default odometry parameters because "
-					 "argument \"--params\" is detected!");
-			exit(0);
-		}
-	}
-
-	VisualOdometry vOdom;
-	ros::spin();
-	return 0;
-}

src/nodelets/point_cloud_xyzrgb.cpp

@@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <image_transport/image_transport.h>
 #include <image_transport/subscriber_filter.h>
 
+#include <image_geometry/pinhole_camera_model.h>
+
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/subscriber.h>
 
@@ -114,14 +116,21 @@ private:
 			cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image, "bgr8");
 			cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(imageDepth);
 
+			image_geometry::PinholeCameraModel model;
+			model.fromCameraInfo(*cameraInfo);
+			float fx = model.fx();
+			float fy = model.fy();
+			float cx = model.cx();
+			float cy = model.cy();
+
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
 			pclCloud = rtabmap::util3d::cloudFromDepthRGB(
 					imagePtr->image,
 					imageDepthPtr->image,
-					cameraInfo->K[2],
-					cameraInfo->K[5],
-					cameraInfo->K[0],
-					cameraInfo->K[4],
+					cx,
+					cy,
+					fx,
+					fy,
 					decimation_);
 
 			if(voxelSize_ > 0.0)