3D map building is a complex robotics task which needs mathematical robust models. From a 3D point cloud, we can use the normal vectors to these points to do feature extraction. In this paper, we propose to extract geometric primitives (e.g. planes) from the cloud. We will present a robust method for normal estimation.