Estimating 6 DOF pose transformation of a TOF laser camera

Relative pose estimation is a hot research topic in the community of robotic vision. 6 DOF pose transformation was estimated by two frames data. Several effective algorithms were proposed to guarantee the precision of the estimation which made full used of TOF camera. Iterative Closest Point(ICP) algorithm was used to estimate the pose transformation, in order to conquer the divergence problem of ICP, scaled Invariant Feature Transform(SIFT) feature pairs were employed to compute the initial value for ICP. The contrast of the image was increased for extracting the effective features by scaling the original gray image according to principle of statistics. Multiple frames were fused to improve the accracy of depth measurement based on the fact that the longer the exposure time was, the higher the accuracy was, and every pixels in the fused frame were captured with the longest valid exposure time. A methed for measuring the difference of two 6 DOF pose transformations was proposed, which was applied to track the iterations of ICP. The experiments have demonstrated the effectiveness of the algorithms proposed in this paper.