Build Efficient 3D lidar map #18
Description
Our current map implementatiion is not efficient for doing 3D searches. Since we have to constantly add and remove points from our map, we'd have to also rebuild a kd tree or octree for searching (e.g., for scan matching).
To speed things up, we can implement something similar to SuperOdometry where they have a hashed voxel map and each voxel contains its own search treer (octree in that case). This way, anytime you add or remove from the map, you only have to rebuild the search tree for the voxels that it changes.
To make full use of this, we'd also have to rewrite all the scan matching algorithms in beam_matching because they currently use pcl. This would take time. We could potentially implement a derived correspondence search in pcl then reimplement their matchers (icp, ndt, gicp, ...) with our new correspondence search method.