Visualize odometry inliers?

[lucasw]

When there aren't enough inliers and odometry fails occasionally I'd like to be able to see matched keypoints during the failure event and leading up to it. I see there is an array of KeyPoints in RGBDImage which I can try overlaying onto the input image (there are nearly 1000- I assume a larger response is a better keypoint so there can be a minimum response threshold to reduce the visual noise), but I'm guessing the inliers that produced the odometry solution are just a subset of those from the current and previous frame.

[ WARN] (2023-02-20 07:50:41.577) OdometryF2M.cpp:561::computeTransform() Registration failed: "Not enough inliers 6/20 (matches=132) between -1 and 285" (guess=xyz=0.099791,-0.024623,-0.010031 rpy=0.006478,-0.005582,-0.045491)

Would I need to modify rtabmap/corelib/src/odometry/OdometryF2M.cpp to return inliers? If there is a non-ROS inlier image overlay I'd use that, perhaps it could be adapted to return an image that could then be converted to a ros image and published on the rtabmap_ros side (unless that already exists).

The same information in 3D in rviz using a Marker would be useful also but I'll start with 2D.

I have some initial experimentation with this here: https://github.com/lucasw/rtabmap_ros/tree/keypoints_viz

[matlabbe]

In OodmInfo message, there are fields about the matches and inliers of rhe current frame and the local feature map:

# F2M odometry
#   std::multimap words;
#   std::vector wordMatches;
#   std::vector wordInliers;
int32[] wordsKeys
KeyPoint[] wordsValues
int32[] wordMatches
int32[] wordInliers
int32[] localMapKeys
Point3f[] localMapValues

The words map (combined wordsKeys with wordsValues) would have roughly always 1000 features max. The wordMatches would correspond to 132 in your log message. For the inliers, they would appear in wordInliers. They correspond to ids in words map. You should be able to associate the image frame with the same stamp than the OdomInfo topic. The topic doesn't provide the descriptors from the current frame (though they could be found in the republished rgbd_image output from the odometry node) and the local feature map (only 3D points are published).

To get the descriptors from the local feature map, you would need to publish it from somewhere here by getting the map data from here, e.g.:

const Signature & featureMap = ((OdometryF2M*)odometry_)->getMap();

featureMap.getWords();
featureMap.getWords3();
featureMap.getWordsDescriptors();

[lucasw]

I'm going to visualize OdomInfo in https://github.com/lucasw/rtabmap_ros/blob/keypoints_viz/scripts/odom_info_viz.py but it's not doing anything yet.

Do the matches refer back only 1 frame, so I can look up the xy keypoint in the previous frame with wordsKeys (and then see how much the match moved to the current frame keypoint)- or because it's F2M Frame-To-Map and not just F2F Frame-To-Frame can the matches go back more than one frame? (I'd actually prefer to be using F2F but it wasn't clear that that's an option in ros)

You should be able to associate the image frame with the same stamp than the OdomInfo topic

Exact time syncing was failing for me in the above script: the stamps are off by some microseconds, can look into that more later, there's a geometry2 tf2 python stamp conversion issue possibly involved there.

[mrtnbm]

Do the matches refer back only 1 frame, so I can look up the xy keypoint in the previous frame with wordsKeys (and then see how much the match moved to the current frame keypoint)- or because it's F2M Frame-To-Map and not just F2F Frame-To-Frame can the matches go back more than one frame?

Yes, F2F only considers the last keyframe while F2M considers groups of previous frames. You will need to use F2F then.