depth_cloud_mld.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 20013, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  *
30  * Created on: Jan 22, 2013
31  * Author: jkammerl
32  */
33 
34 #include <sensor_msgs/CameraInfo.h>
36 
37 #include <string.h>
38 #include <sstream>
39 
40 #include "depth_cloud_mld.h"
41 
43 
44 #define POINT_STEP (sizeof(float)*4)
45 
46 namespace rviz
47 {
48 
49 // Encapsulate differences between processing float and uint16_t depths
50 template<typename T>
51  struct DepthTraits
52  {
53  };
54 
55 template<>
56  struct DepthTraits<uint16_t>
57  {
58  static inline bool valid(float depth)
59  {
60  return depth != 0.0;
61  }
62  static inline float toMeters(uint16_t depth)
63  {
64  return depth * 0.001f;
65  } // originally mm
66  };
67 
68 template<>
69  struct DepthTraits<float>
70  {
71  static inline bool valid(float depth)
72  {
73  return std::isfinite(depth);
74  }
75  static inline float toMeters(float depth)
76  {
77  return depth;
78  }
79  };
80 
81 
82 struct RGBA
83 {
84  uint8_t red;
85  uint8_t green;
86  uint8_t blue;
87  uint8_t alpha;
88 };
89 
90 
91 void MultiLayerDepth::initializeConversion(const sensor_msgs::ImageConstPtr& depth_msg,
92  sensor_msgs::CameraInfoConstPtr& camera_info_msg)
93 {
94 
95  if (!depth_msg || !camera_info_msg)
96  {
97  std::string error_msg ("Waiting for CameraInfo message..");
98  throw( MultiLayerDepthException (error_msg));
99  }
100 
101  // do some sanity checks
102  int binning_x = camera_info_msg->binning_x > 1 ? camera_info_msg->binning_x : 1;
103  int binning_y = camera_info_msg->binning_y > 1 ? camera_info_msg->binning_y : 1;
104 
105  int roi_width = camera_info_msg->roi.width > 0 ? camera_info_msg->roi.width : camera_info_msg->width;
106  int roi_height = camera_info_msg->roi.height > 0 ? camera_info_msg->roi.height : camera_info_msg->height;
107 
108  int expected_width = roi_width / binning_x;
109  int expected_height = roi_height / binning_y;
110 
111  if ( expected_width != depth_msg->width ||
112  expected_height != depth_msg->height )
113  {
114  std::ostringstream s;
115  s << "Depth image size and camera info don't match: ";
116  s << depth_msg->width << " x " << depth_msg->height;
117  s << " vs " << expected_width << " x " << expected_height;
118  s << "(binning: " << binning_x << " x " << binning_y;
119  s << ", ROI size: " << roi_width << " x " << roi_height << ")";
120  throw( MultiLayerDepthException (s.str()));
121  }
122 
123  int width = depth_msg->width;
124  int height = depth_msg->height;
125 
126  std::size_t size = width * height;
127 
128  if (size != shadow_depth_.size())
129  {
130  // Allocate memory for shadow processing
131  shadow_depth_.resize(size, 0.0f);
132  shadow_timestamp_.resize(size, 0.0);
133  shadow_buffer_.resize(size * POINT_STEP, 0);
134 
135  // Procompute 3D projection matrix
136  //
137  // The following computation of center_x,y and fx,fy duplicates
138  // code in the image_geometry package, but this avoids dependency
139  // on OpenCV, which simplifies releasing rviz.
140 
141  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
142  double scale_x = camera_info_msg->binning_x > 1 ? (1.0 / camera_info_msg->binning_x) : 1.0;
143  double scale_y = camera_info_msg->binning_y > 1 ? (1.0 / camera_info_msg->binning_y) : 1.0;
144 
145  // Use correct principal point from calibration
146  float center_x = (camera_info_msg->P[2] - camera_info_msg->roi.x_offset)*scale_x;
147  float center_y = (camera_info_msg->P[6] - camera_info_msg->roi.y_offset)*scale_y;
148 
149  double fx = camera_info_msg->P[0] * scale_x;
150  double fy = camera_info_msg->P[5] * scale_y;
151 
152  float constant_x = 1.0f / fx;
153  float constant_y = 1.0f / fy;
154 
155  projection_map_x_.resize(width);
156  projection_map_y_.resize(height);
157  std::vector<float>::iterator projX = projection_map_x_.begin();
158  std::vector<float>::iterator projY = projection_map_y_.begin();
159 
160  // precompute 3D projection matrix
161  for (int v = 0; v < height; ++v, ++projY)
162  *projY = (v - center_y) * constant_y;
163 
164  for (int u = 0; u < width; ++u, ++projX)
165  *projX = (u - center_x) * constant_x;
166 
167  // reset shadow vectors
168  reset();
169  }
170 }
171 
172 
173 template<typename T>
174  sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudSL(const sensor_msgs::ImageConstPtr& depth_msg,
175  std::vector<uint32_t>& rgba_color_raw)
176  {
177 
178  int width = depth_msg->width;
179  int height = depth_msg->height;
180 
181  sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
182  cloud_msg->data.resize(height * width * cloud_msg->point_step);
183 
184  uint32_t* color_img_ptr = 0;
185 
186  if (rgba_color_raw.size())
187  color_img_ptr = &rgba_color_raw[0];
188 
190  // depth map to point cloud conversion
192 
193  float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
194  const std::size_t point_step = cloud_msg->point_step;
195 
196  std::size_t point_count = 0;
197  std::size_t point_idx = 0;
198 
199  double time_now = ros::Time::now().toSec();
200  double time_expire = time_now+shadow_time_out_;
201 
202  const T* depth_img_ptr = (T*)&depth_msg->data[0];
203 
204  std::vector<float>::iterator proj_x;
205  std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
206 
207  std::vector<float>::iterator proj_y ;
208  std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
209 
210  // iterate over projection matrix
211  for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
212  {
213  for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
214  ++point_idx,
215  ++depth_img_ptr)
216  {
217 
218  T depth_raw = *depth_img_ptr;
219  if (DepthTraits<T>::valid(depth_raw))
220  {
221  float depth = DepthTraits<T>::toMeters(depth_raw);
222 
223  // define point color
224  uint32_t color;
225  if (color_img_ptr)
226  {
227  color = *color_img_ptr;
228  }
229  else
230  {
231  color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
232  }
233 
234  // fill in X,Y,Z and color
235  *cloud_data_ptr = (*proj_x) * depth; ++cloud_data_ptr;
236  *cloud_data_ptr = (*proj_y) * depth; ++cloud_data_ptr;
237  *cloud_data_ptr = depth; ++cloud_data_ptr;
238  *cloud_data_ptr = *reinterpret_cast<float*>(&color); ++cloud_data_ptr;
239 
240  ++point_count;
241  }
242 
243  // increase color iterator pointer
244  if (color_img_ptr)
245  ++color_img_ptr;
246  }
247  }
248 
249  finalizingPointCloud(cloud_msg, point_count);
250 
251  return cloud_msg;
252  }
253 
254 
255 template<typename T>
256  sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudML(const sensor_msgs::ImageConstPtr& depth_msg,
257  std::vector<uint32_t>& rgba_color_raw)
258  {
259  int width = depth_msg->width;
260  int height = depth_msg->height;
261 
262  sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
263  cloud_msg->data.resize(height * width * cloud_msg->point_step * 2);
264 
265  uint32_t* color_img_ptr = 0;
266 
267  if (rgba_color_raw.size())
268  color_img_ptr = &rgba_color_raw[0];
269 
271  // depth map to point cloud conversion
273 
274  float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
275  uint8_t* cloud_shadow_buffer_ptr = &shadow_buffer_[0];
276 
277  const std::size_t point_step = cloud_msg->point_step;
278 
279  std::size_t point_count = 0;
280  std::size_t point_idx = 0;
281 
282  double time_now = ros::Time::now().toSec();
283  double time_expire = time_now-shadow_time_out_;
284 
285  const T* depth_img_ptr = (T*)&depth_msg->data[0];
286 
287  std::vector<float>::iterator proj_x;
288  std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
289 
290  std::vector<float>::iterator proj_y ;
291  std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
292 
293  // iterate over projection matrix
294  for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
295  {
296  for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
297  ++point_idx,
298  ++depth_img_ptr,
299  cloud_shadow_buffer_ptr += point_step)
300  {
301 
302  // lookup shadow depth
303  float shadow_depth = shadow_depth_[point_idx];
304 
305  // check for time-outs
306  if ( (shadow_depth!=0.0f) && (shadow_timestamp_[point_idx]<time_expire) )
307  {
308  // clear shadow pixel
309  shadow_depth = shadow_depth_[point_idx] = 0.0f;
310  }
311 
312  T depth_raw = *depth_img_ptr;
313  if (DepthTraits<T>::valid(depth_raw))
314  {
315  float depth = DepthTraits<T>::toMeters(depth_raw);
316 
317  // pointer to current point data
318  float* cloud_data_pixel_ptr = cloud_data_ptr;
319 
320  // define point color
321  uint32_t color;
322  if (color_img_ptr)
323  {
324  color = *color_img_ptr;
325  }
326  else
327  {
328  color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
329  }
330 
331  // fill in X,Y,Z and color
332  *cloud_data_ptr = (*proj_x) * depth; ++cloud_data_ptr;
333  *cloud_data_ptr = (*proj_y) * depth; ++cloud_data_ptr;
334  *cloud_data_ptr = depth; ++cloud_data_ptr;
335  *cloud_data_ptr = *reinterpret_cast<float*>(&color); ++cloud_data_ptr;
336 
337  ++point_count;
338 
339  // if shadow point exists -> display it
340  if (depth < shadow_depth - shadow_distance_)
341  {
342  // copy point data from shadow buffer to point cloud
343  memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
344  cloud_data_ptr += 4;
345  ++point_count;
346  }
347  else
348  {
349  // save a copy of current point to shadow buffer
350  memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step);
351 
352  // reduce color intensity in shadow buffer
353  RGBA* color = reinterpret_cast<RGBA*>(cloud_shadow_buffer_ptr + sizeof(float) * 3);
354  color->red /= 2;
355  color->green /= 2;
356  color->blue /= 2;
357 
358  // update shadow depth & time out
359  shadow_depth_[point_idx] = depth;
360  shadow_timestamp_[point_idx] = time_now;
361  }
362 
363  }
364  else
365  {
366  // current depth pixel is invalid -> check shadow buffer
367  if (shadow_depth != 0)
368  {
369  // copy shadow point to point cloud
370  memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
371  cloud_data_ptr += 4;
372  ++point_count;
373  }
374  }
375 
376  // increase color iterator pointer
377  if (color_img_ptr)
378  ++color_img_ptr;
379 
380  }
381  }
382 
383  finalizingPointCloud(cloud_msg, point_count);
384 
385  return cloud_msg;
386  }
387 
388 
389 template<typename T>
390 void MultiLayerDepth::convertColor(const sensor_msgs::ImageConstPtr& color_msg,
391  std::vector<uint32_t>& rgba_color_raw)
392  {
393  size_t i;
394  size_t num_pixel = color_msg->width * color_msg->height;
395 
396  // query image properties
397  int num_channels = enc::numChannels(color_msg->encoding);
398 
399  bool rgb_encoding = false;
400  if (color_msg->encoding.find("rgb")!=std::string::npos)
401  rgb_encoding = true;
402 
403  bool has_alpha = enc::hasAlpha(color_msg->encoding);
404 
405  // prepare output vector
406  rgba_color_raw.clear();
407  rgba_color_raw.reserve(num_pixel);
408 
409  uint8_t* img_ptr = (uint8_t*)&color_msg->data[sizeof(T) - 1];// pointer to most significant byte
410 
411  // color conversion
412  switch (num_channels)
413  {
414  case 1:
415  // grayscale image
416  for (i = 0; i < num_pixel; ++i)
417  {
418  uint8_t gray_value = *img_ptr;
419  img_ptr += sizeof(T);
420 
421  rgba_color_raw.push_back((uint32_t)gray_value << 16 | (uint32_t)gray_value << 8 | (uint32_t)gray_value);
422  }
423  break;
424  case 3:
425  case 4:
426  // rgb/bgr encoding
427  for (i = 0; i < num_pixel; ++i)
428  {
429  uint8_t color1 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
430  uint8_t color2 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
431  uint8_t color3 = *((uint8_t*)img_ptr); img_ptr += sizeof(T);
432 
433  if (has_alpha)
434  img_ptr += sizeof(T); // skip alpha values
435 
436  if (rgb_encoding)
437  {
438  // rgb encoding
439  rgba_color_raw.push_back((uint32_t)color1 << 16 | (uint32_t)color2 << 8 | (uint32_t)color3 << 0);
440  } else
441  {
442  // bgr encoding
443  rgba_color_raw.push_back((uint32_t)color3 << 16 | (uint32_t)color2 << 8 | (uint32_t)color1 << 0);
444  }
445  }
446  break;
447  default:
448  break;
449  }
450 
451  }
452 
453 sensor_msgs::PointCloud2Ptr MultiLayerDepth::generatePointCloudFromDepth(sensor_msgs::ImageConstPtr depth_msg,
454  sensor_msgs::ImageConstPtr color_msg,
455  sensor_msgs::CameraInfoConstPtr camera_info_msg)
456 {
457 
458  // Add data to multi depth image
459  sensor_msgs::PointCloud2Ptr point_cloud_out;
460 
461  // Bit depth of image encoding
462  int bitDepth = enc::bitDepth(depth_msg->encoding);
463  int numChannels = enc::numChannels(depth_msg->encoding);
464 
465  if (!camera_info_msg)
466  {
467  throw MultiLayerDepthException("Camera info missing!");
468  }
469 
470  // precompute projection matrix and initialize shadow buffer
471  initializeConversion(depth_msg, camera_info_msg);
472 
473  std::vector<uint32_t> rgba_color_raw_;
474 
475  if (color_msg)
476  {
477  if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height)
478  {
479  std::stringstream error_msg;
480  error_msg << "Depth image resolution (" << (int)depth_msg->width << "x" << (int)depth_msg->height << ") "
481  "does not match color image resolution (" << (int)color_msg->width << "x" << (int)color_msg->height << ")";
482  throw( MultiLayerDepthException ( error_msg.str() ) );
483  }
484 
485  // convert color coding to 8-bit rgb data
486  switch (enc::bitDepth(color_msg->encoding))
487  {
488  case 8:
489  convertColor<uint8_t>(color_msg, rgba_color_raw_);
490  break;
491  case 16:
492  convertColor<uint16_t>(color_msg, rgba_color_raw_);
493  break;
494  default:
495  std::string error_msg ("Color image has invalid bit depth");
496  throw( MultiLayerDepthException (error_msg));
497  break;
498  }
499  }
500 
501  if (!occlusion_compensation_)
502  {
503  // generate single layer depth cloud
504 
505  if ((bitDepth == 32) && (numChannels == 1))
506  {
507  // floating point encoded depth map
508  point_cloud_out = generatePointCloudSL<float>(depth_msg, rgba_color_raw_);
509  }
510  else if ((bitDepth == 16) && (numChannels == 1))
511  {
512  // 32bit integer encoded depth map
513  point_cloud_out = generatePointCloudSL<uint16_t>(depth_msg, rgba_color_raw_);
514  }
515  }
516  else
517  {
518  // generate two layered depth cloud (depth+shadow)
519 
520  if ((bitDepth == 32) && (numChannels == 1))
521  {
522  // floating point encoded depth map
523  point_cloud_out = generatePointCloudML<float>(depth_msg, rgba_color_raw_);
524  }
525  else if ((bitDepth == 16) && (numChannels == 1))
526  {
527  // 32bit integer encoded depth map
528  point_cloud_out = generatePointCloudML<uint16_t>(depth_msg, rgba_color_raw_);
529  }
530  }
531 
532  if ( !point_cloud_out )
533  {
534  std::string error_msg ("Depth image has invalid format (only 16 bit and float are supported)!");
535  throw( MultiLayerDepthException (error_msg));
536  }
537 
538  return point_cloud_out;
539 }
540 
541 sensor_msgs::PointCloud2Ptr MultiLayerDepth::initPointCloud()
542 {
543  sensor_msgs::PointCloud2Ptr point_cloud_out = sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2());
544 
545  point_cloud_out->fields.resize(4);
546  std::size_t point_offset = 0;
547 
548  point_cloud_out->fields[0].name = "x";
549  point_cloud_out->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
550  point_cloud_out->fields[0].count = 1;
551  point_cloud_out->fields[0].offset = point_offset;
552  point_offset += sizeof(float);
553 
554  point_cloud_out->fields[1].name = "y";
555  point_cloud_out->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
556  point_cloud_out->fields[1].count = 1;
557  point_cloud_out->fields[1].offset = point_offset;
558  point_offset += sizeof(float);
559 
560  point_cloud_out->fields[2].name = "z";
561  point_cloud_out->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
562  point_cloud_out->fields[2].count = 1;
563  point_cloud_out->fields[2].offset = point_offset;
564  point_offset += sizeof(float);
565 
566  point_cloud_out->fields[3].name = "rgb";
567  point_cloud_out->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
568  point_cloud_out->fields[3].count = 1;
569  point_cloud_out->fields[3].offset = point_offset;
570  point_offset += sizeof(float);
571 
572  point_cloud_out->point_step = point_offset;
573 
574  point_cloud_out->is_bigendian = false;
575  point_cloud_out->is_dense = false;
576 
577  return point_cloud_out;
578 }
579 
580 void MultiLayerDepth::finalizingPointCloud(sensor_msgs::PointCloud2Ptr& point_cloud, std::size_t size)
581 {
582  point_cloud->width = size;
583  point_cloud->height = 1;
584  point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step);
585  point_cloud->row_step = point_cloud->point_step * point_cloud->width;
586 }
587 
588 }
static float toMeters(float depth)
f
XmlRpcServer s
sensor_msgs::PointCloud2Ptr generatePointCloudSL(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate single-layered depth cloud (depth only)
#define POINT_STEP
static bool valid(float depth)
sensor_msgs::PointCloud2Ptr generatePointCloudFromDepth(sensor_msgs::ImageConstPtr depth_msg, sensor_msgs::ImageConstPtr color_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
sensor_msgs::PointCloud2Ptr initPointCloud()
static Time now()
void initializeConversion(const sensor_msgs::ImageConstPtr &depth_msg, sensor_msgs::CameraInfoConstPtr &camera_info_msg)
Precompute projection matrix, initialize buffers.
static bool valid(float depth)
void convertColor(const sensor_msgs::ImageConstPtr &color_msg, std::vector< uint32_t > &rgba_color_raw)
Convert color data to RGBA format.
void finalizingPointCloud(sensor_msgs::PointCloud2Ptr &point_cloud, std::size_t size)
static float toMeters(uint16_t depth)
sensor_msgs::PointCloud2Ptr generatePointCloudML(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate multi-layered depth cloud (depth+shadow)


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:50