koide3
commented
5 years ago Hi @RickHutten
In this system, we intentionally developed the slam and scan_matching_odometry nodelets as separated modules so that we can replace scan_matching_odometry with another good one to obtain odometry. It's a bit redundant design, but it allows us to use any odometry information source (e.g., robot wheels) to improve the mapping accuracy.
So, actually, in this system, keyframes in scan_matching_odometry do not relate to keyframes in hdl_graph_slam. The former ones are used to manage scan matching while the latter ones are used to judge if a new frame is registered to the graph. (It's confusing. I think I should have given different names to them...)
hdl_graph_slam has its own keyframes and does not use keyframes in scan_matching_odometry, so the mismatch does not affect the mapping as long as the slam nodelet can receive proper odometry data.
In scan_matching_odometry_nodelet, the odometry gets published between every incomming cloud from /filtered_points and its previous keyframe. When the odometry is above some specified threshold, the keyframe against which incomming pointclouds are matched is updated. The hdl_graph_slam_nodelet feeds off the same topic /filtered_points, and synchronizes the pointcloud callback with the odom topic, and then again checks if a new keyframe should be added. However, the last known keyframe from scan_matching_odometry_nodelet is not necessarily the same as the last keyframe in hdl_graph_slam_nodelet, this is because it is determined with two different thresholds (1, 2) if a new keyframe should be added and on top of that you might drop some messages if you don't have the processing power to process all incomming pointclouds. The chance these two pointclouds are the same is actually very small. This means that the odom in the cloud_callback function is not based on the same previous keyframe in hdl_graph_slam_nodelet. I would imagine this has an considerable effect on accuracy of the graph since the odom edge between the two vertices is not correct.
Might this be a bug or is this done on purpose and am I missing something? Why not just publish a custom message from scan_matching_odometry_nodelet that includes both the pointcloud and odom from keyframes only and process those messages in hdl_graph_slam_nodelet? Than you also just have to check once if you have a new keyframe.