weixin_39913141
weixin_39913141
2021-01-12 19:28

Regenerating submap after final optimization

Continuing discussion from Hangout meeting.

I am wondering if cartographer can regenerate probability grids(or submap) after final optimization using optimized trajectory before saving it as asset(or pbstream file).

As mentioned in cartographer_ros#413 and during the hangout, the new approach of occupancy grid generation(submap stitching from the external node) will not have as good quality as the previous approach because it is not a replaying from the optimized trajectory.

Considering that the full map is likely to be loaded from pbstream file to serve for next operation, I would like to expect a better quality of occupancy grid since it is fully optimized. And, I think this can be achieved if cartographer rebuilds submap after optimization.

Also, it might be helpful to find a better scan matching between old submap and new trajectory nodes from a new trajectory.

Would it be possible to rebuild submaps' probability grid just as building occupancy grid 2d?

该提问来源于开源项目:cartographer-project/cartographer

  • 点赞
  • 写回答
  • 关注问题
  • 收藏
  • 复制链接分享
  • 邀请回答

13条回答

  • weixin_39563132 weixin_39563132 3月前

    Ok maybe i'm too focused about this potential solution already. I will take a step back and explain the problem.

    We want to do live-long-slam. Our approach would be to keep a set of submaps that covers the whole area and to remove those submaps that are to some degree covered by newer submaps. (hence #409 ) The problem we see is, that our submaps contain an error from drifting that creates discontinuous and/or double walls when stitching them together. I think the "cartographer way" to improve this would be to reduce the submap size. But we have already quite small ones and performance seams to reduce if we go much lower. By intuition I would also guess that the repetitiveness of the environments where our robots drive dictates a lower bound on the submap size.

    Anyway, loop closures were working well after some tuning, so the OccupancyGrid code, that was recently removed, created a very decent map – much better than stitching together the imperfect submaps. So we thought to improve the submaps probability_grid using the same process of replaying the respective scans along the optimized trajectory. On second thought though; question to you : Would this also improve the scan matching against the submap? To my understanding scans are matched against the probability grid, so improving it should improve the matches. After all what we want to achieve is improved localization. Nice looking maps are just considered an indicator for reliability.

    About your questions: 1. The output of asset_writer main does in fact look much better. The double walls are gone (see attached images, esp. lower end) 2. Ok I was mostly referring to the following snippet. Could you explain what exactly is going to be removed? I'm not sure if i got you right

     C++
      range_data_inserter.Insert(carto::sensor::TransformRangeData(
            Decompress(node.constant_data->range_data),
            node.pose.cast<float>()),
                                   &probability_grid);
      }
    </float>

    3. Thanks for the pointer I was not even aware of this code and the option to define actions for the asset_writer. However: if we would want to improve the scan matching a "single large (sub)map" would not help... would it? Or are you suggesting to replace a couple of submaps with such a optimized bigger submap? And then to use this for localization? That could actually work for us as well. 4. Sounds cool, but this was not initially our aim when we opened this issue

    ____ Attached images x-ray stitched_submaps

    点赞 评论 复制链接分享
  • weixin_39627405 weixin_39627405 3月前
    1. Okay, then the discussed approaches can help you. But as you correctly see, better quality submaps would still be nice since this would improve SLAM results due to better scan-to-submap matches. One more question in this regard: Did you look at the individual submaps? Is there an individual submap which looks bad, or are they just badly aligned to each other?
    2. constant_data->range_data will no longer exist since only a small, heavily filtered subset of it is needed for scan matching. We would like to insert range data into submaps and then drop it.
    3. Yes, that was my suggestion. If you currently serialize submaps and load them, their poses are fixed. Using a single large submap might be beneficial here. Although it would need a change to the max_constraint_distance filter, a single branch-and-bound match against the full map would likely be of better quality and performance.
    4. :-(
    点赞 评论 复制链接分享
  • weixin_39563132 weixin_39563132 3月前
    1. The submaps are slightly skewed so if you would improve the alignment on the lower end (in the screenshot) it would come apart on the other end.
    2. Ok, I see your point there. But for some use cases the accuracy of the map might be more constraining then the memory usage. Would you consider to implement the dropping as an optional feature?
    3. That's actually very interesting then. So lets say we load this old Trajectory with one single big submap and use it for localization. In the new trajectory we would not only keep the last 3 submaps, but also all the submaps that show significant change compared to the patch in the big old map below. After some time we would then bake these "update submaps" into the big old one. For this – and to avoid degenerating the big map over time – we would again need to replay the scans along the optimized trajectory. Or am I missing something here?
    4. Don't be sad :) Its likely a feature that many in the ROS community would love to see. It's just that we hope to use cartographer for mapping as well as for localization in the future. At the moment we in fact use the yaml and pgm from version of cartographer before https://github.com/googlecartographer/cartographer_ros/commit/99da2f20d8ce9a7c9549f68315635b644168a14b to patch small updates into our map. Localization is still done with mrpt
    点赞 评论 复制链接分享
  • weixin_39563132 weixin_39563132 3月前

    Another question about 2.: Can you elaborate on the heavy filtering? The discussion in https://github.com/googlecartographer/cartographer_ros/pull/411 indicates that you want to be able to use old scans for scan matching. Somehow I fail to see how you what to do it without the scan_data. Or to be more precise: I can't think of a way of filtering, that is preserving the scan matching, but inhibits the drawing of submaps.

    点赞 评论 复制链接分享
  • weixin_39528000 weixin_39528000 3月前

    When will you carry out dropping range_data after insertion to submaps? and as asked, how to deal with MatchingOldScans? BTW, if I am only using the standalone cartographer, how can I stitch submaps to generate an occupancy grid?

    点赞 评论 复制链接分享
  • weixin_39528000 weixin_39528000 3月前

    I've tried to totally drop ComputeConstraintForOldScans. And remove all the point cloud in the trajectorynode when scan_queue_ become empty. However, the system dies at some point where I could not found out at present. To my understanding, the history range data will only be used in ComputeConstraintForOldScans and my deletion should already be after all the threads being finished? Is there anything missed in my implementation?

    点赞 评论 复制链接分享
  • weixin_39528000 weixin_39528000 3月前

    problem solved! range data are removed totally after their first usage.

    点赞 评论 复制链接分享
  • weixin_39866867 weixin_39866867 3月前

    offers a possible solution for regenerating submaps outside of Cartographer in #574: The callback can be used to publish or keep range data around and use local SLAM to regenerate submaps in a separate node. This will still be suboptimal vs using the asset writer with the full sensor data, but very similar to the original plan in the OP.

    We currently have no plans of supporting this concept in Cartographer itself, but maybe a sub-repo in Cartographer or a package in cartographer_ros could house this functionality. Closing as this issue is not actionable it its current form.

    点赞 评论 复制链接分享
  • weixin_39627405 weixin_39627405 3月前
    1. Did you verify, e.g. using the assets_writer_main that the increased quality of the result makes this worth the effort?

    2. Please do not reuse the old code. It was using point cloud data which in the future we do not want to keep in memory. Most of this data is not needed for scan matching and it is a large part of our memory consumption right now.

    3. How about following the probability_grid_points_processor to implement writing to a single large (sub)map?

    4. If we wanted to, we could also add writing pgm and yaml (for compatibility with other ROS nodes) and converting from pgm and yaml to a pbstream with a single submap (to allow using Cartographer localization with data from other sources)

    点赞 评论 复制链接分享
  • weixin_39781209 weixin_39781209 3月前

    4) would be awesome! I am definitely planning to start working on that at some point in the future.

    点赞 评论 复制链接分享
  • weixin_39781209 weixin_39781209 3月前

    This could be done only for visualization purposes (perhaps controllable with an option in the Submaps plugin), so that we can have an accurate preview in RViz of what the map built by assets_writer would look like. We would not do additional optimization using scan matching with the regenerated submaps.

    Going one step further, and much more difficult, would be to regenerate the submaps, repeat scan matching, regenerate constraints, and repeat optimization; now we can regenerate submaps again and repeat everything -- maybe this could be repeated until convergence :-)

    点赞 评论 复制链接分享
  • weixin_39913141 weixin_39913141 3月前

    My intention is not for only visualization purpose. I want to have an optimized occupancy grid to feed other localization and navigation nodes.

    点赞 评论 复制链接分享
  • weixin_39563132 weixin_39563132 3月前

    Did anyone start to work on this? Otherwise I would start by trying to reuse the old BuildOccupancyGrid2D code for it.

    点赞 评论 复制链接分享