BLAM跑自己的数据包无法显示全局点云地图解决（速腾聚创RS-LiDAR-16 雷达）-SLAM不学无术小问题

^oprater^

已于 2025-04-02 16:08:17 修改

阅读量2.2k

点赞数 3

文章标签： slam

于 2021-04-21 11:15:25 首次发布

本文链接：https://blog.youkuaiyun.com/weixin_42141088/article/details/115875890

版权

BLAM算法跑自己的数据包无法显示全局点云地图解决（适配速腾聚创RS-LiDAR-16 雷达）

提示：本文笔者使用环境Ubuntu18.04，ROS melodic版本

首先放一个效果链接（由b站up：VladimirDuan上传，非官方）官方视屏外网才可播放。up使用的是64线雷达，效果可以说是很不错了链接: 64线激光雷达数据BLAM算法测试.（侵删）

背景

3D SLAM新手，笔者目前执着与尝试一些3D SLAM 算法，比较其优劣。所以在运行各种算法的时候难免因为各方面条件的差异导致建图失败，这里总结了笔者所遇到的一些问题以及解决方法，一方面记录日志，一方面帮助遇到相似问题的朋友提供解决问题的参考。笔者安装BLAM使用教程是这个：链接: BLAM（Berkeley Localization And Mapping）安装使用教程.大佬的教程已经非常详细，可能因为网络问题，安装过程可能需要多次update才可下载完毕，请耐心使用。
正文开始，笔者目前在尝试一种新的算法-----BLAM（Berkeley Localization And Mapping），在看到官网给出的图之后觉得算法非常不错，但是没有找到官方的数据集，只能用自己的数据集测试了，但是同笔者的另一文一样（链接.），也出现了无法查看全局地图的情况，如下图
在这里插入图片描述
但是因为有了上次跑lego-LOAM的经验之后，解决相对容易些。下面就此算法遇到的问题与解决方法各位分享。

一、确定自己的激光雷达类型

激光雷达的类型一直是建图的一个主要问题，因为经典的的算法源码作者无论是在编写过程还是测试调试过程使用的都倾向于—Velodyne 16线激光雷达VLP-16，对其他的雷达兼容性并不是很好，所以在算法使用过程中难免遇到各种矛盾。BLAM 算法可以处理VLP-16中的Nan点,但是对其它品牌的雷达式中的Nan点处理的不是那么兼容所以在开始就要确认好自己的雷达，或者是自己使用的数据包的雷达类型及其参数，有了这个基础以后相对容易些去修改兼容。
笔者自己使用的激光雷达是 RS-LiDAR-16 一款速腾聚创的自动驾驶激光雷达。这两款雷达的参数在笔者另一文中有介绍：链接: SLAM不学无术小问题：lego-LOAM跑自己的数据包无法显示全局点云地图解决（速腾聚创RS-LiDAR-16 雷达）.了解了区别之后就可以开始修改了，主要是针对launch文件以及原文中添加一些去NAN点的滤波器算法。

二、修改源码

1.修改 PointCloudFilter.cc

一般情况下，不是使用VLP-16雷达，可能会出现这样的报错：

Invalid (NaN, Inf) point coordinates given to nearestKSearch!

出现这个报错的原因是VLP-16雷达可以对NAN点进行很好的处理，但是其他雷达可能没那么兼容，所以需要自己在源码中添加一个滤波的过程滤除NAN点。当出现以上错误时，打开文件

//blam/internal/src/point_cloud_filter/src/PointCloudFilter.cc

在最后添加如下语句：（在最后一个大括号里面添加）

	if (!points->is_dense)
	{
	    points_filtered->is_dense = false;
	    std::vector<int> indices;
	    pcl::removeNaNFromPointCloud(*points_filtered,*points_filtered, indices);
	}

2.修改BlamSlam.cc

第一步修改之后效果不是那么好，那么打开文件

//blam/internal/src/blam_slam/src/BlamSlam.cc

修改以下三处：（即将msg的地方改为msg_filtered）

1、 loop_closure_.AddKeyScanPair(0, msg); 改为：
loop_closure_.AddKeyScanPair(0, msg_filtered);  

2、 if (HandleLoopClosures(msg, &new_keyframe)) 改为：
if (HandleLoopClosures(msg_filtered, &new_keyframe))

3、localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get()); 改为：
localization_.TransformPointsToFixedFrame(*msg_filtered, msg_fixed.get());

3.修改PointCloudMapper.cc

这一步非必要，视情况而定，一般情况下修改以上两处即可，但是笔者在经过以上两步的修改之后，运行launch问件没有问题都正常，但是一播放数据包时就会报错，类似下文：

process[blam/blam_slam-1]: started with pid [26609] [ INFO] [1541090716.155156906]: 
/blam/blam_slam/BlamSlam: Registering online callbacks. blam_slam_node: /build/pcl-OilVEB/pcl-
1.8.1+dfsg1/octree/include/pcl/octree/impl/octree_pointcloud.hpp:688: void 
pcl::octree::OctreePointCloud::genOctreeKeyforPoint(const PointT&, pcl::octree::OctreeKey&) const [with PointT = 
pcl::PointXYZ; LeafContainerT = pcl::octree::OctreeContainerPointIndices; BranchContainerT = 
pcl::octree::OctreeContainerEmpty; OctreeT = pcl::octree::OctreeBase<:octree::octreecontainerpointindices 
pcl::octree::octreecontainerempty>]: Assertion key_arg.x <= this->max_key_.x' failed. [blam/blam_slam-1] process 
has died [pid 26609, exit code -6, cmd /home/jun/catkin_ws/src/blam/internal/devel/lib/blam_slam/blam_slam_node ~pcld:=/velodyne_points 
__name:=blam_slam __log:=/home/jun/.ros/log/4cd720f2-ddec-11e8-bc37-e4a7a0130394/blam-blam_slam-1.log]
. log file: /home/jun/.ros/log/4cd720f2-ddec-11e8-bc37-e4a7a0130394/blam-blam_slam-1*.log all processes on 
machine have died, roslaunch will exit shutting down processing monitor... ... shutting down processing monitor 
complete done</:octree::octreecontainerpointindices>

其实主要的错误是这一句：

Assertion key_arg.x <= this->max_key_.x' failed

解决这个问题需要打开文件：

blam/internal/src/point_cloud_mapper/src/PointCloudMapper.cc

找到  if (!map_octree_->isVoxelOccupiedAtPoint(p)) {   此句注释，改为以下内容：

   double min_x, min_y, min_z, max_x, max_y, max_z;
   map_octree_->getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
   bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
   if (!isInBox || !map_octree_->isVoxelOccupiedAtPoint(p)) {

重新编译workspace通过。

三、修改launch文件（针对test_online.launch）

1.添加rviz（非必要）

关于lidar_slam.rviz文件，原作原文可能考虑到诸多因素并没有将初始化rviz的文件写入launch文件，需要另外的指令打开，但是笔者做测试用，希望launch以后直接打开rviz界面，所以这一部分是非必要修改，仅仅是为了直观方便。下面是操作步骤：
因为原作虽然未将rviz文件直接在launch文件中调用，但是仍然写了完整的rviz文件，在此目录下

/blam/internal/src/blam_example/rviz/lidar_slam.rviz

如果希望直接使用原作者的rviz文件，可以在launch文件添加如下语句：

 <!--- Run Rviz-->
 <node pkg="rviz" type="rviz" name="rviz" args="-d $(find blam_example)/rviz/lidar_slam.rviz" />

注意！！！添加时最好不要在group标签内添加，会报错（笔者深受其害）

2.重映射

这是SLAM学者比较基础的问题了，首先查看自己的数据包的话题：

rosbag info bagfile.bag

这是笔者自己的数据包的topic：

在这里插入图片描述

在launch 文件中作如下修改：（依照个人话题不同做修改）

//将第一行注释或删掉都可，添加第二句重映射即可
<remap from="~pcld" to="/velodyne_points"/>
<remap from="~pcld" to="/points_raw"/>

当然跑数据包不要忘记这里

// <!-- True for simulation, false for real-time and bagfiles -->
     <param name="/use_sim_time" value="true"/>

到此，笔者所遇到的问题基本上就这些，对原作的处理基本也就是这些。下面放上大家可能最感兴趣的launch即笔者自己的launch文件，仅供参考!!!。

<launch>
  <!-- True for simulation, false for real-time and bagfiles -->
  <param name="/use_sim_time" value="true"/>
  <!--- Run Rviz-->
  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find blam_example)/rviz/lidar_slam.rviz" />  
  <group ns="blam">
    <!-- SLAM -->
    <node pkg="blam_slam"
          name="blam_slam"
          type="blam_slam_node"
          output="screen">

      <!-- Topics -->
      <!--  <remap from="~pcld" to="/velodyne_points"/>   -->
      <remap from="~pcld" to="/points_raw"    /> 
     
      <!-- Initial pose -->
      <rosparam param="init">
        position: {x: 0.1, y: 0.1, z: 0.1}
        orientation: {roll: 0.0, pitch: 0.0, yaw: 0.0}
        position_sigma: {x: 0.1, y: 0.1, z: 0.1}
        orientation_sigma: {roll: 0.02, pitch: 0.02, yaw: 0.02}
      </rosparam>

      <!-- Rates -->
      <rosparam file="$(find blam_example)/config/blam_rates.yaml"/>

      <!-- Frames -->
      <rosparam file="$(find blam_example)/config/blam_frames.yaml"/>

      <!-- Point cloud filter -->
      <rosparam file="$(find point_cloud_filter)/config/parameters.yaml"/>

      <!-- Point cloud odometry -->
      <rosparam file="$(find point_cloud_odometry)/config/parameters.yaml"/>

      <!-- Point cloud localization -->
      <rosparam file="$(find point_cloud_localization)/config/parameters.yaml"/>

      <!-- Point cloud mapper -->
      <rosparam file="$(find point_cloud_mapper)/config/parameters.yaml"/>

      <!-- Point cloud visualization -->
      <rosparam file="$(find point_cloud_visualizer)/config/parameters.yaml"/>

      <!-- Loop closure -->
      <rosparam file="$(find laser_loop_closure)/config/parameters.yaml"/>
    </node>
  </group>
</launch>

四、整合测试

1.重新编译

做完以上所有步骤以后，需要重新编译整个workspace

catkin_make

2.运行

启动launch、数据包：播放数据包注意加–clock

//笔者暂时只测试了这个launch
 roslaunch blam_example test_online.launch
 
 rosbag play bagfile.bag  --clock

在这里插入图片描述

没有挂掉的线程，继续

此处注意，如果你未在第一步同笔者一样在launch文件中添加rviz文件，此时是没有变化的，（添加之后会直接弹出rviz）你需要再开一个终端运行另外一条打开rviz的指令：

rosrun rviz rviz -d blam_example/rviz/lidar_slam.rviz

这条指令以原作的初始化文件打开rviz。

3.笔者自己数据包建图效果

在这里插入图片描述

笔者使用的是16线的激光雷达，效果没有64线的好，但是也还可以。

总结

再次附上大佬链接： BLAM（Berkeley Localization And Mapping）安装使用教程.BLAM算法一个特点就是可调参数多，坏处是处处需要自己调，好处是处处可以自己调，可以说是相当个性化了，你可以根据自己建图的效果适时的自己去改变各种参数使得建图效果更佳精确。再次附上链接的原因是大佬在他的教程中给出了各种参数配置的一个参考，笔者在此就不一一赘述了。深入学习的话这些参数是需要自己去调试的。
好，感谢各位看到这里。3D SLAM新手，难免踩坑，难免出错，以上内容经供参考，也欢迎大家交流指正。

关于ROS
关于3D SLAM算法
关于 BLAM
不学无术的又一天

版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/weixin_42141088/article/details/115875890