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 // Encapsulate differences between processing float and uint16_t depths
49 template <typename T>
51 {
52 };
53 
54 template <>
55 struct DepthTraits<uint16_t>
56 {
57  static inline bool valid(float depth)
58  {
59  return depth != 0.0;
60  }
61  static inline float toMeters(uint16_t depth)
62  {
63  return depth * 0.001f;
64  } // originally mm
65 };
66 
67 template <>
68 struct DepthTraits<float>
69 {
70  static inline bool valid(float depth)
71  {
72  return std::isfinite(depth);
73  }
74  static inline float toMeters(float depth)
75  {
76  return depth;
77  }
78 };
79 
80 
81 struct RGBA
82 {
83  uint8_t red;
84  uint8_t green;
85  uint8_t blue;
86  uint8_t alpha;
87 };
88 
89 
90 void MultiLayerDepth::initializeConversion(const sensor_msgs::ImageConstPtr& depth_msg,
91  sensor_msgs::CameraInfoConstPtr& camera_info_msg)
92 {
93  if (!depth_msg || !camera_info_msg)
94  {
95  std::string error_msg("Waiting for CameraInfo message..");
96  throw(MultiLayerDepthException(error_msg));
97  }
98 
99  // do some sanity checks
100  unsigned binning_x = camera_info_msg->binning_x > 1 ? camera_info_msg->binning_x : 1;
101  unsigned binning_y = camera_info_msg->binning_y > 1 ? camera_info_msg->binning_y : 1;
102 
103  unsigned roi_width =
104  camera_info_msg->roi.width > 0 ? camera_info_msg->roi.width : camera_info_msg->width;
105  unsigned roi_height =
106  camera_info_msg->roi.height > 0 ? camera_info_msg->roi.height : camera_info_msg->height;
107 
108  unsigned expected_width = roi_width / binning_x;
109  unsigned expected_height = roi_height / binning_y;
110 
111  if (expected_width != depth_msg->width || expected_height != depth_msg->height)
112  {
113  std::ostringstream s;
114  s << "Depth image size and camera info don't match: ";
115  s << depth_msg->width << " x " << depth_msg->height;
116  s << " vs " << expected_width << " x " << expected_height;
117  s << "(binning: " << binning_x << " x " << binning_y;
118  s << ", ROI size: " << roi_width << " x " << roi_height << ")";
119  throw(MultiLayerDepthException(s.str()));
120  }
121 
122  int width = depth_msg->width;
123  int height = depth_msg->height;
124 
125  std::size_t size = width * height;
126 
127  if (size != shadow_depth_.size())
128  {
129  // Allocate memory for shadow processing
130  shadow_depth_.resize(size, 0.0f);
131  shadow_timestamp_.resize(size, 0.0);
132  shadow_buffer_.resize(size * POINT_STEP, 0);
133 
134  // Procompute 3D projection matrix
135  //
136  // The following computation of center_x,y and fx,fy duplicates
137  // code in the image_geometry package, but this avoids dependency
138  // on OpenCV, which simplifies releasing rviz.
139 
140  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
141  double scale_x = camera_info_msg->binning_x > 1 ? (1.0 / camera_info_msg->binning_x) : 1.0;
142  double scale_y = camera_info_msg->binning_y > 1 ? (1.0 / camera_info_msg->binning_y) : 1.0;
143 
144  // Use correct principal point from calibration
145  float center_x = (camera_info_msg->P[2] - camera_info_msg->roi.x_offset) * scale_x;
146  float center_y = (camera_info_msg->P[6] - camera_info_msg->roi.y_offset) * scale_y;
147 
148  double fx = camera_info_msg->P[0] * scale_x;
149  double fy = camera_info_msg->P[5] * scale_y;
150 
151  float constant_x = 1.0f / fx;
152  float constant_y = 1.0f / fy;
153 
154  projection_map_x_.resize(width);
155  projection_map_y_.resize(height);
156  std::vector<float>::iterator projX = projection_map_x_.begin();
157  std::vector<float>::iterator projY = projection_map_y_.begin();
158 
159  // precompute 3D projection matrix
160  for (int v = 0; v < height; ++v, ++projY)
161  *projY = (v - center_y) * constant_y;
162 
163  for (int u = 0; u < width; ++u, ++projX)
164  *projX = (u - center_x) * constant_x;
165 
166  // reset shadow vectors
167  reset();
168  }
169 }
170 
171 
172 template <typename T>
173 sensor_msgs::PointCloud2Ptr
174 MultiLayerDepth::generatePointCloudSL(const sensor_msgs::ImageConstPtr& depth_msg,
175  std::vector<uint32_t>& rgba_color_raw)
176 {
177  int width = depth_msg->width;
178  int height = depth_msg->height;
179 
180  sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
181  cloud_msg->data.resize(height * width * cloud_msg->point_step);
182 
183  uint32_t* color_img_ptr = nullptr;
184 
185  if (!rgba_color_raw.empty())
186  color_img_ptr = &rgba_color_raw[0];
187 
189  // depth map to point cloud conversion
191 
192  float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
193 
194  std::size_t point_count = 0;
195  std::size_t point_idx = 0;
196 
197  const T* depth_img_ptr = (T*)&depth_msg->data[0];
198 
199  std::vector<float>::iterator proj_x;
200  std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
201 
202  std::vector<float>::iterator proj_y;
203  std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
204 
205  // iterate over projection matrix
206  for (proj_y = projection_map_y_.begin(); proj_y != proj_y_end; ++proj_y)
207  {
208  for (proj_x = projection_map_x_.begin(); proj_x != proj_x_end; ++proj_x, ++point_idx, ++depth_img_ptr)
209  {
210  T depth_raw = *depth_img_ptr;
211  if (DepthTraits<T>::valid(depth_raw))
212  {
213  float depth = DepthTraits<T>::toMeters(depth_raw);
214 
215  // define point color
216  uint32_t color;
217  if (color_img_ptr)
218  {
219  color = *color_img_ptr;
220  }
221  else
222  {
223  color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
224  }
225 
226  // fill in X,Y,Z and color
227  *cloud_data_ptr = (*proj_x) * depth;
228  ++cloud_data_ptr;
229  *cloud_data_ptr = (*proj_y) * depth;
230  ++cloud_data_ptr;
231  *cloud_data_ptr = depth;
232  ++cloud_data_ptr;
233  *cloud_data_ptr = *reinterpret_cast<float*>(&color);
234  ++cloud_data_ptr;
235 
236  ++point_count;
237  }
238 
239  // increase color iterator pointer
240  if (color_img_ptr)
241  ++color_img_ptr;
242  }
243  }
244 
245  finalizingPointCloud(cloud_msg, point_count);
246 
247  return cloud_msg;
248 }
249 
250 
251 template <typename T>
252 sensor_msgs::PointCloud2Ptr
253 MultiLayerDepth::generatePointCloudML(const sensor_msgs::ImageConstPtr& depth_msg,
254  std::vector<uint32_t>& rgba_color_raw)
255 {
256  int width = depth_msg->width;
257  int height = depth_msg->height;
258 
259  sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
260  cloud_msg->data.resize(height * width * cloud_msg->point_step * 2);
261 
262  uint32_t* color_img_ptr = nullptr;
263 
264  if (!rgba_color_raw.empty())
265  color_img_ptr = &rgba_color_raw[0];
266 
268  // depth map to point cloud conversion
270 
271  float* cloud_data_ptr = reinterpret_cast<float*>(&cloud_msg->data[0]);
272  uint8_t* cloud_shadow_buffer_ptr = &shadow_buffer_[0];
273 
274  const std::size_t point_step = cloud_msg->point_step;
275 
276  std::size_t point_count = 0;
277  std::size_t point_idx = 0;
278 
279  double time_now = ros::Time::now().toSec();
280  double time_expire = time_now - shadow_time_out_;
281 
282  const T* depth_img_ptr = (T*)&depth_msg->data[0];
283 
284  std::vector<float>::iterator proj_x;
285  std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
286 
287  std::vector<float>::iterator proj_y;
288  std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
289 
290  // iterate over projection matrix
291  for (proj_y = projection_map_y_.begin(); proj_y != proj_y_end; ++proj_y)
292  {
293  for (proj_x = projection_map_x_.begin(); proj_x != proj_x_end;
294  ++proj_x, ++point_idx, ++depth_img_ptr, cloud_shadow_buffer_ptr += point_step)
295  {
296  // lookup shadow depth
297  float shadow_depth = shadow_depth_[point_idx];
298 
299  // check for time-outs
300  if ((shadow_depth != 0.0f) && (shadow_timestamp_[point_idx] < time_expire))
301  {
302  // clear shadow pixel
303  shadow_depth = shadow_depth_[point_idx] = 0.0f;
304  }
305 
306  T depth_raw = *depth_img_ptr;
307  if (DepthTraits<T>::valid(depth_raw))
308  {
309  float depth = DepthTraits<T>::toMeters(depth_raw);
310 
311  // pointer to current point data
312  float* cloud_data_pixel_ptr = cloud_data_ptr;
313 
314  // define point color
315  uint32_t color;
316  if (color_img_ptr)
317  {
318  color = *color_img_ptr;
319  }
320  else
321  {
322  color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
323  }
324 
325  // fill in X,Y,Z and color
326  *cloud_data_ptr = (*proj_x) * depth;
327  ++cloud_data_ptr;
328  *cloud_data_ptr = (*proj_y) * depth;
329  ++cloud_data_ptr;
330  *cloud_data_ptr = depth;
331  ++cloud_data_ptr;
332  *cloud_data_ptr = *reinterpret_cast<float*>(&color);
333  ++cloud_data_ptr;
334 
335  ++point_count;
336 
337  // if shadow point exists -> display it
338  if (depth < shadow_depth - shadow_distance_)
339  {
340  // copy point data from shadow buffer to point cloud
341  memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
342  cloud_data_ptr += 4;
343  ++point_count;
344  }
345  else
346  {
347  // save a copy of current point to shadow buffer
348  memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step);
349 
350  // reduce color intensity in shadow buffer
351  RGBA* color = reinterpret_cast<RGBA*>(cloud_shadow_buffer_ptr + sizeof(float) * 3);
352  color->red /= 2;
353  color->green /= 2;
354  color->blue /= 2;
355 
356  // update shadow depth & time out
357  shadow_depth_[point_idx] = depth;
358  shadow_timestamp_[point_idx] = time_now;
359  }
360  }
361  else
362  {
363  // current depth pixel is invalid -> check shadow buffer
364  if (shadow_depth != 0)
365  {
366  // copy shadow point to point cloud
367  memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
368  cloud_data_ptr += 4;
369  ++point_count;
370  }
371  }
372 
373  // increase color iterator pointer
374  if (color_img_ptr)
375  ++color_img_ptr;
376  }
377  }
378 
379  finalizingPointCloud(cloud_msg, point_count);
380 
381  return cloud_msg;
382 }
383 
384 
385 template <typename T>
386 void MultiLayerDepth::convertColor(const sensor_msgs::ImageConstPtr& color_msg,
387  std::vector<uint32_t>& rgba_color_raw)
388 {
389  size_t i;
390  size_t num_pixel = color_msg->width * color_msg->height;
391 
392  // query image properties
393  int num_channels = enc::numChannels(color_msg->encoding);
394 
395  bool rgb_encoding = false;
396  if (color_msg->encoding.find("rgb") != std::string::npos)
397  rgb_encoding = true;
398 
399  bool has_alpha = enc::hasAlpha(color_msg->encoding);
400 
401  // prepare output vector
402  rgba_color_raw.clear();
403  rgba_color_raw.reserve(num_pixel);
404 
405  uint8_t* img_ptr = (uint8_t*)&color_msg->data[sizeof(T) - 1]; // pointer to most significant byte
406 
407  // color conversion
408  switch (num_channels)
409  {
410  case 1:
411  // grayscale image
412  for (i = 0; i < num_pixel; ++i)
413  {
414  uint8_t gray_value = *img_ptr;
415  img_ptr += sizeof(T);
416 
417  rgba_color_raw.push_back((uint32_t)gray_value << 16 | (uint32_t)gray_value << 8 |
418  (uint32_t)gray_value);
419  }
420  break;
421  case 3:
422  case 4:
423  // rgb/bgr encoding
424  for (i = 0; i < num_pixel; ++i)
425  {
426  uint8_t color1 = *((uint8_t*)img_ptr);
427  img_ptr += sizeof(T);
428  uint8_t color2 = *((uint8_t*)img_ptr);
429  img_ptr += sizeof(T);
430  uint8_t color3 = *((uint8_t*)img_ptr);
431  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  }
441  else
442  {
443  // bgr encoding
444  rgba_color_raw.push_back((uint32_t)color3 << 16 | (uint32_t)color2 << 8 | (uint32_t)color1 << 0);
445  }
446  }
447  break;
448  default:
449  break;
450  }
451 }
452 
453 sensor_msgs::PointCloud2Ptr
454 MultiLayerDepth::generatePointCloudFromDepth(const sensor_msgs::ImageConstPtr& depth_msg,
455  const sensor_msgs::ImageConstPtr& color_msg,
456  sensor_msgs::CameraInfoConstPtr camera_info_msg)
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  << ") "
482  "does not match color image resolution ("
483  << (int)color_msg->width << "x" << (int)color_msg->height << ")";
484  throw(MultiLayerDepthException(error_msg.str()));
485  }
486 
487  // convert color coding to 8-bit rgb data
488  switch (enc::bitDepth(color_msg->encoding))
489  {
490  case 8:
491  convertColor<uint8_t>(color_msg, rgba_color_raw_);
492  break;
493  case 16:
494  convertColor<uint16_t>(color_msg, rgba_color_raw_);
495  break;
496  default:
497  std::string error_msg("Color image has invalid bit depth");
498  throw(MultiLayerDepthException(error_msg));
499  break;
500  }
501  }
502 
504  {
505  // generate single layer depth cloud
506 
507  if ((bitDepth == 32) && (numChannels == 1))
508  {
509  // floating point encoded depth map
510  point_cloud_out = generatePointCloudSL<float>(depth_msg, rgba_color_raw_);
511  }
512  else if ((bitDepth == 16) && (numChannels == 1))
513  {
514  // 32bit integer encoded depth map
515  point_cloud_out = generatePointCloudSL<uint16_t>(depth_msg, rgba_color_raw_);
516  }
517  }
518  else
519  {
520  // generate two layered depth cloud (depth+shadow)
521 
522  if ((bitDepth == 32) && (numChannels == 1))
523  {
524  // floating point encoded depth map
525  point_cloud_out = generatePointCloudML<float>(depth_msg, rgba_color_raw_);
526  }
527  else if ((bitDepth == 16) && (numChannels == 1))
528  {
529  // 32bit integer encoded depth map
530  point_cloud_out = generatePointCloudML<uint16_t>(depth_msg, rgba_color_raw_);
531  }
532  }
533 
534  if (!point_cloud_out)
535  {
536  std::string error_msg("Depth image has invalid format (only 16 bit and float are supported)!");
537  throw(MultiLayerDepthException(error_msg));
538  }
539 
540  return point_cloud_out;
541 }
542 
543 sensor_msgs::PointCloud2Ptr MultiLayerDepth::initPointCloud()
544 {
545  sensor_msgs::PointCloud2Ptr point_cloud_out =
546  sensor_msgs::PointCloud2Ptr(new sensor_msgs::PointCloud2());
547 
548  point_cloud_out->fields.resize(4);
549  std::size_t point_offset = 0;
550 
551  point_cloud_out->fields[0].name = "x";
552  point_cloud_out->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
553  point_cloud_out->fields[0].count = 1;
554  point_cloud_out->fields[0].offset = point_offset;
555  point_offset += sizeof(float);
556 
557  point_cloud_out->fields[1].name = "y";
558  point_cloud_out->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
559  point_cloud_out->fields[1].count = 1;
560  point_cloud_out->fields[1].offset = point_offset;
561  point_offset += sizeof(float);
562 
563  point_cloud_out->fields[2].name = "z";
564  point_cloud_out->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
565  point_cloud_out->fields[2].count = 1;
566  point_cloud_out->fields[2].offset = point_offset;
567  point_offset += sizeof(float);
568 
569  point_cloud_out->fields[3].name = "rgb";
570  point_cloud_out->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
571  point_cloud_out->fields[3].count = 1;
572  point_cloud_out->fields[3].offset = point_offset;
573  point_offset += sizeof(float);
574 
575  point_cloud_out->point_step = point_offset;
576 
577  point_cloud_out->is_bigendian = false;
578  point_cloud_out->is_dense = false;
579 
580  return point_cloud_out;
581 }
582 
583 void MultiLayerDepth::finalizingPointCloud(sensor_msgs::PointCloud2Ptr& point_cloud, std::size_t size)
584 {
585  point_cloud->width = size;
586  point_cloud->height = 1;
587  point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step);
588  point_cloud->row_step = point_cloud->point_step * point_cloud->width;
589 }
590 
591 } // namespace rviz
rviz::DepthTraits< uint16_t >::valid
static bool valid(float depth)
Definition: depth_cloud_mld.cpp:57
sensor_msgs::image_encodings
rviz::MultiLayerDepth::convertColor
void convertColor(const sensor_msgs::ImageConstPtr &color_msg, std::vector< uint32_t > &rgba_color_raw)
Convert color data to RGBA format.
Definition: depth_cloud_mld.cpp:386
rviz::DepthTraits< uint16_t >::toMeters
static float toMeters(uint16_t depth)
Definition: depth_cloud_mld.cpp:61
rviz::MultiLayerDepth::generatePointCloudFromDepth
sensor_msgs::PointCloud2Ptr generatePointCloudFromDepth(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &color_msg, sensor_msgs::CameraInfoConstPtr camera_info_msg)
Definition: depth_cloud_mld.cpp:454
image_encodings.h
s
XmlRpcServer s
rviz::MultiLayerDepth::shadow_time_out_
double shadow_time_out_
Definition: depth_cloud_mld.h:133
depth_cloud_mld.h
rviz::DepthTraits< float >::toMeters
static float toMeters(float depth)
Definition: depth_cloud_mld.cpp:74
rviz::MultiLayerDepth::generatePointCloudSL
sensor_msgs::PointCloud2Ptr generatePointCloudSL(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate single-layered depth cloud (depth only)
Definition: depth_cloud_mld.cpp:174
rviz::DepthTraits
Definition: depth_cloud_mld.cpp:50
TimeBase< Time, Duration >::toSec
double toSec() const
rviz::MultiLayerDepth::shadow_depth_
std::vector< float > shadow_depth_
Definition: depth_cloud_mld.h:127
POINT_STEP
#define POINT_STEP
Definition: depth_cloud_mld.cpp:44
rviz::MultiLayerDepth::shadow_distance_
float shadow_distance_
Definition: depth_cloud_mld.h:134
f
f
rviz::RGBA::red
uint8_t red
Definition: depth_cloud_mld.cpp:83
rviz::MultiLayerDepth::shadow_buffer_
std::vector< uint8_t > shadow_buffer_
Definition: depth_cloud_mld.h:129
rviz::MultiLayerDepth::reset
void reset()
Definition: depth_cloud_mld.h:89
rviz::MultiLayerDepthException
Definition: depth_cloud_mld.h:47
rviz
Definition: add_display_dialog.cpp:54
rviz::MultiLayerDepth::shadow_timestamp_
std::vector< double > shadow_timestamp_
Definition: depth_cloud_mld.h:128
rviz::RGBA::blue
uint8_t blue
Definition: depth_cloud_mld.cpp:85
rviz::MultiLayerDepth::projection_map_x_
std::vector< float > projection_map_x_
Definition: depth_cloud_mld.h:123
rviz::MultiLayerDepth::initializeConversion
void initializeConversion(const sensor_msgs::ImageConstPtr &depth_msg, sensor_msgs::CameraInfoConstPtr &camera_info_msg)
Precompute projection matrix, initialize buffers.
Definition: depth_cloud_mld.cpp:90
rviz::MultiLayerDepth::projection_map_y_
std::vector< float > projection_map_y_
Definition: depth_cloud_mld.h:124
rviz::RGBA::green
uint8_t green
Definition: depth_cloud_mld.cpp:84
rviz::DepthTraits< float >::valid
static bool valid(float depth)
Definition: depth_cloud_mld.cpp:70
rviz::RGBA::alpha
uint8_t alpha
Definition: depth_cloud_mld.cpp:86
rviz::MultiLayerDepth::initPointCloud
sensor_msgs::PointCloud2Ptr initPointCloud()
Definition: depth_cloud_mld.cpp:543
rviz::MultiLayerDepth::occlusion_compensation_
bool occlusion_compensation_
Definition: depth_cloud_mld.h:132
rviz::MultiLayerDepth::finalizingPointCloud
void finalizingPointCloud(sensor_msgs::PointCloud2Ptr &point_cloud, std::size_t size)
Definition: depth_cloud_mld.cpp:583
rviz::MultiLayerDepth::generatePointCloudML
sensor_msgs::PointCloud2Ptr generatePointCloudML(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate multi-layered depth cloud (depth+shadow)
Definition: depth_cloud_mld.cpp:253
rviz::RGBA
Definition: depth_cloud_mld.cpp:81
ros::Time::now
static Time now()


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:09