431 lines
17 KiB
C++
431 lines
17 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Point Cloud Library (PCL) - www.pointclouds.org
|
|
* Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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/pcl_base.h>
|
|
#include <pcl/PointIndices.h>
|
|
|
|
namespace pcl
|
|
{
|
|
/** \brief OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals,
|
|
* and OrganizedEdgeFromRGBNormals find 3D edges from an organized point
|
|
* cloud data. Given an organized point cloud, they will output a PointCloud
|
|
* of edge labels and a vector of PointIndices.
|
|
* OrganizedEdgeBase accepts PCL_XYZ_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, and EDGELABEL_OCCLUDED.
|
|
* OrganizedEdgeFromRGB accepts PCL_RGB_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_RGB_CANNY.
|
|
* OrganizedEdgeFromNormals accepts PCL_XYZ_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_HIGH_CURVATURE.
|
|
* OrganizedEdgeFromRGBNormals accepts PCL_RGB_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, EDGELABEL_HIGH_CURVATURE, and EDGELABEL_RGB_CANNY.
|
|
*
|
|
* \author Changhyun Choi
|
|
*/
|
|
template <typename PointT, typename PointLT>
|
|
class OrganizedEdgeBase : public PCLBase<PointT>
|
|
{
|
|
using PointCloud = pcl::PointCloud<PointT>;
|
|
using PointCloudPtr = typename PointCloud::Ptr;
|
|
using PointCloudConstPtr = typename PointCloud::ConstPtr;
|
|
|
|
using PointCloudL = pcl::PointCloud<PointLT>;
|
|
using PointCloudLPtr = typename PointCloudL::Ptr;
|
|
using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
|
|
|
|
public:
|
|
using Ptr = shared_ptr<OrganizedEdgeBase<PointT, PointLT> >;
|
|
using ConstPtr = shared_ptr<const OrganizedEdgeBase<PointT, PointLT> >;
|
|
using PCLBase<PointT>::input_;
|
|
using PCLBase<PointT>::indices_;
|
|
using PCLBase<PointT>::initCompute;
|
|
using PCLBase<PointT>::deinitCompute;
|
|
|
|
/** \brief Constructor for OrganizedEdgeBase */
|
|
OrganizedEdgeBase ()
|
|
: th_depth_discon_ (0.02f)
|
|
, max_search_neighbors_ (50)
|
|
, detecting_edge_types_ (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED)
|
|
{
|
|
}
|
|
|
|
/** \brief Destructor for OrganizedEdgeBase */
|
|
|
|
~OrganizedEdgeBase ()
|
|
{
|
|
}
|
|
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities)
|
|
* \param[out] labels a PointCloud of edge labels
|
|
* \param[out] label_indices a vector of PointIndices corresponding to each edge label
|
|
*/
|
|
void
|
|
compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
|
|
|
|
/** \brief Set the tolerance in meters for the relative difference in depth values between neighboring points.
|
|
* e.g. If a point has a depth (z) value of 2.0 meters, a neighboring point is discontinuous if its depth differs by > 2.0 * th. */
|
|
inline void
|
|
setDepthDisconThreshold (const float th)
|
|
{
|
|
th_depth_discon_ = th;
|
|
}
|
|
|
|
/** \brief Get the tolerance in meters for the relative difference in depth values between neighboring points.
|
|
* e.g. If a point has a depth (z) value of 2.0 meters, a neighboring point is discontinuous if its depth differs by > 2.0 * th. */
|
|
inline float
|
|
getDepthDisconThreshold () const
|
|
{
|
|
return (th_depth_discon_);
|
|
}
|
|
|
|
/** \brief Set the max search distance for deciding occluding and occluded edges. */
|
|
inline void
|
|
setMaxSearchNeighbors (const int max_dist)
|
|
{
|
|
max_search_neighbors_ = max_dist;
|
|
}
|
|
|
|
/** \brief Get the max search distance for deciding occluding and occluded edges. */
|
|
inline int
|
|
getMaxSearchNeighbors () const
|
|
{
|
|
return (max_search_neighbors_);
|
|
}
|
|
|
|
/** \brief Set the detecting edge types. */
|
|
inline void
|
|
setEdgeType (int edge_types)
|
|
{
|
|
detecting_edge_types_ = edge_types;
|
|
}
|
|
|
|
/** \brief Get the detecting edge types. */
|
|
inline int
|
|
getEdgeType () const
|
|
{
|
|
return detecting_edge_types_;
|
|
}
|
|
|
|
enum {EDGELABEL_NAN_BOUNDARY=1, EDGELABEL_OCCLUDING=2, EDGELABEL_OCCLUDED=4, EDGELABEL_HIGH_CURVATURE=8, EDGELABEL_RGB_CANNY=16};
|
|
static const int num_of_edgetype_ = 5;
|
|
|
|
protected:
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each edge label
|
|
* \param[out] labels a PointCloud of edge labels
|
|
*/
|
|
void
|
|
extractEdges (pcl::PointCloud<PointLT>& labels) const;
|
|
|
|
/** \brief Assign point indices for each edge label
|
|
* \param[out] labels a PointCloud of edge labels
|
|
* \param[out] label_indices a vector of PointIndices corresponding to each edge label
|
|
*/
|
|
void
|
|
assignLabelIndices (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
|
|
|
|
struct Neighbor
|
|
{
|
|
Neighbor (int dx, int dy, int didx)
|
|
: d_x (dx)
|
|
, d_y (dy)
|
|
, d_index (didx)
|
|
{}
|
|
|
|
int d_x;
|
|
int d_y;
|
|
int d_index; // = dy * width + dx: pre-calculated
|
|
};
|
|
|
|
/** \brief The tolerance in meters for the relative difference in depth values between neighboring points
|
|
* (The default value is set for .02 meters and is adapted with respect to depth value linearly.
|
|
* e.g. If a point has a depth (z) value of 2.0 meters, a neighboring point is discontinuous if its depth differs by > 2.0 * th. */
|
|
float th_depth_discon_;
|
|
|
|
/** \brief The max search distance for deciding occluding and occluded edges */
|
|
int max_search_neighbors_;
|
|
|
|
/** \brief The bit encoded value that represents edge types to detect */
|
|
int detecting_edge_types_;
|
|
};
|
|
|
|
template <typename PointT, typename PointLT>
|
|
class OrganizedEdgeFromRGB : virtual public OrganizedEdgeBase<PointT, PointLT>
|
|
{
|
|
using PointCloud = pcl::PointCloud<PointT>;
|
|
using PointCloudPtr = typename PointCloud::Ptr;
|
|
using PointCloudConstPtr = typename PointCloud::ConstPtr;
|
|
|
|
using PointCloudL = pcl::PointCloud<PointLT>;
|
|
using PointCloudLPtr = typename PointCloudL::Ptr;
|
|
using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
|
|
|
|
public:
|
|
using OrganizedEdgeBase<PointT, PointLT>::input_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::indices_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::initCompute;
|
|
using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
|
|
using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
|
|
|
|
/** \brief Constructor for OrganizedEdgeFromRGB */
|
|
OrganizedEdgeFromRGB ()
|
|
: OrganizedEdgeBase<PointT, PointLT> ()
|
|
, th_rgb_canny_low_ (40.0)
|
|
, th_rgb_canny_high_ (100.0)
|
|
{
|
|
this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY);
|
|
}
|
|
|
|
/** \brief Destructor for OrganizedEdgeFromRGB */
|
|
|
|
~OrganizedEdgeFromRGB ()
|
|
{
|
|
}
|
|
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point indices for each edge label
|
|
* \param[out] labels a PointCloud of edge labels
|
|
* \param[out] label_indices a vector of PointIndices corresponding to each edge label
|
|
*/
|
|
void
|
|
compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
|
|
|
|
/** \brief Set the low threshold value for RGB Canny edge detection */
|
|
inline void
|
|
setRGBCannyLowThreshold (const float th)
|
|
{
|
|
th_rgb_canny_low_ = th;
|
|
}
|
|
|
|
/** \brief Get the low threshold value for RGB Canny edge detection */
|
|
inline float
|
|
getRGBCannyLowThreshold () const
|
|
{
|
|
return (th_rgb_canny_low_);
|
|
}
|
|
|
|
/** \brief Set the high threshold value for RGB Canny edge detection */
|
|
inline void
|
|
setRGBCannyHighThreshold (const float th)
|
|
{
|
|
th_rgb_canny_high_ = th;
|
|
}
|
|
|
|
/** \brief Get the high threshold value for RGB Canny edge detection */
|
|
inline float
|
|
getRGBCannyHighThreshold () const
|
|
{
|
|
return (th_rgb_canny_high_);
|
|
}
|
|
|
|
protected:
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge)
|
|
* \param[out] labels a PointCloud of edge labels
|
|
*/
|
|
void
|
|
extractEdges (pcl::PointCloud<PointLT>& labels) const;
|
|
|
|
/** \brief The low threshold value for RGB Canny edge detection (default: 40.0) */
|
|
float th_rgb_canny_low_;
|
|
|
|
/** \brief The high threshold value for RGB Canny edge detection (default: 100.0) */
|
|
float th_rgb_canny_high_;
|
|
};
|
|
|
|
template <typename PointT, typename PointNT, typename PointLT>
|
|
class OrganizedEdgeFromNormals : virtual public OrganizedEdgeBase<PointT, PointLT>
|
|
{
|
|
using PointCloud = pcl::PointCloud<PointT>;
|
|
using PointCloudPtr = typename PointCloud::Ptr;
|
|
using PointCloudConstPtr = typename PointCloud::ConstPtr;
|
|
|
|
using PointCloudN = pcl::PointCloud<PointNT>;
|
|
using PointCloudNPtr = typename PointCloudN::Ptr;
|
|
using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
|
|
|
|
using PointCloudL = pcl::PointCloud<PointLT>;
|
|
using PointCloudLPtr = typename PointCloudL::Ptr;
|
|
using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
|
|
|
|
public:
|
|
using OrganizedEdgeBase<PointT, PointLT>::input_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::indices_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::initCompute;
|
|
using OrganizedEdgeBase<PointT, PointLT>::deinitCompute;
|
|
using OrganizedEdgeBase<PointT, PointLT>::detecting_edge_types_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
|
|
|
|
/** \brief Constructor for OrganizedEdgeFromNormals */
|
|
OrganizedEdgeFromNormals ()
|
|
: OrganizedEdgeBase<PointT, PointLT> ()
|
|
, normals_ ()
|
|
, th_hc_canny_low_ (0.4f)
|
|
, th_hc_canny_high_ (1.1f)
|
|
{
|
|
this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE);
|
|
}
|
|
|
|
/** \brief Destructor for OrganizedEdgeFromNormals */
|
|
|
|
~OrganizedEdgeFromNormals ()
|
|
{
|
|
}
|
|
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assign point indices for each edge label
|
|
* \param[out] labels a PointCloud of edge labels
|
|
* \param[out] label_indices a vector of PointIndices corresponding to each edge label
|
|
*/
|
|
void
|
|
compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
|
|
|
|
/** \brief Provide a pointer to the input normals.
|
|
* \param[in] normals the input normal cloud
|
|
*/
|
|
inline void
|
|
setInputNormals (const PointCloudNConstPtr &normals)
|
|
{
|
|
normals_ = normals;
|
|
}
|
|
|
|
/** \brief Get the input normals. */
|
|
inline PointCloudNConstPtr
|
|
getInputNormals () const
|
|
{
|
|
return (normals_);
|
|
}
|
|
|
|
/** \brief Set the low threshold value for high curvature Canny edge detection */
|
|
inline void
|
|
setHCCannyLowThreshold (const float th)
|
|
{
|
|
th_hc_canny_low_ = th;
|
|
}
|
|
|
|
/** \brief Get the low threshold value for high curvature Canny edge detection */
|
|
inline float
|
|
getHCCannyLowThreshold () const
|
|
{
|
|
return (th_hc_canny_low_);
|
|
}
|
|
|
|
/** \brief Set the high threshold value for high curvature Canny edge detection */
|
|
inline void
|
|
setHCCannyHighThreshold (const float th)
|
|
{
|
|
th_hc_canny_high_ = th;
|
|
}
|
|
|
|
/** \brief Get the high threshold value for high curvature Canny edge detection */
|
|
inline float
|
|
getHCCannyHighThreshold () const
|
|
{
|
|
return (th_hc_canny_high_);
|
|
}
|
|
|
|
protected:
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions)
|
|
* \param[out] labels a PointCloud of edge labels
|
|
*/
|
|
void
|
|
extractEdges (pcl::PointCloud<PointLT>& labels) const;
|
|
|
|
/** \brief A pointer to the input normals */
|
|
PointCloudNConstPtr normals_;
|
|
|
|
/** \brief The low threshold value for high curvature Canny edge detection (default: 0.4) */
|
|
float th_hc_canny_low_;
|
|
|
|
/** \brief The high threshold value for high curvature Canny edge detection (default: 1.1) */
|
|
float th_hc_canny_high_;
|
|
};
|
|
|
|
template <typename PointT, typename PointNT, typename PointLT>
|
|
class OrganizedEdgeFromRGBNormals : public OrganizedEdgeFromRGB<PointT, PointLT>, public OrganizedEdgeFromNormals<PointT, PointNT, PointLT>
|
|
{
|
|
using PointCloud = pcl::PointCloud<PointT>;
|
|
using PointCloudPtr = typename PointCloud::Ptr;
|
|
using PointCloudConstPtr = typename PointCloud::ConstPtr;
|
|
|
|
using PointCloudN = pcl::PointCloud<PointNT>;
|
|
using PointCloudNPtr = typename PointCloudN::Ptr;
|
|
using PointCloudNConstPtr = typename PointCloudN::ConstPtr;
|
|
|
|
using PointCloudL = pcl::PointCloud<PointLT>;
|
|
using PointCloudLPtr = typename PointCloudL::Ptr;
|
|
using PointCloudLConstPtr = typename PointCloudL::ConstPtr;
|
|
|
|
public:
|
|
using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::input_;
|
|
using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::indices_;
|
|
using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::initCompute;
|
|
using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::deinitCompute;
|
|
using OrganizedEdgeFromNormals<PointT, PointNT, PointLT>::detecting_edge_types_;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_NAN_BOUNDARY;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDING;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_OCCLUDED;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_HIGH_CURVATURE;
|
|
using OrganizedEdgeBase<PointT, PointLT>::EDGELABEL_RGB_CANNY;
|
|
|
|
/** \brief Constructor for OrganizedEdgeFromRGBNormals */
|
|
OrganizedEdgeFromRGBNormals ()
|
|
: OrganizedEdgeFromRGB<PointT, PointLT> ()
|
|
, OrganizedEdgeFromNormals<PointT, PointNT, PointLT> ()
|
|
{
|
|
this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY | EDGELABEL_HIGH_CURVATURE);
|
|
}
|
|
|
|
/** \brief Destructor for OrganizedEdgeFromRGBNormals */
|
|
|
|
~OrganizedEdgeFromRGBNormals ()
|
|
{
|
|
}
|
|
|
|
/** \brief Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature regions) and assign point indices for each edge label
|
|
* \param[out] labels a PointCloud of edge labels
|
|
* \param[out] label_indices a vector of PointIndices corresponding to each edge label
|
|
*/
|
|
void
|
|
compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const;
|
|
};
|
|
}
|
|
|
|
#ifdef PCL_NO_PRECOMPILE
|
|
#include <pcl/features/impl/organized_edge_detection.hpp>
|
|
#endif
|