Problem in Visual-Inertial Odometry using ISAM2

Hi, Thank all the developers of the gtsam library. These days I'm using ISAM2 to implement visual-inertial odometry and meeting some puzzling problem. Specifically, I use the CombinedImuFactor and the ProjectionFactor. The pipeline is just like the IMUFactorsExample. I make a CombinedImuFactor and some ProjectionFactors once I get visual measurements and insert them to the graph immediately. Then call the isam2.update(graph, initial_value). The strange thing is that there is neither the complete result nor any warning or error notes. It's just wait. If I only comment the code of inserting the ProjectionFactors to graph, it also waits and gets worse with less result. But if I comment both of the code of defining and inserting the ProjectionFactors, meaning only using CombinedImuFactor and no ProjectionFactor indeed, It's OK! I print all Values and the entire graph, but there is no difference between the successful test and the failed test. It puzzles me! I step through the program and I find that it seems there is no relation to the graph structure since they are exactly the same as the successful test. It is the TBB parallel_for() function that incurs the strange problem. I don't know what happened to this function if I only define a ProjectionFactor without inserting it in graph. Did any one meet the same problem? Are there any more detailed instructions to use IMUFactor with ISAM2?

Comments (3)