i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros-pkg: added new depth output 16U (mm) to disparity_to_depth nodelet

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1658 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-19 20:13:43 +00:00
parent d59b07927f
commit 6634d92483
1 changed files with 47 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/disparity_to_depth.cpp
+47 -11
View File
@@ -54,7 +54,8 @@ private:
ros::NodeHandle & pnh = getPrivateNodeHandle();
image_transport::ImageTransport it(nh);
pub_ = it.advertise("depth", 1);
pub32f_ = it.advertise("depth", 1);
pub16u_ = it.advertise("depth_raw", 1);
sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
}
@@ -66,12 +67,24 @@ private:
return;
}
if(pub_.getNumSubscribers())
bool publish32f = pub32f_.getNumSubscribers();
bool publish16u = pub16u_.getNumSubscribers();
if(publish32f || publish16u)
{
// 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);
cv::Mat depth32f;
cv::Mat depth16u;
if(publish32f)
{
depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
}
if(publish16u)
{
depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
}
for (int i = 0; i < disparity.rows; i++)
{
for (int j = 0; j < disparity.cols; j++)
@@ -80,23 +93,46 @@ private:
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;
float depth = disparityMsg->T * disparityMsg->f / disparity_value;
if(publish32f)
{
depth32f.at<float>(i,j) = depth;
}
if(publish16u)
{
depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
}
}
}
}
// 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);
if(publish32f)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
sensor_msgs::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub_.publish(depthMsg);
//publish the message
pub32f_.publish(depthMsg);
}
if(publish16u)
{
// convert to ROS sensor_msg::Image
cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
sensor_msgs::Image depthMsg;
cvDepth.toImageMsg(depthMsg);
//publish the message
pub16u_.publish(depthMsg);
}
}
}
private:
image_transport::Publisher pub_;
image_transport::Publisher pub32f_;
image_transport::Publisher pub16u_;
ros::Subscriber sub_;
};