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