点云匹配预备-cartographer
Kong Liangqian Lv6
 2021-11-06 18:38:29  

上节说到，在global_traject_builder 中会进行AddSensorData 加入转化为点云的雷达扫描数据

第一步即，扫描匹配，in GlobalTrajectoryBuilder类的AddSensorData中

1
2
3
4
// 进行扫描匹配, 返回匹配后的结果
std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult>
    matching_result = local_trajectory_builder_->AddRangeData(
        sensor_id, timed_point_cloud_data);

localtrajectory_builder 在构造函数的时候即被赋予了初值。

可以为LocalTrajectoryBuilder2d 或者 LocalTrajectoryBuilder3d

我们先看LocalTrajectoryBuilder2d, 位于cartograpger/mapping/internal/2d/local_trajectory_builder_2d.h

点云数据结构

在介绍匹配之前，我们首先强调一下cartographer中保存点云的数据结构

没有时间同步前的点云sensor::TimedPointCloudData

每一块点云都有一个时间time，表示此点云最后一个点的时间

1
2
3
4
5
6
7
8
// 时间同步前的点云
struct TimedPointCloudData {
  common::Time time;        // 点云最后一个点的时间
  Eigen::Vector3f origin;   // 以tracking_frame_到雷达坐标系的坐标变换为原点
  TimedPointCloud ranges;   // 数据点的集合, 每个数据点包含xyz与time, time是负的
  // 'intensities' has to be same size as 'ranges', or empty.
  std::vector<float> intensities; // 空的
};

再看数据点TimedPointCloud，是一个保存了所有点的一个vector

using TimedPointCloud = std::vector\;

每一个点，都有一个自己的时间，但是是负的，根据time_increment计算得到

1
2
3
4
5
6
7
// Stores 3D position of a point with its relative measurement time.
// See point_cloud.h for more details.
// 带时间戳的单个数据点的坐标
struct TimedRangefinderPoint {
  Eigen::Vector3f position;
  float time; // 相对点云最后一个点的时间, 最后一个点的时间为0, 其他点的时间都为负的
};

LocalTrajectoryBuilder2D头文件

首先看一下头文件，为两个保存结果的结构体

1
2
3
4
5
6
7
8
9
10
11
12
13
14
class LocalTrajectoryBuilder2D {  
  // 将点云插入到地图后的result
  struct InsertionResult {
    std::shared_ptr<const TrajectoryNode::Data> constant_data;
    std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 最多只有2个子图的指针
  };
  // 扫描匹配的result
  struct MatchingResult {
    common::Time time;
    transform::Rigid3d local_pose;
    sensor::RangeData range_data_in_local; // 经过扫描匹配之后位姿校准之后的雷达数据
    // 'nullptr' if dropped by the motion filter.
    std::unique_ptr<const InsertionResult> insertion_result;
  };

看一下成员变量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
ActiveSubmaps2D active_submaps_; // 只有两个子图的vector,当一个子图完成了就删掉， 然后新建一个
MotionFilter motion_filter_; //对运动进行过滤，如果运动的太近，或者运行距离太短就会被过滤掉
scan_matching::RealTimeCorrelativeScanMatcher2D
    real_time_correlative_scan_matcher_;
scan_matching::CeresScanMatcher2D ceres_scan_matcher_;
std::unique_ptr<PoseExtrapolator> extrapolator_; //位姿估计器

int num_accumulated_ = 0;
sensor::RangeData accumulated_range_data_;

absl::optional<std::chrono::steady_clock::time_point> last_wall_time_;
absl::optional<double> last_thread_cpu_time_seconds_;
absl::optional<common::Time> last_sensor_time_;

RangeDataCollator range_data_collator_; //雷达数据时间同步的一个类

构造函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/**
 * @brief 构造函数
 * 
 * @param[in] options 
 * @param[in] expected_range_sensor_ids 所有range类型的话题
 */
LocalTrajectoryBuilder2D::LocalTrajectoryBuilder2D(
    const proto::LocalTrajectoryBuilderOptions2D& options,
    const std::vector<std::string>& expected_range_sensor_ids)
    : options_(options),
      active_submaps_(options.submaps_options()),
      motion_filter_(options_.motion_filter_options()),
      real_time_correlative_scan_matcher_(
          options_.real_time_correlative_scan_matcher_options()),
      ceres_scan_matcher_(options_.ceres_scan_matcher_options()),
      range_data_collator_(expected_range_sensor_ids) {}

构造函数即对一些成员变量赋值

options对应的配置文件为trajectory_builder_2d.lua的所有参数

options.submaps_options() 可以获得这个lua文件中，submaps下大括号的参数配置

options_.motion_filter_options() 可以对应 motion_filter下的参数配置

options_.ceres_scan_matcher_options() 可以获得ceres_scan_matcher下的所有配置

options_.real_time_correlative_scan_matcher_options() 可以获得real_time_correlative_scan_matcher下的所有配置

下面是lua配置文件的整体结构

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
35
36
37
38
39
40
41
42
TRAJECTORY_BUILDER_2D = {
  use_imu_data = true,            -- 是否使用imu数据
  min_range = 0.,                 -- 雷达数据的最远最近滤波, 保存中间值
  max_range = 30.,
  min_z = -0.8,                   -- 雷达数据的最高与最低的过滤, 保存中间值
  max_z = 2.,
  missing_data_ray_length = 5.,   -- 超过最大距离范围的数据点用这个距离代替
  num_accumulated_range_data = 1, -- 几帧有效的点云数据进行一次扫描匹配
  voxel_filter_size = 0.025,      -- 体素滤波的立方体的边长
    
  -- 使用固定的voxel滤波之后, 再使用自适应体素滤波器
  -- 体素滤波器 用于生成稀疏点云 以进行 扫描匹配
  adaptive_voxel_filter = {...
  }
    
  -- 闭环检测的自适应体素滤波器, 用于生成稀疏点云 以进行 闭环检测
  loop_closure_adaptive_voxel_filter = {...
   }
  
  -- 是否使用 real_time_correlative_scan_matcher 为ceres提供先验信息
  -- 计算复杂度高 , 但是很鲁棒 , 在odom或者imu不准时依然能达到很好的效果
  use_online_correlative_scan_matching = false,
  real_time_correlative_scan_matcher = {
      
  -- ceres匹配的一些配置参数
  ceres_scan_matcher = {...
  }
  
  -- 为了防止子图里插入太多数据, 在插入子图之前之前对数据进行过滤
  motion_filter = {
  }
        
  -- TODO(schwoere,wohe): Remove this constant. This is only kept for ROS.
  imu_gravity_time_constant = 10.,

  -- 位姿预测器
  pose_extrapolator = {
            
  -- 子图相关的一些配置
  submaps = {...
	},
 }

AddRangeData 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
/**
 * @brief 处理点云数据, 进行扫描匹配, 将点云写成地图
 * 
 * @param[in] sensor_id 点云数据对应的话题名称
 * @param[in] unsynchronized_data 传入的点云数据
 * @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 匹配后的结果
 */
 std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddRangeData(
    const std::string& sensor_id,
    const sensor::TimedPointCloudData& unsynchronized_data) {
  
  // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
  auto synchronized_data =
      range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);

点云时间同步

第一步，将多个雷达信息的点云时间进行同步，并且返回同步后的数据，点云如何同步查看下一篇。

返回后的数据结构为

1
2
3
4
5
6
7
8
9
10
11
// 时间同步后的点云
struct TimedPointCloudOriginData {
  struct RangeMeasurement {
    TimedRangefinderPoint point_time;   // 带时间戳的单个数据点的坐标 xyz
    float intensity;                    // 强度值
    size_t origin_index;                // 属于第几个origins的点
  };
  common::Time time;                    // 点云的时间
  std::vector<Eigen::Vector3f> origins; // 点云是由几个点云组成, 每个点云的原点
  std::vector<RangeMeasurement> ranges; // 数据点的集合
};

注意，time 表示此点云最后一个点的时间

如果，没有使用imu的话，启动位姿估计器，位姿估计器后续再说

1
2
3
4
5
6
const common::Time& time = synchronized_data.time;
// Initialize extrapolator now if we do not ever use an IMU.
// 如果不用imu, 就在雷达这初始化位姿推测器
if (!options_.use_imu_data()) {
  InitializeExtrapolator(time);
}

整个for循环 利用位姿估计器，估计每一个点的位姿，放入range_data_poses

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
for (const auto& range : synchronized_data.ranges) {
  common::Time time_point = time + common::FromSeconds(range.point_time.time);
  // 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错
  if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
    // 一个循环只报一次错
    if (!warned) {
      LOG(ERROR)
          << "Timestamp of individual range data point jumps backwards from "
          << extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
      warned = true;
    }
    time_point = extrapolator_->GetLastExtrapolatedTime();
  }
  
  // Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
  range_data_poses.push_back(
      extrapolator_->ExtrapolatePose(time_point).cast<float>());
}

运动畸变

下面是运动畸变的去除：

运动畸变产生的原因，雷达转动一圈的过程中，始终已发出第一针的位置为坐标系，因此在运动的过程中测到的雷达距离会不准，比如墙体越来越近，那么画到了原来的坐标系上就会产生一条斜的墙面，见下图

image-20211108003834642

那么只要我们得到每一个点所机器人对应的移动距离，（即我们用位姿估计器得到的值），用这个移动距离把测到的值映射到原来的坐标系中，就可以较为准确的值

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
35
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
// 对每个数据点进行处理
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
  // 获取在tracking frame 下点的坐标
  const sensor::TimedRangefinderPoint& hit =
      synchronized_data.ranges[i].point_time;
  // 将点云的origins坐标转到 local slam 坐标系下
  const Eigen::Vector3f origin_in_local =
      range_data_poses[i] *
      synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
  
  // Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
  sensor::RangefinderPoint hit_in_local =
      range_data_poses[i] * sensor::ToRangefinderPoint(hit); //此函数返回点的位置
  
  // 计算这个点的距离, 这里用的是去畸变之后的点的距离
  const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
  const float range = delta.norm();
  
  // param: min_range max_range
  if (range >= options_.min_range()) {
    if (range <= options_.max_range()) {
      // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
      accumulated_range_data_.returns.push_back(hit_in_local);
    } else {
      // Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
      hit_in_local.position =
          origin_in_local +
          // param: missing_data_ray_length, 是个比例, 不是距离
          options_.missing_data_ray_length() / range * delta;
      accumulated_range_data_.misses.push_back(hit_in_local);
    }
  }
} // end for

关于accumulatedrange_data的数据结构，他保存的是最后的转到local slam坐标系下的值

1
2
3
4
5
6
7
8
9
10
11
12
/**
 * @brief local_slam_data中存储所有雷达点云的数据结构
 * 
 * @param origin  点云的原点在local坐标系下的坐标
 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit
 * @param misses  是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点
 */
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses; // local坐标系下的坐标
};

开始匹配

此时，我们就有了一帧有效的数据

1
2
// 有一帧有效的数据了
++num_accumulated_;

当有效的数据达到配置文件设置的大小，则开始一次匹配，匹配前，获取机器人当前姿态，最后一个点

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
if (num_accumulated_ >= options_.num_accumulated_range_data()) {
    // 计算2次有效点云数据的的时间差
    const common::Time current_sensor_time = synchronized_data.time;
    absl::optional<common::Duration> sensor_duration;
    if (last_sensor_time_.has_value()) {
      sensor_duration = current_sensor_time - last_sensor_time_.value();
    }
    last_sensor_time_ = current_sensor_time;

    // 重置变量
    num_accumulated_ = 0;

    // 获取机器人当前姿态
    const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
        extrapolator_->EstimateGravityOrientation(time));

    // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time
    // 'time'.
    // 以最后一个点的时间戳估计出的坐标为这帧数据的原点
    // range_data_poses为保存着每一个时间点的位姿
    accumulated_range_data_.origin = range_data_poses.back().translation();
    
    return AddAccumulatedRangeData(
        time,
        // 将点云变换到local原点处, 且姿态为0
        TransformToGravityAlignedFrameAndFilter(
            gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
            accumulated_range_data_),
        gravity_alignment, sensor_duration);
  }

  return nullptr;
 Comments