Implemented an in-place method for transforming DataPoints objects

[YoshuaNava]

I noticed that some computations relying on libpointmatcher were losing time transforming point clouds between sensor and map frames.

I looked into the libpointmatcher Transformations' code, and, following the same pattern as the DataPointsFilter, I implemented an inPlace processing method for computing transformations.

This should reduce the number of copies by two: one for copying the input in the compute() function, and the other one for copying the return value (note it could also be moved).

[YoshuaNava]

I compared multiple implementations:

1. In place multiply:

cloud.features = parameters * cloud.features;

2. Apply on the left:

cloud.applyOnTheLeft(parameters);

3. Transpose multiply:

cloud.features.transpose() *= parameters.transpose();

In terms of timing, all of them are in sub-millisecond, for all the point clouds I tried (depth camera, LIDAR, SLAM).

Transpose seems to be the slowest method, while in place multiply and apply on the left tend to be the fastests, although there doesn't seem to be a single winner there. (NOTE: after testing again I have found that my testing setup made inefficient use of Eigen operations, transpose was actually the fastest when properly implemented)

For the final implementation I think I'll go with applyOnTheLeft, because the signature of the function is very idiomatic, and the performance improves.

---

When it comes to the old vs. new methods, the old method is 1.5-10x slower than the new ones, just because of the copy.

[pomerlef]

Since you are digging into this. You can optimize the inverse of a transformation matrix using:

In Python, skipping the inverse lead to some improvements:

With inverse
10.8 µs ± 42.3 ns per loop (mean ± std. dev. of 7 runs, 100000 loops each)

Without inverse
9.42 µs ± 40 ns per loop (mean ± std. dev. of 7 runs, 100000 loops each)
Improvement of 12.27%

[YoshuaNava]

@pomerlef Thank you for the input! Do you have any specific place in mind for this change?

After running a quick search I found the following occurrences:

They're not so many, but only the ones on ICP.cpp seem related to transformations?

[pomerlef]

I didn't check in the code what would be the impact (apparently minor). I just came across this formulation for rigid transformation inverse recently. It was mostly to log it somewhere.

[YoshuaNava]

@pomerlef I extended the unit tests, directly validating the result of transforming DataPoints objects. I also noticed that the similarity transform was not registered, neither had its own constructor, so I added it.

I quickly ran a benchmark of the unit tests on Intel VTune, with the HW sampler, obtaining the following results:

Before:

After:

There is a ~10% improvement in running time and a small bump in microarchitecture usage (could be because we do less copies, so less time moving data in memory)

[pomerlef]

Looking good! An improvement of 10 % is quite good. Thanks for this PR @YoshuaNava.

[YoshuaNava]

@pomerlef Thank you.

Where I saw the benefit of the change the most is when running localization and mapping with parameters for very dense map construction. There the time spent transforming point clouds went down from 7 to around 4%.

For live operation, with point clouds and maps downsampled to have 5 to ~10cm density, the overall improvement was from around 5 to 4%.

---

From all the time spent by the transformation functions, 30% is transforming the features https://github.com/YoshuaNava/libpointmatcher/blob/feature/transformations_inplace_compute/pointmatcher/TransformationsImpl.cpp#L191
and the rest is transforming the descriptors: https://github.com/YoshuaNava/libpointmatcher/blob/feature/transformations_inplace_compute/pointmatcher/TransformationsImpl.cpp#L202

I tried to come up with further optimizations, but couldn't find something faster than Eigen block operations.

For future reference, this is the call stack of the inplacecompute function of rigidtransformation:

[YoshuaNava]

Update: I did find a way to optimize the descriptor assignment further. See snippet here: https://godbolt.org/z/9W547n

Another benchmark for the different methods evaluated before introducing this PR: https://godbolt.org/z/r88h6G

I'll submit another PR tomorrow.