TOF camera point cloud registration algorithm based on image fusion

Aiming at the problem that the ICP (Iterative Closest Point) algorithm tends to fall into the local minimum, a TOF(Time-Of-Flight) camera point cloud registration algorithm based on image fusion is proposed. Taking the advantages of the ability that the TOF camera can obtain depth and grayscale images at the same time, the proposed algorithm performs edge detection on the grayscale and eliminates the edge noise in the corresponding depth image. Then fitting the centroid of the edge to decentralize the query point. Finally, an improved K-Dtree (K-Dimensional tree) algorithm to accelerate the NNS (Nearest Neighbour Search) process between the query points and the model points is proposed, while the 3-Sigma Criteria is adopted to compress the data volume of a single iteration. The simulation results show that compared to traditional ICP algorithm and state-of-the-art methods, the proposed algorithm successfully avoids the point cloud registration process from falling into the local minimum. At the same time, the accurate initial poses are provided and the amount of data is compressed, improving the execution efficiency comprehensively.