From 72d24416d0c514cfff3e5ba467f15448b0ea72fa Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Fri, 13 Dec 2013 17:45:22 +0100 Subject: [PATCH 1/5] New: trajkovic and hedley detector 2D --- .../pcl/keypoints/impl/trajkovic_2d.hpp | 258 ++++++++++++++++++ .../include/pcl/keypoints/trajkovic_2d.h | 175 ++++++++++++ keypoints/src/trajkovic_2d.cpp | 43 +++ 3 files changed, 476 insertions(+) create mode 100644 keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp create mode 100644 keypoints/include/pcl/keypoints/trajkovic_2d.h create mode 100644 keypoints/src/trajkovic_2d.cpp diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp new file mode 100644 index 00000000000..f5181c4e51d --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_2d.hpp @@ -0,0 +1,258 @@ +/* + * 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_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_2D_IMPL_H_ + +template bool +pcl::TrajkovicKeypoint2D::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 >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint2D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + int w = static_cast (input_->width) - half_window_size_; + int h = static_cast (input_->height) - half_window_size_; + + if (method_ == pcl::TrajkovicKeypoint2D::FOUR_CORNERS) + { +#ifdef _OPENMP +#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) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + + float up_center = up - center; + float r1 = up_center * up_center; + float down_center = down - center; + r1+= down_center * down_center; + + float right_center = right - center; + float r2 = right_center * right_center; + float left_center = left - center; + r2+= left_center * left_center; + + float d = std::min (r1, r2); + + if (d < first_threshold_) + continue; + + float b1 = (right - up) * up_center; + b1+= (left - down) * down_center; + float b2 = (right - down) * down_center; + b2+= (left - up) * up_center; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + (*response_) (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#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) + { + float center = intensity_ ((*input_) (i,j)); + float up = intensity_ ((*input_) (i, j-half_window_size_)); + float down = intensity_ ((*input_) (i, j+half_window_size_)); + float left = intensity_ ((*input_) (i-half_window_size_, j)); + float right = intensity_ ((*input_) (i+half_window_size_, j)); + float upleft = intensity_ ((*input_) (i-half_window_size_, j-half_window_size_)); + float upright = intensity_ ((*input_) (i+half_window_size_, j-half_window_size_)); + float downleft = intensity_ ((*input_) (i-half_window_size_, j+half_window_size_)); + float downright = intensity_ ((*input_) (i+half_window_size_, j+half_window_size_)); + std::vector r (4,0); + + float up_center = up - center; + r[0] = up_center * up_center; + float down_center = down - center; + r[0]+= down_center * down_center; + + float upright_center = upright - center; + r[1] = upright_center * upright_center; + float downleft_center = downleft - center; + r[1]+= downleft_center * downleft_center; + + float right_center = right - center; + r[2] = right_center * right_center; + float left_center = left - center; + r[2]+= left_center * left_center; + + float downright_center = downright - center; + r[3] = downright_center * downright_center; + float upleft_center = upleft - center; + r[3]+= upleft_center * upleft_center; + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) + continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = (upright - up) * up_center; + B[0]+= (downleft - down) * down_center; + B[1] = (right - upright) * upright_center; + B[1]+= (left - downleft) * downleft_center; + B[2] = (downright - right) * downright_center; + B[2]+= (upleft - left) * upleft_center; + B[3] = (down - downright) * downright_center; + B[3]+= (up - upleft) * upleft_center; + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + (*response_) (i,j) = *(std::min (D.begin (), D.end ())); + } + else + (*response_) (i,j) = d; + } + } + } + + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&TrajkovicKeypoint2D::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] < second_threshold_) || 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_TrajkovicKeypoint2D(T,U,I) template class PCL_EXPORTS pcl::TrajkovicKeypoint2D; +#endif diff --git a/keypoints/include/pcl/keypoints/trajkovic_2d.h b/keypoints/include/pcl/keypoints/trajkovic_2d.h new file mode 100644 index 00000000000..56aae1e2100 --- /dev/null +++ b/keypoints/include/pcl/keypoints/trajkovic_2d.h @@ -0,0 +1,175 @@ +/* + * 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_TRAJKOVIC_KEYPOINT_2D_H_ +#define PCL_TRAJKOVIC_KEYPOINT_2D_H_ + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint2D implements Trajkovic and Hedley corner detector on + * organized pooint cloud using intensity information. + * It uses first order statistics to find variation of intensities in horizontal + * or vertical directions. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template > + class TrajkovicKeypoint2D : 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_; + + typedef enum { FOUR_CORNERS, EIGHT_CORNERS } ComputationMethod; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint2D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.1, + float second_threshold = 100.0) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint2D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \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 first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] first_threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] second_threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_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_); } + + 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]); + } + + /// computation method + ComputationMethod method_; + /// Window size + int window_size_; + /// half window size + int half_window_size_; + /// intensity field accessor + IntensityT intensity_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include + +#endif // #ifndef PCL_TRAJKOVIC_KEYPOINT_2D_H_ diff --git a/keypoints/src/trajkovic_2d.cpp b/keypoints/src/trajkovic_2d.cpp new file mode 100644 index 00000000000..7bf07d1599d --- /dev/null +++ b/keypoints/src/trajkovic_2d.cpp @@ -0,0 +1,43 @@ +/* + * 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 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. + * + */ + +#include +#include +#include + +// Instantiations of specific point types +//PCL_INSTANTIATE_PRODUCT(HarrisKeypoint2D, (PCL_XYZ_POINT_TYPES)); From 233496a2d9fc9704efd11e7a4e76d3e477a5cf85 Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Sun, 15 Dec 2013 16:40:37 +0100 Subject: [PATCH 2/5] New: port of Trajkovic and Hedley detector to pure geometry --- .../pcl/keypoints/impl/trajkovic_3d.hpp | 273 ++++++++++++++++++ .../include/pcl/keypoints/trajkovic_3d.h | 218 ++++++++++++++ keypoints/src/trajkovic_3d.cpp | 43 +++ 3 files changed, 534 insertions(+) create mode 100644 keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp create mode 100644 keypoints/include/pcl/keypoints/trajkovic_3d.h create mode 100644 keypoints/src/trajkovic_3d.cpp diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp new file mode 100644 index 00000000000..43002ad04af --- /dev/null +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp @@ -0,0 +1,273 @@ +/* + * 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_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ +#define PCL_TRAJKOVIC_KEYPOINT_3D_IMPL_H_ + +#include + +template bool +pcl::TrajkovicKeypoint3D::initCompute () +{ + if (!PCLBase::initCompute ()) + return (false); + + keypoints_indices_.reset (new pcl::PointIndices); + keypoints_indices_->indices.reserve (input_->size ()); + + 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 >= 3x3!\n", name_.c_str ()); + return (false); + } + + half_window_size_ = window_size_ / 2; + + if (!normals_) + { + NormalsPtr normals (new Normals ()); + normals->reserve (normals->size ()); + pcl::IntegralImageNormalEstimation normal_estimation; + normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); + normal_estimation.setInputCloud (input_); + normal_estimation.setNormalSmoothingSize (5.0); + normal_estimation.compute (*normals); + normals_ = normals; + } + + if (normals_->size () != input_->size ()) + { + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_); + return (false); + } + + return (true); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::TrajkovicKeypoint3D::detectKeypoints (PointCloudOut &output) +{ + response_.reset (new pcl::PointCloud (input_->width, input_->height)); + const Normals &normals = *normals_; + const PointCloudIn &input = *input_; + pcl::PointCloud& response = *response_; + const int w = static_cast (input_->width) - half_window_size_; + const int h = static_cast (input_->height) - half_window_size_; + + if (method_ == FOUR_CORNERS) + { +#ifdef _OPENMP +#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) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + // Get rid of isolated points + if (!count) continue; + + float sn1 = squaredNormalsDiff (up, center); + float sn2 = squaredNormalsDiff (down, center); + float r1 = sn1 + sn2; + float r2 = squaredNormalsDiff (right, center) + squaredNormalsDiff (left, center); + + float d = std::min (r1, r2); + if (d < first_threshold_) continue; + + sn1 = sqrt (sn1); + sn2 = sqrt (sn2); + float b1 = normalsDiff (right, up) * sn1; + b1+= normalsDiff (left, down) * sn2; + float b2 = normalsDiff (right, down) * sn2; + b2+= normalsDiff (left, up) * sn1; + float B = std::min (b1, b2); + float A = r2 - r1 - 2*B; + + response (i,j) = ((B < 0) && ((B + A) > 0)) ? r1 - ((B*B)/A) : d; + } + } + } + else + { +#ifdef _OPENMP +#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) + { + if (!isFinite (input (i,j))) continue; + const NormalT ¢er = normals (i,j); + if (!isFinite (center)) continue; + + int count = 0; + const NormalT &up = getNormalOrNull (i, j-half_window_size_, count); + const NormalT &down = getNormalOrNull (i, j+half_window_size_, count); + const NormalT &left = getNormalOrNull (i-half_window_size_, j, count); + const NormalT &right = getNormalOrNull (i+half_window_size_, j, count); + const NormalT &upleft = getNormalOrNull (i-half_window_size_, j-half_window_size_, count); + const NormalT &upright = getNormalOrNull (i+half_window_size_, j-half_window_size_, count); + const NormalT &downleft = getNormalOrNull (i-half_window_size_, j+half_window_size_, count); + const NormalT &downright = getNormalOrNull (i+half_window_size_, j+half_window_size_, count); + // Get rid of isolated points + if (!count) continue; + + std::vector r (4,0); + + r[0] = squaredNormalsDiff (up, center); + r[0]+= squaredNormalsDiff (down, center); + + r[1] = squaredNormalsDiff (upright, center); + r[1]+= squaredNormalsDiff (downleft, center); + + r[2] = squaredNormalsDiff (right, center); + r[2]+= squaredNormalsDiff (left, center); + + r[3] = squaredNormalsDiff (downright, center); + r[3]+= squaredNormalsDiff (upleft, center); + + float d = *(std::min_element (r.begin (), r.end ())); + + if (d < first_threshold_) continue; + + std::vector B (4,0); + std::vector A (4,0); + std::vector sumAB (4,0); + B[0] = normalsDiff (upright, up) * normalsDiff (up, center); + B[0]+= normalsDiff (downleft, down) * normalsDiff (down, center); + B[1] = normalsDiff (right, upright) * normalsDiff (upright, center); + B[1]+= normalsDiff (left, downleft) * normalsDiff (downleft, center); + B[2] = normalsDiff (downright, right) * normalsDiff (downright, center); + B[2]+= normalsDiff (upleft, left) * normalsDiff (upleft, center); + B[3] = normalsDiff (down, downright) * normalsDiff (downright, center); + B[3]+= normalsDiff (up, upleft) * normalsDiff (upleft, center); + A[0] = r[1] - r[0] - B[0] - B[0]; + A[1] = r[2] - r[1] - B[1] - B[1]; + A[2] = r[3] - r[2] - B[2] - B[2]; + A[3] = r[0] - r[3] - B[3] - B[3]; + sumAB[0] = A[0] + B[0]; + sumAB[1] = A[1] + B[1]; + sumAB[2] = A[2] + B[2]; + sumAB[3] = A[3] + B[3]; + if ((*std::max_element (B.begin (), B.end ()) < 0) && + (*std::min_element (sumAB.begin (), sumAB.end ()) > 0)) + { + std::vector D (4,0); + D[0] = B[0] * B[0] / A[0]; + D[1] = B[1] * B[1] / A[1]; + D[2] = B[2] * B[2] / A[2]; + D[3] = B[3] * B[3] / A[3]; + response (i,j) = *(std::min (D.begin (), D.end ())); + } + else + response (i,j) = d; + } + } + } + // Non maximas suppression + std::vector indices = *indices_; + std::sort (indices.begin (), indices.end (), + boost::bind (&TrajkovicKeypoint3D::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] < second_threshold_) || 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 = true; +} + +#define PCL_INSTANTIATE_TrajkovicKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::TrajkovicKeypoint3D; +#endif diff --git a/keypoints/include/pcl/keypoints/trajkovic_3d.h b/keypoints/include/pcl/keypoints/trajkovic_3d.h new file mode 100644 index 00000000000..da1e096d429 --- /dev/null +++ b/keypoints/include/pcl/keypoints/trajkovic_3d.h @@ -0,0 +1,218 @@ +/* + * 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_TRAJKOVIC_KEYPOINT_3D_H_ +#define PCL_TRAJKOVIC_KEYPOINT_3D_H_ + +#include +#include + +namespace pcl +{ + /** \brief TrajkovicKeypoint3D implements Trajkovic and Hedley corner detector on + * point cloud using geometric information. + * It uses first order statistics to find variation of normals. + * This work is part of Nizar Sallem PhD thesis. + * + * \author Nizar Sallem + * \ingroup keypoints + */ + template + class TrajkovicKeypoint3D : 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; + 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::keypoints_indices_; + using Keypoint::initCompute; + + typedef enum { FOUR_CORNERS, EIGHT_CORNERS } ComputationMethod; + + /** \brief Constructor + * \param[in] method the method to be used to determine the corner responses + * \param[in] first_threshold the threshold used in the simple cornerness test. + * \param[in] second_threshold the threshold used to reject weak corners. + */ + TrajkovicKeypoint3D (ComputationMethod method = FOUR_CORNERS, + int window_size = 3, + float first_threshold = 0.00046, + float second_threshold = 0.03589) + : method_ (method) + , window_size_ (window_size) + , first_threshold_ (first_threshold) + , second_threshold_ (second_threshold) + , threads_ (1) + { + name_ = "TrajkovicKeypoint3D"; + } + + /** \brief set the method of the response to be calculated. + * \param[in] method either 4 corners or 8 corners + */ + inline void + setMethod (ComputationMethod method) { method_ = method; } + + /// \brief \return the computation method + inline ComputationMethod + getMethod () const { return (method_); } + + /// \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 first_threshold to reject corners in the simple cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setFirstThreshold (float threshold) { first_threshold_= threshold; } + + /// \brief \return first threshold + inline float + getFirstThreshold () const { return (first_threshold_); } + + /** \brief set the second threshold to reject corners in the final cornerness + * computation stage. + * \param[in] threshold + */ + inline void + setSecondThreshold (float threshold) { second_threshold_= threshold; } + + /// \brief \return second threshold + inline float + getSecondThreshold () const { return (second_threshold_); } + + /** \brief Set normals if precalculated normals are available. + * \param normals + */ + inline void + setNormals (const NormalsConstPtr &normals) { normals_ = normals; } + + /// \brief \return points normals as calculated or given + inline void + getNormals (const NormalsConstPtr &normals) const { return (normals_); } + + /** \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_); } + + protected: + bool + initCompute (); + + void + detectKeypoints (PointCloudOut &output); + + private: + /** Return a const reference to the normal at (i,j) if it is finite else return + * a reference to a null normal. + * If the returned normal is valid \a counter is incremented. + */ + inline const NormalT& + getNormalOrNull (int i, int j, int& counter) const + { + static const NormalT null; + if (!isFinite ((*normals_) (i,j))) return (null); + ++counter; + return ((*normals_) (i,j)); + } + /// \return difference of two normals vectors + inline float + normalsDiff (const NormalT& a, const NormalT& b) const + { + double nx = a.normal_x; + double ny = a.normal_y; + double nz = a.normal_z; + return (static_cast (1.0 - (nx*nx + ny*ny + nz*nz))); + } + /// \return squared difference of two normals vectors + inline float + squaredNormalsDiff (const NormalT& a, const NormalT& b) const + { + float diff = normalsDiff (a,b); + return (diff * diff); + } + /** Comparator for responses intensity + * \return true if \a response_ at index \aa is greater than response at index \ab + */ + inline bool + greaterCornernessAtIndices (int a, int b) const + { + return (response_->points [a] > response_->points [b]); + } + /// computation method + ComputationMethod method_; + /// window size + int window_size_; + /// half window size + int half_window_size_; + /// first threshold for quick rejection + float first_threshold_; + /// second threshold for corner evaluation + float second_threshold_; + /// number of threads to be used + unsigned int threads_; + /// point cloud normals + NormalsConstPtr normals_; + /// point cloud response + pcl::PointCloud::Ptr response_; + }; +} + +#include + +#endif // #ifndef PCL_TRAJKOVIC_KEYPOINT_3D_H_ diff --git a/keypoints/src/trajkovic_3d.cpp b/keypoints/src/trajkovic_3d.cpp new file mode 100644 index 00000000000..11e51727357 --- /dev/null +++ b/keypoints/src/trajkovic_3d.cpp @@ -0,0 +1,43 @@ +/* + * 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 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. + * + */ + +#include +#include +#include + +// Instantiations of specific point types +//PCL_INSTANTIATE_PRODUCT(HarrisKeypoint2D, (PCL_XYZ_POINT_TYPES)); From 134e78754db4824a384f428612b2dec048d1ddc5 Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Thu, 19 Dec 2013 12:45:06 +0100 Subject: [PATCH 3/5] Fix: a silly bug in normals diff computation --- keypoints/include/pcl/keypoints/trajkovic_3d.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/keypoints/include/pcl/keypoints/trajkovic_3d.h b/keypoints/include/pcl/keypoints/trajkovic_3d.h index da1e096d429..a25cb6ca766 100644 --- a/keypoints/include/pcl/keypoints/trajkovic_3d.h +++ b/keypoints/include/pcl/keypoints/trajkovic_3d.h @@ -174,10 +174,9 @@ namespace pcl inline float normalsDiff (const NormalT& a, const NormalT& b) const { - double nx = a.normal_x; - double ny = a.normal_y; - double nz = a.normal_z; - return (static_cast (1.0 - (nx*nx + ny*ny + nz*nz))); + double nx = a.normal_x; double ny = a.normal_y; double nz = a.normal_z; + double mx = b.normal_x; double my = b.normal_y; double mz = b.normal_z; + return (static_cast (1.0 - (nx*mx + ny*my + nz*mz))); } /// \return squared difference of two normals vectors inline float From 8bae358c9f4cae68e60dd97d5f46a31bbd0134b6 Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Mon, 13 Jan 2014 15:06:07 +0100 Subject: [PATCH 4/5] Add: pcl_ni_trajkovic application to detect Trajkovic 2D/3D points from OpenNi device --- apps/CMakeLists.txt | 3 + apps/src/ni_trajkovic.cpp | 249 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 252 insertions(+) create mode 100644 apps/src/ni_trajkovic.cpp diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index ddf4dc217a4..10320c9a63b 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -224,6 +224,9 @@ if(build) PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_susan ${SUBSYS_NAME} src/ni_susan.cpp) target_link_libraries(pcl_ni_susan pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search) + PCL_ADD_EXECUTABLE_OPT_BUNDLE(pcl_ni_trajkovic ${SUBSYS_NAME} src/ni_trajkovic.cpp) + target_link_libraries(pcl_ni_trajkovic pcl_common pcl_visualization pcl_features pcl_keypoints pcl_search) + endif() # OPENNI_FOUND + BUILD_OPENNI endif() # VTK_FOUND diff --git a/apps/src/ni_trajkovic.cpp b/apps/src/ni_trajkovic.cpp new file mode 100644 index 00000000000..e497a272607 --- /dev/null +++ b/apps/src/ni_trajkovic.cpp @@ -0,0 +1,249 @@ +/* + * 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, O R 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. + * + * $Id$ + * + */ + +#define SHOW_FPS 1 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace pcl; +typedef PointXYZRGBA PointT; +typedef PointXYZI KeyPointT; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +class TrajkovicDemo +{ + public: + typedef PointCloud Cloud; + typedef Cloud::Ptr CloudPtr; + typedef Cloud::ConstPtr CloudConstPtr; + + TrajkovicDemo (Grabber& grabber, bool enable_3d) + : cloud_viewer_ ("TRAJKOVIC 3D Keypoints -- PointCloud") + , grabber_ (grabber) + , image_viewer_ ("TRAJKOVIC 3D Keypoints -- Image") + , enable_3d_ (enable_3d) + { + } + + ///////////////////////////////////////////////////////////////////////// + void + cloud_callback_3d (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + + // Compute TRAJKOVIC keypoints 3D + TrajkovicKeypoint3D trajkovic; + trajkovic.setInputCloud (cloud); + trajkovic.setNumberOfThreads (6); + keypoints_.reset (new PointCloud); + trajkovic.compute (*keypoints_); + keypoints_indices_ = trajkovic.getKeypointsIndices (); + } + + ///////////////////////////////////////////////////////////////////////// + void + cloud_callback_2d (const CloudConstPtr& cloud) + { + FPS_CALC ("cloud callback"); + boost::mutex::scoped_lock lock (cloud_mutex_); + cloud_ = cloud; + + // Compute TRAJKOVIC keypoints 2D + TrajkovicKeypoint2D trajkovic; + trajkovic.setInputCloud (cloud); + trajkovic.setNumberOfThreads (6); + keypoints_.reset (new PointCloud); + trajkovic.compute (*keypoints_); + keypoints_indices_ = trajkovic.getKeypointsIndices (); + } + + ///////////////////////////////////////////////////////////////////////// + void + init () + { + boost::function cloud_cb; + if (enable_3d_) + cloud_cb = boost::bind (&TrajkovicDemo::cloud_callback_3d, this, _1); + else + cloud_cb = boost::bind (&TrajkovicDemo::cloud_callback_2d, this, _1); + + cloud_connection = grabber_.registerCallback (cloud_cb); + } + + ///////////////////////////////////////////////////////////////////////// + std::string + getStrBool (bool state) + { + std::ostringstream ss; + ss << state; + return (ss.str ()); + } + + ///////////////////////////////////////////////////////////////////////// + void + run () + { + grabber_.start (); + + bool image_init = false, cloud_init = false; + bool keypts = true; + + while (!cloud_viewer_.wasStopped () && !image_viewer_.wasStopped ()) + { + PointCloud::Ptr keypoints; + CloudConstPtr cloud; + if (cloud_mutex_.try_lock ()) + { + cloud_.swap (cloud); + keypoints_.swap (keypoints); + + cloud_mutex_.unlock (); + } + + if (cloud) + { + int w (cloud->width); + if (!cloud_init) + { + cloud_viewer_.setPosition (0, 0); + cloud_viewer_.setSize (cloud->width, cloud->height); + cloud_init = !cloud_init; + } + + if (!cloud_viewer_.updatePointCloud (cloud, "OpenNICloud")) + { + cloud_viewer_.addPointCloud (cloud, "OpenNICloud"); + cloud_viewer_.resetCameraViewpoint ("OpenNICloud"); + } + + if (!image_init) + { + image_viewer_.setPosition (cloud->width, 0); + image_viewer_.setSize (cloud->width, cloud->height); + image_init = !image_init; + } + + image_viewer_.addRGBImage (cloud); + + if (keypoints && !keypoints->empty ()) + { + image_viewer_.removeLayer (getStrBool (keypts)); + std::vector uv; + uv.reserve (keypoints_indices_->indices.size () * 2); + for (std::vector::const_iterator id = keypoints_indices_->indices.begin (); + id != keypoints_indices_->indices.end (); + ++id) + { + int u (*id % w); + int v (*id / w); + image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 5, getStrBool (!keypts), 0.5); + } + keypts = !keypts; + + visualization::PointCloudColorHandlerCustom blue (keypoints, 0, 0, 255); + if (!cloud_viewer_.updatePointCloud (keypoints, blue, "keypoints")) + cloud_viewer_.addPointCloud (keypoints, blue, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 10, "keypoints"); + cloud_viewer_.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_OPACITY, 0.5, "keypoints"); + } + } + + cloud_viewer_.spinOnce (); + image_viewer_.spinOnce (); + boost::this_thread::sleep (boost::posix_time::microseconds (100)); + } + + grabber_.stop (); + cloud_connection.disconnect (); + } + + visualization::PCLVisualizer cloud_viewer_; + Grabber& grabber_; + boost::mutex cloud_mutex_; + CloudConstPtr cloud_; + + visualization::ImageViewer image_viewer_; + + PointCloud::Ptr keypoints_; + pcl::PointIndicesConstPtr keypoints_indices_; + bool enable_3d_; + private: + boost::signals2::connection cloud_connection; +}; + +/* ---[ */ +int +main (int argc, char** argv) +{ + if (pcl::console::find_switch (argc, argv, "-h")) + { + pcl::console::print_info ("Syntax is: %s [-device device_id_string] [-2d]\n", argv[0]); + return (0); + } + + std::string device_id ("#1"); + bool enable_3d = true; + if (pcl::console::find_switch (argc, argv, "-2d")) + enable_3d = false; + + if (pcl::console::find_argument (argc, argv, "-device")) + pcl::console::parse (argc, argv, "-device", device_id); + + pcl::console::print_info ("Extracting Trajkovic %s keypoints from device %s.\n", + enable_3d ? "3D" : "2D", device_id.c_str ()); + + OpenNIGrabber grabber (device_id); + + TrajkovicDemo openni_viewer (grabber, enable_3d); + + openni_viewer.init (); + openni_viewer.run (); + + return (0); +} +/* ]--- */ From b9b6afb168fbbfb12ed8f4638d99f36a9656d9e4 Mon Sep 17 00:00:00 2001 From: nizar-sallem Date: Fri, 17 Jan 2014 13:23:13 +0100 Subject: [PATCH 5/5] Fix: removal of unused argument and normals_->resize statement --- keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp index 43002ad04af..793e272775c 100644 --- a/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp +++ b/keypoints/include/pcl/keypoints/impl/trajkovic_3d.hpp @@ -72,7 +72,6 @@ pcl::TrajkovicKeypoint3D::initCompute () if (!normals_) { NormalsPtr normals (new Normals ()); - normals->reserve (normals->size ()); pcl::IntegralImageNormalEstimation normal_estimation; normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation::SIMPLE_3D_GRADIENT); normal_estimation.setInputCloud (input_); @@ -83,7 +82,7 @@ pcl::TrajkovicKeypoint3D::initCompute () if (normals_->size () != input_->size ()) { - PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str (), method_); + PCL_ERROR ("[pcl::%s::initCompute] normals given, but the number of normals does not match the number of input points!\n", name_.c_str ()); return (false); }