Point Cloud Library (PCL) 1.12.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.file_name[0]), 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 (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
222template <typename PointXYZT, typename PointRGBT> int
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
317template <typename PointXYZT, typename PointRGBT> bool
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
400template <typename PointXYZT, typename PointRGBT> void
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
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
498template <typename PointXYZT, typename PointRGBT> void
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
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
599template <typename PointXYZT, typename PointRGBT> void
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
646template <typename PointXYZT, typename PointRGBT> void
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
726template <typename PointXYZT, typename PointRGBT> void
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
807template <typename PointXYZT, typename PointRGBT> void
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
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
941template <typename PointXYZT, typename PointRGBT> float
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
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:144
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:314
int template_id
ID of the detected template.
Definition linemod.h:323
int y
y-position of the detection.
Definition linemod.h:321
float scale
scale at which the template was detected.
Definition linemod.h:327
float score
score of the detection.
Definition linemod.h:325
int x
x-position of the detection.
Definition linemod.h:319
A LineRGBD detection.
Definition line_rgbd.h:78
std::size_t template_id
The ID of the template.
Definition line_rgbd.h:83
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition line_rgbd.h:91
std::size_t object_id
The ID of the object corresponding to the template.
Definition line_rgbd.h:85
float response
The response of this detection.
Definition line_rgbd.h:89
std::size_t detection_id
The ID of this detection.
Definition line_rgbd.h:87
RegionXY region
The 2D template region of the detection.
Definition line_rgbd.h:93
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 http://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