3D-SLAM入门教程-多线雷达(velodyne 16)BLAM 三维建图

ROS与SLAM入门教程-多线雷达(velodyne 16)BLAM 三维建图

说明：

• 介绍如何安装和使用BLAM算法库

步骤：

• 安装依赖

#Boost >= 1.43
sudo apt-get install libboost-all-dev
#CMake >= 2.6
sudo apt-get install cmake

git clone https://bitbucket.org/gtborg/gtsam.git
cd gtsam
mkdir build
cd build
cmake ..
sudo make install

• 安装blam

mkdir ~/blam_ws 
cd ~/blam_ws 
git clone https://github.com/erik-nelson/blam.git
cd blam 
./update 

问题一：

• 错误：fatal error: ros/ros.h: No such file or directory
• 修改cd blam/internal/src/geometry_utils
• 添加以下两行到 package.xml

<build_depend>roscpp</build_depend>
<run_depend>roscpp</run_depend>

• 添加以下下两行到CMakeList.txt

find_package(catkin REQUIRED COMPONENTS roscpp)
include_directories(include ${catkin_INCLUDE_DIRS})

问题二：

• 错误：Invalid (NaN, Inf) point coordinates given to nearestKSearch!
• 修改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);
}

• 再修改blam/internal/src/blam_slam/src/BlamSlam.cc

//1、change loop_closure_.AddKeyScanPair(0, msg); to
loop_closure_.AddKeyScanPair(0, msg_filtered);  

//2、change if (HandleLoopClosures(msg, &new_keyframe)) to
if (HandleLoopClosures(msg_filtered, &new_keyframe))

//3、localization_.TransformPointsToFixedFrame(*msg, msg_fixed.get());
localization_.TransformPointsToFixedFrame(*msg_filtered, msg_fixed.get());

• 提示：Assertion key_arg.x <= this->max_key_.x' failed
• 修改blam/internal/src/point_cloud_mapper/src/PointCloudMapper.cc

if (!map_octree_->isVoxelOccupiedAtPoint(p)) {

to:

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)) {

• 修改后重新编译

测试：

• 加载环境

source ~/blam_ws/blam/internal/devel/setup.bash

• 在线模式：
• 使用/velodyne_points话题，消息类型为sensor_msgs::PointCloud2

roslaunch blam_example test_online.launch

• 如是其他多线雷达，修改test_online.launch文件订阅的话题

<remap from="~pcld" to="/rslidar_points"/>

• 离线模式：
• 先执行rosbag文件，如

rosbag play velodyne.bag

• 再执行

  roslaunch blam_example test_offline.launch

• 启动rviz

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

• 效果图：

常见问题：

• 问题一：效果不好
• 注意查看自己的点云数据量和velodyne的区别，如果点云数量少就修改point_cloud_filter/config/parameters.yaml

#关闭voxel grid filter
grid_filter: false

• 问题二：如果效果还不好，旋转特别大
• 修改point_cloud_localization/config/parameters.yaml

# Maximum acceptable incremental rotation and translation.
transform_thresholding: true #false
max_translation: 0.5 #0.05
max_rotation: 0.3 #0.1

• 问题三：不回环
• 观察你的pose相差多远，修改laser_loop_closure/config/parameters.yaml

#默认0.5米检测一次回环或记录关键帧，值越大效率越高，发现构图缓慢的时候可以放大这个值
translation_threshold: 1.0 #0.5

#默认是在1.5米范围内匹配，根据你自己的回环误差放大这个值
proximity_threshold: 10 #1.5

#ICP "fitness score" must be less than this number，就是越小越难回环
max_tolerable_fitness: 5 #0.15

#根据你自己的地图大小适当放宽跳过回环的点数，一般地图越大值越大
skip_recent_poses: 100 #40
poses_before_reclosing: 100 #40

• 问题四：看到位姿回环并修正了，但是点云没有被修正
• 取消订阅/blam/blam_slam/octree_map_updates再重新订阅就看到了

参考：

• https://github.com/erik-nelson/blam
• https://youtu.be/08GTGfNneCI
• https://blog.csdn.net/xmy306538517/article/details/81122663