diff --git a/MANIFEST.in b/MANIFEST.in
index 5035a9ad..da84ead5 100644
--- a/MANIFEST.in
+++ b/MANIFEST.in
@@ -1,5 +1,4 @@
 recursive-include src *.py
-recursive-include src *.yaml
 recursive-include src *.cpp
 recursive-include src *.hpp
 recursive-include src *.h
diff --git a/src/cpp/kiss_icp/CMakeLists.txt b/src/cpp/kiss_icp/CMakeLists.txt
index c613bc9f..4fb636ca 100644
--- a/src/cpp/kiss_icp/CMakeLists.txt
+++ b/src/cpp/kiss_icp/CMakeLists.txt
@@ -37,4 +37,6 @@ include(cmake/CompilerOptions.cmake)
 
 add_subdirectory(core)
 add_subdirectory(metrics)
-add_subdirectory(pipeline)
+if(NOT BUILD_PYTHON_BINDINGS)
+  add_subdirectory(pipeline)
+endif()
diff --git a/src/cpp/kiss_icp/core/Deskew.cpp b/src/cpp/kiss_icp/core/Deskew.cpp
index 79fe5547..424385e0 100644
--- a/src/cpp/kiss_icp/core/Deskew.cpp
+++ b/src/cpp/kiss_icp/core/Deskew.cpp
@@ -30,52 +30,30 @@
 #include <tuple>
 #include <vector>
 
+#include "Transforms.hpp"
+
 namespace {
-Eigen::Isometry3d MakeTransform(const Eigen::Vector3d &xyz, const Eigen::Vector3d &rpy) {
-    Eigen::AngleAxisd omega(rpy.norm(), rpy.normalized());
-    Eigen::Isometry3d transform(omega);
-    transform.translation() = xyz;
-    return transform;
+using VelocityTuple = std::tuple<Eigen::Vector3d, Eigen::Vector3d>;
+VelocityTuple VelocityEstimation(const Eigen::Isometry3d &start_pose,
+                                 const Eigen::Isometry3d &finish_pose,
+                                 double scan_duration) {
+    const auto delta_pose = kiss_icp::ConcatenateIsometries(start_pose.inverse(), finish_pose);
+    const auto linear_velocity = delta_pose.translation() / scan_duration;
+    const auto rotation = Eigen::AngleAxisd(delta_pose.rotation());
+    const auto angular_velocity = rotation.axis() * rotation.angle() / scan_duration;
+    return std::make_tuple(linear_velocity, angular_velocity);
 }
 }  // namespace
 
 namespace kiss_icp {
-using VelocityTuple = MotionCompensator::VelocityTuple;
-using Vector3dVector = std::vector<Eigen::Vector3d>;
-
-VelocityTuple MotionCompensator::VelocityEstimation(const Eigen::Matrix4d &start_pose,
-                                                    const Eigen::Matrix4d &finish_pose) {
-    const Eigen::Matrix4d delta_pose = start_pose.inverse() * finish_pose;
-    const Eigen::Vector3d linear_velocity = delta_pose.block<3, 1>(0, 3) / scan_duration_;
-    const auto rotation = Eigen::AngleAxisd(delta_pose.block<3, 3>(0, 0));
-    const Eigen::Vector3d angular_velocity = rotation.axis() * rotation.angle() / scan_duration_;
-    return std::make_tuple(linear_velocity, angular_velocity);
-}
-
-Vector3dVector MotionCompensator::DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
-                                             const std::vector<double> &timestamps,
-                                             const Eigen::Vector3d &linear_velocity,
-                                             const Eigen::Vector3d &angular_velocity) {
-    std::vector<Eigen::Vector3d> corrected_frame(frame.size());
-    tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
-        const auto &dt = timestamps[i];
-        const auto motion = MakeTransform(dt * linear_velocity, dt * angular_velocity);
-        corrected_frame[i] = motion * frame[i];
-    });
-    return corrected_frame;
-}
 
+using Vector3dVector = std::vector<Eigen::Vector3d>;
 Vector3dVector MotionCompensator::DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
                                              const std::vector<double> &timestamps,
-                                             const std::vector<Eigen::Matrix4d> &poses) {
-    //  If not enough poses for the estimation, do not de-skew
-    const size_t N = poses.size();
-    if (N <= 2) return frame;
-
+                                             const Eigen::Isometry3d &start_pose,
+                                             const Eigen::Isometry3d &finish_pose) {
     // Estimate linear and angular velocities
-    const auto &start_pose = poses[N - 2];
-    const auto &finish_pose = poses[N - 1];
-    const auto [lvel, avel] = VelocityEstimation(start_pose, finish_pose);
+    const auto [lvel, avel] = VelocityEstimation(start_pose, finish_pose, scan_duration_);
 
     // TODO(Nacho) Add explanation of this
     std::vector<double> timestamps_ = timestamps;
@@ -83,8 +61,13 @@ Vector3dVector MotionCompensator::DeSkewScan(const std::vector<Eigen::Vector3d>
         timestamp = scan_duration_ * (timestamp - mid_pose_timestamp_);
     });
 
-    // Compensate using the estimated velocities
-    return MotionCompensator::DeSkewScan(frame, timestamps_, lvel, avel);
+    std::vector<Eigen::Vector3d> corrected_frame(frame.size());
+    tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
+        const auto &dt = timestamps_[i];
+        const auto motion = MakeTransform(dt * lvel, dt * avel);
+        corrected_frame[i] = motion * frame[i];
+    });
+    return corrected_frame;
 }
 
 }  // namespace kiss_icp
diff --git a/src/cpp/kiss_icp/core/Deskew.hpp b/src/cpp/kiss_icp/core/Deskew.hpp
index d83e18f0..6dc73e15 100644
--- a/src/cpp/kiss_icp/core/Deskew.hpp
+++ b/src/cpp/kiss_icp/core/Deskew.hpp
@@ -23,29 +23,21 @@
 #pragma once
 
 #include <Eigen/Core>
-#include <tuple>
+#include <Eigen/Geometry>
 #include <vector>
 
 namespace kiss_icp {
 
 class MotionCompensator {
 public:
+    /// Constructs a Odometry-based motion compensation scheme
     explicit MotionCompensator(double frame_rate) : scan_duration_(1 / frame_rate) {}
-    /// Compensate the frame using the given poses
+
+    /// Compensate the frame by estimatng the velocity between the given poses
     std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
                                             const std::vector<double> &timestamps,
-                                            const std::vector<Eigen::Matrix4d> &poses);
-
-    /// Estimate the linear and angular velocities given 2 poses
-    using VelocityTuple = std::tuple<Eigen::Vector3d, Eigen::Vector3d>;
-    VelocityTuple VelocityEstimation(const Eigen::Matrix4d &start_pose,
-                                     const Eigen::Matrix4d &finish_pose);
-
-    // Expose this function to allow motion compensation using IMUs
-    static std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
-                                                   const std::vector<double> &timestamps,
-                                                   const Eigen::Vector3d &linear_velocity,
-                                                   const Eigen::Vector3d &angular_velocity);
+                                            const Eigen::Isometry3d &start_pose,
+                                            const Eigen::Isometry3d &finish_pose);
 
 private:
     double scan_duration_;
diff --git a/src/cpp/kiss_icp/core/Preprocessing.cpp b/src/cpp/kiss_icp/core/Preprocessing.cpp
index 87e7f473..caf1a2bd 100644
--- a/src/cpp/kiss_icp/core/Preprocessing.cpp
+++ b/src/cpp/kiss_icp/core/Preprocessing.cpp
@@ -33,16 +33,23 @@
 #include <cmath>
 #include <vector>
 
-#include "Voxel.hpp"
+namespace {
+using Voxel = Eigen::Vector3i;
+struct VoxelHash {
+    size_t operator()(const Voxel &voxel) const {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
+    }
+};
+}  // namespace
 
 namespace kiss_icp {
-
 std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
                                              double voxel_size) {
-    tsl::robin_map<Voxel, Eigen::Vector3d> grid;
+    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
     grid.reserve(frame.size());
     for (const auto &point : frame) {
-        const auto voxel = Voxel(point, voxel_size);
+        const auto voxel = Voxel((point / voxel_size).cast<int>());
         if (grid.contains(voxel)) continue;
         grid.insert({voxel, point});
     }
diff --git a/src/cpp/kiss_icp/core/Registration.cpp b/src/cpp/kiss_icp/core/Registration.cpp
index 2846a6d5..f7363294 100644
--- a/src/cpp/kiss_icp/core/Registration.cpp
+++ b/src/cpp/kiss_icp/core/Registration.cpp
@@ -25,11 +25,17 @@
 #include <tbb/blocked_range.h>
 #include <tbb/parallel_reduce.h>
 
-#include <Eigen/Dense>
 #include <algorithm>
 #include <cmath>
 #include <tuple>
 
+#include "Transforms.hpp"
+
+namespace Eigen {
+using Matrix6d = Eigen::Matrix<double, 6, 6>;
+using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
+}  // namespace Eigen
+
 namespace {
 
 inline double square(double x) { return x * x; }
@@ -60,25 +66,15 @@ struct ResultTuple {
     Eigen::Vector6d JTr;
 };
 
-Eigen::Matrix4d TransformVector6dToMatrix4d(const Eigen::Vector6d &x) {
-    Eigen::Matrix4d output = Eigen::Matrix4d::Identity();
-    output.block<3, 3>(0, 0) = (Eigen::AngleAxisd(x(2), Eigen::Vector3d::UnitZ()) *
-                                Eigen::AngleAxisd(x(1), Eigen::Vector3d::UnitY()) *
-                                Eigen::AngleAxisd(x(0), Eigen::Vector3d::UnitX()))
-                                   .matrix();
-    output.block<3, 1>(0, 3) = x.block<3, 1>(3, 0);
-    return output;
-}
-
 }  // namespace
 
 namespace kiss_icp {
 
-std::tuple<Eigen::Vector6d, Eigen::Matrix4d> AlignClouds(const std::vector<Eigen::Vector3d> &source,
-                                                         const std::vector<Eigen::Vector3d> &target,
-                                                         double th) {
+PerturbationMatrixTuple AlignClouds(const std::vector<Eigen::Vector3d> &source,
+                                    const std::vector<Eigen::Vector3d> &target,
+                                    double th) {
     Eigen::Vector6d x = ComputeUpdate(source, target, th);
-    Eigen::Matrix4d update = TransformVector6dToMatrix4d(x);
+    Eigen::Isometry3d update = Vector6dToIsometry3d(x);
     return {x, update};
 }
 
diff --git a/src/cpp/kiss_icp/core/Registration.hpp b/src/cpp/kiss_icp/core/Registration.hpp
index 9966b655..87afc2ee 100644
--- a/src/cpp/kiss_icp/core/Registration.hpp
+++ b/src/cpp/kiss_icp/core/Registration.hpp
@@ -23,12 +23,11 @@
 #pragma once
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <tuple>
 #include <vector>
 
 namespace Eigen {
-using Matrix6d = Eigen::Matrix<double, 6, 6>;
-using Matrix3_6d = Eigen::Matrix<double, 3, 6>;
 using Vector6d = Eigen::Matrix<double, 6, 1>;
 }  // namespace Eigen
 
@@ -38,7 +37,8 @@ Eigen::Vector6d ComputeUpdate(const std::vector<Eigen::Vector3d> &source,
                               const std::vector<Eigen::Vector3d> &target,
                               double th);
 
-std::tuple<Eigen::Vector6d, Eigen::Matrix4d> AlignClouds(const std::vector<Eigen::Vector3d> &source,
-                                                         const std::vector<Eigen::Vector3d> &target,
-                                                         double th);
+using PerturbationMatrixTuple = std::tuple<Eigen::Vector6d, Eigen::Isometry3d>;
+PerturbationMatrixTuple AlignClouds(const std::vector<Eigen::Vector3d> &source,
+                                    const std::vector<Eigen::Vector3d> &target,
+                                    double th);
 }  // namespace kiss_icp
diff --git a/src/cpp/kiss_icp/core/Threshold.cpp b/src/cpp/kiss_icp/core/Threshold.cpp
index d7a3908f..824f6136 100644
--- a/src/cpp/kiss_icp/core/Threshold.cpp
+++ b/src/cpp/kiss_icp/core/Threshold.cpp
@@ -26,12 +26,10 @@
 #include <cmath>
 
 namespace {
-double ComputeModelError(const Eigen::Matrix4d &model_deviation, double max_range) {
-    const Eigen::Matrix3d &R = model_deviation.block<3, 3>(0, 0);
-    const Eigen::Vector3d &t = model_deviation.block<3, 1>(0, 3);
-    const double theta = std::acos(0.5 * (R.trace() - 1));
+double ComputeModelError(const Eigen::Isometry3d &model_deviation, double max_range) {
+    const double theta = Eigen::AngleAxisd(model_deviation.rotation()).angle();
     const double delta_rot = 2.0 * max_range * std::sin(theta / 2.0);
-    const double delta_trans = t.norm();
+    const double delta_trans = model_deviation.translation().norm();
     return delta_trans + delta_rot;
 }
 }  // namespace
diff --git a/src/cpp/kiss_icp/core/Threshold.hpp b/src/cpp/kiss_icp/core/Threshold.hpp
index 397ed0f9..154ff9a8 100644
--- a/src/cpp/kiss_icp/core/Threshold.hpp
+++ b/src/cpp/kiss_icp/core/Threshold.hpp
@@ -22,7 +22,7 @@
 // SOFTWARE.
 #pragma once
 
-#include <Eigen/Core>
+#include <Eigen/Geometry>
 
 namespace kiss_icp {
 
@@ -33,7 +33,7 @@ struct AdaptiveThreshold {
           max_range_(max_range) {}
 
     /// Update the current belief of the deviation from the prediction model
-    inline void UpdateModelDeviation(const Eigen::Matrix4d &current_deviation) {
+    inline void UpdateModelDeviation(const Eigen::Isometry3d &current_deviation) {
         model_deviation_ = current_deviation;
     }
 
@@ -48,7 +48,7 @@ struct AdaptiveThreshold {
     // Local cache for ccomputation
     double model_error_sse2_ = 0;
     int num_samples_ = 0;
-    Eigen::Matrix4d model_deviation_ = Eigen::Matrix4d::Identity();
+    Eigen::Isometry3d model_deviation_ = Eigen::Isometry3d::Identity();
 };
 
 }  // namespace kiss_icp
diff --git a/src/cpp/kiss_icp/core/Transforms.hpp b/src/cpp/kiss_icp/core/Transforms.hpp
new file mode 100644
index 00000000..56984aa5
--- /dev/null
+++ b/src/cpp/kiss_icp/core/Transforms.hpp
@@ -0,0 +1,64 @@
+// MIT License
+//
+// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
+// Stachniss.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in all
+// copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <vector>
+
+namespace Eigen {
+using Vector6d = Eigen::Matrix<double, 6, 1>;
+}  // namespace Eigen
+
+namespace kiss_icp {
+
+inline Eigen::Isometry3d MakeTransform(const Eigen::Vector3d &xyz, const Eigen::Vector3d &theta) {
+    const double angle = theta.norm();
+    const Eigen::Vector3d axis = theta.normalized();
+    Eigen::AngleAxisd omega(angle, axis);
+    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
+    transform.linear() = omega.toRotationMatrix();
+    transform.translation() = xyz;
+    return transform;
+}
+
+inline Eigen::Isometry3d Vector6dToIsometry3d(const Eigen::Vector6d &x) {
+    const Eigen::Vector3d &xyz = x.tail<3>();
+    const Eigen::Vector3d &theta = x.head<3>();
+    return MakeTransform(xyz, theta);
+}
+
+inline void TransformPoints(const Eigen::Isometry3d &T, std::vector<Eigen::Vector3d> &points) {
+    std::transform(points.cbegin(), points.cend(), points.begin(),
+                   [&](const auto &point) { return T * point; });
+}
+
+inline Eigen::Isometry3d ConcatenateIsometries(const Eigen::Isometry3d &lhs,
+                                               const Eigen::Isometry3d &rhs) {
+    Eigen::Isometry3d returned = Eigen::Isometry3d::Identity();
+    returned.linear() = lhs.rotation() * rhs.rotation();
+    returned.translation() = lhs.rotation() * rhs.translation() + lhs.translation();
+    return returned;
+}
+}  // namespace kiss_icp
diff --git a/src/cpp/kiss_icp/core/Voxel.hpp b/src/cpp/kiss_icp/core/Voxel.hpp
deleted file mode 100644
index 1a69a0d1..00000000
--- a/src/cpp/kiss_icp/core/Voxel.hpp
+++ /dev/null
@@ -1,56 +0,0 @@
-// MIT License
-//
-// Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
-// Stachniss.
-//
-// Permission is hereby granted, free of charge, to any person obtaining a copy
-// of this software and associated documentation files (the "Software"), to deal
-// in the Software without restriction, including without limitation the rights
-// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-// copies of the Software, and to permit persons to whom the Software is
-// furnished to do so, subject to the following conditions:
-//
-// The above copyright notice and this permission notice shall be included in all
-// copies or substantial portions of the Software.
-//
-// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-// SOFTWARE.
-
-#pragma once
-
-#include <Eigen/Core>
-#include <array>
-#include <vector>
-
-namespace kiss_icp {
-
-struct Voxel {
-    Voxel(int32_t x, int32_t y, int32_t z) : ijk({x, y, z}) {}
-    Voxel(const Eigen::Vector3d &point, double voxel_size)
-        : ijk({static_cast<int32_t>(point.x() / voxel_size),
-               static_cast<int32_t>(point.y() / voxel_size),
-               static_cast<int32_t>(point.z() / voxel_size)}) {}
-    inline bool operator==(const Voxel &vox) const {
-        return ijk[0] == vox.ijk[0] && ijk[1] == vox.ijk[1] && ijk[2] == vox.ijk[2];
-    }
-
-    std::array<int32_t, 3> ijk;
-};
-}  // namespace kiss_icp
-
-// Specialization of std::hash for our custom type Voxel
-namespace std {
-
-template <>
-struct hash<kiss_icp::Voxel> {
-    std::size_t operator()(const kiss_icp::Voxel &vox) const {
-        const uint32_t *vec = reinterpret_cast<const uint32_t *>(vox.ijk.data());
-        return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
-    }
-};
-}  // namespace std
diff --git a/src/cpp/kiss_icp/core/VoxelHashMap.cpp b/src/cpp/kiss_icp/core/VoxelHashMap.cpp
index abc8c184..1d52b682 100644
--- a/src/cpp/kiss_icp/core/VoxelHashMap.cpp
+++ b/src/cpp/kiss_icp/core/VoxelHashMap.cpp
@@ -26,6 +26,7 @@
 #include <tbb/parallel_reduce.h>
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <algorithm>
 #include <limits>
 #include <tuple>
@@ -33,20 +34,13 @@
 #include <vector>
 
 #include "Registration.hpp"
+#include "Transforms.hpp"
 
 // This parameters are not intended to be changed, therefore we do not expose it
 namespace {
 constexpr int MAX_NUM_ITERATIONS_ = 500;
 constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
 
-inline void TransformPoints(const Eigen::Matrix4d &T, std::vector<Eigen::Vector3d> &points) {
-    std::transform(points.cbegin(), points.cend(), points.begin(), [&](const auto &point) {
-        const Eigen::Matrix3d R = T.block<3, 3>(0, 0);
-        const Eigen::Vector3d translation = T.block<3, 1>(0, 3);
-        return Eigen::Vector3d{R * point + translation};
-    });
-}
-
 struct ResultTuple {
     ResultTuple(std::size_t n) {
         source.reserve(n);
@@ -59,10 +53,10 @@ struct ResultTuple {
 
 namespace kiss_icp {
 
-Eigen::Matrix4d VoxelHashMap::RegisterPoinCloud(const Vector3dVector &points,
-                                                const Eigen::Matrix4d &initial_guess,
-                                                double max_correspondence_distance,
-                                                double kernel) {
+Eigen::Isometry3d VoxelHashMap::RegisterPointCloud(const Vector3dVector &points,
+                                                   const Eigen::Isometry3d &initial_guess,
+                                                   double max_correspondence_distance,
+                                                   double kernel) {
     if (Empty()) return initial_guess;
 
     // Equation (9)
@@ -70,7 +64,7 @@ Eigen::Matrix4d VoxelHashMap::RegisterPoinCloud(const Vector3dVector &points,
     TransformPoints(initial_guess, source);
 
     // ICP-loop
-    Eigen::Matrix4d T_icp = Eigen::Matrix4d::Identity();
+    Eigen::Isometry3d T_icp = Eigen::Isometry3d::Identity();
     for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) {
         // Equation (10)
         const auto &[src, tgt] = GetCorrespondences(source, max_correspondence_distance);
@@ -79,12 +73,12 @@ Eigen::Matrix4d VoxelHashMap::RegisterPoinCloud(const Vector3dVector &points,
         // Equation (12)
         TransformPoints(estimation, source);
         // Update iterations
-        T_icp = estimation * T_icp;
+        T_icp = ConcatenateIsometries(estimation, T_icp);
         // Termination criteria
         if (x.norm() < ESTIMATION_THRESHOLD_) break;
     }
     // Spit the final transformation
-    return T_icp * initial_guess;
+    return ConcatenateIsometries(T_icp, initial_guess);
 }
 
 VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences(
@@ -188,16 +182,16 @@ void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &o
     RemovePointsFarFromLocation(origin);
 }
 
-void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Matrix4d &pose) {
+void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Isometry3d &pose) {
     auto points_t = points;
     TransformPoints(pose, points_t);
-    const Eigen::Vector3d &origin = pose.block<3, 1>(0, 3);
+    const Eigen::Vector3d &origin = pose.translation();
     Update(points_t, origin);
 }
 
 void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
     std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
-        auto voxel = Voxel(point, voxel_size_);
+        auto voxel = Voxel((point / voxel_size_).template cast<int>());
         auto search = map_.find(voxel);
         if (search != map_.end()) {
             auto &voxel_block = search.value();
diff --git a/src/cpp/kiss_icp/core/VoxelHashMap.hpp b/src/cpp/kiss_icp/core/VoxelHashMap.hpp
index 8a7e98b4..2c49af6b 100644
--- a/src/cpp/kiss_icp/core/VoxelHashMap.hpp
+++ b/src/cpp/kiss_icp/core/VoxelHashMap.hpp
@@ -29,47 +29,52 @@
 #include <tsl/robin_map.h>
 
 #include <Eigen/Core>
+#include <Eigen/Geometry>
 #include <vector>
 
-#include "Voxel.hpp"
-
 namespace kiss_icp {
 struct VoxelHashMap {
     using Vector3dVector = std::vector<Eigen::Vector3d>;
     using Vector3dVectorTuple = std::tuple<Vector3dVector, Vector3dVector>;
+    using Voxel = Eigen::Vector3i;
+    struct VoxelBlock {
+        // buffer of points with a max limit of n_points
+        std::vector<Eigen::Vector3d> points;
+        int num_points_;
+        inline void AddPoint(const Eigen::Vector3d &point) {
+            if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point);
+        }
+    };
+    struct VoxelHash {
+        size_t operator()(const Voxel &voxel) const {
+            const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+            return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
+        }
+    };
 
     explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel)
         : voxel_size_(voxel_size),
           max_distance_(max_distance),
           max_points_per_voxel_(max_points_per_voxel) {}
 
-    Eigen::Matrix4d RegisterPoinCloud(const Vector3dVector &points,
-                                      const Eigen::Matrix4d &initial_guess,
-                                      double max_correspondence_distance,
-                                      double kernel);
+    Eigen::Isometry3d RegisterPointCloud(const Vector3dVector &points,
+                                         const Eigen::Isometry3d &initial_guess,
+                                         double max_correspondence_distance,
+                                         double kernel);
 
     Vector3dVectorTuple GetCorrespondences(const Vector3dVector &points,
                                            double max_correspondance_distance) const;
     inline void Clear() { map_.clear(); }
     inline bool Empty() const { return map_.empty(); }
     void Update(const std::vector<Eigen::Vector3d> &points, const Eigen::Vector3d &origin);
-    void Update(const std::vector<Eigen::Vector3d> &points, const Eigen::Matrix4d &pose);
+    void Update(const std::vector<Eigen::Vector3d> &points, const Eigen::Isometry3d &pose);
     void AddPoints(const std::vector<Eigen::Vector3d> &points);
     void RemovePointsFarFromLocation(const Eigen::Vector3d &origin);
     std::vector<Eigen::Vector3d> Pointcloud() const;
 
-    struct VoxelBlock {
-        // buffer of points with a max limit of n_points
-        std::vector<Eigen::Vector3d> points;
-        int num_points_;
-        inline void AddPoint(const Eigen::Vector3d &point) {
-            if (points.size() < static_cast<size_t>(num_points_)) points.push_back(point);
-        }
-    };
-
     double voxel_size_;
     double max_distance_;
     int max_points_per_voxel_;
-    tsl::robin_map<kiss_icp::Voxel, VoxelHashMap::VoxelBlock> map_;
+    tsl::robin_map<Voxel, VoxelBlock, VoxelHash> map_;
 };
 }  // namespace kiss_icp
diff --git a/src/cpp/kiss_icp/metrics/Metrics.cpp b/src/cpp/kiss_icp/metrics/Metrics.cpp
index 83785c56..ce55b0a7 100644
--- a/src/cpp/kiss_icp/metrics/Metrics.cpp
+++ b/src/cpp/kiss_icp/metrics/Metrics.cpp
@@ -24,6 +24,7 @@
 
 #include <Eigen/Core>
 #include <algorithm>
+#include <cassert>
 #include <cmath>
 #include <tuple>
 #include <vector>
diff --git a/src/cpp/kiss_icp/pipeline/KissICP.cpp b/src/cpp/kiss_icp/pipeline/KissICP.cpp
index 0974cde3..99c3f3a5 100644
--- a/src/cpp/kiss_icp/pipeline/KissICP.cpp
+++ b/src/cpp/kiss_icp/pipeline/KissICP.cpp
@@ -29,6 +29,7 @@
 
 #include "kiss_icp/core/Deskew.hpp"
 #include "kiss_icp/core/Preprocessing.hpp"
+#include "kiss_icp/core/Transforms.hpp"
 #include "kiss_icp/core/VoxelHashMap.hpp"
 
 namespace kiss_icp::pipeline {
@@ -38,7 +39,15 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
     const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
         if (!config_.deskew) return frame;
         // TODO(Nacho) Add some asserts here to sanitize the timestamps
-        return compensator_.DeSkewScan(frame, timestamps, poses_);
+
+        //  If not enough poses for the estimation, do not de-skew
+        const size_t N = poses().size();
+        if (N <= 2) return frame;
+
+        // Estimate linear and angular velocities
+        const auto &start_pose = poses_[N - 2];
+        const auto &finish_pose = poses_[N - 1];
+        return compensator_.DeSkewScan(frame, timestamps, start_pose, finish_pose);
     }();
     return RegisterFrame(deskew_frame);
 }
@@ -55,18 +64,16 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
 
     // Compute initial_guess for ICP
     const auto prediction = GetPredictionModel();
-    const auto last_pose = [&]() -> Eigen::Matrix4d {
-        if (poses_.empty()) return Eigen::Matrix4d::Identity();
-        return poses_.back();
-    }();
-    const auto initial_guess = last_pose * prediction;
+    const auto last_pose = !poses_.empty() ? poses_.back() : Eigen::Isometry3d::Identity();
+    const auto initial_guess = ConcatenateIsometries(last_pose, prediction);
 
     // Run icp
-    const Eigen::Matrix4d new_pose = local_map_.RegisterPoinCloud(source,         //
-                                                                  initial_guess,  //
-                                                                  3.0 * sigma,    //
-                                                                  sigma / 3.0);
-    adaptive_threshold_.UpdateModelDeviation(initial_guess.inverse() * new_pose);
+    const Eigen::Isometry3d new_pose = local_map_.RegisterPointCloud(source,         //
+                                                                     initial_guess,  //
+                                                                     3.0 * sigma,    //
+                                                                     sigma / 3.0);
+    const auto model_deviation = ConcatenateIsometries(initial_guess.inverse(), new_pose);
+    adaptive_threshold_.UpdateModelDeviation(model_deviation);
     local_map_.Update(frame_downsample, new_pose);
     poses_.push_back(new_pose);
     return {frame, source};
@@ -86,18 +93,16 @@ double KissICP::GetAdaptiveThreshold() {
     return adaptive_threshold_.ComputeThreshold();
 }
 
-Eigen::Matrix4d KissICP::GetPredictionModel() const {
+Eigen::Isometry3d KissICP::GetPredictionModel() const {
+    Eigen::Isometry3d pred = Eigen::Isometry3d::Identity();
     const size_t N = poses_.size();
-    if (N < 2) return Eigen::Matrix4d::Identity();
-    return poses_[N - 2].inverse() * poses_[N - 1];
+    if (N < 2) return pred;
+    return ConcatenateIsometries(poses_[N - 2].inverse(), poses_[N - 1]);
 }
 
 bool KissICP::HasMoved() {
     if (poses_.empty()) return false;
-    auto ComputeMotion = [&](const Eigen::Matrix4d &T1, const Eigen::Matrix4d &T2) {
-        return ((T1.inverse() * T2).block<3, 1>(0, 3)).norm();
-    };
-    const double motion = ComputeMotion(poses_.front(), poses_.back());
+    const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
     return motion > 5.0 * config_.min_motion_th;
 }
 
diff --git a/src/cpp/kiss_icp/pipeline/KissICP.hpp b/src/cpp/kiss_icp/pipeline/KissICP.hpp
index 34606ebd..01e95903 100644
--- a/src/cpp/kiss_icp/pipeline/KissICP.hpp
+++ b/src/cpp/kiss_icp/pipeline/KissICP.hpp
@@ -20,11 +20,11 @@
 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
-
 #pragma once
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <tuple>
 #include <vector>
 
 #include "kiss_icp/core/Deskew.hpp"
@@ -72,17 +72,17 @@ class KissICP {
                                       const std::vector<double> &timestamps);
     Vector3dVectorTuple Voxelize(const std::vector<Eigen::Vector3d> &frame) const;
     double GetAdaptiveThreshold();
-    Eigen::Matrix4d GetPredictionModel() const;
+    Eigen::Isometry3d GetPredictionModel() const;
     bool HasMoved();
 
 public:
     // Extra C++ API to facilitate ROS debugging
     std::vector<Eigen::Vector3d> LocalMap() const { return local_map_.Pointcloud(); };
-    std::vector<Eigen::Matrix4d> poses() const { return poses_; };
+    std::vector<Eigen::Isometry3d> poses() const { return poses_; };
 
 private:
     // KISS-ICP pipeline modules
-    std::vector<Eigen::Matrix4d> poses_;
+    std::vector<Eigen::Isometry3d> poses_;
     KISSConfig config_;
     MotionCompensator compensator_;
     AdaptiveThreshold adaptive_threshold_;
diff --git a/src/cpp/kiss_icp_pybind/kiss_icp_pybind.cpp b/src/cpp/kiss_icp_pybind/kiss_icp_pybind.cpp
index c821890b..7d0cab7e 100644
--- a/src/cpp/kiss_icp_pybind/kiss_icp_pybind.cpp
+++ b/src/cpp/kiss_icp_pybind/kiss_icp_pybind.cpp
@@ -59,15 +59,27 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
              py::overload_cast<const VoxelHashMap::Vector3dVector &, const Eigen::Vector3d &>(
                  &VoxelHashMap::Update),
              "points"_a, "origin"_a)
-        .def("_update",
-             py::overload_cast<const VoxelHashMap::Vector3dVector &, const Eigen::Matrix4d &>(
-                 &VoxelHashMap::Update),
-             "points"_a, "pose"_a)
+        .def(
+            "_update",
+            [](VoxelHashMap &self, const VoxelHashMap::Vector3dVector &points,
+               const Eigen::Matrix4d &T) {
+                Eigen::Isometry3d pose(T);
+                self.Update(points, pose);
+            },
+            "points"_a, "pose"_a)
         .def("_point_cloud", &VoxelHashMap::Pointcloud)
         .def("_get_correspondences", &VoxelHashMap::GetCorrespondences, "points"_a,
              "max_correspondance_distance"_a)
-        .def("_register_point_cloud", &VoxelHashMap::RegisterPoinCloud, "points"_a,
-             "initial_guess"_a, "max_correspondance_distance"_a, "kernel"_a);
+        .def(
+            "_register_point_cloud",
+            [](VoxelHashMap &self, const std::vector<Eigen::Vector3d> &points,
+               const Eigen::Matrix4d &T_guess, double max_correspondence_distance, double kernel) {
+                Eigen::Isometry3d initial_guess(T_guess);
+                return self
+                    .RegisterPointCloud(points, initial_guess, max_correspondence_distance, kernel)
+                    .matrix();
+            },
+            "points"_a, "initial_guess"_a, "max_correspondance_distance"_a, "kernel"_a);
 
     // AdaptiveThreshold bindings
     py::class_<AdaptiveThreshold> adaptive_threshold(m, "_AdaptiveThreshold", "Don't use this");
@@ -75,17 +87,27 @@ PYBIND11_MODULE(kiss_icp_pybind, m) {
         .def(py::init<double, double, double>(), "initial_threshold"_a, "min_motion_th"_a,
              "max_range"_a)
         .def("_compute_threshold", &AdaptiveThreshold::ComputeThreshold)
-        .def("_update_model_deviation", &AdaptiveThreshold::UpdateModelDeviation,
-             "model_deviation"_a);
+        .def(
+            "_update_model_deviation",
+            [](AdaptiveThreshold &self, const Eigen::Matrix4d &T) {
+                Eigen::Isometry3d model_deviation(T);
+                self.UpdateModelDeviation(model_deviation);
+            },
+            "model_deviation"_a);
 
     // DeSkewScan
     py::class_<MotionCompensator> motion_compensator(m, "_MotionCompensator", "Don't use this");
     motion_compensator.def(py::init<double>(), "frame_rate"_a)
         .def(
             "_deskew_scan",
-            py::overload_cast<const std::vector<Eigen::Vector3d> &, const std::vector<double> &,
-                              const std::vector<Eigen::Matrix4d> &>(&MotionCompensator::DeSkewScan),
-            "frame"_a, "timestamps"_a, "poses"_a);
+            [](MotionCompensator &self, const std::vector<Eigen::Vector3d> &frame,
+               const std::vector<double> &timestamps, const Eigen::Matrix4d &T_start,
+               const Eigen::Matrix4d &T_finish) {
+                Eigen::Isometry3d start_pose(T_start);
+                Eigen::Isometry3d finish_pose(T_finish);
+                return self.DeSkewScan(frame, timestamps, start_pose, finish_pose);
+            },
+            "frame"_a, "timestamps"_a, "start_pose"_a, "finish_pose"_a);
 
     // prerpocessing modules
     m.def("_voxel_down_sample", &VoxelDownsample, "frame"_a, "voxel_size"_a);
diff --git a/src/cpp/kiss_icp_ros/ros1/src/OdometryServer.cpp b/src/cpp/kiss_icp_ros/ros1/src/OdometryServer.cpp
index 91c37320..257410bd 100644
--- a/src/cpp/kiss_icp_ros/ros1/src/OdometryServer.cpp
+++ b/src/cpp/kiss_icp_ros/ros1/src/OdometryServer.cpp
@@ -107,11 +107,11 @@ void OdometryServer::RegisterFrame(const sensor_msgs::PointCloud2ConstPtr &msg)
     const auto &[frame, keypoints] = odometry_.RegisterFrame(points, timestamps);
 
     // PublishPose
-    const Eigen::Matrix4d pose = odometry_.poses().back();
+    const Eigen::Isometry3d pose = odometry_.poses().back();
 
-    // Convert from Eigen::Matrix4d to ROS types
-    const Eigen::Vector3d t_current = pose.block<3, 1>(0, 3);
-    const Eigen::Quaterniond q_current(pose.block<3, 3>(0, 0));
+    // Convert from Eigen to ROS types
+    const Eigen::Vector3d t_current = pose.translation();
+    const Eigen::Quaterniond q_current(pose.rotation());
 
     // Broadcast the tf
     geometry_msgs::TransformStamped transform_msg;
diff --git a/src/python/kiss_icp/config/__init__.py b/src/python/kiss_icp/config/__init__.py
index 88382754..ee471e41 100644
--- a/src/python/kiss_icp/config/__init__.py
+++ b/src/python/kiss_icp/config/__init__.py
@@ -1 +1,24 @@
+# MIT License
+#
+# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill
+# Stachniss.
+#
+# Permission is hereby granted, free of charge, to any person obtaining a copy
+# of this software and associated documentation files (the "Software"), to deal
+# in the Software without restriction, including without limitation the rights
+# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# copies of the Software, and to permit persons to whom the Software is
+# furnished to do so, subject to the following conditions:
+#
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# SOFTWARE.
+
 from .parser import KISSConfig, load_config, write_config
diff --git a/src/python/kiss_icp/datasets/rosbag.py b/src/python/kiss_icp/datasets/rosbag.py
index efb7fbd0..2d5ffced 100644
--- a/src/python/kiss_icp/datasets/rosbag.py
+++ b/src/python/kiss_icp/datasets/rosbag.py
@@ -26,7 +26,6 @@
 import sys
 
 import numpy as np
-import yaml
 
 
 class RosbagDataset:
@@ -43,12 +42,11 @@ def __init__(self, data_dir: Path, topic: str, *_, **__):
         # bagfile
         self.bagfile = data_dir
         self.bag = rosbag.Bag(data_dir, mode="r")
-        self.topic = topic
-        self.check_for_topics()
+        self.topic = self.check_topic(topic)
 
         # Get an iterable
-        self.n_scans = self.bag.get_message_count(topic_filters=topic)
-        self.msgs = self.bag.read_messages(topics=[topic])
+        self.n_scans = self.bag.get_message_count(topic_filters=self.topic)
+        self.msgs = self.bag.read_messages(topics=[self.topic])
 
         # Visualization Options
         self.use_global_visualizer = True
@@ -74,15 +72,32 @@ def read_point_cloud(self, bagfile: Path, topic: str, idx: int):
             timestamps = timestamps / np.max(timestamps) if t_field != "time" else timestamps
         return points.astype(np.float64), timestamps
 
-    def check_for_topics(self):
-        if self.topic:
-            return
-        print("Please provide one of the following topics with the --topic flag")
-        info_dict = yaml.safe_load(self.bag._get_yaml_info())
-        info_dict.keys()
-        print("PointCloud2 topics available:")
-        for topic in info_dict["topics"]:
-            if topic["type"] == "sensor_msgs/PointCloud2":
-                for k, v in topic.items():
-                    print(k.ljust(20), v)
-        exit(1)
+    def check_topic(self, topic: str) -> str:
+        # when user specified the topic don't check
+        if topic:
+            return topic
+
+        # Extract all PointCloud2 msg topics from the bagfile
+        point_cloud_topics = [
+            topic
+            for topic in self.bag.get_type_and_topic_info().topics.items()
+            if topic[1].msg_type == "sensor_msgs/PointCloud2"
+        ]
+
+        if len(point_cloud_topics) == 1:
+            return point_cloud_topics[0][0]  # this is the string topic name, go figure out
+
+        # In any other case we consider this an error
+        if len(point_cloud_topics) == 0:
+            print("[ERROR] Your bagfile does not contain any sensor_msgs/PointCloud2 topic")
+        if len(point_cloud_topics) > 1:
+            print("Multiple sensor_msgs/PointCloud2 topics available.")
+            print("Please provide one of the following topics with the --topic flag")
+            for topic_tuple in point_cloud_topics:
+                print(50 * "-")
+                print(f"Topic   {topic_tuple[0]}")
+                print(f"\tType      {topic_tuple[1].msg_type}")
+                print(f"\tMessages  {topic_tuple[1].message_count}")
+                print(f"\tFrequency {topic_tuple[1].frequency}")
+            print(50 * "-")
+        sys.exit(1)
diff --git a/src/python/kiss_icp/deskew.py b/src/python/kiss_icp/deskew.py
index 13115700..8fbd761b 100644
--- a/src/python/kiss_icp/deskew.py
+++ b/src/python/kiss_icp/deskew.py
@@ -45,6 +45,7 @@ def deskew_scan(self, frame, poses, timestamps):
         deskew_frame = self._compensator._deskew_scan(
             frame=kiss_icp_pybind._Vector3dVector(frame),
             timestamps=timestamps,
-            poses=poses,
+            start_pose=poses[-2],
+            finish_pose=poses[-1],
         )
         return np.asarray(deskew_frame)