$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 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 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ros/ros.h> 00031 00032 #include <sensor_msgs/Image.h> 00033 #include <sensor_msgs/CameraInfo.h> 00034 #include <stereo_msgs/DisparityImage.h> 00035 #include <sensor_msgs/PointCloud2.h> 00036 #include <sensor_msgs/PointCloud.h> 00037 00038 #include <sensor_msgs/point_cloud_conversion.h> 00039 00040 #include <tf/transform_listener.h> 00041 00042 #include "rviz_interaction_tools/image_tools.h" 00043 00044 #include "rgbd_assembler/msg_saver.h" 00045 #include "rgbd_assembler/utils.h" 00046 #include "rgbd_assembler/RgbdAssembly.h" 00047 00048 #include <limits> 00049 #include <math.h> 00050 00051 00052 namespace rgbd_assembler { 00053 00054 class RgbdAssembler { 00055 00056 typedef geometry_msgs::Point32 Point; 00057 00058 private: 00060 ros::NodeHandle root_nh_; 00062 ros::NodeHandle priv_nh_; 00063 00064 ros::ServiceServer rgbd_srv_; 00065 00066 tf::TransformListener listener_; 00067 00068 00069 //------------------ Callbacks ------------------- 00070 00072 bool rgbdServiceCallback(RgbdAssembly::Request &request, 00073 RgbdAssembly::Response &response); 00074 00075 //------------------ Individual processing steps ------- 00076 bool assembleSensorData(ros::Duration time_out); 00077 00078 //------------------- Complete processing ----- 00079 00080 // reconstruct point cloud and fill 3D, 2D and RGB channels 00081 bool assembleRgbd(); 00082 00083 void getPointCloudFromIm(sensor_msgs::PointCloud2 &point_cloud, 00084 const stereo_msgs::DisparityImage &disparity_image, 00085 const sensor_msgs::CameraInfo &narrow_cam_info); 00086 00087 void projectPointCloud( sensor_msgs::PointCloud2 &point_cloud, 00088 const sensor_msgs::CameraInfo &cam_info ); 00089 00090 void getPixel( const sensor_msgs::PointCloud2 &point_cloud, 00091 int i, Point &p); 00092 00093 void extractRgbVals( sensor_msgs::PointCloud2 &point_cloud, 00094 const sensor_msgs::Image &wide_img, 00095 const sensor_msgs::Image &narrow_img ); 00096 00097 // ------------------- The Data -------------- 00098 00099 sensor_msgs::Image narrow_image_; 00100 sensor_msgs::Image wide_image_; 00101 stereo_msgs::DisparityImage disparity_image_; 00102 sensor_msgs::CameraInfo narrow_cam_info_; 00103 sensor_msgs::CameraInfo wide_cam_info_; 00104 sensor_msgs::PointCloud2 point_cloud_; 00105 00106 public: 00107 00108 RgbdAssembler() : 00109 root_nh_(""), priv_nh_("~") 00110 { 00111 rgbd_srv_ = root_nh_.advertiseService(root_nh_.resolveName("/rgbd_assembly"), 00112 &RgbdAssembler::rgbdServiceCallback, this); 00113 00114 00115 ROS_INFO("RGB-D Assembler node started"); 00116 } 00117 00118 ~RgbdAssembler() 00119 {} 00120 00121 }; 00122 00123 bool RgbdAssembler::rgbdServiceCallback(RgbdAssembly::Request &request, 00124 RgbdAssembly::Response &response) 00125 { 00126 ros::Time start_time = ros::Time::now(); 00127 if (!assembleSensorData(ros::Duration(15.0))) return false; 00128 ROS_INFO_STREAM("RGBD Assembler: gathered sensor data after " << ros::Time::now() - start_time << " seconds"); 00129 00130 if (!priv_nh_.ok()) 00131 return false; 00132 00133 if (!assembleRgbd()) { 00134 ROS_INFO("RGB-D assembly has not succeeded;"); 00135 response.result = response.OTHER_ERROR; 00136 } else { 00137 response.point_cloud = point_cloud_; 00138 response.image = narrow_image_; 00139 response.disparity_image = disparity_image_; 00140 response.camera_info = narrow_cam_info_; 00141 response.result = response.SUCCESS; 00142 ROS_INFO_STREAM("RGBD Assembler: finished assembly after " << ros::Time::now() - start_time << " seconds"); 00143 } 00144 return true; 00145 } 00146 00147 bool RgbdAssembler::assembleSensorData(ros::Duration time_out) 00148 { 00149 ROS_INFO("RGB-D Assembly: waiting for messages..."); 00150 00151 MsgSaver< sensor_msgs::Image > 00152 recent_wide_image("/wide_stereo/left/image_rect_color"); 00153 MsgSaver< sensor_msgs::Image > 00154 recent_image("/narrow_stereo/left/image_rect"); 00155 MsgSaver< stereo_msgs::DisparityImage > 00156 recent_disparity_image("/narrow_stereo_textured/disparity"); 00157 MsgSaver< sensor_msgs::CameraInfo > 00158 recent_camera_info("/narrow_stereo_textured/left/camera_info"); 00159 MsgSaver< sensor_msgs::CameraInfo > 00160 recent_wide_camera_info("/wide_stereo/left/camera_info"); 00161 00162 ros::Time start_time = ros::Time::now(); 00163 00164 //ros::spin() is called in the main thread, so we just have to wait.. 00165 while ( (!recent_disparity_image.hasMsg() || 00166 !recent_image.hasMsg() || 00167 !recent_camera_info.hasMsg() || 00168 !recent_wide_camera_info.hasMsg() || 00169 !recent_wide_image.hasMsg()) 00170 && priv_nh_.ok()) { 00171 00172 ros::spinOnce(); 00173 00174 ros::Time current_time = ros::Time::now(); 00175 if (time_out >= ros::Duration(0) && current_time - start_time >= time_out) { 00176 ROS_ERROR("Timed out while waiting for sensor data."); 00177 return false; 00178 } 00179 ros::Duration(0.1).sleep(); 00180 } 00181 if (!priv_nh_.ok()) return false; 00182 00183 narrow_image_ = *recent_image.getMsg(); 00184 wide_image_ = *recent_wide_image.getMsg(); 00185 disparity_image_ = *recent_disparity_image.getMsg(); 00186 narrow_cam_info_ = *recent_camera_info.getMsg(); 00187 wide_cam_info_ = *recent_wide_camera_info.getMsg(); 00188 00189 return true; 00190 } 00191 00192 bool RgbdAssembler::assembleRgbd() { 00193 00194 ROS_INFO("Reconstructing point cloud"); 00195 00196 // Make non-dense point cloud from image coordinates 00197 getPointCloudFromIm(point_cloud_, disparity_image_, narrow_cam_info_); 00198 00199 int w = point_cloud_.width; 00200 int h = point_cloud_.height; 00201 00202 // annoying conversion between point cloud types 00203 sensor_msgs::PointCloud2::ConstPtr point_cloud_ptr 00204 = boost::make_shared<const sensor_msgs::PointCloud2> (point_cloud_); 00205 sensor_msgs::PointCloud old_cloud; 00206 sensor_msgs::convertPointCloud2ToPointCloud (*point_cloud_ptr, old_cloud); 00207 00208 // transform image plane to wide stereo frame 00209 try 00210 { 00211 ROS_DEBUG("Transforming point cloud from frame %s into frame %s", 00212 point_cloud_.header.frame_id.c_str(), 00213 "wide_stereo_optical_frame"); 00214 listener_.transformPointCloud("wide_stereo_optical_frame", old_cloud, old_cloud); 00215 } 00216 catch (tf::TransformException ex) 00217 { 00218 ROS_ERROR("Failed to transform point cloud from frame %s into frame %s; error %s", 00219 old_cloud.header.frame_id.c_str(), 00220 "wide_stereo_optical_frame", 00221 ex.what()); 00222 return false; 00223 } 00224 00225 sensor_msgs::PointCloud2 converted_cloud; 00226 sensor_msgs::convertPointCloudToPointCloud2 (old_cloud, point_cloud_); 00227 00228 // setting back dimensions of point cloud to width and height of the image 00229 point_cloud_.height = h; 00230 point_cloud_.width = w; 00231 00232 // project cloud to wide field stereo and add rgb channel to cloud 00233 projectPointCloud(point_cloud_, wide_cam_info_); 00234 00235 extractRgbVals(point_cloud_, wide_image_, narrow_image_); 00236 00237 return true; 00238 } 00239 00240 void RgbdAssembler::getPointCloudFromIm(sensor_msgs::PointCloud2 &point_cloud, 00241 const stereo_msgs::DisparityImage &disparity_image, 00242 const sensor_msgs::CameraInfo &narrow_cam_info) 00243 { 00244 int w = narrow_cam_info.width; 00245 int h = narrow_cam_info.height; 00246 00247 // Generate PointCloud2 data structure 00248 point_cloud.header = narrow_cam_info.header; 00249 point_cloud.height = h; 00250 point_cloud.width = w; 00251 point_cloud.is_dense = false; 00252 point_cloud.is_bigendian = false; 00253 // size of field is 3 for x, y, z and 3 for rgb and u,v 00254 point_cloud.fields.resize( 3 + 3); 00255 point_cloud.fields[0].name = "x"; point_cloud.fields[1].name = "y"; point_cloud.fields[2].name = "z"; 00256 point_cloud.fields[3].name = "rgb"; point_cloud.fields[4].name = "u"; point_cloud.fields[5].name = "v"; 00257 int offset = 0; 00258 for (size_t d = 0; d < point_cloud.fields.size (); ++d, offset += 4) { 00259 point_cloud.fields[d].offset = offset; 00260 point_cloud.fields[d].datatype = sensor_msgs::PointField::FLOAT32; 00261 point_cloud.fields[d].count = 1; 00262 } 00263 00264 point_cloud.point_step = offset; 00265 point_cloud.row_step = point_cloud.point_step * w; 00266 00267 point_cloud.data.resize (w * h * point_cloud.point_step); 00268 00269 for (int i=0; i<h; i++) { 00270 for (int j=0; j<w; j++) { 00271 int adr = i * w + j; 00272 float x, y, z; 00273 if (rviz_interaction_tools::hasDisparityValue(disparity_image, i, j)) { 00274 00275 rviz_interaction_tools::getPoint( disparity_image, narrow_cam_info_, i, j, x, y, z); 00276 00277 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[0].offset], &x, sizeof (float)); 00278 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[1].offset], &y, sizeof (float)); 00279 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[2].offset], &z, sizeof (float)); 00280 00281 } else { 00282 00283 float nan = std::numeric_limits<float>::quiet_NaN(); 00284 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[0].offset], &nan, sizeof (float)); 00285 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[1].offset], &nan, sizeof (float)); 00286 memcpy (&point_cloud.data[adr * point_cloud.point_step + point_cloud.fields[2].offset], &nan, sizeof (float)); 00287 00288 } 00289 } 00290 } 00291 } 00292 00293 00294 void RgbdAssembler::projectPointCloud(sensor_msgs::PointCloud2 &point_cloud, 00295 const sensor_msgs::CameraInfo &cam_info ){ 00296 00297 float fx = cam_info.P[0*4+0]; 00298 float fy = cam_info.P[1*4+1]; 00299 float cx = cam_info.P[0*4+2]; 00300 float cy = cam_info.P[1*4+2]; 00301 float tx = cam_info.P[0*4+3]; 00302 float ty = cam_info.P[1*4+3]; 00303 00304 int w = cam_info.width; 00305 int h = cam_info.height; 00306 00307 for(int x=0; x<w; ++x){ 00308 for(int y=0; y<h; ++y){ 00309 int i = y*w+x; 00310 float nan; 00311 memcpy (&nan, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[0].offset], sizeof (float)); 00312 if(!isnan(nan)){ 00313 00314 float x, y, z; 00315 00316 memcpy (&x, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[0].offset], sizeof (float)); 00317 memcpy (&y, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[1].offset], sizeof (float)); 00318 memcpy (&z, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[2].offset], sizeof (float)); 00319 00320 float u = (fx * x + tx)/z + cx; 00321 float v = (fy * y + ty)/z + cy; 00322 00323 memcpy (&point_cloud.data[i * point_cloud.point_step + point_cloud.fields[4].offset], &u, sizeof (float)); 00324 memcpy (&point_cloud.data[i * point_cloud.point_step + point_cloud.fields[5].offset], &v, sizeof (float)); 00325 00326 } 00327 } 00328 } 00329 } 00330 00331 void RgbdAssembler::getPixel( const sensor_msgs::PointCloud2 &point_cloud, 00332 int i, Point &p){ 00333 float u, v; 00334 memcpy ( &u, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[4].offset], sizeof (float)); 00335 memcpy ( &v, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[5].offset], sizeof (float)); 00336 00337 p.x = u; 00338 p.y = v; 00339 p.z = 1.0; 00340 00341 } 00342 00343 void RgbdAssembler::extractRgbVals(sensor_msgs::PointCloud2 &point_cloud, 00344 const sensor_msgs::Image &wide_img, 00345 const sensor_msgs::Image &narrow_img) 00346 { 00347 00348 ROS_INFO("Size of point cloud in extractRgbVals %d", (int)point_cloud.data.size()); 00349 00350 for(unsigned int x=0; x<narrow_img.width; ++x){ 00351 for(unsigned int y=0; y<narrow_img.height; ++y){ 00352 int i = y * narrow_img.width + x; 00353 00354 float val = (float)narrow_img.data.at(i)/255.0f; 00355 float r, g, b; 00356 00357 float nan; 00358 memcpy (&nan, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[0].offset], sizeof (float)); 00359 if(!isnan(nan)){ 00360 Point p; 00361 getPixel( point_cloud, i, p); 00362 00363 int wide_i = (int)p.y * (int)wide_img.step + (int)p.x*3; 00364 r = (float)wide_img.data.at(wide_i+2)/255.0f; 00365 g = (float)wide_img.data.at(wide_i+1)/255.0f; 00366 b = (float)wide_img.data.at(wide_i+0)/255.0f; 00367 00368 float h, s, v; 00369 RGBToHSV(r,g,b,h,s,v); 00370 HSVToRGB(h,s,val, r, g, b); 00371 00372 } else { 00373 00374 r = val; 00375 g = val; 00376 b = val; 00377 00378 } 00379 00380 float rgb = getRGB(r,g,b); 00381 memcpy ( &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[3].offset], &rgb, sizeof (float)); 00382 } 00383 } 00384 } 00385 00386 } // namespace rgbd_assembler 00387 00388 int main(int argc, char **argv) 00389 { 00390 ros::init(argc, argv, "rgbd_assembler_node"); 00391 rgbd_assembler::RgbdAssembler node; 00392 ros::spin(); 00393 return 0; 00394 }