Hello World - SLAM
type: channel
link: https://www.phxin.top/index.php/category/SLAM-algorithm/
items: livox-mid70激光雷达使用实录
items: IMU标定
items: WIN10安装VLFeat工具箱教程
items: win10安装PCL点云库（详细教程）
items: gazebo中添加gps模块
items: imu和gnns融合替代里程计
items: robot_localization功能包翻译版本
items: 雷达点云数据保存为mat格式
items: icp算法实操

livox-mid70激光雷达使用实录
type: item
date: 2024-01-22T17:16:50+08:00
description: livox-mid70激光雷达使用实录livox-mid70是由大疆发布的面阵激光雷达，是大疆为自动驾驶推出的一款激光雷达，同时也可用于建立三维地图，文物修复，与大疆无人机相结合使用。你可以在x86和arm架构上都可以使用(arm需要交叉编译)。 bilibili上Livox官方发布了大量的视频demo可以去围观一下，同时Livox也有一个论坛可以去看一下。官方文档https://livox-wiki-cn.readthedocs.io/zh_CN/latest/index.html 这是中文官方文档，请根据以上文档完成硬件上的连接以及Ip配置，一切以官方文档为主。更改IP：一般来说刚拿到手都是静态IP，这时候你只需要将网线与你自己笔记本相连，然后通过用户手册查看你自己雷达的IP，那需要设置你自己电脑的IP为手动，并将其分在同一网段不同的地址下： 如我的Mid70的IP地址为192.168.1.105， 那么我电脑设置时 IP：192.168.1.104， 子网掩码：255.255.255.0, 网关：192.168.0.1如果对IP配置有问题,或想改为动态IP详情参见用户手册.pdf)Livox ViewerLivox Viewer是一款图形化操作的显示软件，有助于快速让你知道你的硬件连接和Ip配置有没有问题。Livox-Viewer for WindowsLivox-Viewer for ubuntu16.04,下载后解压运行./livox viewer.sh即可打开如下界面。官方软件Livox Viewer仅支持Windows或ubuntu16.04/14.04,所以建议如果不是这两个版本有ros可以直接上。Livox-SDK刚刚Livox Viewer只是一个快速尝试，如果你要做开发就必须要安装Livox-SDK(包括后面要在ros上使用也必须先要安装这个)github上中文README先从github上clone下来git clone https://github.com/Livox-SDK/Livox-SDK.git cd Livox-SDK cd build && cmake .. make sudo make install简单运行demo，注意这里的sample是在build文件夹下面的cd sample/lidar && ./lidar_samplelivox-ros-driver不得不说ros是真的好用，所以建议在ros下使用，会使得一切都很清晰明了。装livox-ros-driver前必须要先装Livox-SDK。先从github上clone下来，这里一定要用下面这个命令把它clone到相应位置，否则会出问题。git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src简单的catkin_make一下cd ws_livox catkin_make别忘了添加环境source ./devel/setup.sh之后跑个rviz的demo，让你的激光雷达点云数据在rviz里显示出来roslaunch livox_ros_driver livox_lidar_rviz.launch录制你的点云数据点云数据不像二维图像一样，简简单单录制一个视频就好，作为点云数据记录是比较特殊的。 livox一共有两种录制格式lvx格式lvx格式是livox的一种自定义格式，如果你想在Windows上快速录制，可以使用Livox Viewer进行录制，也可以通过Livox-SDK中的文件录制首先进入以下路径cd ./Livox_SDK/build/sample/lidar_lvx_file然后运行下面这个文件，这里不用-c指定也可以，这里指定了lidar并设定持续10s./lidar_lvx_sample -c "此处为已连接的Lidar的15位广播码" -t 10通过上面的操作会得到一个.lvx格式的文件，这个文件可以在Livox Viewer中直接播放，也可以转成rosbag(一个可以把ros中当前所有topic中message记录下来的东西)。roslaunch livox_ros_driver lvx_to_rosbag.launch lvx_file_path:="/home/livox/test.lvx"rosbag格式rosbag是ros中非常好用的一个东西，我在之前的ros文章中有提到过。当我们在运行livox-ros-driver中的rviz显示时，即roslaunch livox_ros_driver livox_lidar_rviz.launch #详情看上面livox-ros-driver我们可以把此时的所有消息用rosbag…

IMU标定
type: item
date: 2023-12-12T17:16:00+08:00
description: IMU标定[TOC]安装 Ceres首先安装依赖 libdw-dev，在小车系统终端执行以下命令：sudo apt-get install libdw-dev然后需要进行 Ceres 库的安装，首先安装 Ceres 库所需的依赖项。sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libgtest-devCeres 的编译安装https://github.com/ceres-solver/ceres-solver/tree/2.0.0 获取到 Ceres2.0.0 版mkdir ceres-bin cd ceres-bin cmake ../ceres-solver-2.0.0 make -j3 make test sudo make install bin/simple_bundle_adjuster ../ceres-solver-2.0.0/data/problem-16-22106-pre.txt安装 code_utils 和 imu_utils在安装完 Ceres 之后我们需要获取 code_utils 和 imu_utils 两个功能包，用于IMU 数据的误差标定分析。这两个功能包我们都有提供 opencv3 和 opencv4 环境下修改后的版本，由于原版在编译时会遇到一些报错，基于这些报错我们对功能包源码进行了一定修改，使得这两个功能包在我们的环境下能直接编译成功。在编译时需注意，imu_utils 包时依赖于 code_utils 的，所以我们需要先编译code_utils 功能包，之后再编译 imu_utils。这里我们可以采用单独编译的方式，具体编译指令如下。# 创建workspace，将功能包放入src目录内，然后编译 catkin_make -DCATKIN_WHITELIST_PACKAGES=code_utils catkin_make -DCATKIN_WHITELIST_PACKAGES=imu_utils#运行功能包 roslaunch fdilink_ahrs ahrs_data.launch # 录制一小时 timeout 1h rosbag record -O imu_biaoding.bag /imu录制bag按照录制时间和imu发布的话题进行修改，我这里是1h，imu话题是/imutimeout 1h rosbag record -O imu_biaoding.bag /imu修改标定参数打开imu_utils 功能包中的任意launch 文件（所有launch 文件本质是一样的），根据实际情况修改其中的话题名，IMU 名及标定时长三个参数roslaunch imu_utils A3.launch (找到刚才编译的code_utils和imu_utils) rosbag play -r 200 imu.bag (找到你录制bag的文件夹)编译code_utils 报错错误提示表明在构建ROS工程 code_utils 时，CMake找不到名为 Ceres 的包。这可能是由于你的系统中没有安装 Ceres 或者 Ceres 的CMake配置文件未在CMake的搜索路径中。所以，重新安装一下Ceres 包

WIN10安装VLFeat工具箱教程
type: item
date: 2023-12-04T19:48:56+08:00
description: WIN10安装VLFeat工具箱教程下载VLFeat下载地址下载后解压到文件夹，文件夹最好不要含有中文防止报错，win安装下载binary package即可添加路径我解压之后的路径是D:\software\vlfeat-0.9.21\toolbox， 在matlab命令行里输入addpath('D:\software\vlfeat-0.9.21\toolbox');% 添加您的VLFeat工具箱路径 vl_setup;测试安装使用下面的命令来进行测试：vl_version verbose永久添加路径（可选）如果您希望MATLAB在每次启动时都能够访问VLFeat工具箱，可以将上述添加路径的命令添加到您的MATLAB启动脚本（如startup.m文件）中。

win10安装PCL点云库（详细教程）
type: item
date: 2023-12-02T20:59:26+08:00
description: win10安装PCL点云库（详细教程）下载PCL软件点这里跳转Github下载我这里选的是AllInOne版本安装PCL右键以管理员身份运行，然后运行更改，进入软件安装界面下一步 --> 我接受 ---> 勾选 Add PCL to the system PATH for all users --> 下一步 ---> 选择合适的位置安装（最好路径不要有中文）报错 path too long installer unable to modify path如下界面右键点击属性->高级系统设置->环境变量->系统变量修改Path和PCL_ROOTPCL_ROOT选中PCL_ROOT，点击编辑，将路径改为你安装的路径Path选中Path，点击编辑，右上角新建，添加如下路径%PCL_ROOT%\bin %PCL_ROOT%\3rdParty\FLANN\bin %PCL_ROOT%\3rdParty\VTK\bin %PCL_ROOT%\3rdParty\Qhull\bin %PCL_ROOT%\3rdParty\OpenNI2\Tools大功告成！

win10安装PCL点云库（详细教程）
type: item
date: 2023-11-27T19:04:40+08:00
description: win10安装PCL点云库（详细教程）下载PCL软件点这里跳转Github下载我这里选的是AllInOne版本安装PCL右键以管理员身份运行，然后运行更改，进入软件安装界面下一步 --> 我接受 ---> 勾选 Add PCL to the system PATH for all users --> 下一步 ---> 选择合适的位置安装（最好路径不要有中文）报错 path too long installer unable to modify path如下界面右键点击属性->高级系统设置->环境变量->系统变量修改Path和PCL_ROOTPCL_ROOT选中PCL_ROOT，点击编辑，将路径改为你安装的路径Path选中Path，点击编辑，右上角新建，添加如下路径%PCL_ROOT%\bin %PCL_ROOT%\3rdParty\FLANN\bin %PCL_ROOT%\3rdParty\VTK\bin %PCL_ROOT%\3rdParty\Qhull\bin %PCL_ROOT%\3rdParty\OpenNI2\Tools大功告成！

gazebo中添加gps模块
type: item
date: 2023-11-03T14:46:00+08:00
description: gazebo中添加gps模块首先，需下载一个功能包hector gazebo plugins。Hector gazebo plugins 提供了来自 Hector 团队的 gazebo 插件。目前，它包含一个6瓦差分驱动插件，一个 IMU 传感器插件，一个地球磁场传感器插件，一个 GPS 传感器插件和一个声纳测距插件。安装hector_gazebo_plugins：sudo apt-get install ros-melodic-hector-gazebo-plugins添加gps的xacro文件导入gps的物体描述xacro文件；即gps的物体描述代码与功能配置代码，需连接在一起。原因：如果是传感器的功能配置plugin标签，则必须在gazebo标签内的sensor标签下，否则不起作用。而该gps的配置gazebo标签内没有sensor标签，若想plugin标签起作用，就得紧接在物体描述代码后，而不能分成两个单独得文件。<robot name="gps" xmlns:xacro="http://wiki.ros.org/xacro"> <xacro:property name="gps_offset_x" value="0" /> <xacro:property name="gps_offset_y" value="0" /> <xacro:property name="gps_offset_z" value="0.0075" /> <xacro:property name="gps_size" value="0.005" /> <xacro:property name="gps_m" value="0.01" /> <visual> <origin rpy="0 0 0" xyz="0 0 0" /> <geometry> <box size="${gps_size} ${gps_size} ${gps_size}"/> </geometry> <material name= "red" > <color rgba="1.0 0.0 0.0 1.0" /> </material> </visual> <collision> <geometry> <box size="${gps_size} ${gps_size} ${gps_size}" /> </geometry> <origin xyz="0.0 0.0 0" rpy="0.0 0.0 0.0" /> </collision> <xacro:Box_inertial_matrix m = "${gps_m}" l = "${gps_size}" w = "${gps_size}" h = "${gps_size}"/> <joint name="gps2imu" type="fixed"> <origin xyz="${gps_offset_x} ${gps_offset_y} ${gps_offset_z}" rpy="0 0 0" /> <parent link="imu"/> <child link="gps"/> </joint> <gazebo reference="gps"> <material>Gazebo/Green</material> </gazebo> <gazebo> <plugin name="gazebo_ros_gps" filename="libhector_gazebo_ros_gps.so"> <updateRate>4.0</updateRate> <bodyName>gps</bodyName> <frameId>gps</frameId> <topicName>fix</topicName> <velocityTopicName>fix_velocity</velocityTopicName> <referenceLatitude>-30.06022459407145675</referenceLatitude> <referenceLongitude>-51.173913575780311191</referenceLongitude> <referenceHeading>90</referenceHeading> <referenceAltitude>10</referenceAltitude> <offset>0 0 0</offset> <drift>0.001 0.001 0.001</drift> <gaussianNoise>0.05 0.05 0.05</gaussianNoise> <velocityDrift>0.001 0.001 0.001</velocityDrift> <velocityGaussi…

imu和gnns融合替代里程计
type: item
date: 2023-11-01T09:23:00+08:00
description: 将IMU（惯性测量单元）和GNSS（全球导航卫星系统）数据融合以用作里程计的过程可以分为以下步骤：数据采集：收集IMU数据：IMU提供加速度和角速度的测量值。收集GNSS数据：GNSS接收器获取卫星定位信息，包括位置、速度和时间戳。数据预处理：对IMU数据进行预处理：包括去除偏差、噪声滤波和积分加速度和角速度以估算速度和位移。对GNSS数据进行预处理：解决多路径效应、钟差和其他误差，得到高质量的位置和速度测量。坐标系对齐：确保IMU和GNSS数据的坐标系一致。这可能需要进行坐标系变换，以便将它们对齐。状态估计：使用扩展卡尔曼滤波器（EKF）或粒子滤波器等状态估计算法，将IMU和GNSS数据融合。这些算法将帮助你估算车辆或机器人的状态，包括位置、速度和姿态。更新和融合：周期性地将新的IMU和GNSS数据与当前状态估计融合，以更新估计的位置和速度。这通常涉及到预测步骤（使用IMU数据预测状态）和更新步骤（使用GNSS数据校正预测）。误差建模：考虑IMU和GNSS测量的不确定性和误差特性，将其合并到状态估计中，以更好地估算里程和位置。输出估计：输出融合后的位置和速度估计，这可以用作里程计数据。评估性能：定期评估融合后的里程计性能，以检测误差的累积并进行校正。这个过程的关键在于合理处理IMU和GNSS数据的不确定性，以及在融合步骤中正确处理误差。IMU通常提供高频率的数据，但随时间漂移，而GNSS提供全球性的位置信息，但可能受多种误差因素影响。融合算法的设计和参数调整也对最终性能至关重要。需要注意的是，这只是一个高层次的概述，实际的IMU和GNSS融合过程可能会更为复杂，具体取决于你的应用和硬件配置。你可能需要使用专门设计的融合库或框架来帮助实施这一过程。

robot_localization功能包翻译版本
type: item
date: 2023-10-29T09:39:00+08:00
description: State Estimation Nodesekf_localization_nodeekf_localization_node是扩展卡尔曼滤波器的实现。它使用全向运动模型及时预测状态，并使用感知的传感器数据校正该投影估计值。ukf_localization_nodeukf_localization_node是无迹卡尔曼滤波器的实现。它使用一组精心挑选的西格玛点通过 EKF 中使用的相同运动模型投影状态，然后使用这些投影西格玛点来恢复状态估计值和协方差。这消除了雅可比矩阵的使用，并使滤波器更加稳定。但是，它也比 更费力。ekf_localization_node参数ekf_localization_node并共享其绝大多数参数，因为大多数参数控制数据在与核心过滤器融合之前如何处理。ukf_localization_node状态估计节点可用的参数数量相对较多，这使得启动和配置文件成为启动其任何节点的首选方法。该包包含模板启动和配置文件，可帮助用户入门。ekf_localization_node和ukf_localization_node通用参数标准参数~频率滤波器生成状态估计值的实值频率（以 Hz 为单位）。注意：筛选器不会开始计算，直到它从其中一个输入接收至少一条消息。~sensor_timeout实数周期，以秒为单位，表示我们何时将任何传感器视为已超时。在此情况下，我们会在不进行校正的情况下对扩展卡尔曼滤波器（EKF）执行预测周期。可以将这个参数视为滤波器生成新输出的最低频率的倒数。~two_d_mode如果您的机器人在平面环境中运行，并且您愿意忽略地面的细微变化（如IMU报告的那样），则将其设置为true。它将为所有 0D 变量（Z、横滚、俯仰及其各自的速度和加速度）融合 3 值。这样可以防止这些值的协方差爆炸，同时确保机器人的状态估计值仍然固定在 X-Y 平面上。~[frame]具体参数：~map_frame~odom_frame~base_link_frame~base_link_output_frame~world_frame这些参数定义了robot_localization的操作“模式”。REP-105规定了三个主要的坐标框架：map、odom和base_link。base_link是与机器人固定的坐标框架。机器人在odom坐标框架中的位置会随时间漂移，但在短期内是准确的，应该是连续的。map坐标框架与odom坐标框架类似，是一个世界固定的坐标框架，虽然它包含了机器人最全球准确的位置估计，但它可能会发生离散跳跃，例如由于融合GPS数据。以下是如何使用这些参数的方式：将 map_frame odom_frame和 base_link_frame 参数设置为适合系统的帧名称。注意：如果您的系统没有map_frame，只需将其删除，并确保world_frame设置为 odom_frame 的值。注意:如果您正在运行多个 EKF 实例，并且希望“覆盖”输出转换和消息以使其child_frame_id此帧，则可以进行此设置。base_link_output_frame是可选的，将默认为base_link_frame。这有助于在运行多个 EKF 实例时启用断开连接的 TF 树。当计算最终状态时，我们“覆盖”输出转换和消息，使其具有此帧作为其child_frame_id。如果仅融合连续位置数据（如车轮编码器测程法、视觉里程计或 IMU 数据），请设置world_frame 为您的odom_frame值。这是robot_localization中状态估计节点的默认行为，也是最常见的用法。如果要融合受离散跳跃影响的全局绝对位置数据（例如，GPS 或地标观测值的位置更新），则：将您的world_frame 值设置为您的map_frame值确保其他内容正在生成 odom->base_link 转换。这甚至可以是robot_localization状态估计节点的另一个实例。但是，该实例不应融合全局数据。map_frame odom_frame base_link_frame 的默认值分别为map_frame、odom 和 base_link。base_link_output_frame 的默认值为base_link_frame 。world_frame 默认值为odom_frame。~transform_time_offset某些软件包要求您的变换需要在时间上稍微超前一小段时间。该参数的值将被添加到由机器人定位中的状态估计节点生成的 map->odom 或 odom->base_link 变换的时间戳中。~transform_timeoutrobot_localization 包使用tf2的lookupTransform 方法来请求转换。…

雷达点云数据保存为mat格式
type: item
date: 2023-10-12T16:58:00+08:00
description: 雷达点云数据保存为mat格式创建功能包在终端中执行以下命令以创建一个新的ROS功能包：catkin_create_pkg laser_data_collector_pkg rospy sensor_msgs scipy这将创建一个名为laser_data_collector_pkg的ROS功能包，并添加了所需的依赖项。创建src目录并将脚本添加到其中在功能包的根目录下创建src目录，并将你的Python脚本（比如laser_data_collector.py）复制到该目录。修改CMakeLists.txt在功能包的根目录下，找到CMakeLists.txt文件，修改下图163-166行的代码（按照你自己设定）构建功能包在你的Catkin工作空间中执行以下命令以构建你的ROS功能包：catkin_make运行脚本赋予py文件运行权限进入python文件所在目录chmod +x laser_data_collector.py运行python在一个终端中运行roscore，然后在另一个终端中执行以下命令启动你的节点：source devel/setup.bash rosrun laser_data_collector_pkg laser_data_collector.py这将启动你的ROS节点，开始订阅/scan主题，并在收集足够的数据后保存到MAT文件中。请注意，上述步骤是一种创建ROS节点的简单方式。如果你的功能包需要更复杂的结构或功能，请根据需要进行修改。保存数据代码3S数据采集#!/usr/bin/env python # -*- coding: utf-8 -*- import rospy import scipy.io as sio from sensor_msgs.msg import LaserScan from datetime import datetime from collections import deque class LaserDataCollector: def __init__(self): rospy.init_node('laser_data_collector') self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback) self.angle_increment = None self.angles = None self.ranges = None self.data_buffer = deque() # 使用队列来缓存数据 self.start_time = rospy.get_time() def scan_callback(self, data): self.angle_increment = data.angle_increment self.angles = [data.angle_min + data.angle_increment * i for i in range(len(data.ranges))] self.ranges = data.ranges self.data_buffer.append((self.angles, self.ranges)) current_time = rospy.get_time() if current_time - self.start_time >= 3: # 收集3秒的数据 self.save_to_mat() self.start_time = rospy.get_time() # 重置起始时间 self.data_buffer.clear() # 清空数据缓存 def save_to_mat(self): # 生成一个时间戳以用于MAT文件的文件名 timestamp = datetime.now().strftime("%Y%m%d%H%M%S") file_name = 'laser_scan_data_{}.mat'.format(timestamp) # 将数据从队列中收集起来 all_angles, all_ranges = zip(*self.data_buffer) # 保存数据为MAT文件 scan_data = { 'angleIncrement': self.angle_increment, 'angles': all_angles, 'ranges': all_ranges } sio.savemat(file_name, scan_data) rospy.loginfo('Laser scan data saved to {}'.format(file_name)) if __name__ == '__main__'…

icp算法实操
type: item
date: 2023-10-10T17:13:00+08:00
description: 实操全过程确保你已经在ROS环境中，可以通过运行以下命令来检查：roscore如果roscore成功启动，说明你已经在ROS环境中。创建一个ROS包（如果尚未创建）：在终端中导航到你的ROS工作空间（通常是catkin_ws/src），然后使用catkin_create_pkg命令创建一个ROS包，例如：cd ~/catkin_ws/src catkin_create_pkg my_icp_save rospy sensor_msgs这将创建一个名为my_icp_save的ROS包，并将rospy和sensor_msgs添加为依赖项。编写你的Python节点：在my_ros_package包的目录下，创建一个Python脚本，例如my_node.py，并将你的Python节点代码放入其中。#!/usr/bin/env python import rospy import scipy.io as sio from sensor_msgs.msg import LaserScan class LaserDataCollector: def __init__(self): rospy.init_node('laser_data_collector') self.front_scan_sub = rospy.Subscriber('/front_laser_scan_topic', LaserScan, self.front_scan_callback) self.rear_scan_sub = rospy.Subscriber('/rear_laser_scan_topic', LaserScan, self.rear_scan_callback) self.front_scan_data = None self.rear_scan_data = None def front_scan_callback(self, data): self.front_scan_data = { 'angles': data.angle_min + data.angle_increment * range(len(data.ranges)), 'ranges': data.ranges, 'angle_increment': data.angle_increment } self.save_to_mat() def rear_scan_callback(self, data): self.rear_scan_data = { 'angles': data.angle_min + data.angle_increment * range(len(data.ranges)), 'ranges': data.ranges, 'angle_increment': data.angle_increment } self.save_to_mat() def save_to_mat(self): if self.front_scan_data is not None and self.rear_scan_data is not None: # Combine and save the data as a MAT file combined_data = { 'front_scan_data': self.front_scan_data, 'rear_scan_data': self.rear_scan_data } sio.savemat('laser_scan_data.mat', combined_data) rospy.loginfo('Laser scan data saved to laser_scan_data.mat') if __name__ == '__main__': try: data_collector = LaserDataCollector() rospy.spin() except rospy.ROSInterruptException: pass构建ROS包：在终端中，返回到ROS工作空间的根目录，并运行catkin_make来构建你的ROS包：cd ~/catkin_ws catkin_make文件位置和权限：确保你的my_save.py文件位于ROS包的src目录下，并且具有可执行权限。你可以使用以下命令来添加可执行权限：chmod +x my_save.py安装scipy库：在终端中运行以下命令来安装scipy库：bashCopy code pip install scipy这将安装scipy库到你的Python环境中。运行ROS节点：运行你的ROS节点，你可以使用rosrun命令，例如：rosrun my_ros_packa…