diff --git a/CMakeLists.txt b/CMakeLists.txt index 80a0bc71..457837e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -341,6 +341,7 @@ set(POINTMATCHER_SRC pointmatcher/DataPointsFilters/CovarianceSampling.cpp pointmatcher/DataPointsFilters/DistanceLimit.cpp pointmatcher/DataPointsFilters/RemoveSensorBias.cpp + pointmatcher/DataPointsFilters/Sphericality.cpp ) include_directories(${CMAKE_SOURCE_DIR}) diff --git a/pointmatcher/DataPointsFilters/Sphericality.cpp b/pointmatcher/DataPointsFilters/Sphericality.cpp new file mode 100644 index 00000000..cf7de432 --- /dev/null +++ b/pointmatcher/DataPointsFilters/Sphericality.cpp @@ -0,0 +1,180 @@ +// kate: replace-tabs off; indent-width 4; indent-mode normal +// vim: ts=4:sw=4:noexpandtab +/* + +Copyright (c) 2010--2018, +François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland +You can contact the authors at and + + +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 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 ETH-ASL 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 "Sphericality.h" + + +/* + The SphericalityDataPointFilter estimates a heuristic value which indicates how much local geometry around + a given point resemble a plane or a sphere (uniform distribution). The value can lie in <-1,1> interval, -1 for + perfect plane, 1 for perfectly uniform distribution. The estimation is based on three eigenvalues coming from another + data points filter (this filter can operate only on 3D data). In the case the largest eigenvalue is zero, the filter + outputs NaNs. If the middle eigenvalue is zero and the largest one is non-zero, the filter outputs zeros. + + Implemented by Vladimir Kubelka (kubelvla@gmail.com), NORLAB, Universite Laval, 2020 +*/ + +// Constructor +template +SphericalityDataPointsFilter::SphericalityDataPointsFilter(const Parameters& params): + PointMatcher::DataPointsFilter("SphericalityDataPointsFilter", + SphericalityDataPointsFilter::availableParameters(), params), + keepUnstructureness(Parametrizable::get("keepUnstructureness")), + keepStructureness(Parametrizable::get("keepStructureness")) +{ +} + +// Compute +template +typename PointMatcher::DataPoints +SphericalityDataPointsFilter::filter( + const DataPoints& input) +{ + DataPoints output(input); + inPlaceFilter(output); + return output; +} + +// In-place filter +template +void SphericalityDataPointsFilter::inPlaceFilter( + DataPoints& cloud) +{ + + typedef typename DataPoints::View View; + typedef typename DataPoints::Label Label; + typedef typename DataPoints::Labels Labels; + + const size_t pointsCount(cloud.features.cols()); + const size_t descDim(cloud.descriptors.rows()); + const size_t labelDim(cloud.descriptorLabels.size()); + + // Check that the required eigenValue descriptor exists in the pointcloud + if (!cloud.descriptorExists("eigValues")) + { + throw InvalidField("SphericalityDataPointsFilter: Error, no eigValues found in descriptors."); + } + + // Validate descriptors and labels + size_t insertDim(0); + for(unsigned int i = 0; i < labelDim ; ++i) + insertDim += cloud.descriptorLabels[i].span; + if (insertDim != descDim) + throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data"); + + // Reserve memory for new descriptors + const size_t unidimensionalDescriptorDimension(1); + + + boost::optional sphericality; // these optionals may cause a compiler warning, but it is ok, + boost::optional unstructureness; // it is intended to be uninitialized + boost::optional structureness; + + Labels cloudLabels; + cloudLabels.push_back(Label("sphericality", unidimensionalDescriptorDimension)); + if (keepUnstructureness) + cloudLabels.push_back(Label("unstructureness", unidimensionalDescriptorDimension)); + if (keepStructureness) + cloudLabels.push_back(Label("structureness", unidimensionalDescriptorDimension)); + + // Reserve memory + cloud.allocateDescriptors(cloudLabels); + + // Get the views + const View eigValues = cloud.getDescriptorViewByName("eigValues"); + if (eigValues.rows() != 3) // And check the dimensions + { + throw InvalidField("SphericalityDataPointsFilter: Error, the number of eigValues is not 3."); + } + + sphericality = cloud.getDescriptorViewByName("sphericality"); + if (keepUnstructureness) + unstructureness = cloud.getDescriptorViewByName("unstructureness"); + if (keepStructureness) + structureness = cloud.getDescriptorViewByName("structureness"); + + // Iterate through the point cloud and evaluate the Sphericality + for (size_t i = 0; i < pointsCount; ++i) + { + // extract the three eigenvalues relevant to the current point + Vector eig_vals_col = eigValues.col(i); + // might be already sorted but sort anyway + std::sort(eig_vals_col.data(),eig_vals_col.data()+eig_vals_col.size()); + + // Finally, evaluate the Sphericality + T sphericalityVal; + T unstructurenessVal; + T structurenessVal; + + // First, avoid division by zero + //TODO: Is there a more suitable limit for considering the values almost-zero? (VK) + if (eig_vals_col(2) < std::numeric_limits::min() or + eig_vals_col(1) < 0.0 or + eig_vals_col(0) < 0.0) + { + // If the largest eigenvalue is zero or even worse -- any of them is negative, + // these descriptors are not well defined (0/0 or nonsense input) and assigned NaN + sphericalityVal = std::numeric_limits::quiet_NaN(); + unstructurenessVal = std::numeric_limits::quiet_NaN(); + structurenessVal = std::numeric_limits::quiet_NaN(); + + } else if (eig_vals_col(1) < std::numeric_limits::min()) { + // If there are two zero eigenvalues and one non-zero, we assign zeros to the heuristic + sphericalityVal = 0.0; + unstructurenessVal = 0.0; + structurenessVal = 0.0; + + } else { + // Otherwise, follow eq.(1) from Kubelka V. et al., "Radio propagation models for differential GNSS based + // on dense point clouds", JFR, hopefully published in 2020 + unstructurenessVal = eig_vals_col(0) / eig_vals_col(2); + structurenessVal = (eig_vals_col(1) / eig_vals_col(2)) * + ((eig_vals_col(1) - eig_vals_col(0)) / sqrt(eig_vals_col(0)*eig_vals_col(0) + eig_vals_col(1)*eig_vals_col(1))); + sphericalityVal = unstructurenessVal - structurenessVal; + } + + // store it in the pointcloud + // sphericality always + (sphericality.get())(0,i) = sphericalityVal; + // these two only when requested by the user + if (unstructureness) + (unstructureness.get())(0,i) = unstructurenessVal; + if (structureness) + (structureness.get())(0,i) = structurenessVal; + + } +} + +template struct SphericalityDataPointsFilter; +template struct SphericalityDataPointsFilter; \ No newline at end of file diff --git a/pointmatcher/DataPointsFilters/Sphericality.h b/pointmatcher/DataPointsFilters/Sphericality.h new file mode 100644 index 00000000..457fa931 --- /dev/null +++ b/pointmatcher/DataPointsFilters/Sphericality.h @@ -0,0 +1,87 @@ +// kate: replace-tabs off; indent-width 4; indent-mode normal +// vim: ts=4:sw=4:noexpandtab +/* + +Copyright (c) 2010--2018, +François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland +You can contact the authors at and + + +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 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 ETH-ASL 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. + +*/ +#pragma once + +#include "PointMatcher.h" + +/* + The SphericalityDataPointFilter estimates a heuristic value which indicates how much local geometry around + a given point resemble a plane or a sphere (uniform distribution). The value can lie in <-1,1> interval, -1 for + perfect plane, 1 for perfectly uniform distribution. The estimation is based on three eigenvalues coming from another + data points filter (this filter can operate only on 3D data). In the case the largest eigenvalue is zero, the filter + outputs NaNs. If the middle eigenvalue is zero and the largest one is non-zero, the filter outputs zeros. + + Implemented by Vladimir Kubelka (kubelvla@gmail.com), NORLAB, Universite Laval, 2020 +*/ + +template +struct SphericalityDataPointsFilter: public PointMatcher::DataPointsFilter +{ + typedef PointMatcherSupport::Parametrizable Parametrizable; + typedef PointMatcherSupport::Parametrizable P; + typedef Parametrizable::Parameters Parameters; + typedef Parametrizable::ParameterDoc ParameterDoc; + typedef Parametrizable::ParametersDoc ParametersDoc; + typedef Parametrizable::InvalidParameter InvalidParameter; + + typedef typename PointMatcher::Vector Vector; + typedef typename PointMatcher::Matrix Matrix; + typedef typename PointMatcher::DataPoints DataPoints; + typedef typename PointMatcher::DataPoints::InvalidField InvalidField; + + inline static const std::string description() + { + return "This filter computes the level of ’sphericality’ for each point. It describes the shape of local geometry, whether the surrounding points resemble a plane (the sphericality value goes towards -1) or a uniform distribution (the value towards +1). It is intended for 3D point clouds only. Sphericality is computed by subtracting the intermediate values of ‘unstructureness’ and ‘structureness’.\n\n" + "Required descriptors: eigValues (must be three, otherwise exception).\n" + "Produced descritors: sphericality, unstructureness(optional), structureness(optional).\n" + "Altered descriptors: none.\n" + "Altered features: none."; + } + inline static const ParametersDoc availableParameters() + { + return { + {"keepUnstructureness", "whether the value of the unstructureness should be added to the pointcloud", "0"}, + {"keepStructureness", "whether the value of the structureness should be added to the pointcloud", "0"}, + }; + } + + const bool keepUnstructureness; + const bool keepStructureness; + + SphericalityDataPointsFilter(const Parameters& params = Parameters()); + virtual ~SphericalityDataPointsFilter() {}; + virtual DataPoints filter(const DataPoints& input); + virtual void inPlaceFilter(DataPoints& cloud); +}; diff --git a/pointmatcher/DataPointsFiltersImpl.h b/pointmatcher/DataPointsFiltersImpl.h index 022452ba..ca610d57 100644 --- a/pointmatcher/DataPointsFiltersImpl.h +++ b/pointmatcher/DataPointsFiltersImpl.h @@ -62,6 +62,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "DataPointsFilters/CovarianceSampling.h" #include "DataPointsFilters/DistanceLimit.h" #include "DataPointsFilters/RemoveSensorBias.h" +#include "DataPointsFilters/Sphericality.h" template struct DataPointsFiltersImpl @@ -92,6 +93,11 @@ struct DataPointsFiltersImpl typedef ::CovarianceSamplingDataPointsFilter CovarianceSamplingDataPointsFilter; typedef ::DistanceLimitDataPointsFilter DistanceLimitDataPointsFilter; typedef ::RemoveSensorBiasDataPointsFilter RemoveSensorBiasDataPointsFilter; + typedef ::SphericalityDataPointsFilter SphericalityDataPointsFilter; + + + + }; // DataPointsFiltersImpl diff --git a/pointmatcher/Registry.cpp b/pointmatcher/Registry.cpp index bdd44e0b..b3e0d9fc 100644 --- a/pointmatcher/Registry.cpp +++ b/pointmatcher/Registry.cpp @@ -89,7 +89,9 @@ PointMatcher::PointMatcher() ADD_TO_REGISTRAR(DataPointsFilter, CovarianceSamplingDataPointsFilter, typename DataPointsFiltersImpl::CovarianceSamplingDataPointsFilter) ADD_TO_REGISTRAR(DataPointsFilter, DistanceLimitDataPointsFilter, typename DataPointsFiltersImpl::DistanceLimitDataPointsFilter) ADD_TO_REGISTRAR(DataPointsFilter, RemoveSensorBiasDataPointsFilter, typename DataPointsFiltersImpl::RemoveSensorBiasDataPointsFilter) - + ADD_TO_REGISTRAR(DataPointsFilter, SphericalityDataPointsFilter, typename DataPointsFiltersImpl::SphericalityDataPointsFilter) + + ADD_TO_REGISTRAR_NO_PARAM(Matcher, NullMatcher, typename MatchersImpl::NullMatcher) ADD_TO_REGISTRAR(Matcher, KDTreeMatcher, typename MatchersImpl::KDTreeMatcher) ADD_TO_REGISTRAR(Matcher, KDTreeVarDistMatcher, typename MatchersImpl::KDTreeVarDistMatcher)