diff --git a/CMakeLists.txt b/CMakeLists.txt index 36b69685..95d4af29 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -314,6 +314,7 @@ set(POINTMATCHER_SRC pointmatcher/ErrorMinimizers/PointToPointSimilarity.cpp pointmatcher/ErrorMinimizers/Identity.cpp #DataPointsFilters + pointmatcher/DataPointsFilters/AddDescriptor.cpp pointmatcher/DataPointsFilters/Identity.cpp pointmatcher/DataPointsFilters/RemoveNaN.cpp pointmatcher/DataPointsFilters/MaxDist.cpp diff --git a/doc/DataFilters.md b/doc/DataFilters.md index b2dfed34..da24f595 100644 --- a/doc/DataFilters.md +++ b/doc/DataFilters.md @@ -51,19 +51,21 @@ Note that *datapoint filters* differ from *outlier filters* which appear further ### Descriptor Augmenting -1. [Observation Direction Filter](#obsdirectionhead) +1. [Add Descriptor Filter](#adddescriptorhead) -2. [Surface Normal Filter](#surfacenormalhead) +2. [Observation Direction Filter](#obsdirectionhead) -3. [Orient Normals Filter](#orientnormalshead) +3. [Surface Normal Filter](#surfacenormalhead) -4. [Sampling Surface Normal Filter](#samplingnormhead) +4. [Orient Normals Filter](#orientnormalshead) -5. [Simple Sensor Noise Filter](#sensornoisehead) +5. [Sampling Surface Normal Filter](#samplingnormhead) -6. [Saliency Filter](#saliencyhead) +6. [Simple Sensor Noise Filter](#sensornoisehead) -7. [Fixed Step Sampling Filter](#fixedstepsamplinghead) +7. [Saliency Filter](#saliencyhead) + +8. [Fixed Step Sampling Filter](#fixedstepsamplinghead) ## An Example Point Cloud View of an Appartment @@ -74,6 +76,25 @@ The following examples are drawn from the apartment dataset available for [downl ![alt text](images/appt_top.png "Top down view of point cloud from appartment dataset") +## Add Descriptor Filter + +### Description + +Adds a new descriptor to an existing point cloud or overwrites existing descriptor with the same name. + +__Required descriptors:__ none +__Output descriptor:__ User defined. +__Sensor assumed to be at the origin:__ no +__Impact on the number of points:__ none +__Altered descriptors:__ Existing descriptor with name equal to `descriptorName`. +__Altered features:__ none.; + +| Parameter | Description | Default value | Allowable range | +|---------------------|:-----------------------------------------------------------------------------------------------------------------------------------------------------|:--------------|:----------------| +| descriptorName | Name of the descriptor to be added. | | | +| descriptorDimension | Length of the descriptor to be added. | 1 | 1 to 4294967295 | +| descriptorValues | Values of the descriptor to be added. List of 'descriptorDimension' numbers of type T, separated by commas, closed in brackets, e.g. [2.2, 3.0, 6.1] | | | + ## Bounding Box Filter ### Description diff --git a/examples/data/add_descriptor_config.yaml b/examples/data/add_descriptor_config.yaml new file mode 100644 index 00000000..b873a62c --- /dev/null +++ b/examples/data/add_descriptor_config.yaml @@ -0,0 +1,4 @@ +- AddDescriptorDataPointsFilter: + descriptorName: deviation + descriptorDimension: 9 + descriptorValues: [0.0009, 0, 0, 0, 0.0009, 0, 0, 0, 0.0009] \ No newline at end of file diff --git a/pointmatcher/DataPointsFilters/AddDescriptor.cpp b/pointmatcher/DataPointsFilters/AddDescriptor.cpp new file mode 100644 index 00000000..dc629e5b --- /dev/null +++ b/pointmatcher/DataPointsFilters/AddDescriptor.cpp @@ -0,0 +1,60 @@ +// kate: replace-tabs off; indent-width 4; indent-mode normal +// vim: ts=4:sw=4:noexpandtab + +#include "AddDescriptor.h" + +template +AddDescriptorDataPointsFilter::AddDescriptorDataPointsFilter(const Parameters& params) : + PointMatcher::DataPointsFilter("AddDescriptorDataPointsFilter", + AddDescriptorDataPointsFilter::availableParameters(), params), + descriptorName(Parametrizable::get("descriptorName")), + descriptorDimension(Parametrizable::get("descriptorDimension")), + descriptorValues(Parametrizable::getVector("descriptorValues")) +{ + assert(descriptorDimension == descriptorValues.size()); +} + +// AddDescriptorDataPointsFilter +template +typename PointMatcher::DataPoints AddDescriptorDataPointsFilter::filter( + const DataPoints& input) +{ + DataPoints output(input); + inPlaceFilter(output); + return output; +} + +// In-place filter +template +void AddDescriptorDataPointsFilter::inPlaceFilter( + DataPoints& cloud) +{ + Matrix matrix = PM::Matrix::Ones(descriptorDimension, cloud.getNbPoints()); + for(std::size_t i = 0; i < descriptorDimension; ++i) + { + matrix.row(i) *= descriptorValues[i]; + } + if(!cloud.descriptorExists(descriptorName)) + { + cloud.addDescriptor(descriptorName, matrix); + } + else + { + if(descriptorDimension == cloud.getDescriptorDimension(descriptorName)) + { + cloud.getDescriptorViewByName(descriptorName) = matrix; + } + else + { + // FIXME deal with overwriting descriptors that have different dimensions + throw std::runtime_error("Can't overwrite existing descriptor " + descriptorName + " with dimension " + + std::to_string(cloud.getDescriptorDimension(descriptorName)) + + " by a new descriptor with dimension " + + std::to_string(descriptorDimension) + "."); + } + } + +} + +template struct AddDescriptorDataPointsFilter; +template struct AddDescriptorDataPointsFilter; diff --git a/pointmatcher/DataPointsFilters/AddDescriptor.h b/pointmatcher/DataPointsFilters/AddDescriptor.h new file mode 100644 index 00000000..855390a3 --- /dev/null +++ b/pointmatcher/DataPointsFilters/AddDescriptor.h @@ -0,0 +1,57 @@ +// kate: replace-tabs off; indent-width 4; indent-mode normal +// vim: ts=4:sw=4:noexpandtab + +#pragma once + +#include "PointMatcher.h" + +//! Add new descriptor to an existing point cloud +template +struct AddDescriptorDataPointsFilter : public PointMatcher::DataPointsFilter +{ + // Type definitions + typedef PointMatcher PM; + typedef typename PM::DataPoints DataPoints; + typedef typename PM::DataPointsFilter 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::Matrix Matrix; + typedef typename PointMatcher::Vector Vector; + typedef typename PointMatcher::DataPoints::InvalidField InvalidField; + + const std::string descriptorName; + const std::size_t descriptorDimension; + const std::vector descriptorValues; + + inline static const std::string description() + { + return "Adds a new descriptor to an existing point cloud or overwrites existing descriptor with the same name.\n\n" + "Required descriptors: none.\n" + "Produced descriptors: User defined.\n" + "Altered descriptors: none.\n" + "Altered features: none."; + } + + inline static const ParametersDoc availableParameters() + { + return { + {"descriptorName", "Name of the descriptor to be added.", "testDescriptor" }, + {"descriptorDimension", "Length of the descriptor to be added.", "1", "1", "4294967295", &P::Comp < std::size_t > }, + {"descriptorValues", "Values of the descriptor to be added.\n" + "List of 'descriptorDimension' numbers of type T, separated by commas, closed in brackets,\n" + "e.g. [2.2, 3.0, 6.1]", ""} + }; + } + + //Constructor, uses parameter interface + explicit AddDescriptorDataPointsFilter(const Parameters& params = Parameters()); + + virtual DataPoints filter(const DataPoints& input); + virtual void inPlaceFilter(DataPoints& cloud); +}; diff --git a/pointmatcher/DataPointsFiltersImpl.h b/pointmatcher/DataPointsFiltersImpl.h index 8f661293..a24d71d0 100644 --- a/pointmatcher/DataPointsFiltersImpl.h +++ b/pointmatcher/DataPointsFiltersImpl.h @@ -36,6 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef __POINTMATCHER_DATAPOINTSFILTERS_H #define __POINTMATCHER_DATAPOINTSFILTERS_H +#include "DataPointsFilters/AddDescriptor.h" #include "DataPointsFilters/Identity.h" #include "DataPointsFilters/RemoveNaN.h" #include "DataPointsFilters/MaxDist.h" @@ -70,6 +71,7 @@ template struct DataPointsFiltersImpl { typedef ::IdentityDataPointsFilter IdentityDataPointsFilter; + typedef ::AddDescriptorDataPointsFilter AddDescriptorDataPointsFilter; typedef ::RemoveNaNDataPointsFilter RemoveNaNDataPointsFilter; typedef ::MaxDistDataPointsFilter MaxDistDataPointsFilter; typedef ::MinDistDataPointsFilter MinDistDataPointsFilter; diff --git a/pointmatcher/Parametrizable.h b/pointmatcher/Parametrizable.h index 318e7f12..d2c40763 100644 --- a/pointmatcher/Parametrizable.h +++ b/pointmatcher/Parametrizable.h @@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #define BOOST_ASSIGN_MAX_PARAMS 6 #include +#include namespace PointMatcherSupport @@ -77,7 +78,37 @@ namespace PointMatcherSupport { return boost::lexical_cast(arg); } - + + //! Special case of lexical cast to std::vector + template + inline std::vector lexical_cast_vector(const std::string& arg) + { + std::string cleanedInput = arg; + if (cleanedInput.find('[') != 0) + { + throw std::runtime_error("Vector parameter '" + arg + "' must start with '['"); + } + if (cleanedInput.find(']') != cleanedInput.size()-1) + { + throw std::runtime_error("Vector parameter '" + arg + "' must end with ']'"); + } + cleanedInput.erase(std::remove(cleanedInput.begin(), cleanedInput.end(), '['), cleanedInput.end()); + cleanedInput.erase(std::remove(cleanedInput.begin(), cleanedInput.end(), ' '), cleanedInput.end()); + cleanedInput.erase(std::remove(cleanedInput.begin(), cleanedInput.end(), ']'), cleanedInput.end()); + + // Split the string into a vector of strings using commas as the delimiter + std::vector tokens; + boost::algorithm::split(tokens, cleanedInput, boost::algorithm::is_any_of(",")); + + std::vector result; + result.reserve(tokens.size()); + for(const auto& token: tokens) + { + auto value = lexical_cast_scalar_to_string(token); + result.push_back(value); + } + return result; + } //! Special case of lexical cast to float, use lexical_cast_scalar_to_string template<> inline float lexical_cast(const std::string& arg) { return lexical_cast_scalar_to_string(arg); } @@ -87,12 +118,24 @@ namespace PointMatcherSupport // - //! Return the a string value using lexical_cast + //! Return the string value using lexical_cast template - std::string toParam(const S& value) + inline std::string toParam(const S& value) { return lexical_cast(value); } + + //! Return the string value of a std::vector + template + inline std::string toParam(const std::vector& input) + { + std::ostringstream oss; + oss << "["; + std::copy(input.begin(), input.end() - 1, + std::ostream_iterator(oss, ", ")); + oss << input.back() << "]"; + return oss.str(); + } //! The superclass of classes that are constructed using generic parameters. This class provides the parameter storage and fetching mechanism struct Parametrizable @@ -170,7 +213,11 @@ namespace PointMatcherSupport //! Return the value of paramName, lexically-casted to S template S get(const std::string& paramName) { return lexical_cast(getParamValueString(paramName)); } - + + //! Return the value of paramName, lexically-casted std::vector + template + inline std::vector getVector(const std::string& paramName) {return lexical_cast_vector(getParamValueString(paramName)); } + friend std::ostream& operator<< (std::ostream& o, const Parametrizable& p); }; std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p); diff --git a/pointmatcher/Registrar.cpp b/pointmatcher/Registrar.cpp index f8cb080d..de9ffd79 100644 --- a/pointmatcher/Registrar.cpp +++ b/pointmatcher/Registrar.cpp @@ -19,8 +19,21 @@ namespace PointMatcherSupport for(YAML::const_iterator paramIt = mapIt->second.begin(); paramIt != mapIt->second.end(); ++paramIt) { std::string key = paramIt->first.as(); - std::string value = paramIt->second.as(); - params[key] = value; + if (paramIt->second.IsSequence()) + { + std::ostringstream oss; + oss << "["; + for(int i = 0; i < paramIt->second.size()-1; ++i) + { + oss << paramIt->second[i] << ", "; + } + oss << paramIt->second[paramIt->second.size()-1] << "]"; + params[key] = oss.str(); + } + else + { + params[key] = paramIt->second.as(); + } } } } diff --git a/pointmatcher/Registrar.h b/pointmatcher/Registrar.h index 686a4cf1..5bfa8654 100644 --- a/pointmatcher/Registrar.h +++ b/pointmatcher/Registrar.h @@ -113,7 +113,7 @@ namespace PointMatcherSupport { for (const auto& param : params) throw Parametrizable::InvalidParameter( - (boost::format("Parameter %1% was set but module %2% dos not use any parameter") % param.first % className).str() + (boost::format("Parameter %1% was set but module %2% does not use any parameter") % param.first % className).str() ); return std::make_shared(); diff --git a/pointmatcher/Registry.cpp b/pointmatcher/Registry.cpp index 512b2aef..584e56c4 100644 --- a/pointmatcher/Registry.cpp +++ b/pointmatcher/Registry.cpp @@ -64,6 +64,7 @@ PointMatcher::PointMatcher() ADD_TO_REGISTRAR_NO_PARAM(Transformation, PureTranslation, typename TransformationsImpl::PureTranslation) ADD_TO_REGISTRAR_NO_PARAM(Transformation, SimilarityTransformation, typename TransformationsImpl::SimilarityTransformation) + ADD_TO_REGISTRAR(DataPointsFilter, AddDescriptorDataPointsFilter, typename DataPointsFiltersImpl::AddDescriptorDataPointsFilter) ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, IdentityDataPointsFilter, typename DataPointsFiltersImpl::IdentityDataPointsFilter) ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, RemoveNaNDataPointsFilter, typename DataPointsFiltersImpl::RemoveNaNDataPointsFilter) ADD_TO_REGISTRAR(DataPointsFilter, MaxDistDataPointsFilter, typename DataPointsFiltersImpl::MaxDistDataPointsFilter) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 636e0e87..d4ab984b 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -49,6 +49,7 @@ set(PYBIND11_SOURCES errorminimizers/point_to_point_with_cov.cpp #datapointfilters module + datapointsfilters/add_descriptor.cpp datapointsfilters/bounding_box.cpp datapointsfilters/covariance_sampling.cpp datapointsfilters/cut_at_descriptor_threshold.cpp diff --git a/python/datapointsfilters/add_descriptor.cpp b/python/datapointsfilters/add_descriptor.cpp new file mode 100644 index 00000000..639a9749 --- /dev/null +++ b/python/datapointsfilters/add_descriptor.cpp @@ -0,0 +1,26 @@ +#include "bounding_box.h" + +#include "DataPointsFilters/AddDescriptor.h" + +namespace python +{ + namespace datapointsfilters + { + void pybindAddDescriptor(py::module& p_module) + { + using AddDescriptorDataPointsFilter = AddDescriptorDataPointsFilter; + py::class_, DataPointsFilter>(p_module, "AddDescriptorDataPointsFilter") + .def_static("description", &AddDescriptorDataPointsFilter::description) + .def_static("availableParameters", &AddDescriptorDataPointsFilter::availableParameters) + + .def_readonly("descriptorName", &AddDescriptorDataPointsFilter::descriptorName) + .def_readonly("descriptorDimension", &AddDescriptorDataPointsFilter::descriptorDimension) + .def_readonly("descriptorValues", &AddDescriptorDataPointsFilter::descriptorValues) + + .def(py::init(), py::arg("params") = Parameters(), "Constructor, uses parameter interface") + + .def("filter", &AddDescriptorDataPointsFilter::filter, py::arg("input")) + .def("inPlaceFilter", &AddDescriptorDataPointsFilter::inPlaceFilter, py::arg("cloud")); + } + } +} diff --git a/python/datapointsfilters/add_descriptor.h b/python/datapointsfilters/add_descriptor.h new file mode 100644 index 00000000..033229be --- /dev/null +++ b/python/datapointsfilters/add_descriptor.h @@ -0,0 +1,14 @@ +#ifndef PYTHON_DATAPOINTSFILTERS_ADD_DESCRIPTOR_H +#define PYTHON_DATAPOINTSFILTERS_ADD_DESCRIPTOR_H + +#include "pypoint_matcher_helper.h" + +namespace python +{ + namespace datapointsfilters + { + void pybindAddDescriptor(py::module& p_module); + } +} + +#endif //PYTHON_DATAPOINTSFILTERS_ADD_DESCRIPTOR_H diff --git a/utest/ui/DataFilters.cpp b/utest/ui/DataFilters.cpp index 1bff003d..371f8f34 100644 --- a/utest/ui/DataFilters.cpp +++ b/utest/ui/DataFilters.cpp @@ -950,3 +950,71 @@ TEST_F(DataFilterTest, SpectralDecompositionDataPointsFilter) addFilter("SpectralDecompositionDataPointsFilter", params); validate3dTransformation(); } + +TEST_F(DataFilterTest, AddDescriptorDataPointsFilter) +{ + using DPFiltersPtr = std::shared_ptr; + + // Test with point cloud + DP cloud = generateRandomDataPoints(100); + + std::string descriptorName = "test_descriptor"; + std::size_t descriptorDimension = 3; + std::vector descriptorValues{2, 3, 4}; + + // This filter adds a new descriptor + params = PM::Parameters(); + params["descriptorName"] = descriptorName; + params["descriptorDimension"] = toParam(descriptorDimension); + params["descriptorValues"] = toParam(descriptorValues); + + DPFiltersPtr addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create( + "AddDescriptorDataPointsFilter", params + ); + + DP filteredCloud = addDescriptorFilter->filter(cloud); + + EXPECT_EQ(cloud.getNbPoints(), filteredCloud.getNbPoints()); + EXPECT_EQ(cloud.getDescriptorDim()+descriptorDimension, filteredCloud.getDescriptorDim()); + EXPECT_EQ(cloud.getTimeDim(), filteredCloud.getTimeDim()); + + Eigen::Matrix row = Eigen::Matrix::Ones(cloud.getNbPoints()); + EXPECT_EQ(filteredCloud.descriptorLabels.back().text, descriptorName); + EXPECT_EQ(filteredCloud.descriptorLabels.back().span, descriptorDimension); + for(unsigned i = 0; i < descriptorDimension; ++i) + { + EXPECT_EQ(filteredCloud.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]); + } + + + descriptorValues = std::vector{-2, -3, -4}; + params["descriptorValues"] = toParam(descriptorValues); + + addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create( + "AddDescriptorDataPointsFilter", params + ); + DP filteredCloudOvewriteParams = addDescriptorFilter->filter(filteredCloud); + EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().text, descriptorName); + EXPECT_EQ(filteredCloudOvewriteParams.descriptorLabels.back().span, descriptorDimension); + for(unsigned i = 0; i < descriptorDimension; ++i) + { + EXPECT_EQ(filteredCloudOvewriteParams.descriptors.row(filteredCloud.descriptors.rows()-3+i), row*descriptorValues[i]); + } + + + descriptorValues = std::vector{-2, -3, -4, -5}; + params["descriptorDimension"] = toParam(4); + params["descriptorValues"] = toParam(descriptorValues); + addDescriptorFilter = PM::get().DataPointsFilterRegistrar.create( + "AddDescriptorDataPointsFilter", params + ); + EXPECT_THROW(addDescriptorFilter->filter(filteredCloud), std::runtime_error); + + params = PM::Parameters(); + params["descriptorName"] = "my_descriptor"; + params["descriptorDimension"] = toParam(descriptorDimension); + params["descriptorValues"] = "[2, 3, 4]"; + + addFilter("AddDescriptorDataPointsFilter", params); + validate3dTransformation(); +} \ No newline at end of file diff --git a/utest/ui/IO.cpp b/utest/ui/IO.cpp index 5b3aa6ed..41a2380a 100644 --- a/utest/ui/IO.cpp +++ b/utest/ui/IO.cpp @@ -25,6 +25,9 @@ TEST(IOTest, loadYaml) std::ifstream ifs3((dataPath + "unit_tests/badIcpConfig_InvalidModuleType.yaml").c_str()); EXPECT_THROW(icp.loadFromYaml(ifs3), PointMatcherSupport::InvalidModuleType); + + std::ifstream ifs4((dataPath + "add_descriptor_config.yaml").c_str()); + EXPECT_NO_THROW(PM::DataPointsFilters filters(ifs4)); } TEST(IOTest, loadCSV)