VICP: Velocity updating iterative closest point algorithm

We propose a novel method to enhance a family of ICP(iterative closest point) algorithms by updating velocity. Even though ICP algorithms play a dominant role in a model based tracking, it is difficult to avoid an accumulated tracking error during a continuous motion. It is because that typical ICP algorithms assumes that each of the point in one scan are measured simultaneously while most of the available rangefinders measure each point sequentially. Hence conventional ICP algorithms are prone to be erroneous under a fast motion and an accumulated error during the motion cannot be ignored in many cases. In our approach, we estimate a velocity of a rangefinder numerically over ICP iterations. As a result, distortion of a scan due to the motion can be compensated using estimated velocity. In addition, outliers are effectively rejected during the iteration of velocity update, which means that more accurate and robust motion is trackable. Also we verify a performance and an accuracy of our method by demonstrating simulation and real-world experiment results.