- Lidar ICP residuals were added twice to the ceres problem, resulting in much slower optimization.
- Set the max number of threads to use for parallel omp for loops to the number of keypoints types instead of a hard-coded "3 threads".
- Do not build blob KDTree during ego motion registration step
- Remove unsused PoseArray member in KeypointsMatcher
- Reset all keypoints clouds even if unused. This is necessary otherwise old blob cloud is still displayed even if blobs are disabled.