三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）

本文主要是介绍三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分），对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

整个项目是用ROS环境下的catkin make进行编译的，初学者主要关注include、launch、rviz_cfg、src文件夹和README、CMakeLists、package文件。

CMakeLists文件：

find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nav_msgs
  sensor_msgs
  roscpp
  rospy
  rosbag
  std_msgs
  image_transport
  cv_bridge
  tf
)

该框架使用了ROS自带的package。

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

add_executable(ascanRegistration src/scanRegistration.cpp)
target_link_libraries(ascanRegistration ${catkin_LIBRARIES} ${PCL_LIBRARIES})

add_executable(alaserOdometry src/laserOdometry.cpp)
target_link_libraries(alaserOdometry ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})

add_executable(alaserMapping src/laserMapping.cpp)
target_link_libraries(alaserMapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES})

add_executable(kittiHelper src/kittiHelper.cpp)
target_link_libraries(kittiHelper ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBS})

包含这些第三方库的头文件，将第三方库文件跟源文件进行关联。

catkin_package(
  CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs
  DEPENDS EIGEN3 PCL 
  INCLUDE_DIRS include
)

DEPENDS 和 CATKIN_DEPENDS 用来告诉 catkin ，我们建立的程序包有哪些依赖项。

README文件：

介绍了A-LOAM如何部署，还有一些数据集如何获取，作者的信息和Docker的配置。

Package.xml文件：

这是A-LOAM包信息，包含作者邮箱、包构建和运行的依赖项。

Include文件夹：

Common.h：

inline double rad2deg(double radians)
{
  return radians * 180.0 / M_PI;
}

inline double deg2rad(double degrees)
{
  return degrees * M_PI / 180.0;
}

该头文件定义了两个函数，一个是弧度转角度，一个是角度转弧度。

Tic_toc.h ：

class TicToc
{
  public:
    TicToc()
    {
        tic();
    }

    void tic()
    {
        start = std::chrono::system_clock::now();
    }

    double toc()
    {
        end = std::chrono::system_clock::now();
        std::chrono::duration<double> elapsed_seconds = end - start;
        return elapsed_seconds.count() * 1000;
    }

  private: 
        std::chrono::time_point<std::chrono::system_clock> start, end;
};

该头文件定义了一个类，是作者自己写的用于计时的类。用了C++的chrono时间库，调用system_clock类里面的now方法获取当前系统时间，在一个代码块的开头调用tic()方法（构造函数只需要实例化对象就可调用），结尾调用toc()方法，传回的参数就是代码块的执行时间，单位为ms。

Launch文件夹：

<launch>
    
    <param name="scan_line" type="int" value="16" />

    <!-- if 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -->
    <param name="mapping_skip_frame" type="int" value="1" />

    <!-- remove too closed points -->
    <param name="minimum_range" type="double" value="0.3"/>

    <param name="mapping_line_resolution" type="double" value="0.2"/>
    <param name="mapping_plane_resolution" type="double" value="0.4"/>

    <node pkg="aloam_velodyne" type="ascanRegistration" name="ascanRegistration" output="screen" />
    <node pkg="aloam_velodyne" type="alaserOdometry" name="alaserOdometry" output="screen" />

    <node pkg="aloam_velodyne" type="alaserMapping" name="alaserMapping" output="screen" />

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

</launch>

以aloam_velodyne_VLP_16.launch为例，launch文件中通过node符号可以开启多个节点，且rosmaster也同时开启了；同时还定义了包名，这在运行launch文件中会用到。通过param符号可作为参数服务器，以便在程序中读取。同时也可设置ROS自带显示软件RVIZ是否开启和开启的RVIZ配置路径。

rviz_cfg文件夹：

在这个文件夹存放rviz的配置文件，可在rviz软件中修改参数后进行保存。

src文件夹：

kittiHelper.cpp：

这个源文件的作用是将kitti数据转换成ros下的话题数据，并可选择保存为bag文件。首先我们先看主函数的代码注释。

int main(int argc, char** argv)
{
    ros::init(argc, argv, "kitti_helper");  //该节点名称，且初始化
    ros::NodeHandle n("~");
    std::string dataset_folder, sequence_number, output_bag_file;
    n.getParam("dataset_folder", dataset_folder); 
    //从参数服务器获取数据集文件夹和对应号码，可在launch文件中根据数据存放地址来修改
    n.getParam("sequence_number", sequence_number);
    std::cout << "Reading sequence " << sequence_number << " from " << dataset_folder << '\n';
    bool to_bag;
    n.getParam("to_bag", to_bag); //读取是否转换成bag标志
    if (to_bag)
        n.getParam("output_bag_file", output_bag_file); //同上，获取bag输出文件夹
    int publish_delay;
    n.getParam("publish_delay", publish_delay); //同上，获取发布延迟时间
    publish_delay = publish_delay <= 0 ? 1 : publish_delay; //三目运算符，延迟时间<=0 则设为1
    //初始化发布雷达话题数据
    ros::Publisher pub_laser_cloud = n.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 2);

    //用image_transport初始化发布左右相机数据，对ImageTransport类进行实例化
    image_transport::ImageTransport it(n);
    image_transport::Publisher pub_image_left = it.advertise("/image_left", 2);
    image_transport::Publisher pub_image_right = it.advertise("/image_right", 2);

    //初始化里程计发布，ground_truth包括旋转的四元数和位置坐标向量，这里没有用上
    ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
    nav_msgs::Odometry odomGT;
    odomGT.header.frame_id = "/camera_init";  
    odomGT.child_frame_id = "/ground_truth";

    ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
    nav_msgs::Path pathGT;
    pathGT.header.frame_id = "/camera_init";

    //获取时间戳地址
    std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
    std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);

    std::string ground_truth_path = "results/" + sequence_number + ".txt";
    std::ifstream ground_truth_file(dataset_folder + ground_truth_path, std::ifstream::in);
    rosbag::Bag bag_out;
    if (to_bag)
        bag_out.open(output_bag_file, rosbag::bagmode::Write); //新建并打开一个bag文件
    
    Eigen::Matrix3d R_transform;  //用在ground_truth数据中，这里没有用上
    R_transform << 0, 0, 1, -1, 0, 0, 0, -1, 0;
    Eigen::Quaterniond q_transform(R_transform);

    std::string line;
    std::size_t line_num = 0;

    ros::Rate r(10.0 / publish_delay); //ros循环频率

    // 遍历时间戳这个文本文件，得到时间戳信息
    std::cout << "timestamp_file " << std::string(dataset_folder + timestamp_path) << std::endl;
    while (std::getline(timestamp_file, line) && ros::ok())
    {
        // 把string转成浮点型float
        float timestamp = stof(line);
        std::stringstream left_image_path, right_image_path;
        // 找到对应的左右目的图片位置，并用opencv接口读取
        left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
        right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
        cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
        std::stringstream lidar_data_path;
        // 获取lidar数据的文件名
        lidar_data_path << dataset_folder << "velodyne/sequences/" + sequence_number + "/velodyne/" 
                        << std::setfill('0') << std::setw(6) << line_num << ".bin";
        //这里调用读雷达数据的函数
        std::vector<float> lidar_data = read_lidar_data(lidar_data_path.str());
        std::cout << "totally " << lidar_data.size() / 4.0 << " points in this lidar frame \n";

        std::vector<Eigen::Vector3d> lidar_points;
        std::vector<float> lidar_intensities;
        pcl::PointCloud<pcl::PointXYZI> laser_cloud;
        // 每个点数据占四个float数据，分别是xyz，intensity，存到laser_cloud容器中
        for (std::size_t i = 0; i < lidar_data.size(); i += 4)
        {
            lidar_points.emplace_back(lidar_data[i], lidar_data[i+1], lidar_data[i+2]);
            lidar_intensities.push_back(lidar_data[i+3]);
            // 构建pcl的点云格式
            pcl::PointXYZI point;
            point.x = lidar_data[i];
            point.y = lidar_data[i + 1];
            point.z = lidar_data[i + 2];
            point.intensity = lidar_data[i + 3];
            laser_cloud.push_back(point);
        }
        // 转成ros的消息格式
        sensor_msgs::PointCloud2 laser_cloud_msg;
        pcl::toROSMsg(laser_cloud, laser_cloud_msg);
        laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp); //设定雷达时间戳
        laser_cloud_msg.header.frame_id = "/camera_init"; //设定在相机坐标系下
        // 发布点云数据
        pub_laser_cloud.publish(laser_cloud_msg);
        // 图片也转成ros的消息发布出去
        sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
        sensor_msgs::ImagePtr image_right_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", right_image).toImageMsg();
        pub_image_left.publish(image_left_msg);
        pub_image_right.publish(image_right_msg);
        // 也可以写到rosbag包中去
        if (to_bag)
        {
            bag_out.write("/image_left", ros::Time::now(), image_left_msg);
            bag_out.write("/image_right", ros::Time::now(), image_right_msg);
            bag_out.write("/velodyne_points", ros::Time::now(), laser_cloud_msg);
            bag_out.write("/path_gt", ros::Time::now(), pathGT);  //下面两个实际没有数据
            bag_out.write("/odometry_gt", ros::Time::now(), odomGT);  
        }
        line_num ++;
        r.sleep();
    }
    bag_out.close();
    std::cout << "Done \n";
    return 0;
}

我们再看一下read_lidar_data函数，功能是将雷达每一帧的数据读取到一个float类型的vector容器中。

std::vector<float> read_lidar_data(const std::string lidar_data_path)
{
    std::ifstream lidar_data_file(lidar_data_path, std::ifstream::in | std::ifstream::binary);
    lidar_data_file.seekg(0, std::ios::end);    // 文件指针指向文件末尾
    const size_t num_elements = lidar_data_file.tellg() / sizeof(float);    // 统计一下文件有多少float数据
    lidar_data_file.seekg(0, std::ios::beg);    // 再把指针指向文件开始

    std::vector<float> lidar_data_buffer(num_elements);
    // 读取所有的数据
    lidar_data_file.read(reinterpret_cast<char*>(&lidar_data_buffer[0]), num_elements*sizeof(float));
    return lidar_data_buffer;
}

下一节讲解scanRegistration.cpp。

这篇关于三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！

Java教程

三.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---1.项目文件介绍（除主要源码部分）

前端开发

后端开发

移动端开发

数据库

服务器运维

人工智能

区块链

游戏开发

网站运营

大数据/云计算

软件工程

软件/开发工具使用

资讯