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 applyProjectiveDepthICPOnDetections()
Applies projective ICP on detections to find their correct position in depth.
float z
Z-coordinate of the upper left front point.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
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 score
score of the detection.
void removeOverlappingDetections()
Checks for overlapping detections, removes them and keeps only the strongest one. ...
int template_id
ID of the detected template.
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.
Represents a detection of a template using the LINEMOD approach.
Defines a region in XY-space.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
float y
Y-coordinate of the upper left front point.
int y
y-position of the detection.
A multi-modality template constructed from a set of quantized multi-modality features.
uint32_t height
The point cloud height (if organized as an image-structure).
int x
x-position of the region.
void detect(std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > &detections)
Applies the detection process and fills the supplied vector with the detection instances.
float x
X-coordinate of the upper left front point.
int y
y-position of the region.
uint32_t width
The point cloud width (if organized as an image-structure).
bool loadTemplates(const std::string &file_name, size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
float width
Width of the bounding box.
float scale
scale at which the template was detected.
size_t detection_id
The ID of this detection.
RegionXY region
The 2D template region of the detection.
int height
height of the region.
float depth
Depth of the bounding box.
void deserialize(std::istream &stream)
Deserializes the object from the specified stream.
RegionXY region
The region assigned to the template.
size_t object_id
The ID of the object corresponding to the template.
void refineDetectionsAlongDepth()
Refines the found detections along the depth.
void computeTransformedTemplatePoints(const size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
Point Cloud Data (PCD) file format reader.
void resize(size_t n)
Resize the cloud.
High-level class for template matching using the LINEMOD approach based on RGB and Depth data...
int x
x-position of the detection.
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.
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
int width
width of the region.
size_t template_id
The ID of the template.
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, size_t object_id=0)
float response
The response of this detection.
float height
Height of the bounding box.
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
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.