Fix optimization with ISAM2 using IMU+RTSLAM as initialization

We do not use landmarks yet. The RTSLAM data is used to initialize the values
and as a "fake GPS". At least it does not crash complaining about a
ill-posed blablabla thing