Abstract: Stereo matching algorithm based on dynamic programming has the excellence of fast searching speed. But there are strip problem with it. In this paper, a method of similarity measure combined absolute difference(AD) with census transform algorithm was proposed, where a new linear smoothing term and dynamically changing search radius method were adopted. Speediness and accuracy of the matching method were improved. To avoid the occlusion phenomenon in binocular vision, a left-right check occlusion detection algorithm was used to detect occlusion region and a disparity optimization interpolation algorithm based on color and distance similarity was designed to improve the perform of disparity estimate. Experimental results show that the stereo matching algorithm can quickly and accurately estimate the disparity, and meet the needs of the three-dimensional reconstruction.