Tiziano/fix rotation and voxel (#70)

* Treat optimized rotation as so3 rather than RPY

* Manual merge from internal Gitlab dev branch

---------

Co-authored-by: Matt Alvarado <[email protected]>
Co-authored-by: tizianoGuadagnino <[email protected]>

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

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()

src/cpp/kiss_icp/core/Deskew.cpp

@@ -30,61 +30,44 @@
 #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;
     std::for_each(timestamps_.begin(), timestamps_.end(), [&](double &timestamp) {
         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

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_;

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});
     }

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};
 }
 

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

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

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

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