Loading...
Searching...
No Matches
line_rgbd.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#pragma once
#include <pcl/recognition/linemod.h>
#include <pcl/recognition/color_gradient_modality.h>
#include <pcl/recognition/surface_normal_modality.h>
#include <pcl/io/tar.h>
namespace pcl
{
struct BoundingBoxXYZ
{
/** \brief Constructor. */
BoundingBoxXYZ () = default;
/** \brief X-coordinate of the upper left front point */
float x{0.0f};
/** \brief Y-coordinate of the upper left front point */
float y{0.0f};
/** \brief Z-coordinate of the upper left front point */
float z{0.0f};
/** \brief Width of the bounding box */
float width{0.0f};
/** \brief Height of the bounding box */
float height{0.0f};
/** \brief Depth of the bounding box */
float depth{0.0f};
};
/** \brief High-level class for template matching using the LINEMOD approach based on RGB and Depth data.
* \author Stefan Holzer
* \ingroup recognition
*/
template <typename PointXYZT, typename PointRGBT=PointXYZT>
class PCL_EXPORTS LineRGBD
{
public:
/** \brief A LineRGBD detection. */
struct Detection
{
/** \brief Constructor. */
Detection () = default;
/** \brief The ID of the template. */
std::size_t template_id{0};
/** \brief The ID of the object corresponding to the template. */
std::size_t object_id{0};
/** \brief The ID of this detection. This is only valid for the last call of the method detect (...). */
std::size_t detection_id{0};
/** \brief The response of this detection. Responses are between 0 and 1, where 1 is best. */
float response{0.0f};
/** \brief The 3D bounding box of the detection. */
BoundingBoxXYZ bounding_box;
/** \brief The 2D template region of the detection. */
RegionXY region;
};
/** \brief Constructor */
LineRGBD ()
: color_gradient_mod_ ()
, surface_normal_mod_ ()
, cloud_xyz_ ()
, cloud_rgb_ ()
, detections_ ()
{
}
/** \brief Destructor */
virtual ~LineRGBD () = default;
/** \brief Loads templates from a LMT (LineMod Template) file. Overrides old templates.
*
* LineMod Template files are TAR files that store pairs of PCD datasets
* together with their LINEMOD signatures in \ref
* SparseQuantizedMultiModTemplate format.
*
* \param[in] file_name The name of the file that stores the templates.
* \param object_id
*
* \return true, if the operation was successful, false otherwise.
*/
bool
loadTemplates (const std::string &file_name, std::size_t object_id = 0);
bool
addTemplate (const SparseQuantizedMultiModTemplate & sqmmt, pcl::PointCloud<pcl::PointXYZRGBA> & cloud, std::size_t object_id = 0);
/** \brief Sets the threshold for the detection responses. Responses are between 0 and 1, where 1 is a best.
* \param[in] threshold The threshold used to decide where a template is detected.
*/
inline void
setDetectionThreshold (float threshold)
{
linemod_.setDetectionThreshold (threshold);
}
/** \brief Sets the threshold on the magnitude of color gradients. Color gradients with a magnitude below
* this threshold are not considered in the detection process.
* \param[in] threshold The threshold on the magnitude of color gradients.
*/
inline void
setGradientMagnitudeThreshold (const float threshold)
{
color_gradient_mod_.setGradientMagnitudeThreshold (threshold);
}
/** \brief Sets the threshold for the decision whether two detections of the same template are merged or not.
* If ratio between the intersection of the bounding boxes of two detections and the original bounding
* boxes is larger than the specified threshold then they are merged. If detection A overlaps with
* detection B and B with C than A, B, and C are merged. Threshold has to be between 0 and 1.
* \param[in] threshold The threshold on the ratio between the intersection bounding box and the original
* bounding box.
*/
inline void
setIntersectionVolumeThreshold (const float threshold = 1.0f)
{
intersection_volume_threshold_ = threshold;
}
/** \brief Sets the input cloud with xyz point coordinates. The cloud has to be organized.
* \param[in] cloud The input cloud with xyz point coordinates.
*/
inline void
setInputCloud (const typename pcl::PointCloud<PointXYZT>::ConstPtr & cloud)
{
cloud_xyz_ = cloud;
surface_normal_mod_.setInputCloud (cloud);
surface_normal_mod_.processInputData ();
}
/** \brief Sets the input cloud with rgb values. The cloud has to be organized.
* \param[in] cloud The input cloud with rgb values.
*/
inline void
setInputColors (const typename pcl::PointCloud<PointRGBT>::ConstPtr & cloud)
{
cloud_rgb_ = cloud;
color_gradient_mod_.setInputCloud (cloud);
color_gradient_mod_.processInputData ();
}
/** \brief Creates a template from the specified data and adds it to the matching queue.
* \param cloud
* \param object_id
* \param[in] mask_xyz the mask that determine which parts of the xyz-modality are used for creating the template.
* \param[in] mask_rgb the mask that determine which parts of the rgb-modality are used for creating the template.
* \param[in] region the region which will be associated with the template (can be larger than the actual modality-maps).
*/
int
createAndAddTemplate (
pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
const std::size_t object_id,
const MaskMap & mask_xyz,
const MaskMap & mask_rgb,
const RegionXY & region);
/** \brief Applies the detection process and fills the supplied vector with the detection instances.
* \param[out] detections The storage for the detection instances.
*/
void
detect (std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> & detections);
/** \brief Applies the detection process in a semi-scale-invariant manner. This is done by actually
* scaling the template to different sizes.
*/
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);
/** \brief Computes and returns the point cloud of the specified detection. This is the template point
* cloud transformed to the detection coordinates. The detection ID refers to the last call of
* the method detect (...).
* \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
* \param[out] cloud The storage for the transformed points.
*/
void
computeTransformedTemplatePoints (const std::size_t detection_id,
pcl::PointCloud<pcl::PointXYZRGBA> & cloud);
/** \brief Finds the indices of the points in the input cloud which correspond to the specified detection.
* The detection ID refers to the last call of the method detect (...).
* \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
*/
inline std::vector<std::size_t>
findObjectPointIndices (const std::size_t detection_id)
{
if (detection_id >= detections_.size ())
PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
// TODO: compute transform from detection
// TODO: transform template points
std::vector<std::size_t> vec;
return (vec);
}
protected:
/** \brief Aligns the template points with the points found at the detection position of the specified detection.
* The detection ID refers to the last call of the method detect (...).
* \param[in] detection_id The ID of the detection (according to the last call of the method detect (...)).
*/
inline std::vector<std::size_t>
alignTemplatePoints (const std::size_t detection_id)
{
if (detection_id >= detections_.size ())
PCL_ERROR ("ERROR pcl::LineRGBD::computeTransformedTemplatePoints - detection_id is out of bounds\n");
// TODO: compute transform from detection
// TODO: transform template points
std::vector<std::size_t> vec;
return (vec);
}
/** \brief Refines the found detections along the depth. */
void
refineDetectionsAlongDepth ();
/** \brief Applies projective ICP on detections to find their correct position in depth. */
void
applyProjectiveDepthICPOnDetections ();
/** \brief Checks for overlapping detections, removes them and keeps only the strongest one. */
void
removeOverlappingDetections ();
/** \brief Computes the volume of the intersection between two bounding boxes.
* \param[in] box1 First bounding box.
* \param[in] box2 Second bounding box.
*/
static float
computeBoundingBoxIntersectionVolume (const BoundingBoxXYZ &box1, const BoundingBoxXYZ &box2);
private:
/** \brief Read another LTM header chunk. */
bool
readLTMHeader (int fd, pcl::io::TARHeader &header);
/** \brief Intersection volume threshold. */
float intersection_volume_threshold_{1.0f};
/** \brief LINEMOD instance. */
public: pcl::LINEMOD linemod_;
/** \brief Color gradient modality. */
pcl::ColorGradientModality<PointRGBT> color_gradient_mod_;
/** \brief Surface normal modality. */
pcl::SurfaceNormalModality<PointXYZT> surface_normal_mod_;
/** \brief XYZ point cloud. */
typename pcl::PointCloud<PointXYZT>::ConstPtr cloud_xyz_;
/** \brief RGB point cloud. */
typename pcl::PointCloud<PointRGBT>::ConstPtr cloud_rgb_;
/** \brief Point clouds corresponding to the templates. */
pcl::PointCloud<pcl::PointXYZRGBA>::CloudVectorType template_point_clouds_;
/** \brief Bounding boxes corresponding to the templates. */
std::vector<pcl::BoundingBoxXYZ> bounding_boxes_;
/** \brief Object IDs corresponding to the templates. */
std::vector<std::size_t> object_ids_;
/** \brief Detections from last call of method detect (...). */
std::vector<typename pcl::LineRGBD<PointXYZT, PointRGBT>::Detection> detections_;
};
}
#include <pcl/recognition/impl/linemod/line_rgbd.hpp>
pcl::ColorGradientModality
Modality based on max-RGB gradients.
Definition color_gradient_modality.h:59
pcl::LINEMOD
Template matching using the LINEMOD approach.
Definition linemod.h:327
pcl::LineRGBD::setDetectionThreshold
void setDetectionThreshold(float threshold)
Sets the threshold for the detection responses.
Definition line_rgbd.h:131
pcl::LineRGBD::cloud_xyz_
pcl::PointCloud< PointXYZT >::ConstPtr cloud_xyz_
XYZ point cloud.
Definition line_rgbd.h:294
pcl::LineRGBD::linemod_
pcl::LINEMOD linemod_
LINEMOD instance.
Definition line_rgbd.h:287
pcl::LineRGBD::cloud_rgb_
pcl::PointCloud< PointRGBT >::ConstPtr cloud_rgb_
RGB point cloud.
Definition line_rgbd.h:296
pcl::LineRGBD::detections_
std::vector< typename pcl::LineRGBD< PointXYZT, PointRGBT >::Detection > detections_
Detections from last call of method detect (...).
Definition line_rgbd.h:306
pcl::LineRGBD::addTemplate
bool addTemplate(const SparseQuantizedMultiModTemplate &sqmmt, pcl::PointCloud< pcl::PointXYZRGBA > &cloud, std::size_t object_id=0)
Definition line_rgbd.hpp:314
pcl::LineRGBD::setGradientMagnitudeThreshold
void setGradientMagnitudeThreshold(const float threshold)
Sets the threshold on the magnitude of color gradients.
Definition line_rgbd.h:141
pcl::LineRGBD::bounding_boxes_
std::vector< pcl::BoundingBoxXYZ > bounding_boxes_
Bounding boxes corresponding to the templates.
Definition line_rgbd.h:301
pcl::LineRGBD::object_ids_
std::vector< std::size_t > object_ids_
Object IDs corresponding to the templates.
Definition line_rgbd.h:303
pcl::LineRGBD::color_gradient_mod_
pcl::ColorGradientModality< PointRGBT > color_gradient_mod_
Color gradient modality.
Definition line_rgbd.h:289
pcl::LineRGBD::surface_normal_mod_
pcl::SurfaceNormalModality< PointXYZT > surface_normal_mod_
Surface normal modality.
Definition line_rgbd.h:291
pcl::LineRGBD::~LineRGBD
virtual ~LineRGBD()=default
Destructor.
pcl::LineRGBD::template_point_clouds_
pcl::PointCloud< pcl::PointXYZRGBA >::CloudVectorType template_point_clouds_
Point clouds corresponding to the templates.
Definition line_rgbd.h:299
pcl::LineRGBD::setIntersectionVolumeThreshold
void setIntersectionVolumeThreshold(const float threshold=1.0f)
Sets the threshold for the decision whether two detections of the same template are merged or not.
Definition line_rgbd.h:154
pcl::LineRGBD::alignTemplatePoints
std::vector< std::size_t > alignTemplatePoints(const std::size_t detection_id)
Aligns the template points with the points found at the detection position of the specified detection...
Definition line_rgbd.h:248
pcl::LineRGBD::setInputColors
void setInputColors(const typename pcl::PointCloud< PointRGBT >::ConstPtr &cloud)
Sets the input cloud with rgb values.
Definition line_rgbd.h:175
pcl::LineRGBD::setInputCloud
void setInputCloud(const typename pcl::PointCloud< PointXYZT >::ConstPtr &cloud)
Sets the input cloud with xyz point coordinates.
Definition line_rgbd.h:163
pcl::LineRGBD::findObjectPointIndices
std::vector< std::size_t > findObjectPointIndices(const std::size_t detection_id)
Finds the indices of the points in the input cloud which correspond to the specified detection.
Definition line_rgbd.h:229
pcl::LineRGBD::loadTemplates
bool loadTemplates(const std::string &file_name, std::size_t object_id=0)
Loads templates from a LMT (LineMod Template) file.
Definition line_rgbd.hpp:77
pcl::LineRGBD::LineRGBD
LineRGBD()
Constructor.
Definition line_rgbd.h:98
pcl::MaskMap
Definition mask_map.h:45
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::PointCloud::CloudVectorType
std::vector< PointCloud< PointT >, Eigen::aligned_allocator< PointCloud< PointT > > > CloudVectorType
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:415
pcl::SurfaceNormalModality
Modality based on surface normals.
Definition surface_normal_modality.h:297
pcl
Definition convolution.h:46
pcl::BoundingBoxXYZ
Definition line_rgbd.h:49
pcl::BoundingBoxXYZ::width
float width
Width of the bounding box.
Definition line_rgbd.h:61
pcl::BoundingBoxXYZ::x
float x
X-coordinate of the upper left front point.
Definition line_rgbd.h:54
pcl::BoundingBoxXYZ::y
float y
Y-coordinate of the upper left front point.
Definition line_rgbd.h:56
pcl::BoundingBoxXYZ::depth
float depth
Depth of the bounding box.
Definition line_rgbd.h:65
pcl::BoundingBoxXYZ::z
float z
Z-coordinate of the upper left front point.
Definition line_rgbd.h:58
pcl::BoundingBoxXYZ::BoundingBoxXYZ
BoundingBoxXYZ()=default
Constructor.
pcl::BoundingBoxXYZ::height
float height
Height of the bounding box.
Definition line_rgbd.h:63
pcl::LineRGBD::Detection
A LineRGBD detection.
Definition line_rgbd.h:79
pcl::LineRGBD::Detection::template_id
std::size_t template_id
The ID of the template.
Definition line_rgbd.h:84
pcl::LineRGBD::Detection::bounding_box
BoundingBoxXYZ bounding_box
The 3D bounding box of the detection.
Definition line_rgbd.h:92
pcl::LineRGBD::Detection::object_id
std::size_t object_id
The ID of the object corresponding to the template.
Definition line_rgbd.h:86
pcl::LineRGBD::Detection::Detection
Detection()=default
Constructor.
pcl::LineRGBD::Detection::response
float response
The response of this detection.
Definition line_rgbd.h:90
pcl::LineRGBD::Detection::detection_id
std::size_t detection_id
The ID of this detection.
Definition line_rgbd.h:88
pcl::LineRGBD::Detection::region
RegionXY region
The 2D template region of the detection.
Definition line_rgbd.h:94
pcl::RegionXY
Defines a region in XY-space.
Definition region_xy.h:82
pcl::SparseQuantizedMultiModTemplate
A multi-modality template constructed from a set of quantized multi-modality features.
Definition sparse_quantized_multi_mod_template.h:109
pcl::io::TARHeader
A TAR file's header, as described on https://en.wikipedia.org/wiki/Tar_%28file_format%29.
Definition tar.h:50