代码实战 | 八叉树地图构建和重定位

3Ｄ视觉工坊

于 2025-05-18 00:02:54 发布

阅读量14

点赞数

原文链接：https://mp.weixin.qq.com/s?__biz=MzU1MjY4MTA1MQ==&mid=2247727675&idx=4&sn=d1422df5c5d96e930cd6d07b67bb973e&chksm=fa9023176a3ab9abb8655a3637f41d801daef179acb74f8e363bb44273042a4033d3c2a73ff3&scene=126&sessionid=0

版权

来源：古月居

「3D视觉从入门到精通」知识星球(点开有惊喜) ！星球内新增20多门3D视觉系统课程、入门环境配置教程、多场顶会直播、顶会论文最新解读、3D视觉算法源码、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

一、安装依赖

声明：本次使用的是ubuntu 20.04 + ros1 noetic

sudo apt install ros-noetic-joint-state-publisher-guisudo apt install ros-noetic-ros-controlsudo apt install ros-noetic-ros-controllerssudo apt install ros-noetic-ackermann-msgssudo apt install ros-noetic-gmappingsudo apt install ros-noetic-gmappingsudo apt install ros-noetic-teb-local-plannersudo apt install ros-noetic-map-serversudo apt-get install ros-noetic-octomap-rossudo apt-get install ros-noetic-octomap-msgssudo apt-get install ros-noetic-octomap-serversudo apt-get install ros-noetic-octomap-rviz-plugins

下面是更改好的小车平台，相对来说简单易上手：小车平台（https://t11n5ozh20.feishu.cn/docx/OxBRdgrJronAz4xX7yUc6j8unCc#share-UKlfdSAcVoOwZXxFn11cW0uhnvd）

二、Readme

本小车使用github上的一个阿克曼小车仿真更改过来的。

1.获取功能包

mkdir -p car_ws/srccd car_ws/srcgit clonecd ..catkin_make

编译完成后就可以使用了,模型树状图如下，配备了一个深度相机，一个单目相机，一个2d雷达（模型文件里面启动就好，我屏蔽了），一个imu，一个velodyne vlp16雷达惯导平台。

暂时无法在飞书文档外展示此内容下面是启动小车仿真后的tf树：

2.racebot_description

racebot_description中的小车控制使用的是后驱

# rviz模型展示roslaunch racebot_description tianracer_rviz.launch# gazebo演示roslaunch racebot_description racecar_gazebo.launch#键盘控制rosrun racebot_description teleop_racebot.py3.racebot_gazeboracebot_gazebo中的小车采用的是四驱#仿真加键盘roslaunch racebot_gazebo tianracer.launch

三、3dslam加octomap

这里用里程计和camera_init做了静态映射，并使用odom作为八叉树地图的frame_id，这样就可以一边构建3dslam地图一边构建八叉树地图，而且构建八叉树地图的时候也是在构建2d栅格地图，使用该方法进行仿真建图对电脑的性能需求更高一点，在电脑性能不是很足的时候更推荐单独使用octomap进行建图（同样的使用里程计作为frame_id，效果是差不多的，这样甚至可以免除开启gmapping的烦恼），也可以使用FAST_LIO_LOCALIZATION重定位来进行frame_id的选取。

<launch>  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->    <param name="resolution" value="0.1" />
    <!-- name of the fixed frame, needs to be "/map"for SLAM -->    <param name="frame_id" type="string" value="odom" />
    <!-- max range / depth resolution of the kinect in meter -->    <param name="sensor_model/max_range" value="1000.0" />    <param name="latch" value="true" />
    <!-- max/min height for occupancy map, should be in meters -->    <param name="pointcloud_max_z" value="3" />    <param name="pointcloud_min_z" value="0" />
    <!-- topic from where pointcloud2 messages are subscribed -->    <remap from="/cloud_in" to="/velodyne_points" />
  </node>  <!-- 发布一个雷达body到机器人足端body_foot的静态映射 -->  <!--<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 base_link base_footprint" />-->  <!-- 发布一个雷达初始位置camera_init到odom的静态映射 -->  <node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="0 0 0 0 0 0 camera_init odom" />

四、gmapping加octomap

roslaunch racebot_gazebo tianracer.launchroslaunch racebot_gazebo tianracer_gmapping.launch roslaunch racebot_gazebo pointCloud2LaserScan.launchroslaunch racebot_gazebo octomaptransform.launch#保存八叉树地图rosrun octomap_server octomap_saver -f octomap.bt#保存2d地图rosrun map_server map_saver map:=/projected_map -f 2d

2d地图构建八叉树地图使用map

<launch>  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->    <param name="resolution" value="0.1" />
    <!-- name of the fixed frame, needs to be "/map"for SLAM -->    <param name="frame_id" type="string" value="map" />
    <!-- max range / depth resolution of the kinect in meter -->    <param name="sensor_model/max_range" value="1000.0" />    <param name="latch" value="true" />
    <!-- max/min height for occupancy map, should be in meters -->    <param name="pointcloud_max_z" value="3" />    <param name="pointcloud_min_z" value="0" />
    <!-- topic from where pointcloud2 messages are subscribed -->    <remap from="/cloud_in" to="/velodyne_points" />
  </node>  <!-- 发布一个雷达body到机器人足端body_foot的静态映射 -->  <!--<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 base_link base_footprint" />-->  <!-- 发布一个雷达初始位置camera_init到base的静态映射 -->  <!--<node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="0 0 0 0 0 0 camera_init odom" />-->

五、Relocalization

在上面小车算法验证平台上我们使用了机器人里程计来完成机器人的定位（基于机器人里程计的定位），但基于机器人里程计的定位需要确保机器人每次的初始位置都一样，而且我们无法保证机器人的里程计不产生误差，为了避免这种情况的发生，我们需要使用重定位来完成对机器人的定位工作。

这里我们使用一个开源的重定位项目：FAST_LIO_LOCALIZATION，一个基于fast-lio构建的地图实现的Relocalization，廷有意思的，这是一个采用python构建的，如果执行效率有一定要求的话可以试试：livox_relocalization，做动态的话可以尝试:slam_toolbox。

1.安装依赖

声明：本次使用的是ubuntu 20.04 + ros1 noetic

sudo apt-get install ros-noetic-octomap-rossudo apt-get install ros-noetic-octomap-msgssudo apt-get install ros-noetic-octomap-serversudo apt-get install ros-noetic-octomap-rviz-pluginssudo apt install ros-noetic-ros-numpypip install numpypip install open3d

2.获取功能包

1）获取livox_ros_driver

在终端中输入：

git clone https://github.com/Livox-SDK/livox_ros_driver.git

2.获取fast_lio

在终端中输入：

git clone https://github.com/hku-mars/FAST_LIO.gitcd FAST_LIOgit submodule update --initcd ../..catkin_make

编译完后把他给删除了，我们只需要他生成的脚本。

3.获取FAST_LIO_LOCALIZATION

在终端中输入：

git clone https://github.com/davidakhihiero/FAST_LIO_LOCALIZATION-ROS-NOETIC.gitcd FAST_LIO_LOCALIZATION/git submodule update --initcd ../..catkin_makesource devel/setup.bash

将事先准备好的pcd文件导入FAST_LIO_LOCALIZATION的PCD文件中:

修改localization_velodyne.yaml如下：

common:    lid_topic:  "/velodyne_points"    imu_topic:  "/velodyne_imu"    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible    preprocess:    lidar_type: 2                # 1for Livox serials LiDAR, 2for Velodyne LiDAR, 3for ouster LiDAR,     scan_line: 32    blind: 4
mapping:    acc_cov: 0.1    gyr_cov: 0.1    b_acc_cov: 0.0001    b_gyr_cov: 0.0001    fov_degree:    180    det_range:     100.0    extrinsic_T: [ 0, 0, 0.28]    extrinsic_R: [ 1, 0, 0,                    0, 1, 0,                    0, 0, 1]
publish:         scan_publish_en:  1       # 'false' will close all the point cloud output    dense_publish_en: 1       # false will low down the points number in a global-frame point clouds scan.    scan_bodyframe_pub_en: 1  # output the point cloud scans in IMU-body-frame

修改localization_velodyne.launch文件如下

<launch>  <!-- Launch file for velodyne16 VLP-16 LiDAR -->
    <!--<include file="$(find velodyne_pointcloud)/launch/VLS128_points.launch"/>-->    <arg name="rviz"default="true" />
    <rosparam command="load" file="$(find fast_lio_localization)/config/velodyne.yaml" />
    <param name="feature_extract_enable" type="bool" value="0"/>    <param name="point_filter_num" type="int" value="4"/>    <param name="max_iteration" type="int" value="3" />    <param name="filter_size_surf" type="double" value="0.5" />    <param name="filter_size_map" type="double" value="0.5" />    <param name="cube_side_length" type="double" value="1000" />    <param name="runtime_pos_log_enable" type="bool" value="0" />    <param name="pcd_save_enable" type="bool" value="0" />    <node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
    <arg name="map"default="$(find fast_lio_localization)/PCD/scans_pointlio_car.pcd" />    <!-- loalization-->    <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
    <!-- transform  fusion-->    <node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />
    <!-- glbal map-->    <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"          args="$(arg map) 5 _frame_id:=map cloud_pcd:=map" />

    <group if="$(arg rviz)">      <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />    </group>  <!-- 发布一个雷达body到机器人足端base_link的静态映射 -->  <node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_1" args="-0.10 -0.11 0 0 0 0 body base_link" />  <!-- 发布一个map到雷达初始位置camera_init的静态映射 --><node pkg="tf2_ros" type="static_transform_publisher" name="tf_pub_2" args="-0.10 -0.11 0 0 0 0 camera_init odom" />  </launch>

映射是为了将机器人定位到规定的body上

4.测试

roslaunch racebot_gazebo tianracer.launchroslaunch fast_lio_localization localization_velodyne.launch rosrun fast_lio_localization publish_initial_pose.py 000000

重定位后的小车tf树(下面这颗tf树是已经进行了坐标映射后进行导航的tf树)

六、Relocalizatio加octomap

如上视频所演示的一样，我们已经初步完成的机器人在3d点云地图当中的重定位，而在前一篇小车算法验证仿真平台的教程中我们提到在3d地图中构建八叉树地图使用的就是基于机器人里程计的定位需要保证机器人每次的初始位置都一样，且无法保证里程计不会产生误差。接下来我们就是要将重定位与octomap的构建放到一起完成。 octomaptransform.launch文件如下：

<launch>  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">
    <!-- resolution in meters per pixel -->    <param name="resolution" value="0.1" />
    <!-- name of the fixed frame, needs to be "/map"for SLAM -->    <param name="frame_id" type="string" value="map" />
    <!-- max range / depth resolution of the kinect in meter -->    <param name="sensor_model/max_range" value="1000.0" />    <param name="latch" value="true" />
    <!-- max/min height for occupancy map, should be in meters -->    <param name="pointcloud_max_z" value="3" />    <param name="pointcloud_min_z" value="0" />
    <!-- topic from where pointcloud2 messages are subscribed -->    <remap from="/cloud_in" to="/velodyne_points" />
  </node></launch>

roslaunch racebot_gazebo tianracer.launch roslaunch fast_lio_localization localization_velodyne.launchrosrun fast_lio_localization publish_initial_pose.py 000000roslaunch racebot_gazebo octomaptransform.launch

在利用fast-lio-localization进行定位的时候我们发现其定位的及时性相对来说低一点，这与fast-lio-localization本身是采用python构建的有一定的关系,这就导致使用该方法进行定位时机器人无法进行快速运动，快速移动时有会出现定位偏差的问题。(也有可能是我配置的问题)

最后，给大家推荐一款实用的小车