One thing I noticed when working on the build script was the use of a considerable amount of Lin. Alg. code. That makes a lot of sense, given your input data and your goal of trying to map out a mesh of the environment. That kind of workload is seen often enough that there are two routes which I think offer a few key advantages. These are namely fewer LOC, faster computation, and lower power consumption. The two options are, broadly speaking LAPACK or the Vienna Suite. Feel free to close this out when you've had a look.
One thing I noticed when working on the build script was the use of a considerable amount of Lin. Alg. code. That makes a lot of sense, given your input data and your goal of trying to map out a mesh of the environment. That kind of workload is seen often enough that there are two routes which I think offer a few key advantages. These are namely fewer LOC, faster computation, and lower power consumption. The two options are, broadly speaking LAPACK or the Vienna Suite. Feel free to close this out when you've had a look.