Finally 5point algo working quite well