38 #ifndef PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
39 #define PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
42 #include <pcl/io/pcd_io.h>
44 #include <pcl/point_cloud.h>
49 # define pcl_open _open
50 # define pcl_close(fd) _close(fd)
51 # define pcl_lseek(fd,offset,origin) _lseek(fd,offset,origin)
54 # include <sys/mman.h>
55 # define pcl_open open
56 # define pcl_close(fd) close(fd)
57 # define pcl_lseek(fd,offset,origin) lseek(fd,offset,origin)
61 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
65 int result =
static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.
file_name[0]), 512));
76 if (std::string (header.
ustar).substr (0, 5) !=
"ustar")
86 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
90 int ltm_fd = pcl_open (file_name.c_str (), O_RDONLY);
99 std::string pcd_ext (
".pcd");
100 std::string sqmmt_ext (
".sqmmt");
103 while (readLTMHeader (ltm_fd, ltm_header))
108 std::string chunk_name (ltm_header.
file_name);
110 std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), tolower);
111 std::string::size_type it;
113 if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
114 (pcd_ext.size () - (chunk_name.size () - it)) == 0)
116 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
118 template_point_clouds_.resize (template_point_clouds_.size () + 1);
119 pcd_reader.
read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
124 else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
125 (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
127 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
130 char *buffer =
new char[fsize];
131 int result =
static_cast<int> (::read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
135 PCL_ERROR (
"[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
140 std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
141 stream.write (buffer, fsize);
145 linemod_.addTemplate (sqmmt);
146 object_ids_.push_back (object_id);
154 if (static_cast<int> (pcl_lseek (ltm_fd, ltm_offset, SEEK_SET)) < 0)
162 bounding_boxes_.resize (template_point_clouds_.size ());
163 for (
size_t i = 0; i < template_point_clouds_.size (); ++i)
167 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
170 float center_x = 0.0f;
171 float center_y = 0.0f;
172 float center_z = 0.0f;
173 float min_x = std::numeric_limits<float>::max ();
174 float min_y = std::numeric_limits<float>::max ();
175 float min_z = std::numeric_limits<float>::max ();
176 float max_x = -std::numeric_limits<float>::max ();
177 float max_y = -std::numeric_limits<float>::max ();
178 float max_z = -std::numeric_limits<float>::max ();
180 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
184 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
187 min_x = std::min (min_x, p.x);
188 min_y = std::min (min_y, p.y);
189 min_z = std::min (min_z, p.z);
190 max_x = std::max (max_x, p.x);
191 max_y = std::max (max_y, p.y);
192 max_z = std::max (max_z, p.z);
201 center_x /=
static_cast<float> (counter);
202 center_y /=
static_cast<float> (counter);
203 center_z /=
static_cast<float> (counter);
205 bb.
width = max_x - min_x;
206 bb.
height = max_y - min_y;
207 bb.
depth = max_z - min_z;
209 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
210 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
211 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
213 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
217 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
224 template_point_cloud.
points[j] = p;
232 template <
typename Po
intXYZT,
typename Po
intRGBT>
int
235 const size_t object_id,
241 template_point_clouds_.resize (template_point_clouds_.size () + 1);
245 object_ids_.push_back (object_id);
248 bounding_boxes_.resize (template_point_clouds_.size ());
250 const size_t i = template_point_clouds_.size () - 1;
254 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
257 float center_x = 0.0f;
258 float center_y = 0.0f;
259 float center_z = 0.0f;
260 float min_x = std::numeric_limits<float>::max ();
261 float min_y = std::numeric_limits<float>::max ();
262 float min_z = std::numeric_limits<float>::max ();
263 float max_x = -std::numeric_limits<float>::max ();
264 float max_y = -std::numeric_limits<float>::max ();
265 float max_z = -std::numeric_limits<float>::max ();
267 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
271 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
274 min_x = std::min (min_x, p.x);
275 min_y = std::min (min_y, p.y);
276 min_z = std::min (min_z, p.z);
277 max_x = std::max (max_x, p.x);
278 max_y = std::max (max_y, p.y);
279 max_z = std::max (max_z, p.z);
288 center_x /=
static_cast<float> (counter);
289 center_y /=
static_cast<float> (counter);
290 center_z /=
static_cast<float> (counter);
292 bb.
width = max_x - min_x;
293 bb.
height = max_y - min_y;
294 bb.
depth = max_z - min_z;
296 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
297 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
298 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
300 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
304 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
311 template_point_cloud.
points[j] = p;
315 std::vector<pcl::QuantizableModality*> modalities;
316 modalities.push_back (&color_gradient_mod_);
317 modalities.push_back (&surface_normal_mod_);
319 std::vector<MaskMap*> masks;
320 masks.push_back (const_cast<MaskMap*> (&mask_rgb));
321 masks.push_back (const_cast<MaskMap*> (&mask_xyz));
323 return (linemod_.createAndAddTemplate (modalities, masks, region));
328 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
332 template_point_clouds_.resize (template_point_clouds_.size () + 1);
336 linemod_.addTemplate (sqmmt);
337 object_ids_.push_back (object_id);
340 bounding_boxes_.resize (template_point_clouds_.size ());
342 const size_t i = template_point_clouds_.size () - 1;
346 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
349 float center_x = 0.0f;
350 float center_y = 0.0f;
351 float center_z = 0.0f;
352 float min_x = std::numeric_limits<float>::max ();
353 float min_y = std::numeric_limits<float>::max ();
354 float min_z = std::numeric_limits<float>::max ();
355 float max_x = -std::numeric_limits<float>::max ();
356 float max_y = -std::numeric_limits<float>::max ();
357 float max_z = -std::numeric_limits<float>::max ();
359 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
363 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
366 min_x = std::min (min_x, p.x);
367 min_y = std::min (min_y, p.y);
368 min_z = std::min (min_z, p.z);
369 max_x = std::max (max_x, p.x);
370 max_y = std::max (max_y, p.y);
371 max_z = std::max (max_z, p.z);
380 center_x /=
static_cast<float> (counter);
381 center_y /=
static_cast<float> (counter);
382 center_z /=
static_cast<float> (counter);
384 bb.
width = max_x - min_x;
385 bb.
height = max_y - min_y;
386 bb.
depth = max_z - min_z;
388 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
389 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
390 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
392 for (
size_t j = 0; j < template_point_cloud.
size (); ++j)
396 if (!pcl_isfinite (p.x) || !pcl_isfinite (p.y) || !pcl_isfinite (p.z))
403 template_point_cloud.
points[j] = p;
411 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
415 std::vector<pcl::QuantizableModality*> modalities;
416 modalities.push_back (&color_gradient_mod_);
417 modalities.push_back (&surface_normal_mod_);
419 std::vector<pcl::LINEMODDetection> linemod_detections;
420 linemod_.detectTemplates (modalities, linemod_detections);
422 detections_.clear ();
423 detections_.reserve (linemod_detections.size ());
425 detections.reserve (linemod_detections.size ());
426 for (
size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
442 linemod_.getTemplate (linemod_detection.
template_id);
444 const size_t start_x = std::max (linemod_detection.
x, 0);
445 const size_t start_y = std::max (linemod_detection.
y, 0);
446 const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.
region.
width),
447 static_cast<size_t> (cloud_xyz_->width));
448 const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.
region.
height),
449 static_cast<size_t> (cloud_xyz_->height));
451 detection.
region.
x = linemod_detection.
x;
452 detection.
region.
y = linemod_detection.
y;
461 float center_x = 0.0f;
462 float center_y = 0.0f;
463 float center_z = 0.0f;
465 for (
size_t row_index = start_y; row_index < end_y; ++row_index)
467 for (
size_t col_index = start_x; col_index < end_x; ++col_index)
469 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
471 if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z))
480 const float inv_counter = 1.0f /
static_cast<float> (counter);
481 center_x *= inv_counter;
482 center_y *= inv_counter;
483 center_z *= inv_counter;
492 detections_.push_back (detection);
496 refineDetectionsAlongDepth ();
500 removeOverlappingDetections ();
502 for (
size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
504 detections.push_back (detections_[detection_index]);
509 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
512 const float min_scale,
513 const float max_scale,
514 const float scale_multiplier)
516 std::vector<pcl::QuantizableModality*> modalities;
517 modalities.push_back (&color_gradient_mod_);
518 modalities.push_back (&surface_normal_mod_);
520 std::vector<pcl::LINEMODDetection> linemod_detections;
521 linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
523 detections_.clear ();
524 detections_.reserve (linemod_detections.size ());
526 detections.reserve (linemod_detections.size ());
527 for (
size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
543 linemod_.getTemplate (linemod_detection.
template_id);
545 const size_t start_x = std::max (linemod_detection.
x, 0);
546 const size_t start_y = std::max (linemod_detection.
y, 0);
547 const size_t end_x = std::min (static_cast<size_t> (start_x + linemod_template.
region.
width * linemod_detection.
scale),
548 static_cast<size_t> (cloud_xyz_->width));
549 const size_t end_y = std::min (static_cast<size_t> (start_y + linemod_template.
region.
height * linemod_detection.
scale),
550 static_cast<size_t> (cloud_xyz_->height));
552 detection.
region.
x = linemod_detection.
x;
553 detection.
region.
y = linemod_detection.
y;
562 float center_x = 0.0f;
563 float center_y = 0.0f;
564 float center_z = 0.0f;
566 for (
size_t row_index = start_y; row_index < end_y; ++row_index)
568 for (
size_t col_index = start_x; col_index < end_x; ++col_index)
570 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
572 if (pcl_isfinite (point.x) && pcl_isfinite (point.y) && pcl_isfinite (point.z))
581 const float inv_counter = 1.0f /
static_cast<float> (counter);
582 center_x *= inv_counter;
583 center_y *= inv_counter;
584 center_z *= inv_counter;
593 detections_.push_back (detection);
601 removeOverlappingDetections ();
603 for (
size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
605 detections.push_back (detections_[detection_index]);
610 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
614 if (detection_id >= detections_.size ())
615 PCL_ERROR (
"ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
617 const size_t template_id = detections_[detection_id].template_id;
621 const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
631 const float translation_x = detection_bounding_box.
x - template_bounding_box.
x;
632 const float translation_y = detection_bounding_box.
y - template_bounding_box.
y;
633 const float translation_z = detection_bounding_box.
z - template_bounding_box.
z;
640 const size_t nr_points = template_point_cloud.
size ();
644 for (
size_t point_index = 0; point_index < nr_points; ++point_index)
648 point.x += translation_x;
649 point.y += translation_y;
650 point.z += translation_z;
652 cloud.
points[point_index] = point;
657 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
660 const size_t nr_detections = detections_.size ();
661 for (
size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
666 const size_t start_x = detection.
region.
x;
667 const size_t start_y = detection.
region.
y;
668 const size_t end_x = start_x + detection.
region.
width;
672 float min_depth = std::numeric_limits<float>::max ();
673 float max_depth = -std::numeric_limits<float>::max ();
674 for (
size_t row_index = start_y; row_index < end_y; ++row_index)
676 for (
size_t col_index = start_x; col_index < end_x; ++col_index)
678 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
680 if (pcl_isfinite (point.z))
682 min_depth = std::min (min_depth, point.z);
683 max_depth = std::max (max_depth, point.z);
688 const size_t nr_bins = 1000;
689 const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1);
690 std::vector<size_t> depth_bins (nr_bins, 0);
691 for (
size_t row_index = start_y; row_index < end_y; ++row_index)
693 for (
size_t col_index = start_x; col_index < end_x; ++col_index)
695 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
697 if (pcl_isfinite (point.z))
699 const size_t bin_index =
static_cast<size_t> ((point.z - min_depth) / step_size);
700 ++depth_bins[bin_index];
705 std::vector<size_t> integral_depth_bins (nr_bins, 0);
707 integral_depth_bins[0] = depth_bins[0];
708 for (
size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
710 integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
713 const size_t bb_depth_range =
static_cast<size_t> (detection.
bounding_box.
depth / step_size);
715 size_t max_nr_points = 0;
716 size_t max_index = 0;
717 for (
size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index)
719 const size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
721 if (nr_points_in_range > max_nr_points)
723 max_nr_points = nr_points_in_range;
724 max_index = bin_index;
728 const float z =
static_cast<float> (max_index) * step_size + min_depth;
735 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
738 const size_t nr_detections = detections_.size ();
739 for (
size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
746 const size_t start_x = detection.
region.
x;
747 const size_t start_y = detection.
region.
y;
748 const size_t pc_width = point_cloud.
width;
749 const size_t pc_height = point_cloud.
height;
751 std::vector<std::pair<float, float> > depth_matches;
752 for (
size_t row_index = 0; row_index < pc_height; ++row_index)
754 for (
size_t col_index = 0; col_index < pc_width; ++col_index)
757 const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
759 if (!pcl_isfinite (point_template.z) || !pcl_isfinite (point_input.z))
762 depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
767 const size_t nr_matches = depth_matches.size ();
768 const size_t nr_iterations = 100;
769 const float inlier_threshold = 0.01f;
770 size_t best_nr_inliers = 0;
771 float best_z_translation = 0.0f;
772 for (
size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
774 const size_t rand_index = (rand () * nr_matches) / RAND_MAX;
776 const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
778 size_t nr_inliers = 0;
779 for (
size_t match_index = 0; match_index < nr_matches; ++match_index)
781 const float error = fabsf (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
783 if (error <= inlier_threshold)
789 if (nr_inliers > best_nr_inliers)
791 best_nr_inliers = nr_inliers;
792 best_z_translation = z_translation;
796 float average_depth = 0.0f;
797 size_t average_counter = 0;
798 for (
size_t match_index = 0; match_index < nr_matches; ++match_index)
800 const float error = fabsf (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
802 if (error <= inlier_threshold)
805 average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
809 average_depth /=
static_cast<float> (average_counter);
811 detection.
bounding_box.
z = bounding_boxes_[template_id].z + average_depth;
816 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
820 const size_t nr_detections = detections_.size ();
821 Eigen::MatrixXf overlaps (nr_detections, nr_detections);
822 for (
size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1)
824 for (
size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
826 const float bounding_box_volume = detections_[detection_index_1].bounding_box.width
827 * detections_[detection_index_1].bounding_box.height
828 * detections_[detection_index_1].bounding_box.depth;
830 if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
831 overlaps (detection_index_1, detection_index_2) = 0.0f;
833 overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
834 detections_[detection_index_1].bounding_box,
835 detections_[detection_index_2].bounding_box) / bounding_box_volume;
840 std::vector<int> detection_to_cluster_mapping (nr_detections, -1);
841 std::vector<std::vector<size_t> > clusters;
842 for (
size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
844 if (detection_to_cluster_mapping[detection_index] != -1)
847 std::vector<size_t> cluster;
848 const size_t cluster_id = clusters.size ();
850 cluster.push_back (detection_index);
851 detection_to_cluster_mapping[detection_index] =
static_cast<int> (cluster_id);
854 for (
size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
856 const size_t detection_index_1 = cluster[cluster_index];
858 for (
size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
860 if (detection_to_cluster_mapping[detection_index_2] != -1)
863 if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
866 cluster.push_back (detection_index_2);
867 detection_to_cluster_mapping[detection_index_2] =
static_cast<int> (cluster_id);
871 clusters.push_back (cluster);
875 std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
877 const size_t nr_clusters = clusters.size ();
878 for (
size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
880 std::vector<size_t> & cluster = clusters[cluster_id];
882 float average_center_x = 0.0f;
883 float average_center_y = 0.0f;
884 float average_center_z = 0.0f;
885 float weight_sum = 0.0f;
887 float best_response = 0.0f;
888 size_t best_detection_id = 0;
890 float average_region_x = 0.0f;
891 float average_region_y = 0.0f;
893 const size_t elements_in_cluster = cluster.size ();
894 for (
size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
896 const size_t detection_id = cluster[cluster_index];
898 const float weight = detections_[detection_id].response;
900 if (weight > best_response)
902 best_response = weight;
903 best_detection_id = detection_id;
906 const Detection & d = detections_[detection_id];
911 average_center_x += p_center_x * weight;
912 average_center_y += p_center_y * weight;
913 average_center_z += p_center_z * weight;
914 weight_sum += weight;
916 average_region_x += float (detections_[detection_id].region.x) * weight;
917 average_region_y += float (detections_[detection_id].region.y) * weight;
921 detection.
template_id = detections_[best_detection_id].template_id;
922 detection.
object_id = detections_[best_detection_id].object_id;
926 const float inv_weight_sum = 1.0f / weight_sum;
927 const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width;
928 const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height;
929 const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth;
931 detection.
bounding_box.
x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f;
932 detection.
bounding_box.
y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f;
933 detection.
bounding_box.
z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f;
938 detection.
region.
x = int (average_region_x * inv_weight_sum);
939 detection.
region.
y = int (average_region_y * inv_weight_sum);
940 detection.
region.
width = detections_[best_detection_id].region.width;
941 detection.
region.
height = detections_[best_detection_id].region.height;
943 clustered_detections.push_back (detection);
946 detections_ = clustered_detections;
950 template <
typename Po
intXYZT,
typename Po
intRGBT>
float
954 const float x1_min = box1.
x;
955 const float y1_min = box1.
y;
956 const float z1_min = box1.
z;
957 const float x1_max = box1.
x + box1.
width;
958 const float y1_max = box1.
y + box1.
height;
959 const float z1_max = box1.
z + box1.
depth;
961 const float x2_min = box2.
x;
962 const float y2_min = box2.
y;
963 const float z2_min = box2.
z;
964 const float x2_max = box2.
x + box2.
width;
965 const float y2_max = box2.
y + box2.
height;
966 const float z2_max = box2.
z + box2.
depth;
968 const float xi_min = std::max (x1_min, x2_min);
969 const float yi_min = std::max (y1_min, y2_min);
970 const float zi_min = std::max (z1_min, z2_min);
972 const float xi_max = std::min (x1_max, x2_max);
973 const float yi_max = std::min (y1_max, y2_max);
974 const float zi_max = std::min (z1_max, z2_max);
976 const float intersection_width = xi_max - xi_min;
977 const float intersection_height = yi_max - yi_min;
978 const float intersection_depth = zi_max - zi_min;
980 if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
983 const float intersection_volume = intersection_width * intersection_height * intersection_depth;
985 return (intersection_volume);
989 #endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_
void removeOverlappingDetections()
Checks for overlapping detections, removes them and keeps only the strongest one. ...
Represents a detection of a template using the LINEMOD approach.
void resize(size_t n)
Resize the cloud.
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
float depth
Depth of the bounding box.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Point Cloud Data (PCD) file format reader.
void deserialize(std::istream &stream)
Deserializes the object from the specified stream.
int createAndAddTemplate(pcl::PointCloud< pcl::PointXYZRGBA > &cloud, const size_t object_id, const MaskMap &mask_xyz, const MaskMap &mask_rgb, const RegionXY ®ion)
Creates a template from the specified data and adds it to the matching queue.
size_t template_id
The ID of the template.
Defines a region in XY-space.
float y
Y-coordinate of the upper left front point.
unsigned int getFileSize()
bool loadTemplates(const std::string &file_name, size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
A TAR file's header, as described on http://en.wikipedia.org/wiki/Tar_%28file_format%29.
uint32_t width
The point cloud width (if organized as an image-structure).
size_t detection_id
The ID of this detection.
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 refineDetectionsAlongDepth()
Refines the found detections along the depth.
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.
float x
X-coordinate of the upper left front point.
int x
x-position of the detection.
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
float width
Width of the bounding box.
int height
height of the region.
int template_id
ID of the detected template.
float z
Z-coordinate of the upper left front point.
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, size_t object_id=0)
size_t object_id
The ID of the object corresponding to the template.
int y
y-position of the detection.
uint32_t height
The point cloud height (if organized as an image-structure).
int width
width of the region.
High-level class for template matching using the LINEMOD approach based on RGB and Depth data...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
RegionXY region
The region assigned to the template.
float height
Height of the bounding box.
RegionXY region
The 2D template region of the detection.
A multi-modality template constructed from a set of quantized multi-modality features.
void applyProjectiveDepthICPOnDetections()
Applies projective ICP on detections to find their correct position in depth.
int x
x-position of the region.
int y
y-position of the region.
float score
score of the detection.
float response
The response of this detection.
void computeTransformedTemplatePoints(const size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
float scale
scale at which the template was detected.
int read(const std::string &file_name, pcl::PCLPointCloud2 &cloud, Eigen::Vector4f &origin, Eigen::Quaternionf &orientation, int &pcd_version, const int offset=0)
Read a point cloud data from a PCD file and store it into a pcl/PCLPointCloud2.