Nacho/new core propossal

cpp/kiss_icp/core/CMakeLists.txt

@@ -21,6 +21,6 @@
 # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 # SOFTWARE.
 add_library(kiss_icp_core STATIC)
-target_sources(kiss_icp_core PRIVATE Registration.cpp Deskew.cpp VoxelHashMap.cpp Preprocessing.cpp Threshold.cpp)
+target_sources(kiss_icp_core PRIVATE Registration.cpp VoxelHashMap.cpp Voxelization.cpp Preprocessing.cpp Threshold.cpp)
 target_link_libraries(kiss_icp_core PUBLIC Eigen3::Eigen tsl::robin_map TBB::tbb Sophus::Sophus)
 set_global_target_properties(kiss_icp_core)

cpp/kiss_icp/core/Preprocessing.cpp

@@ -23,49 +23,18 @@
 #include "Preprocessing.hpp"
 
 #include <tbb/parallel_for.h>
-#include <tsl/robin_map.h>
 
 #include <Eigen/Core>
 #include <algorithm>
-#include <cmath>
 #include <sophus/se3.hpp>
 #include <vector>
 
 namespace {
-// TODO(all): Maybe try to merge these voxel uitls with VoxelHashMap implementation
-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] * 19349669 ^ vec[2] * 83492791);
-    }
-};
-
-Voxel PointToVoxel(const Eigen::Vector3d &point, double voxel_size) {
-    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
-                 static_cast<int>(std::floor(point.y() / voxel_size)),
-                 static_cast<int>(std::floor(point.z() / voxel_size)));
-}
+/// TODO(Nacho) Explain what is the very important meaning of this param
+constexpr double mid_pose_timestamp{0.5};
 }  // namespace
 
 namespace kiss_icp {
-std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             double voxel_size) {
-    tsl::robin_map<Voxel, Eigen::Vector3d, VoxelHash> grid;
-    grid.reserve(frame.size());
-    for (const auto &point : frame) {
-        const auto voxel = PointToVoxel(point, voxel_size);
-        if (grid.contains(voxel)) continue;
-        grid.insert({voxel, point});
-    }
-    std::vector<Eigen::Vector3d> frame_dowsampled;
-    frame_dowsampled.reserve(grid.size());
-    for (const auto &[voxel, point] : grid) {
-        (void)voxel;
-        frame_dowsampled.emplace_back(point);
-    }
-    return frame_dowsampled;
-}
 
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
                                         double max_range,
@@ -78,14 +47,15 @@ std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &fram
     return inliers;
 }
 
-std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d> &frame) {
-    constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0;
+std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
+                                        const std::vector<double> &timestamps,
+                                        const Sophus::SE3d &delta) {
+    const auto delta_pose = delta.log();
     std::vector<Eigen::Vector3d> corrected_frame(frame.size());
+    // TODO(All): This tbb execution is ignoring the max_n_threads config value
     tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
-        const auto &pt = frame[i];
-        const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.));
-        corrected_frame[i] =
-            Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt;
+        const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);
+        corrected_frame[i] = motion * frame[i];
     });
     return corrected_frame;
 }

cpp/kiss_icp/core/Preprocessing.hpp

@@ -24,22 +24,16 @@
 
 #include <Eigen/Core>
 #include <sophus/se3.hpp>
-#include <utility>
 #include <vector>
 
 namespace kiss_icp {
-
-/// Crop the frame with max/min ranges
 std::vector<Eigen::Vector3d> Preprocess(const std::vector<Eigen::Vector3d> &frame,
                                         double max_range,
                                         double min_range);
 
-/// This function only applies for the KITTI dataset, and should NOT be used by any other dataset,
-/// the original idea and part of the implementation is taking from CT-ICP(Although IMLS-SLAM
-/// Originally introduced the calibration factor)
-std::vector<Eigen::Vector3d> CorrectKITTIScan(const std::vector<Eigen::Vector3d> &frame);
+/// Compensate the frame by interpolating the delta pose
+std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
+                                        const std::vector<double> &timestamps,
+                                        const Sophus::SE3d &delta);
 
-/// Voxelize point cloud keeping the original coordinates
-std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &frame,
-                                             double voxel_size);
 }  // namespace kiss_icp

cpp/kiss_icp/core/Registration.cpp

@@ -36,6 +36,7 @@
 #include <tuple>
 
 #include "VoxelHashMap.hpp"
+#include "Voxelization.hpp"
 
 namespace Eigen {
 using Matrix6d = Eigen::Matrix<double, 6, 6>;
@@ -54,7 +55,7 @@ void TransformPoints(const Sophus::SE3d &T, std::vector<Eigen::Vector3d> &points
                    [&](const auto &point) { return T * point; });
 }
 
-using Voxel = kiss_icp::VoxelHashMap::Voxel;
+using Voxel = kiss_icp::Voxel;
 std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1) {
     std::vector<Voxel> voxel_neighborhood;
     for (int i = voxel.x() - adjacent_voxels; i < voxel.x() + adjacent_voxels + 1; ++i) {
@@ -70,7 +71,7 @@ std::vector<Voxel> GetAdjacentVoxels(const Voxel &voxel, int adjacent_voxels = 1
 std::tuple<Eigen::Vector3d, double> GetClosestNeighbor(const Eigen::Vector3d &point,
                                                        const kiss_icp::VoxelHashMap &voxel_map) {
     // Convert the point to voxel coordinates
-    const auto &voxel = voxel_map.PointToVoxel(point);
+    const auto &voxel = kiss_icp::PointToVoxel(point, voxel_map.voxel_size_);
     // Get nearby voxels on the map
     const auto &query_voxels = GetAdjacentVoxels(voxel);
     // Extract the points contained within the neighborhood voxels

cpp/kiss_icp/core/VoxelHashMap.cpp

@@ -28,6 +28,8 @@
 #include <utility>
 #include <vector>
 
+#include "Voxelization.hpp"
+
 namespace kiss_icp {
 
 std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &query_voxels) const {
@@ -36,9 +38,8 @@ std::vector<Eigen::Vector3d> VoxelHashMap::GetPoints(const std::vector<Voxel> &q
     std::for_each(query_voxels.cbegin(), query_voxels.cend(), [&](const auto &query) {
         auto search = map_.find(query);
         if (search != map_.end()) {
-            for (const auto &point : search->second.points) {
-                points.emplace_back(point);
-            }
+            const auto &voxel_points = search->second.points;
+            points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
         }
     });
     points.shrink_to_fit();
@@ -49,11 +50,8 @@ std::vector<Eigen::Vector3d> VoxelHashMap::Pointcloud() const {
     std::vector<Eigen::Vector3d> points;
     points.reserve(map_.size() * static_cast<size_t>(max_points_per_voxel_));
     std::for_each(map_.cbegin(), map_.cend(), [&](const auto &map_element) {
-        const auto &[voxel, voxel_block] = map_element;
-        (void)voxel;
-        for (const auto &point : voxel_block.points) {
-            points.emplace_back(point);
-        }
+        const auto &voxel_points = map_element.second.points;
+        points.insert(points.end(), voxel_points.cbegin(), voxel_points.cend());
     });
     points.shrink_to_fit();
     return points;
@@ -75,7 +73,7 @@ void VoxelHashMap::Update(const std::vector<Eigen::Vector3d> &points, const Soph
 
 void VoxelHashMap::AddPoints(const std::vector<Eigen::Vector3d> &points) {
     std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
-        auto voxel = PointToVoxel(point);
+        auto voxel = PointToVoxel(point, voxel_size_);
         auto search = map_.find(voxel);
         if (search != map_.end()) {
             auto &voxel_block = search.value();

cpp/kiss_icp/core/VoxelHashMap.hpp

@@ -32,9 +32,10 @@
 #include <sophus/se3.hpp>
 #include <vector>
 
+#include "Voxelization.hpp"
+
 namespace kiss_icp {
 struct VoxelHashMap {
-    using Voxel = Eigen::Vector3i;
     struct VoxelBlock {
         // buffer of points with a max limit of n_points
         std::vector<Eigen::Vector3d> points;
@@ -43,12 +44,6 @@ struct VoxelHashMap {
             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 (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
-        }
-    };
 
     explicit VoxelHashMap(double voxel_size, double max_distance, int max_points_per_voxel)
         : voxel_size_(voxel_size),
@@ -57,11 +52,6 @@ struct VoxelHashMap {
 
     inline void Clear() { map_.clear(); }
     inline bool Empty() const { return map_.empty(); }
-    inline Voxel PointToVoxel(const Eigen::Vector3d &point) const {
-        return Voxel(static_cast<int>(std::floor(point.x() / voxel_size_)),
-                     static_cast<int>(std::floor(point.y() / voxel_size_)),
-                     static_cast<int>(std::floor(point.z() / voxel_size_)));
-    }
     void Update(const std::vector<Eigen::Vector3d> &points, const Eigen::Vector3d &origin);
     void Update(const std::vector<Eigen::Vector3d> &points, const Sophus::SE3d &pose);
     void AddPoints(const std::vector<Eigen::Vector3d> &points);
@@ -72,6 +62,6 @@ struct VoxelHashMap {
     double voxel_size_;
     double max_distance_;
     int max_points_per_voxel_;
-    tsl::robin_map<Voxel, VoxelBlock, VoxelHash> map_;
+    tsl::robin_map<Voxel, VoxelBlock> map_;
 };
 }  // namespace kiss_icp

cpp/kiss_icp/core/Deskew.hpp → cpp/kiss_icp/core/Voxelization.cpp

@@ -20,17 +20,26 @@
 // 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 "Voxelization.hpp"
 
-#include <Eigen/Core>
-#include <sophus/se3.hpp>
-#include <vector>
+#include <tsl/robin_map.h>
 
 namespace kiss_icp {
 
-/// Compensate the frame by interpolating the delta pose
-std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
-                                        const std::vector<double> &timestamps,
-                                        const Sophus::SE3d &delta);
+std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &point_cloud,
+                                             const double voxel_size) {
+    tsl::robin_map<Voxel, Eigen::Vector3d> grid;
+    grid.reserve(point_cloud.size());
+    std::for_each(point_cloud.cbegin(), point_cloud.cend(), [&](const auto &point) {
+        const auto voxel = PointToVoxel(point, voxel_size);
+        if (!grid.contains(voxel)) grid.insert({voxel, point});
+    });
+    std::vector<Eigen::Vector3d> frame_dowsampled;
+    frame_dowsampled.reserve(grid.size());
+    std::for_each(grid.cbegin(), grid.cend(), [&](const auto &voxel_and_point) {
+        frame_dowsampled.emplace_back(voxel_and_point.second);
+    });
+    return frame_dowsampled;
+}
 
 }  // namespace kiss_icp

cpp/kiss_icp/core/Deskew.cpp → cpp/kiss_icp/core/Voxelization.hpp

@@ -20,30 +20,30 @@
 // 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.
-#include "Deskew.hpp"
-
-#include <tbb/parallel_for.h>
+//
+#pragma once
 
 #include <Eigen/Core>
-#include <sophus/se3.hpp>
+#include <cmath>
 #include <vector>
 
-namespace {
-/// TODO(Nacho) Explain what is the very important meaning of this param
-constexpr double mid_pose_timestamp{0.5};
-}  // namespace
-
 namespace kiss_icp {
-std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
-                                        const std::vector<double> &timestamps,
-                                        const Sophus::SE3d &delta) {
-    const auto delta_pose = delta.log();
-    std::vector<Eigen::Vector3d> corrected_frame(frame.size());
-    // TODO(All): This tbb execution is ignoring the max_n_threads config value
-    tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
-        const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);
-        corrected_frame[i] = motion * frame[i];
-    });
-    return corrected_frame;
+using Voxel = Eigen::Vector3i;
+inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) {
+    return Voxel(static_cast<int>(std::floor(point.x() / voxel_size)),
+                 static_cast<int>(std::floor(point.y() / voxel_size)),
+                 static_cast<int>(std::floor(point.z() / voxel_size)));
 }
+/// Voxelize a point cloud keeping the original coordinates
+std::vector<Eigen::Vector3d> VoxelDownsample(const std::vector<Eigen::Vector3d> &point_cloud,
+                                             const double voxel_size);
+
 }  // namespace kiss_icp
+
+template <>
+struct std::hash<kiss_icp::Voxel> {
+    std::size_t operator()(const kiss_icp::Voxel &voxel) const noexcept {
+        const uint32_t *vec = reinterpret_cast<const uint32_t *>(voxel.data());
+        return (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791);
+    }
+};

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -26,7 +26,6 @@
 #include <Eigen/Core>
 #include <vector>
 
-#include "kiss_icp/core/Deskew.hpp"
 #include "kiss_icp/core/Preprocessing.hpp"
 #include "kiss_icp/core/Registration.hpp"
 #include "kiss_icp/core/VoxelHashMap.hpp"

python/kiss_icp/deskew.py → python/kiss_icp/core/__init__.py

@@ -20,26 +20,8 @@
 # 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.
-import numpy as np
-
-from kiss_icp.config import KISSConfig
-from kiss_icp.pybind import kiss_icp_pybind
-
-
-def get_motion_compensator(config: KISSConfig):
-    return MotionCompensator() if config.data.deskew else StubCompensator()
-
-
-class StubCompensator:
-    def deskew_scan(self, frame, timestamps, delta):
-        return frame
-
-
-class MotionCompensator:
-    def deskew_scan(self, frame, timestamps, delta):
-        deskew_frame = kiss_icp_pybind._deskew_scan(
-            frame=kiss_icp_pybind._Vector3dVector(frame),
-            timestamps=timestamps,
-            delta=delta,
-        )
-        return np.asarray(deskew_frame)
+from .preprocess import *
+from .registration import *
+from .threshold import *
+from .voxel_hash_map import *
+from .voxelization import *

python/kiss_icp/preprocess.py → python/kiss_icp/core/preprocess.py

@@ -30,6 +30,10 @@ def get_preprocessor(config: KISSConfig):
     return Preprocessor(config)
 
 
+def get_motion_compensator(config: KISSConfig):
+    return MotionCompensator() if config.data.deskew else StubCompensator()
+
+
 class Preprocessor:
     def __init__(self, config: KISSConfig):
         self.config = config
@@ -42,3 +46,18 @@ def __call__(self, frame: np.ndarray):
                 self.config.data.min_range,
             )
         )
+
+
+class StubCompensator:
+    def deskew_scan(self, frame, timestamps, delta):
+        return frame
+
+
+class MotionCompensator:
+    def deskew_scan(self, frame, timestamps, delta):
+        deskew_frame = kiss_icp_pybind._deskew_scan(
+            frame=kiss_icp_pybind._Vector3dVector(frame),
+            timestamps=timestamps,
+            delta=delta,
+        )
+        return np.asarray(deskew_frame)

python/kiss_icp/registration.py → python/kiss_icp/core/registration.py

@@ -23,9 +23,10 @@
 import numpy as np
 
 from kiss_icp.config.parser import KISSConfig
-from kiss_icp.mapping import VoxelHashMap
 from kiss_icp.pybind import kiss_icp_pybind
 
+from .voxel_hash_map import VoxelHashMap
+
 
 def get_registration(config: KISSConfig):
     return Registration(