From c854917401f9644310b54643657c4b49d89c587f Mon Sep 17 00:00:00 2001 From: TimoHackel Date: Sun, 8 Sep 2013 10:48:46 +0200 Subject: [PATCH 1/6] model outlier removal contribution --- .../content/model_outlier_removal.rst | 81 +++ .../model_outlier_removal/CMakeLists.txt | 12 + .../model_outlier_removal.cpp | 70 +++ filters/CMakeLists.txt | 3 + .../filters/impl/model_outlier_removal.hpp | 271 +++++++++ .../pcl/filters/model_outlier_removal.h | 530 ++++++++++++++++++ filters/src/model_outlier_removal.cpp | 374 ++++++++++++ test/filters/CMakeLists.txt | 5 + test/filters/test_model_outlier_removal.cpp | 107 ++++ 9 files changed, 1453 insertions(+) create mode 100644 doc/tutorials/content/model_outlier_removal.rst create mode 100644 doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt create mode 100644 doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp create mode 100644 filters/include/pcl/filters/impl/model_outlier_removal.hpp create mode 100644 filters/include/pcl/filters/model_outlier_removal.h create mode 100644 filters/src/model_outlier_removal.cpp create mode 100644 test/filters/test_model_outlier_removal.cpp diff --git a/doc/tutorials/content/model_outlier_removal.rst b/doc/tutorials/content/model_outlier_removal.rst new file mode 100644 index 00000000000..b50d2437eae --- /dev/null +++ b/doc/tutorials/content/model_outlier_removal.rst @@ -0,0 +1,81 @@ +.. _model_outlier_removal: + +Filtering a PointCloud using ModelOutlierRemoval +------------------------------------------------ + +This tutorial demonstrates how to extract parametric models for example for planes or spheres +out of a PointCloud by using SAC_Models with known coefficients. +If you don't know the models coefficients take a look at the Sample Consensus tutorials. + +The code +-------- + +First, create a file, let's call it ``model_outlier_removal.cpp``, in your favorite +editor, and place the following inside it: + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :linenos: + +The explanation +--------------- + +Now, let's break down the code piece by piece. + +In the following lines, we define the PointClouds structures, fill in noise, random points +on a plane as well as random points on a sphere and display its content to screen. + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :lines: 7-45 + +Finally we extract the sphere using ModelOutlierRemoval. + +.. literalinclude:: sources/model_outlier_removal/model_outlier_removal.cpp + :language: cpp + :lines: 50-61 + +Compiling and running the program +--------------------------------- + +Add the following lines to your CMakeLists.txt file: + +.. literalinclude:: sources/model_outlier_removal/CMakeLists.txt + :language: cmake + :linenos: + + +After you have made the executable, you can run it. Simply do:: + + $ ./model_outlier_removal + +You will see something similar to:: + + Cloud before filtering: + 0.352222 -0.151883 -0.106395 + -0.397406 -0.473106 0.292602 + -0.731898 0.667105 0.441304 + -0.734766 0.854581 -0.0361733 + -0.4607 -0.277468 -0.916762 + -0.82 -0.341666 0.4592 + -0.728589 0.667873 0.152 + -0.3134 -0.873043 -0.3736 + 0.62553 0.590779 0.5096 + -0.54048 0.823588 -0.172 + -0.707627 0.424576 0.5648 + -0.83153 0.523556 0.1856 + -0.513903 -0.719464 0.4672 + 0.291534 0.692393 0.66 + 0.258758 0.654505 -0.7104 + Sphere after filtering: + -0.82 -0.341666 0.4592 + -0.728589 0.667873 0.152 + -0.3134 -0.873043 -0.3736 + 0.62553 0.590779 0.5096 + -0.54048 0.823588 -0.172 + -0.707627 0.424576 0.5648 + -0.83153 0.523556 0.1856 + -0.513903 -0.719464 0.4672 + 0.291534 0.692393 0.66 + 0.258758 0.654505 -0.7104 + diff --git a/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt new file mode 100644 index 00000000000..de01df66ac8 --- /dev/null +++ b/doc/tutorials/content/sources/model_outlier_removal/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(model_outlier_removal) + +find_package(PCL 1.7 REQUIRED) + +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +add_executable (model_outlier_removal model_outlier_removal.cpp) +target_link_libraries (model_outlier_removal ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp new file mode 100644 index 00000000000..59d0e8f0ccb --- /dev/null +++ b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp @@ -0,0 +1,70 @@ +#include +#include +#include + +int main () +{ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_sphere_filtered (new pcl::PointCloud); + + // 1. generate cloud data + int noise_size = 5; + int sphere_data_size = 10; + cloud->width = noise_size + sphere_data_size; + cloud->height = 1; + cloud->points.resize (cloud->width * cloud->height); + // 1.1 add noise + for (size_t i = 0; i < noise_size; ++i) + { + cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); + cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); + } + // 1.2 add sphere: + double rand_x1 = 1; double rand_x2 = 1; + for (size_t i = noise_size; i < noise_size + sphere_data_size; ++i) + { + // see: http://mathworld.wolfram.com/SpherePointPicking.html + while( pow(rand_x1,2) + pow(rand_x2,2) >= 1) + { + rand_x1 = ( rand() % 100 ) / (50.0f) - 1; + rand_x2 = ( rand() % 100 ) / (50.0f) - 1; + } + double pre_calc = sqrt( 1 - pow( rand_x1, 2 ) - pow( rand_x2, 2 ) ); + cloud->points[i].x = 2 * rand_x1 * pre_calc; + cloud->points[i].y = 2 * rand_x2 * pre_calc; + cloud->points[i].z = 1 - 2 * ( pow( rand_x1, 2 ) + pow( rand_x2, 2 ) ); + rand_x1 = 1; rand_x2 = 1; + } + + std::cerr << "Cloud before filtering: " << std::endl; + for (size_t i = 0; i < cloud->points.size (); ++i) + std::cout << " " << cloud->points[i].x << " " + << cloud->points[i].y << " " + << cloud->points[i].z << std::endl; + + // 2. filter sphere: + // 2.1 generate model: + // modelparameter for this sphere: + // position.x: 0, position.y: 0, position.z:0, radius: 1 + pcl::ModelCoefficients sphere_coeff; + sphere_coeff.values.resize(4); + sphere_coeff.values[0] = 0; sphere_coeff.values[1] = 0; sphere_coeff.values[2] = 0; + sphere_coeff.values[3] = 1; + + pcl::ModelOutlierRemoval sphere_filter; + sphere_filter.setModelCoefficients( sphere_coeff); + sphere_filter.setThreshold( 0.05 ); + sphere_filter.setModelType( pcl::SACMODEL_SPHERE ); + sphere_filter.setInputCloud( cloud ); + sphere_filter.filter( *cloud_sphere_filtered ); + + std::cerr << "Sphere after filtering: " << std::endl; + for (size_t i = 0; i < cloud_sphere_filtered->points.size (); ++i) + std::cout << " " << cloud_sphere_filtered->points[i].x << " " + << cloud_sphere_filtered->points[i].y << " " + << cloud_sphere_filtered->points[i].z << std::endl; + + + return (0); +} diff --git a/filters/CMakeLists.txt b/filters/CMakeLists.txt index 3ad1686c885..731ed72a5b8 100644 --- a/filters/CMakeLists.txt +++ b/filters/CMakeLists.txt @@ -39,6 +39,7 @@ if(build) src/grid_minimum.cpp src/morphological_filter.cpp src/local_maximum.cpp + src/model_outlier_removal.cpp ) set(incs @@ -77,6 +78,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/grid_minimum.h" "include/pcl/${SUBSYS_NAME}/morphological_filter.h" "include/pcl/${SUBSYS_NAME}/local_maximum.h" + "include/pcl/${SUBSYS_NAME}/model_outlier_removal.h" ) set(impl_incs @@ -112,6 +114,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/grid_minimum.hpp" "include/pcl/${SUBSYS_NAME}/impl/morphological_filter.hpp" "include/pcl/${SUBSYS_NAME}/impl/local_maximum.hpp" + "include/pcl/${SUBSYS_NAME}/impl/model_outlier_removal.hpp" ) set(LIB_NAME "pcl_${SUBSYS_NAME}") diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp new file mode 100644 index 00000000000..377003084ca --- /dev/null +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -0,0 +1,271 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, 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_FILTERS_IMPL_ModelOutlierRemoval_HPP_ +#define PCL_FILTERS_IMPL_ModelOutlierRemoval_HPP_ + +#include +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template bool +pcl::ModelOutlierRemoval::initSACModel (int model_type) +{ + model_from_normals = NULL; + // Build the model + switch (model_type) + { + case SACMODEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelPlane (input_)); + break; + } + case SACMODEL_LINE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelLine (input_)); + break; + } + case SACMODEL_CIRCLE2D: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelCircle2D (input_)); + break; + } + case SACMODEL_SPHERE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelSphere (input_)); + break; + } + case SACMODEL_PARALLEL_LINE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelParallelLine (input_)); + break; + } + case SACMODEL_PERPENDICULAR_PLANE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelPerpendicularPlane (input_)); + break; + } + case SACMODEL_CYLINDER: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); + SampleConsensusModelCylinder *ptr = + new SampleConsensusModelCylinder (input_); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalPlane *ptr = + new SampleConsensusModelNormalPlane (input_); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_CONE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); + SampleConsensusModelCone *ptr = + new SampleConsensusModelCone (input_); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_SPHERE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); + SampleConsensusModelNormalSphere *ptr = + new SampleConsensusModelNormalSphere (input_); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_PARALLEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalParallelPlane *ptr + = new SampleConsensusModelNormalParallelPlane (input_); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_PARALLEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelParallelPlane (input_)); + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) +{ + std::vector indices; + if (keep_organized_) + { + bool temp = extract_removed_indices_; + extract_removed_indices_ = true; + applyFilterIndices (indices); + extract_removed_indices_ = temp; + + output = *input_; + for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator + output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_; + if (!pcl_isfinite (user_filter_value_)) + output.is_dense = false; + } + else + { + applyFilterIndices (indices); + copyPointCloud (*input_, indices, output); + } +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) +{ + //The arrays to be used + indices.resize (indices_->size ()); + removed_indices_->resize (indices_->size ()); + int oii = 0, rii = 0; // oii = output indices iterator, rii = removed indices iterator + //is the filtersetup correct? + bool valid_setup = true; + + + valid_setup &= initSACModel (model_type_); + if( model_from_normals ) + { + if( !cloud_normals ) + { + valid_setup = false; + PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); + } else { + model_from_normals->setNormalDistanceWeight( normals_distance_weight ); + model_from_normals->setInputNormals(cloud_normals); + } + } + + //if the filter setup is invalid filter for nan and return; + if(! valid_setup ) + { + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || + !pcl_isfinite (input_->points[(*indices_)[iii]].y) || + !pcl_isfinite (input_->points[(*indices_)[iii]].z)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + indices[oii++] = (*indices_)[iii]; + } + return; + } + // check distance of pointcloud to model + std::vector distances; + //TODO: get signed distances ! + model->getDistancesToModel (model_coefficients, distances); + bool thresh_result; + + // Filter for non-finite entries and the specified field limits + for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator + { + // Non-finite entries are always passed to removed indices + if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || + !pcl_isfinite (input_->points[(*indices_)[iii]].y) || + !pcl_isfinite (input_->points[(*indices_)[iii]].z)) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + + // use threshold function to seperate outliers from inliers: + thresh_result = threshold_function ( distances[iii] ); + + // in normal mode: define outliers as false thresh_result + if (!negative_ && !thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // in negative_ mode: define outliers as true thresh_result + if (negative_ && thresh_result) + { + if (extract_removed_indices_) + (*removed_indices_)[rii++] = (*indices_)[iii]; + continue; + } + + // Otherwise it was a normal point for output (inlier) + indices[oii++] = (*indices_)[iii]; + + } + + // Resize the output arrays + indices.resize (oii); + removed_indices_->resize (rii); + +} + +#define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval; + +#endif // PCL_FILTERS_IMPL_ModelOutlierRemoval_HPP_ + diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h new file mode 100644 index 00000000000..20c7dc271ce --- /dev/null +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -0,0 +1,530 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, 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_FILTERS_ModelOutlierRemoval_H_ +#define PCL_FILTERS_ModelOutlierRemoval_H_ + +#include +#include + +// Sample Consensus models +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + + + + +namespace pcl +{ + + /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). + *

+ * Usage example: + * \code + * pcl::ModelCoefficients model_coeff; + * model_coeff.values.resize(4); + * model_coeff.values[0] = 0; model_coeff.values[1] = 0; model_coeff.values[2] = 1.5; model_coeff.values[3] = 0.5; + * pcl::ModelOutlierRemoval filter; + * filter.setModelCoefficients( model_coeff); + * filter.setThreshold( 0.1 ); + * filter.setModelType( pcl::SACMODEL_PLANE ); + * filter.setInputCloud( *cloud_in ); + * filter.setFilterLimitsNegative(false); + * filter.filter( *cloud_out ); + * \endcode + */ + template + class ModelOutlierRemoval : public FilterIndices + { + protected: + typedef typename FilterIndices::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + typedef typename SampleConsensusModel::Ptr SampleConsensusModelPtr; + + + public: + /** \brief Constructor. + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + ModelOutlierRemoval (bool extract_removed_indices = false) : + FilterIndices::FilterIndices (extract_removed_indices) + { + model_from_normals = NULL; + thresh = 0; + normals_distance_weight = 0; + filter_name_ = "ModelOutlierRemoval"; + setThresholdFunction(&pcl::ModelOutlierRemoval::single_threshold, *this ); + } + /** \brief sets the models coefficients */ + inline void + setModelCoefficients (const pcl::ModelCoefficients model_coefficients_) + { + model_coefficients.resize( model_coefficients_.values.size() ); + for(unsigned int i = 0; i < model_coefficients_.values.size(); i++) + { + model_coefficients[i] = model_coefficients_.values[i]; + } + } + /** \brief returns the models coefficients + */ + pcl::ModelCoefficients + getModelCoefficients() + { + pcl::ModelCoefficients mc; + mc.values.resize( model_coefficients.size() ); + for( unsigned int i = 0; i < mc.values.size(); i++) + mc.values[i] = model_coefficients[i]; + return mc; + } + /** \brief Set the type of SAC model used. */ + inline void + setModelType (int model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline int + getModelType () + { + return (model_type_); + } + + /** \brief Set the thresholdfunction*/ + inline void + setThreshold( float thresh_) + { + thresh = thresh_; + } + + /** \brief Get the thresholdfunction*/ + inline float + getThreshhold() + { + return thresh; + } + + /** \brief Set the normals cloud*/ + inline void + setInputNormals( pcl::PointCloud::Ptr normals_ptr) + { + cloud_normals = normals_ptr; + } + /** \brief Get the normals cloud*/ + inline pcl::PointCloud::Ptr + getInputNormals() + { + return cloud_normals; + } + /** \brief Set the normals distance weight*/ + inline void + setNormalDistanceWeight( const double& weight) + { + normals_distance_weight = weight; + } + /** \brief get the normal distance weight*/ + inline double + getNormalDistanceWeight() + { + return normals_distance_weight; + } + + + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max) + * Default: false. + * \warning This method will be removed in the future. Use setNegative() instead. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + inline void + setFilterLimitsNegative (const bool limit_negative) + { + negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline void + getFilterLimitsNegative (bool &limit_negative) + { + limit_negative = negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool + getFilterLimitsNegative () + { + return (negative_); + } + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + */ + + void + setThresholdFunction (boost::function thresh_) + { + threshold_function = thresh_; + }; + + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + * \param[in] instance + */ + template void + setThresholdFunction (bool (T::*thresh_function) (double), T& instance) + { + setThresholdFunction (boost::bind (thresh_function, boost::ref (instance), _1)); + } + + + protected: + using PCLBase::input_; + using PCLBase::indices_; + using Filter::filter_name_; + using Filter::getClassName; + using FilterIndices::negative_; + using FilterIndices::keep_organized_; + using FilterIndices::user_filter_value_; + using FilterIndices::extract_removed_indices_; + using FilterIndices::removed_indices_; + + /** \brief Filtered results are stored in a separate point cloud. + * \param[out] output The resultant point cloud. + */ + void + applyFilter (PointCloud &output); + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilter (std::vector &indices) + { + applyFilterIndices (indices); + } + + /** \brief Filtered results are indexed by an indices array. + * \param[out] indices The resultant indices. + */ + void + applyFilterIndices (std::vector &indices); + + protected: + double normals_distance_weight; + pcl::PointCloud::Ptr cloud_normals; + SampleConsensusModelFromNormals< PointT, pcl::Normal > *model_from_normals; + /** \brief The model used to calculate distances */ + SampleConsensusModelPtr model; + /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ + float thresh; + /** \brief The model coefficients */ + Eigen::VectorXf model_coefficients; + /** \brief The type of model to use (user given parameter). */ + int model_type_; + boost::function threshold_function; + + bool + single_threshold( double value ) + { + if( value < thresh ) return true; + return false; + } + + private: + virtual bool + initSACModel (int model_type); + }; + + + //////////////////////////////////////////////////////////////////////////////////////////// + /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). + */ + template<> + class PCL_EXPORTS ModelOutlierRemoval : public Filter + { + typedef pcl::PCLPointCloud2 PointCloud2; + typedef PointCloud2::Ptr PointCloud2Ptr; + typedef PointCloud2::ConstPtr PointCloud2ConstPtr; + typedef SampleConsensusModel::Ptr SampleConsensusModelPtr; + + using Filter::removed_indices_; + using Filter::extract_removed_indices_; + + public: + /** \brief Constructor. */ + ModelOutlierRemoval (bool extract_removed_indices = false) : + Filter::Filter (extract_removed_indices) + { + filter_name_ = "ModelOutlierRemoval"; + model_from_normals = NULL; + normals_distance_weight = 0; + thresh = 0; + setThresholdFunction(&pcl::ModelOutlierRemoval::single_threshold, *this ); + } + + /** \brief Set whether the filtered points should be kept and set to the + * value given through \a setUserFilterValue (default: NaN), or removed + * from the PointCloud, thus potentially breaking its organized + * structure. By default, points are removed. + * + * \param[in] val set to true whether the filtered points should be kept and + * set to a given user value (default: NaN) + */ + inline void + setKeepOrganized (bool val) + { + keep_organized_ = val; + } + + /** \brief Obtain the value of the internal \a keep_organized_ parameter. */ + inline bool + getKeepOrganized () + { + return (keep_organized_); + } + + /** \brief Provide a value that the filtered points should be set to + * instead of removing them. Used in conjunction with \a + * setKeepOrganized (). + * \param[in] val the user given value that the filtered point dimensions should be set to + */ + inline void + setUserFilterValue (float val) + { + user_filter_value_ = val; + } + + /** \brief sets the models coefficients */ + inline void + setModelCoefficients (const pcl::ModelCoefficients model_coefficients_) + { + model_coefficients.resize( model_coefficients_.values.size() ); + for(unsigned int i = 0; i < model_coefficients_.values.size(); i++) + { + model_coefficients[i] = model_coefficients_.values[i]; + } + } + /** \brief returns the models coefficients + */ + pcl::ModelCoefficients + getModelCoefficients() + { + pcl::ModelCoefficients mc; + mc.values.resize( model_coefficients.size() ); + for( unsigned int i = 0; i < mc.values.size(); i++) + mc.values[i] = model_coefficients[i]; + return mc; + } + /** \brief Set the type of SAC model used. */ + inline void + setModelType (int model) + { + model_type_ = model; + } + + /** \brief Get the type of SAC model used. */ + inline int + getModelType () + { + return (model_type_); + } + + /** \brief Set the thresholdfunction*/ + inline void + setThreshold( float thresh_) + { + thresh = thresh_; + } + + /** \brief Get the thresholdfunction*/ + inline float + getThreshold() + { + return thresh; + } + + /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max) + * Default: false. + * \warning This method will be removed in the future. Use setNegative() instead. + * \param[in] limit_negative return data inside the interval (false) or outside (true) + */ + inline void + setFilterLimitsNegative (const bool limit_negative) + { + negative_ = limit_negative; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline void + getFilterLimitsNegative (bool &limit_negative) + { + limit_negative = negative_; + } + + /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). + * \warning This method will be removed in the future. Use getNegative() instead. + * \return true if data \b outside the interval [min; max] is to be returned, false otherwise + */ + inline bool + getFilterLimitsNegative () + { + return (negative_); + } + + /** \brief Set the normals cloud*/ + inline void + setInputNormals( pcl::PointCloud::Ptr normals_ptr) + { + cloud_normals = normals_ptr; + } + /** \brief Get the normals cloud*/ + inline pcl::PointCloud::Ptr + getInputNormals() + { + return cloud_normals; + } + /** \brief Set the normals distance weight*/ + inline void + setNormalDistanceWeight( const double& weight) + { + normals_distance_weight = weight; + } + /** \brief get the normal distance weight*/ + inline double + getNormalDistanceWeight() + { + return normals_distance_weight; + } + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + */ + + void + setThresholdFunction (boost::function thresh_) + { + threshold_function = thresh_; + }; + + + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + * \param[in] instance + */ + template void + setThresholdFunction (bool (T::*thresh_function) (double), T& instance) + { + setThresholdFunction (boost::bind (thresh_function, boost::ref (instance), _1)); + } + + + protected: + void + applyFilter (PointCloud2 &output); + + boost::function threshold_function; + + bool + single_threshold( double value ) + { + if( value < thresh ) return true; + return false; + } + + private: + /** \brief Keep the structure of the data organized, by setting the + * filtered points to the a user given value (NaN by default). + */ + bool keep_organized_; + + /** \brief User given value to be set to any filtered point. Casted to + * the correct field type. + */ + float user_filter_value_; + + /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ + bool negative_; + + double normals_distance_weight; + pcl::PointCloud::Ptr cloud_normals; + + SampleConsensusModelFromNormals< pcl::PointXYZ, pcl::Normal > *model_from_normals; + /** \brief The model used to calculate distances */ + SampleConsensusModelPtr model; + /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ + float thresh; + /** \brief The model coefficients */ + Eigen::VectorXf model_coefficients; + /** \brief The type of model to use (user given parameter). */ + int model_type_; + virtual bool + initSACModel (int model_type); + }; + +} + +#endif // PCL_FILTERS_ModelOutlierRemoval_H_ + diff --git a/filters/src/model_outlier_removal.cpp b/filters/src/model_outlier_removal.cpp new file mode 100644 index 00000000000..ba7822258c4 --- /dev/null +++ b/filters/src/model_outlier_removal.cpp @@ -0,0 +1,374 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, 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. + * + * + */ + +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +bool +pcl::ModelOutlierRemoval::initSACModel (int model_type) +{ + // Convert the input data + PointCloud cloud; + fromPCLPointCloud2 (*input_, cloud); + PointCloud::Ptr cloud_ptr = cloud.makeShared (); + + model_from_normals = NULL; + // Build the model + switch (model_type) + { + case SACMODEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelPlane (cloud_ptr)); + break; + } + case SACMODEL_LINE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelLine (cloud_ptr)); + break; + } + case SACMODEL_CIRCLE2D: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelCircle2D (cloud_ptr)); + break; + } + case SACMODEL_SPHERE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelSphere (cloud_ptr)); + break; + } + case SACMODEL_PARALLEL_LINE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelParallelLine (cloud_ptr)); + break; + } + case SACMODEL_PERPENDICULAR_PLANE: + { + //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelPerpendicularPlane (cloud_ptr)); + break; + } + case SACMODEL_CYLINDER: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); + SampleConsensusModelCylinder *ptr = + new SampleConsensusModelCylinder (cloud_ptr); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalPlane *ptr = + new SampleConsensusModelNormalPlane (cloud_ptr); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_CONE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); + SampleConsensusModelCone *ptr = + new SampleConsensusModelCone (cloud_ptr); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_SPHERE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); + SampleConsensusModelNormalSphere *ptr = + new SampleConsensusModelNormalSphere (cloud_ptr); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_NORMAL_PARALLEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalParallelPlane *ptr + = new SampleConsensusModelNormalParallelPlane (cloud_ptr); + model_from_normals = ptr; + model.reset (ptr); + break; + } + case SACMODEL_PARALLEL_PLANE: + { + //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); + model.reset (new SampleConsensusModelParallelPlane (cloud_ptr)); + break; + } + default: + { + PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); + return (false); + } + } + return (true); +} + + + +////////////////////////////////////////////////////////////////////////// +void +pcl::ModelOutlierRemoval::applyFilter (PointCloud2 &output) +{ + if (!input_) + { + PCL_ERROR ("[pcl::%s::applyFilter] Input dataset not given!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.data.clear (); + return; + } + + // If fields x/y/z are not present, we cannot filter + if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) + { + PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.data.clear (); + return; + } + + bool valid_setup = initSACModel (model_type_); + + if( model_from_normals ) + { + if( !cloud_normals ) + { + valid_setup = false; + PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); + } else { + model_from_normals->setNormalDistanceWeight( normals_distance_weight ); + model_from_normals->setInputNormals(cloud_normals); + } + } + + if (!valid_setup) + { + // Silly - if no filtering is actually done, and we want to keep the data organized, + // just copy everything. Any optimizations that can be done here??? + output = *input_; + return; + } + + int nr_points = input_->width * input_->height; + // get distances: + std::vector distances; + model->getDistancesToModel (model_coefficients, distances); + + + // Check if we're going to keep the organized structure of the cloud or not + if (keep_organized_) + { + output.width = input_->width; + output.height = input_->height; + // Check what the user value is: if !finite, set is_dense to false, true otherwise + if (!pcl_isfinite (user_filter_value_)) + output.is_dense = false; + else + output.is_dense = input_->is_dense; + } + else + { + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // filtering breaks the organized structure + // Because we're doing explit checks for isfinite, is_dense = true + output.is_dense = true; + } + output.row_step = input_->row_step; + output.point_step = input_->point_step; + output.is_bigendian = input_->is_bigendian; + output.data.resize (input_->data.size ()); + + removed_indices_->resize (input_->data.size ()); + + int nr_p = 0; + int nr_removed_p = 0; + // Create the first xyz_offset + Eigen::Array4i xyz_offset (input_->fields[x_idx_].offset, input_->fields[y_idx_].offset, + input_->fields[z_idx_].offset, 0); + + Eigen::Vector4f pt = Eigen::Vector4f::Zero (); + + // @todo fixme + if (input_->fields[x_idx_].datatype != pcl::PCLPointField::FLOAT32 || + input_->fields[y_idx_].datatype != pcl::PCLPointField::FLOAT32 || + input_->fields[z_idx_].datatype != pcl::PCLPointField::FLOAT32 + ) + { + PCL_ERROR ("[pcl::%s::downsample] Distance filtering requested, but distances are not float/double in the dataset! Only FLOAT32/FLOAT64 distances are supported right now.\n", getClassName ().c_str ()); + output.width = output.height = 0; + output.data.clear (); + return; + } + float badpt = std::numeric_limits::quiet_NaN (); + // Check whether we need to store filtered valued in place + bool thresh_result = false; + if (keep_organized_) + { + // Go over all points + for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) + { + // Copy all the fields + memcpy (&output.data[cp * output.point_step], &input_->data[cp * output.point_step], output.point_step); + + + // use threshold to seperate outliers (remove list) from inliers + thresh_result = threshold_function ( distances[cp] ); + if (negative_ ) + { + // Use a threshold for cutting out points which inside the interval + if (thresh_result) + { + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy (&output.data[xyz_offset[0]], &badpt, sizeof(float)); + memcpy (&output.data[xyz_offset[1]], &badpt, sizeof(float)); + memcpy (&output.data[xyz_offset[2]], &badpt, sizeof(float)); + continue; + } + else + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + } + } + else + { + // Use a threshold for cutting out points which are too close/far away + if (!thresh_result) + { + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy (&output.data[xyz_offset[0]], &badpt, sizeof(float)); + memcpy (&output.data[xyz_offset[1]], &badpt, sizeof(float)); + memcpy (&output.data[xyz_offset[2]], &badpt, sizeof(float)); + continue; + } + else + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + } + } + } + } + // Remove filtered points + else + { + // Go over all points + for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) + { + + // use threshold to seperate outliers (remove list) from inliers + thresh_result = threshold_function ( distances[cp] ); + if (negative_) + { + // Use a threshold for cutting out points which inside the interval + if (thresh_result) + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + } + else + { + // Use a threshold for cutting out points which are too close/far away + if (thresh_result) + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + } + + // Unoptimized memcpys: assume fields x, y, z are in random order + memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof(float)); + memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof(float)); + memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof(float)); + + // Check if the point is invalid + if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) + { + if (extract_removed_indices_) + { + (*removed_indices_)[nr_removed_p] = cp; + nr_removed_p++; + } + continue; + } + + // Copy all the fields + memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step); + nr_p++; + } + output.width = nr_p; + } // !keep_organized_ + // No distance filtering, process all data. No need to check for is_organized here as we did it above + + output.row_step = output.point_step * output.width; + output.data.resize (output.width * output.height * output.point_step); + + removed_indices_->resize (nr_removed_p); + +} + +#ifndef PCL_NO_PRECOMPILE +#include +#include + +// Instantiations of specific point types +PCL_INSTANTIATE(ModelOutlierRemoval, PCL_XYZ_POINT_TYPES) + +#endif // PCL_NO_PRECOMPILE + diff --git a/test/filters/CMakeLists.txt b/test/filters/CMakeLists.txt index 5859079d18a..9d1246b1aaf 100644 --- a/test/filters/CMakeLists.txt +++ b/test/filters/CMakeLists.txt @@ -18,6 +18,11 @@ PCL_ADD_TEST(filters_bilateral test_filters_bilateral PCL_ADD_TEST(filters_grid_minimum test_filters_grid_minimum FILES test_grid_minimum.cpp LINK_WITH pcl_gtest pcl_common pcl_filters) + +PCL_ADD_TEST(filters_model_outlier_removal test_model_outlier_removal + FILES test_model_outlier_removal.cpp + LINK_WITH pcl_gtest pcl_filters pcl_io pcl_sample_consensus pcl_kdtree pcl_features + ARGUMENTS ${PCL_SOURCE_DIR}/test/milk_cartoon_all_small_clorox.pcd) PCL_ADD_TEST(filters_morphological test_morphological FILES test_morphological.cpp diff --git a/test/filters/test_model_outlier_removal.cpp b/test/filters/test_model_outlier_removal.cpp new file mode 100644 index 00000000000..48430625e50 --- /dev/null +++ b/test/filters/test_model_outlier_removal.cpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2012, Willow Garage, Inc. + * 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. + * + */ + +#include +#include +#include + +#include +#include + +/* expectation: + a model found by ransac has the same inliers and outliers as the same model filtered with model_outlier_removal + as long as the thresholdfunction of ransac and model_outlier_removal is the same */ + + +using namespace pcl; + +PointCloud::Ptr cloud_in (new PointCloud); + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (ModelOutlierRemoval, Model_Outlier_Filter) +{ + PointCloud::Ptr cloud_filter_out (new PointCloud); + std::vector ransac_inliers; + float thresh = 0.01; + //run ransac + Eigen::VectorXf model_coefficients; + pcl::SampleConsensusModelPlane::Ptr + model_p (new pcl::SampleConsensusModelPlane (cloud_in)); + RandomSampleConsensus ransac (model_p); + ransac.setDistanceThreshold ( thresh ); + ransac.computeModel(); + ransac.getInliers(ransac_inliers); + ransac.getModelCoefficients( model_coefficients ); + // test ransacs result + EXPECT_EQ (model_coefficients.size(), 4); + if( model_coefficients.size() != 4) return; + //run filter + pcl::ModelCoefficients model_coeff; + model_coeff.values.resize(4); + for(int i = 0; i < 4; i++) model_coeff.values[i] = model_coefficients[i]; + pcl::ModelOutlierRemoval filter; + filter.setModelCoefficients( model_coeff); + filter.setThreshold( thresh ); + filter.setModelType( pcl::SACMODEL_PLANE ); + filter.setInputCloud( cloud_in ); + filter.filter( *cloud_filter_out ); + //compare results + EXPECT_EQ (cloud_filter_out->points.size(), ransac_inliers.size() ); + //TODO: also compare content +} + +/* ---[ */ +int +main (int argc, + char** argv) +{ + // Load a standard PCD file from disk + if (argc < 2) + { + std::cerr << "No test file given. Please download `milk_cartoon_all_small_clorox.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + char* file_name = argv[1]; + // Load a standard PCD file from disk + io::loadPCDFile (file_name, *cloud_in); + + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */ From f12067520ff5ca937144051d5db14b35fbb699e4 Mon Sep 17 00:00:00 2001 From: Victor Lamoine Date: Mon, 26 May 2014 21:40:35 +0200 Subject: [PATCH 2/6] Code upate Signed-off-by: Victor Lamoine --- .../filters/impl/model_outlier_removal.hpp | 134 ++--- .../pcl/filters/model_outlier_removal.h | 476 ++++-------------- filters/src/model_outlier_removal.cpp | 343 +------------ test/filters/test_model_outlier_removal.cpp | 50 +- 4 files changed, 203 insertions(+), 800 deletions(-) diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp index 377003084ca..65ce9e2e688 100644 --- a/filters/include/pcl/filters/impl/model_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * @@ -16,7 +17,7 @@ * 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 + * * 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. * @@ -32,109 +33,114 @@ * 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_FILTERS_IMPL_ModelOutlierRemoval_HPP_ -#define PCL_FILTERS_IMPL_ModelOutlierRemoval_HPP_ +#ifndef PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ +#define PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::ModelOutlierRemoval::initSACModel (int model_type) +pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) { - model_from_normals = NULL; + model_from_normals_ = NULL; // Build the model switch (model_type) { case SACMODEL_PLANE: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelPlane (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPlane (input_)); break; } case SACMODEL_LINE: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelLine (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelLine (input_)); break; } case SACMODEL_CIRCLE2D: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelCircle2D (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelCircle2D (input_)); break; } case SACMODEL_SPHERE: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelSphere (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelSphere (input_)); break; } case SACMODEL_PARALLEL_LINE: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelParallelLine (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelLine (input_)); break; } case SACMODEL_PERPENDICULAR_PLANE: { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelPerpendicularPlane (input_)); + PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelPerpendicularPlane (input_)); break; } case SACMODEL_CYLINDER: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); - SampleConsensusModelCylinder *ptr = - new SampleConsensusModelCylinder (input_); - model_from_normals = ptr; - model.reset (ptr); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); + SampleConsensusModelCylinder *ptr = new SampleConsensusModelCylinder (input_); + model_from_normals_ = ptr; + model_.reset (ptr); break; } case SACMODEL_NORMAL_PLANE: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalPlane *ptr = - new SampleConsensusModelNormalPlane (input_); - model_from_normals = ptr; - model.reset (ptr); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalPlane *ptr = new SampleConsensusModelNormalPlane (input_); + model_from_normals_ = ptr; + model_.reset (ptr); break; } case SACMODEL_CONE: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); - SampleConsensusModelCone *ptr = - new SampleConsensusModelCone (input_); - model_from_normals = ptr; - model.reset (ptr); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); + SampleConsensusModelCone *ptr = new SampleConsensusModelCone (input_); + model_from_normals_ = ptr; + model_.reset (ptr); break; } case SACMODEL_NORMAL_SPHERE: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); - SampleConsensusModelNormalSphere *ptr = - new SampleConsensusModelNormalSphere (input_); - model_from_normals = ptr; - model.reset (ptr); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); + SampleConsensusModelNormalSphere *ptr = new SampleConsensusModelNormalSphere (input_); + model_from_normals_ = ptr; + model_.reset (ptr); break; } case SACMODEL_NORMAL_PARALLEL_PLANE: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalParallelPlane *ptr - = new SampleConsensusModelNormalParallelPlane (input_); - model_from_normals = ptr; - model.reset (ptr); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); + SampleConsensusModelNormalParallelPlane *ptr = new SampleConsensusModelNormalParallelPlane (input_); + model_from_normals_ = ptr; + model_.reset (ptr); break; } case SACMODEL_PARALLEL_PLANE: { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelParallelPlane (input_)); + PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); + model_.reset (new SampleConsensusModelParallelPlane (input_)); break; } default: @@ -146,7 +152,6 @@ pcl::ModelOutlierRemoval::initSACModel (int model_type) return (true); } - //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) @@ -161,7 +166,7 @@ pcl::ModelOutlierRemoval::applyFilter (PointCloud &output) output = *input_; for (int rii = 0; rii < static_cast (removed_indices_->size ()); ++rii) // rii = removed indices iterator - output.points[(*removed_indices_)[rii]].x = output.points[(*removed_indices_)[rii]].y = output.points[(*removed_indices_)[rii]].z = user_filter_value_; + output.points[ (*removed_indices_)[rii]].x = output.points[ (*removed_indices_)[rii]].y = output.points[ (*removed_indices_)[rii]].z = user_filter_value_; if (!pcl_isfinite (user_filter_value_)) output.is_dense = false; } @@ -183,29 +188,29 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) //is the filtersetup correct? bool valid_setup = true; - valid_setup &= initSACModel (model_type_); - if( model_from_normals ) + if (model_from_normals_) { - if( !cloud_normals ) + if (!cloud_normals_) { valid_setup = false; PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); - } else { - model_from_normals->setNormalDistanceWeight( normals_distance_weight ); - model_from_normals->setInputNormals(cloud_normals); + } + else + { + model_from_normals_->setNormalDistanceWeight (normals_distance_weight_); + model_from_normals_->setInputNormals (cloud_normals_); } } //if the filter setup is invalid filter for nan and return; - if(! valid_setup ) + if (!valid_setup) { for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator { // Non-finite entries are always passed to removed indices - if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || - !pcl_isfinite (input_->points[(*indices_)[iii]].y) || - !pcl_isfinite (input_->points[(*indices_)[iii]].z)) + if (!pcl_isfinite (input_->points[ (*indices_)[iii]].x) || !pcl_isfinite (input_->points[ (*indices_)[iii]].y) + || !pcl_isfinite (input_->points[ (*indices_)[iii]].z)) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; @@ -218,25 +223,23 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) // check distance of pointcloud to model std::vector distances; //TODO: get signed distances ! - model->getDistancesToModel (model_coefficients, distances); + model_->getDistancesToModel (model_coefficients_, distances); bool thresh_result; // Filter for non-finite entries and the specified field limits for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator { // Non-finite entries are always passed to removed indices - if (!pcl_isfinite (input_->points[(*indices_)[iii]].x) || - !pcl_isfinite (input_->points[(*indices_)[iii]].y) || - !pcl_isfinite (input_->points[(*indices_)[iii]].z)) + if (!pcl_isfinite (input_->points[ (*indices_)[iii]].x) || !pcl_isfinite (input_->points[ (*indices_)[iii]].y) + || !pcl_isfinite (input_->points[ (*indices_)[iii]].z)) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; continue; } - // use threshold function to seperate outliers from inliers: - thresh_result = threshold_function ( distances[iii] ); + thresh_result = threshold_function_ (distances[iii]); // in normal mode: define outliers as false thresh_result if (!negative_ && !thresh_result) @@ -267,5 +270,4 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) #define PCL_INSTANTIATE_ModelOutlierRemoval(T) template class PCL_EXPORTS pcl::ModelOutlierRemoval; -#endif // PCL_FILTERS_IMPL_ModelOutlierRemoval_HPP_ - +#endif // PCL_FILTERS_IMPL_MODEL_OUTLIER_REMOVAL_HPP_ diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h index 20c7dc271ce..58577102a7c 100644 --- a/filters/include/pcl/filters/model_outlier_removal.h +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -3,6 +3,7 @@ * * Point Cloud Library (PCL) - www.pointclouds.org * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. * @@ -16,7 +17,7 @@ * 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 + * * 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. * @@ -32,12 +33,10 @@ * 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_FILTERS_ModelOutlierRemoval_H_ -#define PCL_FILTERS_ModelOutlierRemoval_H_ +#ifndef PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ +#define PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ #include #include @@ -45,44 +44,27 @@ // Sample Consensus models #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - - - namespace pcl { - /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. - * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside - * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). - *

- * Usage example: - * \code - * pcl::ModelCoefficients model_coeff; - * model_coeff.values.resize(4); - * model_coeff.values[0] = 0; model_coeff.values[1] = 0; model_coeff.values[2] = 1.5; model_coeff.values[3] = 0.5; - * pcl::ModelOutlierRemoval filter; - * filter.setModelCoefficients( model_coeff); - * filter.setThreshold( 0.1 ); - * filter.setModelType( pcl::SACMODEL_PLANE ); - * filter.setInputCloud( *cloud_in ); - * filter.setFilterLimitsNegative(false); - * filter.filter( *cloud_out ); - * \endcode - */ + * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside + * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). + *

+ * Usage example: + * \code + * pcl::ModelCoefficients model_coeff; + * model_coeff.values.resize(4); + * model_coeff.values[0] = 0; model_coeff.values[1] = 0; model_coeff.values[2] = 1.5; model_coeff.values[3] = 0.5; + * pcl::ModelOutlierRemoval filter; + * filter.setModelCoefficients (model_coeff); + * filter.setThreshold (0.1); + * filter.setModelType (pcl::SACMODEL_PLANE); + * filter.setInputCloud (*cloud_in); + * filter.setFilterLimitsNegative (false); + * filter.filter (*cloud_out); + * \endcode + */ template class ModelOutlierRemoval : public FilterIndices { @@ -92,149 +74,119 @@ namespace pcl typedef typename PointCloud::ConstPtr PointCloudConstPtr; typedef typename SampleConsensusModel::Ptr SampleConsensusModelPtr; - public: /** \brief Constructor. - * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). - */ + * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). + */ + inline ModelOutlierRemoval (bool extract_removed_indices = false) : - FilterIndices::FilterIndices (extract_removed_indices) + FilterIndices::FilterIndices (extract_removed_indices) { - model_from_normals = NULL; - thresh = 0; - normals_distance_weight = 0; + model_from_normals_ = NULL; + thresh_ = 0; + normals_distance_weight_ = 0; filter_name_ = "ModelOutlierRemoval"; - setThresholdFunction(&pcl::ModelOutlierRemoval::single_threshold, *this ); + setThresholdFunction (&pcl::ModelOutlierRemoval::checkSingleThreshold, *this); } + /** \brief sets the models coefficients */ - inline void - setModelCoefficients (const pcl::ModelCoefficients model_coefficients_) + inline void + setModelCoefficients (const pcl::ModelCoefficients model_coefficients) { - model_coefficients.resize( model_coefficients_.values.size() ); - for(unsigned int i = 0; i < model_coefficients_.values.size(); i++) - { - model_coefficients[i] = model_coefficients_.values[i]; - } + model_coefficients_.resize (model_coefficients.values.size ()); + for (unsigned int i = 0; i < model_coefficients.values.size (); i++) + { + model_coefficients_[i] = model_coefficients.values[i]; + } } + /** \brief returns the models coefficients - */ - pcl::ModelCoefficients - getModelCoefficients() + */ + inline pcl::ModelCoefficients + getModelCoefficients () const { - pcl::ModelCoefficients mc; - mc.values.resize( model_coefficients.size() ); - for( unsigned int i = 0; i < mc.values.size(); i++) - mc.values[i] = model_coefficients[i]; - return mc; + pcl::ModelCoefficients mc; + mc.values.resize (model_coefficients_.size ()); + for (unsigned int i = 0; i < mc.values.size (); i++) + mc.values[i] = model_coefficients_[i]; + return (mc); } + /** \brief Set the type of SAC model used. */ inline void - setModelType (int model) + setModelType (pcl::SacModel model) { model_type_ = model; } /** \brief Get the type of SAC model used. */ - inline int - getModelType () + inline pcl::SacModel + getModelType () const { return (model_type_); } /** \brief Set the thresholdfunction*/ inline void - setThreshold( float thresh_) + setThreshold (float thresh) { - thresh = thresh_; + thresh_ = thresh; } /** \brief Get the thresholdfunction*/ inline float - getThreshhold() + getThreshold () const { - return thresh; + return (thresh_); } /** \brief Set the normals cloud*/ inline void - setInputNormals( pcl::PointCloud::Ptr normals_ptr) + setInputNormals (pcl::PointCloud::Ptr normals_ptr) { - cloud_normals = normals_ptr; + cloud_normals_ = normals_ptr; } + /** \brief Get the normals cloud*/ inline pcl::PointCloud::Ptr - getInputNormals() + getInputNormals () const { - return cloud_normals; + return (cloud_normals_); } + /** \brief Set the normals distance weight*/ inline void - setNormalDistanceWeight( const double& weight) + setNormalDistanceWeight (const double weight) { - normals_distance_weight = weight; + normals_distance_weight_ = weight; } + /** \brief get the normal distance weight*/ inline double - getNormalDistanceWeight() + getNormalDistanceWeight () const { - return normals_distance_weight; + return (normals_distance_weight_); } - - - /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max) - * Default: false. - * \warning This method will be removed in the future. Use setNegative() instead. - * \param[in] limit_negative return data inside the interval (false) or outside (true) - */ - inline void - setFilterLimitsNegative (const bool limit_negative) - { - negative_ = limit_negative; - } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). - * \warning This method will be removed in the future. Use getNegative() instead. - * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise - */ - inline void - getFilterLimitsNegative (bool &limit_negative) - { - limit_negative = negative_; - } - - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). - * \warning This method will be removed in the future. Use getNegative() instead. - * \return true if data \b outside the interval [min; max] is to be returned, false otherwise - */ - inline bool - getFilterLimitsNegative () - { - return (negative_); - } - - /** \brief Register a different threshold function - * \param[in] pointer to a threshold function - */ - + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + */ void - setThresholdFunction (boost::function thresh_) + setThresholdFunction (boost::function thresh) { - threshold_function = thresh_; - }; - + threshold_function_ = thresh; + } - /** \brief Register a different threshold function - * \param[in] pointer to a threshold function - * \param[in] instance - */ - template void + /** \brief Register a different threshold function + * \param[in] pointer to a threshold function + * \param[in] instance + */ + template void setThresholdFunction (bool (T::*thresh_function) (double), T& instance) { setThresholdFunction (boost::bind (thresh_function, boost::ref (instance), _1)); } - protected: using PCLBase::input_; using PCLBase::indices_; @@ -247,14 +199,14 @@ namespace pcl using FilterIndices::removed_indices_; /** \brief Filtered results are stored in a separate point cloud. - * \param[out] output The resultant point cloud. - */ + * \param[out] output The resultant point cloud. + */ void applyFilter (PointCloud &output); /** \brief Filtered results are indexed by an indices array. - * \param[out] indices The resultant indices. - */ + * \param[out] indices The resultant indices. + */ void applyFilter (std::vector &indices) { @@ -262,269 +214,43 @@ namespace pcl } /** \brief Filtered results are indexed by an indices array. - * \param[out] indices The resultant indices. - */ + * \param[out] indices The resultant indices. + */ void applyFilterIndices (std::vector &indices); protected: - double normals_distance_weight; - pcl::PointCloud::Ptr cloud_normals; - SampleConsensusModelFromNormals< PointT, pcl::Normal > *model_from_normals; - /** \brief The model used to calculate distances */ - SampleConsensusModelPtr model; - /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ - float thresh; - /** \brief The model coefficients */ - Eigen::VectorXf model_coefficients; - /** \brief The type of model to use (user given parameter). */ - int model_type_; - boost::function threshold_function; + double normals_distance_weight_; + pcl::PointCloud::Ptr cloud_normals_; + SampleConsensusModelFromNormals *model_from_normals_; - bool - single_threshold( double value ) - { - if( value < thresh ) return true; - return false; - } - - private: - virtual bool - initSACModel (int model_type); - }; - - - //////////////////////////////////////////////////////////////////////////////////////////// - /** \brief @b ModelOutlierRemoval filters points in a cloud based on the distance between model and point. - * \details Iterates through the entire input once, automatically filtering non-finite points and the points outside - * the model specified by setSampleConsensusModelPointer() and the threshold specified by setThreholdFunctionPointer(). - */ - template<> - class PCL_EXPORTS ModelOutlierRemoval : public Filter - { - typedef pcl::PCLPointCloud2 PointCloud2; - typedef PointCloud2::Ptr PointCloud2Ptr; - typedef PointCloud2::ConstPtr PointCloud2ConstPtr; - typedef SampleConsensusModel::Ptr SampleConsensusModelPtr; - - using Filter::removed_indices_; - using Filter::extract_removed_indices_; - - public: - /** \brief Constructor. */ - ModelOutlierRemoval (bool extract_removed_indices = false) : - Filter::Filter (extract_removed_indices) - { - filter_name_ = "ModelOutlierRemoval"; - model_from_normals = NULL; - normals_distance_weight = 0; - thresh = 0; - setThresholdFunction(&pcl::ModelOutlierRemoval::single_threshold, *this ); - } - - /** \brief Set whether the filtered points should be kept and set to the - * value given through \a setUserFilterValue (default: NaN), or removed - * from the PointCloud, thus potentially breaking its organized - * structure. By default, points are removed. - * - * \param[in] val set to true whether the filtered points should be kept and - * set to a given user value (default: NaN) - */ - inline void - setKeepOrganized (bool val) - { - keep_organized_ = val; - } - - /** \brief Obtain the value of the internal \a keep_organized_ parameter. */ - inline bool - getKeepOrganized () - { - return (keep_organized_); - } - - /** \brief Provide a value that the filtered points should be set to - * instead of removing them. Used in conjunction with \a - * setKeepOrganized (). - * \param[in] val the user given value that the filtered point dimensions should be set to - */ - inline void - setUserFilterValue (float val) - { - user_filter_value_ = val; - } - - /** \brief sets the models coefficients */ - inline void - setModelCoefficients (const pcl::ModelCoefficients model_coefficients_) - { - model_coefficients.resize( model_coefficients_.values.size() ); - for(unsigned int i = 0; i < model_coefficients_.values.size(); i++) - { - model_coefficients[i] = model_coefficients_.values[i]; - } - } - /** \brief returns the models coefficients - */ - pcl::ModelCoefficients - getModelCoefficients() - { - pcl::ModelCoefficients mc; - mc.values.resize( model_coefficients.size() ); - for( unsigned int i = 0; i < mc.values.size(); i++) - mc.values[i] = model_coefficients[i]; - return mc; - } - /** \brief Set the type of SAC model used. */ - inline void - setModelType (int model) - { - model_type_ = model; - } - - /** \brief Get the type of SAC model used. */ - inline int - getModelType () - { - return (model_type_); - } - - /** \brief Set the thresholdfunction*/ - inline void - setThreshold( float thresh_) - { - thresh = thresh_; - } + /** \brief The model used to calculate distances */ + SampleConsensusModelPtr model_; - /** \brief Get the thresholdfunction*/ - inline float - getThreshold() - { - return thresh; - } + /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ + float thresh_; - /** \brief Set to true if we want to return the data outside the interval specified by setFilterLimits (min, max) - * Default: false. - * \warning This method will be removed in the future. Use setNegative() instead. - * \param[in] limit_negative return data inside the interval (false) or outside (true) - */ - inline void - setFilterLimitsNegative (const bool limit_negative) - { - negative_ = limit_negative; - } + /** \brief The model coefficients */ + Eigen::VectorXf model_coefficients_; - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). - * \warning This method will be removed in the future. Use getNegative() instead. - * \param[out] limit_negative true if data \b outside the interval [min; max] is to be returned, false otherwise - */ - inline void - getFilterLimitsNegative (bool &limit_negative) - { - limit_negative = negative_; - } + /** \brief The type of model to use (user given parameter). */ + pcl::SacModel model_type_; + boost::function threshold_function_; - /** \brief Get whether the data outside the interval (min/max) is to be returned (true) or inside (false). - * \warning This method will be removed in the future. Use getNegative() instead. - * \return true if data \b outside the interval [min; max] is to be returned, false otherwise - */ inline bool - getFilterLimitsNegative () + checkSingleThreshold (double value) { - return (negative_); - } - - /** \brief Set the normals cloud*/ - inline void - setInputNormals( pcl::PointCloud::Ptr normals_ptr) - { - cloud_normals = normals_ptr; - } - /** \brief Get the normals cloud*/ - inline pcl::PointCloud::Ptr - getInputNormals() - { - return cloud_normals; - } - /** \brief Set the normals distance weight*/ - inline void - setNormalDistanceWeight( const double& weight) - { - normals_distance_weight = weight; - } - /** \brief get the normal distance weight*/ - inline double - getNormalDistanceWeight() - { - return normals_distance_weight; - } - - /** \brief Register a different threshold function - * \param[in] pointer to a threshold function - */ - - void - setThresholdFunction (boost::function thresh_) - { - threshold_function = thresh_; - }; - - - /** \brief Register a different threshold function - * \param[in] pointer to a threshold function - * \param[in] instance - */ - template void - setThresholdFunction (bool (T::*thresh_function) (double), T& instance) - { - setThresholdFunction (boost::bind (thresh_function, boost::ref (instance), _1)); - } - - - protected: - void - applyFilter (PointCloud2 &output); - - boost::function threshold_function; - - bool - single_threshold( double value ) - { - if( value < thresh ) return true; - return false; + return (value < thresh_); } private: - /** \brief Keep the structure of the data organized, by setting the - * filtered points to the a user given value (NaN by default). - */ - bool keep_organized_; - - /** \brief User given value to be set to any filtered point. Casted to - * the correct field type. - */ - float user_filter_value_; - - /** \brief Set to true if we want to return the data outside (\a filter_limit_min_;\a filter_limit_max_). Default: false. */ - bool negative_; - - double normals_distance_weight; - pcl::PointCloud::Ptr cloud_normals; - - SampleConsensusModelFromNormals< pcl::PointXYZ, pcl::Normal > *model_from_normals; - /** \brief The model used to calculate distances */ - SampleConsensusModelPtr model; - /** \brief The threshold used to seperate outliers (removed_indices) from inliers (indices) */ - float thresh; - /** \brief The model coefficients */ - Eigen::VectorXf model_coefficients; - /** \brief The type of model to use (user given parameter). */ - int model_type_; virtual bool - initSACModel (int model_type); + initSACModel (pcl::SacModel model_type); }; - } -#endif // PCL_FILTERS_ModelOutlierRemoval_H_ +#ifdef PCL_NO_PRECOMPILE +#include +#endif +#endif // PCL_FILTERS_MODEL_OUTLIER_REMOVAL_H_ diff --git a/filters/src/model_outlier_removal.cpp b/filters/src/model_outlier_removal.cpp index ba7822258c4..67ffc75e69c 100644 --- a/filters/src/model_outlier_removal.cpp +++ b/filters/src/model_outlier_removal.cpp @@ -1,7 +1,10 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2010, Willow Garage, Inc. + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2012-, Open Perception, Inc. + * * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +17,7 @@ * 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 + * * 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. * @@ -30,345 +33,19 @@ * 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 -//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -bool -pcl::ModelOutlierRemoval::initSACModel (int model_type) -{ - // Convert the input data - PointCloud cloud; - fromPCLPointCloud2 (*input_, cloud); - PointCloud::Ptr cloud_ptr = cloud.makeShared (); - - model_from_normals = NULL; - // Build the model - switch (model_type) - { - case SACMODEL_PLANE: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelPlane (cloud_ptr)); - break; - } - case SACMODEL_LINE: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelLINE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelLine (cloud_ptr)); - break; - } - case SACMODEL_CIRCLE2D: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelCIRCLE2D\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelCircle2D (cloud_ptr)); - break; - } - case SACMODEL_SPHERE: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelSPHERE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelSphere (cloud_ptr)); - break; - } - case SACMODEL_PARALLEL_LINE: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPARALLEL_LINE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelParallelLine (cloud_ptr)); - break; - } - case SACMODEL_PERPENDICULAR_PLANE: - { - //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: modelPERPENDICULAR_PLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelPerpendicularPlane (cloud_ptr)); - break; - } - case SACMODEL_CYLINDER: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); - SampleConsensusModelCylinder *ptr = - new SampleConsensusModelCylinder (cloud_ptr); - model_from_normals = ptr; - model.reset (ptr); - break; - } - case SACMODEL_NORMAL_PLANE: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalPlane *ptr = - new SampleConsensusModelNormalPlane (cloud_ptr); - model_from_normals = ptr; - model.reset (ptr); - break; - } - case SACMODEL_CONE: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); - SampleConsensusModelCone *ptr = - new SampleConsensusModelCone (cloud_ptr); - model_from_normals = ptr; - model.reset (ptr); - break; - } - case SACMODEL_NORMAL_SPHERE: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); - SampleConsensusModelNormalSphere *ptr = - new SampleConsensusModelNormalSphere (cloud_ptr); - model_from_normals = ptr; - model.reset (ptr); - break; - } - case SACMODEL_NORMAL_PARALLEL_PLANE: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalParallelPlane *ptr - = new SampleConsensusModelNormalParallelPlane (cloud_ptr); - model_from_normals = ptr; - model.reset (ptr); - break; - } - case SACMODEL_PARALLEL_PLANE: - { - //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelPARALLEL_PLANE\n", getClassName ().c_str ()); - model.reset (new SampleConsensusModelParallelPlane (cloud_ptr)); - break; - } - default: - { - PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ()); - return (false); - } - } - return (true); -} - - - -////////////////////////////////////////////////////////////////////////// -void -pcl::ModelOutlierRemoval::applyFilter (PointCloud2 &output) -{ - if (!input_) - { - PCL_ERROR ("[pcl::%s::applyFilter] Input dataset not given!\n", getClassName ().c_str ()); - output.width = output.height = 0; - output.data.clear (); - return; - } - - // If fields x/y/z are not present, we cannot filter - if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1) - { - PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ()); - output.width = output.height = 0; - output.data.clear (); - return; - } - - bool valid_setup = initSACModel (model_type_); - - if( model_from_normals ) - { - if( !cloud_normals ) - { - valid_setup = false; - PCL_ERROR ("[pcl::ModelOutlierRemoval::applyFilterIndices]: no normals cloud set.\n"); - } else { - model_from_normals->setNormalDistanceWeight( normals_distance_weight ); - model_from_normals->setInputNormals(cloud_normals); - } - } - - if (!valid_setup) - { - // Silly - if no filtering is actually done, and we want to keep the data organized, - // just copy everything. Any optimizations that can be done here??? - output = *input_; - return; - } - - int nr_points = input_->width * input_->height; - // get distances: - std::vector distances; - model->getDistancesToModel (model_coefficients, distances); - - - // Check if we're going to keep the organized structure of the cloud or not - if (keep_organized_) - { - output.width = input_->width; - output.height = input_->height; - // Check what the user value is: if !finite, set is_dense to false, true otherwise - if (!pcl_isfinite (user_filter_value_)) - output.is_dense = false; - else - output.is_dense = input_->is_dense; - } - else - { - // Copy the header (and thus the frame_id) + allocate enough space for points - output.height = 1; // filtering breaks the organized structure - // Because we're doing explit checks for isfinite, is_dense = true - output.is_dense = true; - } - output.row_step = input_->row_step; - output.point_step = input_->point_step; - output.is_bigendian = input_->is_bigendian; - output.data.resize (input_->data.size ()); - - removed_indices_->resize (input_->data.size ()); - - int nr_p = 0; - int nr_removed_p = 0; - // Create the first xyz_offset - Eigen::Array4i xyz_offset (input_->fields[x_idx_].offset, input_->fields[y_idx_].offset, - input_->fields[z_idx_].offset, 0); - - Eigen::Vector4f pt = Eigen::Vector4f::Zero (); - - // @todo fixme - if (input_->fields[x_idx_].datatype != pcl::PCLPointField::FLOAT32 || - input_->fields[y_idx_].datatype != pcl::PCLPointField::FLOAT32 || - input_->fields[z_idx_].datatype != pcl::PCLPointField::FLOAT32 - ) - { - PCL_ERROR ("[pcl::%s::downsample] Distance filtering requested, but distances are not float/double in the dataset! Only FLOAT32/FLOAT64 distances are supported right now.\n", getClassName ().c_str ()); - output.width = output.height = 0; - output.data.clear (); - return; - } - float badpt = std::numeric_limits::quiet_NaN (); - // Check whether we need to store filtered valued in place - bool thresh_result = false; - if (keep_organized_) - { - // Go over all points - for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) - { - // Copy all the fields - memcpy (&output.data[cp * output.point_step], &input_->data[cp * output.point_step], output.point_step); - - - // use threshold to seperate outliers (remove list) from inliers - thresh_result = threshold_function ( distances[cp] ); - if (negative_ ) - { - // Use a threshold for cutting out points which inside the interval - if (thresh_result) - { - // Unoptimized memcpys: assume fields x, y, z are in random order - memcpy (&output.data[xyz_offset[0]], &badpt, sizeof(float)); - memcpy (&output.data[xyz_offset[1]], &badpt, sizeof(float)); - memcpy (&output.data[xyz_offset[2]], &badpt, sizeof(float)); - continue; - } - else - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - } - } - else - { - // Use a threshold for cutting out points which are too close/far away - if (!thresh_result) - { - // Unoptimized memcpys: assume fields x, y, z are in random order - memcpy (&output.data[xyz_offset[0]], &badpt, sizeof(float)); - memcpy (&output.data[xyz_offset[1]], &badpt, sizeof(float)); - memcpy (&output.data[xyz_offset[2]], &badpt, sizeof(float)); - continue; - } - else - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - } - } - } - } - // Remove filtered points - else - { - // Go over all points - for (int cp = 0; cp < nr_points; ++cp, xyz_offset += input_->point_step) - { - - // use threshold to seperate outliers (remove list) from inliers - thresh_result = threshold_function ( distances[cp] ); - if (negative_) - { - // Use a threshold for cutting out points which inside the interval - if (thresh_result) - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - continue; - } - } - else - { - // Use a threshold for cutting out points which are too close/far away - if (thresh_result) - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - continue; - } - } - - // Unoptimized memcpys: assume fields x, y, z are in random order - memcpy (&pt[0], &input_->data[xyz_offset[0]], sizeof(float)); - memcpy (&pt[1], &input_->data[xyz_offset[1]], sizeof(float)); - memcpy (&pt[2], &input_->data[xyz_offset[2]], sizeof(float)); - - // Check if the point is invalid - if (!pcl_isfinite (pt[0]) || !pcl_isfinite (pt[1]) || !pcl_isfinite (pt[2])) - { - if (extract_removed_indices_) - { - (*removed_indices_)[nr_removed_p] = cp; - nr_removed_p++; - } - continue; - } - - // Copy all the fields - memcpy (&output.data[nr_p * output.point_step], &input_->data[cp * output.point_step], output.point_step); - nr_p++; - } - output.width = nr_p; - } // !keep_organized_ - // No distance filtering, process all data. No need to check for is_organized here as we did it above - - output.row_step = output.point_step * output.width; - output.data.resize (output.width * output.height * output.point_step); - - removed_indices_->resize (nr_removed_p); - -} - #ifndef PCL_NO_PRECOMPILE #include #include // Instantiations of specific point types -PCL_INSTANTIATE(ModelOutlierRemoval, PCL_XYZ_POINT_TYPES) +#ifdef PCL_ONLY_CORE_POINT_TYPES + PCL_INSTANTIATE (ModelOutlierRemoval, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)) +#else + PCL_INSTANTIATE (ModelOutlierRemoval, PCL_XYZ_POINT_TYPES) +#endif #endif // PCL_NO_PRECOMPILE - diff --git a/test/filters/test_model_outlier_removal.cpp b/test/filters/test_model_outlier_removal.cpp index 48430625e50..6ed210987ec 100644 --- a/test/filters/test_model_outlier_removal.cpp +++ b/test/filters/test_model_outlier_removal.cpp @@ -2,7 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org - * Copyright (c) 2009-2012, Willow Garage, Inc. + * Copyright (c) 2010-2012, Willow Garage, Inc. * Copyright (c) 2012-, Open Perception, Inc. * * All rights reserved. @@ -33,7 +33,6 @@ * 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 @@ -43,10 +42,9 @@ #include #include -/* expectation: - a model found by ransac has the same inliers and outliers as the same model filtered with model_outlier_removal - as long as the thresholdfunction of ransac and model_outlier_removal is the same */ - +/* Expectation: + A model found by ransac has the same inliers and outliers as the same model filtered with model_outlier_removal + as long as the thresholdfunction of ransac and model_outlier_removal is the same */ using namespace pcl; @@ -60,35 +58,35 @@ TEST (ModelOutlierRemoval, Model_Outlier_Filter) float thresh = 0.01; //run ransac Eigen::VectorXf model_coefficients; - pcl::SampleConsensusModelPlane::Ptr - model_p (new pcl::SampleConsensusModelPlane (cloud_in)); - RandomSampleConsensus ransac (model_p); - ransac.setDistanceThreshold ( thresh ); - ransac.computeModel(); - ransac.getInliers(ransac_inliers); - ransac.getModelCoefficients( model_coefficients ); + pcl::SampleConsensusModelPlane::Ptr model_p (new pcl::SampleConsensusModelPlane (cloud_in)); + RandomSampleConsensus < pcl::PointXYZ > ransac (model_p); + ransac.setDistanceThreshold (thresh); + ransac.computeModel (); + ransac.getInliers (ransac_inliers); + ransac.getModelCoefficients (model_coefficients); // test ransacs result - EXPECT_EQ (model_coefficients.size(), 4); - if( model_coefficients.size() != 4) return; + EXPECT_EQ (model_coefficients.size (), 4); + if (model_coefficients.size () != 4) + return; //run filter pcl::ModelCoefficients model_coeff; - model_coeff.values.resize(4); - for(int i = 0; i < 4; i++) model_coeff.values[i] = model_coefficients[i]; - pcl::ModelOutlierRemoval filter; - filter.setModelCoefficients( model_coeff); - filter.setThreshold( thresh ); - filter.setModelType( pcl::SACMODEL_PLANE ); - filter.setInputCloud( cloud_in ); - filter.filter( *cloud_filter_out ); + model_coeff.values.resize (4); + for (int i = 0; i < 4; i++) + model_coeff.values[i] = model_coefficients[i]; + pcl::ModelOutlierRemoval < pcl::PointXYZ > filter; + filter.setModelCoefficients (model_coeff); + filter.setThreshold (thresh); + filter.setModelType (pcl::SACMODEL_PLANE); + filter.setInputCloud (cloud_in); + filter.filter (*cloud_filter_out); //compare results - EXPECT_EQ (cloud_filter_out->points.size(), ransac_inliers.size() ); + EXPECT_EQ (cloud_filter_out->points.size (), ransac_inliers.size ()); //TODO: also compare content } /* ---[ */ int -main (int argc, - char** argv) +main (int argc, char** argv) { // Load a standard PCD file from disk if (argc < 2) From 9d6efeeacd7aab16d195c5f06e3d4db8134c3be3 Mon Sep 17 00:00:00 2001 From: Victor Lamoine Date: Mon, 26 May 2014 22:19:09 +0200 Subject: [PATCH 3/6] Update tutorial --- doc/tutorials/content/index.rst | 14 +++++ .../content/model_outlier_removal.rst | 2 +- .../model_outlier_removal.cpp | 63 ++++++++++--------- 3 files changed, 47 insertions(+), 32 deletions(-) diff --git a/doc/tutorials/content/index.rst b/doc/tutorials/content/index.rst index 5d66809e90e..fa8e316f9d2 100644 --- a/doc/tutorials/content/index.rst +++ b/doc/tutorials/content/index.rst @@ -1035,6 +1035,20 @@ Segmentation .. |se_10| image:: images/progressive_morphological_filter.png :height: 100px + * :ref:`model_outlier_removal` + + ======= ====== + |se_11| Title: **Model outlier removal** + + Author: *Timo Häckel* + + Compatibility: >= PCL 1.7.2 + + This tutorial describes how to extract points from a point cloud using SAC models + ======= ====== + + .. |se_11| image:: images/pcl_logo.png + :height: 75px .. _surface_tutorial: diff --git a/doc/tutorials/content/model_outlier_removal.rst b/doc/tutorials/content/model_outlier_removal.rst index b50d2437eae..d45a66882c3 100644 --- a/doc/tutorials/content/model_outlier_removal.rst +++ b/doc/tutorials/content/model_outlier_removal.rst @@ -5,7 +5,7 @@ Filtering a PointCloud using ModelOutlierRemoval This tutorial demonstrates how to extract parametric models for example for planes or spheres out of a PointCloud by using SAC_Models with known coefficients. -If you don't know the models coefficients take a look at the Sample Consensus tutorials. +If you don't know the models coefficients take a look at the :ref:`random_sample_consensus` tutorial. The code -------- diff --git a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp index 59d0e8f0ccb..331c97d5f5c 100644 --- a/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp +++ b/doc/tutorials/content/sources/model_outlier_removal/model_outlier_removal.cpp @@ -2,69 +2,70 @@ #include #include -int main () +int +main () { pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud_sphere_filtered (new pcl::PointCloud); - // 1. generate cloud data + // 1. Generate cloud data int noise_size = 5; int sphere_data_size = 10; - cloud->width = noise_size + sphere_data_size; + cloud->width = noise_size + sphere_data_size; cloud->height = 1; cloud->points.resize (cloud->width * cloud->height); - // 1.1 add noise + // 1.1 Add noise for (size_t i = 0; i < noise_size; ++i) { cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f); cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f); cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f); } - // 1.2 add sphere: - double rand_x1 = 1; double rand_x2 = 1; + // 1.2 Add sphere: + double rand_x1 = 1; + double rand_x2 = 1; for (size_t i = noise_size; i < noise_size + sphere_data_size; ++i) { - // see: http://mathworld.wolfram.com/SpherePointPicking.html - while( pow(rand_x1,2) + pow(rand_x2,2) >= 1) + // See: http://mathworld.wolfram.com/SpherePointPicking.html + while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1) { - rand_x1 = ( rand() % 100 ) / (50.0f) - 1; - rand_x2 = ( rand() % 100 ) / (50.0f) - 1; + rand_x1 = (rand () % 100) / (50.0f) - 1; + rand_x2 = (rand () % 100) / (50.0f) - 1; } - double pre_calc = sqrt( 1 - pow( rand_x1, 2 ) - pow( rand_x2, 2 ) ); - cloud->points[i].x = 2 * rand_x1 * pre_calc; - cloud->points[i].y = 2 * rand_x2 * pre_calc; - cloud->points[i].z = 1 - 2 * ( pow( rand_x1, 2 ) + pow( rand_x2, 2 ) ); - rand_x1 = 1; rand_x2 = 1; + double pre_calc = sqrt (1 - pow (rand_x1, 2) - pow (rand_x2, 2)); + cloud->points[i].x = 2 * rand_x1 * pre_calc; + cloud->points[i].y = 2 * rand_x2 * pre_calc; + cloud->points[i].z = 1 - 2 * (pow (rand_x1, 2) + pow (rand_x2, 2)); + rand_x1 = 1; + rand_x2 = 1; } std::cerr << "Cloud before filtering: " << std::endl; for (size_t i = 0; i < cloud->points.size (); ++i) - std::cout << " " << cloud->points[i].x << " " - << cloud->points[i].y << " " - << cloud->points[i].z << std::endl; + std::cout << " " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl; // 2. filter sphere: // 2.1 generate model: // modelparameter for this sphere: - // position.x: 0, position.y: 0, position.z:0, radius: 1 + // position.x: 0, position.y: 0, position.z:0, radius: 1 pcl::ModelCoefficients sphere_coeff; - sphere_coeff.values.resize(4); - sphere_coeff.values[0] = 0; sphere_coeff.values[1] = 0; sphere_coeff.values[2] = 0; - sphere_coeff.values[3] = 1; + sphere_coeff.values.resize (4); + sphere_coeff.values[0] = 0; + sphere_coeff.values[1] = 0; + sphere_coeff.values[2] = 0; + sphere_coeff.values[3] = 1; pcl::ModelOutlierRemoval sphere_filter; - sphere_filter.setModelCoefficients( sphere_coeff); - sphere_filter.setThreshold( 0.05 ); - sphere_filter.setModelType( pcl::SACMODEL_SPHERE ); - sphere_filter.setInputCloud( cloud ); - sphere_filter.filter( *cloud_sphere_filtered ); + sphere_filter.setModelCoefficients (sphere_coeff); + sphere_filter.setThreshold (0.05); + sphere_filter.setModelType (pcl::SACMODEL_SPHERE); + sphere_filter.setInputCloud (cloud); + sphere_filter.filter (*cloud_sphere_filtered); std::cerr << "Sphere after filtering: " << std::endl; for (size_t i = 0; i < cloud_sphere_filtered->points.size (); ++i) - std::cout << " " << cloud_sphere_filtered->points[i].x << " " - << cloud_sphere_filtered->points[i].y << " " - << cloud_sphere_filtered->points[i].z << std::endl; - + std::cout << " " << cloud_sphere_filtered->points[i].x << " " << cloud_sphere_filtered->points[i].y << " " << cloud_sphere_filtered->points[i].z + << std::endl; return (0); } From e531faf87fbf8f0755b76a66ab366ad24b011bb2 Mon Sep 17 00:00:00 2001 From: Victor Lamoine Date: Thu, 5 Jun 2014 10:29:49 +0200 Subject: [PATCH 4/6] Remove model_from_normals_ class member --- .../filters/impl/model_outlier_removal.hpp | 32 ++++++++----------- .../pcl/filters/model_outlier_removal.h | 2 -- 2 files changed, 13 insertions(+), 21 deletions(-) diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp index 65ce9e2e688..a0b28fe2c8b 100644 --- a/filters/include/pcl/filters/impl/model_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -57,7 +57,6 @@ template bool pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) { - model_from_normals_ = NULL; // Build the model switch (model_type) { @@ -100,41 +99,31 @@ pcl::ModelOutlierRemoval::initSACModel (pcl::SacModel model_type) case SACMODEL_CYLINDER: { PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCYLINDER\n", getClassName ().c_str ()); - SampleConsensusModelCylinder *ptr = new SampleConsensusModelCylinder (input_); - model_from_normals_ = ptr; - model_.reset (ptr); + model_.reset (new SampleConsensusModelCylinder (input_)); break; } case SACMODEL_NORMAL_PLANE: { PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalPlane *ptr = new SampleConsensusModelNormalPlane (input_); - model_from_normals_ = ptr; - model_.reset (ptr); + model_.reset (new SampleConsensusModelNormalPlane (input_)); break; } case SACMODEL_CONE: { PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelCONE\n", getClassName ().c_str ()); - SampleConsensusModelCone *ptr = new SampleConsensusModelCone (input_); - model_from_normals_ = ptr; - model_.reset (ptr); + model_.reset (new SampleConsensusModelCone (input_)); break; } case SACMODEL_NORMAL_SPHERE: { PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_SPHERE\n", getClassName ().c_str ()); - SampleConsensusModelNormalSphere *ptr = new SampleConsensusModelNormalSphere (input_); - model_from_normals_ = ptr; - model_.reset (ptr); + model_.reset (new SampleConsensusModelNormalSphere (input_)); break; } case SACMODEL_NORMAL_PARALLEL_PLANE: { PCL_DEBUG ("[pcl::%s::segment] Using a model of type: modelNORMAL_PARALLEL_PLANE\n", getClassName ().c_str ()); - SampleConsensusModelNormalParallelPlane *ptr = new SampleConsensusModelNormalParallelPlane (input_); - model_from_normals_ = ptr; - model_.reset (ptr); + model_.reset (new SampleConsensusModelNormalParallelPlane (input_)); break; } case SACMODEL_PARALLEL_PLANE: @@ -189,7 +178,12 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) bool valid_setup = true; valid_setup &= initSACModel (model_type_); - if (model_from_normals_) + + typedef SampleConsensusModelFromNormals SACModelFromNormals; + // Returns NULL if cast isn't possible + SACModelFromNormals *model_from_normals = dynamic_cast (& (*model_)); + + if (model_from_normals) { if (!cloud_normals_) { @@ -198,8 +192,8 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) } else { - model_from_normals_->setNormalDistanceWeight (normals_distance_weight_); - model_from_normals_->setInputNormals (cloud_normals_); + model_from_normals->setNormalDistanceWeight (normals_distance_weight_); + model_from_normals->setInputNormals (cloud_normals_); } } diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h index 58577102a7c..136d86fe870 100644 --- a/filters/include/pcl/filters/model_outlier_removal.h +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -82,7 +82,6 @@ namespace pcl ModelOutlierRemoval (bool extract_removed_indices = false) : FilterIndices::FilterIndices (extract_removed_indices) { - model_from_normals_ = NULL; thresh_ = 0; normals_distance_weight_ = 0; filter_name_ = "ModelOutlierRemoval"; @@ -222,7 +221,6 @@ namespace pcl protected: double normals_distance_weight_; pcl::PointCloud::Ptr cloud_normals_; - SampleConsensusModelFromNormals *model_from_normals_; /** \brief The model used to calculate distances */ SampleConsensusModelPtr model_; From c8e6181d92de11ad6f0373bb59c341bb53653586 Mon Sep 17 00:00:00 2001 From: Victor Lamoine Date: Wed, 11 Jun 2014 12:19:09 +0200 Subject: [PATCH 5/6] Use pcl::isFinite instead of pcl_isfinite --- filters/include/pcl/filters/impl/model_outlier_removal.hpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/filters/include/pcl/filters/impl/model_outlier_removal.hpp b/filters/include/pcl/filters/impl/model_outlier_removal.hpp index a0b28fe2c8b..e7e9ff695b7 100644 --- a/filters/include/pcl/filters/impl/model_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/model_outlier_removal.hpp @@ -203,8 +203,7 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator { // Non-finite entries are always passed to removed indices - if (!pcl_isfinite (input_->points[ (*indices_)[iii]].x) || !pcl_isfinite (input_->points[ (*indices_)[iii]].y) - || !pcl_isfinite (input_->points[ (*indices_)[iii]].z)) + if (!isFinite (input_->points[ (*indices_)[iii]])) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; @@ -224,8 +223,7 @@ pcl::ModelOutlierRemoval::applyFilterIndices (std::vector &indices) for (int iii = 0; iii < static_cast (indices_->size ()); ++iii) // iii = input indices iterator { // Non-finite entries are always passed to removed indices - if (!pcl_isfinite (input_->points[ (*indices_)[iii]].x) || !pcl_isfinite (input_->points[ (*indices_)[iii]].y) - || !pcl_isfinite (input_->points[ (*indices_)[iii]].z)) + if (!isFinite (input_->points[ (*indices_)[iii]])) { if (extract_removed_indices_) (*removed_indices_)[rii++] = (*indices_)[iii]; From 5206f4f3f3bd0dd1e226e4e40bc1b993ffe195e9 Mon Sep 17 00:00:00 2001 From: Victor Lamoine Date: Wed, 11 Jun 2014 14:47:02 +0200 Subject: [PATCH 6/6] Change cloud_normals_ into a const ptr --- filters/include/pcl/filters/model_outlier_removal.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/filters/include/pcl/filters/model_outlier_removal.h b/filters/include/pcl/filters/model_outlier_removal.h index 136d86fe870..b513ffc0271 100644 --- a/filters/include/pcl/filters/model_outlier_removal.h +++ b/filters/include/pcl/filters/model_outlier_removal.h @@ -75,6 +75,9 @@ namespace pcl typedef typename SampleConsensusModel::Ptr SampleConsensusModelPtr; public: + typedef typename pcl::PointCloud::Ptr PointCloudNPtr; + typedef typename pcl::PointCloud::ConstPtr PointCloudNConstPtr; + /** \brief Constructor. * \param[in] extract_removed_indices Set to true if you want to be able to extract the indices of points being removed (default = false). */ @@ -141,13 +144,13 @@ namespace pcl /** \brief Set the normals cloud*/ inline void - setInputNormals (pcl::PointCloud::Ptr normals_ptr) + setInputNormals (const PointCloudNConstPtr normals_ptr) { cloud_normals_ = normals_ptr; } /** \brief Get the normals cloud*/ - inline pcl::PointCloud::Ptr + inline PointCloudNConstPtr getInputNormals () const { return (cloud_normals_); @@ -220,7 +223,7 @@ namespace pcl protected: double normals_distance_weight_; - pcl::PointCloud::Ptr cloud_normals_; + PointCloudNConstPtr cloud_normals_; /** \brief The model used to calculate distances */ SampleConsensusModelPtr model_;