Structural ICP algorithm for pose estimation based on local features