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>
51 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
55 int result =
static_cast<int> (
io::raw_read (fd, reinterpret_cast<char*> (&header), 512));
66 if (std::string (header.
ustar).substr (0, 5) !=
"ustar")
76 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
89 std::string pcd_ext (
".pcd");
90 std::string sqmmt_ext (
".sqmmt");
93 while (readLTMHeader (ltm_fd, ltm_header))
98 std::string chunk_name (ltm_header.
file_name);
100 std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower);
101 std::string::size_type it;
103 if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
104 (pcd_ext.size () - (chunk_name.size () - it)) == 0)
106 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
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);
114 else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
115 (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
117 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
120 char *buffer =
new char[fsize];
121 int result =
static_cast<int> (
io::raw_read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
125 PCL_ERROR (
"[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
130 std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
131 stream.write (buffer, fsize);
135 linemod_.addTemplate (sqmmt);
136 object_ids_.push_back (object_id);
152 bounding_boxes_.resize (template_point_clouds_.size ());
153 for (std::size_t i = 0; i < template_point_clouds_.size (); ++i)
157 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
160 float center_x = 0.0f;
161 float center_y = 0.0f;
162 float center_z = 0.0f;
163 float min_x = std::numeric_limits<float>::max ();
164 float min_y = std::numeric_limits<float>::max ();
165 float min_z = std::numeric_limits<float>::max ();
166 float max_x = -std::numeric_limits<float>::max ();
167 float max_y = -std::numeric_limits<float>::max ();
168 float max_z = -std::numeric_limits<float>::max ();
169 std::size_t counter = 0;
170 for (
const auto & p : template_point_cloud)
172 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
175 min_x = std::min (min_x, p.x);
176 min_y = std::min (min_y, p.y);
177 min_z = std::min (min_z, p.z);
178 max_x = std::max (max_x, p.x);
179 max_y = std::max (max_y, p.y);
180 max_z = std::max (max_z, p.z);
189 center_x /=
static_cast<float> (counter);
190 center_y /=
static_cast<float> (counter);
191 center_z /=
static_cast<float> (counter);
193 bb.
width = max_x - min_x;
194 bb.
height = max_y - min_y;
195 bb.
depth = max_z - min_z;
197 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
198 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
199 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
201 for (
auto & j : template_point_cloud)
205 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
220 template <
typename Po
intXYZT,
typename Po
intRGBT>
int
223 const std::size_t object_id,
229 template_point_clouds_.resize (template_point_clouds_.size () + 1);
233 object_ids_.push_back (object_id);
236 bounding_boxes_.resize (template_point_clouds_.size ());
238 const std::size_t i = template_point_clouds_.size () - 1;
242 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
245 float center_x = 0.0f;
246 float center_y = 0.0f;
247 float center_z = 0.0f;
248 float min_x = std::numeric_limits<float>::max ();
249 float min_y = std::numeric_limits<float>::max ();
250 float min_z = std::numeric_limits<float>::max ();
251 float max_x = -std::numeric_limits<float>::max ();
252 float max_y = -std::numeric_limits<float>::max ();
253 float max_z = -std::numeric_limits<float>::max ();
254 std::size_t counter = 0;
255 for (
const auto & p : template_point_cloud)
257 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
260 min_x = std::min (min_x, p.x);
261 min_y = std::min (min_y, p.y);
262 min_z = std::min (min_z, p.z);
263 max_x = std::max (max_x, p.x);
264 max_y = std::max (max_y, p.y);
265 max_z = std::max (max_z, p.z);
274 center_x /=
static_cast<float> (counter);
275 center_y /=
static_cast<float> (counter);
276 center_z /=
static_cast<float> (counter);
278 bb.
width = max_x - min_x;
279 bb.
height = max_y - min_y;
280 bb.
depth = max_z - min_z;
282 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
283 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
284 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
286 for (
auto & j : template_point_cloud)
290 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
301 std::vector<pcl::QuantizableModality*> modalities;
302 modalities.push_back (&color_gradient_mod_);
303 modalities.push_back (&surface_normal_mod_);
305 std::vector<MaskMap*> masks;
306 masks.push_back (const_cast<MaskMap*> (&mask_rgb));
307 masks.push_back (const_cast<MaskMap*> (&mask_xyz));
309 return (linemod_.createAndAddTemplate (modalities, masks, region));
313 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
317 template_point_clouds_.resize (template_point_clouds_.size () + 1);
321 linemod_.addTemplate (sqmmt);
322 object_ids_.push_back (object_id);
325 bounding_boxes_.resize (template_point_clouds_.size ());
327 const std::size_t i = template_point_clouds_.size () - 1;
331 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
334 float center_x = 0.0f;
335 float center_y = 0.0f;
336 float center_z = 0.0f;
337 float min_x = std::numeric_limits<float>::max ();
338 float min_y = std::numeric_limits<float>::max ();
339 float min_z = std::numeric_limits<float>::max ();
340 float max_x = -std::numeric_limits<float>::max ();
341 float max_y = -std::numeric_limits<float>::max ();
342 float max_z = -std::numeric_limits<float>::max ();
343 std::size_t counter = 0;
344 for (
const auto & p : template_point_cloud)
346 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
349 min_x = std::min (min_x, p.x);
350 min_y = std::min (min_y, p.y);
351 min_z = std::min (min_z, p.z);
352 max_x = std::max (max_x, p.x);
353 max_y = std::max (max_y, p.y);
354 max_z = std::max (max_z, p.z);
363 center_x /=
static_cast<float> (counter);
364 center_y /=
static_cast<float> (counter);
365 center_z /=
static_cast<float> (counter);
367 bb.
width = max_x - min_x;
368 bb.
height = max_y - min_y;
369 bb.
depth = max_z - min_z;
371 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
372 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
373 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
375 for (
auto & j : template_point_cloud)
379 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
394 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
398 std::vector<pcl::QuantizableModality*> modalities;
399 modalities.push_back (&color_gradient_mod_);
400 modalities.push_back (&surface_normal_mod_);
402 std::vector<pcl::LINEMODDetection> linemod_detections;
403 linemod_.detectTemplates (modalities, linemod_detections);
405 detections_.clear ();
406 detections_.reserve (linemod_detections.size ());
408 detections.reserve (linemod_detections.size ());
409 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
425 linemod_.getTemplate (linemod_detection.
template_id);
427 const std::size_t start_x = std::max (linemod_detection.
x, 0);
428 const std::size_t start_y = std::max (linemod_detection.
y, 0);
429 const std::size_t end_x = std::min (static_cast<std::size_t> (start_x + linemod_template.
region.
width),
430 static_cast<std::size_t> (cloud_xyz_->width));
431 const std::size_t end_y = std::min (static_cast<std::size_t> (start_y + linemod_template.
region.
height),
432 static_cast<std::size_t> (cloud_xyz_->height));
434 detection.
region.
x = linemod_detection.
x;
435 detection.
region.
y = linemod_detection.
y;
444 float center_x = 0.0f;
445 float center_y = 0.0f;
446 float center_z = 0.0f;
447 std::size_t counter = 0;
448 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
450 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
452 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
454 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
463 const float inv_counter = 1.0f /
static_cast<float> (counter);
464 center_x *= inv_counter;
465 center_y *= inv_counter;
466 center_z *= inv_counter;
475 detections_.push_back (detection);
479 refineDetectionsAlongDepth ();
483 removeOverlappingDetections ();
485 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
487 detections.push_back (detections_[detection_index]);
492 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
495 const float min_scale,
496 const float max_scale,
497 const float scale_multiplier)
499 std::vector<pcl::QuantizableModality*> modalities;
500 modalities.push_back (&color_gradient_mod_);
501 modalities.push_back (&surface_normal_mod_);
503 std::vector<pcl::LINEMODDetection> linemod_detections;
504 linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
506 detections_.clear ();
507 detections_.reserve (linemod_detections.size ());
509 detections.reserve (linemod_detections.size ());
510 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
526 linemod_.getTemplate (linemod_detection.
template_id);
528 const std::size_t start_x = std::max (linemod_detection.
x, 0);
529 const std::size_t start_y = std::max (linemod_detection.
y, 0);
530 const std::size_t end_x = std::min (static_cast<std::size_t> (start_x + linemod_template.
region.
width * linemod_detection.
scale),
531 static_cast<std::size_t> (cloud_xyz_->width));
532 const std::size_t end_y = std::min (static_cast<std::size_t> (start_y + linemod_template.
region.
height * linemod_detection.
scale),
533 static_cast<std::size_t> (cloud_xyz_->height));
535 detection.
region.
x = linemod_detection.
x;
536 detection.
region.
y = linemod_detection.
y;
545 float center_x = 0.0f;
546 float center_y = 0.0f;
547 float center_z = 0.0f;
548 std::size_t counter = 0;
549 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
551 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
553 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
555 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
564 const float inv_counter = 1.0f /
static_cast<float> (counter);
565 center_x *= inv_counter;
566 center_y *= inv_counter;
567 center_z *= inv_counter;
576 detections_.push_back (detection);
584 removeOverlappingDetections ();
586 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
588 detections.push_back (detections_[detection_index]);
593 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
597 if (detection_id >= detections_.size ())
598 PCL_ERROR (
"ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
600 const std::size_t template_id = detections_[detection_id].template_id;
604 const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
614 const float translation_x = detection_bounding_box.
x - template_bounding_box.
x;
615 const float translation_y = detection_bounding_box.
y - template_bounding_box.
y;
616 const float translation_z = detection_bounding_box.
z - template_bounding_box.
z;
623 const std::size_t nr_points = template_point_cloud.
size ();
627 for (std::size_t point_index = 0; point_index < nr_points; ++point_index)
631 point.x += translation_x;
632 point.y += translation_y;
633 point.z += translation_z;
635 cloud[point_index] = point;
640 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
643 const std::size_t nr_detections = detections_.size ();
644 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
649 const std::size_t start_x = std::max (detection.
region.
x, 0);
650 const std::size_t start_y = std::max (detection.
region.
y, 0);
651 const std::size_t end_x = std::min (static_cast<std::size_t> (detection.
region.
x + detection.
region.
width),
652 static_cast<std::size_t> (cloud_xyz_->width));
653 const std::size_t end_y = std::min (static_cast<std::size_t> (detection.
region.
y + detection.
region.
height),
654 static_cast<std::size_t> (cloud_xyz_->height));
657 float min_depth = std::numeric_limits<float>::max ();
658 float max_depth = -std::numeric_limits<float>::max ();
659 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
661 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
663 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
665 if (std::isfinite (point.z))
667 min_depth = std::min (min_depth, point.z);
668 max_depth = std::max (max_depth, point.z);
673 constexpr std::size_t nr_bins = 1000;
674 const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1);
675 std::vector<std::size_t> depth_bins (nr_bins, 0);
676 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
678 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
680 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
682 if (std::isfinite (point.z))
684 const auto bin_index =
static_cast<std::size_t
> ((point.z - min_depth) / step_size);
685 ++depth_bins[bin_index];
690 std::vector<std::size_t> integral_depth_bins (nr_bins, 0);
692 integral_depth_bins[0] = depth_bins[0];
693 for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
695 integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
698 const auto bb_depth_range =
static_cast<std::size_t
> (detection.
bounding_box.
depth / step_size);
700 std::size_t max_nr_points = 0;
701 std::size_t max_index = 0;
702 for (std::size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index)
704 const std::size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
706 if (nr_points_in_range > max_nr_points)
708 max_nr_points = nr_points_in_range;
709 max_index = bin_index;
713 const float z =
static_cast<float> (max_index) * step_size + min_depth;
720 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
723 const std::size_t nr_detections = detections_.size ();
724 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
728 const std::size_t template_id = detection.
template_id;
731 const std::size_t start_x = detection.
region.
x;
732 const std::size_t start_y = detection.
region.
y;
733 const std::size_t pc_width = point_cloud.
width;
734 const std::size_t pc_height = point_cloud.
height;
736 std::vector<std::pair<float, float> > depth_matches;
737 for (std::size_t row_index = 0; row_index < pc_height; ++row_index)
739 for (std::size_t col_index = 0; col_index < pc_width; ++col_index)
742 const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
744 if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z))
747 depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
752 const std::size_t nr_matches = depth_matches.size ();
753 const std::size_t nr_iterations = 100;
754 const float inlier_threshold = 0.01f;
755 std::size_t best_nr_inliers = 0;
756 float best_z_translation = 0.0f;
757 for (std::size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
759 const std::size_t rand_index = (rand () * nr_matches) / RAND_MAX;
761 const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
763 std::size_t nr_inliers = 0;
764 for (std::size_t match_index = 0; match_index < nr_matches; ++match_index)
766 const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
768 if (error <= inlier_threshold)
774 if (nr_inliers > best_nr_inliers)
776 best_nr_inliers = nr_inliers;
777 best_z_translation = z_translation;
781 float average_depth = 0.0f;
782 std::size_t average_counter = 0;
783 for (std::size_t match_index = 0; match_index < nr_matches; ++match_index)
785 const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
787 if (error <= inlier_threshold)
790 average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
794 average_depth /=
static_cast<float> (average_counter);
796 detection.
bounding_box.
z = bounding_boxes_[template_id].z + average_depth;
801 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
805 const std::size_t nr_detections = detections_.size ();
806 Eigen::MatrixXf overlaps (nr_detections, nr_detections);
807 for (std::size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1)
809 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
811 const float bounding_box_volume = detections_[detection_index_1].bounding_box.width
812 * detections_[detection_index_1].bounding_box.height
813 * detections_[detection_index_1].bounding_box.depth;
815 if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
816 overlaps (detection_index_1, detection_index_2) = 0.0f;
818 overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
819 detections_[detection_index_1].bounding_box,
820 detections_[detection_index_2].bounding_box) / bounding_box_volume;
825 std::vector<int> detection_to_cluster_mapping (nr_detections, -1);
826 std::vector<std::vector<std::size_t> > clusters;
827 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
829 if (detection_to_cluster_mapping[detection_index] != -1)
832 std::vector<std::size_t> cluster;
833 const std::size_t cluster_id = clusters.size ();
835 cluster.push_back (detection_index);
836 detection_to_cluster_mapping[detection_index] =
static_cast<int> (cluster_id);
839 for (std::size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
841 const std::size_t detection_index_1 = cluster[cluster_index];
843 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
845 if (detection_to_cluster_mapping[detection_index_2] != -1)
848 if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
851 cluster.push_back (detection_index_2);
852 detection_to_cluster_mapping[detection_index_2] =
static_cast<int> (cluster_id);
856 clusters.push_back (cluster);
860 std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
862 const std::size_t nr_clusters = clusters.size ();
863 for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
865 std::vector<std::size_t> & cluster = clusters[cluster_id];
867 float average_center_x = 0.0f;
868 float average_center_y = 0.0f;
869 float average_center_z = 0.0f;
870 float weight_sum = 0.0f;
872 float best_response = 0.0f;
873 std::size_t best_detection_id = 0;
875 float average_region_x = 0.0f;
876 float average_region_y = 0.0f;
878 const std::size_t elements_in_cluster = cluster.size ();
879 for (std::size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
881 const std::size_t detection_id = cluster[cluster_index];
883 const float weight = detections_[detection_id].response;
885 if (weight > best_response)
887 best_response = weight;
888 best_detection_id = detection_id;
891 const Detection & d = detections_[detection_id];
896 average_center_x += p_center_x * weight;
897 average_center_y += p_center_y * weight;
898 average_center_z += p_center_z * weight;
899 weight_sum += weight;
901 average_region_x +=
static_cast<float>(detections_[detection_id].region.x) * weight;
902 average_region_y +=
static_cast<float>(detections_[detection_id].region.y) * weight;
906 detection.
template_id = detections_[best_detection_id].template_id;
907 detection.
object_id = detections_[best_detection_id].object_id;
911 const float inv_weight_sum = 1.0f / weight_sum;
912 const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width;
913 const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height;
914 const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth;
916 detection.
bounding_box.
x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f;
917 detection.
bounding_box.
y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f;
918 detection.
bounding_box.
z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f;
923 detection.
region.
x =
static_cast<int>(average_region_x * inv_weight_sum);
924 detection.
region.
y =
static_cast<int>(average_region_y * inv_weight_sum);
925 detection.
region.
width = detections_[best_detection_id].region.width;
926 detection.
region.
height = detections_[best_detection_id].region.height;
928 clustered_detections.push_back (detection);
931 detections_ = clustered_detections;
935 template <
typename Po
intXYZT,
typename Po
intRGBT>
float
939 const float x1_min = box1.
x;
940 const float y1_min = box1.
y;
941 const float z1_min = box1.
z;
942 const float x1_max = box1.
x + box1.
width;
943 const float y1_max = box1.
y + box1.
height;
944 const float z1_max = box1.
z + box1.
depth;
946 const float x2_min = box2.
x;
947 const float y2_min = box2.
y;
948 const float z2_min = box2.
z;
949 const float x2_max = box2.
x + box2.
width;
950 const float y2_max = box2.
y + box2.
height;
951 const float z2_max = box2.
z + box2.
depth;
953 const float xi_min = std::max (x1_min, x2_min);
954 const float yi_min = std::max (y1_min, y2_min);
955 const float zi_min = std::max (z1_min, z2_min);
957 const float xi_max = std::min (x1_max, x2_max);
958 const float yi_max = std::min (y1_max, y2_max);
959 const float zi_max = std::min (z1_max, z2_max);
961 const float intersection_width = xi_max - xi_min;
962 const float intersection_height = yi_max - yi_min;
963 const float intersection_depth = zi_max - zi_min;
965 if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
968 const float intersection_volume = intersection_width * intersection_height * intersection_depth;
970 return (intersection_volume);
975 #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.
int raw_lseek(int fd, long offset, int whence)
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. ...
std::size_t template_id
The ID of the template.
int raw_open(const char *pathname, int flags, int mode)
int raw_read(int fd, void *buffer, std::size_t count)
std::size_t object_id
The ID of the object corresponding to the template.
int template_id
ID of the detected template.
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.
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.
std::uint32_t width
The point cloud width (if organized as an image-structure).
int y
y-position of the region.
void computeTransformedTemplatePoints(const std::size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
float width
Width of the bounding box.
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.
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, std::size_t object_id=0)
float scale
scale at which the template was detected.
RegionXY region
The 2D template region of the detection.
int height
height of the region.
float depth
Depth of the bounding box.
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.
void deserialize(std::istream &stream)
Deserializes the object from the specified stream.
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t height
The point cloud height (if organized as an image-structure).
RegionXY region
The region assigned to the template.
void refineDetectionsAlongDepth()
Refines the found detections along the depth.
int createAndAddTemplate(pcl::PointCloud< pcl::PointXYZRGBA > &cloud, const std::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.
Point Cloud Data (PCD) file format reader.
int x
x-position of the detection.
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
std::size_t detection_id
The ID of this detection.
bool loadTemplates(const std::string &file_name, std::size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
int width
width of the region.
float response
The response of this detection.
float height
Height of the bounding box.
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.