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>
51template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
66 if (std::string (header.
ustar).substr (0, 5) !=
"ustar")
76template <
typename Po
intXYZT,
typename Po
intRGBT>
bool
101 std::string::size_type
it;
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);
117 PCL_DEBUG (
"[pcl::LineRGBD::loadTemplates] Reading and parsing %s as a SQMMT file.\n",
chunk_name.c_str ());
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);
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 ();
158 bb.width =
bb.height =
bb.depth = 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 ();
174 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
207 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
222template <
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 ();
245 bb.width =
bb.height =
bb.depth = 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 ();
261 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
294 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
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;
313 return (linemod_.createAndAddTemplate (modalities,
masks, region));
317template <
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 ();
336 bb.width =
bb.height =
bb.depth = 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 ();
352 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
385 if (!std::isfinite (p.x) || !std::isfinite (p.y) || !std::isfinite (p.z))
400template <
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_);
411 detections_.clear ();
436 static_cast<std::size_t
> (cloud_xyz_->width));
438 static_cast<std::size_t
> (cloud_xyz_->height));
460 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
485 refineDetectionsAlongDepth ();
489 removeOverlappingDetections ();
498template <
typename Po
intXYZT,
typename Po
intRGBT>
void
505 std::vector<pcl::QuantizableModality*> modalities;
506 modalities.push_back (&color_gradient_mod_);
507 modalities.push_back (&surface_normal_mod_);
512 detections_.clear ();
537 static_cast<std::size_t
> (cloud_xyz_->width));
539 static_cast<std::size_t
> (cloud_xyz_->height));
561 if (std::isfinite (point.x) && std::isfinite (point.y) && std::isfinite (point.z))
590 removeOverlappingDetections ();
599template <
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;
630 cloud.resize (nr_points);
646template <
typename Po
intXYZT,
typename Po
intRGBT>
void
658 static_cast<std::size_t
> (cloud_xyz_->width));
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 ();
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);
688 if (std::isfinite (point.z))
690 const std::size_t bin_index =
static_cast<std::size_t
> ((point.z - min_depth) / step_size);
699 for (std::size_t bin_index = 1; bin_index <
nr_bins; ++bin_index)
719 const float z =
static_cast<float> (
max_index) * step_size + min_depth;
726template <
typename Po
intXYZT,
typename Po
intRGBT>
void
734 const std::size_t template_id =
detection.template_id;
807template <
typename Po
intXYZT,
typename Po
intRGBT>
void
832 std::vector<std::vector<std::size_t> >
clusters;
838 std::vector<std::size_t> cluster;
889 const float weight = detections_[detection_id].response;
897 const Detection & d = detections_[detection_id];
941template <
typename Po
intXYZT,
typename Po
intRGBT>
float
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
void detect(std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > &detections)
Applies the detection process and fills the supplied vector with the detection instances.
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 refineDetectionsAlongDepth()
Refines the found detections along the depth.
void applyProjectiveDepthICPOnDetections()
Applies projective ICP on detections to find their correct position in depth.
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, std::size_t object_id=0)
void computeTransformedTemplatePoints(const std::size_t detection_id, pcl::PointCloud< pcl::PointXYZRGBA > &cloud)
Computes and returns the point cloud of the specified detection.
static float computeBoundingBoxIntersectionVolume(const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2)
Computes the volume of the intersection between two bounding boxes.
void removeOverlappingDetections()
Checks for overlapping detections, removes them and keeps only the strongest one.
bool loadTemplates(const std::string &file_name, std::size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
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.
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.
int raw_lseek(int fd, long offset, int whence)
int raw_open(const char *pathname, int flags, int mode)
int raw_read(int fd, void *buffer, std::size_t count)
float width
Width of the bounding box.
float x
X-coordinate of the upper left front point.
float y
Y-coordinate of the upper left front point.
float depth
Depth of the bounding box.
float z
Z-coordinate of the upper left front point.
float height
Height of the bounding box.
Represents a detection of a template using the LINEMOD approach.
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
Defines a region in XY-space.
A multi-modality template constructed from a set of quantized multi-modality features.