5-LVI-SAM源码分析_imageProjection初步分析

2021SC@SDUSC

LVI-SAM源码分析_imageProjection.cpp初步分析

一、点云结构体

​ 在文件的最开始，先定义了一个结构体点图，根据雷达Velodyne来定义。其中，每个变量的定义如下：

struct PointXYZIRT
{
   
   
    PCL_ADD_POINT4D		// 表示欧几里得 xyz 坐标和强度值的点结构。
    PCL_ADD_INTENSITY;	// 激光点反射的强度，也可以存点的索引，里面是一个float 类型的变量
    uint16_t ring;		//扫描的激光线
    float time;			// 时间
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
// 注册为PCL点云格式
POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT,  
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (float, time, time)
)

二、ImageProjection

1. 成员变量

// imu队列、odom队列互斥锁
std::mutex imuLock;
std::mutex odoLock;

// 订阅原始激光点云
ros::Subscriber subLaserCloud;
ros::Publisher  pubLaserCloud;

// 发布当前帧校正后点云，有效点
ros::Publisher pubExtractedCloud;
ros::Publisher pubLaserCloudInfo;

// imu数据队列（原始数据，转lidar系下）
ros::Subscriber subImu;
std::deque<sensor_msgs::Imu> imuQueue;

// imu里程计队列
ros::Subscriber subOdom;
std::deque<nav_msgs::Odometry> odomQueue;

// 激光点云数据队列
std::deque<sensor_msgs::PointCloud2> cloudQueue;
// 队列front帧，作为当前处理帧点云
sensor_msgs::PointCloud2 currentCloudMsg;

// 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳；用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态
double *imuTime = new double[queueLength];
double *imuRotX = new double[queueLength];
double *imuRotY = new double[queueLength];
double *imuRotZ = new double[queueLength];

int imuPointerCur;
bool firstPointFlag;
Eigen::Affine3f transStartInverse;

// 当前帧原始激光点云
pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
// 当期帧运动畸变校正之后的激光点云
pcl::PointCloud<PointType>::Ptr   fullCloud;
// 从fullCloud中提取有效点
pcl::PointCloud<PointType>::Ptr   extractedCloud;

int deskewFlag;
cv::Mat rangeMat;

bool odomDeskewFlag;
// 当前激光帧起止时刻对应imu里程计位姿变换，该变换对应的平移增量；用于插值计算当前激光帧起止时间范围内，每一时刻的位置
float odomIncreX;
float odomIncreY;
float odomIncreZ;

// 当前帧激光点云运动畸变校正之后的数据，包括点云数据、初始位姿、姿态角等，发布给featureExtraction进行特征提取
lvi_sam::cloud_info cloudInfo;
// 当前帧起始时刻
double timeScanCur;
// 下一帧的开始时刻
double timeScanNext;
// 当前帧header，包含时间戳信息
std_msgs::Header cloudHeader;

2. 构造函数

ImageProjection():
deskewFlag(0)
{
   
   
    // 订阅原始imu数据
    subImu        = nh.subscribe<sensor_msgs::Imu>        (imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
    // 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
    subOdom       = nh.subscribe<nav_msgs::Odometry>      (PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
    // 订阅原始lidar数据
    subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

    // 发布当前激光帧运动畸变校正后的点云，有效点
    pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> (PROJECT_NAME + "/lidar/deskew/cloud_deskewed", 5);
    // 发布当前激光帧运动畸变校正后的点云信息
    pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info>      (PROJECT_NAME + "/lidar/deskew/cloud_info", 5);

    // 初始化 分配内存
    allocateMemory();
    // 重置参数
    resetParameters();

    // pcl日志级别，只打ERROR日志
    pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}

3. 初始化函数

/**
 * 初始化
*/
void allocateMemory()
{
   
   
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
    fullCloud.reset(new pcl::PointCloud<PointType>());
    extractedCloud.reset(new pcl::PointCloud<PointType>());

    fullCloud->points.resize(N_SCAN*Horizon_SCAN);

    cloudInfo.startRingIndex.assign(N_SCAN, 0);
    cloudInfo.endRingIndex.assign(N_SCAN, 0);

    cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);
    cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);

    resetParameters();
}

/**
 * 重置参数，接收每帧lidar数据都要重置这些参数
*/
void resetParameters()
{
   
   
    laserCloudIn->clear();
    extractedCloud->clear();
    // reset range matrix for range image projection
    rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));

    imuPointerCur = 0;
    firstPointFlag = true;
    odomDeskewFlag = false;

    for (int i = 0; i < queueLength; ++i)
    {
   
   
        imuTime[i] = 0;
        imuRotX[i] = 0;
        imuRotY[i]