一起做激光SLAM[一]ros里SLAM常用功能的熟悉

向高翔大佬学习，想写一套从零开始的激光SLAM博客记录一下自己学激光SLAM的过程，目前是研一学生，有大佬发现问题请告诉我，或者大家想看啥。

希望囊括slam里ros的基本使用，激光特征提取，地面提取，位姿计算，ceres对于优化的使用，gtsam因子图优化使用，imu与激光雷达融合使用，GPS使用，闭环检测这些基础用法。

引用一下高翔的话。大家加油呀。

一个完整的程序含有大量的算法与GUI的代码，你读一遍需要多久？弄清楚原理要多久？
别人工具都做好了，代码都写完了，参数也调好了，你拿过来运行。那顶多给别人做一下评测。
你迟早要自己写代码。

这里推荐一个知乎专栏，从零开始做激光SLAM（文章汇总）

本节目标：了解launch，package.xml,rviz文件的写法与使用，接收rosbag点云数据，然后分两层输出到rviz。

预期效果：

rosbag数据: https://pan.baidu.com/s/1GaZ2eGZdfc-cluSc-bkQng 提取码: 9zsp

程序：https://gitee.com/eminbogen/one_liom

安装

cmakelist和package.xml

rviz

launch

cpp

安装

首先要安装ros

例如：https://blog.csdn.net/unlimitedai/article/details/105390609

之后是pcl和ceres，个人选择源码安装。

下载好rosbag数据，再下载我的github中程序，解压如图，右键cmd输入catkin_make

eminbogen@eminbogen-To-be-filled-by-O-E-M:~/my_ros/one_liom_all/one_liom1$ catkin_make

产生build与之后在自己home路径下找.bashrc 比如我的就是 /home/eminbogen/.bashrc（显示隐藏文件用ctrl+h）

在最后添加setup.bash源记得改成你的路径

source /home/eminbogen/my_ros/one_liom_all/one_liom1/devel/setup.bash

之后在两个cmd中输入（记得改rosbag路径）

roslaunch one_liom run.launch

rosbag play '/home/eminbogen/data/nsh_indoor_outdoor.bag'

就可以产生目标效果图。

cmakelist和package.xml

cmakelist设置一下编译环境，引用库，联系c++文件和库

cmake_minimum_required(VERSION 2.8.3)
project(one_liom)set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")find_package(catkin REQUIRED COMPONENTSgeometry_msgssensor_msgsroscpprospyrosbagstd_msgsimage_transportcv_bridgetf
)#find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Ceres REQUIRED)include_directories(include${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}${CERES_INCLUDE_DIRS}${OpenCV_INCLUDE_DIRS})catkin_package(CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgsDEPENDS EIGEN3 PCL INCLUDE_DIRS include
)add_executable(rosbagregist src/rosbagregist.cpp)
target_link_libraries(rosbagregist ${catkin_LIBRARIES} ${PCL_LIBRARIES})

package.xml添加描述和依赖库

<?xml version="1.0"?>
<package format="2"><name>one_liom</name><version>0.0.0</version><description>The one_liom package</description><maintainer email="eminbogen@163.com">hcx</maintainer><license>TODO</license><buildtool_depend>catkin</buildtool_depend><build_depend>geometry_msgs</build_depend><build_depend>roscpp</build_depend><build_depend>rospy</build_depend><build_depend>std_msgs</build_depend><build_depend>turtlesim</build_depend><build_export_depend>geometry_msgs</build_export_depend><build_export_depend>roscpp</build_export_depend><build_export_depend>rospy</build_export_depend><build_export_depend>std_msgs</build_export_depend><build_export_depend>turtlesim</build_export_depend><exec_depend>geometry_msgs</exec_depend><exec_depend>roscpp</exec_depend><exec_depend>rospy</exec_depend><exec_depend>std_msgs</exec_depend><exec_depend>turtlesim</exec_depend><build_depend>message_generation</build_depend><exec_depend>message_runtime</exec_depend><export></export>
</package>

rviz

启动原始rviz可以使用两个cmd打开，使用rviz可以调节视角，参数，增加观察量，保存时保存为rviz参数文件，

roscore

rosrun rviz rviz

相比于原始的rviz文件，我程序添加了tf信息，两个显示量。

tf为了建立世界统一坐标系，赋予原点。

    - Class: rviz/TFEnabled: trueFrame Timeout: 15Frames:All Enabled: trueaft_mapped:Value: truemap:Value: trueMarker Scale: 1Name: TFShow Arrows: trueShow Axes: trueShow Names: trueTree:map:aft_mapped:{}Update Interval: 0Value: true

两个显示的rviz/PointCloud2分别是上下两半点云。

    - Alpha: 1Autocompute Intensity Bounds: trueAutocompute Value Bounds:Max Value: 10Min Value: -10Value: trueAxis: ZChannel Name: intensityClass: rviz/PointCloud2Color: 255; 85; 0Color Transformer: FlatColorDecay Time: 0Enabled: trueInvert Rainbow: falseMax Color: 255; 255; 255Max Intensity: 0Min Color: 0; 0; 0Min Intensity: 0Name: velodyne_cloud_downPosition Transformer: XYZQueue Size: 10Selectable: trueSize (Pixels): 3Size (m): 0.00999999978Style: PointsTopic: /velodyne_cloud_downUnreliable: falseUse Fixed Frame: trueUse rainbow: trueValue: true- Alpha: 1Autocompute Intensity Bounds: trueAutocompute Value Bounds:Max Value: 20.4566002Min Value: -3.58307004Value: trueAxis: ZChannel Name: intensityClass: rviz/PointCloud2Color: 255; 255; 0Color Transformer: FlatColorDecay Time: 0Enabled: trueInvert Rainbow: falseMax Color: 255; 255; 255Max Intensity: 96Min Color: 0; 0; 0Min Intensity: 1Name: velodyne_cloud_upPosition Transformer: XYZQueue Size: 2Selectable: trueSize (Pixels): 3Size (m): 0.00999999978Style: PointsTopic: /velodyne_cloud_upUnreliable: falseUse Fixed Frame: trueUse rainbow: trueValue: true

颜色，点大小都可以调，颜色可以按点云坐标，固定颜色，强度赋值。

launch

launch说明一下程序的名字，启动的节点，启动rviz

<launch><node pkg="one_liom" type="rosbagregist" name="rosbagregist" output="screen" /><arg name="rviz" default="true" /><group if="$(arg rviz)"><node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find one_liom)/rviz_cfg/rviz.rviz" /></group></launch>

cpp

主程序我们希望接收rosbag的数据，转换到pcl格式，使用仰角作为计算标准，将点云赋予1-16线号，并将1-8和9-16分别输出rviz，期间为了rviz显示还需要tf数据，我们使用0原点建立参考系。

1.接收数据，主程序中sub为接收数据，当接收到topic为/velodyne_points数据时，会启动cloud_Callhandle程序对点云数据进行处理。

//ros初始化，建立节点，接收topic为/velodyne_points的点云数据，发送
//topic为/velodyne_cloud_up，down的点云，循环
int main(int argc, char **argv)
{ ros::init(argc, argv, "rosbagregist");ros::NodeHandle n;ros::Subscriber cloud_sub = n.subscribe("/velodyne_points", 100, cloud_Callhandle);pubLaserCloudup = n.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_up", 100);pubLaserClouddown = n.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_down", 100);ros::spin();return 0;
}

以后我们还会用其他rosbag，要了解rosbag里数据的格式使用cmd输入（改路径）

rosbag info '/home/eminbogen/dash_indoor_outdoor.bag'

后面我们的处理都在cloud_Callhandle程序里。

2.将点云从ros的msg格式转换为pcl格式，并计算各点的仰角分析其线数，保存在laserCloudScans(N_SCANS)里

    //接收点云转换为PCL格式pcl::PointCloud<pcl::PointXYZ> laserCloudIn;pcl::fromROSMsg(ros_cloud, laserCloudIn);//计数用于循环int cloudSize = laserCloudIn.points.size();int count = cloudSize;PointType point;//判定各点的线数，按线数保存std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);for (int i = 0; i < cloudSize; i++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;//仰角float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;int scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}laserCloudScans[scanID].push_back(point);}

3.按1-8,9-16线保存点云，并转化为ros的msg格式输出,输出的topic在主函数的pub处声明过

    //按1-8,9-16线保存pcl::PointCloud<PointType>::Ptr laserCloudup(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr laserClouddown(new pcl::PointCloud<PointType>());for (int i = 0; i < N_SCANS/2; i++){ *laserCloudup += laserCloudScans[i];}for (int i = N_SCANS/2; i < N_SCANS; i++){ *laserClouddown += laserCloudScans[i];}//pcl转ros输出格式，map是统一坐标系sensor_msgs::PointCloud2 laserCloudupOutMsg;pcl::toROSMsg(*laserCloudup, laserCloudupOutMsg);laserCloudupOutMsg.header.stamp = ros_cloud.header.stamp;laserCloudupOutMsg.header.frame_id = "map";pubLaserCloudup.publish(laserCloudupOutMsg);sensor_msgs::PointCloud2 laserClouddownOutMsg;pcl::toROSMsg(*laserClouddown, laserClouddownOutMsg);laserClouddownOutMsg.header.stamp = ros_cloud.header.stamp;laserClouddownOutMsg.header.frame_id = "map";pubLaserClouddown.publish(laserClouddownOutMsg);

4.为了rviz显示还需要tf数据，我们使用0原点建立参考系

    //输出世界参考系的坐标static tf::TransformBroadcaster br;tf::Transform transform;tf::Quaternion q;transform.setOrigin(tf::Vector3(0,0,0));q.setW(1);q.setX(0);q.setY(0);q.setZ(0);transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform, ros_cloud.header.stamp, "map", "map_child"));

整体程序如下

#include <cmath>
#include <vector>
#include <string>
//ros用
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
//世界统一参考系用
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
//pcl
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>typedef pcl::PointXYZI PointType;int N_SCANS=16;
ros::Publisher pubLaserCloudup;
ros::Publisher pubLaserClouddown;void cloud_Callhandle(const sensor_msgs::PointCloud2 ros_cloud)
{//输出世界参考系的坐标static tf::TransformBroadcaster br;tf::Transform transform;tf::Quaternion q;transform.setOrigin(tf::Vector3(0,0,0));q.setW(1);q.setX(0);q.setY(0);q.setZ(0);transform.setRotation(q);br.sendTransform(tf::StampedTransform(transform, ros_cloud.header.stamp, "map", "map_child"));//接收点云转换为PCL格式pcl::PointCloud<pcl::PointXYZ> laserCloudIn;pcl::fromROSMsg(ros_cloud, laserCloudIn);//计数用于循环int cloudSize = laserCloudIn.points.size();int count = cloudSize;PointType point;//判定各点的线数，按线数保存std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);for (int i = 0; i < cloudSize; i++){point.x = laserCloudIn.points[i].x;point.y = laserCloudIn.points[i].y;point.z = laserCloudIn.points[i].z;//仰角float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;int scanID = 0;if (N_SCANS == 16){scanID = int((angle + 15) / 2 + 0.5);if (scanID > (N_SCANS - 1) || scanID < 0){count--;continue;}}laserCloudScans[scanID].push_back(point);}//按1-8,9-16线保存pcl::PointCloud<PointType>::Ptr laserCloudup(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr laserClouddown(new pcl::PointCloud<PointType>());for (int i = 0; i < N_SCANS/2; i++){ *laserCloudup += laserCloudScans[i];}for (int i = N_SCANS/2; i < N_SCANS; i++){ *laserClouddown += laserCloudScans[i];}//pcl转ros输出格式，map是统一坐标系sensor_msgs::PointCloud2 laserCloudupOutMsg;pcl::toROSMsg(*laserCloudup, laserCloudupOutMsg);laserCloudupOutMsg.header.stamp = ros_cloud.header.stamp;laserCloudupOutMsg.header.frame_id = "map";pubLaserCloudup.publish(laserCloudupOutMsg);sensor_msgs::PointCloud2 laserClouddownOutMsg;pcl::toROSMsg(*laserClouddown, laserClouddownOutMsg);laserClouddownOutMsg.header.stamp = ros_cloud.header.stamp;laserClouddownOutMsg.header.frame_id = "map";pubLaserClouddown.publish(laserClouddownOutMsg);
}//ros初始化，建立节点，接收topic为/velodyne_points的点云数据，发送
//topic为/velodyne_cloud_up，down的点云，循环
int main(int argc, char **argv)
{ ros::init(argc, argv, "rosbagregist");ros::NodeHandle n;ros::Subscriber cloud_sub = n.subscribe("/velodyne_points", 100, cloud_Callhandle);pubLaserCloudup = n.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_up", 100);pubLaserClouddown = n.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_down", 100);ros::spin();return 0;
}