扫描匹配-cartographer
Kong Liangqian Lv6
 2021-11-22 18:38:29  

首先还是local_trajectory_builder_2d

可以对暴力匹配的部分单独提取出来，进行一个重定位。

AddAccumulatedRangeData

预测time时间的先验位姿，

1
2
3
4
// Computes a gravity aligned pose prediction.
// 进行位姿的预测, 先验位姿
const transform::Rigid3d non_gravity_aligned_pose_prediction =
    extrapolator_->ExtrapolatePose(time);

对重力方向进行校准，即让z轴和重力方向一致，得到了二维的先验位姿

1
2
3
// 将三维位姿先旋转到姿态为0, 再取xy坐标将三维位姿转成二维位姿
const transform::Rigid2d pose_prediction = transform::Project2D(
    non_gravity_aligned_pose_prediction * gravity_alignment.inverse());

进行一个自适应体素滤波，就可以进行扫描匹配了

会返回一个pose_estimate_2d，一个扫描匹配之后的位姿

1
2
3
4
// local map frame <- gravity-aligned frame
// 扫描匹配, 进行点云与submap的匹配
std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
    ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

把扫描之后的位姿进行返回，变换到扫描匹配之前的一个状态

1
2
3
// 将二维坐标旋转回之前的姿态
const transform::Rigid3d pose_estimate =
    transform::Embed3D(*pose_estimate_2d) * gravity_alignment;

pose_estimate 就是扫描匹配之后的一个位姿了，然后再校准

1
2
// 校准位姿估计器
extrapolator_->AddPose(time, pose_estimate);

ScanMatch

如果没有活跃的子图，那么就将先验位姿直接返回

1
2
3
if (active_submaps_.submaps().empty()) {
  return absl::make_unique<transform::Rigid2d>(pose_prediction);
}

活跃子图一般有俩，第一个用于扫描匹配，取出子图

1
2
3
// 使用active_submaps_的第一个子图进行匹配
std::shared_ptr<const Submap2D> matching_submap =
    active_submaps_.submaps().front();

使用相关性扫描匹配，即暴力匹配，对传入的位姿进行校准

1
2
3
4
5
6
7
8
9
transform::Rigid2d initial_ceres_pose = pose_prediction;

// 根据参数决定是否 使用correlative_scan_matching对先验位姿进行校准
if (options_.use_online_correlative_scan_matching()) {
  const double score = real_time_correlative_scan_matcher_.Match(
      pose_prediction, filtered_gravity_aligned_point_cloud,
      *matching_submap->grid(), &initial_ceres_pose);
  kRealTimeCorrelativeScanMatcherScoreMetric->Observe(score);
}

使用校准完的Pose进行ceres的基于优化的扫描匹配的实现，由于校准之后的位姿比较准，因为这里的效果会比较好

1
2
3
4
5
6
7
auto pose_observation = absl::make_unique<transform::Rigid2d>();
ceres::Solver::Summary summary;
// 使用ceres进行扫描匹配
ceres_scan_matcher_.Match(pose_prediction.translation(), initial_ceres_pose,
                          filtered_gravity_aligned_point_cloud,
                          *matching_submap->grid(), pose_observation.get(),
                          &summary);

匹配成功之后返回结果pose_observation

scan_matching::RealTimeCorrelativeScanMatcher2D

暴力的实时相关性扫描匹配，其文件位于cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h文件中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
// An implementation of "Real-Time Correlative Scan Matching" by Olson.
class RealTimeCorrelativeScanMatcher2D {
 public:
  explicit RealTimeCorrelativeScanMatcher2D(
      const proto::RealTimeCorrelativeScanMatcherOptions& options);

  RealTimeCorrelativeScanMatcher2D(const RealTimeCorrelativeScanMatcher2D&) =
      delete;
  RealTimeCorrelativeScanMatcher2D& operator=(
      const RealTimeCorrelativeScanMatcher2D&) = delete;

  // Aligns 'point_cloud' within the 'grid' given an
  // 'initial_pose_estimate' then updates 'pose_estimate' with the result and
  // returns the score.
  double Match(const transform::Rigid2d& initial_pose_estimate,
               const sensor::PointCloud& point_cloud, const Grid2D& grid,
               transform::Rigid2d* pose_estimate) const;

  // Computes the score for each Candidate2D in a collection. The cost is
  // computed as the sum of probabilities or normalized TSD values, different
  // from the Ceres CostFunctions: http://ceres-solver.org/modeling.html
  //
  // Visible for testing.
  void ScoreCandidates(const Grid2D& grid,
                       const std::vector<DiscreteScan2D>& discrete_scans,
                       const SearchParameters& search_parameters,
                       std::vector<Candidate2D>* candidates) const;

 private:
  std::vector<Candidate2D> GenerateExhaustiveSearchCandidates(
      const SearchParameters& search_parameters) const;

  const proto::RealTimeCorrelativeScanMatcherOptions options_;
  };

头文件，只有两个函数 和两个成员变量

其他所有的函数都是通过Match函数进行调用的，因此只看Match，注意Match里面的点云都是相对于Local坐标系的，其点云的原点也是在local坐标系原点的

Match

Match函数位于cartographer/mapping/internal/2d/scan_matching/real_time_correlative_scan_matcher_2d.h中，

RealTimeCorrelativeScanMatcher2D::Match 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
/**
 * @brief 相关性扫描匹配 - 计算量很大
 * 
 * @param[in] initial_pose_estimate 预测出来的先验位姿
 * @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
 * @param[in] grid 用于匹配的栅格地图
 * @param[out] pose_estimate 校正后的位姿
 * @return double 匹配得分
 */
double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const

把点云旋转到先验位姿上

1
2
3
4
5
6
// Step: 1 将点云旋转到预测的方向上
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())));

SearchParameters是一个配置参数的位置，传入的参数有

  • linear_search_window 从 配置文件来，线性搜索窗口的大小
  • angular_search_window 从配置文件来，角度搜索窗口的大小
  • 旋转后的点云
  • 网格的分辨率
1
2
3
4
// 根据配置参数初始化 SearchParameters
const SearchParameters search_parameters(
    options_.linear_search_window(), options_.angular_search_window(),
    rotated_point_cloud, grid.limits().resolution());

search_parameters 里面存储了

1
2
3
4
5
6
7
8
9
// Tightens the search window as much as possible.
void ShrinkToFit(const std::vector<DiscreteScan2D>& scans,
                 const CellLimits& cell_limits);

int num_angular_perturbations;            // 个数
double angular_perturbation_step_size;    // 角度分辨率
double resolution;
int num_scans;                            // 旋转后的点云集合的个数
std::vector<LinearBounds> linear_bounds;

按照角度分辨率和旋转后点云集合的个数，对点云进行旋转保存于vector。至于要如何生成 这个个数。可以查看论文

1
2
3
4
// Step: 2 生成按照不同角度旋转后的点云集合
const std::vector<sensor::PointCloud> rotated_scans =
    GenerateRotatedScans(rotated_point_cloud, search_parameters);

对点云进行平移，返回的内容不是点云本身，而是经过平移之后的在栅格中的索引。大vector里面的元素代表着第几个旋转点云

DiscreteScan2D 也是一个vector typedef std::vector<Eigen::Array2i> DiscreteScan2D; 这个vector的元素表示每一个点平移后的索引

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

// Step: 4 生成所有的候选解

1
2
3
// Step: 4 生成所有的候选解
std::vector<Candidate2D> candidates =
    GenerateExhaustiveSearchCandidates(search_parameters);

需要注意的是：

max_element 可以用的原因是因为类Candidate2D里面重载了< 和 >。

1
2
3
4
5
6
7
8
9
10
11
12
13
// Step: 5 计算所有候选解的加权得分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);

  // Step: 6 获取最优解
  const Candidate2D& best_candidate =
      *std::max_element(candidates.begin(), candidates.end());
  
  // Step: 7 将计算出的偏差量加上原始位姿获得校正后的位姿
  *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;
 Comments