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;
}