edge.hpp
/*
* 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.
*
*/
#ifndef PCL_2D_EDGE_IMPL_HPP
#define PCL_2D_EDGE_IMPL_HPP
#include <pcl/2d/convolution.h>
#include <pcl/common/common_headers.h> // rad2deg()
#include <pcl/console/time.h>
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::detectEdgeSobel (
pcl::PointCloud<PointOutT> &output)
{
//pcl::console::TicToc tt;
//tt.tic ();
convolution_.setInputCloud (input_);
pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
kernel_.fetchKernel (*kernel_x);
convolution_.setKernel (*kernel_x);
convolution_.filter (*magnitude_x);
//PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic ();
pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
kernel_.fetchKernel (*kernel_y);
convolution_.setKernel (*kernel_y);
convolution_.filter (*magnitude_y);
//PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic ();
const int height = input_->height;
const int width = input_->width;
output.resize (height * width);
output.height = height;
output.width = width;
for (size_t i = 0; i < output.size (); ++i)
{
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
(*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
output[i].direction =
atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
}
//PCL_ERROR ("Rest: %g\n", tt.toc ());
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::sobelMagnitudeDirection (
const pcl::PointCloud<PointInT> &input_x,
const pcl::PointCloud<PointInT> &input_y,
pcl::PointCloud<PointOutT> &output)
{
convolution_.setInputCloud (input_x.makeShared());
pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::SOBEL_X);
kernel_.fetchKernel (*kernel_x);
convolution_.setKernel (*kernel_x);
convolution_.filter (*magnitude_x);
convolution_.setInputCloud (input_y.makeShared());
pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::SOBEL_Y);
kernel_.fetchKernel (*kernel_y);
convolution_.setKernel (*kernel_y);
convolution_.filter (*magnitude_y);
const int height = input_x.height;
const int width = input_x.width;
output.resize (height * width);
output.height = height;
output.width = width;
for (size_t i = 0; i < output.size (); ++i)
{
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
(*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
output[i].direction =
atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
}
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::detectEdgePrewitt (pcl::PointCloud<PointOutT> &output)
{
convolution_.setInputCloud (input_);
pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::PREWITT_X);
kernel_.fetchKernel (*kernel_x);
convolution_.setKernel (*kernel_x);
convolution_.filter (*magnitude_x);
pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::PREWITT_Y);
kernel_.fetchKernel (*kernel_y);
convolution_.setKernel (*kernel_y);
convolution_.filter (*magnitude_y);
const int height = input_->height;
const int width = input_->width;
output.resize (height * width);
output.height = height;
output.width = width;
for (size_t i = 0; i < output.size (); ++i)
{
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
(*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
output[i].direction =
atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
}
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::detectEdgeRoberts (pcl::PointCloud<PointOutT> &output)
{
convolution_.setInputCloud (input_);
pcl::PointCloud<PointXYZI>::Ptr kernel_x (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_x (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_X);
kernel_.fetchKernel (*kernel_x);
convolution_.setKernel (*kernel_x);
convolution_.filter (*magnitude_x);
pcl::PointCloud<PointXYZI>::Ptr kernel_y (new pcl::PointCloud<PointXYZI>);
pcl::PointCloud<PointXYZI>::Ptr magnitude_y (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::ROBERTS_Y);
kernel_.fetchKernel (*kernel_y);
convolution_.setKernel (*kernel_y);
convolution_.filter (*magnitude_y);
const int height = input_->height;
const int width = input_->width;
output.resize (height * width);
output.height = height;
output.width = width;
for (size_t i = 0; i < output.size (); ++i)
{
output[i].magnitude_x = (*magnitude_x)[i].intensity;
output[i].magnitude_y = (*magnitude_y)[i].intensity;
output[i].magnitude =
std::sqrt ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity +
(*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity);
output[i].direction =
atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity);
}
}
//////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::cannyTraceEdge (
int rowOffset, int colOffset, int row, int col,
pcl::PointCloud<PointXYZI> &maxima)
{
int newRow = row + rowOffset;
int newCol = col + colOffset;
PointXYZI &pt = maxima (newCol, newRow);
if (newRow > 0 && newRow < static_cast<int> (maxima.height) && newCol > 0 && newCol < static_cast<int> (maxima.width))
{
if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits<float>::max ())
return;
pt.intensity = std::numeric_limits<float>::max ();
cannyTraceEdge ( 1, 0, newRow, newCol, maxima);
cannyTraceEdge (-1, 0, newRow, newCol, maxima);
cannyTraceEdge ( 1, 1, newRow, newCol, maxima);
cannyTraceEdge (-1, -1, newRow, newCol, maxima);
cannyTraceEdge ( 0, -1, newRow, newCol, maxima);
cannyTraceEdge ( 0, 1, newRow, newCol, maxima);
cannyTraceEdge (-1, 1, newRow, newCol, maxima);
cannyTraceEdge ( 1, -1, newRow, newCol, maxima);
}
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::discretizeAngles (pcl::PointCloud<PointOutT> &thet)
{
const int height = thet.height;
const int width = thet.width;
float angle;
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
angle = pcl::rad2deg (thet (j, i).direction);
if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5))
thet (j, i).direction = 0;
else
if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5)))
thet (j, i).direction = 45;
else
if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5)))
thet (j, i).direction = 90;
else
if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5)))
thet (j, i).direction = 135;
}
}
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::suppressNonMaxima (
const pcl::PointCloud<PointXYZIEdge> &edges,
pcl::PointCloud<PointXYZI> &maxima, float tLow)
{
const int height = edges.height;
const int width = edges.width;
maxima.height = height;
maxima.width = width;
maxima.resize (height * width);
for (size_t i = 0; i < maxima.size (); ++i)
maxima[i].intensity = 0.0f;
// tHigh and non-maximal supression
for (int i = 1; i < height - 1; i++)
{
for (int j = 1; j < width - 1; j++)
{
const PointXYZIEdge &ptedge = edges (j, i);
PointXYZI &ptmax = maxima (j, i);
if (ptedge.magnitude < tLow)
continue;
//maxima (j, i).intensity = 0;
switch (int (ptedge.direction))
{
case 0:
{
if (ptedge.magnitude >= edges (j - 1, i).magnitude &&
ptedge.magnitude >= edges (j + 1, i).magnitude)
ptmax.intensity = ptedge.magnitude;
break;
}
case 45:
{
if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude &&
ptedge.magnitude >= edges (j + 1, i + 1).magnitude)
ptmax.intensity = ptedge.magnitude;
break;
}
case 90:
{
if (ptedge.magnitude >= edges (j, i - 1).magnitude &&
ptedge.magnitude >= edges (j, i + 1).magnitude)
ptmax.intensity = ptedge.magnitude;
break;
}
case 135:
{
if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude &&
ptedge.magnitude >= edges (j - 1, i + 1).magnitude)
ptmax.intensity = ptedge.magnitude;
break;
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::detectEdgeCanny (pcl::PointCloud<PointOutT> &output)
{
float tHigh = hysteresis_threshold_high_;
float tLow = hysteresis_threshold_low_;
const int height = input_->height;
const int width = input_->width;
output.resize (height * width);
output.height = height;
output.width = width;
//pcl::console::TicToc tt;
//tt.tic ();
// Noise reduction using gaussian blurring
pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
PointCloudInPtr smoothed_cloud (new PointCloudIn);
kernel_.setKernelSize (3);
kernel_.setKernelSigma (1.0);
kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
kernel_.fetchKernel (*gaussian_kernel);
convolution_.setKernel (*gaussian_kernel);
convolution_.setInputCloud (input_);
convolution_.filter (*smoothed_cloud);
//PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic ();
// Edge detection usign Sobel
pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
setInputCloud (smoothed_cloud);
detectEdgeSobel (*edges);
//PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic ();
// Edge discretization
discretizeAngles (*edges);
//PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic ();
// tHigh and non-maximal supression
pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
suppressNonMaxima (*edges, *maxima, tLow);
//PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic ();
// Edge tracing
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
continue;
(*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
cannyTraceEdge ( 1, 0, i, j, *maxima);
cannyTraceEdge (-1, 0, i, j, *maxima);
cannyTraceEdge ( 1, 1, i, j, *maxima);
cannyTraceEdge (-1, -1, i, j, *maxima);
cannyTraceEdge ( 0, -1, i, j, *maxima);
cannyTraceEdge ( 0, 1, i, j, *maxima);
cannyTraceEdge (-1, 1, i, j, *maxima);
cannyTraceEdge ( 1, -1, i, j, *maxima);
}
}
//PCL_ERROR ("Edge tracing: %g\n", tt.toc ());
// Final thresholding
for (size_t i = 0; i < input_->size (); ++i)
{
if ((*maxima)[i].intensity == std::numeric_limits<float>::max ())
output[i].magnitude = 255;
else
output[i].magnitude = 0;
}
}
//////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::canny (
const pcl::PointCloud<PointInT> &input_x,
const pcl::PointCloud<PointInT> &input_y,
pcl::PointCloud<PointOutT> &output)
{
float tHigh = hysteresis_threshold_high_;
float tLow = hysteresis_threshold_low_;
const int height = input_x.height;
const int width = input_x.width;
output.resize (height * width);
output.height = height;
output.width = width;
// Noise reduction using gaussian blurring
pcl::PointCloud<PointXYZI>::Ptr gaussian_kernel (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelSize (3);
kernel_.setKernelSigma (1.0);
kernel_.setKernelType (kernel<PointXYZI>::GAUSSIAN);
kernel_.fetchKernel (*gaussian_kernel);
convolution_.setKernel (*gaussian_kernel);
PointCloudIn smoothed_cloud_x;
convolution_.setInputCloud (input_x.makeShared());
convolution_.filter (smoothed_cloud_x);
PointCloudIn smoothed_cloud_y;
convolution_.setInputCloud (input_y.makeShared());
convolution_.filter (smoothed_cloud_y);
// Edge detection usign Sobel
pcl::PointCloud<PointXYZIEdge>::Ptr edges (new pcl::PointCloud<PointXYZIEdge>);
sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ());
// Edge discretization
discretizeAngles (*edges);
pcl::PointCloud<PointXYZI>::Ptr maxima (new pcl::PointCloud<PointXYZI>);
suppressNonMaxima (*edges, *maxima, tLow);
// Edge tracing
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
continue;
(*maxima)(j, i).intensity = std::numeric_limits<float>::max ();
cannyTraceEdge ( 1, 0, i, j, *maxima);
cannyTraceEdge (-1, 0, i, j, *maxima);
cannyTraceEdge ( 1, 1, i, j, *maxima);
cannyTraceEdge (-1, -1, i, j, *maxima);
cannyTraceEdge ( 0, -1, i, j, *maxima);
cannyTraceEdge ( 0, 1, i, j, *maxima);
cannyTraceEdge (-1, 1, i, j, *maxima);
cannyTraceEdge ( 1, -1, i, j, *maxima);
}
}
// Final thresholding
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
if ((*maxima)(j, i).intensity == std::numeric_limits<float>::max ())
output (j, i).magnitude = 255;
else
output (j, i).magnitude = 0;
}
}
}
//////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointOutT> void
pcl::Edge<PointInT, PointOutT>::detectEdgeLoG (
const float kernel_sigma, const float kernel_size,
pcl::PointCloud<PointOutT> &output)
{
convolution_.setInputCloud (input_);
pcl::PointCloud<PointXYZI>::Ptr log_kernel (new pcl::PointCloud<PointXYZI>);
kernel_.setKernelType (kernel<PointXYZI>::LOG);
kernel_.setKernelSigma (kernel_sigma);
kernel_.setKernelSize (kernel_size);
kernel_.fetchKernel (*log_kernel);
convolution_.setKernel (*log_kernel);
convolution_.filter (output);
}
#endif
pcl::PointXYZIEdge
This typedef is used to represent a point cloud containing edge information.
Definition: convolution.h:50
pcl::kernel
Definition: kernel.h:45
pcl::rad2deg
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
pcl::_PointXYZI::intensity
float intensity
Definition: point_types.hpp:443
pcl::Edge
Definition: edge.h:48
pcl::PointCloud::height
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::PointXYZI
Definition: point_types.hpp:451
pcl::Edge::sobelMagnitudeDirection
void sobelMagnitudeDirection(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Definition: edge.hpp:91
pcl::PointCloud::width
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::Edge::detectEdgePrewitt
void detectEdgePrewitt(pcl::PointCloud< PointOutT > &output)
Uses the Prewitt kernel for edge detection.
Definition: edge.hpp:133
pcl::Edge::canny
void canny(const pcl::PointCloud< PointInT > &input_x, const pcl::PointCloud< PointInT > &input_y, pcl::PointCloud< PointOutT > &output)
Perform Canny edge detection with two separated input images for horizontal and vertical derivatives...
Definition: edge.hpp:403
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::Edge::detectEdgeSobel
void detectEdgeSobel(pcl::PointCloud< PointOutT > &output)
Uses the Sobel kernel for edge detection.
Definition: edge.hpp:47
pcl::Edge::detectEdgeLoG
void detectEdgeLoG(const float kernel_sigma, const float kernel_size, pcl::PointCloud< PointOutT > &output)
Uses the LoG kernel for edge detection.
Definition: edge.hpp:479
pcl::PointXYZIEdge::direction
float direction
Definition: convolution.h:54
pcl::PointCloud::makeShared
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
Definition: point_cloud.h:588
pcl::PointCloud::resize
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:455
pcl::PointXYZIEdge::magnitude
float magnitude
Definition: convolution.h:53
pcl::Edge::detectEdgeCanny
void detectEdgeCanny(pcl::PointCloud< PointOutT > &output)
All edges of magnitude above t_high are always classified as edges.
Definition: edge.hpp:329
pcl::Edge::detectEdgeRoberts
void detectEdgeRoberts(pcl::PointCloud< PointOutT > &output)
Uses the Roberts kernel for edge detection.
Definition: edge.hpp:172
pcl::PointCloud::size
size_t size() const
Definition: point_cloud.h:448