Added KNNSearch in SkeletonFitting, step when trying to move goal decreased.