weixin_39911916
2020-11-24 12:18 阅读 3

Can we get a flag wether we had successful matches in localization mode ?

Hi,

Thanks for your excellent work in cartographer. Now the pure localization function works well.

As we want to localization before navigation, can we get a flag when successful matches the map we load in ROS node directly rather than from sparse_pose_graph/constraint_builder.xx in the further?

Thanks !

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

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

19条回答 默认 最新

  • weixin_39699121 weixin_39699121 2020-11-24 12:18

    I too would like to know the strength of the current base_link-to-map transform. I publish the actual pose from Node::PublishTrajectoryStates. Is there some way to compute the variance of the position there? I'm doing it now with a hard-coded (small) variance.

    点赞 评论 复制链接分享
  • weixin_39866867 weixin_39866867 2020-11-24 12:18

    The request is quite vague. Could you outline how you would like to receive such a flag? Would it be part of one of the messages cartographer_ros currently publishes or something else entirely?

    In general, I think this could be implemented after googlecartographer/cartographer#574 is merged and cartographer_ros refactored to using it.

    点赞 评论 复制链接分享
  • weixin_39699121 weixin_39699121 2020-11-24 12:18

    What I would like is for cartographer_ros to (optionally) publish a pose message containing covariance -- one of these: nav_msgs/Odometry or geometry_msgs/PoseWithCovarianceStamped. Odometry is probably better as it has the child_frame_id. The covariance would be very high if the initial position search was still moving in drastic hops.

    I dove into this a bit: I can see how to get covariance out of the Ceres scan matcher using the Ceres API for such. However, I could not see how to get that or anything similar from the current submap position/rotation strength. I don't understand how that part works.

    Reducing the entire thing to a boolean of good/bad point seems feasible to me. We just won't publish a point if it's not "good". It seems that the Node class ought to work this way already. I have traditionally published my points from Node::PublishTrajectoryStates (see here: https://github.com/asirobots/cartographer_ros/blob/93932a6dfa04aa1a1131f2a6e051fda5f6776e28/cartographer_ros/cartographer_ros/node.cc#L233). The availability of a boolean or confidence number in that method would be sufficient for my needs. Maybe we could have a configurable "minimum confidence" required to publish the base_link->map transform.

    点赞 评论 复制链接分享
  • weixin_39866867 weixin_39866867 2020-11-24 12:18

    I think there is a misconception of what localization actually entails though. Initially, there is no "successful localization", so the flag would be false. Once we tied the pure localization trajectory to the existing map, we have a match, but that is updated only by global slam. This "match" will be developed by local SLAM in the current pure localization trajectory only.

    I would not be sure what sort of semantics either a bool or a covariance would have. For example: the match might be very good against the current submap in the pure localization trajectory, so we are confident that we are correct relative to this. However, the last global match to the existing map might have been a while, so we might not perfectly align to that anymore. what covariance would you want then?

    点赞 评论 复制链接分享
  • weixin_39699121 weixin_39699121 2020-11-24 12:18

    Initially, there is no "successful localization"...

    What are we doing during this phase? Are we publishing our transforms during this time? Can we simply avoid publishing anything during that time?

    Concerning the covariance approach:

    I think the covariance can be computed by the traditional distance-from-average approach inside RealTimeCorrelativeScanMatcher::Match. I don't think it would add much computationally there as the necessary loops already exist. You can add the covariance calculation the Ceres scan matcher like the sample below. I don't know how expensive the hit for that is. The trick comes in combining them.

    the last global match... might have been a while...

    Yes, the covariance from the global scan matcher will need to degrade (aka, increase) over time and/or distance. Typically, when combining covariances, the variance becomes the weight. To determine this, I need further explanation on how it works presently. Do we replace the current pose with the global match 's best pose when it runs? Do we only use that as input into Ceres? Do we combine the two points somehow? Use a Bayesian combination?

    Alternatively, instead of computing covaraince in the scan matchers, we could have a moving average window of the computed points. IOW, compute it further down the line. The window would probably need to be long enough to include several global scan match calls so that we had sufficient variance.

    Covariance in Ceres scan matcher:

    cpp
    *pose_estimate = ...;
    ceres::Covariance covariance(ceres_covariance_options_); 
    auto success = covariance.Compute({std::make_pair(ceres_pose_estimate, ceres_pose_estimate)}, &problem); 
      if (success) { 
        double matrix[9]; 
        success = covariance.GetCovarianceBlock(ceres_pose_estimate, ceres_pose_estimate, matrix); 
        ...
    
    点赞 评论 复制链接分享
  • weixin_39699121 weixin_39699121 2020-11-24 12:18

    With the metrics (project 14) going into Cartographer -- will those better enable the ability to acquire the covariance of the current position estimate?

    点赞 评论 复制链接分享
  • weixin_39742392 weixin_39742392 2020-11-24 12:18

    Hi, i saw your brilliant analysis about localization. So can i get the current match score in ROS and how to achieve it . Thanks for your guide.

    点赞 评论 复制链接分享
  • weixin_39737240 weixin_39737240 2020-11-24 12:18

    Any updates?

    点赞 评论 复制链接分享
  • weixin_39593744 weixin_39593744 2020-11-24 12:18

    How did you solve this?

    点赞 评论 复制链接分享
  • weixin_39593744 weixin_39593744 2020-11-24 12:18

    could you help me to understand this?

    I'm trying to use pure localization to autonomous movements. How could I know if the vehicle is well located? What would be the possibles approach in order to trust the position??

    I imagine I can compare with my last confident position and check if it differs from what cartographer says, but I could imagine many problems this could originate.

    点赞 评论 复制链接分享
  • weixin_39737240 weixin_39737240 2020-11-24 12:18

    indeed it is a bad idea: 1. if cartographer has initially localized wrong, you do want it to make the jump and relocalize 2. cartographer already has that functionality and "jump" thresholds can be set from config files

    However, there are a few solutions: 1. use a locally accurate sensor (odometry) and tune trust values in cartographer 2. you can use a global visual odometry-based localizer that can determine the "region" in which you should be

    点赞 评论 复制链接分享
  • weixin_39593744 weixin_39593744 2020-11-24 12:18

    First of all many thanks for your reply, I really need some people help me understanding some things. My solution is to save all checked positions (now I manually indicates when the first position is correct) in a file in order to restart cartographer with a start trajectory. But in order to check if position is correct in all time, i verify periodically in the next way:

    1. Using the position cartographer says the agv is, I calculate a vector of distances, in function of angle, of what lidar should be seeing from the pgm map image I saved when I create the map.
    2. I compare, periodically, the vector of distances from 1st point with the lidar scan data using least squares.
    3. If the error between two vectors is so big I can assume the localization is incorrect.

    Is this an incorrect approach?

    I have no encoders on my wheels and I'm not using IMU. Should I close the loop using speed that I send to motor controllers?

    点赞 评论 复制链接分享
  • weixin_40000999 weixin_40000999 2020-11-24 12:18

    Did that once by simply checking if inter constraints to a different trajectory get published.

    Additional points for checking if the residuals are small (for confidence of proper localization)

    点赞 评论 复制链接分享
  • weixin_39737240 weixin_39737240 2020-11-24 12:18

    First of all many thanks for your reply, I really need some people help me understanding some things. My solution is to save all checked positions (now I manually indicates when the first position is correct) in a file in order to restart cartographer with a start trajectory. But in order to check if position is correct in all time, i verify periodically in the next way:

    1. Using the position cartographer says the agv is, I calculate a vector of distances, in function of angle, of what lidar should be seeing from the pgm map image I saved when I create the map.
    2. I compare, periodically, the vector of distances from 1st point with the lidar scan data using least squares.
    3. If the error between two vectors is so big I can assume the localization is incorrect.

    Is this an incorrect approach?

    I am not sure I understand it correctly, so as long as it works - it's good.

    I have no encoders on my wheels and I'm not using IMU. Should I close the loop using speed that I send to motor controllers?

    Having no encoders sucks, cause feeding target velocity into a feedback loop is usually a bad idea. It may be better than nothing, but I'd look into alternatives. I would suggest running laserscan correlation to measure frameshift, but I am pretty sure Cartographer does that already as well.

    All in all, I would play with cartographer settings first and, if the localization is still unsatisfactory, try to fit additional sensors (maybe a cheap camera for optical flow-based VO?).

    点赞 评论 复制链接分享
  • weixin_39953629 weixin_39953629 2020-11-24 12:18

    Hi everyone!

    I'm kind of having the similar questions with .

    Is there anyway to measure in pure localization mode how well the laser scan points matched the loaded map? Which basically means, how to measure the pure localization (re-localization) quantitatively?

    If the measurement can be achieved by obtaining the values of those constraints, like mapping_constraints_constraint_builder_{2d, 3d}_constraints, mapping_constraints_constraint_builder_{2d, 3d}_scores, and mapping_{2d, 3d}_pose_graph_constraints, then how can we obtain these values? I didn't see any topic or log info where they are published.

    Any suggestions or hints are appreciated. Thanks!

    点赞 评论 复制链接分享
  • weixin_39737240 weixin_39737240 2020-11-24 12:18

    Hi everyone!

    I'm kind of having the similar questions with .

    Is there anyway to measure in pure localization mode how well the laser scan points matched the loaded map? Which basically means, how to measure the pure localization (re-localization) quantitatively?

    If the measurement can be achieved by obtaining the values of those constraints, like mapping_constraints_constraint_builder_{2d, 3d}_constraints, mapping_constraints_constraint_builder_{2d, 3d}_scores, and mapping_{2d, 3d}_pose_graph_constraints, then how can we obtain these values? I didn't see any topic or log info where they are published.

    Any suggestions or hints are appreciated. Thanks!

    yeah, i am pretty you you can't. i have the same issue, but from what i have researched, there is no standard way to do this. there was an issue a while back that explained that they tried to expose position covariance, but it ended up being meaningless.

    my workaround is to have static ar markers, like aruco or alvar and try to find them on startup, to give cartographer a reference position, as it works well after that.

    you could try looking something like that in Ceres matcher documentation, but a word of caution: in a dynamic environment, you may localize correctly, but the match will be bad in some areas anyway. you may end up with a lot of false positives for incorrect pose estimation this way

    点赞 评论 复制链接分享
  • weixin_39586395 weixin_39586395 2020-11-24 12:18

    indeed it is a bad idea:

    1. if cartographer has initially localized wrong, you do want it to make the jump and relocalize
    2. cartographer already has that functionality and "jump" thresholds can be set from config files

    However, there are a few solutions:

    1. use a locally accurate sensor (odometry) and tune trust values in cartographer
    2. you can use a global visual odometry-based localizer that can determine the "region" in which you should be

    What are these "jump" thresholds you are referring are these parameters in the lua file?

    点赞 评论 复制链接分享
  • weixin_39572794 weixin_39572794 2020-11-24 12:18

    Dear Community, First of all, Thanks for opening an interesting discussion. Currently am working for Autonomous vehicle localization (self-driving car). I agree it's very necessary to verify the pose computed from the scan to the map-matching algorithm. As there is not any direct method to estimate the covariance, which is very crucial especially for fusion-based localization approaches.

    In my case, I am trying to use LIDAR / GPS as the main source of POSE and IMU for tuning trajectory based on EKF fusion. Our localization model mainly depending on lidar (ndt matching, scan to map) localizer, and we are looking to estimate the covariance for the matching output. And based on matching reliability, the Fusion approach decides to consider POSE from LiDAR / GPS to fuse with IMU measurements.

    My Question:

    My idea: Currently, I am working on estimating the pose covariance by integrating scan-scan matching along with the scan-map matching. My goal is to verify the transformation estimated NDT matching (Scan to Map) with the Scan-Scan matching. In Scan to Scan matching am considering very few feature points in each scan.

    1. Is anyone here tried this before? Is Scan to scan matching is feasible enough to estimate the ndt scan to map reliability?
    2. Or is there any other method to estimate the reliability?

    Thanks in advance, appreciate your suggestions.

    Regards, Ajay

    点赞 评论 复制链接分享
  • weixin_39775872 weixin_39775872 2020-11-24 12:18

    As has already explained, saying whether there was really a successful localization match from inside SLAM is very difficult / impossible.

    Although nowadays you can get some hints by using the metrics interface by using the /read_metrics ROS service and checking e.g. the number and scores of the found constraints. A similar discussion goes more into detail: https://github.com/cartographer-project/cartographer_ros/issues/1258#issuecomment-481612460

    点赞 评论 复制链接分享

相关推荐