devbox
你好赵伟
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Cartographer源码阅读---点云数据的预处理_cartographer 滤波

作者：你好赵伟 | 2024-07-24 21:35:34

赞

踩

cartographer 滤波

cartographer提供了多激光雷达的传感器接口, 为了实现多激光雷达数据的融合,cartographer进行了基于时间戳的多激光雷达传感器的融合, 又对激光雷达固有的畸变进行处理. 这么多的点云点必然会消耗很多算力,而且不需要这么多的点云点,于是cartographer对点云进行了数据的滤波. 这些操作听起来很简单,但是cartographer工程化的代码却比较难懂,特别是点云数据处理过程中涉及到的坐标转换.

上一节我们已经看到了, 传感器数据是通过CollatedTrajectoryBuilder类的HandleCollatedSensorData函数 传递给 GlobalTrajectoryBuilder类的相应函数. 从GlobalTrajectoryBuilder开始, 传感器数据才真正进入到Cartographer的前后端. Cartographer最重要的数据类型就是点云, 所以这节我们看看点云这个传感器数据类型在传入Cartographer之前, 都做了什么处理.

  1. GlobalTrajectoryBuilder类

上一节已经说过, GlobalTrajectoryBuilder是LocalTrajectoryBuilder与PoseGraph的结合, 这两个类都是继承于TrajectoryBuilderInterface.

GlobalTrajectoryBuilder与LocalTrajectoryBuilder联系是建立在AddSensorData上面

  1.   /**
  2.    * @brief 点云数据的处理, 先进行扫描匹配, 然后将扫描匹配的结果当做节点插入到后端的位姿图中
  3.    * 
  4.    * @param[in] sensor_id topic名字
  5.    * @param[in] timed_point_cloud_data 点云数据
  6.    */
  7.   void AddSensorData(
  8.       const std::string& sensor_id,
  9.       const sensor::TimedPointCloudData& timed_point_cloud_data) override {
  10.     CHECK(local_trajectory_builder_)
  11.         << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
  12.  
  13.     // 进行前端(扫描匹配), 返回匹配后的结果
  14.     std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> //local_trajectory_builder_Xd.h中定义MatchingResult
  15.         matching_result = local_trajectory_builder_->AddRangeData(
  16.             sensor_id, timed_point_cloud_data);
  17.     ......

GlobalTrajectoryBuilder的AddSensorData通过调用LocalTrajectoryBuilder的AddXxxData成员函数, 把传感器数据传递给LocalTrajectoryBuilder, 实现前端. 接下来我们看看点云数据传入之前做了哪些处理. 咱们进到LocalTrajectoryBuilder2D的AddRangeData中去看看

  1. /**
  2.  * @brief 处理点云数据, 进行扫描匹配, 将点云写成地图
  3.  * 
  4.  * @param[in] sensor_id 点云数据对应的话题名称
  5.  * @param[in] unsynchronized_data 传入的点云数据
  6.  * @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> 匹配后的结果
  7.  */
  8. std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
  9. LocalTrajectoryBuilder2D::AddRangeData(
  10.     const std::string& sensor_id,
  11.     const sensor::TimedPointCloudData& unsynchronized_data)
  12. {
  13.     ......
  14. }

  1. 多激光雷达数据时间同步与融合

首先,在local_trajectory_builder_2d.cc中,AddRangeData这个函数的第一步,也就是range_data_collator_.AddRangeData, 这个函数做到了多雷达的点云时间同步.

  1.   // Step: 1 进行多个雷达点云数据的时间同步, 点云的坐标是相对于tracking_frame的
  2.   auto synchronized_data =
  3.       range_data_collator_.AddRangeData(sensor_id, unsynchronized_data);
  4.   if (synchronized_data.ranges.empty()) {
  5.     LOG(INFO) << "Range data collator filling buffer.";
  6.     return nullptr;
  7.   }

咱们看看range_data_collator_.AddRangeData这块. range_data_collator_是RangeDataCollator的实例化, 进到RangeDataCollator去看看AddRangeData成员函数. 这个函数的作用就是将多个雷达数据的时间进行同步

  1. /**
  2.  * @brief 多个雷达数据的时间同步
  3.  * 
  4.  * @param[in] sensor_id 雷达数据的话题
  5.  * @param[in] timed_point_cloud_data 雷达数据
  6.  * @return sensor::TimedPointCloudOriginData 根据时间处理之后的数据
  7.  */
  8. sensor::TimedPointCloudOriginData RangeDataCollator::AddRangeData(
  9.     const std::string& sensor_id,
  10.     sensor::TimedPointCloudData timed_point_cloud_data)
  11. {
  12.     ...
  13. }

这个函数里面判断了两种电影情况:

1. 如果当前点云还有没处理的, 就同步没处理的点云,然后将当前点云保存

  1.   if (id_to_pending_data_.count(sensor_id) != 0) {
  2.     // current_end_为上一次时间同步的结束时间
  3.     // current_start_为本次时间同步的开始时间
  4.     current_start_ = current_end_;
  5.     // When we have two messages of the same sensor, move forward the older of
  6.     // the two (do not send out current).
  7.     // 本次时间同步的结束时间为这帧点云数据的结束时间
  8.     current_end_ = id_to_pending_data_.at(sensor_id).time;
  9.     auto result = CropAndMerge();
  10.     // 保存当前点云
  11.     id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
  12.     return result;
  13.   }

2. 当前点云处理完了,处理下一个点云

  1. // 先将当前点云添加到 等待时间同步的map中
  2.   id_to_pending_data_.emplace(sensor_id, std::move(timed_point_cloud_data));
  3.  
  4.   // 等到range数据的话题都到来之后再进行处理
  5.   if (expected_sensor_ids_.size() != id_to_pending_data_.size()) {
  6.     return {};
  7.   }
  8.  
  9.   current_start_ = current_end_;
  10.   // We have messages from all sensors, move forward to oldest.
  11.   common::Time oldest_timestamp = common::Time::max(); //最大时间9999999年
  12.   // 找到所有传感器数据中最早的时间戳(点云最后一个点的时间)
  13.   for (const auto& pair : id_to_pending_data_) {
  14.     oldest_timestamp = std::min(oldest_timestamp, pair.second.time);
  15.   }
  16.   // current_end_是本次时间同步的结束时间
  17.   // 是待时间同步map中的 所有点云中最早的时间戳
  18.   current_end_ = oldest_timestamp;
  19.   return CropAndMerge();

这两个情况的区别就只有current_end_, 也就是本次时间同步结束的时间不一样. 之前提到过, 每次scan的时间戳是每次scan的最后一个点的时间戳. 于是处理点云的时候,current_end_是待时间同步的点云中最早的点云的最后一个点的时间戳. 而current_start_是上一次处理点云的current_end_时间.

在激光雷达的同步融合的部分, 最重要的函数是CropAndMerge()这个函数, 顾名思义,就是把不同的点云裁剪合并,

CropAndMerge()

  1.     // 找到点云中 最后一个时间戳小于current_start_的点的索引
  2.     auto overlap_begin = ranges.begin();
  3.     while (overlap_begin < ranges.end() &&
  4.            data.time + common::FromSeconds((*overlap_begin).time) <
  5.                current_start_) {
  6.       ++overlap_begin;
  7.     }
  8.  
  9.     // 找到点云中 最后一个时间戳小于等于current_end_的点的索引
  10.     auto overlap_end = overlap_begin;
  11.     while (overlap_end < ranges.end() &&
  12.            data.time + common::FromSeconds((*overlap_end).time) <=
  13.                current_end_) {
  14.       ++overlap_end;
  15.     }

这一块是找到最后一个小于current_start_和current_end_的点云的时间, current_start_是上一次的的current_end_的时间戳, current_end_是当前未处理点云中最早一帧点云的最后一个点的时间戳. 上面这些代码作用是找到current_start_和current_end_之间所有雷达的点的索引,用于限制范围.

  1.     // 丢弃点云中时间比起始时间早的点, 每执行一下CropAndMerge()打印一次log, 一般不会触发
  2.     if (ranges.begin() < overlap_begin && !warned_for_dropped_points) {
  3.       LOG(WARNING) << "Dropped " << std::distance(ranges.begin(), overlap_begin)
  4.                    << " earlier points.";
  5.       warned_for_dropped_points = true;
  6.     }

上面的代码一般不会触发,除非雷达自己的时间戳就有问题

  1.     // Copy overlapping range.
  2.     if (overlap_begin < overlap_end) {
  3.       // 获取下个点云的index, 即当前vector的个数
  4.       std::size_t origin_index = result.origins.size();
  5.       result.origins.push_back(data.origin);  // 插入原点坐标
  6.  
  7.       // 获取此传感器时间与集合时间戳的误差, 
  8.       const float time_correction =
  9.           static_cast<float>(common::ToSeconds(data.time - current_end_));
  10.  
  11.       auto intensities_overlap_it =
  12.           intensities.begin() + (overlap_begin - ranges.begin());
  13.       // reserve() 在预留空间改变时, 会将之前的数据拷贝到新的内存中
  14.       result.ranges.reserve(result.ranges.size() +
  15.                             std::distance(overlap_begin, overlap_end));
  16.       
  17.       // 填充数据
  18.       for (auto overlap_it = overlap_begin; overlap_it != overlap_end;
  19.            ++overlap_it, ++intensities_overlap_it) {
  20.         sensor::TimedPointCloudOriginData::RangeMeasurement point{
  21.             *overlap_it, *intensities_overlap_it, origin_index};
  22.         // current_end_ + point_time[3]_after == in_timestamp +
  23.         // point_time[3]_before
  24.         // 针对每个点时间戳进行修正, 让最后一个点的时间为0
  25.         point.point_time.time += time_correction;  
  26.         result.ranges.push_back(point);
  27.       } // end for
  28.     } // end if (overlap_begin < overlap_end)

上面代码是CropAndMerge()的核心,通过for循环,遍历每一个在start和end之间的点云点,重新生成对应的cartographer的点云点. time_correction是data.time - current_end_的结果, current_end_是待时间同步map中,所有点云最早的时间戳, data.time是点云中最后一个点的时间戳, data.time - current_end_结果是正的. 而之前说过, 点云的时间戳都是负的,通过point.point_time.time += time_correction让最后一个点的时间戳为0,前面的点的时间戳为负数. 

  1.   std::sort(result.ranges.begin(), result.ranges.end(),
  2.             [](const sensor::TimedPointCloudOriginData::RangeMeasurement& a,
  3.                const sensor::TimedPointCloudOriginData::RangeMeasurement& b) {
  4.               return a.point_time.time < b.point_time.time;
  5.             });

然后通过sort函数把点云按照时间从小到大排列起来,再返回出去. 这样就得到了融合之后,时间戳归一化后的的一帧点云.

下面补一张图, 更好的说明一下.

当上一次扫描结束后,current_end_在current end0处,然后currentend0变为currentstart1,然后在两个scan中找到未处理的scan的最早的一个时间戳, 也就是scan2的第一帧scan的末尾时间, 作为current_start_,也就是current_end1处,然后进行CropAndMerge. 然后再把currentend1变为current start2 再找到未处理的scan的最早的一个时间戳,也就是scan1的第二帧,作为current_end2, 再进行CropAndMerge,然后current_end2变为current_start3,在找到current_end4,继续这个步骤,就完成了两个scan的合并.

  1. 点云数据的畸变矫正

cartographer的点云畸变矫正是通过位姿推测器(pose_extrapolator)和点云中每个点的时间戳, 去估计一帧点云中的点去畸变后的位子. 位姿推测器可以估计出每时每刻当前机器人的线速度和角速度,然后得到每时的位姿变化,通过点云的每个点时间戳,然后对每个点的相于原点的位子进行矫正.

直接看程序,在local_trajectory_builder_2d.cc的AddRangeData中. 我们上一小节已经看过了多雷达点云的时间同步了,这个AddRangeData这个函数的第一步,第二步就是点云的畸变矫正.

在真正进入到点云的畸变矫正之前, 还判断了是否有IMU数据,如果没有的话还初始化了位姿推测器(位姿推测器在cartographer中是很重要的,这个以后再说)

  1.   if (!options_.use_imu_data()) {
  2.     InitializeExtrapolator(time);
  3.   }

还有关于时间相关的处理: 得到点云第一个点的时间戳

  1.   const common::Time& time = synchronized_data.time;
  2. ...
  3.   CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);
  4.   const common::Time time_first_point =
  5.       time +
  6.       common::FromSeconds(synchronized_data.ranges.front().point_time.time);

time是点云融合之后的时间戳, 是最后一个点的绝对时间戳,是正数

synchronized_data.ranges.front().point_time.time是点云点相对于点云的相对时间戳,也就是某个点云点相对于最后最后一个点云点的时间差,是负数,所以要做个CHECK_LE判断是不是小于等于0.0 所以这两个一相加就是第一个点云点的绝对时间戳, 是正数.

然后预测每个时间点下机器人的位姿

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

extrapolator_->ExtrapolatePose(time_point).cast<float>())通过位姿推测器,预测出每个点云点在他的时间戳时刻下机器人在local 坐标系下的位姿.

然后进行运动畸变的去除

  1.   for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
  2.     // 获取在tracking frame 下点的坐标
  3.     const sensor::TimedRangefinderPoint& hit =
  4.         synchronized_data.ranges[i].point_time;
  5.     // 将点云的origins坐标转到 local slam 坐标系下
  6.     const Eigen::Vector3f origin_in_local =
  7.         range_data_poses[i] *
  8.         synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
  9.     
  10.     // 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
  11.     sensor::RangefinderPoint hit_in_local =
  12.         range_data_poses[i] * sensor::ToRangefinderPoint(hit);
  13.     
  14.     // 计算这个点的距离, 这里用的是去畸变之后的点的距离
  15.     const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
  16.     const float range = delta.norm();
  17.     
  18.     // param: min_range max_range
  19.     if (range >= options_.min_range()) {
  20.       if (range <= options_.max_range()) {
  21.         // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
  22.         accumulated_range_data_.returns.push_back(hit_in_local);
  23.       } else {
  24.         // 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
  25.         hit_in_local.position =
  26.             origin_in_local +
  27.             // param: missing_data_ray_length, 是个比例, 不是距离
  28.             options_.missing_data_ray_length() / range * delta;
  29.         accumulated_range_data_.misses.push_back(hit_in_local);
  30.       }
  31.     }
  32.   } // end for

上面的函数是对每个点云点进行处理, 步骤如下

  1. 获取现在点云距离的原始数据(变量为hit)

  1. 讲点云的原始距离数据转化为local坐标系下的数据(origin_in_local), 此时只是坐标的转换, 并没有去畸变.

  1. 真正的去畸变(hit_in_local), 方法是将原始数据转化为local坐标系下的坐标, range_data_poses是通过位姿推测器预测出的点的时间戳

  1. 计算去畸变点云的距离,并且用配置参数的最大最小距离去进行数据的过滤, 对距离合适的数据放在returns这个数组里, 对于距离不合适的放在misses这个数组里.

上面的这些步骤就把点云的畸变去除了, 并且把点云点的距离重原始数据的坐标系转化到local坐标系下了.

  1. Z轴过滤

在TransformToGravityAlignedFrameAndFilter里面,调用了sensor的CropRangeData

  1. sensor::CropRangeData(sensor::TransformRangeData(range_data,
  2. transform_to_gravity_aligned_frame), options_.min_z(), options_.max_z())

这个函数主要是针对多线雷达的, 去过滤掉打在地面上的雷达数据. 通过重力把位姿估计出来, 然后就能知道哪些点云点是打在地上的, 地上的雷达点的Z轴数据都是负数的. 这里我没用多线雷达没有深入研究.

值得注意的是, 如果用单线雷达在配置文件里不能设置大于0 的min_z, 因为单线雷达的z就是0, 这样就会过滤所有雷达数据.

  1. 点云的体素滤波与后处理

体素滤波就是在规定体积大小的小方块里, 用一个点云去代表这个小方块里所有的点云, 这个点云是用这个方块内所有点云的质心得到的. 但cartographer不是这么做的, 他通过随机数去随机找格子里的点以减小计算量,代码在voxel_filter.cc中, 很简单不细说了

体素滤波的使用的代码依然在TransformToGravityAlignedFrameAndFilter里面

  1.   return sensor::RangeData{
  2.       cropped.origin,
  3.       sensor::VoxelFilter(cropped.returns, options_.voxel_filter_size()), // param: voxel_filter_size
  4.       sensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};

核心函数是sensor::VoxelFilter, 对hit和miss的点云都进行滤波

然后这个返回值放到了AddAccumulatedRangeData函数中, 这个AddAccumulatedRangeData函数通过对齐位姿到0去转3D到2D,又调用自适应体素滤波,得到一个点数较少的点云. 

  1.   const sensor::PointCloud& filtered_gravity_aligned_point_cloud =
  2.       sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,
  3.                                   options_.adaptive_voxel_filter_options());

然后把这个点云与submap进行匹配(这个之后再说)

  1.   std::unique_ptr<transform::Rigid2d> pose_estimate_2d =
  2.       ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);

此时点云都是围绕着local坐标系而不是当前机器人位姿下的坐标系, 所以我们又要转回去:

  1.   sensor::RangeData range_data_in_local =
  2.       TransformRangeData(gravity_aligned_range_data,
  3.                          transform::Embed3D(pose_estimate_2d->cast<float>()));

再将矫正后的雷达数据写入submap

  1.   std::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(
  2.       time, range_data_in_local, filtered_gravity_aligned_point_cloud,
  3.       pose_estimate, gravity_alignment.rotation())

InsertIntoSubmap这个函数可以判断是不是真的需要更新地图, 如果移动距离过小或者时间过短都不会更新:

  1.   if (motion_filter_.IsSimilar(time, pose_estimate)) {
  2.     return nullptr;
  3.   }

这里的pose_estimate是扫描匹配后的三维位姿(类型为Rigid3d), 而不是二维的, 这和单线雷达的子图是平面图不太一样.

然后,量将点云的数据写入submap中

  1.   std::vector<std::shared_ptr<const Submap2D>> insertion_submaps =
  2.       active_submaps_.InsertRangeData(range_data_in_local);

再将整个点云数据与时间戳等打包成InsertionResult格式的数据进行返回

  1.   return absl::make_unique<InsertionResult>(InsertionResult{
  2.       std::make_shared<const TrajectoryNode::Data>(TrajectoryNode::Data{
  3.           time,
  4.           gravity_alignment,
  5.           filtered_gravity_aligned_point_cloud, 
  6.           {},  // 'high_resolution_point_cloud' is only used in 3D.
  7.           {},  // 'low_resolution_point_cloud' is only used in 3D.
  8.           {},  // 'rotational_scan_matcher_histogram' is only used in 3D.
  9.           pose_estimate}),
  10.       std::move(insertion_submaps)});

先看看InsertionResult这个结构体, 这个是点云插入到地图后的结果

  1.   struct InsertionResult {
  2.     std::shared_ptr<const TrajectoryNode::Data> constant_data;
  3.     std::vector<std::shared_ptr<const Submap2D>> insertion_submaps; // 最多只有2个子图的指针
  4.   };

insertion_submaps就是要插入的子图, 结构类型为Submap2D指针, 存在vector中, 由于之前讲过子图会保留正在新建的一个和已经建好的一个, 所以这个vector一般是两个Submap的指针

再看看TrajectoryNode::Data, 结构体定义在trajectory_node.h中, 

  1.   struct Data {
  2.     common::Time time;
  3.  
  4.     // Transform to approximately gravity align the tracking frame as
  5.     // determined by local SLAM.
  6.     Eigen::Quaterniond gravity_alignment;
  7.  
  8.     // Used for loop closure in 2D: voxel filtered returns in the
  9.     // 'gravity_alignment' frame.
  10.     sensor::PointCloud filtered_gravity_aligned_point_cloud;
  11.  
  12.     // Used for loop closure in 3D.
  13.     sensor::PointCloud high_resolution_point_cloud;
  14.     sensor::PointCloud low_resolution_point_cloud;
  15.     Eigen::VectorXf rotational_scan_matcher_histogram;
  16.  
  17.     // The node pose in the local SLAM frame.
  18.     transform::Rigid3d local_pose;
  19.   };

到这里肯定大家一辆懵逼, Data里面的参数gravity_alignment, filtered_gravity_aligned_point_cloud等到底是啥.

gravity_alignment: 实际上这个参数是gravity_alignment.rotation(), 用来把点云旋转到和重力水平, 如果没有IMU的话还没看是咋回事, 待补充...

filtered_gravity_aligned_point_cloud: 这里存的是体素滤波后的点云, 为了2D闭环用的

{}*3 : 为了3D闭环用的, 没研究

pose_estimate: local坐标系下的节点位姿, 是3D位姿

最后在通过std::move(insertion_submaps)把点云数据放入InsertionResult, 然后返回整个InsertionResult.

额外说一下自适应体素滤波AdaptivelyVoxelFiltered. 这个函数实现在voxel_filter.cc中

传入参数只有选项option和点云point_cloud. 然后分四个步骤进行

1.如果点云已经很稀疏了，直接返回。

2.以max_size为间距，稀疏提取point，调用VoxelFilter。

3.使用max_length过滤，大于就返回

4.没有超过，则继续二分法滤波，使得以low_length为间距。

  1. 点云坐标转换总结

上面参数传递, 函数调用一层一层的, 一开始看很难看懂, 真的需要自己仔仔细细捋一捋, 在这简单做个总结, 总结一下点云数据预处理中坐标的转换

第一步: 进行多个雷达点云的时间同步, 此时点云的坐标是相对于tracking_frame的

初始点云坐标为(10, 8)

第二步: 预测出每个点云的时间戳, 通过位姿推测器推测出tracking frame 在local坐标系下的坐标.

第三步: 去除运动畸变, 方法是将tracking_frame的hit坐标转化成local坐标系下的坐标

sensor::RangefinderPoint hit_in_local = range_data_poses[i] * sensor::ToRangefinderPoint(hit);

此时在tracking_frame下的坐标为(10, 8)的点转换到了local_frame坐标下, 为(200, -80)了, 此时点云还在围绕着tracking_frame而不是local_frame, 但坐标原点从tracking_frame转换到了local_frame上.

第四步: 对于超距离范围的点云, 用一个固定距离代替, 并放在misses里面.

那么, 此时我们用啥时候的时间戳下的坐标系当成点云的origin坐标呢?是用点云的最后一个点的时间预测出来的坐标作为点云的origin坐标: 

accumulated_range_data_.origin = range_data_poses.back().translation()

第五步: 将点云从围绕着当前位姿(tracking 坐标系)转到local坐标系下

gravity_alignment.cast<float>() * range_data_poses.back().inverse()

上面这个式子得到了tracking到local的转换矩阵,

然后通过point_cloud.cc的函数TransformPointCloud把每个点云转化到了围绕local坐标系下的点云

  1. PointCloud TransformPointCloud(const PointCloud& point_cloud,
  2.                                const transform::Rigid3f& transform) {
  3.   std::vector<RangefinderPoint> points;
  4.   points.reserve(point_cloud.size());
  5.   for (const RangefinderPoint& point : point_cloud.points()) {
  6.     points.emplace_back(transform * point);
  7.   }
  8.   return PointCloud(points, point_cloud.intensities());
  9. }

此时这一帧的所有点云都围绕在了local_frame下

第六步: 进行点云的z轴过滤与体素滤波等处理

第七步: 扫描匹配(前端)

第八步: 将原点位于local坐标系处的点云再转变为原点位于匹配后的位姿处的点云, 也就是又变回去了

  1.   sensor::RangeData range_data_in_local =
  2.       TransformRangeData(gravity_aligned_range_data,
  3.                          transform::Embed3D(pose_estimate_2d->cast<float>()));

现在有个疑问, 把点云转化到local坐标系下咋进行前端匹配呢?这个后面再说.

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/你好赵伟/article/detail/876828
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号