disparity_to_depth.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <ros/ros.h>
00029 #include <pluginlib/class_list_macros.h>
00030 #include <nodelet/nodelet.h>
00031 
00032 #include <sensor_msgs/Image.h>
00033 #include <sensor_msgs/image_encodings.h>
00034 #include <stereo_msgs/DisparityImage.h>
00035 
00036 #include <image_transport/image_transport.h>
00037 
00038 #include <cv_bridge/cv_bridge.h>
00039 
00040 namespace rtabmap_ros
00041 {
00042 
00043 class DisparityToDepth : public nodelet::Nodelet
00044 {
00045 public:
00046         DisparityToDepth() {}
00047 
00048         virtual ~DisparityToDepth(){}
00049 
00050 private:
00051         virtual void onInit()
00052         {
00053                 ros::NodeHandle & nh = getNodeHandle();
00054                 ros::NodeHandle & pnh = getPrivateNodeHandle();
00055 
00056                 image_transport::ImageTransport it(nh);
00057                 pub32f_ = it.advertise("depth", 1);
00058                 pub16u_ = it.advertise("depth_raw", 1);
00059                 sub_ = nh.subscribe("disparity", 1, &DisparityToDepth::callback, this);
00060         }
00061 
00062         void callback(const stereo_msgs::DisparityImageConstPtr& disparityMsg)
00063         {
00064                 if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0)
00065                 {
00066                         NODELET_ERROR("Input type must be disparity=32FC1");
00067                         return;
00068                 }
00069 
00070                 bool publish32f = pub32f_.getNumSubscribers();
00071                 bool publish16u = pub16u_.getNumSubscribers();
00072 
00073                 if(publish32f || publish16u)
00074                 {
00075                         // sensor_msgs::image_encodings::TYPE_32FC1
00076                         cv::Mat disparity(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
00077 
00078                         cv::Mat depth32f;
00079                         cv::Mat depth16u;
00080                         if(publish32f)
00081                         {
00082                                 depth32f = cv::Mat::zeros(disparity.rows, disparity.cols, CV_32F);
00083                         }
00084                         if(publish16u)
00085                         {
00086                                 depth16u = cv::Mat::zeros(disparity.rows, disparity.cols, CV_16U);
00087                         }
00088                         for (int i = 0; i < disparity.rows; i++)
00089                         {
00090                                 for (int j = 0; j < disparity.cols; j++)
00091                                 {
00092                                         float disparity_value = disparity.at<float>(i,j);
00093                                         if (disparity_value > disparityMsg->min_disparity && disparity_value < disparityMsg->max_disparity)
00094                                         {
00095                                                 // baseline * focal / disparity
00096                                                 float depth = disparityMsg->T * disparityMsg->f / disparity_value;
00097                                                 if(publish32f)
00098                                                 {
00099                                                         depth32f.at<float>(i,j) = depth;
00100                                                 }
00101                                                 if(publish16u)
00102                                                 {
00103                                                         depth16u.at<unsigned short>(i,j) = (unsigned short)(depth*1000.0f);
00104                                                 }
00105                                         }
00106                                 }
00107                         }
00108 
00109                         if(publish32f)
00110                         {
00111                                 // convert to ROS sensor_msg::Image
00112                                 cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_32FC1, depth32f);
00113                                 sensor_msgs::Image depthMsg;
00114                                 cvDepth.toImageMsg(depthMsg);
00115 
00116                                 //publish the message
00117                                 pub32f_.publish(depthMsg);
00118                         }
00119 
00120                         if(publish16u)
00121                         {
00122                                 // convert to ROS sensor_msg::Image
00123                                 cv_bridge::CvImage cvDepth(disparityMsg->header, sensor_msgs::image_encodings::TYPE_16UC1, depth16u);
00124                                 sensor_msgs::Image depthMsg;
00125                                 cvDepth.toImageMsg(depthMsg);
00126 
00127                                 //publish the message
00128                                 pub16u_.publish(depthMsg);
00129                         }
00130                 }
00131 }
00132 
00133 private:
00134         image_transport::Publisher pub32f_;
00135         image_transport::Publisher pub16u_;
00136         ros::Subscriber sub_;
00137 };
00138 
00139 PLUGINLIB_EXPORT_CLASS(rtabmap_ros::DisparityToDepth, nodelet::Nodelet);
00140 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:07