Support initial_pose in openvslam node

[AlexeyMerzlyakov]

This ticket is about adding the support of initial_pose (geometry_msgs::msg::PoseWithCovarianceStamped) topic containing robot/camera pose to set. The algorithm should set the robot pose in localization mode immediately to the pose taken from input message. This is the "golden standard" for other SLAM-s, like AMCL or SLAM_Toolbox, so OpenVSLAM node should also support this, I suppose.

This should be supported on openvslam_ros side as initial_pose topic subscriber plus base_link->camera_link TF converter. Additionally, setting a forced pose should be supported on openvslam side too. The system class has tracker_ tracking module which contains current frame (curr_frm_). This frame has the pose, as preliminary analysis shown which is used by tracking_module::track_current_frame() to set the camera(robot) position. The change of the curr_frm_ to required position should forcibly move the robot to that pose.

[AlexeyMerzlyakov]

Further analysis is in progress. Any ideas about it are welcoming!

[ymd-stella]

I think it would be better to find a keyframe (or keyframes) closest to initial_pose and run https://github.com/OpenVSLAM-Community/openvslam/blob/00cdcc6d5c979c0ebd461f59974b4aa43e3c1a2b/src/openvslam/module/relocalizer.cc#L40-L147 with the keyframe as a candidate. If you simply rewrite the current position, I don't think it will be stable.

[AlexeyMerzlyakov]

Thanks for the idea! Honestly, since all 3 tracking approaches are tied to current and previous frame, just changing the camera position might be unstable. So, re-localization at key-frame position should be better solution. However, what should we do, if we have no keyframe nearby (e.g. in radius of preset threshold): still move robot to nearest keyframe seems not to be a good idea, if, say, it is located at a distance of 2 meters from the requested position. Probably is it better in this case to add a new frame with an initial_pose position and try to re-localize there (if we possibly can do that)?

[SteveMacenski]

How does the pure localization mode handle starting up (does it take in an initial pose estimate to seed the initial matching?) Why not just in the /initialpose message receipt essentially just restart that process and clear out the prior existing track?

[ymd-stella]

So, re-localization at key-frame position should be better solution.

There seems to be a misunderstanding: relocalizer computes the relative position from the keyframe to the current frame by solving a PnP problem. Without being able to solve the PnP problem, OpenVSLAM cannot start tracking.

[ymd-stella]

How does the pure localization mode handle starting up (does it take in an initial pose estimate to seed the initial matching?)

OpenVSLAM performs the search from all of the keyframes (by bag of visual words). When it finds a similar scene, it computes the relative position from that keyframe.

[SteveMacenski]

That seems to be problematic that we can't give it an initial pose seed to use instead of purely global localization. Especially in dynamic worlds where objects move from the original map, being able to seed in repetitive spaces is necessary for a robust enough solution to rely on. Else, you're kind of "sorry out of luck" if it doesn't match correctly on the first pose which the user could potentially provide information about for a general area.

So there seems to be 2 issues:

• No current ability to relocalize from an external input /initialpose
• No current ability to even start initial localization in a particular position (even if openvslam refines it before use in the local region)

Is there a way to support this feature? Issue 2 on that list seems like the more pertinent problem, if that's solved, the first one could make use of the same solution.

[ymd-stella]

It is possible with multi-session Visual SLAM; you can add a keyframe to initial_pose and SLAM from there. OpenVSLAM does not support multi-session at now. rtabmap and ORB_SLAM2 include functions for multi-session/multi-map.

[SteveMacenski]

We're just talking about the pure localization mode right now, not really interested in multi-session continue mapping. Do you disagree that it would be useful (and probably necessary to use openVSLAM localization in production) to have a way to set the initial pose on startup of pure localization to seed the match? I'm not saying add a keyframe there, just to look at that point and the region around it rather than searching the entire map for the initial pose match -- if an initial pose estimate is given

[ymd-stella]

If that's the case, I think what I wrote in https://github.com/OpenVSLAM-Community/openvslam_ros/issues/11#issuecomment-814032968 is sufficient. What other information do you want?

[AlexeyMerzlyakov]

Added work-in-progress PR to openvslam part for setting the camera to a known pose as recommended in https://github.com/OpenVSLAM-Community/openvslam_ros/issues/11#issuecomment-814032968. The development for openvslam_ros part is in progress and the second PR will be added later as it becomes available.