diff --git a/pointmatcher/TransformationsImpl.cpp b/pointmatcher/TransformationsImpl.cpp index 36da5a02..562f063f 100644 --- a/pointmatcher/TransformationsImpl.cpp +++ b/pointmatcher/TransformationsImpl.cpp @@ -125,8 +125,14 @@ typename PointMatcher::TransformationParameters TransformationsImpl::Rigid } else if(ortho.cols() == 3) { + const T epsilon = 0.001; + // R = [ a b] // [-b a] + if(anyabs(parameters(0,0) - parameters(1,1)) > epsilon || anyabs(parameters(1,0) + parameters(0,1)) > epsilon) + { + throw TransformationError("RigidTransformation: Error, only proper rigid transformations are supported."); + } // mean of a and b T a = (parameters(0,0) + parameters(1,1))/2; diff --git a/utest/ui/Transformations.cpp b/utest/ui/Transformations.cpp index 23a887e4..918b5533 100644 --- a/utest/ui/Transformations.cpp +++ b/utest/ui/Transformations.cpp @@ -103,20 +103,29 @@ TEST(Transformation, RigidTransformation) //------------------------------------- // Construct a 2D non-orthogonal matrix - PM::Matrix T_2D = PM::Matrix::Identity(3,3); - T_2D(1,0) = 8.99; - T_2D(0,1) = 4.03; + 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)); + EXPECT_FALSE(rigidTrans->checkParameters(T_2D_non_ortho)); - EXPECT_THROW(rigidTrans->compute(data2D, T_2D), TransformationError); + EXPECT_THROW(rigidTrans->compute(data2D, T_2D_non_ortho), TransformationError); // Check stability over iterations for(int i = 0; i < 10; i++) { - T_2D = rigidTrans->correctParameters(T_2D); - EXPECT_TRUE(rigidTrans->checkParameters(T_2D)); + 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); + delete rigidTrans; }