ALOM节点图
1.特征点提取原理
根据曲率来提取两种特征点,分别为角点与面点
2.实现
2.1主函数
功能:初始化ROS节点,订阅激光点云数据,发布处理后的点云数据。代码的主要功能如下:
- 初始化ROS节点,设置节点名称和参数。
- 设置扫描线数量(N_SCANS),最小距离(MINIMUM_RANGE)。
- 打印扫描线数量和最小距离。
- 检查扫描线数量是否支持16、32或64条扫描线,否则输出提示信息并退出程序。
- 订阅激光点云数据(/points_raw),并设置回调函数laserCloudHandler。
- 发布处理后的点云数据(/velodyne_cloud_2),(/laser_cloud_sharp),(/laser_cloud_less_sharp),(/laser_cloud_flat),(/laser_cloud_less_flat),(/laser_remove_points)。
- 如果PUB_EACH_LINE设置为true,则为每个扫描线发布单独的点云数据。
- 运行ROS事件循环。
int main(int argc, char **argv)
{
// 初始化节点
ros::init(argc, argv, "scanRegistration");
// 获取节点句柄
ros::NodeHandle nh;
// 获取参数:扫描线数量
nh.param<int>("scan_line", N_SCANS, 16);
// 获取参数:最小距离
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
// 打印参数:扫描线数量
printf("scan line number %d \n", N_SCANS);
// 判断扫描线数量是否支持
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
// 订阅激光点云
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/points_raw", 100, laserCloudHandler);
// 发布激光点云
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
// 发布角点点云(锐角)
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
// 发布角点点云(稍锐角)
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
// 发布平面点云(平坦)
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
// 发布平面点云(稍平坦)
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
// 发布移除点云
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
// 判断是否发布每个扫描线
if(PUB_EACH_LINE)
{
// 发布每个扫描线
for(int i = 0; i < N_SCANS; i++)
{
ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
pubEachScan.push_back(tmp);
}
}
// 订阅
ros::spin();
return 0;
}
2.2 laserCloudHandler 函数
订阅LiDAR点云数据,并对其进行处理,最后将处理后的点云数据发布出去。处理过程包括去噪、过滤、投影、提取角点、分割平面等。
1.定义了一些变量和结构体,如点云类型、点云尺寸等。
2.定义了一个名为laserCloudHandler的回调函数,该函数会在接收到LiDAR点云数据时被调用。
3.在laserCloudHandler函数中,首先检查系统是否初始化,如果没有,则计数器自增,直到达到系统延迟时间。然后,计算预处理时间。
4.对LiDAR点云数据进行预处理,包括去噪、过滤等。预处理后的点云数据存储在laserCloudIn变量中。
5.计算处理时间。
6.遍历预处理后的点云数据,提取角点、分割平面等。
7.将处理后的点云数据发布出去。
2.3 removeClosedPointCloud函数
接收两个pcl::PointCloud类型的参数:cloud_in(输入点云)和cloud_out(输出点云)。函数的目的是从输入点云中移除那些距离原点(坐标系原点)小于特定阈值的点。
原理:是遍历输入点云中的每个点,检查其三维空间坐标(x、y和z)是否满足条件。如果满足条件,将该点复制到输出点云中,否则跳过该点。最后,调整输出点云的大小以匹配有效点的数量。
注意:
输入和输出点云必须是指向同一内存空间的指针,否则会导致未定义行为。
函数使用三维空间中的点距离公式(平方和)来检查点是否满足条件。
函数使用size_t类型来表示点cloud_in中的索引和cloud_out中的有效点数。
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
pcl::PointCloud<PointT> &cloud_out, float thres)
{
// 如果输入和输出点云不是同一内存空间,则抛出异常或采取其他措施
if (&cloud_in != &cloud_out)
{
// 复制输入点云的头部信息到输出点云
cloud_out.header = cloud_in.header;
// 调整输出点云的大小以匹配有效点数
cloud_out.points.resize(cloud_in.points.size());
}
// 初始化输出点云的有效点数
size_t j = 0;
// 遍历输入点云中的每个点
for (size_t i = 0; i < cloud_in.points.size(); ++i)
{
// 检查该点的三维空间坐标是否满足条件
if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
continue;
// 将该点复制到输出点云中
cloud_out.points[j] = cloud_in.points[i];
// 有效点数加一
j++;
}
// 调整输出点云的大小以匹配有效点数
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
// 设置输出点云的属性
cloud_out.height = 1;
cloud_out.width = static_cast<uint32_t>(j);
cloud_out.is_dense = true;
}