cartographer从入门到放弃4---核心算法分析---数据处理与Local SLAM

最新推荐文章于 2022-12-05 18:45:26 发布

年少的梦007

最新推荐文章于 2022-12-05 18:45:26 发布

阅读量1.4k

点赞数

分类专栏： slam

本文链接：https://blog.csdn.net/weixin_41518014/article/details/117660434

版权

slam 专栏收录该内容

10 篇文章 1 订阅

订阅专栏

从网上搜索大量的文献来解决这一部分难题，进度上更慢一些，理解可能会有偏差，有错误之处请多多指出

数据处理+Local SLAM

输入：

1.激光雷达的数据经过Voxel Filter、Adaptive Voxel Filter(调用体素滤波，如果体素滤波后点数大于阈值，则返回，如果小于阈值，则接着使用二分法进行体素滤波)后的点云数据

2.经过里程计、惯导及点云匹配后的位姿态预估下一时刻的位姿

输出：

1.Submaps

Local SLAM主要是运用了ScanMatching的方法，当获得一次扫描数据后，扫描数据与当前最近的submap匹配，获得最优的位置姿态（？？？？）后，将这一帧点云加入到submap中，当submap里的帧数达到一定数目（关键帧插入方法？？？）后就不在更新这个子地图，接下来会创建下一个submap继续以上工作。

流程：

多激光传感器数据基于时间戳同步融合
开启位置估计器
根据位置估计器，针对每个点云point的时间戳进行预测位置进行畸变校准
校准后的点云转换成scanmath和map 更新使用的range格式，并将miss和hit分类
获取校准后的origin pos
获取预测的重力加速度方向
根据重力加速度方向投影点云
降采样滤波，并抛弃设置高度范围外的所有点云
然后调用匹配方法
插入并更新submap
获取匹配后的结果内容，包括时间，轨迹节点位置，节点位置对应点云，submap

Local SLAM之相关匹配（correlative scan matcher）

原理

目前寻找的最好的资料：Real-Time Correlative Scan Matching完全解析（CSM帧匹配算法）

通过Real-time correlative scan matching 论文算法分析、[翻译]Real-Time Correlative Scan Matching对算法有了大致的了解，Cartographer是采取的双搜索的方式进行的，先用一次real-time correlative scan matcher（三维窗口遍历寻优），再构建优化等式，利用ceres优化求解。（栅格概率， T的偏差，R的偏差）

数学模型

$x_{t}^{*} = argmax_{x_{t}} \left \{ p(z_{t}|x_{t},m_{t-1})p(x_{t}|x_{t-1},u_{t-1}) \right \}$

模型为利用最大似然估计计算在t时刻最有可能的位姿，可以理解为匹配的越好时候的位姿 $x_{t}$ ，上面公式的概率越大，我们的目的就是求解这个，其中

$x_{t}$ 、 $z_{t}$ 表示t时刻的位姿数据和激光雷达的观测数据；

$m_{t-1}$ 在此处理解为距离最近的submap；

$x_{t-1}$ ， $u_{t-1}$ 表示为 $t-1$ 时刻的位姿和控制量信息，控制信息可以理解为从里程计和IMU推导出来的数据，有地方公式将 $u_{t-1}$ 也表达为 $u_{t}$ ；

$p(z_{t}|x_{t},m_{t-1})$ 为观测模型，理解为在 $x_{t}$ 位姿， $m_{t-1}$ 地图下，观测数据为 $z_{t}$ 的概率，详细可以参照卡尔曼滤波相关知识；

$p(x_{t}|x_{t-1},u_{t-1})$ 为运动模型，理解为在 $x_{t-1}$ ， $u_{t-1}$ 下 $t$ 时刻最可能的位姿。

公式细化

虽然有公式了，但是还不够详细，激光雷达来一帧数据，是实实在在的数据，得踏踏实实的处理这些数据

1.运动模型 $p(x_{t}|x_{t-1},u_{t-1})$

cartographer部分利用imu构建预测模型，采取UKF进行运动预测。

具体运动模型推测见：cartographer 处理IMU（激光，里程计等）流程

2.观测模型 $p(z_{t}|x_{t},m_{t-1})$

单线激光雷达的数据一般是一个距离和一个角度的极坐标信息，通过求解可以得出x，y；多线激光雷达直接输出x,y,z

一帧数据写到每个点上：

$p(z_{t}|x_{t},m_{t-1})=\prod_{i}^{}p(z_{t}^{i}|x_{t},m_{t-1}) \alpha \sum_{i}^{}log p(z_{t}^{i}|x_{t},m_{t-1})$

其中 $z_{t}^{i}$ 为激光雷达每一个点的概率值，将数据进行栅格化后再计算

栅格地图的知识可以参考占据栅格地图（Occupancy Grid Map）

每一帧数据都可进行栅格化，如下图所示， $m_{t-1}$ 用来表示submap， $z_{t}$ 为当前帧的数据所形成的栅格地图

$m_{t-1}$ $z_{t}$

cartographer 是一种暴力搜索的模式，主要的算法流程：

1.在 $t-1$ 时刻设定一个search window（这个search window用来和当前的激光雷达的帧的数据进行匹配）

2.在search window区域生成当前的位姿的 $z_{t}$ 可能值，作为一个子集

3.遍历所有的子集，计算每一个子集上 $z_{t}$ 所有的概率和，概率最高的那个位姿就是我们要找的当前时刻的位姿（概率和？当前帧数据的栅格和submap里的概率和）

算法流程（未完成）

1.构造粗分辨率和细分辨率两个似然场

2.在粗分辨率似然场上进行搜索，获取最优位姿

3.粗分辨率最优位姿对应的栅格进行细分辨率划分

4.进行细分辨率搜索，获取最优位姿

5.计算位姿匹配方差

代码解析

double RealTimeCorrelativeScanMatcher2D::Match(
    const transform::Rigid2d& initial_pose_estimate,
    const sensor::PointCloud& point_cloud, const Grid2D& grid,
    transform::Rigid2d* pose_estimate) const {
  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())));
  //定义搜索空间，包括平移的窗口和角度窗口
  //option_参数由上层调用传入？？？？？？
  const SearchParameters search_parameters(
      options_.linear_search_window(), options_.angular_search_window(),
      rotated_point_cloud, grid.limits().resolution());
  //在一定的角度搜索空间中（一定的窗口内，一定分辨率，对当前点云放入vector中）
  const std::vector<sensor::PointCloud> rotated_scans =
      GenerateRotatedScans(rotated_point_cloud, search_parameters);
  //将点云进行坐标变换转换到世界坐标系下，根据当前的位置（里程计的位置）进行转换
  const std::vector<DiscreteScan2D> discrete_scans = DiscretizeScans(
      grid.limits(), rotated_scans,
      Eigen::Translation2f(initial_pose_estimate.translation().x(),
                           initial_pose_estimate.translation().y()));
  //初始化xy平移空间内偏移量，即窗口大小和偏移量
  std::vector<Candidate2D> candidates =
      GenerateExhaustiveSearchCandidates(search_parameters);
  //计算当前帧在角度、x、y三层窗口中每个状态在grid中的匹配评分
  ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);
  //采用标准库，迭代获取最大置信度元素
  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;
}

基础太差了，全都细细的分析，步履维艰，脑补c++

1.double RealTimeCorrelativeScanMatcher2D::Match(const transform::Rigid2d& initial_pose_estimate,const sensor::PointCloud& point_cloud, const Grid2D& grid,transform::Rigid2d* pose_estimate)

1.1 transform::Rigid2d& initial_pose_estimate（定义在catkin_ws/src_carto/cartographer/cartographer/transform/transform.h）//初始的位姿信息

template <typename FloatType> class Rigid2{}；

using Rigid2d = Rigid2<double>;

1.2 const sensor::PointCloud& point_cloud;点云的类型

1.3 transform::Rigid2d* pose_estimate //估计的位姿

2.const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();

3.const sensor::PointCloud rotated_point_cloud = sensor::TransformPointCloud(point_cloud,transform::Rigid3f::Rotation(Eigen::AngleAxisf(initial_rotation.cast<float>().angle(), Eigen::Vector3f::UnitZ())));

3.1const sensor::PointCloud

定义在（/catkin_ws/src_carto/cartographer/cartographer/sensor/point_cloud.h)

using PointCloud = std::vector<RangefinderPoint>;

struct RangefinderPoint {

Eigen::Vector3f position;

};

3.2 sensor::TransformPointCloud函数（ vector::reserve 、c++11 之emplace_back 与 push_back的区别）

PointCloud TransformPointCloud(const PointCloud& point_cloud,
                               const transform::Rigid3f& transform) {
  PointCloud result;
  result.reserve(point_cloud.size());
  for (const RangefinderPoint& point : point_cloud) {
    result.emplace_back(transform * point);
//c++开发中我们会经常用到插入操作对stl的各种容器进行操作,常使用push_back()向容器中加入一个右值元
//素(临时对象)时，首先会调用构造函数构造这个临时对象，然后需要调用拷贝构造函数将这个临时对象放入容
//器中。原来的临时变量释放。这样造成的问题就是临时变量申请资源的浪费。 
//引入了右值引用，转移构造函数后，push_back()右值时就会调用构造函数和转移构造函数,如果可以在插入
//的时候直接构造，就只需要构造一次即可。这就是c++11 新加的emplace_back。
  }
  return result;
}

for实现对vector的遍历

假设实现一个vector遍历可以用以下方法

vector<int> line={1,2,3,4,5,6,7,8,9};
（1）

for (vector<int>::const_iterator iter = line.cbegin();iter != line.cend(); iter++) {

    cout << (*iter) << endl;

}

（2）

for (auto iter = line.cbegin(); iter != line.cend(); iter++) {

    cout << (*iter) << endl;

  }

（3）

for (auto lin : line) {
    cout << lin;
  }

4.const SearchParameters search_parameters(options_.linear_search_window(), options_.angular_search_window(),rotated_point_cloud, grid.limits().resolution());

4.1 SearchParameters

定义在/catkin_ws/src_carto/cartographer/cartographer/mapping/internal/2d/scan_matching/correlative_scan_matcher_2d.h

struct SearchParameters {
  // Linear search window in pixel offsets; bounds are inclusive.
  struct LinearBounds {
    int min_x;
    int max_x;
    int min_y;
    int max_y;
  };
 SearchParameters(double linear_search_window, double angular_search_window,
                   const sensor::PointCloud& point_cloud, double resolution);

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);
  }
  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;

  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});
  }
}

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

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()))));//每一个点计算旋转之后的坐标
  }
  return rotated_scans;
}

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

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>();
      discrete_scans.back().push_back(
          map_limits.GetCellIndex(translated_point));
    }
  }
  return discrete_scans;
}

7.std::vector<Candidate2D> candidates = GenerateExhaustiveSearchCandidates(search_parameters);///初始化x,y平移空间内偏移量

8.ScoreCandidates(grid, discrete_scans, search_parameters, &candidates);//计算得分

9.const Candidate2D& best_candidate = *std::max_element(candidates.begin(), candidates.end());//获取最大的置信度

10.*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));

11.return best_candidate.score;//返回得分

--------------------------------------

参考文献

1.cartographer 代码思想解读（1）- 相关匹配

2.Cartographe纯雷达计算位姿的前后端总结

3.Real-time correlative scan matching 论文算法分析

4.Cartographer中对激光雷达运动畸变的处理方法分析

年少的梦007

关注

0
点赞
踩
11

收藏

觉得还不错? 一键收藏
1
评论
cartographer从入门到放弃4---核心算法分析---数据处理与Local SLAM

Scan Matchingcartographer采用的前端匹配算法并非一种，在这里具体分析相关匹配的算法--------------------------------------参考文献1.cartographer 代码思想解读（1）- 相关匹配
复制链接

扫一扫