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