点云前端-LIOSAM
Kong Liangqian Lv6
 2022-02-27 12:59:22  

该节针对于图像投影节点

把整个点云转到cv_Mat上，方便做后续的处理

构造函数

topic 订阅发布

订阅

  • imu消息
  • 增量的odom
  • 点云的消息

发布

  • 运动补偿后的点云
  • 点云的相关信息
1
2
3
4
5
6
subImu        = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
subOdom       = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());

pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);

分配内存

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void allocateMemory()
{
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
    tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
    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();
}

回调函数

ImageProjection::imuHandler

储存imu到imu队列，把imu数据转到雷达坐标系？

1
2
3
4
5
6
7
8
9
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
    sensor_msgs::Imu thisImu = imuConverter(*imuMsg);

    std::lock_guard<std::mutex> lock1(imuLock);
    imuQueue.push_back(thisImu);


}

ImageProjection::odometryHandler

把odom加入到队列

1
2
3
4
5
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
    std::lock_guard<std::mutex> lock2(odoLock);
    odomQueue.push_back(*odometryMsg);
}

ImageProjection::cloudHandler

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    if (!cachePointCloud(laserCloudMsg))
        return;

    if (!deskewInfo())
        return;

    projectPointCloud();

    cloudExtraction();

    publishClouds();

    resetParameters();
}

cachePointCloud

转换点云为pcl的点云格式，然后存储起来

对相关的数据进行检查

deskewInfo

利用IMU获取运动补偿的信息

利用Odometry获取运动补偿

如果

 Comments