Deleted the wrong length calculation. The algoritm work correctly on first four frames.
After that image  point are searched incorrectly.