From 4adee3b9da79d43dacc34f96ac5898ce5005677b Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 8 Oct 2023 16:54:11 +0200 Subject: [PATCH 1/2] Let setInputCloud of search methods indicate success or failure Instead of returning nothing, return true on success and false on failure. This is especially useful for organized neighbor to indicate whether the input cloud is suitable, but might also come in handy for other methods in the future. In filters, use the returned value to abort in case of failure. Fixes https://github.com/PointCloudLibrary/pcl/issues/5819 --- filters/include/pcl/filters/impl/local_maximum.hpp | 8 +++++++- .../include/pcl/filters/impl/radius_outlier_removal.hpp | 8 +++++++- .../pcl/filters/impl/statistical_outlier_removal.hpp | 8 +++++++- filters/include/pcl/filters/local_maximum.h | 6 ++++++ filters/include/pcl/filters/radius_outlier_removal.h | 6 ++++++ filters/include/pcl/filters/statistical_outlier_removal.h | 6 ++++++ search/include/pcl/search/flann_search.h | 2 +- search/include/pcl/search/impl/flann_search.hpp | 3 ++- search/include/pcl/search/impl/kdtree.hpp | 3 ++- search/include/pcl/search/impl/organized.hpp | 8 +++++--- search/include/pcl/search/kdtree.h | 2 +- search/include/pcl/search/octree.h | 3 ++- search/include/pcl/search/organized.h | 6 +++--- search/include/pcl/search/search.h | 3 ++- 14 files changed, 57 insertions(+), 15 deletions(-) diff --git a/filters/include/pcl/filters/impl/local_maximum.hpp b/filters/include/pcl/filters/impl/local_maximum.hpp index 59c821bb362..e2ab6c0232b 100644 --- a/filters/include/pcl/filters/impl/local_maximum.hpp +++ b/filters/include/pcl/filters/impl/local_maximum.hpp @@ -97,7 +97,13 @@ pcl::LocalMaximum::applyFilterIndices (Indices &indices) else searcher_.reset (new pcl::search::KdTree (false)); } - searcher_->setInputCloud (cloud_projected); + if (!searcher_->setInputCloud (cloud_projected)) + { + PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } // The arrays to be used indices.resize (indices_->size ()); diff --git a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp index 47c7a8e44be..8e13ec06824 100644 --- a/filters/include/pcl/filters/impl/radius_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/radius_outlier_removal.hpp @@ -64,7 +64,13 @@ pcl::RadiusOutlierRemoval::applyFilterIndices (Indices &indices) else searcher_.reset (new pcl::search::KdTree (false)); } - searcher_->setInputCloud (input_); + if (!searcher_->setInputCloud (input_)) + { + PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } // The arrays to be used Indices nn_indices (indices_->size ()); diff --git a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp index e6c3182292f..0e9359f0bad 100644 --- a/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp +++ b/filters/include/pcl/filters/impl/statistical_outlier_removal.hpp @@ -56,7 +56,13 @@ pcl::StatisticalOutlierRemoval::applyFilterIndices (Indices &indices) else searcher_.reset (new pcl::search::KdTree (false)); } - searcher_->setInputCloud (input_); + if (!searcher_->setInputCloud (input_)) + { + PCL_ERROR ("[pcl::%s::applyFilter] Error when initializing search method!\n", getClassName ().c_str ()); + indices.clear (); + removed_indices_->clear (); + return; + } // The arrays to be used const int searcher_k = mean_k_ + 1; // Find one more, since results include the query point. diff --git a/filters/include/pcl/filters/local_maximum.h b/filters/include/pcl/filters/local_maximum.h index 90755be55e7..cd65455f58d 100644 --- a/filters/include/pcl/filters/local_maximum.h +++ b/filters/include/pcl/filters/local_maximum.h @@ -84,6 +84,12 @@ namespace pcl inline float getRadius () const { return (radius_); } + /** \brief Provide a pointer to the search object. + * Calling this is optional. If not called, the search method will be chosen automatically. + * \param[in] searcher a pointer to the spatial search object. + */ + inline void + setSearchMethod (const SearcherPtr &searcher) { searcher_ = searcher; } protected: using PCLBase::input_; using PCLBase::indices_; diff --git a/filters/include/pcl/filters/radius_outlier_removal.h b/filters/include/pcl/filters/radius_outlier_removal.h index 677284df29a..23f1d48317d 100644 --- a/filters/include/pcl/filters/radius_outlier_removal.h +++ b/filters/include/pcl/filters/radius_outlier_removal.h @@ -136,6 +136,12 @@ namespace pcl return (min_pts_radius_); } + /** \brief Provide a pointer to the search object. + * Calling this is optional. If not called, the search method will be chosen automatically. + * \param[in] searcher a pointer to the spatial search object. + */ + inline void + setSearchMethod (const SearcherPtr &searcher) { searcher_ = searcher; } protected: using PCLBase::input_; using PCLBase::indices_; diff --git a/filters/include/pcl/filters/statistical_outlier_removal.h b/filters/include/pcl/filters/statistical_outlier_removal.h index 3c55dc0a25f..4abfd12316e 100644 --- a/filters/include/pcl/filters/statistical_outlier_removal.h +++ b/filters/include/pcl/filters/statistical_outlier_removal.h @@ -140,6 +140,12 @@ namespace pcl return (std_mul_); } + /** \brief Provide a pointer to the search object. + * Calling this is optional. If not called, the search method will be chosen automatically. + * \param[in] searcher a pointer to the spatial search object. + */ + inline void + setSearchMethod (const SearcherPtr &searcher) { searcher_ = searcher; } protected: using PCLBase::input_; using PCLBase::indices_; diff --git a/search/include/pcl/search/flann_search.h b/search/include/pcl/search/flann_search.h index f3de1362268..8d994895c59 100644 --- a/search/include/pcl/search/flann_search.h +++ b/search/include/pcl/search/flann_search.h @@ -254,7 +254,7 @@ namespace pcl * \param[in] cloud the const boost shared pointer to a PointCloud message * \param[in] indices the point indices subset that is to be used from \a cloud */ - void + bool setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override; using Search::nearestKSearch; diff --git a/search/include/pcl/search/impl/flann_search.hpp b/search/include/pcl/search/impl/flann_search.hpp index 94270ba1bcf..218fd690998 100644 --- a/search/include/pcl/search/impl/flann_search.hpp +++ b/search/include/pcl/search/impl/flann_search.hpp @@ -91,7 +91,7 @@ pcl::search::FlannSearch::~FlannSearch() } ////////////////////////////////////////////////////////////////////////////////////////////// -template void +template bool pcl::search::FlannSearch::setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices) { input_ = cloud; @@ -99,6 +99,7 @@ pcl::search::FlannSearch::setInputCloud (const PointCloud convertInputToFlannMatrix (); index_ = creator_->createIndex (input_flann_); index_->buildIndex (); + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/search/include/pcl/search/impl/kdtree.hpp b/search/include/pcl/search/impl/kdtree.hpp index b3eff66740f..281b1cda360 100644 --- a/search/include/pcl/search/impl/kdtree.hpp +++ b/search/include/pcl/search/impl/kdtree.hpp @@ -72,7 +72,7 @@ pcl::search::KdTree::setEpsilon (float eps) } /////////////////////////////////////////////////////////////////////////////////////////// -template void +template bool pcl::search::KdTree::setInputCloud ( const PointCloudConstPtr& cloud, const IndicesConstPtr& indices) @@ -80,6 +80,7 @@ pcl::search::KdTree::setInputCloud ( tree_->setInputCloud (cloud, indices); input_ = cloud; indices_ = indices; + return true; } /////////////////////////////////////////////////////////////////////////////////////////// diff --git a/search/include/pcl/search/impl/organized.hpp b/search/include/pcl/search/impl/organized.hpp index fc5ce6cfda7..12e6da32962 100644 --- a/search/include/pcl/search/impl/organized.hpp +++ b/search/include/pcl/search/impl/organized.hpp @@ -329,7 +329,7 @@ pcl::search::OrganizedNeighbor::computeCameraMatrix (Eigen::Matrix3f& ca } ////////////////////////////////////////////////////////////////////////////////////////////// -template void +template bool pcl::search::OrganizedNeighbor::estimateProjectionMatrix () { // internally we calculate with double but store the result into float matrices. @@ -337,7 +337,7 @@ pcl::search::OrganizedNeighbor::estimateProjectionMatrix () if (input_->height == 1 || input_->width == 1) { PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", this->getName ().c_str ()); - return; + return false; } const unsigned ySkip = (std::max) (input_->height >> pyramid_level_, static_cast(1)); @@ -363,7 +363,7 @@ pcl::search::OrganizedNeighbor::estimateProjectionMatrix () if (std::abs (residual_sqr) > eps_ * static_cast(indices.size ())) { PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not from a projective device!\nResidual (MSE) %g, using %d valid points\n", this->getName ().c_str (), residual_sqr / double (indices.size()), indices.size ()); - return; + return false; } // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]] @@ -383,8 +383,10 @@ pcl::search::OrganizedNeighbor::estimateProjectionMatrix () if (!projectPoint(test_point, q) || std::abs(q.x-test_index%input_->width)>1 || std::abs(q.y-test_index/input_->width)>1) { PCL_WARN ("[pcl::%s::estimateProjectionMatrix] Input dataset does not seem to be from a projective device! (point %zu (%g,%g,%g) projected to pixel coordinates (%g,%g), but actual pixel coordinates are (%zu,%zu))\n", this->getName ().c_str (), test_index, test_point.x, test_point.y, test_point.z, q.x, q.y, static_cast(test_index%input_->width), static_cast(test_index/input_->width)); + return false; } } + return true; } ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/search/include/pcl/search/kdtree.h b/search/include/pcl/search/kdtree.h index c0503a12588..59018eac220 100644 --- a/search/include/pcl/search/kdtree.h +++ b/search/include/pcl/search/kdtree.h @@ -128,7 +128,7 @@ namespace pcl * \param[in] cloud the const boost shared pointer to a PointCloud message * \param[in] indices the point indices subset that is to be used from \a cloud */ - void + bool setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr& indices = IndicesConstPtr ()) override; diff --git a/search/include/pcl/search/octree.h b/search/include/pcl/search/octree.h index 283a02600e1..827667cb723 100644 --- a/search/include/pcl/search/octree.h +++ b/search/include/pcl/search/octree.h @@ -114,7 +114,7 @@ namespace pcl * \param[in] cloud the const boost shared pointer to a PointCloud message * \param[in] indices the point indices subset that is to be used from \a cloud */ - inline void + inline bool setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices) override { tree_->deleteTree (); @@ -122,6 +122,7 @@ namespace pcl tree_->addPointsFromInputCloud (); input_ = cloud; indices_ = indices; + return true; } /** \brief Search for the k-nearest neighbors for the given query point. diff --git a/search/include/pcl/search/organized.h b/search/include/pcl/search/organized.h index 6d5e6461d82..fe07756ef44 100644 --- a/search/include/pcl/search/organized.h +++ b/search/include/pcl/search/organized.h @@ -125,7 +125,7 @@ namespace pcl * \param[in] cloud the const boost shared pointer to a PointCloud message * \param[in] indices the const boost shared pointer to PointIndices */ - void + bool setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()) override { input_ = cloud; @@ -143,7 +143,7 @@ namespace pcl else mask_.assign (input_->size (), 1); - estimateProjectionMatrix (); + return estimateProjectionMatrix () && isValid (); } /** \brief Search for all neighbors of query point that are within a given radius. @@ -164,7 +164,7 @@ namespace pcl unsigned int max_nn = 0) const override; /** \brief estimated the projection matrix from the input cloud. */ - void + bool estimateProjectionMatrix (); /** \brief Search for the k-nearest neighbors for a given query point. diff --git a/search/include/pcl/search/search.h b/search/include/pcl/search/search.h index 396e1829d10..0991c98a967 100644 --- a/search/include/pcl/search/search.h +++ b/search/include/pcl/search/search.h @@ -113,8 +113,9 @@ namespace pcl /** \brief Pass the input dataset that the search will be performed on. * \param[in] cloud a const pointer to the PointCloud data * \param[in] indices the point indices subset that is to be used from the cloud + * \return True if successful, false if an error occurred, for example because the point cloud is unsuited for the search method. */ - virtual void + virtual bool setInputCloud (const PointCloudConstPtr& cloud, const IndicesConstPtr &indices = IndicesConstPtr ()); From 633cbe0841b6bc314b4699cb035b7107c461fbd2 Mon Sep 17 00:00:00 2001 From: Markus Vieth Date: Sun, 8 Oct 2023 17:35:30 +0200 Subject: [PATCH 2/2] fixup --- search/include/pcl/search/impl/search.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/search/include/pcl/search/impl/search.hpp b/search/include/pcl/search/impl/search.hpp index aa8d229fd89..9f24d244acd 100644 --- a/search/include/pcl/search/impl/search.hpp +++ b/search/include/pcl/search/impl/search.hpp @@ -71,12 +71,13 @@ pcl::search::Search::getSortedResults () } /////////////////////////////////////////////////////////////////////////////////////////// -template void +template bool pcl::search::Search::setInputCloud ( const PointCloudConstPtr& cloud, const IndicesConstPtr &indices) { input_ = cloud; indices_ = indices; + return true; }