cartographer（8）点云匹配

1.处理点云数据LocalTrajectoryBuilder2D::AddRangeData

函数实现的功能：
1：依据分发出来的点云中每个点的时间（time），使用位姿推断器推断time时刻tracking坐标系的位姿。
2：依据1中推断出来的位姿将点云中的点转换到local坐标系下。
3：将传入的点云的origins坐标转到 local slam 坐标系下，

做运动畸变的去除相对于tracking_frame的hit坐标转成 local坐标系下的坐标

两点相减得到距离在激光有效范围内的点保存起来。（此时此刻点云中的点到其对应的原点距离，距离超过最大值（传感器最大值），要做特别的处理）。
4：将3得到点云做自适应体素滤波，并做匹配。
5：根据匹配结果将点云写入submap
6：把匹配得到的位姿加入位姿推断器


/*** @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);if (synchronized_data.ranges.empty()) {LOG(INFO) << "Range data collator filling buffer.";return nullptr;}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);}if (extrapolator_ == nullptr) {// Until we've initialized the extrapolator with our first IMU message, we// cannot compute the orientation of the rangefinder.LOG(INFO) << "Extrapolator not yet initialized.";return nullptr;}CHECK(!synchronized_data.ranges.empty());// TODO(gaschler): Check if this can strictly be 0.CHECK_LE(synchronized_data.ranges.back().point_time.time, 0.f);// 计算第一个点的时间const common::Time time_first_point =time +common::FromSeconds(synchronized_data.ranges.front().point_time.time);// 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()if (time_first_point < extrapolator_->GetLastPoseTime()) {LOG(INFO) << "Extrapolator is still initializing.";return nullptr;}std::vector<transform::Rigid3f> range_data_poses;range_data_poses.reserve(synchronized_data.ranges.size());bool warned = false;// 预测得到每一个时间点的位姿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>());}if (num_accumulated_ == 0) {// 'accumulated_range_data_.origin' is uninitialized until the last// accumulation.accumulated_range_data_ = sensor::RangeData{{}, {}, {}};}// 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_rangeif (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// 有一帧有效的数据了++num_accumulated_;// param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配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'.// 以最后一个点的时间戳估计出的坐标为这帧数据的原点accumulated_range_data_.origin = range_data_poses.back().translation();return AddAccumulatedRangeData(time,// 将点云变换到local原点处, 且姿态为0TransformToGravityAlignedFrameAndFilter(gravity_alignment.cast<float>() * range_data_poses.back().inverse(),accumulated_range_data_),gravity_alignment, sensor_duration);}return nullptr;
}

预测出每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿

extrapolator_->ExtrapolatePose(time_point).cast<float>());

// 预测得到time时刻 tracking frame 在 local 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {const TimedPose& newest_timed_pose = timed_pose_queue_.back();CHECK_GE(time, newest_timed_pose.time);// 如果本次预测时间与上次计算时间相同 就不再重复计算if (cached_extrapolated_pose_.time != time) {// 预测tracking frame在local坐标系下time时刻的位置const Eigen::Vector3d translation =ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();// 预测tracking frame在local坐标系下time时刻的姿态const Eigen::Quaterniond rotation =newest_timed_pose.pose.rotation() *ExtrapolateRotation(time, extrapolation_imu_tracker_.get());cached_extrapolated_pose_ =TimedPose{time, transform::Rigid3d{translation, rotation}};}return cached_extrapolated_pose_.pose;
}

先进行点云的旋转与z方向的滤波, 然后再进行体素滤波减少数据量

/*** @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_sizesensor::VoxelFilter(cropped.misses, options_.voxel_filter_size())};
}

进行扫描匹配, 将点云写入地图


/*** @brief 进行扫描匹配, 将点云写入地图* * @param[in] time 点云的时间戳* @param[in] gravity_aligned_range_data 原点位于local坐标系原点处的点云* @param[in] gravity_alignment 机器人当前姿态* @param[in] sensor_duration 2帧点云数据的时间差* @return std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult> */
std::unique_ptr<LocalTrajectoryBuilder2D::MatchingResult>
LocalTrajectoryBuilder2D::AddAccumulatedRangeData(const common::Time time,const sensor::RangeData& gravity_aligned_range_data,const transform::Rigid3d& gravity_alignment,const absl::optional<common::Duration>& sensor_duration) {// 如果处理完点云之后数据为空, 就报错. 使用单线雷达时不要设置min_zif (gravity_aligned_range_data.returns.empty()) {LOG(WARNING) << "Dropped empty horizontal range data.";return nullptr;}// Computes a gravity aligned pose prediction.// 进行位姿的预测, 先验位姿const transform::Rigid3d non_gravity_aligned_pose_prediction =extrapolator_->ExtrapolatePose(time);// 将三维位姿先旋转到姿态为0, 再取xy坐标将三维位姿转成二维位姿const transform::Rigid2d pose_prediction = transform::Project2D(non_gravity_aligned_pose_prediction * gravity_alignment.inverse());// Step: 7 对 returns点云 进行自适应体素滤波，返回的点云的数据类型是PointCloudconst sensor::PointCloud& filtered_gravity_aligned_point_cloud =sensor::AdaptiveVoxelFilter(gravity_aligned_range_data.returns,options_.adaptive_voxel_filter_options());if (filtered_gravity_aligned_point_cloud.empty()) {return nullptr;}// local map frame <- gravity-aligned frame// 扫描匹配, 进行点云与submap的匹配std::unique_ptr<transform::Rigid2d> pose_estimate_2d =ScanMatch(time, pose_prediction, filtered_gravity_aligned_point_cloud);if (pose_estimate_2d == nullptr) {LOG(WARNING) << "Scan matching failed.";return nullptr;}// 将二维坐标旋转回之前的姿态const transform::Rigid3d pose_estimate =transform::Embed3D(*pose_estimate_2d) * gravity_alignment;// 校准位姿估计器extrapolator_->AddPose(time, pose_estimate);// Step: 8 将 原点位于local坐标系原点处的点云 变换成 原点位于匹配后的位姿处的点云sensor::RangeData range_data_in_local =TransformRangeData(gravity_aligned_range_data,transform::Embed3D(pose_estimate_2d->cast<float>()));// 将校正后的雷达数据写入submapstd::unique_ptr<InsertionResult> insertion_result = InsertIntoSubmap(time, range_data_in_local, filtered_gravity_aligned_point_cloud,pose_estimate, gravity_alignment.rotation());// 计算耗时const auto wall_time = std::chrono::steady_clock::now();if (last_wall_time_.has_value()) {const auto wall_time_duration = wall_time - last_wall_time_.value();kLocalSlamLatencyMetric->Set(common::ToSeconds(wall_time_duration));if (sensor_duration.has_value()) {kLocalSlamRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /common::ToSeconds(wall_time_duration));}}// 计算cpu耗时const double thread_cpu_time_seconds = common::GetThreadCpuTimeSeconds();if (last_thread_cpu_time_seconds_.has_value()) {const double thread_cpu_duration_seconds =thread_cpu_time_seconds - last_thread_cpu_time_seconds_.value();if (sensor_duration.has_value()) {kLocalSlamCpuRealTimeRatio->Set(common::ToSeconds(sensor_duration.value()) /thread_cpu_duration_seconds);}}last_wall_time_ = wall_time;last_thread_cpu_time_seconds_ = thread_cpu_time_seconds;// 返回结果 return absl::make_unique<MatchingResult>(MatchingResult{time, pose_estimate, std::move(range_data_in_local),std::move(insertion_result)});
}

cartographer（8）点云匹配相关推荐

ICP算法进行点云匹配
[原文:http://www.cnblogs.com/yhlx125/p/5234156.html] 上一篇:http://www.cnblogs.com/yhlx125/p/4924283.html ...
lio-sam框架：点云匹配之手写高斯牛顿下降优化求状态量更新
lio-sam框架:点云匹配之手写高斯牛顿优化求状态量更新前言代码分前言 LIO-SAM的全称是:Tightly-coupled Lidar Inertial Odometry via Smoo ...
点云匹配和ICP算法概述
[原文:http://www.cnblogs.com/yhlx125/p/4955337.html] Iterative Closest Point (ICP) [1][2][3] is an alg ...
暗恋云匹配匿名交友聊天系统开发
暗恋云匹配匿名交友聊天系统开发软件背景事实上,大部分恋爱,都发生在身边的熟人,而他们,大部分时间,都偷偷地喜欢着对方.暗恋是刚需,而捅破暗恋这层纸,是刚需中的刚需. 软件简介暗恋匹配,让用户可以 ...
LIO-SAM框架：点云匹配前戏之初值计算及局部地图构建
LIO-SAM框架:点云匹配前戏之初值计算及局部地图构建前言建图优化代码点云匹配前戏之初值计算总结点云匹配之前戏局部地图构建总结前言 LIO-SAM的全称是:Tightly-couple ...
CVPR论文解读 | 点云匹配的旋转不变变压器
原创 | 文 BFT机器人传统的手工特征描述符通常具有内在的旋转不变性,但是最近的深度匹配器通常通过数据增强来获得旋转不变性. 然而,由于增强旋转数量有限,无法覆盖连续SO(3)空间中所有可能的旋转 ...
icp点云匹配迭代最近邻算法
一.含义: 1.icp算法能够使两个不同坐标系下的点集匹配到一个坐标系中,这个过程就是配准,配准的操作就是找到从坐标系1变换到坐标系2的刚性变换. 2.icp的本质就是配准,但有不同的配准方案,icp ...
三维重建10：点云配准和点云匹配
点云的配准一般分为等价集合和律属集合两种配准,其中等价集合配准叫做匹配过程,律属集合配准被称为Alignment. 点云的匹配一般使用ICP方法( ICP:Iterative Closest Poi ...
ECCV2018--点云匹配
Efficient Global Point Cloud Registration by Matching Rotation Invariant Features Through Translatio ...

cartographer（8）点云匹配

cartographer（8）点云匹配相关推荐

最新文章

热门文章