Tune slam output velocity

[Cristian-wp]

Hello, I am able to run the slam and it works really good, but especially at the beginning of the motion I am facing a possible computation time problem. I am using an Ouster OS0-64, and for what concern the motion on Y an Z I did not notice any problem, but when I walk with my sensor for 10m X remain around the initial value until I reach ~7m. After the output value start to change really fast and reach the correct value (and mantain it for almost all the test). To be more clear:

Real distance(m) | 0 | 0.5 | 1.0  | 1.5  | 2.0     | 2.5  | 3.0  | 3.5 | 4.0    | 4.5   | 5.0   | 5.5 | 6.0   | 6.5  | 7.0 | 8.0 | 9.0 | 10.0 | 10.0 | 10.0 | 
--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
/ct_icp/pose(m)  | 0 | 0.1 | -0.2 | 0.015 | -0.03 | 0.3 | 0.40 | -0.2 | 0.10   | 0.15 | 0.16 |  0.2 | 0.10   | 0.15   | 1.0| 3.0 | 5.0 | 7.0 | 9.0 | 10.0  | 

Now my PC CPUs are not in saturation, and them speed is 4.8Ghz so I do not think is an hardware problem, the ram occupation is also low (7Gb/16Gb)

There is a portion of code to modify in order to speed up the computation of the output pose? I have try to change some parameters in ct_icp/include/ct_icp/odometry.h but nothing has changed.