WIP: Adding ability to add points to flann index and writing test to ensure its correct