针对Shan Tixiao开源的经典LIO框架LIO-SAM源代码进行逐行解读,总结。记录在这里希望能够给大家提供一些参考。
这里是第一部分,主要是开始的一些准备工作,包括 utility.h文件,cloud_info.msg文件等。
utility.h文件
utility.h文件中的内容还是比较容易理解的,其引用了各文件中所需的第三方库文件,构建了一个ParamServer类,读取各种参数。
// PCL相关
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>
// TF相关
#include <tf/LinearMath/Quaternion.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
// C++相关
#include <vector>
#include <cmath>
#include <algorithm>
#include <queue>
#include <deque>
#include <iostream>
#include <fstream>
#include <ctime>
#include <cfloat>
#include <iterator>
#include <sstream>
#include <string>
#include <limits>
#include <iomanip>
#include <array>
#include <thread>
#include <mutex>
using namespace std;
// 定义点类型
typedef pcl::PointXYZI PointType;
// C++11 中,枚举
enum class SensorType { VELODYNE, OUSTER };
// 定义一个类,读取一系列参数
class ParamServer
{
public:
ros::NodeHandle nh;
std::string robot_id;
//Topics
string pointCloudTopic;
string imuTopic;
string odomTopic;
string gpsTopic;
//Frames
string lidarFrame;
string baselinkFrame;
string odometryFrame;
string mapFrame;
// GPS Settings
bool useImuHeadingInitialization;
bool useGpsElevation;
float gpsCovThreshold;
float poseCovThreshold;
// Save pcd
bool savePCD;
string savePCDDirectory;
// Lidar Sensor Configuration
SensorType sensor;
int N_SCAN;
int Horizon_SCAN;
int downsampleRate;
float lidarMinRange;
float lidarMaxRange;
// IMU
float imuAccNoise;
float imuGyrNoise;
float imuAccBiasN;
float imuGyrBiasN;
float imuGravity;
float imuRPYWeight;
vector<double> extRotV;
vector<double> extRPYV;
vector<double> extTransV;
Eigen::Matrix3d extRot;
Eigen::Matrix3d extRPY;
Eigen::Vector3d extTrans;
Eigen::Quaterniond extQRPY;
// LOAM 特征点相关
float edgeThreshold;
float surfThreshold;
int edgeFeatureMinValidNum;
int surfFeatureMinValidNum;
// voxel filter paprams
float odometrySurfLeafSize;
float mappingCornerLeafSize;
float mappingSurfLeafSize ;
float z_tollerance;
float rotation_tollerance;
// CPU Params
int numberOfCores;
// seconds, regulate mapping frequency(参数文件中默认设置为0.15 s)
double mappingProcessInterval;
// Surrounding map
float surroundingkeyframeAddingDistThreshold;
float surroundingkeyframeAddingAngleThreshold;
float surroundingKeyframeDensity;
float surroundingKeyframeSearchRadius;
// Loop closure
bool loopClosureEnableFlag;
float loopClosureFrequency;
int surroundingKeyframeSize;
float historyKeyframeSearchRadius;
float historyKeyframeSearchTimeDiff;
int historyKeyframeSearchNum;
float historyKeyframeFitnessScore;
// global map visualization radius 全局地图可视化半径
float globalMapVisualizationSearchRadius;
float globalMapVisualizationPoseDensity;
float globalMapVisualizationLeafSize;
// 读取一系列参数
ParamServer()
{
nh.param<std::string>("/robot_id", robot_id, "roboat");
nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link");
nh.param<std::string>("lio_sam/baselinkFrame", baselinkFrame, "base_link");
nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom");
nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map");
nh.param<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false);
nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
nh.param<float>("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
nh.param<bool>("lio_sam/savePCD", savePCD, false);
nh.param<std::string>("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
std::string sensorStr;
nh.param<std::string>("lio_sam/sensor", sensorStr, "");
if (sensorStr == "velodyne")
{
sensor = SensorType::VELODYNE;
}
else if (sensorStr == "ouster")
{
sensor = SensorType::OUSTER;
}
else
{
ROS_ERROR_STREAM(
"Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr);
ros::shutdown();
}
nh.param<int>("lio_sam/N_SCAN", N_SCAN, 16);
nh.param<int>("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
nh.param<int>("lio_sam/downsampleRate", downsampleRate, 1);
nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0);
nh.param<float>("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);
nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01);
nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
extQRPY = Eigen::Quaterniond(extRPY);
nh.param<float>("lio_sam/edgeThreshold", edgeThreshold, 0.1);
nh.param<float>("lio_sam/surfThreshold", surfThreshold, 0.1);
nh.param<int>("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
nh.param<int>("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
nh.param<float>("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
nh.param<float>("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
nh.param<float>("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
// FLT_MAX最大的float值
nh.param<float>("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
nh.param<float>("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
nh.param<int>("lio_sam/numberOfCores", numberOfCores, 2);
nh.param<double>("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
nh.param<float>("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
nh.param<float>("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
// 当前帧相关关键帧位置控制,使其不太密
nh.param<float>("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
nh.param<float>("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
nh.param<float>("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);
nh.param<int>("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
nh.param<float>("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
nh.param<float>("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
nh.param<int>("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
nh.param<float>("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
nh.param<float>("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
nh.param<float>("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
nh.param<float>("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
usleep(100);
}
此外,其还包括几个常用的函数,方便在其他文件中进行使用:
// 发布点云函数
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
{
sensor_msgs::PointCloud2 tempCloud;
pcl::toROSMsg(*thisCloud, tempCloud);
tempCloud.header.stamp = thisStamp;
tempCloud.header.frame_id = thisFrame;
if (thisPub->getNumSubscribers() != 0)
thisPub->publish(tempCloud);
return tempCloud;
}
// C++中用于定义模板的固定格式
// 实现输入任何类型的msg消息,输出double类型的时间
template<typename T>
double ROS_TIME(T msg)
{
return msg->header.stamp.toSec();
}
/**
* @brief 输入IMU消息,输出角速度
*
* @tparam T
* @param thisImuMsg
* @param angular_x
* @param angular_y
* @param angular_z
*/
template<typename T>
void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
{
*angular_x = thisImuMsg->angular_velocity.x;
*angular_y = thisImuMsg->angular_velocity.y;
*angular_z = thisImuMsg->angular_velocity.z;
}
// 输入IMU消息,输出任何类型的加速度
template<typename T>
void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
{
*acc_x = thisImuMsg->linear_acceleration.x;
*acc_y = thisImuMsg->linear_acceleration.y;
*acc_z = thisImuMsg->linear_acceleration.z;
}
/**
* @brief 获取IMU消息中的磁力计朝向数据,转换成RPY
* IMU消息中的朝向,转换为欧拉角,rpy
* @tparam T
* @param thisImuMsg
* @param rosRoll
* @param rosPitch
* @param rosYaw
*/
template<typename T>
void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
{
double imuRoll, imuPitch, imuYaw;
tf::Quaternion orientation;
tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
*rosRoll = imuRoll;
*rosPitch = imuPitch;
*rosYaw = imuYaw;
}
// 返回点到坐标系原点的距离
float pointDistance(PointType p)
{
return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
}
// 两个点之间的距离
float pointDistance(PointType p1, PointType p2)
{
return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
}
#endif
cloud_info.msg文件
cloud_info.msg文件主要作用是构建了一个自定义的ROS Message类型。其中的各个变量包含不同的含义,在各函数中发挥不同的作用。
# Cloud Info
Header header
# 激光点的起始索引与终止索引(在所有激光点云中的序号)
int32[] startRingIndex
int32[] endRingIndex
# 当前点对应的列号与距离
int32[] pointColInd # point column index in range image
float32[] pointRange # point range
# 是否可以使用IMU原始数据以及IMU里程计数据进行去除运动畸变
int64 imuAvailable
int64 odomAvailable
# Attitude for LOAM initialization
# IMU磁力计测量值
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit
# Initial guess from imu pre-integration
# IMU里程计发布的位姿,对应每一帧激光点云的起始时刻
float32 initialGuessX
float32 initialGuessY
float32 initialGuessZ
float32 initialGuessRoll
float32 initialGuessPitch
float32 initialGuessYaw
# Point cloud messages
sensor_msgs/PointCloud2 cloud_deskewed # original cloud deskewed
sensor_msgs/PointCloud2 cloud_corner # extracted corner feature
sensor_msgs/PointCloud2 cloud_surface # extracted surface feature