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

专栏系列文章如下：

一：Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono)，基于视觉-激光-惯导里程计的SLAM框架，环境搭建和跑通过程_goldqiu的博客-CSDN博客

二.激光SLAM框架学习之A-LOAM框架---介绍及其演示_goldqiu的博客-CSDN博客

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

CMakeLists文件：

find_package(catkin REQUIRED COMPONENTSgeometry_msgsnav_msgssensor_msgsroscpprospyrosbagstd_msgsimage_transportcv_bridgetf
)

该框架使用了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_msgsDEPENDS 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转成浮点型floatfloat 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.项目文件介绍（除主要源码部分）相关推荐

十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
十九.激光和惯导LIO-SLAM框架学习之项目工程代码介绍---代码框架和一些文件解释
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
四.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---2.scanRegistration.cpp--前端雷达处理和特征提取
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
激光SLAM入门学习笔记
激光SLAM入门学习笔记激光SLAM入门学习笔记一.推荐阅读书籍二.推荐公众号.知乎.博客 1.公众号 2.知乎 3.博客三.推荐阅读论文&代码(参考泡泡机器人) 2D激光SLAM 3 ...
彻底搞懂基于LOAM框架的3D激光SLAM全套学习资料汇总！
地图定位算法是自动驾驶模块的核心,而激光SLAM则是地图定位算法的关键技术,其重要性不言而喻,在许多AI产品中应用非常多(包括但不限于自动驾驶.移动机器人.扫地机等).相比于传统的视觉传感器,激光传感 ...
激光SLAM算法学习（三）——3D激光SLAM
3D激光SLAM 1.3D激光SLAM的介绍 3D激光SLAM的输入: IMU数据 3D激光雷达数据里程计数据 3D激光SLAM的输出: 3D点云地图机器人的轨迹 or PoseGraph 2.3 ...
激光slam课程学习笔记－－第11课：3D激光SLAM介绍
前言:这系列笔记是学习曾书格老师的激光slam课程所得,这里分享只是个人理解,有误之处,望大佬们赐教.这节课主要介绍一些3d slam数学知识,以及典型代表loam. 1. 3d激光SLAM 1.1 ...

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

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

最新文章

热门文章