Visual method including binocular stereo vision method and monocular vision method of the relative position and pose measurement for space target has become relatively mature, and many researchers focus on the method based on three-dimension measurement recently. ICP alignment, which is the key of three-dimension pattern measurement method, has the problem of low efficiency in large data sets. Considering this problem, an improved ICP algorithm is proposed in this paper. The improved ICP algorithm is the combination of the original ICP algorithm and KD-TREE. The experimental comparison between the improved ICP algorithm and the traditional ICP algorithm in efficiency has been given in this paper, which shows that the improved ICP algorithm can get much better performance.