Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 */ 00037 00038 #ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ 00039 #define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ 00040 00041 //#include <pcl/recognition/linemod/line_rgbd.h> 00042 #include <pcl/io/pcd_io.h> 00043 #include <fcntl.h> 00044 #include <pcl/point_cloud.h> 00045 #include <limits> 00046 #ifdef _WIN32 00047 # include <io.h> 00048 # include <windows.h> 00049 # define pcl_open _open 00050 # define pcl_close(fd) _close(fd) 00051 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin) 00052 #else 00053 #include <unistd.h> 00054 # include <sys/mman.h> 00055 # define pcl_open open 00056 # define pcl_close(fd) close(fd) 00057 # define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin) 00058 #endif 00059 00060 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00061 template <typename PointXYZT, typename PointRGBT> bool 00062 pcl::LineRGBD<PointXYZT, PointRGBT>::readLTMHeader (int fd, pcl::io::TARHeader &header) 00063 { 00064 // Read in the header 00065 int result = static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.file_name[0]), 512)); 00066 if (result == -1) 00067 return (false); 00068 00069 // We only support regular files for now. 00070 // Addional file types in TAR include: hard links, symbolic links, device/special files, block devices, 00071 // directories, and named pipes. 00072 if (header.file_type[0] != '0' && header.file_type[0] != '\0') 00073 return (false); 00074 00075 // We only support USTAR version 0 files for now 00076 if (std::string (header.ustar).substr (0, 5) != "ustar") 00077 return (false); 00078 00079 if (header.getFileSize () == 0) 00080 return (false); 00081 00082 return (true); 00083 } 00084 00085 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00086 template <typename PointXYZT, typename PointRGBT> bool 00087 pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id) 00088 { 00089 // Open the file 00090 int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY); 00091 if (ltm_fd == -1) 00092 return (false); 00093 00094 int ltm_offset = 0; 00095 00096 pcl::io::TARHeader ltm_header; 00097 PCDReader pcd_reader; 00098 00099 std::string pcd_ext (".pcd"); 00100 std::string sqmmt_ext (".sqmmt"); 00101 00102 // While there still is an LTM header to be read 00103 while (readLTMHeader (ltm_fd, ltm_header)) 00104 { 00105 ltm_offset += 512; 00106 00107 // Search for extension 00108 std::string chunk_name (ltm_header.file_name); 00109 00110 std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower); 00111 std::string::size_type it; 00112 00113 if ((it = chunk_name.find (pcd_ext)) != std::string::npos && 00114 (pcd_ext.size () - (chunk_name.size () - it)) == 0) 00115 { 00116 PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ()); 00117 // Read the next PCD file 00118 template_point_clouds_.resize (template_point_clouds_.size () + 1); 00119 pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset); 00120 00121 // Increment the offset for the next file 00122 ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512); 00123 } 00124 else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos && 00125 (sqmmt_ext.size () - (chunk_name.size () - it)) == 0) 00126 { 00127 PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ()); 00128 00129 unsigned int fsize = ltm_header.getFileSize (); 00130 char *buffer = new char[fsize]; 00131 int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize)); 00132 if (result == -1) 00133 { 00134 delete [] buffer; 00135 PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n"); 00136 break; 00137 } 00138 00139 // Read a SQMMT file 00140 std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary); 00141 stream.write (buffer, fsize); 00142 SparseQuantizedMultiModTemplate sqmmt; 00143 sqmmt.deserialize (stream); 00144 00145 linemod_.addTemplate (sqmmt); 00146 object_ids_.push_back (object_id); 00147 00148 // Increment the offset for the next file 00149 ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512); 00150 00151 delete [] buffer; 00152 } 00153 00154 if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0) 00155 break; 00156 } 00157 00158 // Close the file 00159 pcl_close (ltm_fd); 00160 00161 // Compute 3D bounding boxes from the template point clouds 00162 bounding_boxes_.resize (template_point_clouds_.size ()); 00163 for (size_t i = 0; i < template_point_clouds_.size (); ++i) 00164 { 00165 PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i]; 00166 BoundingBoxXYZ & bb = bounding_boxes_[i]; 00167 bb.x = bb.y = bb.z = std::numeric_limits<float>::max (); 00168 bb.width = bb.height = bb.depth = 0.0f; 00169 00170 float center_x = 0.0f; 00171 float center_y = 0.0f; 00172 float center_z = 0.0f; 00173 float min_x = std::numeric_limits<float>::max (); 00174 float min_y = std::numeric_limits<float>::max (); 00175 float min_z = std::numeric_limits<float>::max (); 00176 float max_x = -std::numeric_limits<float>::max (); 00177 float max_y = -std::numeric_limits<float>::max (); 00178 float max_z = -std::numeric_limits<float>::max (); 00179 size_t counter = 0; 00180 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00181 { 00182 const PointXYZRGBA & p = template_point_cloud.points[j]; 00183 00184 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00185 continue; 00186 00187 min_x = std::min (min_x, p.x); 00188 min_y = std::min (min_y, p.y); 00189 min_z = std::min (min_z, p.z); 00190 max_x = std::max (max_x, p.x); 00191 max_y = std::max (max_y, p.y); 00192 max_z = std::max (max_z, p.z); 00193 00194 center_x += p.x; 00195 center_y += p.y; 00196 center_z += p.z; 00197 00198 ++counter; 00199 } 00200 00201 center_x /= static_cast<float> (counter); 00202 center_y /= static_cast<float> (counter); 00203 center_z /= static_cast<float> (counter); 00204 00205 bb.width = max_x - min_x; 00206 bb.height = max_y - min_y; 00207 bb.depth = max_z - min_z; 00208 00209 bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f; 00210 bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f; 00211 bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f; 00212 00213 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00214 { 00215 PointXYZRGBA p = template_point_cloud.points[j]; 00216 00217 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00218 continue; 00219 00220 p.x -= center_x; 00221 p.y -= center_y; 00222 p.z -= center_z; 00223 00224 template_point_cloud.points[j] = p; 00225 } 00226 } 00227 00228 return (true); 00229 } 00230 00231 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00232 template <typename PointXYZT, typename PointRGBT> int 00233 pcl::LineRGBD<PointXYZT, PointRGBT>::createAndAddTemplate ( 00234 pcl::PointCloud<pcl::PointXYZRGBA> & cloud, 00235 const size_t object_id, 00236 const MaskMap & mask_xyz, 00237 const MaskMap & mask_rgb, 00238 const RegionXY & region) 00239 { 00240 // add point cloud 00241 template_point_clouds_.resize (template_point_clouds_.size () + 1); 00242 pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); 00243 00244 // add template 00245 object_ids_.push_back (object_id); 00246 00247 // Compute 3D bounding boxes from the template point clouds 00248 bounding_boxes_.resize (template_point_clouds_.size ()); 00249 { 00250 const size_t i = template_point_clouds_.size () - 1; 00251 00252 PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i]; 00253 BoundingBoxXYZ & bb = bounding_boxes_[i]; 00254 bb.x = bb.y = bb.z = std::numeric_limits<float>::max (); 00255 bb.width = bb.height = bb.depth = 0.0f; 00256 00257 float center_x = 0.0f; 00258 float center_y = 0.0f; 00259 float center_z = 0.0f; 00260 float min_x = std::numeric_limits<float>::max (); 00261 float min_y = std::numeric_limits<float>::max (); 00262 float min_z = std::numeric_limits<float>::max (); 00263 float max_x = -std::numeric_limits<float>::max (); 00264 float max_y = -std::numeric_limits<float>::max (); 00265 float max_z = -std::numeric_limits<float>::max (); 00266 size_t counter = 0; 00267 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00268 { 00269 const PointXYZRGBA & p = template_point_cloud.points[j]; 00270 00271 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00272 continue; 00273 00274 min_x = std::min (min_x, p.x); 00275 min_y = std::min (min_y, p.y); 00276 min_z = std::min (min_z, p.z); 00277 max_x = std::max (max_x, p.x); 00278 max_y = std::max (max_y, p.y); 00279 max_z = std::max (max_z, p.z); 00280 00281 center_x += p.x; 00282 center_y += p.y; 00283 center_z += p.z; 00284 00285 ++counter; 00286 } 00287 00288 center_x /= static_cast<float> (counter); 00289 center_y /= static_cast<float> (counter); 00290 center_z /= static_cast<float> (counter); 00291 00292 bb.width = max_x - min_x; 00293 bb.height = max_y - min_y; 00294 bb.depth = max_z - min_z; 00295 00296 bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f; 00297 bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f; 00298 bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f; 00299 00300 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00301 { 00302 PointXYZRGBA p = template_point_cloud.points[j]; 00303 00304 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00305 continue; 00306 00307 p.x -= center_x; 00308 p.y -= center_y; 00309 p.z -= center_z; 00310 00311 template_point_cloud.points[j] = p; 00312 } 00313 } 00314 00315 std::vector<pcl::QuantizableModality*> modalities; 00316 modalities.push_back (&color_gradient_mod_); 00317 modalities.push_back (&surface_normal_mod_); 00318 00319 std::vector<MaskMap*> masks; 00320 masks.push_back (const_cast<MaskMap*> (&mask_rgb)); 00321 masks.push_back (const_cast<MaskMap*> (&mask_xyz)); 00322 00323 return (linemod_.createAndAddTemplate (modalities, masks, region)); 00324 } 00325 00326 00327 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00328 template <typename PointXYZT, typename PointRGBT> bool 00329 pcl::LineRGBD<PointXYZT, PointRGBT>::addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, size_t object_id) 00330 { 00331 // add point cloud 00332 template_point_clouds_.resize (template_point_clouds_.size () + 1); 00333 pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]); 00334 00335 // add template 00336 linemod_.addTemplate (sqmmt); 00337 object_ids_.push_back (object_id); 00338 00339 // Compute 3D bounding boxes from the template point clouds 00340 bounding_boxes_.resize (template_point_clouds_.size ()); 00341 { 00342 const size_t i = template_point_clouds_.size () - 1; 00343 00344 PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i]; 00345 BoundingBoxXYZ & bb = bounding_boxes_[i]; 00346 bb.x = bb.y = bb.z = std::numeric_limits<float>::max (); 00347 bb.width = bb.height = bb.depth = 0.0f; 00348 00349 float center_x = 0.0f; 00350 float center_y = 0.0f; 00351 float center_z = 0.0f; 00352 float min_x = std::numeric_limits<float>::max (); 00353 float min_y = std::numeric_limits<float>::max (); 00354 float min_z = std::numeric_limits<float>::max (); 00355 float max_x = -std::numeric_limits<float>::max (); 00356 float max_y = -std::numeric_limits<float>::max (); 00357 float max_z = -std::numeric_limits<float>::max (); 00358 size_t counter = 0; 00359 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00360 { 00361 const PointXYZRGBA & p = template_point_cloud.points[j]; 00362 00363 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00364 continue; 00365 00366 min_x = std::min (min_x, p.x); 00367 min_y = std::min (min_y, p.y); 00368 min_z = std::min (min_z, p.z); 00369 max_x = std::max (max_x, p.x); 00370 max_y = std::max (max_y, p.y); 00371 max_z = std::max (max_z, p.z); 00372 00373 center_x += p.x; 00374 center_y += p.y; 00375 center_z += p.z; 00376 00377 ++counter; 00378 } 00379 00380 center_x /= static_cast<float> (counter); 00381 center_y /= static_cast<float> (counter); 00382 center_z /= static_cast<float> (counter); 00383 00384 bb.width = max_x - min_x; 00385 bb.height = max_y - min_y; 00386 bb.depth = max_z - min_z; 00387 00388 bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f; 00389 bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f; 00390 bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f; 00391 00392 for (size_t j = 0; j < template_point_cloud.size (); ++j) 00393 { 00394 PointXYZRGBA p = template_point_cloud.points[j]; 00395 00396 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z)) 00397 continue; 00398 00399 p.x -= center_x; 00400 p.y -= center_y; 00401 p.z -= center_z; 00402 00403 template_point_cloud.points[j] = p; 00404 } 00405 } 00406 00407 return (true); 00408 } 00409 00410 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00411 template <typename PointXYZT, typename PointRGBT> void 00412 pcl::LineRGBD<PointXYZT, PointRGBT>::detect ( 00413 std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections) 00414 { 00415 std::vector<pcl::QuantizableModality*> modalities; 00416 modalities.push_back (&color_gradient_mod_); 00417 modalities.push_back (&surface_normal_mod_); 00418 00419 std::vector<pcl::LINEMODDetection> linemod_detections; 00420 linemod_.detectTemplates (modalities, linemod_detections); 00421 00422 detections_.clear (); 00423 detections_.reserve (linemod_detections.size ()); 00424 detections.clear (); 00425 detections.reserve (linemod_detections.size ()); 00426 for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) 00427 { 00428 pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; 00429 00430 typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection detection; 00431 detection.template_id = linemod_detection.template_id; 00432 detection.object_id = object_ids_[linemod_detection.template_id]; 00433 detection.detection_id = detection_id; 00434 detection.response = linemod_detection.score; 00435 00436 // compute bounding box: 00437 // we assume that the bounding boxes of the templates are relative to the center of mass 00438 // of the template points; so we also compute the center of mass of the points 00439 // covered by the 00440 00441 const pcl::SparseQuantizedMultiModTemplate & linemod_template = 00442 linemod_.getTemplate (linemod_detection.template_id); 00443 00444 const size_t start_x = std::max (linemod_detection.x, 0); 00445 const size_t start_y = std::max (linemod_detection.y, 0); 00446 const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width), 00447 static_cast<size_t> (cloud_xyz_->width)); 00448 const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height), 00449 static_cast<size_t> (cloud_xyz_->height)); 00450 00451 detection.region.x = linemod_detection.x; 00452 detection.region.y = linemod_detection.y; 00453 detection.region.width = linemod_template.region.width; 00454 detection.region.height = linemod_template.region.height; 00455 00456 //std::cerr << "detection region: " << linemod_detection.x << ", " 00457 // << linemod_detection.y << ", " 00458 // << linemod_template.region.width << ", " 00459 // << linemod_template.region.height << std::endl; 00460 00461 float center_x = 0.0f; 00462 float center_y = 0.0f; 00463 float center_z = 0.0f; 00464 size_t counter = 0; 00465 for (size_t row_index = start_y; row_index < end_y; ++row_index) 00466 { 00467 for (size_t col_index = start_x; col_index < end_x; ++col_index) 00468 { 00469 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); 00470 00471 if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z)) 00472 { 00473 center_x += point.x; 00474 center_y += point.y; 00475 center_z += point.z; 00476 ++counter; 00477 } 00478 } 00479 } 00480 const float inv_counter = 1.0f / static_cast<float> (counter); 00481 center_x *= inv_counter; 00482 center_y *= inv_counter; 00483 center_z *= inv_counter; 00484 00485 pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; 00486 00487 detection.bounding_box = template_bounding_box; 00488 detection.bounding_box.x += center_x; 00489 detection.bounding_box.y += center_y; 00490 detection.bounding_box.z += center_z; 00491 00492 detections_.push_back (detection); 00493 } 00494 00495 // refine detections along depth 00496 refineDetectionsAlongDepth (); 00497 //applyprojectivedepthicpondetections(); 00498 00499 // remove overlaps 00500 removeOverlappingDetections (); 00501 00502 for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) 00503 { 00504 detections.push_back (detections_[detection_index]); 00505 } 00506 } 00507 00508 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00509 template <typename PointXYZT, typename PointRGBT> void 00510 pcl::LineRGBD<PointXYZT, PointRGBT>::detectSemiScaleInvariant ( 00511 std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections, 00512 const float min_scale, 00513 const float max_scale, 00514 const float scale_multiplier) 00515 { 00516 std::vector<pcl::QuantizableModality*> modalities; 00517 modalities.push_back (&color_gradient_mod_); 00518 modalities.push_back (&surface_normal_mod_); 00519 00520 std::vector<pcl::LINEMODDetection> linemod_detections; 00521 linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier); 00522 00523 detections_.clear (); 00524 detections_.reserve (linemod_detections.size ()); 00525 detections.clear (); 00526 detections.reserve (linemod_detections.size ()); 00527 for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id) 00528 { 00529 pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id]; 00530 00531 typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection detection; 00532 detection.template_id = linemod_detection.template_id; 00533 detection.object_id = object_ids_[linemod_detection.template_id]; 00534 detection.detection_id = detection_id; 00535 detection.response = linemod_detection.score; 00536 00537 // compute bounding box: 00538 // we assume that the bounding boxes of the templates are relative to the center of mass 00539 // of the template points; so we also compute the center of mass of the points 00540 // covered by the 00541 00542 const pcl::SparseQuantizedMultiModTemplate & linemod_template = 00543 linemod_.getTemplate (linemod_detection.template_id); 00544 00545 const size_t start_x = std::max (linemod_detection.x, 0); 00546 const size_t start_y = std::max (linemod_detection.y, 0); 00547 const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width * linemod_detection.scale), 00548 static_cast<size_t> (cloud_xyz_->width)); 00549 const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height * linemod_detection.scale), 00550 static_cast<size_t> (cloud_xyz_->height)); 00551 00552 detection.region.x = linemod_detection.x; 00553 detection.region.y = linemod_detection.y; 00554 detection.region.width = linemod_template.region.width * linemod_detection.scale; 00555 detection.region.height = linemod_template.region.height * linemod_detection.scale; 00556 00557 //std::cerr << "detection region: " << linemod_detection.x << ", " 00558 // << linemod_detection.y << ", " 00559 // << linemod_template.region.width << ", " 00560 // << linemod_template.region.height << std::endl; 00561 00562 float center_x = 0.0f; 00563 float center_y = 0.0f; 00564 float center_z = 0.0f; 00565 size_t counter = 0; 00566 for (size_t row_index = start_y; row_index < end_y; ++row_index) 00567 { 00568 for (size_t col_index = start_x; col_index < end_x; ++col_index) 00569 { 00570 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); 00571 00572 if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z)) 00573 { 00574 center_x += point.x; 00575 center_y += point.y; 00576 center_z += point.z; 00577 ++counter; 00578 } 00579 } 00580 } 00581 const float inv_counter = 1.0f / static_cast<float> (counter); 00582 center_x *= inv_counter; 00583 center_y *= inv_counter; 00584 center_z *= inv_counter; 00585 00586 pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id]; 00587 00588 detection.bounding_box = template_bounding_box; 00589 detection.bounding_box.x += center_x; 00590 detection.bounding_box.y += center_y; 00591 detection.bounding_box.z += center_z; 00592 00593 detections_.push_back (detection); 00594 } 00595 00596 // refine detections along depth 00597 //refineDetectionsAlongDepth (); 00598 //applyProjectiveDepthICPOnDetections(); 00599 00600 // remove overlaps 00601 removeOverlappingDetections (); 00602 00603 for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index) 00604 { 00605 detections.push_back (detections_[detection_index]); 00606 } 00607 } 00608 00609 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00610 template <typename PointXYZT, typename PointRGBT> void 00611 pcl::LineRGBD<PointXYZT, PointRGBT>::computeTransformedTemplatePoints ( 00612 const size_t detection_id, pcl::PointCloud<pcl::PointXYZRGBA> &cloud) 00613 { 00614 if (detection_id >= detections_.size ()) 00615 PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n"); 00616 00617 const size_t template_id = detections_[detection_id].template_id; 00618 const pcl::PointCloud<pcl::PointXYZRGBA> & template_point_cloud = template_point_clouds_[template_id]; 00619 00620 const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id]; 00621 const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box; 00622 00623 //std::cerr << "detection: " 00624 // << detection_bounding_box.x << ", " 00625 // << detection_bounding_box.y << ", " 00626 // << detection_bounding_box.z << std::endl; 00627 //std::cerr << "template: " 00628 // << template_bounding_box.x << ", " 00629 // << template_bounding_box.y << ", " 00630 // << template_bounding_box.z << std::endl; 00631 const float translation_x = detection_bounding_box.x - template_bounding_box.x; 00632 const float translation_y = detection_bounding_box.y - template_bounding_box.y; 00633 const float translation_z = detection_bounding_box.z - template_bounding_box.z; 00634 00635 //std::cerr << "translation: " 00636 // << translation_x << ", " 00637 // << translation_y << ", " 00638 // << translation_z << std::endl; 00639 00640 const size_t nr_points = template_point_cloud.size (); 00641 cloud.resize (nr_points); 00642 cloud.width = template_point_cloud.width; 00643 cloud.height = template_point_cloud.height; 00644 for (size_t point_index = 0; point_index < nr_points; ++point_index) 00645 { 00646 pcl::PointXYZRGBA point = template_point_cloud.points[point_index]; 00647 00648 point.x += translation_x; 00649 point.y += translation_y; 00650 point.z += translation_z; 00651 00652 cloud.points[point_index] = point; 00653 } 00654 } 00655 00656 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00657 template <typename PointXYZT, typename PointRGBT> void 00658 pcl::LineRGBD<PointXYZT, PointRGBT>::refineDetectionsAlongDepth () 00659 { 00660 const size_t nr_detections = detections_.size (); 00661 for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index) 00662 { 00663 typename LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index]; 00664 00665 // find depth with most valid points 00666 const size_t start_x = detection.region.x; 00667 const size_t start_y = detection.region.y; 00668 const size_t end_x = start_x + detection.region.width; 00669 const size_t end_y = start_y + detection.region.height; 00670 00671 00672 float min_depth = std::numeric_limits<float>::max (); 00673 float max_depth = -std::numeric_limits<float>::max (); 00674 for (size_t row_index = start_y; row_index < end_y; ++row_index) 00675 { 00676 for (size_t col_index = start_x; col_index < end_x; ++col_index) 00677 { 00678 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); 00679 00680 if (/*pcl_isfinite (point.x) && pcl_isfinite (point.y) && */pcl_isfinite (point.z)) 00681 { 00682 min_depth = std::min (min_depth, point.z); 00683 max_depth = std::max (max_depth, point.z); 00684 } 00685 } 00686 } 00687 00688 const size_t nr_bins = 1000; 00689 const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1); 00690 std::vector<size_t> depth_bins (nr_bins, 0); 00691 for (size_t row_index = start_y; row_index < end_y; ++row_index) 00692 { 00693 for (size_t col_index = start_x; col_index < end_x; ++col_index) 00694 { 00695 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index); 00696 00697 if (/*pcl_isfinite (point.x) && pcl_isfinite (point.y) && */pcl_isfinite (point.z)) 00698 { 00699 const size_t bin_index = static_cast<size_t> ((point.z - min_depth) / step_size); 00700 ++depth_bins[bin_index]; 00701 } 00702 } 00703 } 00704 00705 std::vector<size_t> integral_depth_bins (nr_bins, 0); 00706 00707 integral_depth_bins[0] = depth_bins[0]; 00708 for (size_t bin_index = 1; bin_index < nr_bins; ++bin_index) 00709 { 00710 integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1]; 00711 } 00712 00713 const size_t bb_depth_range = static_cast<size_t> (detection.bounding_box.depth / step_size); 00714 00715 size_t max_nr_points = 0; 00716 size_t max_index = 0; 00717 for (size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index) 00718 { 00719 const size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index]; 00720 00721 if (nr_points_in_range > max_nr_points) 00722 { 00723 max_nr_points = nr_points_in_range; 00724 max_index = bin_index; 00725 } 00726 } 00727 00728 const float z = static_cast<float> (max_index) * step_size + min_depth; 00729 00730 detection.bounding_box.z = z; 00731 } 00732 } 00733 00734 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00735 template <typename PointXYZT, typename PointRGBT> void 00736 pcl::LineRGBD<PointXYZT, PointRGBT>::applyProjectiveDepthICPOnDetections () 00737 { 00738 const size_t nr_detections = detections_.size (); 00739 for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index) 00740 { 00741 typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index]; 00742 00743 const size_t template_id = detection.template_id; 00744 pcl::PointCloud<pcl::PointXYZRGBA> & point_cloud = template_point_clouds_[template_id]; 00745 00746 const size_t start_x = detection.region.x; 00747 const size_t start_y = detection.region.y; 00748 const size_t pc_width = point_cloud.width; 00749 const size_t pc_height = point_cloud.height; 00750 00751 std::vector<std::pair<float, float> > depth_matches; 00752 for (size_t row_index = 0; row_index < pc_height; ++row_index) 00753 { 00754 for (size_t col_index = 0; col_index < pc_width; ++col_index) 00755 { 00756 const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index); 00757 const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y); 00758 00759 if (!pcl_isfinite (point_template.z) || !pcl_isfinite (point_input.z)) 00760 continue; 00761 00762 depth_matches.push_back (std::make_pair (point_template.z, point_input.z)); 00763 } 00764 } 00765 00766 // apply ransac on matches 00767 const size_t nr_matches = depth_matches.size (); 00768 const size_t nr_iterations = 100; // todo: should be a parameter... 00769 const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter... 00770 size_t best_nr_inliers = 0; 00771 float best_z_translation = 0.0f; 00772 for (size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index) 00773 { 00774 const size_t rand_index = (rand () * nr_matches) / RAND_MAX; 00775 00776 const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first; 00777 00778 size_t nr_inliers = 0; 00779 for (size_t match_index = 0; match_index < nr_matches; ++match_index) 00780 { 00781 const float error = fabsf (depth_matches[match_index].first + z_translation - depth_matches[match_index].second); 00782 00783 if (error <= inlier_threshold) 00784 { 00785 ++nr_inliers; 00786 } 00787 } 00788 00789 if (nr_inliers > best_nr_inliers) 00790 { 00791 best_nr_inliers = nr_inliers; 00792 best_z_translation = z_translation; 00793 } 00794 } 00795 00796 float average_depth = 0.0f; 00797 size_t average_counter = 0; 00798 for (size_t match_index = 0; match_index < nr_matches; ++match_index) 00799 { 00800 const float error = fabsf (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second); 00801 00802 if (error <= inlier_threshold) 00803 { 00804 //average_depth += depth_matches[match_index].second; 00805 average_depth += depth_matches[match_index].second - depth_matches[match_index].first; 00806 ++average_counter; 00807 } 00808 } 00809 average_depth /= static_cast<float> (average_counter); 00810 00811 detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f; 00812 } 00813 } 00814 00815 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00816 template <typename PointXYZT, typename PointRGBT> void 00817 pcl::LineRGBD<PointXYZT, PointRGBT>::removeOverlappingDetections () 00818 { 00819 // compute overlap between each detection 00820 const size_t nr_detections = detections_.size (); 00821 Eigen::MatrixXf overlaps (nr_detections, nr_detections); 00822 for (size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1) 00823 { 00824 for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) 00825 { 00826 const float bounding_box_volume = detections_[detection_index_1].bounding_box.width 00827 * detections_[detection_index_1].bounding_box.height 00828 * detections_[detection_index_1].bounding_box.depth; 00829 00830 if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id) 00831 overlaps (detection_index_1, detection_index_2) = 0.0f; 00832 else 00833 overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume ( 00834 detections_[detection_index_1].bounding_box, 00835 detections_[detection_index_2].bounding_box) / bounding_box_volume; 00836 } 00837 } 00838 00839 // create clusters of detections 00840 std::vector<int> detection_to_cluster_mapping (nr_detections, -1); 00841 std::vector<std::vector<size_t> > clusters; 00842 for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index) 00843 { 00844 if (detection_to_cluster_mapping[detection_index] != -1) 00845 continue; // already part of a cluster 00846 00847 std::vector<size_t> cluster; 00848 const size_t cluster_id = clusters.size (); 00849 00850 cluster.push_back (detection_index); 00851 detection_to_cluster_mapping[detection_index] = static_cast<int> (cluster_id); 00852 00853 // check for detections which have sufficient overlap with a detection in the cluster 00854 for (size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index) 00855 { 00856 const size_t detection_index_1 = cluster[cluster_index]; 00857 00858 for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2) 00859 { 00860 if (detection_to_cluster_mapping[detection_index_2] != -1) 00861 continue; // already part of a cluster 00862 00863 if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_) 00864 continue; // not enough overlap 00865 00866 cluster.push_back (detection_index_2); 00867 detection_to_cluster_mapping[detection_index_2] = static_cast<int> (cluster_id); 00868 } 00869 } 00870 00871 clusters.push_back (cluster); 00872 } 00873 00874 // compute detection representatives for every cluster 00875 std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections; 00876 00877 const size_t nr_clusters = clusters.size (); 00878 for (size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id) 00879 { 00880 std::vector<size_t> & cluster = clusters[cluster_id]; 00881 00882 float average_center_x = 0.0f; 00883 float average_center_y = 0.0f; 00884 float average_center_z = 0.0f; 00885 float weight_sum = 0.0f; 00886 00887 float best_response = 0.0f; 00888 size_t best_detection_id = 0; 00889 00890 float average_region_x = 0.0f; 00891 float average_region_y = 0.0f; 00892 00893 const size_t elements_in_cluster = cluster.size (); 00894 for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index) 00895 { 00896 const size_t detection_id = cluster[cluster_index]; 00897 00898 const float weight = detections_[detection_id].response; 00899 00900 if (weight > best_response) 00901 { 00902 best_response = weight; 00903 best_detection_id = detection_id; 00904 } 00905 00906 const Detection & d = detections_[detection_id]; 00907 const float p_center_x = d.bounding_box.x + d.bounding_box.width / 2.0f; 00908 const float p_center_y = d.bounding_box.y + d.bounding_box.height / 2.0f; 00909 const float p_center_z = d.bounding_box.z + d.bounding_box.depth / 2.0f; 00910 00911 average_center_x += p_center_x * weight; 00912 average_center_y += p_center_y * weight; 00913 average_center_z += p_center_z * weight; 00914 weight_sum += weight; 00915 00916 average_region_x += float (detections_[detection_id].region.x) * weight; 00917 average_region_y += float (detections_[detection_id].region.y) * weight; 00918 } 00919 00920 typename LineRGBD<PointXYZT, PointRGBT>::Detection detection; 00921 detection.template_id = detections_[best_detection_id].template_id; 00922 detection.object_id = detections_[best_detection_id].object_id; 00923 detection.detection_id = cluster_id; 00924 detection.response = best_response; 00925 00926 const float inv_weight_sum = 1.0f / weight_sum; 00927 const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width; 00928 const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height; 00929 const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth; 00930 00931 detection.bounding_box.x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f; 00932 detection.bounding_box.y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f; 00933 detection.bounding_box.z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f; 00934 detection.bounding_box.width = best_detection_bb_width; 00935 detection.bounding_box.height = best_detection_bb_height; 00936 detection.bounding_box.depth = best_detection_bb_depth; 00937 00938 detection.region.x = int (average_region_x * inv_weight_sum); 00939 detection.region.y = int (average_region_y * inv_weight_sum); 00940 detection.region.width = detections_[best_detection_id].region.width; 00941 detection.region.height = detections_[best_detection_id].region.height; 00942 00943 clustered_detections.push_back (detection); 00944 } 00945 00946 detections_ = clustered_detections; 00947 } 00948 00949 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00950 template <typename PointXYZT, typename PointRGBT> float 00951 pcl::LineRGBD<PointXYZT, PointRGBT>::computeBoundingBoxIntersectionVolume ( 00952 const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2) 00953 { 00954 const float x1_min = box1.x; 00955 const float y1_min = box1.y; 00956 const float z1_min = box1.z; 00957 const float x1_max = box1.x + box1.width; 00958 const float y1_max = box1.y + box1.height; 00959 const float z1_max = box1.z + box1.depth; 00960 00961 const float x2_min = box2.x; 00962 const float y2_min = box2.y; 00963 const float z2_min = box2.z; 00964 const float x2_max = box2.x + box2.width; 00965 const float y2_max = box2.y + box2.height; 00966 const float z2_max = box2.z + box2.depth; 00967 00968 const float xi_min = std::max (x1_min, x2_min); 00969 const float yi_min = std::max (y1_min, y2_min); 00970 const float zi_min = std::max (z1_min, z2_min); 00971 00972 const float xi_max = std::min (x1_max, x2_max); 00973 const float yi_max = std::min (y1_max, y2_max); 00974 const float zi_max = std::min (z1_max, z2_max); 00975 00976 const float intersection_width = xi_max - xi_min; 00977 const float intersection_height = yi_max - yi_min; 00978 const float intersection_depth = zi_max - zi_min; 00979 00980 if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f) 00981 return 0.0f; 00982 00983 const float intersection_volume = intersection_width * intersection_height * intersection_depth; 00984 00985 return (intersection_volume); 00986 } 00987 00988 00989 #endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_ 00990