diff --git a/cpp/kiss_icp/core/Preprocessing.cpp b/cpp/kiss_icp/core/Preprocessing.cpp index 0e74910b..7587dc4b 100644 --- a/cpp/kiss_icp/core/Preprocessing.cpp +++ b/cpp/kiss_icp/core/Preprocessing.cpp @@ -22,13 +22,11 @@ // SOFTWARE. #include "Preprocessing.hpp" -#include #include #include #include #include -#include #include #include "VoxelUtils.hpp" @@ -63,15 +61,4 @@ std::vector Preprocess(const std::vector &fram return inliers; } -std::vector CorrectKITTIScan(const std::vector &frame) { - constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; - std::vector corrected_frame(frame.size()); - 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; - }); - return corrected_frame; -} } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Preprocessing.hpp b/cpp/kiss_icp/core/Preprocessing.hpp index 1a8ae1ee..004abd6c 100644 --- a/cpp/kiss_icp/core/Preprocessing.hpp +++ b/cpp/kiss_icp/core/Preprocessing.hpp @@ -32,11 +32,6 @@ std::vector Preprocess(const std::vector &fram 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 CorrectKITTIScan(const std::vector &frame); - /// Voxelize point cloud keeping the original coordinates std::vector VoxelDownsample(const std::vector &frame, double voxel_size); diff --git a/python/kiss_icp/pybind/kiss_icp_pybind.cpp b/python/kiss_icp/pybind/kiss_icp_pybind.cpp index f3dc6d4d..2291c04f 100644 --- a/python/kiss_icp/pybind/kiss_icp_pybind.cpp +++ b/python/kiss_icp/pybind/kiss_icp_pybind.cpp @@ -27,6 +27,8 @@ #include #include +#include +#include #include #include @@ -118,7 +120,21 @@ PYBIND11_MODULE(kiss_icp_pybind, m) { // prerpocessing modules m.def("_voxel_down_sample", &VoxelDownsample, "frame"_a, "voxel_size"_a); m.def("_preprocess", &Preprocess, "frame"_a, "max_range"_a, "min_range"_a); - m.def("_correct_kitti_scan", &CorrectKITTIScan, "frame"_a); + /// 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) + m.def( + "_correct_kitti_scan", + [](const std::vector &frame) { + constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0; + std::vector frame_ = frame; + std::transform(frame_.cbegin(), frame_.cend(), frame_.begin(), [&](const auto pt) { + const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.)); + return Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt; + }); + return frame_; + }, + "frame"_a); // Metrics m.def("_kitti_seq_error", &metrics::SeqError, "gt_poses"_a, "results_poses"_a);