A method with sub-pixel accuracy is proposed to extract feature points, and P??P and OI algorithms are combined to measure the pose of a ping-pong robot body. According to the approximate Gaussian distribution of the dispersion spot of camera imaging, the edges of the color mark are obtained in sub-pixel accuracy. Their intersections form high accurate corners, which are considered as feature points. The estimated pose value obtained by the P??P-based algorithm is taken as the initial value. The OI algorithm is employed to optimize the pose in order to ensure the orthogonality of the orientation matrix. Experimental results show that the proposed method can measure the pose of robot body quickly and accurately.