Reduce repeated computation in lidar odometry

[bloom256]

This PR improves performance in the lidar odometry step by reducing repeated computation inside the optimization loop.

Changes include:

• Precomputing Tait–Bryan pose terms for the intermediate trajectory.
• Using AtPA/AtPB function variants that accept precomputed sin/cos values.
• Minor internal refactoring to make Hessian computation code paths explicit.
• Exposing convergence_delta_threshold in the lidar_odometry_control config.
• Adding a TODO note for a potential follow-up optimization.

Performance impact:

• Approximately 20% speedup observed in compute_step_2 (lidar_odometry_utils_optimizers.cpp).

Notes:

• The change is intended to be behavior-preserving; mathematical results should remain unchanged.
• The new configuration parameter defaults to the previous behavior.

[yuckinus]

I might be wrong but this feels like an AI driven change. Was the change tested against a reference to make sure mathematical model is not altered and end odometry result is the same? Also did similar changes before and always had to make small changes in order to keep AI proposals relevant and accurate

[bloom256]

I might be wrong but this feels like an AI driven change. Was the change tested against a reference to make sure mathematical model is not altered and end odometry result is the same? Also did similar changes before and always had to make small changes in order to keep AI proposals relevant and accurate

The mathematical model was not changed. All equations and Jacobians are identical; the change only avoids recomputing the same sin/cos and pose terms per point by precomputing and reusing them per iteration. This was driven by profiling of compute_step_2. AI was used only for routine operations. On a reference dataset, the resulting trajectory appears identical to me; if there is an established automated way to verify equivalence (beyond visual comparison), I would be glad to use it. There will be a follow-up PR (possibly more than one), as profiling shows additional low-risk opportunities for further quick improvements in odometry performance.

[yuckinus]

Great news then, happy that more people are working on all these improvement opportunities which I also saw but don't have the time to address. Keep up the good work! For automated testing you have to ask Janusz, I pretty sure it's possible (either through python or command line execution) but haven't tried it, did manual tests like you