From 289a90604c0c9a12a76a3e4657527d930e265ab7 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Wed, 2 Dec 2020 13:08:33 +0100 Subject: [PATCH 1/3] Optimized transformations by transposing and then multiplying in place --- pointmatcher/TransformationsImpl.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index d1fce4f71..e1e7a15e3 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -74,7 +74,8 @@ void TransformationsImpl::RigidTransformation::inPlaceCompute( throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal."); // Apply the transformation to features - cloud.features.applyOnTheLeft(parameters); + // B = A * B translates to B.transpose *= A.transpose() + cloud.features.transpose().applyOnTheRight(parameters.transpose()); // Apply the transformation to descriptors int row(0); @@ -85,7 +86,8 @@ void TransformationsImpl::RigidTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { - cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R); + // B = A * B translates to B.transpose *= A.transpose() + cloud.descriptors.block(row, 0, span, descCols).transpose() *= R.transpose(); } row += span; @@ -188,7 +190,8 @@ void TransformationsImpl::SimilarityTransformation::inPlaceCompute( throw TransformationError("SimilarityTransformation: Error, invalid similarity transform."); // Apply the transformation to features - cloud.features.applyOnTheLeft(parameters); + // B = A * B translates to B.transpose *= A.transpose() + cloud.features.transpose().applyOnTheRight(parameters.transpose()); // Apply the transformation to descriptors int row(0); @@ -199,7 +202,8 @@ void TransformationsImpl::SimilarityTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { - cloud.descriptors.block(row, 0, span, descCols).applyOnTheLeft(R); + // B = A * B translates to B.transpose *= A.transpose() + cloud.descriptors.block(row, 0, span, descCols).transpose() *= R.transpose(); } row += span; @@ -243,7 +247,8 @@ void TransformationsImpl::PureTranslation::inPlaceCompute( throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity."); // Apply the transformation to features - cloud.features.applyOnTheLeft(parameters); + // B = A * B translates to B.transpose *= A.transpose() + cloud.features.transpose().applyOnTheRight(parameters.transpose()); } template From 1eb485072844172576b3fa107ae927f79b79d60c Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Wed, 2 Dec 2020 15:57:54 +0100 Subject: [PATCH 2/3] Better comments --- pointmatcher/TransformationsImpl.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index e1e7a15e3..f5ef411d2 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -73,7 +73,7 @@ void TransformationsImpl::RigidTransformation::inPlaceCompute( if(this->checkParameters(parameters) == false) throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal."); - // Apply the transformation to features + // Apply the transformation to features. // B = A * B translates to B.transpose *= A.transpose() cloud.features.transpose().applyOnTheRight(parameters.transpose()); @@ -86,6 +86,7 @@ void TransformationsImpl::RigidTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { + // Rotate descriptors. // B = A * B translates to B.transpose *= A.transpose() cloud.descriptors.block(row, 0, span, descCols).transpose() *= R.transpose(); } @@ -202,6 +203,7 @@ void TransformationsImpl::SimilarityTransformation::inPlaceCompute( const std::string& name(cloud.descriptorLabels[i].text); if (name == "normals" || name == "observationDirections") { + // Rotate descriptors. // B = A * B translates to B.transpose *= A.transpose() cloud.descriptors.block(row, 0, span, descCols).transpose() *= R.transpose(); } @@ -246,8 +248,8 @@ void TransformationsImpl::PureTranslation::inPlaceCompute( if(this->checkParameters(parameters) == false) throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part not identity."); - // Apply the transformation to features - // B = A * B translates to B.transpose *= A.transpose() + // Apply the transformation to features. + // B = A * B translates to B.transpose() *= A.transpose() cloud.features.transpose().applyOnTheRight(parameters.transpose()); } From 520f67fdbddb4cea635051d6723c136d0d682e06 Mon Sep 17 00:00:00 2001 From: YoshuaNava Date: Wed, 2 Dec 2020 16:38:34 +0100 Subject: [PATCH 3/3] Adjust transformations unit test epsilon --- utest/ui/Transformations.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utest/ui/Transformations.cpp b/utest/ui/Transformations.cpp index 8e048bab4..cd43667e5 100644 --- a/utest/ui/Transformations.cpp +++ b/utest/ui/Transformations.cpp @@ -24,7 +24,7 @@ static inline Eigen::Transform buildUpTransformat static inline void assertOnDataPointsTransformation(const PM::DataPoints& cloud, const PM::TransformationParameters& transformation, std::shared_ptr& transformator, - const NumericType kEpsilonNumericalError = 0) + const NumericType kEpsilonNumericalError = 1e-13) { // Transform point cloud. auto transformedCloud = cloud;