weixin_39563132
weixin_39563132
2021-01-12 19:33

Implementation of an initial pose for new trajectories

Hey, I was searching for a way to initialize new trajectories with a pose estimate.

The behaviour I would like to get is that cartographer searches for constraints in a certain window around the given the initial pose in order to connect to trajectory 0 (lets say trajectory 0 is enough for now). At some point the SPG should be optimized and the initial pose estimate would then be improved. If no constraint is found inside the window it should either fail or fall back to global localization.

So, what do you think about saving the initial pose as the current pose of node 0 and submap 0 of the new trajectory by saving it to the respective pose in node_data_ and submap_data_ of the optimization problem? Further I would TrajectoryConnectivityState::Connect() the new trajectory to trajectory 0. This would make respective calls of SparsePoseGraph::ComputeConstraint() to fall into the local search case with the given initial pose as the center of the local search window.

This would be an alternative to #511

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

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

7条回答

  • weixin_39526185 weixin_39526185 3月前

    Thanks, there is already #469. Having prior knowledge about where the robot is and using that to speed up localization definitely is something we want to add.

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

    you just closed this without answering the question... is basically asking for the initialpose feature development direction. Please comment at least if you have a different idea of developing initialpose or if it is similar to what you plan on initialpose.

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

    Alternatively I can repost my suggestions there. I just opened a new issue, because I wanted to discuss a potential implementation with the goal of preparing a pull request afterwards.

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

    Thanks, Jihoon and Moritz. I am trying to keep the discussion around initial poses contained in the original issue as I would prefer not to have it spread out over multiple.

    I had added an (admittedly short) comment on #469 about the direction for initial pose development we are thinking, that I have just expanded.

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

    Sorry if I was too aggressive. And thanks for the explanation. It explains enough to check if we are moving to the same direction or somewhere else. :) We will continue the discussion in #469.

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

    Oh, I did not have that impression, so all good :) And I think you made a good point in that I should start live streaming more of my thoughts into the tracker even if I am not confident yet that they make sense :)

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

    Yes. It would be really helpful for us to sync thoughts and plan what features to contribute in cartographer.

    点赞 评论 复制链接分享