[fix] Fix loop indices in loop closure

Goal

This MR is to synchronize MR !376 (merged) for ros2 branch

Fix the "[ERROR] Error: could not cast the vertex" error.

Changes

First bug was a weird check on a vertex in Loop closure that is removed

Second bug was due to the fact that the loop indices were badly input to the graph optimizer

Results

The loop closure is well set