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>
48 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
52 int result = static_cast<int> (::read (fd, reinterpret_cast<char*> (&header.
file_name[0]), 512));
63 if (std::string (header.
ustar).substr (0, 5) !=
"ustar")
73 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
77 int ltm_fd = io::raw_open(file_name.c_str (), O_RDONLY);
86 std::string pcd_ext (
".pcd");
87 std::string sqmmt_ext (
".sqmmt");
90 while (readLTMHeader (ltm_fd, ltm_header))
95 std::string chunk_name (ltm_header.
file_name);
97 std::transform (chunk_name.begin (), chunk_name.end (), chunk_name.begin (), ::tolower);
98 std::string::size_type it;
100 if ((it = chunk_name.find (pcd_ext)) != std::string::npos &&
101 (pcd_ext.size () - (chunk_name.size () - it)) == 0)
103 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a PCD file.\n", chunk_name.c_str ());
105 template_point_clouds_.resize (template_point_clouds_.size () + 1);
106 pcd_reader.
read (file_name, template_point_clouds_[template_point_clouds_.size () - 1], ltm_offset);
111 else if ((it = chunk_name.find (sqmmt_ext)) != std::string::npos &&
112 (sqmmt_ext.size () - (chunk_name.size () - it)) == 0)
114 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n", chunk_name.c_str ());
117 char *buffer =
new char[fsize];
118 int result = static_cast<int> (::
read (ltm_fd, reinterpret_cast<char*> (&buffer[0]), fsize));
122 PCL_ERROR (
"[pcl::LineRGBD::loadTemplates] Error reading SQMMT template from file!\n");
127 std::stringstream stream (std::stringstream::in | std::stringstream::out | std::stringstream::binary);
128 stream.write (buffer, fsize);
132 linemod_.addTemplate (sqmmt);
133 object_ids_.push_back (object_id);
141 if (io::raw_lseek(ltm_fd, ltm_offset, SEEK_SET) < 0)
146 io::raw_close(ltm_fd);
149 bounding_boxes_.resize (template_point_clouds_.size ());
150 for (std::size_t i = 0; i < template_point_clouds_.size (); ++i)
154 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
157 float center_x = 0.0f;
158 float center_y = 0.0f;
159 float center_z = 0.0f;
160 float min_x = std::numeric_limits<float>::max ();
161 float min_y = std::numeric_limits<float>::max ();
162 float min_z = std::numeric_limits<float>::max ();
163 float max_x = -std::numeric_limits<float>::max ();
164 float max_y = -std::numeric_limits<float>::max ();
165 float max_z = -std::numeric_limits<float>::max ();
166 std::size_t counter = 0;
167 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
171 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
174 min_x = std::min (min_x, p.x);
175 min_y = std::min (min_y, p.y);
176 min_z = std::min (min_z, p.z);
177 max_x = std::max (max_x, p.x);
178 max_y = std::max (max_y, p.y);
179 max_z = std::max (max_z, p.z);
188 center_x /= static_cast<float> (counter);
189 center_y /= static_cast<float> (counter);
190 center_z /= static_cast<float> (counter);
192 bb.
width = max_x - min_x;
193 bb.
height = max_y - min_y;
194 bb.
depth = max_z - min_z;
196 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
197 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
198 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
200 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
204 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
211 template_point_cloud.
points[j] = p;
219 template <
typename Po
intXYZT,
typename Po
intRGBT>
int
222 const std::size_t object_id,
228 template_point_clouds_.resize (template_point_clouds_.size () + 1);
232 object_ids_.push_back (object_id);
235 bounding_boxes_.resize (template_point_clouds_.size ());
237 const std::size_t i = template_point_clouds_.size () - 1;
241 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
244 float center_x = 0.0f;
245 float center_y = 0.0f;
246 float center_z = 0.0f;
247 float min_x = std::numeric_limits<float>::max ();
248 float min_y = std::numeric_limits<float>::max ();
249 float min_z = std::numeric_limits<float>::max ();
250 float max_x = -std::numeric_limits<float>::max ();
251 float max_y = -std::numeric_limits<float>::max ();
252 float max_z = -std::numeric_limits<float>::max ();
253 std::size_t counter = 0;
254 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
258 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
261 min_x = std::min (min_x, p.x);
262 min_y = std::min (min_y, p.y);
263 min_z = std::min (min_z, p.z);
264 max_x = std::max (max_x, p.x);
265 max_y = std::max (max_y, p.y);
266 max_z = std::max (max_z, p.z);
275 center_x /= static_cast<float> (counter);
276 center_y /= static_cast<float> (counter);
277 center_z /= static_cast<float> (counter);
279 bb.
width = max_x - min_x;
280 bb.
height = max_y - min_y;
281 bb.
depth = max_z - min_z;
283 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
284 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
285 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
287 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
291 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
298 template_point_cloud.
points[j] = p;
302 std::vector<pcl::QuantizableModality*> modalities;
303 modalities.push_back (&color_gradient_mod_);
304 modalities.push_back (&surface_normal_mod_);
306 std::vector<MaskMap*> masks;
307 masks.push_back (const_cast<MaskMap*> (&mask_rgb));
308 masks.push_back (const_cast<MaskMap*> (&mask_xyz));
310 return (linemod_.createAndAddTemplate (modalities, masks, region));
315 template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
319 template_point_clouds_.resize (template_point_clouds_.size () + 1);
323 linemod_.addTemplate (sqmmt);
324 object_ids_.push_back (object_id);
327 bounding_boxes_.resize (template_point_clouds_.size ());
329 const std::size_t i = template_point_clouds_.size () - 1;
333 bb.
x = bb.
y = bb.
z = std::numeric_limits<float>::max ();
336 float center_x = 0.0f;
337 float center_y = 0.0f;
338 float center_z = 0.0f;
339 float min_x = std::numeric_limits<float>::max ();
340 float min_y = std::numeric_limits<float>::max ();
341 float min_z = std::numeric_limits<float>::max ();
342 float max_x = -std::numeric_limits<float>::max ();
343 float max_y = -std::numeric_limits<float>::max ();
344 float max_z = -std::numeric_limits<float>::max ();
345 std::size_t counter = 0;
346 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
350 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
353 min_x = std::min (min_x, p.x);
354 min_y = std::min (min_y, p.y);
355 min_z = std::min (min_z, p.z);
356 max_x = std::max (max_x, p.x);
357 max_y = std::max (max_y, p.y);
358 max_z = std::max (max_z, p.z);
367 center_x /= static_cast<float> (counter);
368 center_y /= static_cast<float> (counter);
369 center_z /= static_cast<float> (counter);
371 bb.
width = max_x - min_x;
372 bb.
height = max_y - min_y;
373 bb.
depth = max_z - min_z;
375 bb.
x = (min_x + bb.
width / 2.0f) - center_x - bb.
width / 2.0f;
376 bb.
y = (min_y + bb.
height / 2.0f) - center_y - bb.
height / 2.0f;
377 bb.
z = (min_z + bb.
depth / 2.0f) - center_z - bb.
depth / 2.0f;
379 for (std::size_t j = 0; j < template_point_cloud.
size (); ++j)
383 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
390 template_point_cloud.
points[j] = p;
398 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
402 std::vector<pcl::QuantizableModality*> modalities;
403 modalities.push_back (&color_gradient_mod_);
404 modalities.push_back (&surface_normal_mod_);
406 std::vector<pcl::LINEMODDetection> linemod_detections;
407 linemod_.detectTemplates (modalities, linemod_detections);
409 detections_.clear ();
410 detections_.reserve (linemod_detections.size ());
412 detections.reserve (linemod_detections.size ());
413 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
429 linemod_.getTemplate (linemod_detection.
template_id);
431 const std::size_t start_x = std::max (linemod_detection.
x, 0);
432 const std::size_t start_y = std::max (linemod_detection.
y, 0);
433 const std::size_t end_x = std::min (static_cast<std::size_t> (start_x + linemod_template.
region.
width),
434 static_cast<std::size_t> (cloud_xyz_->width));
435 const std::size_t end_y = std::min (static_cast<std::size_t> (start_y + linemod_template.
region.
height),
436 static_cast<std::size_t> (cloud_xyz_->height));
438 detection.
region.
x = linemod_detection.
x;
439 detection.
region.
y = linemod_detection.
y;
448 float center_x = 0.0f;
449 float center_y = 0.0f;
450 float center_z = 0.0f;
451 std::size_t counter = 0;
452 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
454 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
456 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
458 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
467 const float inv_counter = 1.0f / static_cast<float> (counter);
468 center_x *= inv_counter;
469 center_y *= inv_counter;
470 center_z *= inv_counter;
479 detections_.push_back (detection);
483 refineDetectionsAlongDepth ();
487 removeOverlappingDetections ();
489 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
491 detections.push_back (detections_[detection_index]);
496 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
499 const float min_scale,
500 const float max_scale,
501 const float scale_multiplier)
503 std::vector<pcl::QuantizableModality*> modalities;
504 modalities.push_back (&color_gradient_mod_);
505 modalities.push_back (&surface_normal_mod_);
507 std::vector<pcl::LINEMODDetection> linemod_detections;
508 linemod_.detectTemplatesSemiScaleInvariant (modalities, linemod_detections, min_scale, max_scale, scale_multiplier);
510 detections_.clear ();
511 detections_.reserve (linemod_detections.size ());
513 detections.reserve (linemod_detections.size ());
514 for (std::size_t detection_id = 0; detection_id < linemod_detections.size (); ++detection_id)
530 linemod_.getTemplate (linemod_detection.
template_id);
532 const std::size_t start_x = std::max (linemod_detection.
x, 0);
533 const std::size_t start_y = std::max (linemod_detection.
y, 0);
534 const std::size_t end_x = std::min (static_cast<std::size_t> (start_x + linemod_template.
region.
width * linemod_detection.
scale),
535 static_cast<std::size_t> (cloud_xyz_->width));
536 const std::size_t end_y = std::min (static_cast<std::size_t> (start_y + linemod_template.
region.
height * linemod_detection.
scale),
537 static_cast<std::size_t> (cloud_xyz_->height));
539 detection.
region.
x = linemod_detection.
x;
540 detection.
region.
y = linemod_detection.
y;
549 float center_x = 0.0f;
550 float center_y = 0.0f;
551 float center_z = 0.0f;
552 std::size_t counter = 0;
553 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
555 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
557 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
559 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
568 const float inv_counter = 1.0f / static_cast<float> (counter);
569 center_x *= inv_counter;
570 center_y *= inv_counter;
571 center_z *= inv_counter;
580 detections_.push_back (detection);
588 removeOverlappingDetections ();
590 for (std::size_t detection_index = 0; detection_index < detections_.size (); ++detection_index)
592 detections.push_back (detections_[detection_index]);
597 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
601 if (detection_id >= detections_.size ())
602 PCL_ERROR (
"ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
604 const std::size_t template_id = detections_[detection_id].template_id;
608 const pcl::BoundingBoxXYZ & detection_bounding_box = detections_[detection_id].bounding_box;
618 const float translation_x = detection_bounding_box.
x - template_bounding_box.
x;
619 const float translation_y = detection_bounding_box.
y - template_bounding_box.
y;
620 const float translation_z = detection_bounding_box.
z - template_bounding_box.
z;
627 const std::size_t nr_points = template_point_cloud.
size ();
631 for (std::size_t point_index = 0; point_index < nr_points; ++point_index)
635 point.x += translation_x;
636 point.y += translation_y;
637 point.z += translation_z;
639 cloud.
points[point_index] = point;
644 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
647 const std::size_t nr_detections = detections_.size ();
648 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
653 const std::size_t start_x = std::max (detection.
region.
x, 0);
654 const std::size_t start_y = std::max (detection.
region.
y, 0);
655 const std::size_t end_x = std::min (static_cast<std::size_t> (detection.
region.
x + detection.
region.
width),
656 static_cast<std::size_t> (cloud_xyz_->width));
657 const std::size_t end_y = std::min (static_cast<std::size_t> (detection.
region.
y + detection.
region.
height),
658 static_cast<std::size_t> (cloud_xyz_->height));
661 float min_depth = std::numeric_limits<float>::max ();
662 float max_depth = -std::numeric_limits<float>::max ();
663 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
665 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
667 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
669 if (std::isfinite (point.z))
671 min_depth = std::min (min_depth, point.z);
672 max_depth = std::max (max_depth, point.z);
677 const std::size_t nr_bins = 1000;
678 const float step_size = (max_depth - min_depth) / static_cast<float> (nr_bins-1);
679 std::vector<std::size_t> depth_bins (nr_bins, 0);
680 for (std::size_t row_index = start_y; row_index < end_y; ++row_index)
682 for (std::size_t col_index = start_x; col_index < end_x; ++col_index)
684 const PointXYZT & point = (*cloud_xyz_) (col_index, row_index);
686 if (std::isfinite (point.z))
688 const std::size_t bin_index = static_cast<std::size_t> ((point.z - min_depth) / step_size);
689 ++depth_bins[bin_index];
694 std::vector<std::size_t> integral_depth_bins (nr_bins, 0);
696 integral_depth_bins[0] = depth_bins[0];
697 for (std::size_t bin_index = 1; bin_index < nr_bins; ++bin_index)
699 integral_depth_bins[bin_index] = depth_bins[bin_index] + integral_depth_bins[bin_index-1];
702 const std::size_t bb_depth_range = static_cast<std::size_t> (detection.
bounding_box.
depth / step_size);
704 std::size_t max_nr_points = 0;
705 std::size_t max_index = 0;
706 for (std::size_t bin_index = 0; (bin_index+bb_depth_range) < nr_bins; ++bin_index)
708 const std::size_t nr_points_in_range = integral_depth_bins[bin_index+bb_depth_range] - integral_depth_bins[bin_index];
710 if (nr_points_in_range > max_nr_points)
712 max_nr_points = nr_points_in_range;
713 max_index = bin_index;
717 const float z = static_cast<float> (max_index) * step_size + min_depth;
724 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
727 const std::size_t nr_detections = detections_.size ();
728 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
732 const std::size_t template_id = detection.
template_id;
735 const std::size_t start_x = detection.
region.
x;
736 const std::size_t start_y = detection.
region.
y;
737 const std::size_t pc_width = point_cloud.
width;
738 const std::size_t pc_height = point_cloud.
height;
740 std::vector<std::pair<float, float> > depth_matches;
741 for (std::size_t row_index = 0; row_index < pc_height; ++row_index)
743 for (std::size_t col_index = 0; col_index < pc_width; ++col_index)
746 const PointXYZT & point_input = (*cloud_xyz_) (col_index + start_x, row_index + start_y);
748 if (!std::isfinite (point_template.z) || !std::isfinite (point_input.z))
751 depth_matches.push_back (std::make_pair (point_template.z, point_input.z));
756 const std::size_t nr_matches = depth_matches.size ();
757 const std::size_t nr_iterations = 100;
758 const float inlier_threshold = 0.01f;
759 std::size_t best_nr_inliers = 0;
760 float best_z_translation = 0.0f;
761 for (std::size_t iteration_index = 0; iteration_index < nr_iterations; ++iteration_index)
763 const std::size_t rand_index = (rand () * nr_matches) / RAND_MAX;
765 const float z_translation = depth_matches[rand_index].second - depth_matches[rand_index].first;
767 std::size_t nr_inliers = 0;
768 for (std::size_t match_index = 0; match_index < nr_matches; ++match_index)
770 const float error = std::abs (depth_matches[match_index].first + z_translation - depth_matches[match_index].second);
772 if (error <= inlier_threshold)
778 if (nr_inliers > best_nr_inliers)
780 best_nr_inliers = nr_inliers;
781 best_z_translation = z_translation;
785 float average_depth = 0.0f;
786 std::size_t average_counter = 0;
787 for (std::size_t match_index = 0; match_index < nr_matches; ++match_index)
789 const float error = std::abs (depth_matches[match_index].first + best_z_translation - depth_matches[match_index].second);
791 if (error <= inlier_threshold)
794 average_depth += depth_matches[match_index].second - depth_matches[match_index].first;
798 average_depth /= static_cast<float> (average_counter);
800 detection.
bounding_box.
z = bounding_boxes_[template_id].z + average_depth;
805 template <
typename Po
intXYZT,
typename Po
intRGBT>
void
809 const std::size_t nr_detections = detections_.size ();
810 Eigen::MatrixXf overlaps (nr_detections, nr_detections);
811 for (std::size_t detection_index_1 = 0; detection_index_1 < nr_detections; ++detection_index_1)
813 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
815 const float bounding_box_volume = detections_[detection_index_1].bounding_box.width
816 * detections_[detection_index_1].bounding_box.height
817 * detections_[detection_index_1].bounding_box.depth;
819 if (detections_[detection_index_1].object_id != detections_[detection_index_2].object_id)
820 overlaps (detection_index_1, detection_index_2) = 0.0f;
822 overlaps (detection_index_1, detection_index_2) = computeBoundingBoxIntersectionVolume (
823 detections_[detection_index_1].bounding_box,
824 detections_[detection_index_2].bounding_box) / bounding_box_volume;
829 std::vector<int> detection_to_cluster_mapping (nr_detections, -1);
830 std::vector<std::vector<std::size_t> > clusters;
831 for (std::size_t detection_index = 0; detection_index < nr_detections; ++detection_index)
833 if (detection_to_cluster_mapping[detection_index] != -1)
836 std::vector<std::size_t> cluster;
837 const std::size_t cluster_id = clusters.size ();
839 cluster.push_back (detection_index);
840 detection_to_cluster_mapping[detection_index] = static_cast<int> (cluster_id);
843 for (std::size_t cluster_index = 0; cluster_index < cluster.size (); ++cluster_index)
845 const std::size_t detection_index_1 = cluster[cluster_index];
847 for (std::size_t detection_index_2 = detection_index_1+1; detection_index_2 < nr_detections; ++detection_index_2)
849 if (detection_to_cluster_mapping[detection_index_2] != -1)
852 if (overlaps (detection_index_1, detection_index_2) < intersection_volume_threshold_)
855 cluster.push_back (detection_index_2);
856 detection_to_cluster_mapping[detection_index_2] = static_cast<int> (cluster_id);
860 clusters.push_back (cluster);
864 std::vector<typename LineRGBD<PointXYZT, PointRGBT>::Detection> clustered_detections;
866 const std::size_t nr_clusters = clusters.size ();
867 for (std::size_t cluster_id = 0; cluster_id < nr_clusters; ++cluster_id)
869 std::vector<std::size_t> & cluster = clusters[cluster_id];
871 float average_center_x = 0.0f;
872 float average_center_y = 0.0f;
873 float average_center_z = 0.0f;
874 float weight_sum = 0.0f;
876 float best_response = 0.0f;
877 std::size_t best_detection_id = 0;
879 float average_region_x = 0.0f;
880 float average_region_y = 0.0f;
882 const std::size_t elements_in_cluster = cluster.size ();
883 for (std::size_t cluster_index = 0; cluster_index < elements_in_cluster; ++cluster_index)
885 const std::size_t detection_id = cluster[cluster_index];
887 const float weight = detections_[detection_id].response;
889 if (weight > best_response)
891 best_response = weight;
892 best_detection_id = detection_id;
895 const Detection & d = detections_[detection_id];
900 average_center_x += p_center_x * weight;
901 average_center_y += p_center_y * weight;
902 average_center_z += p_center_z * weight;
903 weight_sum += weight;
905 average_region_x += float (detections_[detection_id].region.x) * weight;
906 average_region_y += float (detections_[detection_id].region.y) * weight;
910 detection.
template_id = detections_[best_detection_id].template_id;
911 detection.
object_id = detections_[best_detection_id].object_id;
915 const float inv_weight_sum = 1.0f / weight_sum;
916 const float best_detection_bb_width = detections_[best_detection_id].bounding_box.width;
917 const float best_detection_bb_height = detections_[best_detection_id].bounding_box.height;
918 const float best_detection_bb_depth = detections_[best_detection_id].bounding_box.depth;
920 detection.
bounding_box.
x = average_center_x * inv_weight_sum - best_detection_bb_width/2.0f;
921 detection.
bounding_box.
y = average_center_y * inv_weight_sum - best_detection_bb_height/2.0f;
922 detection.
bounding_box.
z = average_center_z * inv_weight_sum - best_detection_bb_depth/2.0f;
927 detection.
region.
x = int (average_region_x * inv_weight_sum);
928 detection.
region.
y = int (average_region_y * inv_weight_sum);
929 detection.
region.
width = detections_[best_detection_id].region.width;
930 detection.
region.
height = detections_[best_detection_id].region.height;
932 clustered_detections.push_back (detection);
935 detections_ = clustered_detections;
939 template <
typename Po
intXYZT,
typename Po
intRGBT>
float
943 const float x1_min = box1.
x;
944 const float y1_min = box1.
y;
945 const float z1_min = box1.
z;
946 const float x1_max = box1.
x + box1.
width;
947 const float y1_max = box1.
y + box1.
height;
948 const float z1_max = box1.
z + box1.
depth;
950 const float x2_min = box2.
x;
951 const float y2_min = box2.
y;
952 const float z2_min = box2.
z;
953 const float x2_max = box2.
x + box2.
width;
954 const float y2_max = box2.
y + box2.
height;
955 const float z2_max = box2.
z + box2.
depth;
957 const float xi_min = std::max (x1_min, x2_min);
958 const float yi_min = std::max (y1_min, y2_min);
959 const float zi_min = std::max (z1_min, z2_min);
961 const float xi_max = std::min (x1_max, x2_max);
962 const float yi_max = std::min (y1_max, y2_max);
963 const float zi_max = std::min (z1_max, z2_max);
965 const float intersection_width = xi_max - xi_min;
966 const float intersection_height = yi_max - yi_min;
967 const float intersection_depth = zi_max - zi_min;
969 if (intersection_width <= 0.0f || intersection_height <= 0.0f || intersection_depth <= 0.0f)
972 const float intersection_volume = intersection_width * intersection_height * intersection_depth;
974 return (intersection_volume);
978 #endif // PCL_RECOGNITION_LINEMOD_LINE_RGBD_IMPL_HPP_