34 #include <sensor_msgs/CameraInfo.h> 44 #define POINT_STEP (sizeof(float)*4) 58 static inline bool valid(
float depth)
64 return depth * 0.001f;
71 static inline bool valid(
float depth)
73 return std::isfinite(depth);
92 sensor_msgs::CameraInfoConstPtr& camera_info_msg)
95 if (!depth_msg || !camera_info_msg)
97 std::string error_msg (
"Waiting for CameraInfo message..");
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;
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;
108 int expected_width = roi_width / binning_x;
109 int expected_height = roi_height / binning_y;
111 if ( expected_width != depth_msg->width ||
112 expected_height != depth_msg->height )
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 <<
")";
123 int width = depth_msg->width;
124 int height = depth_msg->height;
126 std::size_t size = width * height;
128 if (size != shadow_depth_.size())
131 shadow_depth_.resize(size, 0.0
f);
132 shadow_timestamp_.resize(size, 0.0);
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;
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;
149 double fx = camera_info_msg->P[0] * scale_x;
150 double fy = camera_info_msg->P[5] * scale_y;
152 float constant_x = 1.0f / fx;
153 float constant_y = 1.0f / fy;
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();
161 for (
int v = 0; v < height; ++v, ++projY)
162 *projY = (v - center_y) * constant_y;
164 for (
int u = 0; u < width; ++u, ++projX)
165 *projX = (u - center_x) * constant_x;
175 std::vector<uint32_t>& rgba_color_raw)
178 int width = depth_msg->width;
179 int height = depth_msg->height;
181 sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
182 cloud_msg->data.resize(height * width * cloud_msg->point_step);
184 uint32_t* color_img_ptr = 0;
186 if (rgba_color_raw.size())
187 color_img_ptr = &rgba_color_raw[0];
193 float* cloud_data_ptr =
reinterpret_cast<float*
>(&cloud_msg->data[0]);
194 const std::size_t point_step = cloud_msg->point_step;
196 std::size_t point_count = 0;
197 std::size_t point_idx = 0;
200 double time_expire = time_now+shadow_time_out_;
202 const T* depth_img_ptr = (T*)&depth_msg->data[0];
204 std::vector<float>::iterator proj_x;
205 std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
207 std::vector<float>::iterator proj_y ;
208 std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
211 for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
213 for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
218 T depth_raw = *depth_img_ptr;
227 color = *color_img_ptr;
231 color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
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;
249 finalizingPointCloud(cloud_msg, point_count);
257 std::vector<uint32_t>& rgba_color_raw)
259 int width = depth_msg->width;
260 int height = depth_msg->height;
262 sensor_msgs::PointCloud2Ptr cloud_msg = initPointCloud();
263 cloud_msg->data.resize(height * width * cloud_msg->point_step * 2);
265 uint32_t* color_img_ptr = 0;
267 if (rgba_color_raw.size())
268 color_img_ptr = &rgba_color_raw[0];
274 float* cloud_data_ptr =
reinterpret_cast<float*
>(&cloud_msg->data[0]);
275 uint8_t* cloud_shadow_buffer_ptr = &shadow_buffer_[0];
277 const std::size_t point_step = cloud_msg->point_step;
279 std::size_t point_count = 0;
280 std::size_t point_idx = 0;
283 double time_expire = time_now-shadow_time_out_;
285 const T* depth_img_ptr = (T*)&depth_msg->data[0];
287 std::vector<float>::iterator proj_x;
288 std::vector<float>::const_iterator proj_x_end = projection_map_x_.end();
290 std::vector<float>::iterator proj_y ;
291 std::vector<float>::const_iterator proj_y_end = projection_map_y_.end();
294 for (proj_y = projection_map_y_.begin(); proj_y !=proj_y_end; ++proj_y)
296 for (proj_x = projection_map_x_.begin(); proj_x !=proj_x_end; ++proj_x,
299 cloud_shadow_buffer_ptr += point_step)
303 float shadow_depth = shadow_depth_[point_idx];
306 if ( (shadow_depth!=0.0
f) && (shadow_timestamp_[point_idx]<time_expire) )
309 shadow_depth = shadow_depth_[point_idx] = 0.0f;
312 T depth_raw = *depth_img_ptr;
318 float* cloud_data_pixel_ptr = cloud_data_ptr;
324 color = *color_img_ptr;
328 color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255);
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;
340 if (depth < shadow_depth - shadow_distance_)
343 memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
350 memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step);
353 RGBA* color =
reinterpret_cast<RGBA*
>(cloud_shadow_buffer_ptr +
sizeof(float) * 3);
359 shadow_depth_[point_idx] = depth;
360 shadow_timestamp_[point_idx] = time_now;
367 if (shadow_depth != 0)
370 memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
383 finalizingPointCloud(cloud_msg, point_count);
391 std::vector<uint32_t>& rgba_color_raw)
394 size_t num_pixel = color_msg->width * color_msg->height;
397 int num_channels = enc::numChannels(color_msg->encoding);
399 bool rgb_encoding =
false;
400 if (color_msg->encoding.find(
"rgb")!=std::string::npos)
403 bool has_alpha = enc::hasAlpha(color_msg->encoding);
406 rgba_color_raw.clear();
407 rgba_color_raw.reserve(num_pixel);
409 uint8_t* img_ptr = (uint8_t*)&color_msg->data[
sizeof(T) - 1];
412 switch (num_channels)
416 for (i = 0; i < num_pixel; ++i)
418 uint8_t gray_value = *img_ptr;
419 img_ptr +=
sizeof(T);
421 rgba_color_raw.push_back((uint32_t)gray_value << 16 | (uint32_t)gray_value << 8 | (uint32_t)gray_value);
427 for (i = 0; i < num_pixel; ++i)
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);
434 img_ptr +=
sizeof(T);
439 rgba_color_raw.push_back((uint32_t)color1 << 16 | (uint32_t)color2 << 8 | (uint32_t)color3 << 0);
443 rgba_color_raw.push_back((uint32_t)color3 << 16 | (uint32_t)color2 << 8 | (uint32_t)color1 << 0);
454 sensor_msgs::ImageConstPtr color_msg,
455 sensor_msgs::CameraInfoConstPtr camera_info_msg)
459 sensor_msgs::PointCloud2Ptr point_cloud_out;
462 int bitDepth = enc::bitDepth(depth_msg->encoding);
463 int numChannels = enc::numChannels(depth_msg->encoding);
465 if (!camera_info_msg)
471 initializeConversion(depth_msg, camera_info_msg);
473 std::vector<uint32_t> rgba_color_raw_;
477 if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height)
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 <<
")";
486 switch (enc::bitDepth(color_msg->encoding))
489 convertColor<uint8_t>(color_msg, rgba_color_raw_);
492 convertColor<uint16_t>(color_msg, rgba_color_raw_);
495 std::string error_msg (
"Color image has invalid bit depth");
501 if (!occlusion_compensation_)
505 if ((bitDepth == 32) && (numChannels == 1))
508 point_cloud_out = generatePointCloudSL<float>(depth_msg, rgba_color_raw_);
510 else if ((bitDepth == 16) && (numChannels == 1))
513 point_cloud_out = generatePointCloudSL<uint16_t>(depth_msg, rgba_color_raw_);
520 if ((bitDepth == 32) && (numChannels == 1))
523 point_cloud_out = generatePointCloudML<float>(depth_msg, rgba_color_raw_);
525 else if ((bitDepth == 16) && (numChannels == 1))
528 point_cloud_out = generatePointCloudML<uint16_t>(depth_msg, rgba_color_raw_);
532 if ( !point_cloud_out )
534 std::string error_msg (
"Depth image has invalid format (only 16 bit and float are supported)!");
538 return point_cloud_out;
543 sensor_msgs::PointCloud2Ptr point_cloud_out = sensor_msgs::PointCloud2Ptr(
new sensor_msgs::PointCloud2());
545 point_cloud_out->fields.resize(4);
546 std::size_t point_offset = 0;
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);
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);
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);
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);
572 point_cloud_out->point_step = point_offset;
574 point_cloud_out->is_bigendian =
false;
575 point_cloud_out->is_dense =
false;
577 return point_cloud_out;
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;
static float toMeters(float depth)
sensor_msgs::PointCloud2Ptr generatePointCloudSL(const sensor_msgs::ImageConstPtr &depth_msg, std::vector< uint32_t > &rgba_color_raw)
Generate single-layered depth cloud (depth only)
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()
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)