depth_cloud_mld.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 20013, 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  *  Created on: Jan 22, 2013
00031  *      Author: jkammerl
00032  */
00033 
00034 #include <image_geometry/pinhole_camera_model.h>
00035 #include <sensor_msgs/image_encodings.h>
00036 
00037 #include <string.h>
00038 #include <sstream>
00039 
00040 #include "depth_cloud_mld.h"
00041 
00042 namespace enc = sensor_msgs::image_encodings;
00043 
00044 #define POINT_STEP (sizeof(float)*4)
00045 
00046 namespace rviz
00047 {
00048 
00049 // Encapsulate differences between processing float and uint16_t depths
00050 template<typename T>
00051   struct DepthTraits
00052   {
00053   };
00054 
00055 template<>
00056   struct DepthTraits<uint16_t>
00057   {
00058     static inline bool valid(float depth)
00059     {
00060       return depth != 0.0;
00061     }
00062     static inline float toMeters(uint16_t depth)
00063     {
00064       return depth * 0.001f;
00065     } // originally mm
00066   };
00067 
00068 template<>
00069   struct DepthTraits<float>
00070   {
00071     static inline bool valid(float depth)
00072     {
00073       return std::isfinite(depth);
00074     }
00075     static inline float toMeters(float depth)
00076     {
00077       return depth;
00078     }
00079   };
00080 
00081 
00082 struct RGBA
00083 {
00084   uint8_t red;
00085   uint8_t green;
00086   uint8_t blue;
00087   uint8_t alpha;
00088 };
00089 
00090 
00091 void MultiLayerDepth::initializeConversion(const sensor_msgs::ImageConstPtr& depth_msg,
00092                                            sensor_msgs::CameraInfoConstPtr& camera_info_msg)
00093 {
00094 
00095   if (!depth_msg || !camera_info_msg)
00096   {
00097     std::string error_msg ("Waiting for CameraInfo message..");
00098     throw( MultiLayerDepthException (error_msg));
00099   }
00100 
00101   // do some sanity checks
00102   int binning_x = camera_info_msg->binning_x > 1 ? camera_info_msg->binning_x : 1;
00103   int binning_y = camera_info_msg->binning_y > 1 ? camera_info_msg->binning_y : 1;
00104 
00105   int roi_width = camera_info_msg->roi.width > 0 ? camera_info_msg->roi.width : camera_info_msg->width;
00106   int roi_height = camera_info_msg->roi.height > 0 ? camera_info_msg->roi.height : camera_info_msg->height;
00107 
00108   int expected_width = roi_width / binning_x;
00109   int expected_height = roi_height / binning_y;
00110 
00111   if ( expected_width != depth_msg->width ||
00112        expected_height != depth_msg->height )
00113   {
00114     std::ostringstream s;
00115     s << "Depth image size and camera info don't match: ";
00116     s << depth_msg->width << " x " << depth_msg->height;
00117     s << " vs " << expected_width << " x " << expected_height;
00118     s << "(binning: " << binning_x << " x " << binning_y;
00119     s << ", ROI size: " << roi_width << " x " << roi_height << ")";
00120     throw( MultiLayerDepthException (s.str()));
00121   }
00122 
00123   int width = depth_msg->width;
00124   int height = depth_msg->height;
00125 
00126   std::size_t size = width * height;
00127 
00128   if (size != shadow_depth_.size())
00129   {
00130     // Allocate memory for shadow processing
00131     shadow_depth_.resize(size, 0.0f);
00132     shadow_timestamp_.resize(size, 0.0);
00133     shadow_buffer_.resize(size * POINT_STEP, 0);
00134 
00135     // Procompute 3D projection matrix
00136     //
00137     // The following computation of center_x,y and fx,fy duplicates
00138     // code in the image_geometry package, but this avoids dependency
00139     // on OpenCV, which simplifies releasing rviz.
00140 
00141     // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00142     double scale_x = camera_info_msg->binning_x > 1 ? (1.0 / camera_info_msg->binning_x) : 1.0;
00143     double scale_y = camera_info_msg->binning_y > 1 ? (1.0 / camera_info_msg->binning_y) : 1.0;
00144 
00145     // Use correct principal point from calibration
00146     float center_x = (camera_info_msg->P[2] - camera_info_msg->roi.x_offset)*scale_x;
00147     float center_y = (camera_info_msg->P[6] - camera_info_msg->roi.y_offset)*scale_y;
00148 
00149     double fx = camera_info_msg->P[0] * scale_x;
00150     double fy = camera_info_msg->P[5] * scale_y;
00151 
00152     float constant_x = 1.0f / fx;
00153     float constant_y = 1.0f / fy;
00154 
00155     projection_map_x_.resize(width);
00156     projection_map_y_.resize(height);
00157     std::vector<float>::iterator projX = projection_map_x_.begin();
00158     std::vector<float>::iterator projY = projection_map_y_.begin();
00159 
00160     // precompute 3D projection matrix
00161     for (int v = 0; v < height; ++v, ++projY)
00162       *projY = (v - center_y) * constant_y;
00163 
00164     for (int u = 0; u < width; ++u, ++projX)
00165       *projX = (u - center_x) * constant_x;
00166 
00167     // reset shadow vectors
00168     reset();
00169   }
00170 }
00171 
00172 
00173 template<typename T>
00174   sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudSL(const sensor_msgs::ImageConstPtr& depth_msg,
00175                                                                     std::vector<uint32_t>& rgba_color_raw)
00176   {
00177 
00178     int width = depth_msg->width;
00179     int height = depth_msg->height;
00180 
00181     sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
00182     cloud_msg->data.resize(height * width * cloud_msg->point_step);
00183 
00184     uint32_t* color_img_ptr = 0;
00185 
00186     if (rgba_color_raw.size())
00187       color_img_ptr = &rgba_color_raw[0];
00188 
00190     // depth map to point cloud conversion
00192 
00193     float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
00194     const std::size_t point_step = cloud_msg->point_step;
00195 
00196     std::size_t point_count = 0;
00197     std::size_t point_idx = 0;
00198 
00199     double time_now = ros::Time::now().toSec();
00200     double time_expire = time_now+shadow_time_out_;
00201 
00202     const T* depth_img_ptr = (T*)&depth_msg->data[0];
00203 
00204     std::vector<float>::iterator proj_x;
00205     std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
00206 
00207     std::vector<float>::iterator proj_y ;
00208     std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
00209 
00210     // iterate over projection matrix
00211     for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
00212     {
00213       for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
00214                                                                     ++point_idx,
00215                                                                     ++depth_img_ptr)
00216       {
00217 
00218         T depth_raw = *depth_img_ptr;
00219         if (DepthTraits<T>::valid(depth_raw))
00220         {
00221           float depth = DepthTraits<T>::toMeters(depth_raw);
00222 
00223           // define point color
00224           uint32_t color;
00225           if (color_img_ptr)
00226           {
00227             color = *color_img_ptr;
00228           }
00229           else
00230           {
00231             color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
00232           }
00233 
00234           // fill in X,Y,Z and color
00235           *cloud_data_ptr = (*proj_x) * depth;  ++cloud_data_ptr;
00236           *cloud_data_ptr = (*proj_y) * depth;  ++cloud_data_ptr;
00237           *cloud_data_ptr = depth; ++cloud_data_ptr;
00238           *cloud_data_ptr = *reinterpret_cast<float*>(&color); ++cloud_data_ptr;
00239 
00240           ++point_count;
00241         }
00242 
00243         // increase color iterator pointer
00244         if (color_img_ptr)
00245           ++color_img_ptr;
00246       }
00247     }
00248 
00249     finalizingPointCloud(cloud_msg, point_count);
00250 
00251     return cloud_msg;
00252   }
00253 
00254 
00255 template<typename T>
00256   sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudML(const sensor_msgs::ImageConstPtr& depth_msg,
00257                                                                     std::vector<uint32_t>& rgba_color_raw)
00258   {
00259     int width = depth_msg->width;
00260     int height = depth_msg->height;
00261 
00262     sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
00263     cloud_msg->data.resize(height * width * cloud_msg->point_step * 2);
00264 
00265     uint32_t* color_img_ptr = 0;
00266 
00267     if (rgba_color_raw.size())
00268       color_img_ptr = &rgba_color_raw[0];
00269 
00271     // depth map to point cloud conversion
00273 
00274     float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
00275     uint8_t* cloud_shadow_buffer_ptr = &shadow_buffer_[0];
00276 
00277     const std::size_t point_step = cloud_msg->point_step;
00278 
00279     std::size_t point_count = 0;
00280     std::size_t point_idx = 0;
00281 
00282     double time_now = ros::Time::now().toSec();
00283     double time_expire = time_now-shadow_time_out_;
00284 
00285     const T* depth_img_ptr = (T*)&depth_msg->data[0];
00286 
00287     std::vector<float>::iterator proj_x;
00288     std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
00289 
00290     std::vector<float>::iterator proj_y ;
00291     std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
00292 
00293     // iterate over projection matrix
00294     for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
00295     {
00296       for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
00297                                                                     ++point_idx,
00298                                                                     ++depth_img_ptr,
00299                                                                     cloud_shadow_buffer_ptr += point_step)
00300       {
00301 
00302         // lookup shadow depth
00303         float shadow_depth = shadow_depth_[point_idx];
00304 
00305         // check for time-outs
00306         if ( (shadow_depth!=0.0f) && (shadow_timestamp_[point_idx]<time_expire) )
00307         {
00308           // clear shadow pixel
00309           shadow_depth = shadow_depth_[point_idx] = 0.0f;
00310         }
00311 
00312         T depth_raw = *depth_img_ptr;
00313         if (DepthTraits<T>::valid(depth_raw))
00314         {
00315           float depth = DepthTraits<T>::toMeters(depth_raw);
00316 
00317           // pointer to current point data
00318           float* cloud_data_pixel_ptr = cloud_data_ptr;
00319 
00320           // define point color
00321           uint32_t color;
00322           if (color_img_ptr)
00323           {
00324             color = *color_img_ptr;
00325           }
00326           else
00327           {
00328             color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
00329           }
00330 
00331           // fill in X,Y,Z and color
00332           *cloud_data_ptr = (*proj_x) * depth;  ++cloud_data_ptr;
00333           *cloud_data_ptr = (*proj_y) * depth;  ++cloud_data_ptr;
00334           *cloud_data_ptr = depth; ++cloud_data_ptr;
00335           *cloud_data_ptr = *reinterpret_cast<float*>(&color); ++cloud_data_ptr;
00336 
00337           ++point_count;
00338 
00339           // if shadow point exists -> display it
00340           if (depth < shadow_depth - shadow_distance_)
00341           {
00342             // copy point data from shadow buffer to point cloud
00343             memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
00344             cloud_data_ptr += 4;
00345             ++point_count;
00346           }
00347           else
00348           {
00349             // save a copy of current point to shadow buffer
00350             memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step);
00351 
00352             // reduce color intensity in shadow buffer
00353             RGBA* color = reinterpret_cast<RGBA*>(cloud_shadow_buffer_ptr + sizeof(float) * 3);
00354             color->red /= 2;
00355             color->green /= 2;
00356             color->blue /= 2;
00357 
00358             // update shadow depth & time out
00359             shadow_depth_[point_idx] = depth;
00360             shadow_timestamp_[point_idx] = time_now;
00361           }
00362 
00363         }
00364         else
00365         {
00366           // current depth pixel is invalid -> check shadow buffer
00367           if (shadow_depth != 0)
00368           {
00369             // copy shadow point to point cloud
00370             memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
00371             cloud_data_ptr += 4;
00372             ++point_count;
00373           }
00374         }
00375 
00376         // increase color iterator pointer
00377         if (color_img_ptr)
00378           ++color_img_ptr;
00379 
00380       }
00381     }
00382 
00383     finalizingPointCloud(cloud_msg, point_count);
00384 
00385     return cloud_msg;
00386   }
00387 
00388 
00389 template<typename T>
00390 void MultiLayerDepth::convertColor(const sensor_msgs::ImageConstPtr& color_msg,
00391                                    std::vector<uint32_t>& rgba_color_raw)
00392   {
00393     size_t i;
00394     size_t num_pixel = color_msg->width * color_msg->height;
00395 
00396     // query image properties
00397     int num_channels = enc::numChannels(color_msg->encoding);
00398 
00399     bool rgb_encoding = false;
00400     if (color_msg->encoding.find("rgb")!=std::string::npos)
00401       rgb_encoding = true;
00402 
00403     bool has_alpha = enc::hasAlpha(color_msg->encoding);
00404 
00405     // prepare output vector
00406     rgba_color_raw.clear();
00407     rgba_color_raw.reserve(num_pixel);
00408 
00409     uint8_t* img_ptr = (uint8_t*)&color_msg->data[sizeof(T) - 1];// pointer to most significant byte
00410 
00411     // color conversion
00412     switch (num_channels)
00413     {
00414       case 1:
00415         // grayscale image
00416         for (i = 0; i < num_pixel; ++i)
00417         {
00418           uint8_t gray_value = *img_ptr;
00419           img_ptr += sizeof(T);
00420 
00421           rgba_color_raw.push_back((uint32_t)gray_value << 16 | (uint32_t)gray_value << 8 | (uint32_t)gray_value);
00422         }
00423         break;
00424       case 3:
00425       case 4:
00426         // rgb/bgr encoding
00427         for (i = 0; i < num_pixel; ++i)
00428         {
00429           uint8_t color1 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
00430           uint8_t color2 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
00431           uint8_t color3 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
00432 
00433           if (has_alpha)
00434             img_ptr += sizeof(T); // skip alpha values
00435 
00436           if (rgb_encoding)
00437           {
00438             // rgb encoding
00439             rgba_color_raw.push_back((uint32_t)color1 << 16 | (uint32_t)color2 << 8 | (uint32_t)color3 << 0);
00440           } else
00441           {
00442             // bgr encoding
00443             rgba_color_raw.push_back((uint32_t)color3 << 16 | (uint32_t)color2 << 8 | (uint32_t)color1 << 0);
00444           }
00445         }
00446         break;
00447       default:
00448         break;
00449     }
00450 
00451   }
00452 
00453 sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudFromDepth(sensor_msgs::ImageConstPtr depth_msg,
00454                                                                          sensor_msgs::ImageConstPtr color_msg,
00455                                                                          sensor_msgs::CameraInfoConstPtr camera_info_msg)
00456 {
00457 
00458   // Add data to multi depth image
00459   sensor_msgs::PointCloud2Ptr point_cloud_out;
00460 
00461   // Bit depth of image encoding
00462   int bitDepth = enc::bitDepth(depth_msg->encoding);
00463   int numChannels = enc::numChannels(depth_msg->encoding);
00464 
00465   if (!camera_info_msg)
00466   {
00467     throw MultiLayerDepthException("Camera info missing!");
00468   }
00469 
00470   // precompute projection matrix and initialize shadow buffer
00471   initializeConversion(depth_msg, camera_info_msg);
00472 
00473   std::vector<uint32_t> rgba_color_raw_;
00474 
00475   if (color_msg)
00476   {
00477     if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height)
00478     {
00479       std::stringstream error_msg;
00480       error_msg << "Depth image resolution (" << (int)depth_msg->width << "x" << (int)depth_msg->height << ") "
00481           "does not match color image resolution (" << (int)color_msg->width << "x" << (int)color_msg->height << ")";
00482       throw( MultiLayerDepthException ( error_msg.str() ) );
00483     }
00484 
00485     // convert color coding to 8-bit rgb data
00486     switch (enc::bitDepth(color_msg->encoding))
00487     {
00488       case 8:
00489         convertColor<uint8_t>(color_msg, rgba_color_raw_);
00490         break;
00491       case 16:
00492         convertColor<uint16_t>(color_msg, rgba_color_raw_);
00493         break;
00494       default:
00495         std::string error_msg ("Color image has invalid bit depth");
00496         throw( MultiLayerDepthException (error_msg));
00497         break;
00498     }
00499   }
00500 
00501   if (!occlusion_compensation_)
00502   {
00503     // generate single layer depth cloud
00504 
00505     if ((bitDepth == 32) && (numChannels == 1))
00506     {
00507       // floating point encoded depth map
00508       point_cloud_out = generatePointCloudSL<float>(depth_msg, rgba_color_raw_);
00509     }
00510     else if ((bitDepth == 16) && (numChannels == 1))
00511     {
00512       // 32bit integer encoded depth map
00513       point_cloud_out = generatePointCloudSL<uint16_t>(depth_msg, rgba_color_raw_);
00514     }
00515   }
00516   else
00517   {
00518     // generate two layered depth cloud (depth+shadow)
00519 
00520     if ((bitDepth == 32) && (numChannels == 1))
00521     {
00522       // floating point encoded depth map
00523       point_cloud_out = generatePointCloudML<float>(depth_msg, rgba_color_raw_);
00524     }
00525     else if ((bitDepth == 16) && (numChannels == 1))
00526     {
00527       // 32bit integer encoded depth map
00528       point_cloud_out = generatePointCloudML<uint16_t>(depth_msg, rgba_color_raw_);
00529     }
00530   }
00531 
00532   if ( !point_cloud_out )
00533   {
00534     std::string error_msg ("Depth image has invalid format (only 16 bit and float are supported)!");
00535     throw( MultiLayerDepthException (error_msg));
00536   }
00537 
00538   return point_cloud_out;
00539 }
00540 
00541 sensor_msgs::PointCloud2Ptr MultiLayerDepth::initPointCloud()
00542 {
00543   sensor_msgs::PointCloud2Ptr point_cloud_out = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2());
00544 
00545   point_cloud_out->fields.resize(4);
00546   std::size_t point_offset = 0;
00547 
00548   point_cloud_out->fields[0].name = "x";
00549   point_cloud_out->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00550   point_cloud_out->fields[0].count = 1;
00551   point_cloud_out->fields[0].offset = point_offset;
00552   point_offset += sizeof(float);
00553 
00554   point_cloud_out->fields[1].name = "y";
00555   point_cloud_out->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00556   point_cloud_out->fields[1].count = 1;
00557   point_cloud_out->fields[1].offset = point_offset;
00558   point_offset += sizeof(float);
00559 
00560   point_cloud_out->fields[2].name = "z";
00561   point_cloud_out->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00562   point_cloud_out->fields[2].count = 1;
00563   point_cloud_out->fields[2].offset = point_offset;
00564   point_offset += sizeof(float);
00565 
00566   point_cloud_out->fields[3].name = "rgb";
00567   point_cloud_out->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00568   point_cloud_out->fields[3].count = 1;
00569   point_cloud_out->fields[3].offset = point_offset;
00570   point_offset += sizeof(float);
00571 
00572   point_cloud_out->point_step = point_offset;
00573 
00574   point_cloud_out->is_bigendian = false;
00575   point_cloud_out->is_dense = false;
00576 
00577   return point_cloud_out;
00578 }
00579 
00580 void MultiLayerDepth::finalizingPointCloud(sensor_msgs::PointCloud2Ptr& point_cloud, std::size_t size)
00581 {
00582   point_cloud->width = size;
00583   point_cloud->height = 1;
00584   point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step);
00585   point_cloud->row_step = point_cloud->point_step * point_cloud->width;
00586 }
00587 
00588 }


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Thu Aug 27 2015 15:02:27