High precision camera calibration is the premise of establishing 3D surface measurement system with excellent performance. Therefore, it is necessary to conduct systematic research on camera calibration method to improve measurement efficiency and accuracy. The original Tsai Two-Step method had two main disadvantages: firstly, different points on different planes should be adopted for camera calibration; Secondly, only radial distortion of the lens is considered. Many scholars optimized the method, but most of them optimized one of its shortcomings. In this paper, comprehensive optimization was considered to improve the two shortcomings together. At the same time, Taylor series expansion method is used to process the data and improve the calibration speed and accuracy. The improved method is proved to be feasible by comparing experiments with the Zhang's calibration method.