Generally fix code quality + improve ICP modularity

[ethanuppal]

• You need to fix your code quality, which might involve editing sdl-wrapper
• You also need to make ICP more modular; instead of taking in fixed point sets, for example, you allow dynamic setting of point sets (but the $n$ stays the same)
• You should try to see if quad trees let you speed up performance
  • Lidar range is bounded, so it works maybe?

[ethanuppal]

https://en.wikipedia.org/wiki/Quadtree#Point-region_(PR)_quadtree

[ethanuppal]

• https://eng.libretexts.org/Bookshelves/Mechanical_Engineering/Introduction_to_Autonomous_Robots_(Correll)/12%3A__RGB-D_SLAM/12.02%3A_The_Iterative_Closest_Point_(ICP)_Algorithm
• https://ieeexplore.ieee.org/document/4543181
• https://arxiv.org/pdf/2007.07627.pdf

[ethanuppal]

Figure out how to maybe factor out into loss $L$ and $\frac{\partial L}{\partial t_x}$ functions and the like so that the iterate itself can be automated. This would lead to reduced code duplication and improved modality.

Also try to see if the dist and pair arrays should be local to just point-to-point etc. instead of baked into ICP,

[ethanuppal]

Completed relevant stuff, others like quad trees should be own issues/PRs after benchmarking is implemented