Refactor KissICP pipeline to mantain just 1 pose instead of full trajectory. Fixes #122

[nachovizzo]

The idea of this PR is to provide a solution to #122 that also improves the whole KISS-ICP implementation

General Idea

I think I'm happy in general with moving away to maintaining an infinite list of poses just to always use the current one (current_pose_) and the difference to the last (current_delta_). So I think the system is benefiting from this change as we don't add extra stuff just because. It models better the problem we are trying to solve and expresses more clearly what we need to keep track of to solve that problem with our approach

What we changed

• Removed the std::vector<Sophus::SE3d> poses_ member from the C++ pipeline.
• Added a current_pose_ (name to be defined) and current_delta_ (as this is the only variable we were generating from the poses_[N-2]
• Extended the deskew function to account for being called with the delta estimation instead of the start/end poses. (this was not overloaded in the Python module) done in a8c39f6
• Simplified the adaptive threshold, and removed a bunch of ugly HasMoved, etc utility functions
• Try to merge more conceptually the Python and C++ pipelines
• Added some minor comments into the main loops to improve readability for new users

Important

The Python pipeline will be now the only system that contains the full trajectory. As the C++ one is mainly consumed in ROS it's trivial to create a trajectory by just subscribing to the kiss/odometry topic in general

Update: a8c39f6 shows how this trajectory should be handled in the "application" and not in the KISS ICP mehotd itself, making the kiss_icp.py even more similar to the C++ evil twin

To do

• Tests this in a bunch of datasets
• Analyze the there is a real impact with the new approach for the adaptive threshold (97% sure that there is no real change)
• Define what to do with the Deskew API (1 function with just the delta as input, 2 functions, pybinds)
• DEFINE NEW NAMES. delta Is a bit generic, current_pose is the current, the last, which pose? I put pose since the moment you ask Kiss for this data member will give you the current estimate it has. Should delta be renamed to prediction ? I dunno

[nachovizzo]

@tizianoGuadagnino @benemer this is ready for a real round of reviews.

I modified the PR description, title, etc so please re-read.

[benemer]

Regarding the naming, I would vote for last_pose_; this also avoids confusion with the new_pose. Regarding the current_delta_, how about connecting it directly to the velocity? In this way, it becomes clear that de-skewing is integrating the velocity and that the initial guess is based on the constant velocity model.

[benemer]

Regarding Deskewing, I think we can just keep one with the delta/velocity and compute it on the Python side in the MotionCompensator when calling deskew_scan

[tizianoGuadagnino]

Regarding the naming, I would vote for last_pose_; this also avoids confusion with the new_pose. Regarding the current_delta_, how about connecting it directly to the velocity? In this way, it becomes clear that de-skewing is integrating the velocity and that the initial guess is based on the constant velocity model.

Executed now what was current became last, and I also feel now the flow it is more clear

[tizianoGuadagnino]

Regarding Deskewing, I think we can just keep one with the delta/velocity and compute it on the Python side in the MotionCompensator when calling deskew_scan

Didn't fully get what you mean here @benemer

[benemer]

Regarding Deskewing, I think we can just keep one with the delta/velocity and compute it on the Python side in the MotionCompensator when calling deskew_scan

Didn't fully get what you mean here @benemer

I was referring to this Todo list's point:

Define what to do with the Deskew API (1 function with just the delta as input, 2 functions, pybinds)

I now removed the deskewing that takes start and end pose because it was not used anyway.

[benemer]

Given that the performance is not affected (Tiziano and I will check later), I am now happy with the changes!

[nachovizzo]

While I review this I'll split the diffs in different PRs to keep a cleaner changelog and for future criminology purposes :) great work guys thanks!

[tizianoGuadagnino]

@nachovizzo I think the log is quite clear, we wanted to use a single pose rather then a vector -> Consequence we need to change the AdaptiveThreshold (more specifically we need to handle the initialization phase differently) -> Consequence, we need to evaluate the change in performance -> Consequence we review the metrics.

Any other change is collateral and minor, from my point of view. I would rather add more comments and discussions here than have you do different PR now, which is more time consumed for you and probably more confusion for me and Ben later on.

Lets make a more detailed log here and move forward.

[tizianoGuadagnino]

FYI we will give the final answer about the new AdaptiveThreshold performances this morning, but of course testing just on public datasets might not be enough.

[tizianoGuadagnino]

PS: we discuss with @benemer and ATE change goes in fact in a different PR because we are still in the process of getting it right. Ben is working on splitting this, after that we can merge

[benemer]

New PR with the ATE change is now at #324

I reverted it here 👍

[tizianoGuadagnino]

Confirmed that the results do not change significantly in any of the datasets we tested with @benemer. We can proceed to merge this.