From 87c7dc4ed32333a70994d92dae2c9e4daa39389a Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Thu, 19 Nov 2020 19:35:44 +0100 Subject: [PATCH 1/4] Implemented an in-place method for transforming DataPoints objects --- pointmatcher/PointMatcher.h | 3 + pointmatcher/TransformationsImpl.cpp | 84 +++++++++++++++++----------- pointmatcher/TransformationsImpl.h | 3 + utest/ui/Transformations.cpp | 53 ++++++++++++++++++ 4 files changed, 111 insertions(+), 32 deletions(-) diff --git a/pointmatcher/PointMatcher.h b/pointmatcher/PointMatcher.h index 4c991e2e..f26aa2ce 100644 --- a/pointmatcher/PointMatcher.h +++ b/pointmatcher/PointMatcher.h @@ -410,6 +410,9 @@ struct PointMatcher //! Transform input using the transformation matrix virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const = 0; + //! Transform point cloud in-place using the transformation matrix + virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const = 0; + //! Return whether the given parameters respect the expected constraints virtual bool checkParameters(const TransformationParameters& parameters) const = 0; diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index 562f063f..116c06a7 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -51,7 +51,18 @@ typename PointMatcher::DataPoints TransformationsImpl::RigidTransformation const DataPoints& input, const TransformationParameters& parameters) const { - assert(input.features.rows() == parameters.rows()); + DataPoints transformedCloud = input; + inPlaceCompute(parameters, transformedCloud); + return transformedCloud; +} + +//! RigidTransformation +template +void TransformationsImpl::RigidTransformation::inPlaceCompute( + const TransformationParameters& parameters, + DataPoints& cloud) const +{ + assert(cloud.features.rows() == parameters.rows()); assert(parameters.rows() == parameters.cols()); const unsigned int nbRows = parameters.rows()-1; @@ -62,28 +73,25 @@ typename PointMatcher::DataPoints TransformationsImpl::RigidTransformation if(this->checkParameters(parameters) == false) throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal."); - //DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols()); - DataPoints transformedCloud = input; - // Apply the transformation to features - transformedCloud.features = parameters * input.features; - + cloud.features.applyOnTheLeft(parameters); + // Apply the transformation to descriptors int row(0); - const int descCols(input.descriptors.cols()); - for (size_t i = 0; i < input.descriptorLabels.size(); ++i) + const int descCols(cloud.descriptors.cols()); + for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i) { - const int span(input.descriptorLabels[i].span); - const std::string& name(input.descriptorLabels[i].text); - const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols)); - BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols)); + const int span(cloud.descriptorLabels[i].span); + const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") + { + const BOOST_AUTO(inputDesc, cloud.descriptors.block(row, 0, span, descCols)); + BOOST_AUTO(outputDesc, cloud.descriptors.block(row, 0, span, descCols)); outputDesc = R * inputDesc; + } row += span; } - - return transformedCloud; } //! Ensure orthogonality of the rotation matrix @@ -158,6 +166,17 @@ template typename PointMatcher::DataPoints TransformationsImpl::SimilarityTransformation::compute( const DataPoints& input, const TransformationParameters& parameters) const +{ + DataPoints transformedCloud = input; + inPlaceCompute(parameters, transformedCloud); + return transformedCloud; +} + +//! SimilarityTransformation +template +void TransformationsImpl::SimilarityTransformation::inPlaceCompute( + const TransformationParameters& parameters, + DataPoints& cloud) const { assert(input.features.rows() == parameters.rows()); assert(parameters.rows() == parameters.cols()); @@ -170,28 +189,25 @@ typename PointMatcher::DataPoints TransformationsImpl::SimilarityTransform if(this->checkParameters(parameters) == false) throw TransformationError("SimilarityTransformation: Error, invalid similarity transform."); - //DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols()); - DataPoints transformedCloud = input; - // Apply the transformation to features - transformedCloud.features = parameters * input.features; + cloud.features.applyOnTheLeft(parameters); // Apply the transformation to descriptors int row(0); - const int descCols(input.descriptors.cols()); - for (size_t i = 0; i < input.descriptorLabels.size(); ++i) + const int descCols(cloud.descriptors.cols()); + for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i) { - const int span(input.descriptorLabels[i].span); - const std::string& name(input.descriptorLabels[i].text); - const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols)); - BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols)); + const int span(cloud.descriptorLabels[i].span); + const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") + { + const BOOST_AUTO(inputDesc, cloud.descriptors.block(row, 0, span, descCols)); + BOOST_AUTO(outputDesc, cloud.descriptors.block(row, 0, span, descCols)); outputDesc = R * inputDesc; + } row += span; } - - return transformedCloud; } //! Nothing to check for a similarity transform @@ -215,19 +231,23 @@ template struct TransformationsImpl::SimilarityTransformation; template typename PointMatcher::DataPoints TransformationsImpl::PureTranslation::compute(const DataPoints& input, const TransformationParameters& parameters) const { + DataPoints transformedCloud = input; + inPlaceCompute(parameters, transformedCloud); + return transformedCloud; +} + +template +void TransformationsImpl::PureTranslation::inPlaceCompute( + const TransformationParameters& parameters, + DataPoints& cloud) const { assert(input.features.rows() == parameters.rows()); assert(parameters.rows() == parameters.cols()); if(this->checkParameters(parameters) == false) throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity."); - //DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.features.cols()); - DataPoints transformedCloud = input; - // Apply the transformation to features - transformedCloud.features = parameters * input.features; - - return transformedCloud; + cloud.features.applyOnTheLeft(parameters); } template diff --git a/pointmatcher/TransformationsImpl.h b/pointmatcher/TransformationsImpl.h index faf156e1..a202a351 100644 --- a/pointmatcher/TransformationsImpl.h +++ b/pointmatcher/TransformationsImpl.h @@ -60,6 +60,7 @@ struct TransformationsImpl RigidTransformation() : Transformation("RigidTransformation", ParametersDoc(), Parameters()) {} virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const; + virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const; virtual bool checkParameters(const TransformationParameters& parameters) const; virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const; }; @@ -72,6 +73,7 @@ struct TransformationsImpl } virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const; + virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const; virtual bool checkParameters(const TransformationParameters& parameters) const; virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const; }; @@ -85,6 +87,7 @@ struct TransformationsImpl PureTranslation() : Transformation("PureTranslation", ParametersDoc(), Parameters()) {} virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const; + virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const; virtual bool checkParameters(const TransformationParameters& parameters) const; virtual TransformationParameters correctParameters(const TransformationParameters& parameters) const; }; diff --git a/utest/ui/Transformations.cpp b/utest/ui/Transformations.cpp index 2c17b8f0..71ae6b13 100644 --- a/utest/ui/Transformations.cpp +++ b/utest/ui/Transformations.cpp @@ -129,3 +129,56 @@ TEST(Transformation, RigidTransformation) EXPECT_THROW(rigidTrans->correctParameters(T_2D_reflection), TransformationError); } + +TEST(Transformation, InPlaceRigidTransformation) +{ + std::shared_ptr rigidTrans; + rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); + + //------------------------------------- + // Construct a 3D non-orthogonal matrix + PM::Matrix T_3D = PM::Matrix::Identity(4,4); + //T_3D(0,0) = 2.3; + //T_3D(0,1) = 0.03; + T_3D << 0.99935116, 0.13669771, 0.03436585, 1.71138524, + -0.02633967, 0.99326295, -0.04907545, -0.10860933, + -0.03615132, 0.04400287, 0.99820427, -0.04454497, + 0. , 0. , 0. , 1.; + + EXPECT_FALSE(rigidTrans->checkParameters(T_3D)); + + EXPECT_THROW(rigidTrans->inPlaceCompute(T_3D, data3D), TransformationError); + + // Check stability over iterations + for(int i = 0; i < 10; i++) + { + T_3D = rigidTrans->correctParameters(T_3D); + ASSERT_TRUE(rigidTrans->checkParameters(T_3D)); + } + + //------------------------------------- + // Construct a 2D non-orthogonal matrix + PM::Matrix T_2D_non_ortho = PM::Matrix::Identity(3,3); + T_2D_non_ortho(0,0) = 0.8; + T_2D_non_ortho(0,1) = -0.5; + T_2D_non_ortho(1,0) = 0.5; + T_2D_non_ortho(1,1) = 0.8; + + EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho)); + + EXPECT_THROW(rigidTrans->inPlaceCompute(T_2D_non_ortho, data2D), TransformationError); + + // Check stability over iterations + for(int i = 0; i < 10; i++) + { + T_2D_non_ortho = rigidTrans->correctParameters(T_2D_non_ortho); + EXPECT_TRUE(rigidTrans->checkParameters(T_2D_non_ortho)); + } + + //------------------------------------- + // Construct a 2D reflection matrix + PM::Matrix T_2D_reflection = PM::Matrix::Identity(3,3); + T_2D_reflection(1,1) = -1; + + EXPECT_THROW(rigidTrans->correctParameters(T_2D_reflection), TransformationError); +} From 2f6065adc2c73c8f1bbd3a85596c72b1ee332329 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Thu, 19 Nov 2020 20:06:15 +0100 Subject: [PATCH 2/4] Fix typo --- pointmatcher/TransformationsImpl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index 116c06a7..00184b90 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -178,7 +178,7 @@ void TransformationsImpl::SimilarityTransformation::inPlaceCompute( const TransformationParameters& parameters, DataPoints& cloud) const { - assert(input.features.rows() == parameters.rows()); + assert(cloud.features.rows() == parameters.rows()); assert(parameters.rows() == parameters.cols()); const unsigned int nbRows = parameters.rows()-1; @@ -240,7 +240,7 @@ template void TransformationsImpl::PureTranslation::inPlaceCompute( const TransformationParameters& parameters, DataPoints& cloud) const { - assert(input.features.rows() == parameters.rows()); + assert(cloud.features.rows() == parameters.rows()); assert(parameters.rows() == parameters.cols()); if(this->checkParameters(parameters) == false) From 7c3530890dc586c068579c8862d0a68100d0c54f Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Thu, 19 Nov 2020 20:33:48 +0100 Subject: [PATCH 3/4] Optimized descriptor rotations --- pointmatcher/TransformationsImpl.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index 00184b90..d1fce4f7 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -85,9 +85,7 @@ void TransformationsImpl::RigidTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { - const BOOST_AUTO(inputDesc, cloud.descriptors.block(row, 0, span, descCols)); - BOOST_AUTO(outputDesc, cloud.descriptors.block(row, 0, span, descCols)); - outputDesc = R * inputDesc; + cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R); } row += span; @@ -201,9 +199,7 @@ void TransformationsImpl::SimilarityTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { - const BOOST_AUTO(inputDesc, cloud.descriptors.block(row, 0, span, descCols)); - BOOST_AUTO(outputDesc, cloud.descriptors.block(row, 0, span, descCols)); - outputDesc = R * inputDesc; + cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R); } row += span; From 3e65d0623fb621bfce79920ab4322d537a9451b6 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Tue, 1 Dec 2020 18:49:31 +0100 Subject: [PATCH 4/4] Implemented unit tests, added sim transformation to registry, set new in place functions to work --- pointmatcher/Registry.cpp | 1 + pointmatcher/Transformation.cpp | 5 +- pointmatcher/TransformationsImpl.h | 1 + utest/ui/Transformations.cpp | 533 +++++++++++++++++++++-------- utest/utest.h | 3 +- 5 files changed, 390 insertions(+), 153 deletions(-) diff --git a/pointmatcher/Registry.cpp b/pointmatcher/Registry.cpp index 2e436c61..512b2aef 100644 --- a/pointmatcher/Registry.cpp +++ b/pointmatcher/Registry.cpp @@ -62,6 +62,7 @@ PointMatcher::PointMatcher() { ADD_TO_REGISTRAR_NO_PARAM(Transformation, RigidTransformation, typename TransformationsImpl::RigidTransformation) ADD_TO_REGISTRAR_NO_PARAM(Transformation, PureTranslation, typename TransformationsImpl::PureTranslation) + ADD_TO_REGISTRAR_NO_PARAM(Transformation, SimilarityTransformation, typename TransformationsImpl::SimilarityTransformation) ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, IdentityDataPointsFilter, typename DataPointsFiltersImpl::IdentityDataPointsFilter) ADD_TO_REGISTRAR_NO_PARAM(DataPointsFilter, RemoveNaNDataPointsFilter, typename DataPointsFiltersImpl::RemoveNaNDataPointsFilter) diff --git a/pointmatcher/Transformation.cpp b/pointmatcher/Transformation.cpp index 3c580828..734c47d1 100644 --- a/pointmatcher/Transformation.cpp +++ b/pointmatcher/Transformation.cpp @@ -66,12 +66,9 @@ void PointMatcher::Transformations::apply(DataPoints& cloud, const Transforma // TODO: This API must be re-written to not even have the concept // of chain for this classs. int num_iter = 0; - - DataPoints transformedCloud; for (TransformationsConstIt it = this->begin(); it != this->end(); ++it) { - transformedCloud = (*it)->compute(cloud, parameters); - swapDataPoints(cloud, transformedCloud); + (*it)->inPlaceCompute(parameters, cloud); num_iter++; } if (num_iter != 1) diff --git a/pointmatcher/TransformationsImpl.h b/pointmatcher/TransformationsImpl.h index a202a351..0e2998d0 100644 --- a/pointmatcher/TransformationsImpl.h +++ b/pointmatcher/TransformationsImpl.h @@ -72,6 +72,7 @@ struct TransformationsImpl return "Similarity transformation (rotation + translation + scale)."; } + SimilarityTransformation() : Transformation("SimilarityTransformation", ParametersDoc(), Parameters()) {} virtual DataPoints compute(const DataPoints& input, const TransformationParameters& parameters) const; virtual void inPlaceCompute(const TransformationParameters& parameters, DataPoints& cloud) const; virtual bool checkParameters(const TransformationParameters& parameters) const; diff --git a/utest/ui/Transformations.cpp b/utest/ui/Transformations.cpp index 71ae6b13..8e048bab 100644 --- a/utest/ui/Transformations.cpp +++ b/utest/ui/Transformations.cpp @@ -4,181 +4,418 @@ using namespace std; using namespace PointMatcherSupport; +static inline Eigen::Transform buildUpTransformation2D(const Eigen::Matrix& translation, + const Eigen::Rotation2D& rotation, + const NumericType scale = 1.0) +{ + const Eigen::Transform transformation = + Eigen::Translation(translation) * rotation * Eigen::Scaling(scale); + return transformation; +} + +static inline Eigen::Transform buildUpTransformation3D(const Eigen::Matrix& translation, + const Eigen::Quaternion& rotation, + const NumericType scale = 1.0) +{ + const Eigen::Transform transformation = + Eigen::Translation(translation) * Eigen::AngleAxis(rotation.normalized()) * Eigen::Scaling(scale); + return transformation; +} + +static inline void assertOnDataPointsTransformation(const PM::DataPoints& cloud, const PM::TransformationParameters& transformation, + std::shared_ptr& transformator, + const NumericType kEpsilonNumericalError = 0) +{ + // Transform point cloud. + auto transformedCloud = cloud; + transformator->inPlaceCompute(transformation, transformedCloud); + // Assert on the result. + // Transformation quality. 'Who tests the unit test?' + EXPECT_TRUE(transformator->checkParameters(transformation)); + // Features. + for (size_t i = 0; i < cloud.getNbPoints(); ++i) + { + const auto transformedFeature = transformation * cloud.features.col(i); + ASSERT_TRUE(transformedFeature.isApprox(transformedCloud.features.col(i), kEpsilonNumericalError)); + } + + // Descriptors. + int row(0); + const int descCols(cloud.descriptors.cols()); + const unsigned int nbRows = transformation.rows() - 1; + const unsigned int nbCols = transformation.cols() - 1; + const PM::TransformationParameters R(transformation.topLeftCorner(nbRows, nbCols)); + for (size_t i = 0; i < cloud.descriptorLabels.size(); ++i) + { + const int span(cloud.descriptorLabels[i].span); + const std::string& name(cloud.descriptorLabels[i].text); + if (name == "normals" || name == "observationDirections") + { + const auto transformedDescriptor = R * cloud.descriptors.block(row, 0, span, descCols); + ASSERT_TRUE(transformedDescriptor.isApprox(transformedCloud.descriptors.block(row, 0, span, descCols), kEpsilonNumericalError)); + } + row += span; + } + EXPECT_EQ(cloud.featureLabels, transformedCloud.featureLabels); + EXPECT_EQ(cloud.descriptorLabels, transformedCloud.descriptorLabels); + EXPECT_EQ(cloud.timeLabels, transformedCloud.timeLabels); + EXPECT_EQ(cloud.times, transformedCloud.times); +} + //--------------------------- // Transformation Checker modules //--------------------------- // Utility classes -class TransformationCheckerTest: public IcpHelper +class TransformationCheckerTest : public IcpHelper { public: - std::shared_ptr transformCheck; - - // Will be called for every tests - virtual void SetUp() - { - icp.setDefault(); - // Uncomment for consol outputs - //setLogger(PM::get().LoggerRegistrar.create("FileLogger")); - - icp.transformationCheckers.clear(); - } - - // Will be called for every tests - virtual void TearDown(){} - - void addFilter(string name, PM::Parameters params) - { - transformCheck = - PM::get().TransformationCheckerRegistrar.create(name, params); - - icp.transformationCheckers.push_back(transformCheck); - } + std::shared_ptr transformCheck; + + // Will be called for every tests + virtual void SetUp() + { + icp.setDefault(); + // Uncomment for consol outputs + //setLogger(PM::get().LoggerRegistrar.create("FileLogger")); + + icp.transformationCheckers.clear(); + } + + // Will be called for every tests + virtual void TearDown() {} + + void addFilter(string name, PM::Parameters params) + { + transformCheck = PM::get().TransformationCheckerRegistrar.create(name, params); + + icp.transformationCheckers.push_back(transformCheck); + } }; TEST_F(TransformationCheckerTest, CounterTransformationChecker) { - addFilter("CounterTransformationChecker", {{"maxIterationCount", toParam(20)}}); - validate2dTransformation(); + addFilter("CounterTransformationChecker", { { "maxIterationCount", toParam(20) } }); + validate2dTransformation(); } TEST_F(TransformationCheckerTest, DifferentialTransformationChecker) { - addFilter("DifferentialTransformationChecker", { - {"minDiffRotErr", toParam(0.001)}, - {"minDiffTransErr", toParam(0.001)}, - {"smoothLength", toParam(4)} - } - ); - validate2dTransformation(); + addFilter("DifferentialTransformationChecker", + { { "minDiffRotErr", toParam(0.001) }, { "minDiffTransErr", toParam(0.001) }, { "smoothLength", toParam(4) } }); + validate2dTransformation(); } TEST_F(TransformationCheckerTest, BoundTransformationChecker) { - // Since that transChecker is trigger when the distance is growing - // and that we do not expect that to happen in the test dataset, we - // keep the Counter to get out of the looop - std::shared_ptr extraTransformCheck; - - extraTransformCheck = PM::get().TransformationCheckerRegistrar.create( - "CounterTransformationChecker" - ); - icp.transformationCheckers.push_back(extraTransformCheck); - - addFilter("BoundTransformationChecker", { - {"maxRotationNorm", toParam(1.0)}, - {"maxTranslationNorm", toParam(1.0)} - } - ); - validate2dTransformation(); + // Since that transChecker is trigger when the distance is growing + // and that we do not expect that to happen in the test dataset, we + // keep the Counter to get out of the looop + std::shared_ptr extraTransformCheck; + + extraTransformCheck = PM::get().TransformationCheckerRegistrar.create("CounterTransformationChecker"); + icp.transformationCheckers.push_back(extraTransformCheck); + + addFilter("BoundTransformationChecker", { { "maxRotationNorm", toParam(1.0) }, { "maxTranslationNorm", toParam(1.0) } }); + validate2dTransformation(); } //--------------------------- // Transformation //--------------------------- -TEST(Transformation, RigidTransformation) +TEST(Transformation, RigidTransformationParameterCheck) +{ + std::shared_ptr rigidTrans; + rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); + + //------------------------------------- + // Construct a 3D non-orthogonal matrix + PM::Matrix T_3D = PM::Matrix::Identity(4, 4); + //T_3D(0,0) = 2.3; + //T_3D(0,1) = 0.03; + T_3D << 0.99935116, 0.13669771, 0.03436585, 1.71138524, -0.02633967, 0.99326295, -0.04907545, -0.10860933, -0.03615132, 0.04400287, + 0.99820427, -0.04454497, 0., 0., 0., 1.; + + EXPECT_FALSE(rigidTrans->checkParameters(T_3D)); + + EXPECT_THROW(rigidTrans->compute(data3D, T_3D), TransformationError); + + // Check stability over iterations + for (int i = 0; i < 10; i++) + { + T_3D = rigidTrans->correctParameters(T_3D); + ASSERT_TRUE(rigidTrans->checkParameters(T_3D)); + } + + //------------------------------------- + // Construct a 2D non-orthogonal matrix + PM::Matrix T_2D_non_ortho = PM::Matrix::Identity(3, 3); + T_2D_non_ortho(0, 0) = 0.8; + T_2D_non_ortho(0, 1) = -0.5; + T_2D_non_ortho(1, 0) = 0.5; + T_2D_non_ortho(1, 1) = 0.8; + + EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho)); + + EXPECT_THROW(rigidTrans->compute(data2D, T_2D_non_ortho), TransformationError); + EXPECT_THROW(rigidTrans->inPlaceCompute(T_2D_non_ortho, data2D), TransformationError); + + // Check stability over iterations + for (int i = 0; i < 10; i++) + { + T_2D_non_ortho = rigidTrans->correctParameters(T_2D_non_ortho); + EXPECT_TRUE(rigidTrans->checkParameters(T_2D_non_ortho)); + } + + //------------------------------------- + // Construct a 2D reflection matrix + PM::Matrix T_2D_reflection = PM::Matrix::Identity(3, 3); + T_2D_reflection(1, 1) = -1; + + EXPECT_THROW(rigidTrans->correctParameters(T_2D_reflection), TransformationError); +} + +TEST(Transformation, ComputePureTranslationDataPoints2D) { - std::shared_ptr rigidTrans; - rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); - - //------------------------------------- - // Construct a 3D non-orthogonal matrix - PM::Matrix T_3D = PM::Matrix::Identity(4,4); - //T_3D(0,0) = 2.3; - //T_3D(0,1) = 0.03; - T_3D << 0.99935116, 0.13669771, 0.03436585, 1.71138524, - -0.02633967, 0.99326295, -0.04907545, -0.10860933, - -0.03615132, 0.04400287, 0.99820427, -0.04454497, - 0. , 0. , 0. , 1.; - - EXPECT_FALSE(rigidTrans->checkParameters(T_3D)); - - EXPECT_THROW(rigidTrans->compute(data3D, T_3D), TransformationError); - - // Check stability over iterations - for(int i = 0; i < 10; i++) - { - T_3D = rigidTrans->correctParameters(T_3D); - ASSERT_TRUE(rigidTrans->checkParameters(T_3D)); - } - - //------------------------------------- - // Construct a 2D non-orthogonal matrix - PM::Matrix T_2D_non_ortho = PM::Matrix::Identity(3,3); - T_2D_non_ortho(0,0) = 0.8; - T_2D_non_ortho(0,1) = -0.5; - T_2D_non_ortho(1,0) = 0.5; - T_2D_non_ortho(1,1) = 0.8; - - EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho)); - - EXPECT_THROW(rigidTrans->compute(data2D, T_2D_non_ortho), TransformationError); - - // Check stability over iterations - for(int i = 0; i < 10; i++) - { - T_2D_non_ortho = rigidTrans->correctParameters(T_2D_non_ortho); - EXPECT_TRUE(rigidTrans->checkParameters(T_2D_non_ortho)); - } - - //------------------------------------- - // Construct a 2D reflection matrix - PM::Matrix T_2D_reflection = PM::Matrix::Identity(3,3); - T_2D_reflection(1,1) = -1; - - EXPECT_THROW(rigidTrans->correctParameters(T_2D_reflection), TransformationError); + std::shared_ptr transformator = PM::get().REG(Transformation).create("PureTranslation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure translation. + { + const Eigen::Matrix translation{ -1.0001, 34.5 }; + const Eigen::Rotation2D rotation{ 0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } } -TEST(Transformation, InPlaceRigidTransformation) +TEST(Transformation, ComputePureTranslationDataPoints3D) { - std::shared_ptr rigidTrans; - rigidTrans = PM::get().REG(Transformation).create("RigidTransformation"); - - //------------------------------------- - // Construct a 3D non-orthogonal matrix - PM::Matrix T_3D = PM::Matrix::Identity(4,4); - //T_3D(0,0) = 2.3; - //T_3D(0,1) = 0.03; - T_3D << 0.99935116, 0.13669771, 0.03436585, 1.71138524, - -0.02633967, 0.99326295, -0.04907545, -0.10860933, - -0.03615132, 0.04400287, 0.99820427, -0.04454497, - 0. , 0. , 0. , 1.; - - EXPECT_FALSE(rigidTrans->checkParameters(T_3D)); - - EXPECT_THROW(rigidTrans->inPlaceCompute(T_3D, data3D), TransformationError); - - // Check stability over iterations - for(int i = 0; i < 10; i++) - { - T_3D = rigidTrans->correctParameters(T_3D); - ASSERT_TRUE(rigidTrans->checkParameters(T_3D)); - } - - //------------------------------------- - // Construct a 2D non-orthogonal matrix - PM::Matrix T_2D_non_ortho = PM::Matrix::Identity(3,3); - T_2D_non_ortho(0,0) = 0.8; - T_2D_non_ortho(0,1) = -0.5; - T_2D_non_ortho(1,0) = 0.5; - T_2D_non_ortho(1,1) = 0.8; - - EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho)); - - EXPECT_THROW(rigidTrans->inPlaceCompute(T_2D_non_ortho, data2D), TransformationError); - - // Check stability over iterations - for(int i = 0; i < 10; i++) - { - T_2D_non_ortho = rigidTrans->correctParameters(T_2D_non_ortho); - EXPECT_TRUE(rigidTrans->checkParameters(T_2D_non_ortho)); - } - - //------------------------------------- - // Construct a 2D reflection matrix - PM::Matrix T_2D_reflection = PM::Matrix::Identity(3,3); - T_2D_reflection(1,1) = -1; - - EXPECT_THROW(rigidTrans->correctParameters(T_2D_reflection), TransformationError); + std::shared_ptr transformator = PM::get().REG(Transformation).create("PureTranslation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + + // Pure translation. + { + const Eigen::Matrix translation{ -1.0001, 5, -12321.234 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } } + +TEST(Transformation, ComputeRigidTransformDataPoints2D) +{ + std::shared_ptr transformator = PM::get().REG(Transformation).create("RigidTransformation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure translation. + { + const Eigen::Matrix translation{ -1.0001, 34.5 }; + const Eigen::Rotation2D rotation{ 0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure rotation. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 3.53453 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Translation + rotation. + { + const Eigen::Matrix translation{ -3.11, 100.222 }; + const Eigen::Rotation2D rotation{ -123.3 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } +} + +TEST(Transformation, ComputeRigidTransformDataPoints3D) +{ + std::shared_ptr transformator = PM::get().REG(Transformation).create("RigidTransformation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + + // Pure translation. + { + const Eigen::Matrix translation{ -1.0001, 5, -12321.234 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure rotation. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, -5, 23, 0.5 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Translation + rotation. + { + const NumericType kEpsilonNumericalError = 1e-6; + const Eigen::Matrix translation{ 1, -3, -4 }; + const Eigen::Quaternion rotation{ 0, -2.54, 0, 0.5 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator, kEpsilonNumericalError); + } +} + +TEST(Transformation, ComputeSimilarityTransformDataPoints2D) +{ + std::shared_ptr transformator = PM::get().REG(Transformation).create("SimilarityTransformation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 0 }; + const NumericType scale{ 1.0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure Upscaling. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 0 }; + const NumericType scale{ 5.0 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure Downscaling. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 0 }; + const NumericType scale{ 0.1 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure translation + Downscaling. + { + const Eigen::Matrix translation{ -1.0001, 34.5 }; + const Eigen::Rotation2D rotation{ 0 }; + const NumericType scale{ 0.5 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure rotation + Upscaling. + { + const Eigen::Matrix translation{ 0, 0 }; + const Eigen::Rotation2D rotation{ 3.53453 }; + const NumericType scale{ 1.9 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Translation + rotation + Upscaling. + { + const Eigen::Matrix translation{ -3.11, 100.222 }; + const Eigen::Rotation2D rotation{ -123.3 }; + const NumericType scale{ 1.9 }; + const Eigen::Transform transformation = buildUpTransformation2D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } +} + +TEST(Transformation, ComputeSimilarityTransformDataPoints3D) +{ + std::shared_ptr transformator = PM::get().REG(Transformation).create("SimilarityTransformation"); + + // Identity. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const NumericType scale{ 1.0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure Upscaling. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const NumericType scale{ 5.0 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure Downscaling. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const NumericType scale{ 0.1 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure translation + Downscaling. + { + const Eigen::Matrix translation{ -1.0001, 5, -12321.234 }; + const Eigen::Quaternion rotation{ 1, 0, 0, 0 }; + const NumericType scale{ 0.5 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Pure rotation + Upscaling. + { + const Eigen::Matrix translation{ 0, 0, 0 }; + const Eigen::Quaternion rotation{ 1, -5, 23, 0.5 }; + const NumericType scale{ 1.9 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator); + } + // Translation + rotation + Upscaling. + { + const NumericType kEpsilonNumericalError = 1e-6; + const Eigen::Matrix translation{ 1, -3, -4 }; + const Eigen::Quaternion rotation{ 0, -2.54, 0, 0.5 }; + const NumericType scale{ 1.9 }; + const Eigen::Transform transformation = buildUpTransformation3D(translation, rotation, scale); + // Transform and assert on the result. + assertOnDataPointsTransformation(data3D, transformation.matrix(), transformator, kEpsilonNumericalError); + } +} \ No newline at end of file diff --git a/utest/utest.h b/utest/utest.h index 43844ab0..5322546f 100644 --- a/utest/utest.h +++ b/utest/utest.h @@ -12,7 +12,8 @@ #include "boost/filesystem/path.hpp" #include "boost/filesystem/operations.hpp" -typedef PointMatcher PM; +typedef float NumericType; +typedef PointMatcher PM; typedef PM::DataPoints DP; extern std::string dataPath;