Previously, I finished the feature matching and id assignment in the SLAM frontend. Last week, I combine the result with the preintegrated IMU data to formulate an optimization problem.
Initial values
I initialize the landmark position by triangulate two frames. The selection of these two points has great impact on the optimization performance. The following are two optimization results with different landmark initial values.
Loop closure
The ideal optimized trajectory will have overlapped start point and end point. Currently, with only IMU and image constraints, I can not get such result. Preassigning the end position with respect to the start position seems essential to enable such loop closure.