From a0c847d187aeb1064551c9452e0571a93d6433dc Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Fri, 17 Jan 2014 14:50:03 +0100 Subject: [PATCH 1/2] New: 3D implementation of SUSAN keypoint detector Update: Make cases for when the cloud is organized and when not --- keypoints/CMakeLists.txt | 2 + .../include/pcl/keypoints/impl/susan_3d.hpp | 509 ++++++++++++++++++ keypoints/include/pcl/keypoints/susan_3d.h | 238 ++++++++ 3 files changed, 749 insertions(+) create mode 100644 keypoints/include/pcl/keypoints/impl/susan_3d.hpp create mode 100644 keypoints/include/pcl/keypoints/susan_3d.h diff --git a/keypoints/CMakeLists.txt b/keypoints/CMakeLists.txt index 17f84dc4f57..38d81c33bab 100644 --- a/keypoints/CMakeLists.txt +++ b/keypoints/CMakeLists.txt @@ -33,6 +33,7 @@ if(build) include/pcl/${SUBSYS_NAME}/susan.h include/pcl/${SUBSYS_NAME}/iss_3d.h include/pcl/${SUBSYS_NAME}/brisk_2d.h + include/pcl/${SUBSYS_NAME}/susan_3d.h ) set(impl_incs include/pcl/${SUBSYS_NAME}/impl/keypoint.hpp @@ -45,6 +46,7 @@ if(build) include/pcl/${SUBSYS_NAME}/impl/susan.hpp include/pcl/${SUBSYS_NAME}/impl/iss_3d.hpp include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp + include/pcl/${SUBSYS_NAME}/impl/susan_3d.hpp ) set(LIB_NAME pcl_${SUBSYS_NAME}) diff --git a/keypoints/include/pcl/keypoints/impl/susan_3d.hpp b/keypoints/include/pcl/keypoints/impl/susan_3d.hpp new file mode 100644 index 00000000000..1d1943d5e06 --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/susan_3d.hpp @@ -0,0 +1,509 @@ +/* + * 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. + * + */ + +#ifndef PCL_SUSAN_KEYPOINT_3D_IMPL_HPP_ +#define PCL_SUSAN_KEYPOINT_3D_IMPL_HPP_ + +#include +#include +#include + +///////////////////////////////////////////////////////////////////////////////////////////// +template inline const NormalT& +pcl::SusanKeypoint3D::getNormalOrNull (std::size_t pos, int& counter) const +{ + static const NormalT null; + if (!isFinite (normals_->points[pos])) return (null); + ++counter; + return (normals_->points[pos]); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template inline const NormalT& +pcl::SusanKeypoint3D::getNormalOrNull (int u, int v, int& counter) const +{ + static const NormalT null; + const NormalT& n = (*normals_) (u, v); + if (!isFinite (n)) return (null); + ++counter; + return (n); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template inline float +pcl::SusanKeypoint3D::normalsDiff (const NormalT& a, const NormalT& b) const +{ + return (a.normal_x*b.normal_x + a.normal_y*b.normal_y + a.normal_z*b.normal_z); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint3D::lineToCentroid (int x1, int y1, std::vector& points) +{ + static int x0 = half_window_size_; + static int y0 = half_window_size_; + points.reserve (window_size_ * window_size_); + int dx = abs (x1-x0); + int dy = abs (y1-y0); + int sx = (x0 < x1) ? 1 : -1; + int sy = (y0 < y1) ? 1 : -1; + int err = dx - dy; + + while (true) + { + if ((x0 > 0) && (y0 > 0) && (x0 < window_size_) && (y0 < window_size_)) + points.push_back (Eigen::Vector2i (x0, y0)); + if ((x0 == x1) && (y0 == y1)) + break; + int e2 = 2 * err; + if (e2 > -dy) + { + err-= dy; + x0 += sx; + } + + if ((x0 == x1) && (y0 == y1)) + { + if ((x0 > 0) && (y0 > 0) && (x0 < window_size_) && (y0 < window_size_)) + points.push_back (Eigen::Vector2i (x0, y0)); + break; + } + + if (e2 < dx) + { + err+= dx; + y0+= sy; + } + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SusanKeypoint3D::initCompute () +{ + if (!input_->isOrganized ()) + { + if (!Keypoint::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] init failed!\n", name_.c_str ()); + return (false); + } + + if (!normals_) + { + NormalsPtr normals (new Normals ()); + pcl::NormalEstimation normal_estimation; + normal_estimation.setInputCloud (surface_); + normal_estimation.setRadiusSearch (search_radius_); + normal_estimation.compute (*normals); + normals_ = normals; + } + } + else + { + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be at least 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + if (!surface_) + surface_ = input_; + + if (!normals_) + { + NormalsPtr normals (new Normals ()); + IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (surface_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + normals_ = normals; + } + + if (test_distance_ && (distance_threshold_ < 0.01)) + { + PCL_WARN ("[pcl::%s::initCompute] Distance threshold metric is in pixels. Please consider higher value 0.01 for instance.\n", name_.c_str ()); + } + } + + if (normals_->size () != surface_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals %d does not match the number of input points %d!\n", name_.c_str (), normals_->size (), input_->size ()); + return (false); + } + + threshold_ = cos (angular_threshold_); + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + if (input_->isOrganized ()) + detectKeypointsOrganized (output); + else + detectKeypointsNonOrganized (output); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint3D::detectKeypointsNonOrganized (PointCloudOut &output) +{ + typename PointCloudOut::Ptr response (new PointCloudOut); + response->resize (input_->size ()); + + const int input_size (input_->size ()); +#if defined (HAVE_OPENMP) && (defined (_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))) +#pragma omp parallel for num_threads (threads_) +#endif + for (int point_index = 0; point_index < input_size; ++point_index) + { + const PointInT& point = input_->points [point_index]; + const NormalT& normal = normals_->points [point_index]; + response->points [point_index].getVector3fMap () = point.getVector3fMap (); + response->points [point_index].intensity = 0; + if (!isFinite (point)) + continue; + if (!isFinite (normal)) + continue; + const Eigen::Vector3f &nucleus = point.getVector3fMap (); + const Eigen::Vector3f &nucleus_normal = normals_->points [point_index].getNormalVector3fMap (); + int counter = 0; + std::vector nn_indices; + std::vector nn_dists; + if (searchForNeighbors (point_index, search_radius_, nn_indices, nn_dists) <= 1) + continue; + + float area = 0; + Eigen::Vector3f centroid = Eigen::Vector3f::Zero (); + std::vector usan (nn_indices.size (), false); + std::vector::iterator usan_it = usan.begin (); + for (std::vector::const_iterator index = nn_indices.begin (); index != nn_indices.end (); ++index, ++usan_it) + { + if (point_index == *index) + continue; + + const NormalT &n = getNormalOrNull (*index, counter); + if (normalsDiff (normal, n) >= threshold_) + { + *usan_it = true; + area+= 1; + centroid += input_->points[*index].getVector3fMap (); + } + } + + if (area > 0) + { + centroid /= area; + float geometric_threshold = 0.25 * counter; + if (area < geometric_threshold) + { + response->points [point_index].intensity = 1.f / (geometric_threshold - area); + // Remove false positives + // If the centroid is not far enough then it is not a corner + if (test_distance_) + { + Eigen::Vector3f nc = centroid - nucleus; + float nc_sq_norm = nc.squaredNorm (); + float nc_norm = sqrt (nc_sq_norm); + if (nc_norm <= distance_threshold_) + { + response->points [point_index].intensity = 0; + continue; + } + + if (test_contiguity_) + { + std::vector::const_iterator const_usan_it = usan.begin (); + // All the points on the line from the nucleus to the centroid must be part of the USAN + for (std::vector::const_iterator index = nn_indices.begin (); + index != nn_indices.end (); + ++index, ++usan_it) + { + // skip if me + if (point_index == *index) + continue; + // skip if not in USAN + if (!(*const_usan_it)) + continue; + Eigen::Vector3f np = input_->points[*index].getVector3fMap () - nucleus; + // check if the point is between the nucleus and the centroid + float r = np.dot (nc) / nc_sq_norm; + if ((r > 0) && (r < 1)) + continue; + else + { + response->points [point_index].intensity = 0; + break; + } + } + } + } + } + } + } + + if (!nonmax_) + { + output = *response; + output.is_dense = response->is_dense; + for (size_t i = 0; i < response->size (); ++i) + keypoints_indices_->indices.push_back (i); + } + else + { + output.points.clear (); + output.points.reserve (response->size ()); + +#if defined (HAVE_OPENMP) && (defined (_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))) +#pragma omp parallel for shared (output) num_threads (threads_) +#endif + for (int idx = 0; idx < input_size; ++idx) + { + const PointOutT& point = response->points [idx]; + const NormalT& normal = normals_->points [idx]; + const float &intensity = response->points[idx].intensity; + if (!isFinite (point) || !isFinite (normal) || (intensity == 0)) + continue; + std::vector nn_indices; + std::vector nn_dists; + searchForNeighbors (idx, search_radius_, nn_indices, nn_dists); + bool is_maxima = true; + for (std::vector::const_iterator neighbor = nn_indices.begin (); neighbor != nn_indices.end (); ++neighbor) + { + if (intensity < response->points[*neighbor].intensity) + { + is_maxima = false; + break; + } + } + if (is_maxima) +#if defined (HAVE_OPENMP) && (defined (_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))) +#pragma omp critical +#endif + { + output.points.push_back (response->points[idx]); + keypoints_indices_->indices.push_back (idx); + } + } + + output.height = 1; + output.width = static_cast (output.points.size ()); + output.is_dense = true; + } +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint3D::detectKeypointsOrganized (PointCloudOut &output) +{ + pcl::PointCloud::Ptr response_ (new pcl::PointCloud (input_->width, input_->height)); + pcl::PointCloud &response = *response_; + + int w = static_cast (input_->width) - half_window_size_; + int h = static_cast (input_->height) - half_window_size_; + const float geometric_threshold = 0.25 * window_size_ * window_size_; +#if defined (HAVE_OPENMP) && (defined (_WIN32) || ((__GNUC__ > 4) && (__GNUC_MINOR__ > 2))) +#pragma omp parallel for num_threads (threads_) +#endif + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + (*response_) (i, j) = 0; + if (!pcl::isFinite ((*input_) (i,j))) + continue; + + const NormalT& normal = (*normals_) (i,j); + if (!pcl::isFinite (normal)) + continue; + + Eigen::Vector2f nucleus (i, j); + Eigen::Vector2f centroid = Eigen::Vector2f::Zero (); + Eigen::Array usan (window_size_, window_size_); + usan.setZero (); + + int area = 0; + int counter = 0; + for (int y = 0, jj = j - half_window_size_; y < window_size_; ++jj, ++y) + for (int x = 0, ii = i - half_window_size_; x < window_size_; ++ii, ++x) + { + const NormalT& n = getNormalOrNull (ii,jj,counter); + if (normalsDiff (normal, n) >= threshold_) + { + usan (x, y) = true; + ++area; + centroid[0] += static_cast (ii); + centroid[1] += static_cast (jj); + } + } + + if (counter == 0) + continue; + + if (area >= geometric_threshold) + continue; + + response (i, j) = 1.f / (geometric_threshold - area); + if (!test_distance_) + continue; + + centroid /= area; + Eigen::Vector2f nc = centroid - nucleus; + if (nc.norm () <= distance_threshold_) + { + response (i,j) = 0; + continue; + } + + if (!test_contiguity_) + continue; + + // All the points on the line from the nucleus to the centroid must be part of the USAN + std::vector nucleus_center; + float shifted_x = half_window_size_ + centroid[0] - nucleus[0]; + float shifted_y = half_window_size_ + centroid[1] - nucleus[1]; + lineToCentroid (shifted_x, shifted_y, nucleus_center); + bool all_usan_on_line = true; + for (std::vector::const_iterator p = nucleus_center.begin (); + p != nucleus_center.end (); + ++p) + { + if (usan ((*p)[0], (*p)[1]) == false) + { + all_usan_on_line = false; + break; + } + } + if (!all_usan_on_line) + { + response (i,j) = 0; + continue; + } + } + } + + if (!nonmax_) + { + output = PointCloudOut (input_->width, input_->height); + output.is_dense = response_->is_dense; + keypoints_indices_->indices.resize (input_->size ()); +#ifdef _OPENMP +#pragma omp parallel for num_threads (threads_) +#endif + for (size_t i = 0; i < response_->size (); ++i) + { + keypoints_indices_->indices[i] = i; + output.points[i].getVector3fMap () = input_->points[i].getVector3fMap (); + output.points[i].intensity = response_->points[i]; + } + } + else + { + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&SusanKeypoint3D::greaterCornernessAtIndices, this, _1, _2, response_)); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + const int occupency_map_size (indices.size ()); + int nb_occupied = 0; + +#ifdef _OPENMP +#pragma omp parallel for shared (output, keypoints_indices_) num_threads (threads_) +#endif + for (int i = 0; i < indices.size () && nb_occupied < input_->size (); ++i) + { + int idx = indices[i]; + if ((response_->points[idx] == 0) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + { + occupency_map[v*width + u] = true; + ++nb_occupied; + } + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = input_->is_dense; + } +} + +#define PCL_INSTANTIATE_SUSAN_3D (T,U,N) template class PCL_EXPORTS pcl::SusanKeypoint3D; +#endif // #ifndef PCL_SUSAN_KEYPOINT_3D_IMPL_H_ diff --git a/keypoints/include/pcl/keypoints/susan_3d.h b/keypoints/include/pcl/keypoints/susan_3d.h new file mode 100644 index 00000000000..8239484e6a7 --- /dev/null +++ b/keypoints/include/pcl/keypoints/susan_3d.h @@ -0,0 +1,238 @@ +/* + * 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. + * + */ + +#ifndef PCL_SUSAN_KEYPOINT_3D_H_ +#define PCL_SUSAN_KEYPOINT_3D_H_ + +#include + +namespace pcl +{ + /** \brief SusanKeypoint3D exploitss the idea behind SUSAN detector, but instead of relying + * on image intensity, it uses surface normals. + * It should be faster than HarrisKeypoint3D since it computes first order statistics. + * Behaviour is slightly different when input cloud is organized and not. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template + class SusanKeypoint3D : public Keypoint + { + public: + typedef typename Keypoint::PointCloudIn PointCloudIn; + typedef typename Keypoint::PointCloudOut PointCloudOut; + typedef typename Keypoint::KdTree KdTree; + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef typename pcl::PointCloud Normals; + typedef typename Normals::Ptr NormalsPtr; + typedef typename Normals::ConstPtr NormalsConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::surface_; + using Keypoint::keypoints_indices_; + using Keypoint::search_radius_; + using Keypoint::searchForNeighbors; + using Keypoint::initCompute; + + /** \brief Constructor + * \param[in] radius the radius for normal estimation as well as for non maxima + * suppression, default 0.01m. If cloud is organized set the window size instead. + * \param[in] distance to test if the nucleus is far enough from the centroid, + * default 0.001. If your cloud is organized distance will be in pixels. + * \param[in] angle to test if normals are parallel in radians. Good value should be + * between 5 and 20 degrees, default 20 degrees. + */ + SusanKeypoint3D (float radius = 0.01f, float distance = 0.001f, float angle = M_PI / 9) + : distance_threshold_ (distance) + , angular_threshold_ (angle) + , threads_ (1) + { + name_ = "SusanKeypoint3D"; + search_radius_ = radius; + window_size_ = 7; + test_distance_ = true; + test_contiguity_ = false; + nonmax_ = true; + } + + /** \brief set the radius for normal estimation and non maxima supression. + * \param[in] radius + */ + void + setRadius (float radius) { search_radius_ = radius; } + + /// \brief Set window size, effective only if cloud is organized, default 7 + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width and height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief Set the minimal distance between nucleus and centroid to reject false + * positive. This is only evaluated if distance test is turned on. + * \param[in] distance minimal distance between centroid and nucleus. + */ + void + setDistanceThreshold (float distance) { distance_threshold_ = distance; } + + /// \brief \return distance threshold + float + getDistanceThreshold () const { return (distance_threshold_); } + + /** \brief set the angular threshold value for detecting corners. Normals are + * considered as parallel if angle (Ni, Nj) <= angular_threshold. + * \param[in] angular_threshold + */ + void + setAngularThreshold (float angle) { angular_threshold_ = angle; } + + /// \brief \return angular threshold + float + getAngularThreshold () const { return (angular_threshold_); } + + /** \brief set normals if precalculated normals are available. + * \param normals + */ + void + setNormals (const NormalsConstPtr &normals) { normals_.reset (normals); } + + /// \brief \return points normals as calculated or given + inline void + getNormals (const NormalsConstPtr &normals) const { return (normals_); } + + virtual void + setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; normals_.reset (); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use (0 sets the value back to + * automatic), default 1. + */ + void + setNumberOfThreads (int nr_threads) { threads_ = (nr_threads < 0) ? 1 : nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + /** \brief test that a point in the USAN is part of the [nucleus centroid] to filter + * out false positives. I recommend not setting it to true unless cloud is organized + * or really dense, default is false. + * \param[in] test_contiguity whether or not to test for continguity + */ + void + setTestForContiguity (bool test_contiguity) { test_contiguity_ = test_contiguity; } + + /** \brief test if the nucleus and the centrooid are far enough to filter out false + * positives, default true. + * \param[in] test_distance whether or not to test for distance + */ + void + setTestForDistance (bool test_distance) { test_distance_ = test_distance; } + + protected: + bool + initCompute (); + + void + detectKeypoints (PointCloudOut &output); + + private: + /// detect keypoints on organized input + void + detectKeypointsOrganized (PointCloudOut &output); + /// detect keypoints on non organized input + void + detectKeypointsNonOrganized (PointCloudOut &output); + /** \return a const reference to the normal at (i,j) if finite else return a reference + * to a null normal. + * If the returned normal is valid \a counter is incremented. + */ + inline const NormalT& + getNormalOrNull (std::size_t pos, int& counter) const; + /** \return a const reference to the normal at (i,j) if finite else return a reference + * to a null normal. + * If the returned normal is valid \a counter is incremented. + */ + inline const NormalT& + getNormalOrNull (int u, int v, int& counter) const; + /// \return difference of two normals vectors + inline float + normalsDiff (const NormalT& a, const NormalT& b) const; + /** get pixels indices that form a line segment between nucleus and centroid using + * Bresenheim algorithm within the search window. + * param[in] centroid_x the centroid coordinate on x axis + * param[in] centroid_y the centroid coordinate on y axis + * param[out] points indices of points lying on [nucleus centroid] segment + */ + void + lineToCentroid (int centroid_x, int centroid_y, std::vector& points); + /// comparator for responses intensity + inline bool + greaterCornernessAtIndices (int a, int b, const pcl::PointCloud::ConstPtr &response) const + { + return (response->points [a] > response->points [b]); + } + /// Window size + int window_size_; + /// half window size + int half_window_size_; + /// nucleus to centroid distance threshold + float distance_threshold_; + /// angular threshold between normals + float angular_threshold_; + /// threshold computed from angular_threshold_ + float threshold_; + /// pointer to normals + NormalsConstPtr normals_; + /// number of threads + int threads_; + /// test for distance between centroid and nucleus + bool test_distance_; + /// test for contiguity inside USAN + bool test_contiguity_; + /// non maximal suppression + bool nonmax_; + }; +} + +#include + +#endif // #ifndef PCL_SUSAN_KEYPOINT_3D_H_ From 1fa49e21e4c18cb2e19db581f712b151303d79bd Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Sun, 19 Jan 2014 18:35:08 +0100 Subject: [PATCH 2/2] New: add 2D version of SUSAN keypoint detector Update: various fixes and enhancements --- keypoints/CMakeLists.txt | 6 +- .../include/pcl/keypoints/impl/susan_2d.hpp | 247 ++++++++++++++++++ keypoints/include/pcl/keypoints/susan_2d.h | 190 ++++++++++++++ 3 files changed, 441 insertions(+), 2 deletions(-) create mode 100644 keypoints/include/pcl/keypoints/impl/susan_2d.hpp create mode 100644 keypoints/include/pcl/keypoints/susan_2d.h diff --git a/keypoints/CMakeLists.txt b/keypoints/CMakeLists.txt index 38d81c33bab..91e43d39e13 100644 --- a/keypoints/CMakeLists.txt +++ b/keypoints/CMakeLists.txt @@ -33,7 +33,8 @@ if(build) include/pcl/${SUBSYS_NAME}/susan.h include/pcl/${SUBSYS_NAME}/iss_3d.h include/pcl/${SUBSYS_NAME}/brisk_2d.h - include/pcl/${SUBSYS_NAME}/susan_3d.h + include/pcl/${SUBSYS_NAME}/susan_3d.h + include/pcl/${SUBSYS_NAME}/susan_2d.h ) set(impl_incs include/pcl/${SUBSYS_NAME}/impl/keypoint.hpp @@ -46,7 +47,8 @@ if(build) include/pcl/${SUBSYS_NAME}/impl/susan.hpp include/pcl/${SUBSYS_NAME}/impl/iss_3d.hpp include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp - include/pcl/${SUBSYS_NAME}/impl/susan_3d.hpp + include/pcl/${SUBSYS_NAME}/impl/susan_3d.hpp + include/pcl/${SUBSYS_NAME}/impl/susan_2d.hpp ) set(LIB_NAME pcl_${SUBSYS_NAME}) diff --git a/keypoints/include/pcl/keypoints/impl/susan_2d.hpp b/keypoints/include/pcl/keypoints/impl/susan_2d.hpp new file mode 100644 index 00000000000..881601f8b9a --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/susan_2d.hpp @@ -0,0 +1,247 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, 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 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. + * + */ + +#ifndef PCL_SUSAN_KEYPOINT_2D_IMPL_H_ +#define PCL_SUSAN_KEYPOINT_2D_IMPL_H_ + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint2D::lineToCentroid (int x1, int y1, std::vector& points) +{ + static int x0 = half_window_size_; + static int y0 = half_window_size_; + points.reserve (window_size_ * window_size_); + int dx = abs (x1-x0); + int dy = abs (y1-y0); + int sx = (x0 < x1) ? 1 : -1; + int sy = (y0 < y1) ? 1 : -1; + int err = dx - dy; + + while (true) + { + if ((x0 > 0) && (y0 > 0) && (x0 < window_size_) && (y0 < window_size_)) + points.push_back (Eigen::Vector2i (x0, y0)); + if ((x0 == x1) && (y0 == y1)) + break; + int e2 = 2 * err; + if (e2 > -dy) + { + err-= dy; + x0 += sx; + } + + if ((x0 == x1) && (y0 == y1)) + { + if ((x0 > 0) && (y0 > 0) && (x0 < window_size_) && (y0 < window_size_)) + points.push_back (Eigen::Vector2i (x0, y0)); + break; + } + + if (e2 < dx) + { + err+= dx; + y0+= sy; + } + } +} +///////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::SusanKeypoint2D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + if (!input_->isOrganized ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support non organized clouds!\n", name_.c_str ()); + return (false); + } + + if (indices_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] %s doesn't support setting indices!\n", name_.c_str ()); + return (false); + } + + if ((window_size_%2) == 0) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be odd!\n", name_.c_str ()); + return (false); + } + + if (window_size_ < 3) + { + PCL_ERROR ("[pcl::%s::initCompute] Window size must be at least 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + geometric_threshold_ = 0.5 * window_size_ * window_size_; + return (true); +} +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::SusanKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + pcl::PointCloud &response = *response_; + + int w = static_cast (input_->width) - half_window_size_; + int h = static_cast (input_->height) - half_window_size_; + + for(int j = half_window_size_; j < h; ++j) + { + for(int i = half_window_size_; i < w; ++i) + { + float center = intensity_ ((*input_) (i,j)); + Eigen::Vector2f nucleus (i, j); + Eigen::Vector2f centroid = Eigen::Vector2f::Zero (); + Eigen::Array usan (window_size_, window_size_); + usan.setZero (); + + float area = 0; + for (int y = 0, jj = j - half_window_size_; y < window_size_; ++jj, ++y) + for (int x = 0, ii = i - half_window_size_; x < window_size_; ++ii, ++x) + { + float intensity = intensity_ ((*input_) (ii,jj)); + float c = fabs (center - intensity); + if (c <= threshold_) + { + usan (x, y) = true; + ++area; + centroid[0] += static_cast (ii); + centroid[1] += static_cast (jj); + } + } + + if (area >= geometric_threshold_) + continue; + + response (i, j) = 1.f / (geometric_threshold_ - area); + if (!test_distance_) + continue; + + centroid /= area; + Eigen::Vector2f nc = centroid - nucleus; + if (nc.norm () <= distance_threshold_) + { + response (i,j) = 0; + continue; + } + + if (!test_contiguity_) + continue; + + // All the points on the line from the nucleus to the centroid must be part of the USAN + std::vector nucleus_center; + float shifted_x = half_window_size_ + centroid[0] - nucleus[0]; + float shifted_y = half_window_size_ + centroid[1] - nucleus[1]; + lineToCentroid (shifted_x, shifted_y, nucleus_center); + bool all_usan_on_line = true; + for (std::vector::const_iterator p = nucleus_center.begin (); + p != nucleus_center.end (); + ++p) + { + if (usan ((*p)[0], (*p)[1]) == false) + { + all_usan_on_line = false; + break; + } + } + + if (!all_usan_on_line) + { + response (i,j) = 0; + continue; + } + } + } + + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&SusanKeypoint2D::greaterCornernessAtIndices, this, _1, _2)); + + output.clear (); + output.reserve (input_->size ()); + + std::vector occupency_map (indices.size (), false); + const int width (input_->width); + const int height (input_->height); + const int occupency_map_size (indices.size ()); + +#ifdef _OPENMP +#pragma omp parallel for shared (output, keypoints_indices_) num_threads (threads_) +#endif + for (int i = 0; i < indices.size (); ++i) + { + int idx = indices[i]; + if ((response_->points[idx] == 0) || occupency_map[idx]) + continue; + + PointOutT p; + p.getVector3fMap () = input_->points[idx].getVector3fMap (); + p.intensity = response_->points [idx]; + +#ifdef _OPENMP +#pragma omp critical +#endif + { + output.push_back (p); + keypoints_indices_->indices.push_back (idx); + } + + const int x = idx % width; + const int y = idx / width; + const int u_end = std::min (width, x + half_window_size_); + const int v_end = std::min (height, y + half_window_size_); + for(int v = std::max (0, y - half_window_size_); v < v_end; ++v) + for(int u = std::max (0, x - half_window_size_); u < u_end; ++u) + occupency_map[v*width + u] = true; + } + + output.height = 1; + output.width = static_cast (output.size()); + // we don not change the denseness + output.is_dense = input_->is_dense; +} + +#define PCL_INSTANTIATE_SusanKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::SusanKeypoint2D; +#endif diff --git a/keypoints/include/pcl/keypoints/susan_2d.h b/keypoints/include/pcl/keypoints/susan_2d.h new file mode 100644 index 00000000000..f127bd2d26b --- /dev/null +++ b/keypoints/include/pcl/keypoints/susan_2d.h @@ -0,0 +1,190 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2013-, 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 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. + * + */ + +#ifndef PCL_SUSAN_KEYPOINT_2D_H_ +#define PCL_SUSAN_KEYPOINT_2D_H_ + +#include +#include + +namespace pcl +{ + /** \brief SusanKeypoint2D implements Susan and Hedley corner detector on + * organized pooint cloud using intensity information. + * It uses first order statistics to find variation of intensities inside a mask. + * Originally the mask is circular which is quite slow instead we use the circumsing + * sqaure window. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class SusanKeypoint2D : public Keypoint + { + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + typedef typename Keypoint::PointCloudIn PointCloudIn; + typedef typename Keypoint::PointCloudOut PointCloudOut; + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + + using Keypoint::name_; + using Keypoint::input_; + using Keypoint::indices_; + using Keypoint::keypoints_indices_; + + /** \brief Constructor + * \param[in] radius the radius for keypoint detection and for non maxima + * suppression, default 3.4px. + * \param[in] threshold for intensity comparison. + * \param[in] distance to test if the nucleus is far enough from the centroid, + */ + SusanKeypoint2D (int window_size = 7, float threshold = 20 / 255.f, float distance = 0.01f) + : window_size_ (window_size) + , threshold_ (threshold) + , distance_threshold_ (distance) + , threads_ (1) + { + name_ = "SusanKeypoint2D"; + test_distance_ = true; + test_contiguity_ = false; + } + + /// \brief Set window size + inline void + setWindowSize (int window_size) { window_size_= window_size; } + + /// \brief \return window size i.e. window width or height + inline int + getWindowSize () const { return (window_size_); } + + /** \brief set the threshold to reject non similar points. + * \param[in] threshold + */ + inline void + setThreshold (float threshold) { threshold_= threshold; } + + /// \brief \return first threshold + inline float + getThreshold () const { return (threshold_); } + + /** \brief Set the minimal distance between nucleus and centroid to reject false + * positive. This is only evaluated if distance test is turned on. + * \param[in] distance minimal distance between centroid and nucleus. + */ + void + setDistanceThreshold (float distance) { distance_threshold_ = distance; } + + /// \brief \return distance threshold + float + getDistanceThreshold () const { return (distance_threshold_); } + + /** \brief Initialize the scheduler and set the number of threads to use. + * \param nr_threads the number of hardware threads to use, 0 for automatic. + */ + inline void + setNumberOfThreads (unsigned int nr_threads = 0) { threads_ = nr_threads; } + + /// \brief \return the number of threads + inline unsigned int + getNumberOfThreads () const { return (threads_); } + + /** \brief test that a point in the USAN is part of the [nucleus centroid] to filter + * out false positives, default is false. + * \remark setting this value to true can drastically slow down computation. + * \param[in] test_contiguity whether or not to test for continguity + */ + void + setTestForContiguity (bool test_contiguity) { test_contiguity_ = test_contiguity; } + + /** \brief test if the nucleus and the centrooid are far enough to filter out false + * positives, default true. + * \param[in] test_distance whether or not to test for distance + */ + void + setTestForDistance (bool test_distance) { test_distance_ = test_distance; } + + protected: + bool + initCompute (); + + void + detectKeypoints (PointCloudOut &output); + + private: + /// comparator for responses intensity + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + + /** get pixels indices that form a line segment between start and end using Bresenheim + * algorithm. + * param[in] centroid_x the centroid coordinate on x axis + * param[in] centroid_y the centroid coordinate on y axis + * param[out] points indices of points lying on [start end] segment + */ + void + lineToCentroid (int centroid_x, int centroid_y, std::vector& points); + + /// Window size + int window_size_; + /// half window size + int half_window_size_; + /// nucleus to centroid distance threshold + float distance_threshold_; + /// threshold for c computation + float threshold_; + /// geometric threshold + float geometric_threshold_; + /// test for distance between centroid and nucleus + bool test_distance_; + /// test for contiguity inside USAN + bool test_contiguity_; + /// number of threads to be used + unsigned int threads_; + /// intensity field accessor + IntensityT intensity_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include + +#endif // #ifndef PCL_SUSAN_KEYPOINT_2D_H_