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 (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[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[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[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[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_