We are now able to demonstrate that the performances of EM-based SLAM algorithms are comparable to that of optimization-based. However, it is not easy to observe that same thing in the real-world dataset.
simulation result
The first challenge is whether to use IMU data or to only use the output from the visual-inertial odometry. Furthermore, if IMU data are used, how to use them is another concern. In general, IMU data over a short period of time works fine, and I think it is easier to directly use IMU data over the consecutive keyframes.
While EM SLAM contains E-step and M-step, there is not much room for playing around M-step. Therefore, we have to focus on the E-step. By carefully analyzing the components of EM SLAM, there are 3 steps in the E-step:
- forward time update
- forward observation update
- backward RTS smoothing
In order to achieve trajectory correction in loop-closure, forward observation update will bring the two ends of trajectory closer, and the backward RTS smoothing will make sure the the trajectory is constrained by the IMU data. It requires careful design to find the balance between two effects. In order the debug, I will carefully check the effectiveness of each steps.
Currently, the end of the trajectory is not brought back to the origin. Also, the trajectory is not smooth enough. But these are two clear goals that can guide my debugging process.