10-visual_feature_lidar_callback

2021SC@SDUSC

visual_feature_lidar_callback

​ 经过上一篇blog——VINS-Mono的分析,我们现如今对LVI-SAM的visual_feature有了初步的了解。现在,开始对visual_feature的两个重要的回调函数开始分析。

​ 由上一篇blog,我们已知LVI-SAM利用lidar的相关信息获取到了图像中现实的深度信息,并且把信息进行处理作为图像信息的初始化数据。因此,这一篇blog我们先来研究lidar的回调函数。

​ 整个过程分为了9个步骤。

准备:降采样

​ 在开始处理数据前,我们为了减少数据的大小,提高处理的速度,进行降采样的操作。在这里,作者每4个就会跳过一次处理。

static int lidar_count = -1;
if (++lidar_count % (LIDAR_SKIP+1) != 0)
    return;

​ 在这里LIDAR_SKIP 在相同文件夹下的parameters.cpp已经定义,然后在前面Blog-8-visual_feature_初探中介绍过导入的函数。作者设计的值是3,也就是每隔3个就跳过一次。

lidar_skip: 3                    # skip this amount of scans

0. 接受位姿变换信息

​ 降采样通过之后,便可以正式开始处理数据。我们首先从laser_msg中,获取位姿的信息。

static tf::TransformListener listener;
static tf::StampedTransform transform;
try{
    listener.waitForTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, ros::Duration(0.01));
    listener.lookupTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, transform);
} 
catch (tf::TransformException ex){
    // ROS_ERROR("lidar no tf");
    return;
}

​ 然后从获取到的位姿信息中,提取x,y,z和旋转角的信息。然后通过旋转角获取RPY的三个角度,最后,在通过这6个信息,去获得当前位姿的变换矩阵。

double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
xCur = transform.getOrigin().x();
yCur = transform.getOrigin().y();
zCur = transform.getOrigin().z();
tf::Matrix3x3 m(transform.getRotation());
m.getRPY(rollCur, pitchCur, yawCur);
Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);

1. 将点云转换成PCL格式

​ 首先,一开始先创建一个pcl格式的容器,然后再通过pcl库中内置的pcl::fromROSMsg的方法,去转换格式。PCL格式在前面的blog中也有介绍。

pcl::PointCloud<PointType>::Ptr laser_cloud_in(new pcl::PointCloud<PointType>());
pcl::fromROSMsg(*laser_msg, *laser_cloud_in);

2. 调用PCL的降采样算法

​ 这一步的作用,和前面粗暴的采用丢弃数据去降采样的方法不大一样。这是用PCL内置的过滤器使得数据规模下降,节约内存空间。

// 生成新的PCL格式的点云容器
pcl::PointCloud<PointType>::Ptr laser_cloud_in_ds(new pcl::PointCloud<PointType>());
// 生成过滤器
static pcl::VoxelGrid<PointType> downSizeFilter;
// 设置过滤的大小,设置采样体素大小
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
// 设置输入的点源
downSizeFilter.setInputCloud(laser_cloud_in);
// 开始过滤,并将输出放到刚刚生成的PCL的容器里
downSizeFilter.filter(*laser_cloud_in_ds);
// 把过滤好的数据覆盖原本的数据
*laser_cloud_in = *laser_cloud_in_ds;

3. 保证当前点云的点在当前相机视角内

​ 这里也需要用到过滤器。但是,过滤器的规则是我们自己制定的。在这里的规则是,只保留当前相机视角内的所有点。那么,怎么确定当前相机的视角呢?

​ 在这里,作者定义相机视角为3个平面围成的空间。分别是, x = 0 x=0 x=0 ∣ y x ∣ |\frac{y}{x}| xy ∣ z x ∣ |\frac{z}{x}| xz,三个平面围成。

​ 然后,依次扫描这一帧内所有的点,把符合的点放入新的PCL点云容器,最后再把筛选后的数据覆盖原来的数据。

// 生成新的PCL格式的点云容器
pcl::PointCloud<PointType>::Ptr laser_cloud_in_filter(new pcl::PointCloud<PointType>());
// 遍历所有的点
for (int i = 0; i < (int)laser_cloud_in->size(); ++i)
{
    PointType p = laser_cloud_in->points[i];
    // 符合条件的数据,放入laser_cloud_in_filter
    if (p.x >= 0 && abs(p.y / p.x) <= 10 && abs(p.z / p.x) <= 10)
        laser_cloud_in_filter->push_back(p);
}
// 把过滤好的数据覆盖原本的数据
*laser_cloud_in = *laser_cloud_in_filter;

4. 将点云从激光雷达坐标系变成相机坐标系

​ 这一部分是代码中重要的部分。通过这一部分,可以为后面的图像信息中的特征点生成深度信息。不过,这里的代码很简单,这里是直接调用PCL中的相关函数。这里的变换应该和前面blog介绍的差不多。

​ 但主要的是,这里面生成变换矩阵的参数。

pcl::PointCloud<PointType>::Ptr laser_cloud_offset(new pcl::PointCloud<PointType>());
// 获得tf变换信息,生成变换矩阵。
Eigen::Affine3f transOffset = pcl::getTransformation(L_C_TX, L_C_TY, L_C_TZ, L_C_RX, L_C_RY, L_C_RZ);
// 从lidar坐标系转变到相机坐标系
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_offset, transOffset);
// 覆盖原有数据
*laser_cloud_in = *laser_cloud_offset;

​ 读取参数:

L_C_TX = fsSettings["lidar_to_cam_tx"];
L_C_TY = fsSettings["lidar_to_cam_ty"];
L_C_TZ = fsSettings["lidar_to_cam_tz"];
L_C_RX = fsSettings["lidar_to_cam_rx"];
L_C_RY = fsSettings["lidar_to_cam_ry"];
L_C_RZ = fsSettings["lidar_to_cam_rz"];

​ 作者在参数文件中的设定如下,这里数值的设定应该是作者自己测量得出:

# lidar to camera extrinsic
lidar_to_cam_tx: 0.05
lidar_to_cam_ty: -0.07
lidar_to_cam_tz: -0.07
lidar_to_cam_rx: 0.0
lidar_to_cam_ry: 0.0
lidar_to_cam_rz: -0.04

5. 再把点云变换到世界坐标系(静态)

​ 这里,把前面第0步获取到的变换矩阵,用来将坐标系变换到世界的坐标系。至于为什么要到这里才做,应该是经过前面的筛选,把数据量降低,提高程序的效率。

pcl::PointCloud<PointType>::Ptr laser_cloud_global(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_global, transNow);

6. 最后把变换完成的点云存储到待处理队列

double timeScanCur = laser_msg->header.stamp.toSec();
cloudQueue.push_back(*laser_cloud_global);
timeQueue.push_back(timeScanCur);

7. 保持队列的时效

​ 检索时效性,当队列的最末尾,和当前刚刚处理的点云相差时间超过5s,就将最晚的点云删掉,直到最末尾的点云帧和当前时间不超过5s。

while (!timeQueue.empty())
{
    if (timeScanCur - timeQueue.front() > 5.0)
    {
        cloudQueue.pop_front();
        timeQueue.pop_front();
    } else {
        break;
    }
}

8. 将队列里的点云输入作为总体的待处理深度图

​ 这里需要访问到共享内存,在访问前会拿到共享内存的锁std::lock_guard<std::mutex> lock(mtx_lidar);。这里把depthCloud深度点云图先初始化为空,然后将刚刚处理的数据,放入其中。

depthCloud->clear();
for (int i = 0; i < (int)cloudQueue.size(); ++i)
    *depthCloud += cloudQueue[i];

9. 降采样总体的深度图

​ 降采样总体深度点云图,这里和前面的降采样操作一样。

pcl::PointCloud<PointType>::Ptr depthCloudDS(new pcl::PointCloud<PointType>());
// 设置过滤的大小,设置采样体素大小
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.setInputCloud(depthCloud);
downSizeFilter.filter(*depthCloudDS);
*depthCloud = *depthCloudDS;

附录:代码

void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& laser_msg)
{
    static int lidar_count = -1;
    if (++lidar_count % (LIDAR_SKIP+1) != 0)
        return;

    // 0. listen to transform
    static tf::TransformListener listener;
    static tf::StampedTransform transform;
    try{
        listener.waitForTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, ros::Duration(0.01));
        listener.lookupTransform("vins_world", "vins_body_ros", laser_msg->header.stamp, transform);
    } 
    catch (tf::TransformException ex){
        // ROS_ERROR("lidar no tf");
        return;
    }

    double xCur, yCur, zCur, rollCur, pitchCur, yawCur;
    xCur = transform.getOrigin().x();
    yCur = transform.getOrigin().y();
    zCur = transform.getOrigin().z();
    tf::Matrix3x3 m(transform.getRotation());
    m.getRPY(rollCur, pitchCur, yawCur);
    Eigen::Affine3f transNow = pcl::getTransformation(xCur, yCur, zCur, rollCur, pitchCur, yawCur);

    // 1. convert laser cloud message to pcl
    pcl::PointCloud<PointType>::Ptr laser_cloud_in(new pcl::PointCloud<PointType>());
    pcl::fromROSMsg(*laser_msg, *laser_cloud_in);

    // 2. downsample new cloud (save memory)
    pcl::PointCloud<PointType>::Ptr laser_cloud_in_ds(new pcl::PointCloud<PointType>());
    static pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.setInputCloud(laser_cloud_in);
    downSizeFilter.filter(*laser_cloud_in_ds);
    *laser_cloud_in = *laser_cloud_in_ds;

    // 3. filter lidar points (only keep points in camera view)
    pcl::PointCloud<PointType>::Ptr laser_cloud_in_filter(new pcl::PointCloud<PointType>());
    for (int i = 0; i < (int)laser_cloud_in->size(); ++i)
    {
        PointType p = laser_cloud_in->points[i];
        if (p.x >= 0 && abs(p.y / p.x) <= 10 && abs(p.z / p.x) <= 10)
            laser_cloud_in_filter->push_back(p);
    }
    *laser_cloud_in = *laser_cloud_in_filter;

    // TODO: transform to IMU body frame
    // 4. offset T_lidar -> T_camera 
    pcl::PointCloud<PointType>::Ptr laser_cloud_offset(new pcl::PointCloud<PointType>());
    Eigen::Affine3f transOffset = pcl::getTransformation(L_C_TX, L_C_TY, L_C_TZ, L_C_RX, L_C_RY, L_C_RZ);
    pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_offset, transOffset);
    *laser_cloud_in = *laser_cloud_offset;

    // 5. transform new cloud into global odom frame
    pcl::PointCloud<PointType>::Ptr laser_cloud_global(new pcl::PointCloud<PointType>());
    pcl::transformPointCloud(*laser_cloud_in, *laser_cloud_global, transNow);

    // 6. save new cloud
    double timeScanCur = laser_msg->header.stamp.toSec();
    cloudQueue.push_back(*laser_cloud_global);
    timeQueue.push_back(timeScanCur);

    // 7. pop old cloud
    while (!timeQueue.empty())
    {
        if (timeScanCur - timeQueue.front() > 5.0)
        {
            cloudQueue.pop_front();
            timeQueue.pop_front();
        } else {
            break;
        }
    }

    std::lock_guard<std::mutex> lock(mtx_lidar);
    // 8. fuse global cloud
    depthCloud->clear();
    for (int i = 0; i < (int)cloudQueue.size(); ++i)
        *depthCloud += cloudQueue[i];

    // 9. downsample global cloud
    pcl::PointCloud<PointType>::Ptr depthCloudDS(new pcl::PointCloud<PointType>());
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.setInputCloud(depthCloud);
    downSizeFilter.filter(*depthCloudDS);
    *depthCloud = *depthCloudDS;
}

引用

LVI-SAM代码注释 – xuwuzhou的SLAM之路 – 静心,慎思,明辨,笃学

  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值