Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
48namespace pcl
49{
50
51template <typename PointXYZT, typename PointRGBT> bool
52LineRGBD<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
76template <typename PointXYZT, typename PointRGBT> bool
77LineRGBD<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);
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
220template <typename PointXYZT, typename PointRGBT> int
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
313template <typename PointXYZT, typename PointRGBT> bool
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
394template <typename PointXYZT, typename PointRGBT> void
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
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
492template <typename PointXYZT, typename PointRGBT> void
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
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
593template <typename PointXYZT, typename PointRGBT> void
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
640template <typename PointXYZT, typename PointRGBT> void
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
720template <typename PointXYZT, typename PointRGBT> void
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
801template <typename PointXYZT, typename PointRGBT> void
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
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
935template <typename PointXYZT, typename PointRGBT> float
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
void detect(std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > &detections)
Applies the detection process and fills the supplied vector with the detection instances.
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.
void refineDetectionsAlongDepth()
Refines the found detections along the depth.
void applyProjectiveDepthICPOnDetections()
Applies projective ICP on detections to find their correct position in depth.
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, std::size_t object_id=0)
void computeTransformedTemplatePoints(const std::size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
void removeOverlappingDetections()
Checks for overlapping detections, removes them and keeps only the strongest one.
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
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.
Point Cloud Data (PCD) file format reader.
Definition pcd_io.h:55
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
std::size_t size() const
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
int raw_close(int fd)
int raw_lseek(int fd, long offset, int whence)
int raw_open(const char *pathname, int flags, int mode)
int raw_read(int fd, void *buffer, std::size_t count)
float width
Width of the bounding box.
Definition line_rgbd.h:61
float x
X-coordinate of the upper left front point.
Definition line_rgbd.h:54
float y
Y-coordinate of the upper left front point.
Definition line_rgbd.h:56
float depth
Depth of the bounding box.
Definition line_rgbd.h:65
float z
Z-coordinate of the upper left front point.
Definition line_rgbd.h:58
float height
Height of the bounding box.
Definition line_rgbd.h:63
Represents a detection of a template using the LINEMOD approach.
Definition linemod.h:305
int template_id
ID of the detected template.
Definition linemod.h:314
int y
y-position of the detection.
Definition linemod.h:312
float scale
scale at which the template was detected.
Definition linemod.h:318
float score
score of the detection.
Definition linemod.h:316
int x
x-position of the detection.
Definition linemod.h:310
A LineRGBD detection.
Definition line_rgbd.h:79
std::size_t template_id
The ID of the template.
Definition line_rgbd.h:84
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition line_rgbd.h:92
std::size_t object_id
The ID of the object corresponding to the template.
Definition line_rgbd.h:86
float response
The response of this detection.
Definition line_rgbd.h:90
std::size_t detection_id
The ID of this detection.
Definition line_rgbd.h:88
RegionXY region
The 2D template region of the detection.
Definition line_rgbd.h:94
A point structure representing Euclidean xyz coordinates, and the RGBA color.
Defines a region in XY-space.
Definition region_xy.h:82
int x
x-position of the region.
Definition region_xy.h:87
int width
width of the region.
Definition region_xy.h:91
int y
y-position of the region.
Definition region_xy.h:89
int height
height of the region.
Definition region_xy.h:93
A multi-modality template constructed from a set of quantized multi-modality features.
RegionXY region
The region assigned to the template.
void deserialize(std::istream &stream)
Deserializes the object from the specified stream.
A TAR file's header, as described on https://en.wikipedia.org/wiki/Tar_%28file_format%29.
Definition tar.h:50
char file_name[100]
Definition tar.h:51
unsigned int getFileSize()
get file size
Definition tar.h:71
char ustar[6]
Definition tar.h:60
char file_type[1]
Definition tar.h:58