autoware的icp_maching解读
param_callback:变量初始化函数:
根据gnss数据的变动和最初定位数据的变化来更新最初定位
initial_pose :最初定位
localizer_pose:icp定位出来的位置
previous_pose:以前的位置
current_pose:当前的位置
map_callback:地图更新:
只接收一次地图,并添加到icp配准器里面
gnss_callback:惯导回调数据:
获取当前惯导的gps位置和位姿数据,持续更新previous_gnss_pose为当前惯导数据,当开启gnss并且没有初始化变量 或者 icp配准打分超过阈值时更新以下数据:
previous_pose:更新为previous_gnss_pose数据
current_pose:更新为当前惯导数据
offset:current_pose-previous_pose
initialpose_callback:
不知谁来发送,不知发送频率
会利用收到的数据来更新previous_pose和current_pose还有offset
points_callback:icp配准回调
使用previous_pose加offset_x 来更新predict_pose预测值
setMaximumIterations:设置迭代次数
setTransformationEpsilon:设置终止条件设置最小转换差异
setMaxCorrespondenceDistance:设置点云距离最大对应距离,距离过大的将被忽略
setEuclideanFitnessEpsilon:设置欧几里得距离差
setRANSACOutlierRejectionThreshold:未知
计算icp匹配出来的icp_pose和previous_pose的误差
本来是根据匹配出来的误差来判定 使用 icp_pose还是previous_pose来更新current_pose 但是这个代码强制使用icp_pose来更新current_pose,不明白为什么。
predict_pose_error = sqrt((icp_pose.x - predict_pose.x) * (icp_pose.x - predict_pose.x) +(icp_pose.y - predict_pose.y) * (icp_pose.y - predict_pose.y) +(icp_pose.z - predict_pose.z) * (icp_pose.z - predict_pose.z));if (predict_pose_error <= PREDICT_POSE_THRESHOLD){use_predict_pose = 0;}else{use_predict_pose = 1;}use_predict_pose = 0;if (use_predict_pose == 0){current_pose.x = icp_pose.x;current_pose.y = icp_pose.y;current_pose.z = icp_pose.z;current_pose.roll = icp_pose.roll;current_pose.pitch = icp_pose.pitch;current_pose.yaw = icp_pose.yaw;}else{current_pose.x = predict_pose.x;current_pose.y = predict_pose.y;current_pose.z = predict_pose.z;current_pose.roll = predict_pose.roll;current_pose.pitch = predict_pose.pitch;current_pose.yaw = predict_pose.yaw;}
然后计算这次和上次位置的误差来更新当前的一些数据
scan_duration:这次匹配所用时间
diff:这次运动的距离
current_velocity:当前的速度
current_velocity_smooth:取连续3次速度的平均值,如果小于0.2 就设为0.0
current_accel:当前加速度
estimated_vel_mps:当前速度m/s,并发布出去
estimated_vel_kmph:当前速度km/h,并发布出去
发布predict_pose_pub,icp_pose_pub,localizer_pose_pub
发布"/map" 到 "/base_link"的tf变换
发布time_icp_matching_pub(icp匹配所花时间)
发布estimate_twist_pub,estimated_vel_pub(当前时刻速度)
发布icp_stat_pub(icp的一些数据)
写入icp日志
更新offset(此次移动的距离)
更新previous_pose,previous_scan_time,previous_velocity等数据
autoware的icp_maching解读相关推荐
- autoware planning trajectory_smoother 模块解读
autoware planning trajectory_smoother 模块解读 说明 总结 代码解读 函数 TrajectorySmoother 函数 Filter 说明 博主仅凭代码本身进行理 ...
- 【导航业务框架】开源无人驾驶项目autoware解读
系列文章目录 提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理 文章目录 系列文章目录 前言 一.Autoware的整体框架和模块 1.Autoware介绍 2. ...
- 解读Autoware.Universe规划模块:Behavior Path Planner
解读Autoware.Universe规划模块:Behavior Path Planner 前言 文章列表 行为路径规划器(Behavior Path Planner) 目的(Purpose) 场景( ...
- autoware下ndt_mapping节点解读
利用激光雷达进行建图,首先需要得到稠密点云,然后进行体素滤波进行过滤得到包含特征的点云数据.接着利用每一帧扫描的点云地图进行ndt配准逐帧拼接,最后能够得到激光雷达扫描路径下的整体点云地图. ndt_ ...
- Autoware感知瞎学笔记(一)lidar_kf_contour_track
Autoware感知瞎学笔记(一)lidar_kf_contour_track 目录 代码分析: 一.雷达目标Kalman滤波器 1. lidar_kf_contour_track.cpp 2. li ...
- Python Re 模块超全解读!详细
内行必看!Python Re 模块超全解读! 2019.08.08 18:59:45字数 953阅读 121 re模块下的函数 compile(pattern):创建模式对象 > import ...
- Bert系列(二)——源码解读之模型主体
本篇文章主要是解读模型主体代码modeling.py.在阅读这篇文章之前希望读者们对bert的相关理论有一定的了解,尤其是transformer的结构原理,网上的资料很多,本文内容对原理部分就不做过多 ...
- Bert系列(三)——源码解读之Pre-train
https://www.jianshu.com/p/22e462f01d8c pre-train是迁移学习的基础,虽然Google已经发布了各种预训练好的模型,而且因为资源消耗巨大,自己再预训练也不现 ...
- NLP突破性成果 BERT 模型详细解读 bert参数微调
https://zhuanlan.zhihu.com/p/46997268 NLP突破性成果 BERT 模型详细解读 章鱼小丸子 不懂算法的产品经理不是好的程序员 关注她 82 人赞了该文章 Goo ...
最新文章
- Android中的网络(字节跳动)
- composer的基本运用
- mathtype6在word2019中闪退的问题
- c++函数模板(c++细节篇十)
- ITK:概念检查是同维的
- 对飞行前请求的响应未通过访问控制检查:它没有http ok状态。_对不起,看完这篇HTTP,真的可以吊打面试官...
- 复古多变“格子控”混搭 夏季继续魅力四射
- APUE 第四章总结
- 直播技术原理:CDN技术详解
- 数字化城管信息系统项目需求说明书
- Froala Editor 4.0.11 Crack
- 我的团长我的团第八集
- JZOJ4991. Skyfall
- (三)matplotlib3.0画三维图(二维热图)
- linux卸载软件时卸载失败,ubuntu软件卸载不干净报错解决
- 科技类毕业论文文献有哪些?
- 安全准入考试(配电专业一般工作人员)安规题库(含答案)
- EMC风险分析-电缆屏蔽(CS)分析
- Zotero使用Onedrive实现云同步
- MySQL(InnoDB剖析):24---B+树索引(聚集索引与非聚集索引(辅助索引))、B+树索引的分裂