Complete code for the Loop closure approach