Mapping Offline needs a rethink

[Huguet57]

After the big bug resolved, the compensator still has the old structure made for surpassing it (with the "global" variable), it is overly complex for what it does and now it has two known bugs:

• It doesn't compensate all points it's given. Seems like the ones after last IMU are ignored or similar. Easy to see when lowering delta to 1/200 or lower.
• Accumulating the pointcloud it's supposed to map when mapping online and then mapping the accumulated result offline works way better than mapping offline. It's not using the known states well to compensate.

Apart from the bugs and the over-complexity, it's the main reason that PointCloud types are around the code leading to unnecessary and time-consuming conversions.

So a general rewrite is needed. Objectives:

• Fixing the "compensating fully" bug
• Fixing the "offline mapping" bug
• Reducing complexity of the code
• Replacing PointCloud for Points, its canonic representation.

[Huguet57]

⚠️ Encountered a structural issue with the "mapping offline".

Motion compensation works by putting local points between t1 and t2 in an estimated (by IMUs) frame \hat{X}_t2^-1. Then, we find iteratively the exact X_t2 such that transporting our points from the \hat{X}_t2^-1 frame to global (applying X_t2) fit the map.

That means, generally \hat{X}_t2^-1!= X_t2^-1, and X_t2 is the "right one" for this exact estimation. If we later replace \hat{X}_t2^-1 for X_t2^-1, applying X_t2 will not exactly fit the map since it's not in the \hat{X}_t2^-1 frame.

A solution would be to not only update X_t2 but also \hat{X}_t2 so if they would converge, they would be the same and we could not only apply X_t2 to fit the map exactly, but in turn have a better motion compensation since \hat{X}_t2 ~= X_t2.

A first try has been done by moving the compensate() function inside the h function so at every iteration it would recompensate to the updated X_t2^-1 frame. Additionally, compensating by interpolating between two states helped with the IMUs seems to improve quite a lot but it's not at all a perfect solution.

Another solution for real_time=false only has also been coded. It basically accumulates what it would map instead of mapping it directly. With real_time=true, skipping parts would cause to not have those parts on the full-rotation cloud. A fix for this could be to map the not-skipped parts and fill the skipped parts with estimates when passing it to the dynamic object remover.