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

向高翔大佬学习，想写一套从零开始的激光SLAM博客记录一下自己学激光SLAM的过程，目前是研一学生，有大佬发现问题请告诉我，或者大家想看啥。希望囊括slam里ros的基本使用，激光特征提取，地面提取，位姿计算，ceres对于优化的使用，gtsam因子图优化使用，imu与激光雷达融合使用，GPS使用，闭环检测这些基础用法。引用一下高翔的话。大家加油呀。

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

这里推荐一个知乎专栏，从零开始做激光SLAM（文章汇总）本节目标：了解launch，package.xml,rviz文件的写法与使用，接收rosbag点云数据，然后分两层输出到rviz。预期效果：
一起做激光SLAM[一]ros里SLAM常用功能的熟悉 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
一起做激光SLAM[一]ros里SLAM常用功能的熟悉

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 COMPONENTS
  geometry_msgs
  sensor_msgs
  roscpp
  rospy
  rosbag
  std_msgs
  image_transport
  cv_bridge
  tf
)
 
#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_msgs
  DEPENDS 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

一起做激光SLAM[一]ros里SLAM常用功能的熟悉相比于原始的rviz文件，我程序添加了tf信息，两个显示量。 tf为了建立世界统一坐标系，赋予原点。

    - Class: rviz/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        aft_mapped:
          Value: true
        map:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        map:
          aft_mapped:
            {}
      Update Interval: 0
      Value: true

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

    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 10
        Min Value: -10
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 255; 85; 0
      Color Transformer: FlatColor
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 0
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: velodyne_cloud_down
      Position Transformer: XYZ
      Queue Size: 10
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.00999999978
      Style: Points
      Topic: /velodyne_cloud_down
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 20.4566002
        Min Value: -3.58307004
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz/PointCloud2
      Color: 255; 255; 0
      Color Transformer: FlatColor
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 96
      Min Color: 0; 0; 0
      Min Intensity: 1
      Name: velodyne_cloud_up
      Position Transformer: XYZ
      Queue Size: 2
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.00999999978
      Style: Points
      Topic: /velodyne_cloud_up
      Unreliable: false
      Use Fixed Frame: true
      Use rainbow: true
      Value: true

颜色，点大小都可以调，颜色可以按点云坐标，固定颜色，强度赋值。
一起做激光SLAM[一]ros里SLAM常用功能的熟悉

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'

一起做激光SLAM[一]ros里SLAM常用功能的熟悉后面我们的处理都在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;
}

安装

cmakelist和package.xml

rviz

launch

cpp

您必须 登录 才能发表评论！

您必须登录才能发表评论！