Fix for new Transform::rotation() behavior for isometries

Author: kzampog

examples/ransac_transform_estimator.cpp

@@ -48,7 +48,7 @@ int main(int argc, char** argv) {
       // Randomly transform src
       cilantro::RigidTransform3f tform;
       tform.linear().setRandom();
-      tform.linear() = tform.rotation();
+      tform.linear() = cilantro::LinearTransform<float, 3, false>(tform.linear()).rotation();
       tform.translation().setRandom();
 
       // Build noisy correspondences

include/cilantro/core/space_transformations.hpp

@@ -121,7 +121,7 @@ class TransformSet : public internal::TransformSetBase<TransformT> {
 #pragma omp parallel for
     for (size_t i = 0; i < this->size(); i++) {
       (*this)[i] = other[i] * (*this)[i];
-      (*this)[i].linear() = (*this)[i].rotation();
+      (*this)[i].linear() = LinearTransform<Scalar, Dim, false>((*this)[i].linear()).rotation();
     }
     return *this;
   }
@@ -146,7 +146,7 @@ class TransformSet : public internal::TransformSetBase<TransformT> {
 #pragma omp parallel for
     for (size_t i = 0; i < this->size(); i++) {
       (*this)[i] = (*this)[i] * other[i];
-      (*this)[i].linear() = (*this)[i].rotation();
+      (*this)[i].linear() = LinearTransform<Scalar, Dim, false>((*this)[i].linear()).rotation();
     }
     return *this;
   }

include/cilantro/registration/icp_single_transform_combined_metric.hpp

@@ -205,7 +205,9 @@ class CombinedMetricSingleTransformICP
     }
 
     if constexpr (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
-      tform_iter.linear() = tform_iter.rotation();
+      tform_iter.linear() =
+          LinearTransform<typename TransformT::Scalar, TransformT::Dim, false>(tform_iter.linear())
+              .rotation();
     }
 
     this->transform_ = tform_iter * this->transform_;

include/cilantro/registration/icp_single_transform_point_to_point_metric.hpp

@@ -53,8 +53,10 @@ class PointToPointMetricSingleTransformICP
                                         corr_getter_proxy.getPointToPointCorrespondences(),
                                         tform_iter);
 
-    if (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
-      tform_iter.linear() = tform_iter.rotation();
+    if constexpr (int(Base::Transform::Mode) == int(Eigen::Isometry)) {
+      tform_iter.linear() =
+          LinearTransform<typename TransformT::Scalar, TransformT::Dim, false>(tform_iter.linear())
+              .rotation();
     }
     this->transform_ = tform_iter * this->transform_;
     this->last_delta_norm_ =

include/cilantro/registration/warp_field_estimation.hpp

@@ -707,7 +707,7 @@ estimateDenseWarpFieldCombinedMetric(
          Eigen::AngleAxis<ScalarT>(tforms_vec[6 * i + 1], Eigen::Matrix<ScalarT, 3, 1>::UnitY()) *
          Eigen::AngleAxis<ScalarT>(tforms_vec[6 * i + 0], Eigen::Matrix<ScalarT, 3, 1>::UnitX()))
             .matrix();
-    transforms[i].linear() = transforms[i].rotation();
+    transforms[i].linear() = LinearTransform<ScalarT, 3, false>(transforms[i].linear()).rotation();
     transforms[i].translation() = tforms_vec.template segment<3>(6 * i + 3);
   }
 
@@ -1838,7 +1838,7 @@ estimateSparseWarpFieldCombinedMetric(
          Eigen::AngleAxis<ScalarT>(tforms_vec[6 * i + 1], Eigen::Matrix<ScalarT, 3, 1>::UnitY()) *
          Eigen::AngleAxis<ScalarT>(tforms_vec[6 * i + 0], Eigen::Matrix<ScalarT, 3, 1>::UnitX()))
             .matrix();
-    transforms[i].linear() = transforms[i].rotation();
+    transforms[i].linear() = LinearTransform<ScalarT, 3, false>(transforms[i].linear()).rotation();
     transforms[i].translation() = tforms_vec.template segment<3>(6 * i + 3);
   }
 

include/cilantro/registration/warp_field_utilities.hpp

@@ -36,8 +36,11 @@ resampleTransforms(const TransformSet<TransformT>& old_transforms,
     } else {
       total_weight = (typename TransformT::Scalar)(1.0) / total_weight;
       new_transforms[i].linear() *= total_weight;
-      if (int(TransformT::Mode) == int(Eigen::Isometry)) {
-        new_transforms[i].linear() = new_transforms[i].rotation();
+      if constexpr (int(TransformT::Mode) == int(Eigen::Isometry)) {
+        new_transforms[i].linear() =
+            LinearTransform<typename TransformT::Scalar, TransformT::Dim, false>(
+                new_transforms[i].linear())
+                .rotation();
       }
       new_transforms[i].translation() *= total_weight;
     }
@@ -88,8 +91,11 @@ resampleTransforms(
     } else {
       total_weight = (typename TransformT::Scalar)(1.0) / total_weight;
       new_transforms[i].linear() *= total_weight;
-      if (int(TransformT::Mode) == int(Eigen::Isometry)) {
-        new_transforms[i].linear() = new_transforms[i].rotation();
+      if constexpr (int(TransformT::Mode) == int(Eigen::Isometry)) {
+        new_transforms[i].linear() =
+            LinearTransform<typename TransformT::Scalar, TransformT::Dim, false>(
+                new_transforms[i].linear())
+                .rotation();
       }
       new_transforms[i].translation() *= total_weight;
     }