Ros

RealTimeCorrelativeScanMatcher2D

ancientcc

cartographer估算出一个初始位姿(initial_pose_estimate)后，真正位姿往往并不是它，而是在它附近，实时相关扫描匹配器(RealTimeCorrelativeScanMatcher2D)就用于找到这个真正位姿。它根据初始位姿和点云，以初始位姿为中心，在指定范围内枚举出所有候选位姿，并生成每个位姿对应的点云。然后分别计算每个点云中hit点落在概率图上的位置，并计算这些位置处的占据概率之和，即点云和概率图的重合度。通过这种暴力匹配，认为占据概率之和最大的就是真正位姿。

图1 实时相关扫描匹配

图1的angular_search_window表示角度半径阀值，单位弧度，linear_search_window表示平移半径阀值，单位米，它们共同决定了暴力搜索范围。代码中，表示范围的数据结构是std::vector<Candidate2D>，假设变量名叫candidates。候选位姿数(candidates.size()) = 角度数 * 每个角度上的平移数。

  • 角度数(num_scans)。先算出个角度步长angular_perturbation_step_size，然后角度半径阀值angular_search_window除以angular_perturbation_step_size得到num_angular_perturbations。由于角度范围是正、负各一半，再加一个0值，于是角度数num_scans等于“2 * num_angular_perturbations + 1”。
  • 每个角度上的平移数。来自于linear_search_window，和角度类似，以一个步长去除linear_search_window。这个步长就是resolution(0.05m)，当linear_search_window是0.1米时，x、y方向上各有5个位置，总合平移数就是5*5=25，图1的#0、#1到#24分别表示了这个25个平移位置。

以下是angular_search_window=0.35、linear_search_window=0.1，并且angular_perturbation_step_size=0.009时，那算出的候选项数：

num_scans(79) * 25 = 1975

针对1975中的某个位姿，可粗略这么认为：旋转部分是79个角度中的某一个，平移部分是雷达中心放在图1中25个位置中某一个

 

一、RealTimeCorrelativeScanMatcher2D::Match

Match执行实时相关扫描匹配，RealTimeCorrelativeScanMatcher2D其它成员可说都是它的辅助函数。

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  1. initial_pose_estimate：初始位姿。CeresScanMatcher2D::Match中，就是odom坐标系下欲优化的local_pose。
  2. point_cloud：点云。CeresScanMatcher2D::Match中，就是base_footprint坐标系下，经过重力修正后的点云。
  3. pose_estimate：用于存储真正初始位姿。CeresScanMatcher2D::Match中，值就是&initial_pose_estimate，因为在最后赋值，不影响前面用它作为输入值。
  CHECK(pose_estimate != nullptr);

  const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
  const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(
      point_cloud,
      transform::Rigid3f::Rotation(Eigen::AngleAxisf(
          initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());

首先把点云从base_footprint变换到odom坐标系，但只是修改旋转，没改平移，结果存放在rotated_point_cloud。然后根据范围参数linear_search_window和angular_search_window生成搜索参数，这些参数存储在search_parameters。

  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);

根据搜索参数生成num_scans个旋转后的点云，即rotated_scans.size=num_scans。注意，这些点云只是经过了旋转变换，没经过平移。

  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));

将rotated_scan中的num_scans个点云进行平移变换。discrete_scans.size依旧是num_scans。

  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);

根据搜索范围，生成candidates，candidates长度是num_scans*25。

  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

对candidates中各个位姿进行打分，分数最好的那个赋值给best_candidate。不论平移还是旋转，best_candidate存储的都是相对于initial_pose_estimate的偏移，所以真实位姿是“initial_pose_estimate + best_candidate”。

  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  *pose_estimate = transform::Rigid2d(
      {initial_pose_estimate.translation().x() + best_candidate.x,
       initial_pose_estimate.translation().y() + best_candidate.y},
      initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
  return best_candidate.score;
}

 

二、SearchParameters::SearchParameters

SearchParameters::SearchParameters(const double linear_search_window,
                                   const double angular_search_window,
                                   const sensor::PointCloud& point_cloud,
                                   const double resolution)
    : resolution(resolution) {
  // We set this value to something on the order of resolution to make sure that
  // the std::acos() below is defined.
  float max_scan_range = 3.f * resolution;
  for (const sensor::RangefinderPoint& point : point_cloud) {
    const float range = point.position.head<2>().norm();
    max_scan_range = std::max(range, max_scan_range);
  }

max_scan_range是点云中最远扫描点和雷达中心距离。for内变量range是某个点离雷达中心距离，即直角三角形中的。可以这么认为：以雷达中心为圆心、max_scan_range为半径的圆，恰好包含point_cloud中的所有点。

  const double kSafetyMargin = 1. - 1e-3;
  angular_perturbation_step_size =
      kSafetyMargin * std::acos(1. - common::Pow2(resolution) /
                                         (2. * common::Pow2(max_scan_range)));
  num_angular_perturbations =
      std::ceil(angular_search_window / angular_perturbation_step_size);
  num_scans = 2 * num_angular_perturbations + 1;

如何推导角步长(angular_perturbation_step_size)公式？——由上所述，会出现个以雷达中心为圆心、max_scan_range为半径的圆，恰好包含point_cloud中的所有点。

图2 推导角度步长

图2中O是圆心，即雷达中心，OAB是等腰三角形，等边OA、OB长度是圆半径max_scan_range，底边AB长度是栅格分辨率resolution。由余弦定理：​，对于本三角形有：，代入公式有：。

角度数(num_scans)。先算出个角度步长angular_perturbation_step_size，然后角度半径阀值angular_search_window除以angular_perturbation_step_size得到num_angular_perturbations。由于角度范围是正、负各一半，再加一个0值，于是角度数num_scans等于“2 * num_angular_perturbations + 1”。当中各变量值可参考图2。

  const int num_linear_perturbations =
      std::ceil(linear_search_window / resolution);
  linear_bounds.reserve(num_scans);
  for (int i = 0; i != num_scans; ++i) {
    linear_bounds.push_back(
        LinearBounds{-num_linear_perturbations, num_linear_perturbations,
                     -num_linear_perturbations, num_linear_perturbations});
图2 SearchParameters

num_linear_perturbations表示在长度上，一侧有多少个搜索点。和角度类似，以一个步长去除linear_search_window，这个步长就是resolution(0.05m)。当linear_search_window是0.1米时，num_linear_perturbations就2，它表示在长度上，每一侧有两个搜索点。x、y的方向的初始值均为（-num_linear_perturbations, num_linear_perturbations），图2中(-2, 2)，之所以正负范围，是因为要根据初始位姿四周进行搜索。由于搜索点是正、负各一半，再加一个0值，所以x、y方向上各有5个欲搜索位置，总合平移数就是5*5=25，图1的#0、#1到#24分别表示了这个25个平移位置。

  }
}

 

三、GenerateRotatedScans

根据搜索参数生成num_scans个“加”了旋转偏移后的点云，

std::vector<sensor::PointCloud> GenerateRotatedScans(
    const sensor::PointCloud& point_cloud,
    const SearchParameters& search_parameters) {
  std::vector<sensor::PointCloud> rotated_scans;
  rotated_scans.reserve(search_parameters.num_scans);

  double delta_theta = -search_parameters.num_angular_perturbations *
                       search_parameters.angular_perturbation_step_size;
  for (int scan_index = 0; scan_index < search_parameters.num_scans;
       ++scan_index,
           delta_theta += search_parameters.angular_perturbation_step_size) {
    rotated_scans.push_back(sensor::TransformPointCloud(
        point_cloud, transform::Rigid3f::Rotation(Eigen::AngleAxisf(
                         delta_theta, Eigen::Vector3f::UnitZ()))));

point_cloud是旋转初始位姿了的odom坐标系下点云。此处以它为基础按可能角度偏移生成num_scans个点云。

  • [0/91]delta_theta: -0.35565
  • [1/91]delta_theta: -0.34774
  • [2/91]delta_theta: -0.33984
  • ...
  • [88/91]delta_theta: 0.33984
  • [89/91]delta_theta: 0.34774
  • [90/91]delta_theta: 0.35565

以上是num_scans中增加的角度偏移，依着索引升序，值由小变大，相邻角度间相差angular_perturbation_step_size，即角度步长。第一个偏移角度-angular_search_window，最后一个angular_search_window。

  }
  return rotated_scans;
}

 

四、DiscretizeScans

将rotated_scan中的num_scans个点云进行平移变换。返回值discrete_scans长度依旧是num_scans。

std::vector<DiscreteScan2D> DiscretizeScans(
    const MapLimits& map_limits, const std::vector<sensor::PointCloud>& scans,
    const Eigen::Translation2f& initial_translation) {
  std::vector<DiscreteScan2D> discrete_scans;
  discrete_scans.reserve(scans.size());
  for (const sensor::PointCloud& scan : scans) {
    discrete_scans.emplace_back();
    discrete_scans.back().reserve(scan.size());
    for (const sensor::RangefinderPoint& point : scan) {
      const Eigen::Vector2f translated_point =
          Eigen::Affine2f(initial_translation) * point.position.head<2>();
      }

Affine2f是仿射变化，但这里只有平移。以下是6个点的变换示例。(1,2)是初始位姿中平移部分。

  • Affine2f(initial:(1.00000, 2.00000)) * point.position(-0.58136, -3.53174) => translated_point(0.41863, -1.53174)
  • Affine2f(initial:(1.00000, 2.00000)) * point.position(-0.09588, -4.87782) => translated_point(0.90411, -2.87782)
  • Affine2f(initial:(1.00000, 2.00000)) * point.position(-0.05497, -4.87491) => translated_point(0.94502, -2.87491)
  • Affine2f(initial:(1.00000, 2.00000)) * point.position(0.10711, -5.20639) => translated_point(1.10711, -3.20639)
  • Affine2f(initial:(1.00000, 2.00000)) * point.position(0.60323, -5.97015) => translated_point(1.60323, -3.97015)
  • Affine2f(initial:(1.00000, 2.00000)) * point.position(0.65065, -5.94937) => translated_point(1.65065, -3.94937)
      discrete_scans.back().push_back(
          map_limits.GetCellIndex(translated_point));
    }
  }
  return discrete_scans;
}

 

五、RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates

根据搜索范围，生成candidates，candidates长度是num_scans*25。

std::vector<Candidate2D>
RealTimeCorrelativeScanMatcher2D::GenerateExhaustiveSearchCandidates(
    const SearchParameters& search_parameters) const {
  int num_candidates = 0;
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    const int num_linear_x_candidates =
        (search_parameters.linear_bounds[scan_index].max_x -
         search_parameters.linear_bounds[scan_index].min_x + 1);
    const int num_linear_y_candidates =
        (search_parameters.linear_bounds[scan_index].max_y -
         search_parameters.linear_bounds[scan_index].min_y + 1);
    num_candidates += num_linear_x_candidates * num_linear_y_candidates;
  }

预计算出candidates长度：num_candidates。

  std::vector<Candidate2D> candidates;
  candidates.reserve(num_candidates);
  for (int scan_index = 0; scan_index != search_parameters.num_scans;
       ++scan_index) {
    for (int x_index_offset = search_parameters.linear_bounds[scan_index].min_x;
         x_index_offset <= search_parameters.linear_bounds[scan_index].max_x;
         ++x_index_offset) {
      for (int y_index_offset =
               search_parameters.linear_bounds[scan_index].min_y;
           y_index_offset <= search_parameters.linear_bounds[scan_index].max_y;
           ++y_index_offset) {
        candidates.emplace_back(scan_index, x_index_offset, y_index_offset,
                                search_parameters);
      }
    }
  }
  CHECK_EQ(candidates.size(), num_candidates);
  return candidates;
}
图3 candidates

图3显示了GenerateExhaustiveSearchCandidates之后、ScoreCandidates之前的相关变量值。candidates中的长度是num_scans(91)*25=2275。当中以25个单元为一组，Candidate2D.scan_index是组索引，正如Watch4所示，每隔25个后增1。同组Candidate2D有着相同角度(orientation)。Watch4显示的candidates[2274]是最后一个单元，scan_index=90，x_index_offset、y_index_offset位在了图1的#24位置。

除了score字段，到现在已填充了Candidate2D其它字段，ScoreCandidates就是要填充score。

 

六、RealTimeCorrelativeScanMatcher2D::ScoreCandidates

对candidates中各个候选项进行打分，

6.1 RealTimeCorrelativeScanMatcher2D::ScoreCandidates

void RealTimeCorrelativeScanMatcher2D::ScoreCandidates(
    const Grid2D& grid, const std::vector<DiscreteScan2D>& discrete_scans,
    const SearchParameters& search_parameters,
    std::vector<Candidate2D>* const candidates) const {
  for (Candidate2D& candidate : *candidates) {
    switch (grid.GetGridType()) {
      case GridType::PROBABILITY_GRID:
        candidate.score = ComputeCandidateScore(
            static_cast<const ProbabilityGrid&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);

默认使用概率图，launcher用的也是概率图。

        break;
      case GridType::TSDF:
        candidate.score = ComputeCandidateScore(
            static_cast<const TSDF2D&>(grid),
            discrete_scans[candidate.scan_index], candidate.x_index_offset,
            candidate.y_index_offset);
        break;
    }
    candidate.score *=
        std::exp(-common::Pow2(std::hypot(candidate.x, candidate.y) *
                                   options_.translation_delta_cost_weight() +
                               std::abs(candidate.orientation) *
                                   options_.rotation_delta_cost_weight()));
  }
}

6.2 ComputeCandidateScore

对某个候选项打分。

float ComputeCandidateScore(const ProbabilityGrid& probability_grid,
                            const DiscreteScan2D& discrete_scan,
                            int x_index_offset, int y_index_offset) {
  float candidate_score = 0.f;
  for (const Eigen::Array2i& xy_index : discrete_scan) {
    const Eigen::Array2i proposed_xy_index(xy_index.x() + x_index_offset,
                                           xy_index.y() + y_index_offset);

xy_index存储着某个点以index表示的栅格坐标，所属点云的雷达中心栅格是初始位姿对应的那个栅格；(x_index_offset, y_index_offset)是偏移坐标系中的25个中的某一个，它们相加就是该候选项对应的雷达中心栅格。在旋转部分，这个候选项中的点云已经过“初始值+偏移角度”旋转，所以要调整的只是平移。

    const float probability =
        probability_grid.GetProbability(proposed_xy_index);
    candidate_score += probability;
  }
  candidate_score /= static_cast<float>(discrete_scan.size());

candidate_score是累加点云所在栅格的占据概率，candidate_score是该点云中所有点在概率图占据概率的平均值。

  CHECK_GT(candidate_score, 0.f);
  return candidate_score;
}

 

七、SearchParameters::ShrinkToFit

参考“cartographer--后端match--减小搜索框策略(ShrinkToFit)[1] ”。

参考

  1. cartographer--后端match--减小搜索框策略(ShrinkToFit) https://blog.csdn.net/weixin_44684139/article/details/106161379
发布于 2022-03-01 20:24:28

全部评论: 0 条

    写评论: