search

suchetanrs / ORB-SLAM3-ROS2-Docker

This repository contains a full wrapper class for running ORB-SLAM3 on a docker container with ROS2 Humble with Ubuntu 22.04.
134 stars 26 forks source link

The KF was not found in allKFs_. This should not happen #13

Open eloiht opened 2 months ago

eloiht commented 2 months ago

Hi @suchetanrs

First of all thank you for creating this project

I have managed to run the project successfully but when tracking is lost and it creates a second map the code runs into the following exception

https://github.com/suchetanrs/ORB-SLAM3-ROS2-Docker/blob/a25df1b38f949ce1c84503a4f601ed13060c3b70/orb_slam3_ros2_wrapper/src/orb_slam3_interface.cpp#L99

What could be causing this and how to fix it/ avoid the situtation?

Thank you

suchetanrs commented 2 months ago

Hi @eloiht !

Thanks for your ticket. This has always been a pain point :p The logic of using mapsList[c]->GetInitKFid() - 1 to find the parent keyframe is not the most appropriate out there and is not fool proof. Hence the error. I'll try to find a better fix to this. Can you give me a little more information? You should find a log like so [rgbd-1] First KF:89; Map init KF:79 before the runtime error. Could you please try to recreate it and post this log and anything else you may find relevant?

eloiht commented 2 months ago

Hi @suchetanrs

Thank you for your response. I have reproduced the issue, and uncommented some logs to have extra information.

Hope this helps. Please let me know if there is anything I can do on my side to help you address this issue.

suchetanrs commented 2 months ago

Hi, thanks for your reply. I initially thought this can be achieved in the following way: A map's parent keyframe is the previous map's last keyframe. We can get this through ORB-SLAM3's classes. However, the issue arises when multiple map's merge and the previous map is not the map of interest at all for us to get the parent keyframe. I need to read ORB-SLAM3's code to understand how to deal with this better.

If you have time and would like to fix this, here is the context. When tracking is lost, we want the TF published to continue from the last tracked position so that the pose is atleast partially right. This needs to be catered by odometry during loss of tracking but for now, continuing through the last tracked position should work. What you could do is read the code further and try to find a better way to find this "last tracked position" after loss of tracking. Please let me know if you find anything. I will need to find some time to work on this again. Will let you know soon.

eloiht commented 2 months ago

Hi @suchetanrs

Could you please confirm each of the following statements if I understood right?

Thank you

suchetanrs commented 2 months ago

Hello, Here is what I think:

  • calculateReferencePoses must store in mapReferencePoses_ for each ORB_SLAM3::Map * the origin coordinate of each map in the "map" (world) reference frame.

Yes. The origin from ORB_SLAM3::Map * is always zero for every map internally to ORB_SLAM3 and hence the extra variable.

  • For the first map, it will always be robot_x, robot_y. For subsequent maps, it will be an estimation, as tracking has been lost so there is no guaranteed positon for the ORB_SLAM3::Map origin in the "map" (world) frame.

Yes. The origin of the map in the world frame is simply zero inside of ORB_SLAM3 and hence the extra estimation. Yes. For the first map it will be robot_x, robot_y.

  • Maps will eventually be merged so with a single map and origin in robot_x, robot_y the position in the "map" frame will be correct.

We obviously cannot guarantee that all the maps will be merged. There might be some maps that are still unmerged if they did not find common features with other maps in the atlas. But yes, the idea is right. If all maps are merged then all the keyframe poses will be accurate in reference to the world frame.

  • Currently you are approximating the position of the subsequent maps as the last known position of the previous map.

Yes. I am also under a very bad assumption that the previous keyframe id is just current map's init kf - 1. Which is somewhat incorrect.

  • In the current approach some frames are deleted so there is no guarantee for all frames to exist in the frame array (frame with id [map origin frame - 1 ] may not exist anymore, that is why it crashes.

Yes. I also think that if a keyframe is marked as bad then it won't exist in the vector we get when we do "GetAllKeyFrames()". I am not completely sure about the second statement. Will need to confirm.

  • Would approximating the map origin by taking the previous map position (position of base_link->map when the last odom->map transform was published) + variation in odometry since that point in time make sense?

Yes. This would be right to do if we assume maps never merge. Consider this, map_1 -> tracking_lost -> map_2 -> tracking_lost -> map_3 -> tracking_lost -> map_4. In this case, if map_4 and map_2 have a match, I am not sure how ORB_SLAM3 determine's which map merges into the other and which is the map that remains later. I think a good way to start would be to figure this out and see if this information is available for use with ORB_SLAM3. Please let me know if you have any idea about this. If we assume, map_4 merges into map_2 and only map_2 remains after the merge is complete and the current active map during tracking is map_2 then we would ideally use map_2's origin as the reference pose to publish the TF. This is just an initial thought :p Not sure if its exactly right.

hsutzu commented 1 month ago

Hi, i was having the same problem,it shows: [monocular-1] ORB-SLAM failed: Not initialized. [monocular-1] [WARN] [1723188059.004310467] [ORB_SLAM3_MONO_ROS2]: ORB-SLAM3 failed: Not initialized. [monocular-1] ORB-SLAM failed: Not initialized. [monocular-1] [WARN] [1723188059.043300284] [ORB_SLAM3_MONO_ROS2]: ORB-SLAM3 failed: Not initialized. [monocular-1] First KF:87; Map init KF:85 [monocular-1] New Map created with 228 points [monocular-1] terminate called after throwing an instance of 'std::runtime_error' [monocular-1] what(): The KF was not found in allKFs_. This should not happen [ERROR] [monocular-1]: process has died [pid 10111, exit code -6, cmd '/root/colcon_ws/install/orb_slam3_ros2_wrapper/lib/orb_slam3_ros2_wrapper/monocular /home/orb/ORB_SLAM3/Vocabulary/ORBvoc.txt /root/colcon_ws/install/orb_slam3_ros2_wrapper/share/orb_slam3_ros2_wrapper/params/mono.yaml --ros-args -r __ns:=/ --params-file /tmp/tmpljra90z_']. Is there any solutions?

suchetanrs commented 1 week ago

Hi @eloiht @hsutzu I was able to find some time to work on this again. I have pushed a fix for this on the master branch but mind you that this is still quite hacky. I am working on fully integrating the odometry but i've pushed this as an intermediate fix so the program atleast won't crash :p