From b9b6a19f68e801b05907c989497fd7d4560d2c17 Mon Sep 17 00:00:00 2001 From: Bradley J Chambers Date: Thu, 25 Jun 2020 11:07:53 -0500 Subject: [PATCH] Use Eigen Affine3d instead --- filters/TransformationFilter.cpp | 43 +++++++++++++++----------------- 1 file changed, 20 insertions(+), 23 deletions(-) diff --git a/filters/TransformationFilter.cpp b/filters/TransformationFilter.cpp index cf6de05976..6563b4150d 100644 --- a/filters/TransformationFilter.cpp +++ b/filters/TransformationFilter.cpp @@ -151,29 +151,26 @@ void TransformationFilter::initialize() Transform& matrix = *m_matrix; - // rotation matrix P is simply inverted - Matrix3d P; - P << matrix[0], matrix[1], matrix[2], - matrix[4], matrix[5], matrix[6], - matrix[8], matrix[9], matrix[10]; - Matrix3d Pinv = P.inverse(); - matrix[0] = Pinv(0,0); - matrix[1] = Pinv(0,1); - matrix[2] = Pinv(0,2); - matrix[4] = Pinv(1,0); - matrix[5] = Pinv(1,1); - matrix[6] = Pinv(1,2); - matrix[8] = Pinv(2,0); - matrix[9] = Pinv(2,1); - matrix[10] = Pinv(2,2); - - // translation vector v is multiplied by the inverse of negative P - Vector3d v; - v << matrix[3], matrix[7], matrix[11]; - Vector3d negPinvV = (-P).inverse() * v; - matrix[3] = negPinvV(0); - matrix[7] = negPinvV(1); - matrix[11] = negPinvV(2); + Affine3d T; + Matrix4d m; + m << matrix[0], matrix[1], matrix[2], matrix[3], + matrix[4], matrix[5], matrix[6], matrix[7], + matrix[8], matrix[9], matrix[10], matrix[11], + matrix[12], matrix[13], matrix[14], matrix[15]; + T.matrix() = m; + Affine3d Tinv = T.inverse(); + matrix[0] = Tinv.matrix()(0,0); + matrix[1] = Tinv.matrix()(0,1); + matrix[2] = Tinv.matrix()(0,2); + matrix[3] = Tinv.matrix()(0,3); + matrix[4] = Tinv.matrix()(1,0); + matrix[5] = Tinv.matrix()(1,1); + matrix[6] = Tinv.matrix()(1,2); + matrix[7] = Tinv.matrix()(1,3); + matrix[8] = Tinv.matrix()(2,0); + matrix[9] = Tinv.matrix()(2,1); + matrix[10] = Tinv.matrix()(2,2); + matrix[11] = Tinv.matrix()(2,3); } }