Terrain Mapping and Obstacle Detection based on Dense Disparity Map