line_rgbd.hpp
Go to the documentation of this file.
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:25:13