Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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 }
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
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
00131 shadow_depth_.resize(size, 0.0f);
00132 shadow_timestamp_.resize(size, 0.0);
00133 shadow_buffer_.resize(size * POINT_STEP, 0);
00134
00135
00136
00137
00138
00139
00140
00141
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
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
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
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
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
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
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
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
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
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
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
00303 float shadow_depth = shadow_depth_[point_idx];
00304
00305
00306 if ( (shadow_depth!=0.0f) && (shadow_timestamp_[point_idx]<time_expire) )
00307 {
00308
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
00318 float* cloud_data_pixel_ptr = cloud_data_ptr;
00319
00320
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
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
00340 if (depth < shadow_depth - shadow_distance_)
00341 {
00342
00343 memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
00344 cloud_data_ptr += 4;
00345 ++point_count;
00346 }
00347 else
00348 {
00349
00350 memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step);
00351
00352
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
00359 shadow_depth_[point_idx] = depth;
00360 shadow_timestamp_[point_idx] = time_now;
00361 }
00362
00363 }
00364 else
00365 {
00366
00367 if (shadow_depth != 0)
00368 {
00369
00370 memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step);
00371 cloud_data_ptr += 4;
00372 ++point_count;
00373 }
00374 }
00375
00376
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
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
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];
00410
00411
00412 switch (num_channels)
00413 {
00414 case 1:
00415
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
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);
00435
00436 if (rgb_encoding)
00437 {
00438
00439 rgba_color_raw.push_back((uint32_t)color1 << 16 | (uint32_t)color2 << 8 | (uint32_t)color3 << 0);
00440 } else
00441 {
00442
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
00459 sensor_msgs::PointCloud2Ptr point_cloud_out;
00460
00461
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
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
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
00504
00505 if ((bitDepth == 32) && (numChannels == 1))
00506 {
00507
00508 point_cloud_out = generatePointCloudSL<float>(depth_msg, rgba_color_raw_);
00509 }
00510 else if ((bitDepth == 16) && (numChannels == 1))
00511 {
00512
00513 point_cloud_out = generatePointCloudSL<uint16_t>(depth_msg, rgba_color_raw_);
00514 }
00515 }
00516 else
00517 {
00518
00519
00520 if ((bitDepth == 32) && (numChannels == 1))
00521 {
00522
00523 point_cloud_out = generatePointCloudML<float>(depth_msg, rgba_color_raw_);
00524 }
00525 else if ((bitDepth == 16) && (numChannels == 1))
00526 {
00527
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 }