坐标变换和点云滤波-cartographer
Kong Liangqian Lv6
 2021-11-08 18:38:29  

之前说到雷达会经过时间同步，和畸变校准，并且最后来到了匹配

1
2
3
4
5
6
7
8
9
10
11
12
   // 获取机器人当前姿态
   const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation(
       extrapolator_->EstimateGravityOrientation(time));
accumulated_range_data_.origin = range_data_poses.back().translation();
return AddAccumulatedRangeData(
       time,
       // 将点云变换到local原点处, 且姿态为0
       TransformToGravityAlignedFrameAndFilter(
           // range_data_poses保存各时间机器人的位姿
           gravity_alignment.cast<float>() * range_data_poses.back().inverse(),
           accumulated_range_data_),
       gravity_alignment, sensor_duration);

在进行匹配之前，还是需要介绍一下TransformToGravityAlignedFrameAndFilter

TransformToGravityAlignedFrameAndFilter

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
/**
 * @brief 先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量
 * 
 * @param[in] transform_to_gravity_aligned_frame 将点云变换到原点处, 且姿态为0的坐标变换
 * @param[in] range_data 传入的点云
 * @return sensor::RangeData 处理后的点云 拷贝
 */
sensor::RangeData
LocalTrajectoryBuilder2D::TransformToGravityAlignedFrameAndFilter(
    const transform::Rigid3f& transform_to_gravity_aligned_frame,
    const sensor::RangeData& range_data) const {
  // Step: 5 将原点位于机器人当前位姿处的点云 转成 原点位于local坐标系原点处的点云, 再进行z轴上的过滤
  const sensor::RangeData cropped =
      sensor::CropRangeData(sensor::TransformRangeData(
                                range_data, transform_to_gravity_aligned_frame),
                            options_.min_z(), options_.max_z()); // param: min_z max_z
  // Step: 6 对点云进行体素滤波
  return sensor::RangeData{
      cropped.origin,
      sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
      sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}

TransformRangeData

此函数接受数据和变换，RangeData 包含一个Vector3f的数据 origin，直接乘以transform即可，下面的函数同样的道理，都是调用里面的 数据，用transform * data 进行变换

1
2
3
4
5
6
7
8
9
// 根据给定的坐标变换, 分别对origin, returns, misses做变换, 拷贝
RangeData TransformRangeData(const RangeData& range_data,
                             const transform::Rigid3f& transform) {
  return RangeData{
      transform * range_data.origin,
      TransformPointCloud(range_data.returns, transform), // 拷贝
      TransformPointCloud(range_data.misses, transform),  // 拷贝
  };
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
/**
 * @brief 对输入的点云做坐标变换
 * 
 * @param[in] point_cloud 输入的点云
 * @param[in] transform 坐标变换
 * @return PointCloud 变换之后的点云
 */
PointCloud TransformPointCloud(const PointCloud& point_cloud,
                               const transform::Rigid3f& transform) {
  std::vector<RangefinderPoint> points;
  points.reserve(point_cloud.size());
  for (const RangefinderPoint& point : point_cloud.points()) {
    points.emplace_back(transform * point);
  }
  return PointCloud(points, point_cloud.intensities());
}

CropRangeData

此函数接受一个变换完之后的点云，并且还有一个z轴的最小和最大值，在此范围之外的数据全部裁剪掉

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/**
 * @brief 对输入的点云进行z轴上的过滤
 * 
 * @param[in] range_data 原始点云数据
 * @param[in] min_z 最小的z坐标
 * @param[in] max_z 最大的z坐标
 * @return RangeData 裁剪之后的点云 拷贝
 */
RangeData CropRangeData(const RangeData& range_data, const float min_z,
                        const float max_z) {
  return RangeData{range_data.origin,
                   CropPointCloud(range_data.returns, min_z, max_z),  // 拷贝
                   CropPointCloud(range_data.misses, min_z, max_z)};  // 拷贝
}

体素滤波

1
2
3
4
5
6
std::vector<RangefinderPoint> VoxelFilter(
    const std::vector<RangefinderPoint>& points, const float resolution) {
  return RandomizedVoxelFilter(
      points, resolution,
      [](const RangefinderPoint& point) { return point.position; });
}

再说

 Comments