ros-pkg: added disparity_to_depth nodelet

Aded new demo using stereo disparity instead of Kinect depth: launch/demo/demo_stereo.launch

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

CMakeLists.txt

@@ -118,6 +118,7 @@ add_library(rtabmap_ros
    src/nodelets/data_throttle.cpp
    src/nodelets/data_odom_sync.cpp
    src/nodelets/point_cloud_xyzrgb.cpp 
+   src/nodelets/disparity_to_depth.cpp 
    src/MsgConversion.cpp
    src/rviz/MapCloudDisplay.cpp
    src/rviz/InfoDisplay.cpp

launch/demo/demo_stereo.launch

@@ -0,0 +1,82 @@
+<launch>
+    <!-- 6DOF stereo demo: download a bag example from http://projects.csail.mit.edu/stata/downloads.php.
+    You will need to remove the transform /combined_odometry from the /tf messages:
+    $ rosbag filter 2011-01-20-07-18-45.bag out.bag 'topic != "/tf" or topic == "/tf" and m.transforms[0].header.frame_id != "/odom_combined"' 
+    Run the example:
+    $ roslaunch rtabmap demo_stereo.launch
+    $ rosbag play -.-clock out.bag        (replace -.- by double-dashes)
+    -->
+
+	<param name="use_sim_time" type="bool" value="True"/>
+
+	<!-- Run the ROS package stereo_image_proc for image rectification and disparity computation -->
+	<group ns="/wide_stereo">
+	   <node pkg="nodelet" type="nodelet" name="standalone_nodelet"  args="manager" output="screen"/>
+       <node pkg="nodelet" type="nodelet" name="disparity" args="load stereo_image_proc/disparity standalone_nodelet"/>
+	   <node pkg="nodelet" type="nodelet" name="disparity2depth" args="load rtabmap/disparity_to_depth standalone_nodelet"/>
+	</group>
+
+	<!-- Odometry: Run the viso2_ros package -->
+	<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
+		<remap from="stereo" to="/wide_stereo"/>
+		<remap from="image" to="image_rect"/>
+		<param name="base_link_frame_id" value="/base_footprint"/>
+		<param name="odom_frame_id" value="/odom"/>
+		<param name="ref_frame_change_method" value="1"/>
+	</node>
+	
+  <group ns="rtabmap">
+  
+    <!-- Visual SLAM (robot side) -->
+    <!-- args: "delete_db_on_start" and "udebug" -->
+    <node name="rtabmap" pkg="rtabmap" type="rtabmap" output="screen" args="--delete_db_on_start">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_laserScan" type="bool" value="true"/>
+	
+      <remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
+      <remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
+      
+      <remap from="depth/image" to="/wide_stereo/depth"/>
+      
+      <remap from="odom" to="/stereo_odometer/odometry"/>
+      <remap from="scan" to="/base_scan"/>
+      
+      <param name="frame_id" type="string" value="/base_footprint"/>
+      <param name="queue_size" type="int" value="30"/>
+      
+      <param name="Rtabmap/TimeThr" type="string" value="700"/>
+      <param name="Rtabmap/DetectionRate" type="string" value="1"/>
+      <param name="SURF/HessianThreshold" type="string" value="600"/>
+      <param name="LccBow/MaxDepth" type="string" value="0"/>
+      <param name="RGBD/LocalLoopDetectionSpace" type="string" value="false"/> 
+	  <param name="RGBD/LocalLoopDetectionTime" type="string" value="false"/>
+      <param name="LccBow/MinInliers" type="string" value="10"/>
+      <param name="LccBow/InlierDistance" type="string" value="0.05"/>
+           
+      <!-- Uncomment to force 3dof loop closure constraint using                   -->
+      <!-- the 2d scans (set ScanMatchingSize=1 to correct odometry with laser)    -->
+      <!--
+	  <param name="LccIcp/Type" type="string" value="2"/>
+	  <param name="LccIcp2/CorrespondenceRatio" type="string" value="0.3"/>
+	  <param name="LccIcp2/MaxFitness" type="string" value="5"/>
+	  <param name="RGBD/ScanMatchingSize" type="string" value="0"/>
+	  -->
+
+    </node>
+  
+    <!-- Visualisation (client side) -->
+    <node pkg="rtabmap" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap)/launch/config/rgbd_gui.ini" output="screen">
+      <param name="subscribe_depth" type="bool" value="true"/>
+      <param name="subscribe_laserScan" type="bool" value="true"/>
+      <param name="queue_size" type="int" value="30"/>
+    
+      <remap from="rgb/image" to="/wide_stereo/left/image_rect"/>
+      <remap from="rgb/camera_info" to="/wide_stereo/left/camera_info"/>
+      <remap from="depth/image" to="/wide_stereo/depth"/>
+      
+      <remap from="scan" to="/base_scan"/>
+      <remap from="odom" to="/stereo_odometer/odometry"/>
+    </node>
+  </group>
+	
+</launch>

nodelet_plugins.xml

@@ -22,4 +22,12 @@
       This is my nodelet.
     </description>
   </class>
+  
+  <class name="rtabmap/disparity_to_depth" 
+         type="rtabmap::DisparityToDepth" 
+         base_class_type="nodelet::Nodelet">
+    <description>
+      This is my nodelet.
+    </description>
+  </class>
 </library>

package.xml

@@ -19,6 +19,7 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>std_srvs</build_depend>
   <build_depend>nav_msgs</build_depend>
+  <build_depend>stereo_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>tf</build_depend>
   <build_depend>tf_conversions</build_depend>
@@ -38,6 +39,7 @@
   <run_depend>std_msgs</run_depend>
   <run_depend>std_srvs</run_depend>
   <run_depend>nav_msgs</run_depend>
+  <run_depend>stereo_msgs</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>tf_conversions</run_depend>

src/CameraNode.cpp

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

src/CoreNode.cpp

@@ -50,6 +50,10 @@ int main(int argc, char** argv)
 		{
 			ULogger::setLevel(ULogger::kDebug);
 		}
+		else if(strcmp(argv[i], "--uinfo") == 0)
+		{
+			ULogger::setLevel(ULogger::kInfo);
+		}
 		else if(strcmp(argv[i], "--params") == 0 || strcmp(argv[i], "--params-all") == 0)
 		{
 			rtabmap::ParametersMap parameters = rtabmap::Parameters::getDefaultParameters();
@@ -98,6 +102,11 @@ int main(int argc, char** argv)
 					 "argument \"--params\" is detected!");
 			exit(0);
 		}
+		else
+		{
+			ROS_ERROR("Not recognized argument \"%s\"", argv[i]);
+			exit(-1);
+		}
 	}
 
 	CoreWrapper * rtabmap = new CoreWrapper(deleteDbOnStart);

src/CoreWrapper.cpp

@@ -34,6 +34,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Camera.h>
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Memory.h>
+#include <rtabmap/core/VWDictionary.h>
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UFile.h>
@@ -45,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl_ros/transforms.h>
 #include <pcl_conversions/pcl_conversions.h>
 #include <laser_geometry/laser_geometry.h>
+#include <image_geometry/stereo_camera_model.h>
 
 //msgs
 #include "rtabmap/Info.h"
@@ -159,10 +162,10 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
 	if(isRGBD)
 	{
 		// RGBD SLAM
-		if(!subscribeDepth && !subscribeLaserScan)
+		if(!subscribeDepth)
 		{
-			ROS_WARN("ROS param subscribe_depth and subscribe_laserScan are false, but RTAB-Map "
+			ROS_WARN("ROS param subscribe_depth is false, but RTAB-Map "
-					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth and subscribe_laserScan "
+					  "parameter \"RGBD/Enabled\" is true! Please set subscribe_depth "
 					  "to true to use rtabmap node for RGB-D SLAM, or set \"RGBD/Enabled\" to false for loop closure "
 					  "detection on images-only.");
 		}
@@ -212,8 +215,6 @@ CoreWrapper::~CoreWrapper()
 		delete transformThread_;
 	}
 
-	if(scanSync_)
-		delete scanSync_;
 	if(depthSync_)
 		delete depthSync_;
 	if(depthScanSync_)
@@ -411,78 +412,6 @@ void CoreWrapper::depthCallback(
 	}
 }
 
-void CoreWrapper::scanCallback(
-		const sensor_msgs::ImageConstPtr& imageMsg,
-		const nav_msgs::OdometryConstPtr & odomMsg,
-		const sensor_msgs::LaserScanConstPtr& scanMsg)
-{
-	if(!paused_)
-	{
-		if(rate_>0.0f)
-		{
-			if(ros::Time::now() - time_ < ros::Duration(1.0f/rate_))
-			{
-				return;
-			}
-		}
-		time_ = ros::Time::now();
-
-		if(!(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
-				imageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
-		{
-			ROS_ERROR("Input type must be image=mono8,mono16,rgb8,bgr8");
-			return;
-		}
-
-		// TF ready?
-		try
-		{
-			tf::StampedTransform tmp;
-			tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
-		}
-		catch(tf::TransformException & ex)
-		{
-			ROS_WARN("%s",ex.what());
-			return;
-		}
-
-		//transform in frameId_ frame
-		sensor_msgs::PointCloud2 scanOut;
-		laser_geometry::LaserProjection projection;
-		projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
-		pcl::PointCloud<pcl::PointXYZ> pclScan;
-		pcl::fromROSMsg(scanOut, pclScan);
-		cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);
-
-		Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
-
-		cv_bridge::CvImageConstPtr ptrImage;
-		if(imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
-		   imageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
-		{
-			ptrImage = cv_bridge::toCvShare(imageMsg, "mono8");
-		}
-		else
-		{
-			ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		}
-
-		process(ptrImage->header.seq,
-				ptrImage->image,
-				odom,
-				odomMsg->header.frame_id,
-				cv::Mat(),
-				0.0f,
-				0.0f,
-				0.0f,
-				0.0f,
-				Transform(),
-				scan);
-	}
-}
-
 void CoreWrapper::depthScanCallback(
 		const sensor_msgs::ImageConstPtr& imageMsg,
 		const nav_msgs::OdometryConstPtr & odomMsg,
@@ -610,7 +539,7 @@ void CoreWrapper::process(
 			depth16 = depth;
 		}
 
-		Image data(image,
+		SensorData data(image,
 				depth16,
 				scan,
 				depthFx,
@@ -1278,6 +1207,15 @@ void CoreWrapper::setupCallbacks(
 		bool subscribeLaserScan,
 		int queueSize)
 {
+	if(subscribeLaserScan)
+	{
+		if(!subscribeDepth)
+		{
+			ROS_WARN("When subscribing to laser scan, you should subscribe to depth too. Subscribing to depth...");
+			subscribeDepth = true;
+		}
+	}
+
 	ros::NodeHandle nh; // public
 	ros::NodeHandle pnh("~"); // private
 	ros::NodeHandle rgb_nh(nh, "rgb");
@@ -1310,15 +1248,6 @@ void CoreWrapper::setupCallbacks(
 		depthSync_ = new message_filters::Synchronizer<MyDepthSyncPolicy>(MyDepthSyncPolicy(queueSize), imageSub_, odomSub_, imageDepthSub_, cameraInfoSub_);
 		depthSync_->registerCallback(boost::bind(&CoreWrapper::depthCallback, this, _1, _2, _3, _4));
 	}
-	else if(!subscribeDepth && subscribeLaserScan)
-	{
-		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(&CoreWrapper::scanCallback, this, _1, _2, _3));
-	}
 	else
 	{
 		ROS_INFO("Registering default callback...");

src/CoreWrapper.h

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <sensor_msgs/CameraInfo.h>
 #include <sensor_msgs/LaserScan.h>
 #include <nav_msgs/Odometry.h>
+#include <stereo_msgs/DisparityImage.h>
 
 #include <rtabmap/core/Statistics.h>
 #include <rtabmap/core/Parameters.h>
@@ -72,9 +73,6 @@ private:
 					   const nav_msgs::OdometryConstPtr & odomMsg,
 					   const sensor_msgs::ImageConstPtr& imageDepthMsg,
 					   const sensor_msgs::CameraInfoConstPtr& camInfoMsg);
-	void scanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
-					  const nav_msgs::OdometryConstPtr & odomMsg,
-					  const sensor_msgs::LaserScanConstPtr& scanMsg);
 	void depthScanCallback(const sensor_msgs::ImageConstPtr& imageMsg,
 						   const nav_msgs::OdometryConstPtr & odomMsg,
 						   const sensor_msgs::ImageConstPtr& imageDepthMsg,
@@ -130,7 +128,9 @@ private:
 	image_transport::Subscriber defaultSub_;
 	image_transport::SubscriberFilter imageSub_;
 	image_transport::SubscriberFilter imageDepthSub_;
+	image_transport::SubscriberFilter imageSubRight_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSubRight_;
 	message_filters::Subscriber<nav_msgs::Odometry> odomSub_;
 	message_filters::Subscriber<sensor_msgs::LaserScan> scanSub_;
 
@@ -149,12 +149,6 @@ private:
 			sensor_msgs::CameraInfo> MyDepthSyncPolicy;
 	message_filters::Synchronizer<MyDepthSyncPolicy> * depthSync_;
 
-	typedef message_filters::sync_policies::ApproximateTime<
-			sensor_msgs::Image,
-			nav_msgs::Odometry,
-			sensor_msgs::LaserScan> MyScanSyncPolicy;
-	message_filters::Synchronizer<MyScanSyncPolicy> * scanSync_;
-
 	tf::TransformBroadcaster tfBroadcaster_;
 	tf::TransformListener tfListener_;
 

src/DataRecorderNode.cpp

@@ -171,7 +171,7 @@ private:
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg)
 	{
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
-		rtabmap::Image image(
+		rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			cv::Mat(),
 			cv::Mat(),
@@ -237,7 +237,7 @@ private:
 			depth16 = ptrDepth->image;
 		}
 
-		rtabmap::Image image(
+		rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
@@ -306,7 +306,7 @@ private:
 			depth16 = ptrDepth->image;
 		}
 
-		rtabmap::Image image(
+		rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			depth16,
 			cv::Mat(),
@@ -348,7 +348,7 @@ private:
 
 		cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 
-		rtabmap::Image image(
+		rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			cv::Mat(),
 			scan,
@@ -428,7 +428,7 @@ private:
 			depth16 = ptrDepth->image;
 		}
 
-		rtabmap::Image image(
+		rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			depth16,
 			scan,

src/GuiWrapper.cpp

@@ -551,16 +551,9 @@ void GuiWrapper::handleEvent(UEvent * anEvent)
 void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
 {
 	Transform odom = transformFromPoseMsg(odomMsg->pose.pose);
-	rtabmap::Image image(
+	rtabmap::SensorData data;
-			cv::Mat(),
+	data.setPose(odom);
-			cv::Mat(),
+	this->post(new OdometryEvent(data));
-			0.0f,
-			0.0f,
-			0.0f,
-			0.0f,
-			odom,
-			Transform());
-	this->post(new OdometryEvent(image));
 }
 
 void GuiWrapper::depthCallback(
@@ -593,7 +586,7 @@ void GuiWrapper::depthCallback(
 	float depthCx = cameraInfoMsg->K[2];
 	float depthCy = cameraInfoMsg->K[5];
 
-	rtabmap::Image image(
+	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
 			depthFx,
@@ -634,7 +627,7 @@ void GuiWrapper::scanCallback(
 
 	cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(imageMsg, "bgr8");
 
-	rtabmap::Image image(
+	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			cv::Mat(),
 			scan,
@@ -687,7 +680,7 @@ void GuiWrapper::depthScanCallback(
 	float depthCx = cameraInfoMsg->K[2];
 	float depthCy = cameraInfoMsg->K[5];
 
-	rtabmap::Image image(
+	rtabmap::SensorData image(
 			ptrImage->image.clone(),
 			ptrDepth->image.clone(),
 			scan,

src/VisualOdometryNode.cpp

@@ -215,7 +215,7 @@ public:
 				cv_bridge::CvImageConstPtr ptrImage = cv_bridge::toCvShare(image, "mono8");
 				cv_bridge::CvImageConstPtr ptrDepth = cv_bridge::toCvShare(depth);
 
-				rtabmap::Image data(ptrImage->image,
+				rtabmap::SensorData data(ptrImage->image,
 						ptrDepth->image.type() == CV_32FC1?util3d::cvtDepthFromFloat(ptrDepth->image):ptrDepth->image,
 						depthFx,
 						depthFy,

src/nodelets/disparity_to_depth.cpp

@@ -0,0 +1,104 @@
+/*
+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 <pluginlib/class_list_macros.h>
+#include <nodelet/nodelet.h>
+
+#include <sensor_msgs/Image.h>
+#include <sensor_msgs/image_encodings.h>
+#include <stereo_msgs/DisparityImage.h>
+
+#include <image_transport/image_transport.h>
+
+#include <cv_bridge/cv_bridge.h>
+
+namespace rtabmap
+{
+
+class DisparityToDepth : public nodelet::Nodelet
+{
+public:
+	DisparityToDepth() {}
+
+	virtual ~DisparityToDepth(){}
+
+private:
+	virtual void onInit()
+	{
+		ros::NodeHandle & nh = getNodeHandle();
+		ros::NodeHandle & pnh = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it(nh);
+		pub_ = it.advertise("depth", 1);
+		sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
+	}
+
+	void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
+	{
+		if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
+		{
+			ROS_ERROR("Input type must be disparity=32FC1");
+			return;
+		}
+
+		if(pub_.getNumSubscribers())
+		{
+			// sensor_msgs::image_encodings::TYPE_32FC1
+			cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
+
+			cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
+			for (int i = 0; i < disparity.rows; i++)
+			{
+				for (int j = 0; j < disparity.cols; j++)
+				{
+					float disparity_value = disparity.at<float>(i,j);
+					if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
+					{
+						// baseline * focal / disparity
+						depth.at<float>(i,j) = disparityMsg->T * disparityMsg->f / disparity_value;
+					}
+				}
+			}
+
+			// convert to ROS sensor_msg::Image
+			cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth);
+			sensor_msgs::Image depthMsg;
+			cvDepth.toImageMsg(depthMsg);
+
+			//publish the message
+			pub_.publish(depthMsg);
+		}
+}
+
+private:
+	image_transport::Publisher pub_;
+	ros::Subscriber sub_;
+};
+
+PLUGINLIB_EXPORT_CLASS(rtabmap::DisparityToDepth, nodelet::Nodelet);
+}