Variable to indicate when the mapping started

[mirellameelo]

Hello, everyone!
When I was testing the TF tree (please refer to this PR), I got lots of errors and warnings due to the SLAM node being publishing info even when the mapping hasn't started.
I believe we need a bool variable to indicate when mapping really starts, otherwise, we'll be publishing a pose that does not exist yet. The same for the TF, but in this case, leads to errors and warnings. Does it make sense? If so, does this variable is already available at Openvslam? In a negative case, a suggestion is to implement this bool by verifying if the number of keyframes > 0.

To test the TF, I did a temporary solution by verifying if the image(s) arrives (which assumes the user does not input an empty image's frame_id). But I don't believe this is a proper implementation because the mapping not necessarily start when the very first image(s) arrives.

Looking forward for other opinions. Thanks in advance!

[dimaxano]

Hi! I also faced that problem.
Solved that by sending pose information only when tracking state is TRACKING

[AlexeyMerzlyakov]

Agree with @dimaxano. Tracking state == tracker_state_t::Tracking should work. I think, this could be a new API call in system.h/system.cc accessing the tracker (via lock mutex on it) and its state.

[mirellameelo]

@dimaxano @AlexeyMerzlyakov Thanks for replying!
This is what I did.
I'm just confused about this mutex Alexey suggested. I mean, the methods feed_monocular_frame, feed_stereo_frame and feed_RGBD_frame also read tracker_->tracking_state_, but none of them blocks the var before reading. This is something that needs to be fixed for all methods then? Or am I missing something? Thanks.

[dimaxano]

But that new tracker status getting API in System class will provide read-only function for the tracker state. Does it necessary in that case to use mutex locking?
P.S. Sorry, not an expert in that part of C++

[dimaxano]

Just tried to find some evidence that enum is atomic, but I can't.
Agree that, if implementing separate API in System for obtaining tracking state, we should provide tread-safe mechanism for accessing it in mapping or loop closing thread.

But I would propose just to make the current tracker state enum to be atomic (like here)

[AlexeyMerzlyakov]

Even more: if writing to enum would be an atomic operation, this still won't be consistent overall. Imagine, you have a particular frame where tracker changes its state from Lost -> to Tracking. In parallel you are reading tracker state, but before its track_*_image() method will complete. This will give an inconsistent tracker state (sometimes Lost, sometimes Tracking on the same frame). To avoid this, we need to wait until tracker will finish its frame processing and after that ask for its state.

[mirellameelo]

Thanks for clarifying, @AlexeyMerzlyakov. So, seems that you already implemented and added all these mutexes, right? I think that once your PR is accepted, I can use the same mtx_tracking_ to add this, how does it sound?

[AlexeyMerzlyakov]

Yes, I've adding this in another activity where I faced the similar questions: parallel using a tracker_ should being locked by mtx_tracking_. This mutex could be used in your code in bool system::is_tracking() or anything else using tracker_ resources, I suppose.

[ymd-stella]

@mirellameelo

we'll be publishing a pose that does not exist yet.

Can you please describe it accurately? Is the pose with 0 translation and unit quaternion published?
The referenced Pose seems to be initialized with the unit matrix.
(ref. https://github.com/OpenVSLAM-Community/openvslam/blob/00cdcc6d5c979c0ebd461f59974b4aa43e3c1a2b/src/openvslam/publish/map_publisher.h#L90 )

If OpenVSLAM is outputting a valid Pose, I don't think anything will go wrong. Please explain why and what the problem occurs.

[AlexeyMerzlyakov]

I think, this is a rather trade-off between pose reliability and the possibility of pose publishing.

If talking about possibility, one can be published after system object is created. Indeed, in the system::system() constructor map_publisher_ is being made with the identity transform at the moment of its creation. So the zero-pose will be available right after system construction.

Regarding reliability, I thought about it more. Tracking state == Tracking should give a consistent pose. However, this may be too weak condition for transform publishing, I think. For example, we could have a sensor fusion (tracker could get the pose by IMU, odometry, GPS, etc... which could be added to OpenVSLAM is a foreseen future) or intentional pose setting by a developer (stella-cv/stella_vslam#79). In this case even if tracking state is Lost the pose still might be considered to be somehow valid. So, we may count on that tracking was started (first image received). Probably on something like this [1],[2] (not committed yet).
Maybe, I am wrong. @mirellameelo, could you please describe which problem did you face with when using a pose obtained before tracking was started?

[mirellameelo]

@ymd-stella Sorry, I was not explicit regarding the error.

1. The camera_link_ is set when images come.
2. Supposing frames did not arrive yet, and "run_slam" is already waiting for them...
   2.1) The Odometry type message can handle an empty frame and will publish the pose without errors.
   2.2) But the TF, cannot, and it leads to the error:
   [ERROR] [run_slam]: Transform failed: Invalid argument passed to lookupTransform argument source_frame in tf2 frame_ids cannot be empty
3. Then I also noticed this possible inconsistency regarding the pose published. As I said before, the pose is being published no matter what, even if the tracking status is "lost".
4. So, I thought a way to fix both 2.2 and 3, could be checking the tracking status.
   Does this make sense for you?

[ymd-stella]

I think the first issue can be solved by simply not calling publish_pose() when camera_link_ is empty. For the second issue, I think it is better to solve it by publishing the states. I think the data for a similar situation is NavSatFix for GNSS.

[mirellameelo]

@ymd-stella Sorry for the late reply. Yes, I was doing the verification by the camera_link_'s string (empty or not). But I don't get the point of your second suggestion. For this, I'd have to get from the system the current state and, only after that, publish it. This is a good one and it can be done, but I think this is not necessary at the moment - From my understanding, I just need to know if the tracking is happening or not. Please, could you take a look at these last two commits? This is solving the TF issue for now. Is it enough for a PR? Thanks in advance.

[ymd-stella]

LGTM. Please submit a PR.

[ymd-stella]

I think we need to keep in mind that immediately after relocalization is unreliable. openvslam_ros might be better to simply output the status, and the nodes that receive it will filter according to the status.

[ymd-stella]

@mirellameelo Is #19 solving the problem?

[mirellameelo]

Yes, it is! Thanks!!