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

183
0
2021年1月31日 09时03分

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

 

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

 

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

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

 

这里推荐一个知乎专栏,从零开始做激光SLAM(文章汇总)

 

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

 

预期效果:

 

微信图片_20210126155822

 

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

 

微信图片_20210126155843

 

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

 

微信图片_20210126155950

 

相比于原始的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

 

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

 

微信图片_20210126160024

 

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' 

 

微信图片_20210126160102

 

后面我们的处理都在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;
}

发表评论

后才能评论