-
Notifications
You must be signed in to change notification settings - Fork 726
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Map loading and Transform/Odom topic refactoring #214
Conversation
Thanks a lot for the nice PR. I pushed a fix for a problem on the CI that wrongly failed. Could you merge The PR looks nice, and I'm going to merge it with slight modifications. But, please note that my review would be slow because I'm a bit busy at this moment for writing a paper... |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Sorry for the very late response. The PR looks fine, but I would like to request to revert a few points to keep the default behavior unchanged because some changes can affect the systems of existing users.
@koide3, I might be interested in these changes, so I can wrap up the work of merging this PR. Is this something that still interests you? |
Hey guys, apologies for neglecting this. I moved into a different role and didn't have time to continue working on it / totally forgot. My bad :). Though it does work, this PR was more a PoC than a serious attempt at getting it working robustly. My understanding at the time was somewhat surface level, so there is certainly more that can be done to get map reuse working to a level where it would be useful in practice. From memory, some helper processes to aid in roughly aligning the map on load would go a long way. The kidnapped robot problem is still real. I had renamed the topics to add clarity for our use context and remove contentions with other systems we were running ( We used this on a Boston Dynamics Spot in conjunction with a number of other solutions ). However, I agree it makes sense to change back. I don't have the capability to test this anymore, but I can make the requested changes and push a new commit to the PR. |
Hey @TirelessDev, thanks for coming back and making some new commits. Let me know when you're done with new commits, I can test them for you with some test bags. |
@marcelinomalmeidan Not a prob; it should be up to date with master now and have those default names reverted. Adding the extra Yeah, as mentioned in the PR, the robot needs to be in a very similar position and orientation when booting up for the map to converge correctly. Practically this wasn't an issue for us, as Spot always returned to the same spot to charge. However, one of the interesting drivers for map reuse for us was actually map sharing and map collaboration. While finding kidnapped robots in SLAM maps is an entire research topic, there are practical ways that the problem can be worked around. Having solid anchors ( QR codes ) in the map to help localise and provide an initial pose correction would probably be the easiest. However, the map as it stands would need to be extended to support this sort of anchor. Alternatively, such behaviour could be sideloaded to a different service quite easily. So much potential, so little time. |
apps/hdl_graph_slam_nodelet.cpp
Outdated
// Iterate over the items in this directory and count how many sub directories there are. | ||
// This will give an upper limit on how many keyframe indexes we can expect to find. | ||
boost::filesystem::directory_iterator begin(directory), end; | ||
int maxDirectoryCount = std::count_if(begin, end, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
nit: this repo is mostly using camel_case
for its variables, so I'd recommend updating maxDirectoryCount
and keyframeDir
to camel case
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Good suggestion, have updated now
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
The new solution worked to me! It seems good to go, IMO
Unfortunately, I can't approve this PR, but hopefully we'll hear soon from @koide3
Wow, thanks guys for your work! I'll make time to review and approve the PR in a few days. |
apps/hdl_graph_slam_nodelet.cpp
Outdated
floor_plane_node = static_cast<g2o::VertexPlane*>(graph_slam->graph->vertex(id)); | ||
} | ||
} | ||
std::cout << "loaded special nodes - anchor_node: "<< anchor_node->id() << " anchor_edge: "<< anchor_edge->id() << " floor_node: "<< floor_plane_node->id() << std::endl; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
there is a minor issue in this line. If either anchor_node
, anchor_edge
or floor_plane_node
is a null pointer, then we cannot print ->id()
for them (segfault). This happens if any of them is -1
in the loaded CSV file, and it happened to me.
I'd suggest only printing when they're not null pointers. I think that only anchor_node
needs to be non-null to be able to load the graph successfully
The code now looks good to me, and I did a quick test in my environment . I'll merge this PR once the last issue pointed by @marcelinomalmeidan is fixed. |
@TirelessDev ping :-) |
Thanks again @TirelessDev for your contribution! I also thank @marcelinomalmeidan for handling this PR! |
Not a problem. Sorry for missing that final semicolon. There is always something, looks like you caught it in the merge though. |
@TirelessDev thanks for finalizing the PR! |
This PR covers two changes to the hdl_graph_slam master.
To the first point. We have added a
load
service endpoint that takes a string of the path to a map previously dumped using thedump
endpoint. This is a folder that contains pose graph information and keyframes.This loading should be performed on a fresh start of hdl_graph_slam, and before new point clouds are received. After loading new point clouds will be used to extend the loaded map.
Of particular importance to know is that the current odometry information from the previous map generation process is not retained, ie where it figured the Velodyne frame was within the Map frame. This means that after starting a new instance of hdl_graph_slam and after loading a previous map the "current estimated position" of the Velodyne will be at the origin. This is super important because if point clouds don't then arrive and describe the same local environment, the registration to the existing map will be incorrect. There may be ways to help alleviate this problem, perhaps by providing an initial position estimate to update the internal odom estimate. However, this is a topic for future work.
An example of the std output from the loading service call.
With respect to the second lot of changes, to the topic subscriptions. These basically involved some minor modifications to the nodelet code to allow additional parameters to be passed into the node to configure the transform frame names. Basically, just to remove hardcoded values like
"/odom"
so we can avoid conflicts when integrating with other systems on the robot.