Point Cloud Library (PCL)  1.7.0
line_rgbd.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
39 #define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
40 
41 //#include <pcl/recognition/linemod/line_rgbd.h>
42 #include <pcl/io/pcd_io.h>
43 #include <fcntl.h>
44 #include <pcl/point_cloud.h>
45 #include <limits>
46 #ifdef _WIN32
47 # include <io.h>
48 # include <windows.h>
49 # define pcl_open _open
50 # define pcl_close(fd) _close(fd)
51 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin)
52 #else
53 #include <unistd.h>
54 # include <sys/mman.h>
55 # define pcl_open open
56 # define pcl_close(fd) close(fd)
57 # define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin)
58 #endif
59 
60 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
61 template <typename PointXYZT, typename PointRGBT> bool
63 {
64  // Read in the header
65  int result = static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.file_name[0]), 512));
66  if (result == -1)
67  return (false);
68 
69  // We only support regular files for now.
70  // Addional file types in TAR include: hard links, symbolic links, device/special files, block devices,
71  // directories, and named pipes.
72  if (header.file_type[0] != '0' && header.file_type[0] != '\0')
73  return (false);
74 
75  // We only support USTAR version 0 files for now
76  if (std::string (header.ustar).substr (0, 5) != "ustar")
77  return (false);
78 
79  if (header.getFileSize () == 0)
80  return (false);
81 
82  return (true);
83 }
84 
85 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
86 template <typename PointXYZT, typename PointRGBT> bool
87 pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
88 {
89  // Open the file
90  int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
91  if (ltm_fd == -1)
92  return (false);
93 
94  int ltm_offset = 0;
95 
96  pcl::io::TARHeader ltm_header;
97  PCDReader pcd_reader;
98 
99  std::string pcd_ext (".pcd");
100  std::string sqmmt_ext (".sqmmt");
101 
102  // While there still is an LTM header to be read
103  while (readLTMHeader (ltm_fd, ltm_header))
104  {
105  ltm_offset += 512;
106 
107  // Search for extension
108  std::string chunk_name (ltm_header.file_name);
109 
110  std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower);
111  std::string::size_type it;
112 
113  if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
114  (pcd_ext.size () - (chunk_name.size () - it)) == 0)
115  {
116  PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
117  // Read the next PCD file
118  template_point_clouds_.resize (template_point_clouds_.size () + 1);
119  pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
120 
121  // Increment the offset for the next file
122  ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
123  }
124  else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
125  (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
126  {
127  PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
128 
129  unsigned int fsize = ltm_header.getFileSize ();
130  char *buffer = new char[fsize];
131  int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
132  if (result == -1)
133  {
134  delete [] buffer;
135  PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
136  break;
137  }
138 
139  // Read a SQMMT file
140  std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
141  stream.write (buffer, fsize);
143  sqmmt.deserialize (stream);
144 
145  linemod_.addTemplate (sqmmt);
146  object_ids_.push_back (object_id);
147 
148  // Increment the offset for the next file
149  ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
150 
151  delete [] buffer;
152  }
153 
154  if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
155  break;
156  }
157 
158  // Close the file
159  pcl_close (ltm_fd);
160 
161  // Compute 3D bounding boxes from the template point clouds
162  bounding_boxes_.resize (template_point_clouds_.size ());
163  for (size_t i = 0; i < template_point_clouds_.size (); ++i)
164  {
165  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
166  BoundingBoxXYZ & bb = bounding_boxes_[i];
167  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
168  bb.width = bb.height = bb.depth = 0.0f;
169 
170  float center_x = 0.0f;
171  float center_y = 0.0f;
172  float center_z = 0.0f;
173  float min_x = std::numeric_limits<float>::max ();
174  float min_y = std::numeric_limits<float>::max ();
175  float min_z = std::numeric_limits<float>::max ();
176  float max_x = -std::numeric_limits<float>::max ();
177  float max_y = -std::numeric_limits<float>::max ();
178  float max_z = -std::numeric_limits<float>::max ();
179  size_t counter = 0;
180  for (size_t j = 0; j < template_point_cloud.size (); ++j)
181  {
182  const PointXYZRGBA & p = template_point_cloud.points[j];
183 
184  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
185  continue;
186 
187  min_x = std::min (min_x, p.x);
188  min_y = std::min (min_y, p.y);
189  min_z = std::min (min_z, p.z);
190  max_x = std::max (max_x, p.x);
191  max_y = std::max (max_y, p.y);
192  max_z = std::max (max_z, p.z);
193 
194  center_x += p.x;
195  center_y += p.y;
196  center_z += p.z;
197 
198  ++counter;
199  }
200 
201  center_x /= static_cast<float> (counter);
202  center_y /= static_cast<float> (counter);
203  center_z /= static_cast<float> (counter);
204 
205  bb.width = max_x - min_x;
206  bb.height = max_y - min_y;
207  bb.depth = max_z - min_z;
208 
209  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
210  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
211  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
212 
213  for (size_t j = 0; j < template_point_cloud.size (); ++j)
214  {
215  PointXYZRGBA p = template_point_cloud.points[j];
216 
217  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
218  continue;
219 
220  p.x -= center_x;
221  p.y -= center_y;
222  p.z -= center_z;
223 
224  template_point_cloud.points[j] = p;
225  }
226  }
227 
228  return (true);
229 }
230 
231 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
232 template <typename PointXYZT, typename PointRGBT> int
235  const size_t object_id,
236  const MaskMap & mask_xyz,
237  const MaskMap & mask_rgb,
238  const RegionXY & region)
239 {
240  // add point cloud
241  template_point_clouds_.resize (template_point_clouds_.size () + 1);
242  pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]);
243 
244  // add template
245  object_ids_.push_back (object_id);
246 
247  // Compute 3D bounding boxes from the template point clouds
248  bounding_boxes_.resize (template_point_clouds_.size ());
249  {
250  const size_t i = template_point_clouds_.size () - 1;
251 
252  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
253  BoundingBoxXYZ & bb = bounding_boxes_[i];
254  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
255  bb.width = bb.height = bb.depth = 0.0f;
256 
257  float center_x = 0.0f;
258  float center_y = 0.0f;
259  float center_z = 0.0f;
260  float min_x = std::numeric_limits<float>::max ();
261  float min_y = std::numeric_limits<float>::max ();
262  float min_z = std::numeric_limits<float>::max ();
263  float max_x = -std::numeric_limits<float>::max ();
264  float max_y = -std::numeric_limits<float>::max ();
265  float max_z = -std::numeric_limits<float>::max ();
266  size_t counter = 0;
267  for (size_t j = 0; j < template_point_cloud.size (); ++j)
268  {
269  const PointXYZRGBA & p = template_point_cloud.points[j];
270 
271  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
272  continue;
273 
274  min_x = std::min (min_x, p.x);
275  min_y = std::min (min_y, p.y);
276  min_z = std::min (min_z, p.z);
277  max_x = std::max (max_x, p.x);
278  max_y = std::max (max_y, p.y);
279  max_z = std::max (max_z, p.z);
280 
281  center_x += p.x;
282  center_y += p.y;
283  center_z += p.z;
284 
285  ++counter;
286  }
287 
288  center_x /= static_cast<float> (counter);
289  center_y /= static_cast<float> (counter);
290  center_z /= static_cast<float> (counter);
291 
292  bb.width = max_x - min_x;
293  bb.height = max_y - min_y;
294  bb.depth = max_z - min_z;
295 
296  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
297  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
298  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
299 
300  for (size_t j = 0; j < template_point_cloud.size (); ++j)
301  {
302  PointXYZRGBA p = template_point_cloud.points[j];
303 
304  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
305  continue;
306 
307  p.x -= center_x;
308  p.y -= center_y;
309  p.z -= center_z;
310 
311  template_point_cloud.points[j] = p;
312  }
313  }
314 
315  std::vector<pcl::QuantizableModality*> modalities;
316  modalities.push_back (&color_gradient_mod_);
317  modalities.push_back (&surface_normal_mod_);
318 
319  std::vector<MaskMap*> masks;
320  masks.push_back (const_cast<MaskMap*> (&mask_rgb));
321  masks.push_back (const_cast<MaskMap*> (&mask_xyz));
322 
323  return (linemod_.createAndAddTemplate (modalities, masks, region));
324 }
325 
326 
327 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
328 template <typename PointXYZT, typename PointRGBT> bool
330 {
331  // add point cloud
332  template_point_clouds_.resize (template_point_clouds_.size () + 1);
333  pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]);
334 
335  // add template
336  linemod_.addTemplate (sqmmt);
337  object_ids_.push_back (object_id);
338 
339  // Compute 3D bounding boxes from the template point clouds
340  bounding_boxes_.resize (template_point_clouds_.size ());
341  {
342  const size_t i = template_point_clouds_.size () - 1;
343 
344  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
345  BoundingBoxXYZ & bb = bounding_boxes_[i];
346  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
347  bb.width = bb.height = bb.depth = 0.0f;
348 
349  float center_x = 0.0f;
350  float center_y = 0.0f;
351  float center_z = 0.0f;
352  float min_x = std::numeric_limits<float>::max ();
353  float min_y = std::numeric_limits<float>::max ();
354  float min_z = std::numeric_limits<float>::max ();
355  float max_x = -std::numeric_limits<float>::max ();
356  float max_y = -std::numeric_limits<float>::max ();
357  float max_z = -std::numeric_limits<float>::max ();
358  size_t counter = 0;
359  for (size_t j = 0; j < template_point_cloud.size (); ++j)
360  {
361  const PointXYZRGBA & p = template_point_cloud.points[j];
362 
363  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
364  continue;
365 
366  min_x = std::min (min_x, p.x);
367  min_y = std::min (min_y, p.y);
368  min_z = std::min (min_z, p.z);
369  max_x = std::max (max_x, p.x);
370  max_y = std::max (max_y, p.y);
371  max_z = std::max (max_z, p.z);
372 
373  center_x += p.x;
374  center_y += p.y;
375  center_z += p.z;
376 
377  ++counter;
378  }
379 
380  center_x /= static_cast<float> (counter);
381  center_y /= static_cast<float> (counter);
382  center_z /= static_cast<float> (counter);
383 
384  bb.width = max_x - min_x;
385  bb.height = max_y - min_y;
386  bb.depth = max_z - min_z;
387 
388  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
389  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
390  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
391 
392  for (size_t j = 0; j < template_point_cloud.size (); ++j)
393  {
394  PointXYZRGBA p = template_point_cloud.points[j];
395 
396  if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
397  continue;
398 
399  p.x -= center_x;
400  p.y -= center_y;
401  p.z -= center_z;
402 
403  template_point_cloud.points[j] = p;
404  }
405  }
406 
407  return (true);
408 }
409 
410 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
411 template <typename PointXYZT, typename PointRGBT> void
413  std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections)
414 {
415  std::vector<pcl::QuantizableModality*> modalities;
416  modalities.push_back (&color_gradient_mod_);
417  modalities.push_back (&surface_normal_mod_);
418 
419  std::vector<pcl::LINEMODDetection> linemod_detections;
420  linemod_.detectTemplates (modalities, linemod_detections);
421 
422  detections_.clear ();
423  detections_.reserve (linemod_detections.size ());
424  detections.clear ();
425  detections.reserve (linemod_detections.size ());
426  for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
427  {
428  pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id];
429 
431  detection.template_id = linemod_detection.template_id;
432  detection.object_id = object_ids_[linemod_detection.template_id];
433  detection.detection_id = detection_id;
434  detection.response = linemod_detection.score;
435 
436  // compute bounding box:
437  // we assume that the bounding boxes of the templates are relative to the center of mass
438  // of the template points; so we also compute the center of mass of the points
439  // covered by the
440 
441  const pcl::SparseQuantizedMultiModTemplate & linemod_template =
442  linemod_.getTemplate (linemod_detection.template_id);
443 
444  const size_t start_x = std::max (linemod_detection.x, 0);
445  const size_t start_y = std::max (linemod_detection.y, 0);
446  const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width),
447  static_cast<size_t> (cloud_xyz_->width));
448  const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height),
449  static_cast<size_t> (cloud_xyz_->height));
450 
451  detection.region.x = linemod_detection.x;
452  detection.region.y = linemod_detection.y;
453  detection.region.width = linemod_template.region.width;
454  detection.region.height = linemod_template.region.height;
455 
456  //std::cerr << "detection region: " << linemod_detection.x << ", "
457  // << linemod_detection.y << ", "
458  // << linemod_template.region.width << ", "
459  // << linemod_template.region.height << std::endl;
460 
461  float center_x = 0.0f;
462  float center_y = 0.0f;
463  float center_z = 0.0f;
464  size_t counter = 0;
465  for (size_t row_index = start_y; row_index < end_y; ++row_index)
466  {
467  for (size_t col_index = start_x; col_index < end_x; ++col_index)
468  {
469  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
470 
471  if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z))
472  {
473  center_x += point.x;
474  center_y += point.y;
475  center_z += point.z;
476  ++counter;
477  }
478  }
479  }
480  const float inv_counter = 1.0f / static_cast<float> (counter);
481  center_x *= inv_counter;
482  center_y *= inv_counter;
483  center_z *= inv_counter;
484 
485  pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id];
486 
487  detection.bounding_box = template_bounding_box;
488  detection.bounding_box.x += center_x;
489  detection.bounding_box.y += center_y;
490  detection.bounding_box.z += center_z;
491 
492  detections_.push_back (detection);
493  }
494 
495  // refine detections along depth
496  refineDetectionsAlongDepth ();
497  //applyprojectivedepthicpondetections();
498 
499  // remove overlaps
500  removeOverlappingDetections ();
501 
502  for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
503  {
504  detections.push_back (detections_[detection_index]);
505  }
506 }
507 
508 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
509 template <typename PointXYZT, typename PointRGBT> void
511  std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections,
512  const float min_scale,
513  const float max_scale,
514  const float scale_multiplier)
515 {
516  std::vector<pcl::QuantizableModality*> modalities;
517  modalities.push_back (&color_gradient_mod_);
518  modalities.push_back (&surface_normal_mod_);
519 
520  std::vector<pcl::LINEMODDetection> linemod_detections;
521  linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
522 
523  detections_.clear ();
524  detections_.reserve (linemod_detections.size ());
525  detections.clear ();
526  detections.reserve (linemod_detections.size ());
527  for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
528  {
529  pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id];
530 
532  detection.template_id = linemod_detection.template_id;
533  detection.object_id = object_ids_[linemod_detection.template_id];
534  detection.detection_id = detection_id;
535  detection.response = linemod_detection.score;
536 
537  // compute bounding box:
538  // we assume that the bounding boxes of the templates are relative to the center of mass
539  // of the template points; so we also compute the center of mass of the points
540  // covered by the
541 
542  const pcl::SparseQuantizedMultiModTemplate & linemod_template =
543  linemod_.getTemplate (linemod_detection.template_id);
544 
545  const size_t start_x = std::max (linemod_detection.x, 0);
546  const size_t start_y = std::max (linemod_detection.y, 0);
547  const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width * linemod_detection.scale),
548  static_cast<size_t> (cloud_xyz_->width));
549  const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height * linemod_detection.scale),
550  static_cast<size_t> (cloud_xyz_->height));
551 
552  detection.region.x = linemod_detection.x;
553  detection.region.y = linemod_detection.y;
554  detection.region.width = linemod_template.region.width * linemod_detection.scale;
555  detection.region.height = linemod_template.region.height * linemod_detection.scale;
556 
557  //std::cerr << "detection region: " << linemod_detection.x << ", "
558  // << linemod_detection.y << ", "
559  // << linemod_template.region.width << ", "
560  // << linemod_template.region.height << std::endl;
561 
562  float center_x = 0.0f;
563  float center_y = 0.0f;
564  float center_z = 0.0f;
565  size_t counter = 0;
566  for (size_t row_index = start_y; row_index < end_y; ++row_index)
567  {
568  for (size_t col_index = start_x; col_index < end_x; ++col_index)
569  {
570  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
571 
572  if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z))
573  {
574  center_x += point.x;
575  center_y += point.y;
576  center_z += point.z;
577  ++counter;
578  }
579  }
580  }
581  const float inv_counter = 1.0f / static_cast<float> (counter);
582  center_x *= inv_counter;
583  center_y *= inv_counter;
584  center_z *= inv_counter;
585 
586  pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id];
587 
588  detection.bounding_box = template_bounding_box;
589  detection.bounding_box.x += center_x;
590  detection.bounding_box.y += center_y;
591  detection.bounding_box.z += center_z;
592 
593  detections_.push_back (detection);
594  }
595 
596  // refine detections along depth
597  //refineDetectionsAlongDepth ();
598  //applyProjectiveDepthICPOnDetections();
599 
600  // remove overlaps
601  removeOverlappingDetections ();
602 
603  for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
604  {
605  detections.push_back (detections_[detection_index]);
606  }
607 }
608 
609 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
610 template <typename PointXYZT, typename PointRGBT> void
612  const size_t detection_id, pcl::PointCloud<pcl::PointXYZRGBA> &cloud)
613 {
614  if (detection_id >= detections_.size ())
615  PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
616 
617  const size_t template_id = detections_[detection_id].template_id;
618  const pcl::PointCloud<pcl::PointXYZRGBA> & template_point_cloud = template_point_clouds_[template_id];
619 
620  const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id];
621  const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
622 
623  //std::cerr << "detection: "
624  // << detection_bounding_box.x << ", "
625  // << detection_bounding_box.y << ", "
626  // << detection_bounding_box.z << std::endl;
627  //std::cerr << "template: "
628  // << template_bounding_box.x << ", "
629  // << template_bounding_box.y << ", "
630  // << template_bounding_box.z << std::endl;
631  const float translation_x = detection_bounding_box.x - template_bounding_box.x;
632  const float translation_y = detection_bounding_box.y - template_bounding_box.y;
633  const float translation_z = detection_bounding_box.z - template_bounding_box.z;
634 
635  //std::cerr << "translation: "
636  // << translation_x << ", "
637  // << translation_y << ", "
638  // << translation_z << std::endl;
639 
640  const size_t nr_points = template_point_cloud.size ();
641  cloud.resize (nr_points);
642  cloud.width = template_point_cloud.width;
643  cloud.height = template_point_cloud.height;
644  for (size_t point_index = 0; point_index < nr_points; ++point_index)
645  {
646  pcl::PointXYZRGBA point = template_point_cloud.points[point_index];
647 
648  point.x += translation_x;
649  point.y += translation_y;
650  point.z += translation_z;
651 
652  cloud.points[point_index] = point;
653  }
654 }
655 
656 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
657 template <typename PointXYZT, typename PointRGBT> void
659 {
660  const size_t nr_detections = detections_.size ();
661  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
662  {
663  typename LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
664 
665  // find depth with most valid points
666  const size_t start_x = detection.region.x;
667  const size_t start_y = detection.region.y;
668  const size_t end_x = start_x + detection.region.width;
669  const size_t end_y = start_y + detection.region.height;
670 
671 
672  float min_depth = std::numeric_limits<float>::max ();
673  float max_depth = -std::numeric_limits<float>::max ();
674  for (size_t row_index = start_y; row_index < end_y; ++row_index)
675  {
676  for (size_t col_index = start_x; col_index < end_x; ++col_index)
677  {
678  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
679 
680  if (/*pcl_isfinite (point.x) && pcl_isfinite (point.y) && */pcl_isfinite (point.z))
681  {
682  min_depth = std::min (min_depth, point.z);
683  max_depth = std::max (max_depth, point.z);
684  }
685  }
686  }
687 
688  const size_t nr_bins = 1000;
689  const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1);
690  std::vector<size_t> depth_bins (nr_bins, 0);
691  for (size_t row_index = start_y; row_index < end_y; ++row_index)
692  {
693  for (size_t col_index = start_x; col_index < end_x; ++col_index)
694  {
695  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
696 
697  if (/*pcl_isfinite (point.x) && pcl_isfinite (point.y) && */pcl_isfinite (point.z))
698  {
699  const size_t bin_index = static_cast<size_t> ((point.z - min_depth) / step_size);
700  ++depth_bins[bin_index];
701  }
702  }
703  }
704 
705  std::vector<size_t> integral_depth_bins (nr_bins, 0);
706 
707  integral_depth_bins[0] = depth_bins[0];
708  for (size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
709  {
710  integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
711  }
712 
713  const size_t bb_depth_range = static_cast<size_t> (detection.bounding_box.depth / step_size);
714 
715  size_t max_nr_points = 0;
716  size_t max_index = 0;
717  for (size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index)
718  {
719  const size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
720 
721  if (nr_points_in_range > max_nr_points)
722  {
723  max_nr_points = nr_points_in_range;
724  max_index = bin_index;
725  }
726  }
727 
728  const float z = static_cast<float> (max_index) * step_size + min_depth;
729 
730  detection.bounding_box.z = z;
731  }
732 }
733 
734 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
735 template <typename PointXYZT, typename PointRGBT> void
737 {
738  const size_t nr_detections = detections_.size ();
739  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
740  {
741  typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
742 
743  const size_t template_id = detection.template_id;
744  pcl::PointCloud<pcl::PointXYZRGBA> & point_cloud = template_point_clouds_[template_id];
745 
746  const size_t start_x = detection.region.x;
747  const size_t start_y = detection.region.y;
748  const size_t pc_width = point_cloud.width;
749  const size_t pc_height = point_cloud.height;
750 
751  std::vector<std::pair<float, float> > depth_matches;
752  for (size_t row_index = 0; row_index < pc_height; ++row_index)
753  {
754  for (size_t col_index = 0; col_index < pc_width; ++col_index)
755  {
756  const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index);
757  const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
758 
759  if (!pcl_isfinite (point_template.z) || !pcl_isfinite (point_input.z))
760  continue;
761 
762  depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
763  }
764  }
765 
766  // apply ransac on matches
767  const size_t nr_matches = depth_matches.size ();
768  const size_t nr_iterations = 100; // todo: should be a parameter...
769  const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter...
770  size_t best_nr_inliers = 0;
771  float best_z_translation = 0.0f;
772  for (size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
773  {
774  const size_t rand_index = (rand () * nr_matches) / RAND_MAX;
775 
776  const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
777 
778  size_t nr_inliers = 0;
779  for (size_t match_index = 0; match_index < nr_matches; ++match_index)
780  {
781  const float error = fabsf (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
782 
783  if (error <= inlier_threshold)
784  {
785  ++nr_inliers;
786  }
787  }
788 
789  if (nr_inliers > best_nr_inliers)
790  {
791  best_nr_inliers = nr_inliers;
792  best_z_translation = z_translation;
793  }
794  }
795 
796  float average_depth = 0.0f;
797  size_t average_counter = 0;
798  for (size_t match_index = 0; match_index < nr_matches; ++match_index)
799  {
800  const float error = fabsf (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
801 
802  if (error <= inlier_threshold)
803  {
804  //average_depth += depth_matches[match_index].second;
805  average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
806  ++average_counter;
807  }
808  }
809  average_depth /= static_cast<float> (average_counter);
810 
811  detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f;
812  }
813 }
814 
815 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
816 template <typename PointXYZT, typename PointRGBT> void
818 {
819  // compute overlap between each detection
820  const size_t nr_detections = detections_.size ();
821  Eigen::MatrixXf overlaps (nr_detections, nr_detections);
822  for (size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1)
823  {
824  for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
825  {
826  const float bounding_box_volume = detections_[detection_index_1].bounding_box.width
827  * detections_[detection_index_1].bounding_box.height
828  * detections_[detection_index_1].bounding_box.depth;
829 
830  if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
831  overlaps (detection_index_1, detection_index_2) = 0.0f;
832  else
833  overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
834  detections_[detection_index_1].bounding_box,
835  detections_[detection_index_2].bounding_box) / bounding_box_volume;
836  }
837  }
838 
839  // create clusters of detections
840  std::vector<int> detection_to_cluster_mapping (nr_detections, -1);
841  std::vector<std::vector<size_t> > clusters;
842  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
843  {
844  if (detection_to_cluster_mapping[detection_index] != -1)
845  continue; // already part of a cluster
846 
847  std::vector<size_t> cluster;
848  const size_t cluster_id = clusters.size ();
849 
850  cluster.push_back (detection_index);
851  detection_to_cluster_mapping[detection_index] = static_cast<int> (cluster_id);
852 
853  // check for detections which have sufficient overlap with a detection in the cluster
854  for (size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
855  {
856  const size_t detection_index_1 = cluster[cluster_index];
857 
858  for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
859  {
860  if (detection_to_cluster_mapping[detection_index_2] != -1)
861  continue; // already part of a cluster
862 
863  if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
864  continue; // not enough overlap
865 
866  cluster.push_back (detection_index_2);
867  detection_to_cluster_mapping[detection_index_2] = static_cast<int> (cluster_id);
868  }
869  }
870 
871  clusters.push_back (cluster);
872  }
873 
874  // compute detection representatives for every cluster
875  std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
876 
877  const size_t nr_clusters = clusters.size ();
878  for (size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
879  {
880  std::vector<size_t> & cluster = clusters[cluster_id];
881 
882  float average_center_x = 0.0f;
883  float average_center_y = 0.0f;
884  float average_center_z = 0.0f;
885  float weight_sum = 0.0f;
886 
887  float best_response = 0.0f;
888  size_t best_detection_id = 0;
889 
890  float average_region_x = 0.0f;
891  float average_region_y = 0.0f;
892 
893  const size_t elements_in_cluster = cluster.size ();
894  for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
895  {
896  const size_t detection_id = cluster[cluster_index];
897 
898  const float weight = detections_[detection_id].response;
899 
900  if (weight > best_response)
901  {
902  best_response = weight;
903  best_detection_id = detection_id;
904  }
905 
906  const Detection & d = detections_[detection_id];
907  const float p_center_x = d.bounding_box.x + d.bounding_box.width / 2.0f;
908  const float p_center_y = d.bounding_box.y + d.bounding_box.height / 2.0f;
909  const float p_center_z = d.bounding_box.z + d.bounding_box.depth / 2.0f;
910 
911  average_center_x += p_center_x * weight;
912  average_center_y += p_center_y * weight;
913  average_center_z += p_center_z * weight;
914  weight_sum += weight;
915 
916  average_region_x += float (detections_[detection_id].region.x) * weight;
917  average_region_y += float (detections_[detection_id].region.y) * weight;
918  }
919 
921  detection.template_id = detections_[best_detection_id].template_id;
922  detection.object_id = detections_[best_detection_id].object_id;
923  detection.detection_id = cluster_id;
924  detection.response = best_response;
925 
926  const float inv_weight_sum = 1.0f / weight_sum;
927  const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width;
928  const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height;
929  const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth;
930 
931  detection.bounding_box.x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f;
932  detection.bounding_box.y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f;
933  detection.bounding_box.z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f;
934  detection.bounding_box.width = best_detection_bb_width;
935  detection.bounding_box.height = best_detection_bb_height;
936  detection.bounding_box.depth = best_detection_bb_depth;
937 
938  detection.region.x = int (average_region_x * inv_weight_sum);
939  detection.region.y = int (average_region_y * inv_weight_sum);
940  detection.region.width = detections_[best_detection_id].region.width;
941  detection.region.height = detections_[best_detection_id].region.height;
942 
943  clustered_detections.push_back (detection);
944  }
945 
946  detections_ = clustered_detections;
947 }
948 
949 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
950 template <typename PointXYZT, typename PointRGBT> float
952  const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
953 {
954  const float x1_min = box1.x;
955  const float y1_min = box1.y;
956  const float z1_min = box1.z;
957  const float x1_max = box1.x + box1.width;
958  const float y1_max = box1.y + box1.height;
959  const float z1_max = box1.z + box1.depth;
960 
961  const float x2_min = box2.x;
962  const float y2_min = box2.y;
963  const float z2_min = box2.z;
964  const float x2_max = box2.x + box2.width;
965  const float y2_max = box2.y + box2.height;
966  const float z2_max = box2.z + box2.depth;
967 
968  const float xi_min = std::max (x1_min, x2_min);
969  const float yi_min = std::max (y1_min, y2_min);
970  const float zi_min = std::max (z1_min, z2_min);
971 
972  const float xi_max = std::min (x1_max, x2_max);
973  const float yi_max = std::min (y1_max, y2_max);
974  const float zi_max = std::min (z1_max, z2_max);
975 
976  const float intersection_width = xi_max - xi_min;
977  const float intersection_height = yi_max - yi_min;
978  const float intersection_depth = zi_max - zi_min;
979 
980  if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
981  return 0.0f;
982 
983  const float intersection_volume = intersection_width * intersection_height * intersection_depth;
984 
985  return (intersection_volume);
986 }
987 
988 
989 #endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
990