New DataPoints filter for descriptor augmentation

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

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]

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 <typename T>
+AddDescriptorDataPointsFilter<T>::AddDescriptorDataPointsFilter(const Parameters& params) :
+	PointMatcher<T>::DataPointsFilter("AddDescriptorDataPointsFilter",
+		AddDescriptorDataPointsFilter::availableParameters(), params),
+	descriptorName(Parametrizable::get<std::string>("descriptorName")),
+	descriptorDimension(Parametrizable::get<std::size_t>("descriptorDimension")),
+	descriptorValues(Parametrizable::getVector<T>("descriptorValues"))
+{
+    assert(descriptorDimension == descriptorValues.size());
+}
+
+// AddDescriptorDataPointsFilter
+template<typename T>
+typename PointMatcher<T>::DataPoints AddDescriptorDataPointsFilter<T>::filter(
+	const DataPoints& input)
+{
+	DataPoints output(input);
+	inPlaceFilter(output);
+	return output;
+}
+
+// In-place filter
+template<typename T>
+void AddDescriptorDataPointsFilter<T>::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<float>;
+template struct AddDescriptorDataPointsFilter<double>;

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<typename T>
+struct AddDescriptorDataPointsFilter : public PointMatcher<T>::DataPointsFilter
+{
+    // Type definitions
+    typedef PointMatcher<T> 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<T>::Matrix Matrix;
+    typedef typename PointMatcher<T>::Vector Vector;
+    typedef typename PointMatcher<T>::DataPoints::InvalidField InvalidField;
+
+    const std::string descriptorName;
+    const std::size_t descriptorDimension;
+    const std::vector<T> 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);
+};

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<typename T>
 struct DataPointsFiltersImpl
 {
 	typedef ::IdentityDataPointsFilter<T>   IdentityDataPointsFilter;
+	typedef ::AddDescriptorDataPointsFilter<T>   AddDescriptorDataPointsFilter;
 	typedef ::RemoveNaNDataPointsFilter<T>  RemoveNaNDataPointsFilter;
 	typedef ::MaxDistDataPointsFilter<T>	MaxDistDataPointsFilter;
 	typedef ::MinDistDataPointsFilter<T>	MinDistDataPointsFilter;

pointmatcher/Parametrizable.h

@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <limits>
 #define BOOST_ASSIGN_MAX_PARAMS 6
 #include <boost/assign/list_inserter.hpp>
+#include <boost/algorithm/string.hpp>
 
 
 namespace PointMatcherSupport
@@ -77,7 +78,37 @@ namespace PointMatcherSupport
 	{
 		return boost::lexical_cast<Target>(arg);
 	}
-
+
+    //! Special case of lexical cast to std::vector<T>
+    template<typename T>
+    inline std::vector<T> 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<std::string> tokens;
+        boost::algorithm::split(tokens, cleanedInput, boost::algorithm::is_any_of(","));
+
+        std::vector<T> result;
+        result.reserve(tokens.size());
+        for(const auto& token: tokens)
+        {
+            auto value = lexical_cast_scalar_to_string<T>(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<float>(arg); }
@@ -87,12 +118,24 @@ namespace PointMatcherSupport
 
 	//
 
-	//! Return the a string value using lexical_cast
+	//! Return the string value using lexical_cast
 	template<typename S>
-	std::string toParam(const S& value)
+	inline std::string toParam(const S& value)
 	{
 		return lexical_cast<std::string>(value);
 	}
+
+    //! Return the string value of a std::vector<T>
+    template<typename T>
+    inline std::string toParam(const std::vector<T>& input)
+    {
+        std::ostringstream oss;
+        oss << "[";
+        std::copy(input.begin(), input.end() - 1,
+                  std::ostream_iterator<T>(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<typename S>
 		S get(const std::string& paramName) { return lexical_cast<S>(getParamValueString(paramName)); }
-
+
+		//! Return the value of paramName, lexically-casted std::vector<S>
+		template<typename T>
+		inline std::vector<T> getVector(const std::string& paramName) {return lexical_cast_vector<T>(getParamValueString(paramName)); }
+
 		friend std::ostream& operator<< (std::ostream& o, const Parametrizable& p);
 	};
 	std::ostream& operator<< (std::ostream& o, const Parametrizable::ParametersDoc& p);

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>();
-				std::string value = paramIt->second.as<std::string>();
-				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<std::string>();
+                }
 			}
 		}
 	}

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<C>();

pointmatcher/Registry.cpp

@@ -64,6 +64,7 @@ PointMatcher<T>::PointMatcher()
 	ADD_TO_REGISTRAR_NO_PARAM(Transformation, PureTranslation, typename TransformationsImpl<T>::PureTranslation)
 	ADD_TO_REGISTRAR_NO_PARAM(Transformation, SimilarityTransformation, typename TransformationsImpl<T>::SimilarityTransformation)	
 
+	ADD_TO_REGISTRAR(DataPointsFilter, AddDescriptorDataPointsFilter, typename DataPointsFiltersImpl<T>::AddDescriptorDataPointsFilter)
 	ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, IdentityDataPointsFilter, typename DataPointsFiltersImpl<T>::IdentityDataPointsFilter)
 	ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, RemoveNaNDataPointsFilter, typename DataPointsFiltersImpl<T>::RemoveNaNDataPointsFilter)
 	ADD_TO_REGISTRAR(DataPointsFilter, MaxDistDataPointsFilter, typename DataPointsFiltersImpl<T>::MaxDistDataPointsFilter)

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<PM::DataPointsFilter>;
+
+	// Test with point cloud
+	DP cloud = generateRandomDataPoints(100);
+
+    std::string descriptorName = "test_descriptor";
+    std::size_t descriptorDimension = 3;
+    std::vector<float> 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<float, 1, Eigen::Dynamic> row = Eigen::Matrix<float, 1, Eigen::Dynamic>::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<float>{-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<float>{-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();
+}

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)