An optimization approach for 3D environment mapping using normal vector uncertainty

In this paper a novel approach for 3D environment mapping using registered robot poses is presented. The proposed algorithm focuses on improving the quality of robot generated 3D maps by incorporating the uncertainty of 3D points and propagating it into the normal vectors of surfaces. The uncertainty of normal vectors is an indicator of the quality of the detected surface. A controlled random search algorithm is applied to optimize a non-convex function of uncertain normal vectors and number of clusters in order to find the optimal threshold parameter for the segmentation process. This approach leads to an improved cluster coherence and thus better maps.