Point Cloud Library (PCL)  1.9.1-dev
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 
47 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointXYZT, typename PointRGBT> bool
50 {
51  // Read in the header
52  int result = static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.file_name[0]), 512));
53  if (result == -1)
54  return (false);
55 
56  // We only support regular files for now.
57  // Additional file types in TAR include: hard links, symbolic links, device/special files, block devices,
58  // directories, and named pipes.
59  if (header.file_type[0] != '0' && header.file_type[0] != '\0')
60  return (false);
61 
62  // We only support USTAR version 0 files for now
63  if (std::string (header.ustar).substr (0, 5) != "ustar")
64  return (false);
65 
66  if (header.getFileSize () == 0)
67  return (false);
68 
69  return (true);
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
73 template <typename PointXYZT, typename PointRGBT> bool
74 pcl::LineRGBD<PointXYZT, PointRGBT>::loadTemplates (const std::string &file_name, const size_t object_id)
75 {
76  // Open the file
77  int ltm_fd = io::raw_open(file_name.c_str (), O_RDONLY);
78  if (ltm_fd == -1)
79  return (false);
80 
81  int ltm_offset = 0;
82 
83  pcl::io::TARHeader ltm_header;
84  PCDReader pcd_reader;
85 
86  std::string pcd_ext (".pcd");
87  std::string sqmmt_ext (".sqmmt");
88 
89  // While there still is an LTM header to be read
90  while (readLTMHeader (ltm_fd, ltm_header))
91  {
92  ltm_offset += 512;
93 
94  // Search for extension
95  std::string chunk_name (ltm_header.file_name);
96 
97  std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower);
98  std::string::size_type it;
99 
100  if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
101  (pcd_ext.size () - (chunk_name.size () - it)) == 0)
102  {
103  PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
104  // Read the next PCD file
105  template_point_clouds_.resize (template_point_clouds_.size () + 1);
106  pcd_reader.read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
107 
108  // Increment the offset for the next file
109  ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
110  }
111  else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
112  (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
113  {
114  PCL_DEBUG ("[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
115 
116  unsigned int fsize = ltm_header.getFileSize ();
117  char *buffer = new char[fsize];
118  int result = static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
119  if (result == -1)
120  {
121  delete [] buffer;
122  PCL_ERROR ("[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
123  break;
124  }
125 
126  // Read a SQMMT file
127  std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
128  stream.write (buffer, fsize);
130  sqmmt.deserialize (stream);
131 
132  linemod_.addTemplate (sqmmt);
133  object_ids_.push_back (object_id);
134 
135  // Increment the offset for the next file
136  ltm_offset += (ltm_header.getFileSize ()) + (512 - ltm_header.getFileSize () % 512);
137 
138  delete [] buffer;
139  }
140 
141  if (io::raw_lseek(ltm_fd, ltm_offset, SEEK_SET) < 0)
142  break;
143  }
144 
145  // Close the file
146  io::raw_close(ltm_fd);
147 
148  // Compute 3D bounding boxes from the template point clouds
149  bounding_boxes_.resize (template_point_clouds_.size ());
150  for (size_t i = 0; i < template_point_clouds_.size (); ++i)
151  {
152  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
153  BoundingBoxXYZ & bb = bounding_boxes_[i];
154  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
155  bb.width = bb.height = bb.depth = 0.0f;
156 
157  float center_x = 0.0f;
158  float center_y = 0.0f;
159  float center_z = 0.0f;
160  float min_x = std::numeric_limits<float>::max ();
161  float min_y = std::numeric_limits<float>::max ();
162  float min_z = std::numeric_limits<float>::max ();
163  float max_x = -std::numeric_limits<float>::max ();
164  float max_y = -std::numeric_limits<float>::max ();
165  float max_z = -std::numeric_limits<float>::max ();
166  size_t counter = 0;
167  for (size_t j = 0; j < template_point_cloud.size (); ++j)
168  {
169  const PointXYZRGBA & p = template_point_cloud.points[j];
170 
171  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
172  continue;
173 
174  min_x = std::min (min_x, p.x);
175  min_y = std::min (min_y, p.y);
176  min_z = std::min (min_z, p.z);
177  max_x = std::max (max_x, p.x);
178  max_y = std::max (max_y, p.y);
179  max_z = std::max (max_z, p.z);
180 
181  center_x += p.x;
182  center_y += p.y;
183  center_z += p.z;
184 
185  ++counter;
186  }
187 
188  center_x /= static_cast<float> (counter);
189  center_y /= static_cast<float> (counter);
190  center_z /= static_cast<float> (counter);
191 
192  bb.width = max_x - min_x;
193  bb.height = max_y - min_y;
194  bb.depth = max_z - min_z;
195 
196  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
197  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
198  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
199 
200  for (size_t j = 0; j < template_point_cloud.size (); ++j)
201  {
202  PointXYZRGBA p = template_point_cloud.points[j];
203 
204  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
205  continue;
206 
207  p.x -= center_x;
208  p.y -= center_y;
209  p.z -= center_z;
210 
211  template_point_cloud.points[j] = p;
212  }
213  }
214 
215  return (true);
216 }
217 
218 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
219 template <typename PointXYZT, typename PointRGBT> int
222  const size_t object_id,
223  const MaskMap & mask_xyz,
224  const MaskMap & mask_rgb,
225  const RegionXY & region)
226 {
227  // add point cloud
228  template_point_clouds_.resize (template_point_clouds_.size () + 1);
229  pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]);
230 
231  // add template
232  object_ids_.push_back (object_id);
233 
234  // Compute 3D bounding boxes from the template point clouds
235  bounding_boxes_.resize (template_point_clouds_.size ());
236  {
237  const size_t i = template_point_clouds_.size () - 1;
238 
239  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
240  BoundingBoxXYZ & bb = bounding_boxes_[i];
241  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
242  bb.width = bb.height = bb.depth = 0.0f;
243 
244  float center_x = 0.0f;
245  float center_y = 0.0f;
246  float center_z = 0.0f;
247  float min_x = std::numeric_limits<float>::max ();
248  float min_y = std::numeric_limits<float>::max ();
249  float min_z = std::numeric_limits<float>::max ();
250  float max_x = -std::numeric_limits<float>::max ();
251  float max_y = -std::numeric_limits<float>::max ();
252  float max_z = -std::numeric_limits<float>::max ();
253  size_t counter = 0;
254  for (size_t j = 0; j < template_point_cloud.size (); ++j)
255  {
256  const PointXYZRGBA & p = template_point_cloud.points[j];
257 
258  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
259  continue;
260 
261  min_x = std::min (min_x, p.x);
262  min_y = std::min (min_y, p.y);
263  min_z = std::min (min_z, p.z);
264  max_x = std::max (max_x, p.x);
265  max_y = std::max (max_y, p.y);
266  max_z = std::max (max_z, p.z);
267 
268  center_x += p.x;
269  center_y += p.y;
270  center_z += p.z;
271 
272  ++counter;
273  }
274 
275  center_x /= static_cast<float> (counter);
276  center_y /= static_cast<float> (counter);
277  center_z /= static_cast<float> (counter);
278 
279  bb.width = max_x - min_x;
280  bb.height = max_y - min_y;
281  bb.depth = max_z - min_z;
282 
283  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
284  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
285  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
286 
287  for (size_t j = 0; j < template_point_cloud.size (); ++j)
288  {
289  PointXYZRGBA p = template_point_cloud.points[j];
290 
291  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
292  continue;
293 
294  p.x -= center_x;
295  p.y -= center_y;
296  p.z -= center_z;
297 
298  template_point_cloud.points[j] = p;
299  }
300  }
301 
302  std::vector<pcl::QuantizableModality*> modalities;
303  modalities.push_back (&color_gradient_mod_);
304  modalities.push_back (&surface_normal_mod_);
305 
306  std::vector<MaskMap*> masks;
307  masks.push_back (const_cast<MaskMap*> (&mask_rgb));
308  masks.push_back (const_cast<MaskMap*> (&mask_xyz));
309 
310  return (linemod_.createAndAddTemplate (modalities, masks, region));
311 }
312 
313 
314 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
315 template <typename PointXYZT, typename PointRGBT> bool
317 {
318  // add point cloud
319  template_point_clouds_.resize (template_point_clouds_.size () + 1);
320  pcl::copyPointCloud (cloud, template_point_clouds_[template_point_clouds_.size () - 1]);
321 
322  // add template
323  linemod_.addTemplate (sqmmt);
324  object_ids_.push_back (object_id);
325 
326  // Compute 3D bounding boxes from the template point clouds
327  bounding_boxes_.resize (template_point_clouds_.size ());
328  {
329  const size_t i = template_point_clouds_.size () - 1;
330 
331  PointCloud<PointXYZRGBA> & template_point_cloud = template_point_clouds_[i];
332  BoundingBoxXYZ & bb = bounding_boxes_[i];
333  bb.x = bb.y = bb.z = std::numeric_limits<float>::max ();
334  bb.width = bb.height = bb.depth = 0.0f;
335 
336  float center_x = 0.0f;
337  float center_y = 0.0f;
338  float center_z = 0.0f;
339  float min_x = std::numeric_limits<float>::max ();
340  float min_y = std::numeric_limits<float>::max ();
341  float min_z = std::numeric_limits<float>::max ();
342  float max_x = -std::numeric_limits<float>::max ();
343  float max_y = -std::numeric_limits<float>::max ();
344  float max_z = -std::numeric_limits<float>::max ();
345  size_t counter = 0;
346  for (size_t j = 0; j < template_point_cloud.size (); ++j)
347  {
348  const PointXYZRGBA & p = template_point_cloud.points[j];
349 
350  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
351  continue;
352 
353  min_x = std::min (min_x, p.x);
354  min_y = std::min (min_y, p.y);
355  min_z = std::min (min_z, p.z);
356  max_x = std::max (max_x, p.x);
357  max_y = std::max (max_y, p.y);
358  max_z = std::max (max_z, p.z);
359 
360  center_x += p.x;
361  center_y += p.y;
362  center_z += p.z;
363 
364  ++counter;
365  }
366 
367  center_x /= static_cast<float> (counter);
368  center_y /= static_cast<float> (counter);
369  center_z /= static_cast<float> (counter);
370 
371  bb.width = max_x - min_x;
372  bb.height = max_y - min_y;
373  bb.depth = max_z - min_z;
374 
375  bb.x = (min_x + bb.width / 2.0f) - center_x - bb.width / 2.0f;
376  bb.y = (min_y + bb.height / 2.0f) - center_y - bb.height / 2.0f;
377  bb.z = (min_z + bb.depth / 2.0f) - center_z - bb.depth / 2.0f;
378 
379  for (size_t j = 0; j < template_point_cloud.size (); ++j)
380  {
381  PointXYZRGBA p = template_point_cloud.points[j];
382 
383  if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
384  continue;
385 
386  p.x -= center_x;
387  p.y -= center_y;
388  p.z -= center_z;
389 
390  template_point_cloud.points[j] = p;
391  }
392  }
393 
394  return (true);
395 }
396 
397 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
398 template <typename PointXYZT, typename PointRGBT> void
400  std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections)
401 {
402  std::vector<pcl::QuantizableModality*> modalities;
403  modalities.push_back (&color_gradient_mod_);
404  modalities.push_back (&surface_normal_mod_);
405 
406  std::vector<pcl::LINEMODDetection> linemod_detections;
407  linemod_.detectTemplates (modalities, linemod_detections);
408 
409  detections_.clear ();
410  detections_.reserve (linemod_detections.size ());
411  detections.clear ();
412  detections.reserve (linemod_detections.size ());
413  for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
414  {
415  pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id];
416 
418  detection.template_id = linemod_detection.template_id;
419  detection.object_id = object_ids_[linemod_detection.template_id];
420  detection.detection_id = detection_id;
421  detection.response = linemod_detection.score;
422 
423  // compute bounding box:
424  // we assume that the bounding boxes of the templates are relative to the center of mass
425  // of the template points; so we also compute the center of mass of the points
426  // covered by the
427 
428  const pcl::SparseQuantizedMultiModTemplate & linemod_template =
429  linemod_.getTemplate (linemod_detection.template_id);
430 
431  const size_t start_x = std::max (linemod_detection.x, 0);
432  const size_t start_y = std::max (linemod_detection.y, 0);
433  const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width),
434  static_cast<size_t> (cloud_xyz_->width));
435  const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height),
436  static_cast<size_t> (cloud_xyz_->height));
437 
438  detection.region.x = linemod_detection.x;
439  detection.region.y = linemod_detection.y;
440  detection.region.width = linemod_template.region.width;
441  detection.region.height = linemod_template.region.height;
442 
443  //std::cerr << "detection region: " << linemod_detection.x << ", "
444  // << linemod_detection.y << ", "
445  // << linemod_template.region.width << ", "
446  // << linemod_template.region.height << std::endl;
447 
448  float center_x = 0.0f;
449  float center_y = 0.0f;
450  float center_z = 0.0f;
451  size_t counter = 0;
452  for (size_t row_index = start_y; row_index < end_y; ++row_index)
453  {
454  for (size_t col_index = start_x; col_index < end_x; ++col_index)
455  {
456  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
457 
458  if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
459  {
460  center_x += point.x;
461  center_y += point.y;
462  center_z += point.z;
463  ++counter;
464  }
465  }
466  }
467  const float inv_counter = 1.0f / static_cast<float> (counter);
468  center_x *= inv_counter;
469  center_y *= inv_counter;
470  center_z *= inv_counter;
471 
472  pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id];
473 
474  detection.bounding_box = template_bounding_box;
475  detection.bounding_box.x += center_x;
476  detection.bounding_box.y += center_y;
477  detection.bounding_box.z += center_z;
478 
479  detections_.push_back (detection);
480  }
481 
482  // refine detections along depth
483  refineDetectionsAlongDepth ();
484  //applyprojectivedepthicpondetections();
485 
486  // remove overlaps
487  removeOverlappingDetections ();
488 
489  for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
490  {
491  detections.push_back (detections_[detection_index]);
492  }
493 }
494 
495 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496 template <typename PointXYZT, typename PointRGBT> void
498  std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections,
499  const float min_scale,
500  const float max_scale,
501  const float scale_multiplier)
502 {
503  std::vector<pcl::QuantizableModality*> modalities;
504  modalities.push_back (&color_gradient_mod_);
505  modalities.push_back (&surface_normal_mod_);
506 
507  std::vector<pcl::LINEMODDetection> linemod_detections;
508  linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
509 
510  detections_.clear ();
511  detections_.reserve (linemod_detections.size ());
512  detections.clear ();
513  detections.reserve (linemod_detections.size ());
514  for (size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
515  {
516  pcl::LINEMODDetection & linemod_detection = linemod_detections[detection_id];
517 
519  detection.template_id = linemod_detection.template_id;
520  detection.object_id = object_ids_[linemod_detection.template_id];
521  detection.detection_id = detection_id;
522  detection.response = linemod_detection.score;
523 
524  // compute bounding box:
525  // we assume that the bounding boxes of the templates are relative to the center of mass
526  // of the template points; so we also compute the center of mass of the points
527  // covered by the
528 
529  const pcl::SparseQuantizedMultiModTemplate & linemod_template =
530  linemod_.getTemplate (linemod_detection.template_id);
531 
532  const size_t start_x = std::max (linemod_detection.x, 0);
533  const size_t start_y = std::max (linemod_detection.y, 0);
534  const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.region.width * linemod_detection.scale),
535  static_cast<size_t> (cloud_xyz_->width));
536  const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.region.height * linemod_detection.scale),
537  static_cast<size_t> (cloud_xyz_->height));
538 
539  detection.region.x = linemod_detection.x;
540  detection.region.y = linemod_detection.y;
541  detection.region.width = linemod_template.region.width * linemod_detection.scale;
542  detection.region.height = linemod_template.region.height * linemod_detection.scale;
543 
544  //std::cerr << "detection region: " << linemod_detection.x << ", "
545  // << linemod_detection.y << ", "
546  // << linemod_template.region.width << ", "
547  // << linemod_template.region.height << std::endl;
548 
549  float center_x = 0.0f;
550  float center_y = 0.0f;
551  float center_z = 0.0f;
552  size_t counter = 0;
553  for (size_t row_index = start_y; row_index < end_y; ++row_index)
554  {
555  for (size_t col_index = start_x; col_index < end_x; ++col_index)
556  {
557  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
558 
559  if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
560  {
561  center_x += point.x;
562  center_y += point.y;
563  center_z += point.z;
564  ++counter;
565  }
566  }
567  }
568  const float inv_counter = 1.0f / static_cast<float> (counter);
569  center_x *= inv_counter;
570  center_y *= inv_counter;
571  center_z *= inv_counter;
572 
573  pcl::BoundingBoxXYZ template_bounding_box = bounding_boxes_[detection.template_id];
574 
575  detection.bounding_box = template_bounding_box;
576  detection.bounding_box.x += center_x;
577  detection.bounding_box.y += center_y;
578  detection.bounding_box.z += center_z;
579 
580  detections_.push_back (detection);
581  }
582 
583  // refine detections along depth
584  //refineDetectionsAlongDepth ();
585  //applyProjectiveDepthICPOnDetections();
586 
587  // remove overlaps
588  removeOverlappingDetections ();
589 
590  for (size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
591  {
592  detections.push_back (detections_[detection_index]);
593  }
594 }
595 
596 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
597 template <typename PointXYZT, typename PointRGBT> void
599  const size_t detection_id, pcl::PointCloud<pcl::PointXYZRGBA> &cloud)
600 {
601  if (detection_id >= detections_.size ())
602  PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
603 
604  const size_t template_id = detections_[detection_id].template_id;
605  const pcl::PointCloud<pcl::PointXYZRGBA> & template_point_cloud = template_point_clouds_[template_id];
606 
607  const pcl::BoundingBoxXYZ & template_bounding_box = bounding_boxes_[template_id];
608  const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
609 
610  //std::cerr << "detection: "
611  // << detection_bounding_box.x << ", "
612  // << detection_bounding_box.y << ", "
613  // << detection_bounding_box.z << std::endl;
614  //std::cerr << "template: "
615  // << template_bounding_box.x << ", "
616  // << template_bounding_box.y << ", "
617  // << template_bounding_box.z << std::endl;
618  const float translation_x = detection_bounding_box.x - template_bounding_box.x;
619  const float translation_y = detection_bounding_box.y - template_bounding_box.y;
620  const float translation_z = detection_bounding_box.z - template_bounding_box.z;
621 
622  //std::cerr << "translation: "
623  // << translation_x << ", "
624  // << translation_y << ", "
625  // << translation_z << std::endl;
626 
627  const size_t nr_points = template_point_cloud.size ();
628  cloud.resize (nr_points);
629  cloud.width = template_point_cloud.width;
630  cloud.height = template_point_cloud.height;
631  for (size_t point_index = 0; point_index < nr_points; ++point_index)
632  {
633  pcl::PointXYZRGBA point = template_point_cloud.points[point_index];
634 
635  point.x += translation_x;
636  point.y += translation_y;
637  point.z += translation_z;
638 
639  cloud.points[point_index] = point;
640  }
641 }
642 
643 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
644 template <typename PointXYZT, typename PointRGBT> void
646 {
647  const size_t nr_detections = detections_.size ();
648  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
649  {
650  typename LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
651 
652  // find depth with most valid points
653  const size_t start_x = std::max (detection.region.x, 0);
654  const size_t start_y = std::max (detection.region.y, 0);
655  const size_t end_x = std::min (static_cast<size_t> (detection.region.x + detection.region.width),
656  static_cast<size_t> (cloud_xyz_->width));
657  const size_t end_y = std::min (static_cast<size_t> (detection.region.y + detection.region.height),
658  static_cast<size_t> (cloud_xyz_->height));
659 
660 
661  float min_depth = std::numeric_limits<float>::max ();
662  float max_depth = -std::numeric_limits<float>::max ();
663  for (size_t row_index = start_y; row_index < end_y; ++row_index)
664  {
665  for (size_t col_index = start_x; col_index < end_x; ++col_index)
666  {
667  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
668 
669  if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z))
670  {
671  min_depth = std::min (min_depth, point.z);
672  max_depth = std::max (max_depth, point.z);
673  }
674  }
675  }
676 
677  const size_t nr_bins = 1000;
678  const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1);
679  std::vector<size_t> depth_bins (nr_bins, 0);
680  for (size_t row_index = start_y; row_index < end_y; ++row_index)
681  {
682  for (size_t col_index = start_x; col_index < end_x; ++col_index)
683  {
684  const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
685 
686  if (/*std::isfinite (point.x) && std::isfinite (point.y) && */std::isfinite (point.z))
687  {
688  const size_t bin_index = static_cast<size_t> ((point.z - min_depth) / step_size);
689  ++depth_bins[bin_index];
690  }
691  }
692  }
693 
694  std::vector<size_t> integral_depth_bins (nr_bins, 0);
695 
696  integral_depth_bins[0] = depth_bins[0];
697  for (size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
698  {
699  integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
700  }
701 
702  const size_t bb_depth_range = static_cast<size_t> (detection.bounding_box.depth / step_size);
703 
704  size_t max_nr_points = 0;
705  size_t max_index = 0;
706  for (size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index)
707  {
708  const size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
709 
710  if (nr_points_in_range > max_nr_points)
711  {
712  max_nr_points = nr_points_in_range;
713  max_index = bin_index;
714  }
715  }
716 
717  const float z = static_cast<float> (max_index) * step_size + min_depth;
718 
719  detection.bounding_box.z = z;
720  }
721 }
722 
723 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
724 template <typename PointXYZT, typename PointRGBT> void
726 {
727  const size_t nr_detections = detections_.size ();
728  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
729  {
730  typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection & detection = detections_[detection_index];
731 
732  const size_t template_id = detection.template_id;
733  pcl::PointCloud<pcl::PointXYZRGBA> & point_cloud = template_point_clouds_[template_id];
734 
735  const size_t start_x = detection.region.x;
736  const size_t start_y = detection.region.y;
737  const size_t pc_width = point_cloud.width;
738  const size_t pc_height = point_cloud.height;
739 
740  std::vector<std::pair<float, float> > depth_matches;
741  for (size_t row_index = 0; row_index < pc_height; ++row_index)
742  {
743  for (size_t col_index = 0; col_index < pc_width; ++col_index)
744  {
745  const pcl::PointXYZRGBA & point_template = point_cloud (col_index, row_index);
746  const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
747 
748  if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z))
749  continue;
750 
751  depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
752  }
753  }
754 
755  // apply ransac on matches
756  const size_t nr_matches = depth_matches.size ();
757  const size_t nr_iterations = 100; // todo: should be a parameter...
758  const float inlier_threshold = 0.01f; // 5cm // todo: should be a parameter...
759  size_t best_nr_inliers = 0;
760  float best_z_translation = 0.0f;
761  for (size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
762  {
763  const size_t rand_index = (rand () * nr_matches) / RAND_MAX;
764 
765  const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
766 
767  size_t nr_inliers = 0;
768  for (size_t match_index = 0; match_index < nr_matches; ++match_index)
769  {
770  const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
771 
772  if (error <= inlier_threshold)
773  {
774  ++nr_inliers;
775  }
776  }
777 
778  if (nr_inliers > best_nr_inliers)
779  {
780  best_nr_inliers = nr_inliers;
781  best_z_translation = z_translation;
782  }
783  }
784 
785  float average_depth = 0.0f;
786  size_t average_counter = 0;
787  for (size_t match_index = 0; match_index < nr_matches; ++match_index)
788  {
789  const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
790 
791  if (error <= inlier_threshold)
792  {
793  //average_depth += depth_matches[match_index].second;
794  average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
795  ++average_counter;
796  }
797  }
798  average_depth /= static_cast<float> (average_counter);
799 
800  detection.bounding_box.z = bounding_boxes_[template_id].z + average_depth;// - detection.bounding_box.depth/2.0f;
801  }
802 }
803 
804 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
805 template <typename PointXYZT, typename PointRGBT> void
807 {
808  // compute overlap between each detection
809  const size_t nr_detections = detections_.size ();
810  Eigen::MatrixXf overlaps (nr_detections, nr_detections);
811  for (size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1)
812  {
813  for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
814  {
815  const float bounding_box_volume = detections_[detection_index_1].bounding_box.width
816  * detections_[detection_index_1].bounding_box.height
817  * detections_[detection_index_1].bounding_box.depth;
818 
819  if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
820  overlaps (detection_index_1, detection_index_2) = 0.0f;
821  else
822  overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
823  detections_[detection_index_1].bounding_box,
824  detections_[detection_index_2].bounding_box) / bounding_box_volume;
825  }
826  }
827 
828  // create clusters of detections
829  std::vector<int> detection_to_cluster_mapping (nr_detections, -1);
830  std::vector<std::vector<size_t> > clusters;
831  for (size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
832  {
833  if (detection_to_cluster_mapping[detection_index] != -1)
834  continue; // already part of a cluster
835 
836  std::vector<size_t> cluster;
837  const size_t cluster_id = clusters.size ();
838 
839  cluster.push_back (detection_index);
840  detection_to_cluster_mapping[detection_index] = static_cast<int> (cluster_id);
841 
842  // check for detections which have sufficient overlap with a detection in the cluster
843  for (size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
844  {
845  const size_t detection_index_1 = cluster[cluster_index];
846 
847  for (size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
848  {
849  if (detection_to_cluster_mapping[detection_index_2] != -1)
850  continue; // already part of a cluster
851 
852  if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
853  continue; // not enough overlap
854 
855  cluster.push_back (detection_index_2);
856  detection_to_cluster_mapping[detection_index_2] = static_cast<int> (cluster_id);
857  }
858  }
859 
860  clusters.push_back (cluster);
861  }
862 
863  // compute detection representatives for every cluster
864  std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
865 
866  const size_t nr_clusters = clusters.size ();
867  for (size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
868  {
869  std::vector<size_t> & cluster = clusters[cluster_id];
870 
871  float average_center_x = 0.0f;
872  float average_center_y = 0.0f;
873  float average_center_z = 0.0f;
874  float weight_sum = 0.0f;
875 
876  float best_response = 0.0f;
877  size_t best_detection_id = 0;
878 
879  float average_region_x = 0.0f;
880  float average_region_y = 0.0f;
881 
882  const size_t elements_in_cluster = cluster.size ();
883  for (size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
884  {
885  const size_t detection_id = cluster[cluster_index];
886 
887  const float weight = detections_[detection_id].response;
888 
889  if (weight > best_response)
890  {
891  best_response = weight;
892  best_detection_id = detection_id;
893  }
894 
895  const Detection & d = detections_[detection_id];
896  const float p_center_x = d.bounding_box.x + d.bounding_box.width / 2.0f;
897  const float p_center_y = d.bounding_box.y + d.bounding_box.height / 2.0f;
898  const float p_center_z = d.bounding_box.z + d.bounding_box.depth / 2.0f;
899 
900  average_center_x += p_center_x * weight;
901  average_center_y += p_center_y * weight;
902  average_center_z += p_center_z * weight;
903  weight_sum += weight;
904 
905  average_region_x += float (detections_[detection_id].region.x) * weight;
906  average_region_y += float (detections_[detection_id].region.y) * weight;
907  }
908 
910  detection.template_id = detections_[best_detection_id].template_id;
911  detection.object_id = detections_[best_detection_id].object_id;
912  detection.detection_id = cluster_id;
913  detection.response = best_response;
914 
915  const float inv_weight_sum = 1.0f / weight_sum;
916  const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width;
917  const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height;
918  const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth;
919 
920  detection.bounding_box.x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f;
921  detection.bounding_box.y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f;
922  detection.bounding_box.z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f;
923  detection.bounding_box.width = best_detection_bb_width;
924  detection.bounding_box.height = best_detection_bb_height;
925  detection.bounding_box.depth = best_detection_bb_depth;
926 
927  detection.region.x = int (average_region_x * inv_weight_sum);
928  detection.region.y = int (average_region_y * inv_weight_sum);
929  detection.region.width = detections_[best_detection_id].region.width;
930  detection.region.height = detections_[best_detection_id].region.height;
931 
932  clustered_detections.push_back (detection);
933  }
934 
935  detections_ = clustered_detections;
936 }
937 
938 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
939 template <typename PointXYZT, typename PointRGBT> float
941  const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
942 {
943  const float x1_min = box1.x;
944  const float y1_min = box1.y;
945  const float z1_min = box1.z;
946  const float x1_max = box1.x + box1.width;
947  const float y1_max = box1.y + box1.height;
948  const float z1_max = box1.z + box1.depth;
949 
950  const float x2_min = box2.x;
951  const float y2_min = box2.y;
952  const float z2_min = box2.z;
953  const float x2_max = box2.x + box2.width;
954  const float y2_max = box2.y + box2.height;
955  const float z2_max = box2.z + box2.depth;
956 
957  const float xi_min = std::max (x1_min, x2_min);
958  const float yi_min = std::max (y1_min, y2_min);
959  const float zi_min = std::max (z1_min, z2_min);
960 
961  const float xi_max = std::min (x1_max, x2_max);
962  const float yi_max = std::min (y1_max, y2_max);
963  const float zi_max = std::min (z1_max, z2_max);
964 
965  const float intersection_width = xi_max - xi_min;
966  const float intersection_height = yi_max - yi_min;
967  const float intersection_depth = zi_max - zi_min;
968 
969  if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
970  return 0.0f;
971 
972  const float intersection_volume = intersection_width * intersection_height * intersection_depth;
973 
974  return (intersection_volume);
975 }
976 
977 
978 #endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
979 
void applyProjectiveDepthICPOnDetections()
Applies projective ICP on detections to find their correct position in depth.
Definition: line_rgbd.hpp:725
float z
Z-coordinate of the upper left front point.
Definition: line_rgbd.h:58
A TAR file&#39;s header, as described on http://en.wikipedia.org/wiki/Tar_%28file_format%29.
Definition: tar.h:49
unsigned int getFileSize()
get file size
Definition: tar.h:71
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:426
void detectSemiScaleInvariant(std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > &detections, float min_scale=0.6944444f, float max_scale=1.44f, float scale_multiplier=1.2f)
Applies the detection process in a semi-scale-invariant manner.
Definition: line_rgbd.hpp:497
void resize(std::size_t n)
Resize the cloud.
Definition: point_cloud.h:471
float score
score of the detection.
Definition: linemod.h:325
void removeOverlappingDetections()
Checks for overlapping detections, removes them and keeps only the strongest one. ...
Definition: line_rgbd.hpp:806
std::size_t size() const
Definition: point_cloud.h:464
A LineRGBD detection.
Definition: line_rgbd.h:77
int template_id
ID of the detected template.
Definition: linemod.h:323
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
Represents a detection of a template using the LINEMOD approach.
Definition: linemod.h:313
Defines a region in XY-space.
Definition: region_xy.h:81
A point structure representing Euclidean xyz coordinates, and the RGBA color.
float y
Y-coordinate of the upper left front point.
Definition: line_rgbd.h:56
int y
y-position of the detection.
Definition: linemod.h:321
A multi-modality template constructed from a set of quantized multi-modality features.
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:431
int x
x-position of the region.
Definition: region_xy.h:87
void detect(std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > &detections)
Applies the detection process and fills the supplied vector with the detection instances.
Definition: line_rgbd.hpp:399
float x
X-coordinate of the upper left front point.
Definition: line_rgbd.h:54
int y
y-position of the region.
Definition: region_xy.h:89
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:429
bool loadTemplates(const std::string &file_name, size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
Definition: line_rgbd.hpp:74
float width
Width of the bounding box.
Definition: line_rgbd.h:61
float scale
scale at which the template was detected.
Definition: linemod.h:327
size_t detection_id
The ID of this detection.
Definition: line_rgbd.h:87
RegionXY region
The 2D template region of the detection.
Definition: line_rgbd.h:93
int height
height of the region.
Definition: region_xy.h:93
float depth
Depth of the bounding box.
Definition: line_rgbd.h:65
char ustar[6]
Definition: tar.h:60
char file_name[100]
Definition: tar.h:51
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0) override
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.
void deserialize(std::istream &stream)
Deserializes the object from the specified stream.
char file_type[1]
Definition: tar.h:58
RegionXY region
The region assigned to the template.
size_t object_id
The ID of the object corresponding to the template.
Definition: line_rgbd.h:85
void refineDetectionsAlongDepth()
Refines the found detections along the depth.
Definition: line_rgbd.hpp:645
void computeTransformedTemplatePoints(const size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
Definition: line_rgbd.hpp:598
Point Cloud Data (PCD) file format reader.
Definition: pcd_io.h:52
High-level class for template matching using the LINEMOD approach based on RGB and Depth data...
Definition: line_rgbd.h:72
int x
x-position of the detection.
Definition: linemod.h:319
int createAndAddTemplate(pcl::PointCloud< pcl::PointXYZRGBA > &cloud, const size_t object_id, const MaskMap &mask_xyz, const MaskMap &mask_rgb, const RegionXY &region)
Creates a template from the specified data and adds it to the matching queue.
Definition: line_rgbd.hpp:220
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
Definition: line_rgbd.hpp:940
int width
width of the region.
Definition: region_xy.h:91
size_t template_id
The ID of the template.
Definition: line_rgbd.h:83
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, size_t object_id=0)
Definition: line_rgbd.hpp:316
float response
The response of this detection.
Definition: line_rgbd.h:89
float height
Height of the bounding box.
Definition: line_rgbd.h:63
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition: line_rgbd.h:91