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.
file_name[0]), 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 (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
174 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
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);
191 center_x /=
static_cast<float> (counter);
192 center_y /=
static_cast<float> (counter);
193 center_z /=
static_cast<float> (counter);
195 bb.
width = max_x - min_x;
196 bb.
height = max_y - min_y;
197 bb.
depth = max_z - min_z;
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;
203 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
207 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
214 template_point_cloud.
points[j] = p;
222 template <
typename Po
intXYZT,
typename Po
intRGBT>
int
225 const std::size_t object_id,
231 template_point_clouds_.resize (template_point_clouds_.size () + 1);
235 object_ids_.push_back (object_id);
238 bounding_boxes_.resize (template_point_clouds_.size ());
240 const std::size_t i = template_point_clouds_.size () - 1;
244 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
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)
261 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
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);
278 center_x /=
static_cast<float> (counter);
279 center_y /=
static_cast<float> (counter);
280 center_z /=
static_cast<float> (counter);
282 bb.
width = max_x - min_x;
283 bb.
height = max_y - min_y;
284 bb.
depth = max_z - min_z;
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;
290 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
294 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
301 template_point_cloud.
points[j] = p;
305 std::vector<pcl::QuantizableModality*> modalities;
306 modalities.push_back (&color_gradient_mod_);
307 modalities.push_back (&surface_normal_mod_);
309 std::vector<MaskMap*> masks;
310 masks.push_back (const_cast<MaskMap*> (&mask_rgb));
311 masks.push_back (const_cast<MaskMap*> (&mask_xyz));
313 return (linemod_.createAndAddTemplate (modalities, masks, region));
317 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
321 template_point_clouds_.resize (template_point_clouds_.size () + 1);
325 linemod_.addTemplate (sqmmt);
326 object_ids_.push_back (object_id);
329 bounding_boxes_.resize (template_point_clouds_.size ());
331 const std::size_t i = template_point_clouds_.size () - 1;
335 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
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)
352 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
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);
369 center_x /=
static_cast<float> (counter);
370 center_y /=
static_cast<float> (counter);
371 center_z /=
static_cast<float> (counter);
373 bb.
width = max_x - min_x;
374 bb.
height = max_y - min_y;
375 bb.
depth = max_z - min_z;
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;
381 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
385 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
392 template_point_cloud.
points[j] = p;
400 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
404 std::vector<pcl::QuantizableModality*> modalities;
405 modalities.push_back (&color_gradient_mod_);
406 modalities.push_back (&surface_normal_mod_);
408 std::vector<pcl::LINEMODDetection> linemod_detections;
409 linemod_.detectTemplates (modalities, linemod_detections);
411 detections_.clear ();
412 detections_.reserve (linemod_detections.size ());
414 detections.reserve (linemod_detections.size ());
415 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
431 linemod_.getTemplate (linemod_detection.
template_id);
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));
440 detection.
region.
x = linemod_detection.
x;
441 detection.
region.
y = linemod_detection.
y;
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)
456 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
458 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
460 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
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;
481 detections_.push_back (detection);
485 refineDetectionsAlongDepth ();
489 removeOverlappingDetections ();
491 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
493 detections.push_back (detections_[detection_index]);
498 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
501 const float min_scale,
502 const float max_scale,
503 const float scale_multiplier)
505 std::vector<pcl::QuantizableModality*> modalities;
506 modalities.push_back (&color_gradient_mod_);
507 modalities.push_back (&surface_normal_mod_);
509 std::vector<pcl::LINEMODDetection> linemod_detections;
510 linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
512 detections_.clear ();
513 detections_.reserve (linemod_detections.size ());
515 detections.reserve (linemod_detections.size ());
516 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
532 linemod_.getTemplate (linemod_detection.
template_id);
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));
541 detection.
region.
x = linemod_detection.
x;
542 detection.
region.
y = linemod_detection.
y;
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)
557 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
559 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
561 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
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;
582 detections_.push_back (detection);
590 removeOverlappingDetections ();
592 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
594 detections.push_back (detections_[detection_index]);
599 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
603 if (detection_id >= detections_.size ())
604 PCL_ERROR (
"ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
606 const std::size_t template_id = detections_[detection_id].template_id;
610 const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
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;
629 const std::size_t nr_points = template_point_cloud.
size ();
633 for (std::size_t point_index = 0; point_index < nr_points; ++point_index)
637 point.x += translation_x;
638 point.y += translation_y;
639 point.z += translation_z;
641 cloud.
points[point_index] = point;
646 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
649 const std::size_t nr_detections = detections_.size ();
650 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
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));
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)
667 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
669 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
671 if (std::isfinite (point.z))
673 min_depth = std::min (min_depth, point.z);
674 max_depth = std::max (max_depth, point.z);
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)
684 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
686 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
688 if (std::isfinite (point.z))
690 const std::size_t bin_index =
static_cast<std::size_t
> ((point.z - min_depth) / step_size);
691 ++depth_bins[bin_index];
696 std::vector<std::size_t> integral_depth_bins (nr_bins, 0);
698 integral_depth_bins[0] = depth_bins[0];
699 for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
701 integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
704 const std::size_t bb_depth_range =
static_cast<std::size_t
> (detection.
bounding_box.
depth / step_size);
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)
710 const std::size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
712 if (nr_points_in_range > max_nr_points)
714 max_nr_points = nr_points_in_range;
715 max_index = bin_index;
719 const float z =
static_cast<float> (max_index) * step_size + min_depth;
726 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
729 const std::size_t nr_detections = detections_.size ();
730 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
734 const std::size_t template_id = detection.
template_id;
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;
742 std::vector<std::pair<float, float> > depth_matches;
743 for (std::size_t row_index = 0; row_index < pc_height; ++row_index)
745 for (std::size_t col_index = 0; col_index < pc_width; ++col_index)
748 const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
750 if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z))
753 depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
758 const std::size_t nr_matches = depth_matches.size ();
759 const std::size_t nr_iterations = 100;
760 const float inlier_threshold = 0.01f;
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)
765 const std::size_t rand_index = (rand () * nr_matches) / RAND_MAX;
767 const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
769 std::size_t nr_inliers = 0;
770 for (std::size_t match_index = 0; match_index < nr_matches; ++match_index)
772 const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
774 if (error <= inlier_threshold)
780 if (nr_inliers > best_nr_inliers)
782 best_nr_inliers = nr_inliers;
783 best_z_translation = z_translation;
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)
791 const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
793 if (error <= inlier_threshold)
796 average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
800 average_depth /=
static_cast<float> (average_counter);
802 detection.
bounding_box.
z = bounding_boxes_[template_id].z + average_depth;
807 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
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)
815 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
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;
821 if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
822 overlaps (detection_index_1, detection_index_2) = 0.0f;
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;
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)
835 if (detection_to_cluster_mapping[detection_index] != -1)
838 std::vector<std::size_t> cluster;
839 const std::size_t cluster_id = clusters.size ();
841 cluster.push_back (detection_index);
842 detection_to_cluster_mapping[detection_index] =
static_cast<int> (cluster_id);
845 for (std::size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
847 const std::size_t detection_index_1 = cluster[cluster_index];
849 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
851 if (detection_to_cluster_mapping[detection_index_2] != -1)
854 if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
857 cluster.push_back (detection_index_2);
858 detection_to_cluster_mapping[detection_index_2] =
static_cast<int> (cluster_id);
862 clusters.push_back (cluster);
866 std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
868 const std::size_t nr_clusters = clusters.size ();
869 for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
871 std::vector<std::size_t> & cluster = clusters[cluster_id];
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;
878 float best_response = 0.0f;
879 std::size_t best_detection_id = 0;
881 float average_region_x = 0.0f;
882 float average_region_y = 0.0f;
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)
887 const std::size_t detection_id = cluster[cluster_index];
889 const float weight = detections_[detection_id].response;
891 if (weight > best_response)
893 best_response = weight;
894 best_detection_id = detection_id;
897 const Detection & d = detections_[detection_id];
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;
907 average_region_x += float (detections_[detection_id].region.x) * weight;
908 average_region_y += float (detections_[detection_id].region.y) * weight;
912 detection.
template_id = detections_[best_detection_id].template_id;
913 detection.
object_id = detections_[best_detection_id].object_id;
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;
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;
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;
934 clustered_detections.push_back (detection);
937 detections_ = clustered_detections;
941 template <
typename Po
intXYZT,
typename Po
intRGBT>
float
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;
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;
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);
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);
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;
971 if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
974 const float intersection_volume = intersection_width * intersection_height * intersection_depth;
976 return (intersection_volume);
981 #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)
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.
void resize(std::size_t n)
Resize the cloud.
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.
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.