Dense 3-D Mapping with Spatial Correlation Via Gaussian Filtering