mcl_3dl阅读整理

最新推荐文章于 2024-04-18 10:01:44 发布
最新推荐文章于 2024-04-18 10:01:44 发布
阅读量1.7k 收藏 14
点赞数 4
分类专栏： 工程项目 笔记 文章标签： 算法 slam
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/wuliyanyan/article/details/104419855
版权

算法简介

（这部分是github上ReadME搬运过来的）mcl_3dl节点是基于三维激光雷达的定位算法，核心是蒙特卡洛定位（MCL），也称为粒子滤波定位，通过用粒子的密度和权重来表示待估计位姿的概率分布。

观测：

1 LIDAR测量模型：

通常波束模型与似然域模型相比，可以减少地图中被测点的false positive匹配，但更复杂。mcl_3dl节点同时使用这两个模型来以良好的计算能力实现好的匹配结果。计算粒子的likelihood的时候，将用大量随机采样点做似然域模型得到的结果，与用少量随机采样点做波束模型预测的结果相乘，计算不复杂但是又减少了误匹配。
具体做法是：
在似然域模型中，likelihood是每个观测点到地图中最近点的距离之和，最近点的搜索使用分块Kd树，观测点进行体素滤波并随机采样以降低计算复杂度。具体实现在lidar_measurement_model_likelihood.cpp
在波束模型中，likelihood通过ray casting计算，N是观测点的射线数量，n是穿过地图中障碍物的射线数量，alpha是拒绝权重，计算公式是alpha ^（n / N）。具体实现在lidar_measurement_model_beam.cpp

2 轴相关距离权重

在墙壁稀疏只有地板的一些环境中，由于可以修正估计Z位置的点的数量相对较大，在Z方向上的距离就更少关注。距离计算的权重系数可以由参数更改，在上述环境下权重设置为：（（dist_weight_x，dist_weight_y，dist_weight_z）=（1.0，1.0，5.0））。这个在代码里面没有找到体现

3 加速度测量模型 imu产生的观测

通过获取IMU数据中的加速度矢量与粒子姿态之间的差来计算姿态的likelihood。

预测

根据带有噪声参数的里程计数据预测粒子的姿态，建议使用由IMU补偿的里程计数据。

重采样：

mcl_3dl节点通过复制较大概率的粒子来对粒子重新采样，复制的粒子会因高斯噪声而扩散。

扩展重置Expansion resetting

mcl_3dl临时实现了扩展重置方法。如果所有观测点中，具有相关联地图点的观测点的比例小于阈值，则通过给定的高斯噪声扩展粒子分布。一定的程度上可以解决局部极值问题。
代码里的实现就是在之前的基础上给粒子加噪声，然后等待重新收敛

执行

roslaunch mcl_3dl test.launch use_pointcloud_map:=false use_cad_map:=false \ use_bag_file:=true bag_file:=${HOME}/Downloads/short_test.bag

有几个参数：
use_neonavigation = true（默认false） 会执行下面两个其他地方的包：

trajectory_tracker trajectory_recorder path_recorder
dist_interval = 0.1
ang_interval = 0.2
trajectory_tracker trajectory_recorder poses_ref
dist_interval = 0.5
ang_interval = 100.0

use_cad_map = true默认! use_pointcloud_map 会执行下面这个包：

obj_to_pointcloud obj_to_pointcloud obj_to_pointcloud
    <param name="objs" value="$(arg map_objs)" />
    <param name="offset_x" value="$(arg map_offset_x)" />
    <param name="offset_y" value="$(arg map_offset_y)" />
    <param name="scale" value="$(arg map_scale)" />
    <remap from="~/cloud" to="mapcloud" />

use_pointcloud_map = true（默认true）会执行下面这个包：

<node pkg="pcl_ros" type="pcd_to_pointcloud" name="pcd_to_pointcloud"
      args="$(arg map_pcd)" if="$(arg use_pointcloud_map)">
    <remap from="cloud_pcd" to="mapcloud" />
  </node>

上面三个参数额外执行的包都不是mcl_3dl提供的

主要结构

运行graph
这是mcl_3dl节点执行后的graph

Topic：
接收

  • /cloud(雷达发布)
  • /odom(码盘)
  • /imu/data(imu)
  • /tf(一系列的tf变换)
  • /mapcloud(点云地图，插入到chunk kdtree，会随时检测地图更新)
  • /initialpose(发布这个话题，粒子就在提供的位置处叠加噪声，如果不发布这个话题，粒子就在默认位置处叠加噪声（默认位置可以通过ros参数修改和读取）)

发布

  • /tf(odom到map 还有一个floor到map)
  • /amcl_pose(map坐标系下的位置)

服务：

  • /resize_mcl_particle 可以实时调整粒子数（调整粒子数的操作和重采样很相似，都是大概率的粒子多复制）
  • /global_localization 实时启动全局定位
  • /expansion_resetting 实时进行扩展重置

在test/src/ test_global_localization.cpp文件中给出了全局定位的使用过程：

  • 发布cloud_map；
  • 发布生成的cloud imu odom数据 并等待初始化（status变成normal；
  • 然后发了5次odom数据 用来那边做预测
  • 调用重定位服务并等待状态变成GLOBAL_LOCALIZATION （下面的过程会一直发/cloud /odom /imu/data数据）
  • 等待全局定位结束
  • 等了40个循环提高位姿精度

部分代码

  • chunked_kdtree.h 块kd树 在pcl kdtree基础上增加了分块 在每个分块内进行pcl的kdtree搜索：kdtree_->radiusSearch(p, radius, id, dist_sq, num) 点，半径，紧邻索引值，近邻中心距，

  • raycast.h 定义了迭代器，与块kd树相结合的射线遍历 用在雷达波束模型中。这里有一个点：头文件里面重载了*操作符，因为for的范围循环在循环前，会对每一个对象进行一次" * "，再赋值 这个重载真牛逼；然后遍历射线 对射线的每个点在kdtree上搜索最近点。里面进行了两次搜索，没有看太懂 最后用两次的结果计算了sin_ang 做进一步判断

  • filter.h 提供了两种滤波方式 LPF和HPF 低通滤波和高通滤波

  • nd.h 正态分布 一维和多维的

  • point_types.h 定义了pointcloud类型然后注册到点云库中

  • point_conversion.h 定义了ros msg到pcl pointcloud转换

  • point_cloud_random_sampler.h 随机生成一坨点云

  • vec3.h 自己定义了位姿

  • quat.h 自己定义了旋转 以四元数形式存储 中间又RPY和轴角的转换

  • pf.h 定义了ParticleWeightedMean ParticleFilter（这个里面实现了重采样） Particle ParticleBase

  • state_6dof.h 扩展到6自由度位姿

  • parameters.h 参数集合

  • imu_measurement_model.h 重力加速度计观测 用观测与predict的第三分量，求cos 然后作为方差 返回对应的高斯分布的概率；之前对IMU做观测了解的不多，第一次遇到这种做法。

  • motion_prediction_model.h 差分轮式预测模型 设置前与当前的pose与时间间隔 预测下一时刻的pose 运动模型更新部分

  • noise_generator.h 生成噪声 正态分布 只在对角线生成和全矩阵生成两种

  • lidar_measurement_model.h 雷达观测模型 likelihood和beam模型

  • compatibility.h ros subscribe和advertise等兼容性相关的文件

  • voxel_grid.h 对点云体素化 然后进行滤波（取每个体素中所有三维点中间一点的点）输出

  • lidar_measurement_model_beam.cpp 波束模型的具体实现

  • lidar_measurement_model_likelihood.cpp 似然域模型的具体实现

  • mcl_3dl.cpp 整个的ros节点，各种topic的回调函数，粒子滤波的过程在/cloud对应的回调函数里面，这个文件比较大 做了一部分注释贴上来

理解

1 mcl_3dl节点的全局定位实现就是和正常定位一样，在整个地图撒粒子，这样开始的时候计算效率肯定不高，但是随着收敛迭代，会不断减小粒子数，这一点想法很好。因为没法实际测试，所以不知道具体的效果怎么样，收敛效率怎么样。
2 全局定位似乎不一定能解决局部收敛问题，比如集中在某几个地方会存在大量的粒子；
3 在定位的过程中也会构建新的地图，但是这个地图没有与传入的地图融合

（最后就是发一些感慨，最近疫情期间，在家学习效率太低了……希望借这篇博客给自己提提神，接下来抓紧时间多学习啊啊啊啊 最后的最后 武汉加油！！！

茗絮封芯
关注
  • 4
    点赞
  • 踩
  • 14
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
专栏目录
3d_mcl
03-31
3d_mcl 3d_mcl是在点云图中执行3D定位（x，y和yaw）的ROS节点。 仍在开发中。
matlab中等值线代码-MCL_CCP:MCL_CCP
06-19
matlab中等值线代码
机器人学习--基于3D激光雷达数据的MCL全局定位
GGY1102的博客
11-18 4569
论文作者：Xieyuanli Chen,Ignacio Vizzo,Thomas Läbe,Jens Behley,Cyrill Stachniss 大概方法：作者将3Dlidar扫描的360度距离信息 转变成 range image，然后通过图像特征比对，当前的3D数据 和数据库中的range 图像作比较。 数据库中的range image 貌似是作者用算法 GPU和 OpenGL 生成的 合成图像。 其实传感器数据表达合适之后，剩下的就是观测模型，经典粒子滤波或者贝叶斯滤波中的观测...
2 ubuntu18.04安装mcl_3dl
chencaw的专栏
12-10 1017
参考了 https://github.com/at-wat/mcl_3dl https://blog.csdn.net/jc15988821760/article/details/100263564 1、直接安装二进制包 sudo apt-get install ros-melodic-mcl-3dl 2、下载测试包测试 # Download the example bag...
基于地图的激光雷达定位算法汇总（含开源代码）
zhaoliang38的博客
05-26 2593
基于地图的激光雷达定位算法汇总（含开源代码）
基于三维点云地图定位导航（更新中。。。。。）
u013630299的博客
01-07 6676
因为想要做6D localization，因此先尝试跑了下lego_loam 1 安装好lego-loam以后，按照如下命令跑了下 roslaunch lego_loam run.launch rosbag play 2018-05-18-14-57-12_8.bag --clock --topic /velodyne_points /imu/data 2 下一步要做的，保存三维地图，...
mcmc.rar_MATLAB MCL_MCL_MCL matlab_Matlab蒙特卡罗_节点移动 matlab
09-19
3. 传感器观测：将每个粒子的新位置与实际传感器观测值进行比较，计算对应于每个粒子的权重。 4. 重采样：根据权重对粒子进行重采样，以保持粒子集的多样性并避免“粒子退化”（即所有粒子都聚集在某一区域）。 5. ...
mcl.zip_MCL_localization_localization .tcl
09-14
标题中的“mcl.zip_MCL_localization_localization.tcl”提到了MCL（Monte Carlo Localization）算法在机器人定位中的应用。MCL是一种基于概率的定位方法，常用于移动机器人领域，通过模拟大量的随机粒子来估计...
mcl.rar_MCL_mcl算法_马尔_马尔科夫_马尔科夫链
09-23
3. **网络分析**：在网络拓扑中，MCL可以帮助识别关键节点和社区结构，例如在社交网络或互联网路由中。 4. **生物信息学**：在基因序列分析中，马尔科夫链用于预测蛋白质结构或转录因子结合位点。 ### 四、案例...
L-SLAM:低维同时定位和映射 (L-SLAM)
06-30
L-SLAM 低维同时定位和映射 (L-SLAM) 作者 尼科斯·齐科斯
探索未来科技：mcl_3dl - 一个高效3D定位库
最新发布
gitblog_00059的博客
04-18 448
探索未来科技：mcl_3dl - 一个高效3D定位库 mcl_3dlA ROS node to perform a probabilistic 3-D/6-DOF localization system for mobile robots with 3-D LIDAR(s). It implements pointcloud based Monte Carlo localization that...
ROS实现pcd点云文件转前视图
weixin_47275328的博客
10-01 832
本文将实现在ROS系统下将pcd点云文件转为深度前视图。 一 准备工作 系统：ubuntu 18.04 ROS版本：ros-melodic IDE：VScode 语言：C++ 二 程序实现 直接上代码！ 1.创建工作空间 创建工作空间存，src下存放执行文件。 // 创建工作空间 mkdir -p ~/deep_image/src //进入工作空间 cd ~/deep_image //编译 catkin_make //进入VScode code . 主要的VS配置主要参考赵虚左老师的教程，具体配置点此处链
机器人学习--粒子滤波SLAM/MCL定位参考资料+学习经验
GGY1102的博客
04-01 634
学习材料1： 《概率机器人学》 谷歌无人驾驶之父Sebastian Thrun等人著作。 注释： 可能是本人智商有限，或者是移动机器人学领域的基础知识了解不多。 刚刚看这本书的时候，尤其吃力，胡乱翻了几页，根本看不进去，看的目光呆滞，食欲不振。 如果，同样遇到类似的情况，无法耐心静心全身心的从头到尾看完，那说明可能是因为 先验知识的不足。 学习方法建议： 假设学习这本书的读者，已经在Ubuntu OS中安装 ROS ，至少跑过 gmapping 或 cartograph...
马尔可夫聚类算法（MCL）
GatsbyNewton
12-05 3万+
1.基础 1.1Random Walks 在图中，通过Random Walks处理，可以找到数据在哪里聚集，或者聚簇在哪。 图中的Random Walks是使用马尔可夫链计算求出。 1.2马尔可夫链（Markov Chain） 先看一个简单的例子： 第一步，结点1的Random Walker有33%的概率到达结点2、3和4，且有0%的概率到达结点5、6和7。 对于结点
SLAM算法
热门推荐
在路上@Amos
08-19 7万+
一、概述 Simultaneous Localization and Mapping (SLAM)原本是Robotics领域用来做机器人定位的，最早的SLAM算法其实是没有用视觉camera的（Robotics领域一般用Laser Range Finder来做SLAM）。本文主要关注基于camera图像做SLAM的算法，即Visual SLAM算法。本文对SLAM和Visual SLAM不做区分...
o3d.io.read_point_cloud源代码
05-24
o3d.io.read_point_cloud函数是Open3D库中用于读取点云数据的函数。其源代码如下： ```python def read_point_cloud(filename: str, format: Optional[str] = None, remove_nan_points: bool = True, remove_infinite_points: bool = True, print_progress: bool = False) -> 'open3d.geometry.PointCloud': """Function to read point cloud from file. Args: filename (str): path of the point cloud file. format (str, optional): format of the file. If not provided, the format will be inferred from the filename extension. remove_nan_points (bool, optional): whether to remove NaN points. remove_infinite_points (bool, optional): whether to remove infinite points. print_progress (bool, optional): whether to print progress information. Returns: open3d.geometry.PointCloud: an instance of the `open3d.geometry.PointCloud` containing the point cloud data. """ if format is None: format = utility._get_file_extension_lower(filename) format = format.lower() # Check if format is supported. if format not in _READABLE_FORMATS: raise ValueError(f"Unsupported file format '{format}'") # Call the appropriate read function based on format. if format == 'xyz': return read_xyz(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'xyzn': return read_xyz_normal(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'pts': return read_pts(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'ply': return read_ply(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'pcd': return read_pcd(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'fbx': return read_fbx(filename, remove_nan_points, remove_infinite_points, print_progress) elif format == 'obj': return read_triangle_mesh(filename).sample_points_poisson_disk( number_of_points=0) elif format == 'stl': return read_triangle_mesh(filename).sample_points_poisson_disk( number_of_points=0) else: raise ValueError(f"Unsupported file format '{format}'") ``` 该函数首先判断文件格式是否支持，然后根据文件格式调用对应的读取函数，例如对于PLY格式的点云文件，会调用`read_ply`函数进行读取。最终返回一个`open3d.geometry.PointCloud`对象。
评论 8
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值