一、节点关系
二、自定义格式消息类型cloud_info
# Cloud Info
Header header ## 标头
startRingIndex ## 点云的第i条扫描线(ring)上的第一个可计算曲率的点
endRingIndex ## 点云的第i条扫描线(ring)上的最后一个可计算曲率的点
pointColInd # point column index in range image ## 点云中每个点在投影图片中的列序号
pointRange # point range ## 点云中每个点与LiDAR的距离,即投影图片的像素值
int64 imuAvailable ## imu的旋转测量是否对齐到LiDAR,若对齐说明imu旋转测量可用
int64 odomAvailable ## 是否有与当前帧最近的两个相邻帧之间的位姿变换可用
# Attitude for lidar odometry initialization
## 可用的imu旋转测量,作为LIS帧间位姿变换的预测值
float32 imuRollInit
float32 imuPitchInit
float32 imuYawInit
# Odometry
## 可用的相邻帧位姿变换,同样作为LIS帧间位姿变换的预测值
float32 odomX
float32 odomY
float32 odomZ
float32 odomRoll
float32 odomPitch
float32 odomYaw
# Odometry reset ID
## 从里程计获得的位姿变换协方差的取整(四舍五入),可以用于计算该位姿变换值的可信度
int64 odomResetId
# Point cloud messages
cloud_deskewed # original cloud deskewed 原始去畸变点云
cloud_corner # extracted corner feature 角点组成的点云
cloud_surface # extracted surface feature 面点组成的点云
三、主函数
int main(int argc, char** argv)
{
ros::init(argc, argv, "lidar");
//// 与imageProjection相似,核心工作都由FeatureExtraction类的构造函数完成
FeatureExtraction FE;
ROS_INFO("\033[1;32m----> Lidar Feature Extraction Started.\033[0m");
ros::spin();
return 0;
}
四、FeatureExtraction类
4.1 父类ParamServer类
4.2 FeatureExtraction类使用到的结构体
struct smoothness_t{
float value; //// 曲率值
size_t ind; //// 点序号
};
struct by_value{
//// 仿函数用来做比较器,用来做
bool operator()(smoothness_t const &left, smoothness_t const &right) {
return left.value < right.value;
}
};
4.3 FeatureExtraction类成员概览
4.4 FeatureExtraction类的几个优秀函数
4.4.1 calculateSmoothness() 计算曲率
void calculateSmoothness()
{
int cloudSize = extractedCloud->points.size();
for (int i = 5; i < cloudSize - 5; i++)
{
//// 采用距离来计算曲率,这是对之前imageProjection节点计算过的距离信息再次利用,提高信息利用率和计算效率。
float diffRange = cloudInfo.pointRange[i-5] + cloudInfo.pointRange[i-4]
+ cloudInfo.pointRange[i-3] + cloudInfo.pointRange[i-2]
+ cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i] * 10
+ cloudInfo.pointRange[i+1] + cloudInfo.pointRange[i+2]
+ cloudInfo.pointRange[i+3] + cloudInfo.pointRange[i+4]
+ cloudInfo.pointRange[i+5];
cloudCurvature[i] = diffRange*diffRange;//diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
// cloudSmoothness for sorting
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
4.4.2 markOccludedPoints() 遮挡点标记
void markOccludedPoints()
{
int cloudSize = extractedCloud->points.size();
// mark occluded points and parallel beam points
for (int i = 5; i < cloudSize - 6; ++i)
{
// occluded points
float depth1 = cloudInfo.pointRange[i];
float depth2 = cloudInfo.pointRange[i+1];
int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));
//// 相近点:在有序点云中顺序相邻,并且在距离图像上的列序号之差小于10
if (columnDiff < 10){
// 10 pixel diff in range image
//// 两个相近的点与LiDAR的距离之差大于0.3m,则认为它们是互相遮挡的点
//// 标记后景点,防止把遮挡点当做角点
if (depth1 - depth2 > 0.3){
cloudNeighborPicked[i - 5] = 1;
cloudNeighborPicked[i - 4] = 1;
cloudNeighborPicked[i - 3] = 1;
cloudNeighborPicked[i - 2] = 1;
cloudNeighborPicked[i - 1] = 1;
cloudNeighborPicked[i] = 1;
}else if (depth2 - depth1 > 0.3){
cloudNeighborPicked[i + 1] = 1;
cloudNeighborPicked[i + 2] = 1;
cloudNeighborPicked[i + 3] = 1;
cloudNeighborPicked[i + 4] = 1;
cloudNeighborPicked[i + 5] = 1;
cloudNeighborPicked[i + 6] = 1;
}
}
// parallel beam
//// 理论上物体距离越远时,点云才应该越稀疏,相邻点距离较远
//// 但是对于平行于激光光束的表面来说,即使距离激光雷达很近,依然会导致两个相邻点很远
//// 所以当相邻点距离之差大于0.02倍距离时,认为它是平行于激光光束的平面,排除掉该点
float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));
if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
cloudNeighborPicked[i] = 1;
}
}
4.4.3 freeCloudInfoMemory() 释放自定义点云内存
void freeCloudInfoMemory()
{
cloudInfo.startRingIndex.clear();
cloudInfo.startRingIndex.shrink_to_fit();//// 释放数组的capacity
cloudInfo.endRingIndex.clear();
cloudInfo.endRingIndex.shrink_to_fit();
cloudInfo.pointColInd.clear();
cloudInfo.pointColInd.shrink_to_fit();
cloudInfo.pointRange.clear();
cloudInfo.pointRange.shrink_to_fit();
}
4.5 FeatureExtraction类的构造函数
FeatureExtraction()
{
//// 订阅去畸变、有序化的自定义格式点云cloudInfo,回调函数负责去除干扰点,根据曲率提取角点和平面点
subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
//// 发布注册了角点点云和面点点云,并且“瘦身”之后的cloudInfo
pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
//// 发布角点点云和面点点云
pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);
//// 重置所有参数,设置体素化滤波器分辨率
initializationValue();
}
3D视觉精品课程推荐:
3.彻底搞透视觉三维重建:原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM:基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
▲长按关注公众号