vins fusion optimization nan

[BeatScherrer]

I am trying to run vins fusion with my data from gazebo but sometimes receive the following errors (see log file) vins_fusion_log.txt

I am using the following configuration: vins_fusion_sim.txt

The error does not always occur but if it does not occur loop closure does not work. Funnily enough if I run a dataset with real data the optimization and also loop closure is working properly, it seems to be dependent on IMU noise parameters...

Towards the loop closure I only managed to get the following information which might be linked to the issue: when enabling the ros debug level the following extract indicates that there might be an issue with the /clock queue, since I'm running with simulated time.

Node: /loop_closure
Time: 00:10:02.501000000 (1970-01-01)
Severity: Debug
Published Topics: /loop_closure/base_path, /loop_closure/camera_pose_visual, /loop_closure/margin_cloud_loop_rect, /loop_closure/match_image, /loop_closure/odometry_rect, /loop_closure/path_1, /loop_closure/path_2, /loop_closure/path_3, /loop_closure/path_4, /loop_closure/path_5, /loop_closure/path_6, /loop_closure/path_7, /loop_closure/path_8, /loop_closure/path_9, /loop_closure/point_cloud_loop_rect, /loop_closure/pose_graph, /loop_closure/pose_graph_path, /rosout

Incoming queue was full for topic "/clock". Discarded oldest message (current queue size [0])

Location:
/tmp/binarydeb/ros-melodic-roscpp-1.14.5/src/libros/subscription_queue.cpp:SubscriptionQueue::push:67

----------------------------------------------------------------------------------------------------

EDIT: I think the loop closure issue was due to the simulation environment when looking at the matched features image the features correspondencies are quite messed up. I created a blender feature with a high texture inteded to improve the feature tracking and loop closing performance but it somehow backfired...

I changed my simulation scene and loop closures are detected properly.

I took a dataset from the simulation and the optimization error still persists and fails at the same spot when running multiple times.

[Pallav1299]

@BeatScherrer, I am facing the same issue. Any updates?

[BeatScherrer]

@BeatScherrer, I am facing the same issue. Any updates?

IIRC the issue was feature correspondences as illustrated above. Try to visualize the matched features. This error is most likely due to something going terribly wrong due to these correspondences manifesting in NaNs

[Pallav1299]

IIRC the issue was feature correspondences as illustrated above. Try to visualize the matched features. This error is most likely due to something going terribly wrong due to these correspondences manifesting in NaNs

I am not getting stable results in gazebo simulation, with the optimization error log popping up. On the other hand, it's working pretty well in real world. Any ideas, why is this happening? Have you explored this?

[BeatScherrer]

IIRC the issue was feature correspondences as illustrated above. Try to visualize the matched features. This error is most likely due to something going terribly wrong due to these correspondences manifesting in NaNs

I am not getting stable results in gazebo simulation, with the optimization error log popping up. On the other hand, it's working pretty well in real world. Any ideas, why is this happening? Have you explored this?

Did you make sure your feature correspondences are reasonable? I explained why I received this error in the edit and therefore I would check if your environment is appropriate for the algorithms used. Feature matching can be difficult in simulations whereas it's usually working very well in the real world.

[Pallav1299]

Did you make sure your feature correspondences are reasonable? I explained why I received this error in the edit and therefore I would check if your environment is appropriate for the algorithms used. Feature matching can be difficult in simulations whereas it's usually working very well in the real world.

@BeatScherrer, Here is the world that I used in gazebo, in case you want to have a look. Please let me know your views about the same.

[BeatScherrer]

@BeatScherrer, Here is the world that I used in gazebo, in case you want to have a look. Please let me know your views about the same.

I don't have anything set up to be able to test this and honestly don't want to take the time. If it's not the environment, which you can easily verify by taking one of these worlds https://github.com/ethz-asl/rotors_simulator/tree/master/rotors_gazebo/worlds, I don't know what your problem might be. The optimization is not too explicit about its error in VINS-Fusion AFAIK.

edit: btw this issue tracked down the underlying error https://github.com/HKUST-Aerial-Robotics/VINS-Fusion/issues/147

[Pallav1299]

@BeatScherrer Thanks for the help.