A-LOAM工程笔记（二）：前端点云预处置处罚及特征提取 ...

一、前言

LOAM算法前端主要包含对lidar 点云的预处置处罚及点线特征和点面特征的提取。这些功能实现在scanRegistration.cpp中。
二、主要流程

1. int main(int argc, char **argv)
2. {
3.     ros::init(argc, argv, "scanRegistration");
4.     ros::NodeHandle nh;
6.     nh.param<int>("scan_line", N_SCANS, 16);
8.     nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
10.     printf("scan line number %d \n", N_SCANS);
12.     if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
13.     {
14.         printf("only support velodyne with 16, 32 or 64 scan line!");
15.         return 0;
16.     }
18.     ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
20.     pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
22.     pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
24.     pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
26.     pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
28.     pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
30.     pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
32.     if(PUB_EACH_LINE)
33.     {
34.         for(int i = 0; i < N_SCANS; i++)
35.         {
36.             ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
37.             pubEachScan.push_back(tmp);
38.         }
39.     }
40.     ros::spin();
42.     return 0;
43. }

复制代码

主要流程包含：
（1）ros节点和句柄初始化，雷达线数"scan_line"和盲区"minimum_range"两个参数的设置，分别设置为16线和0.1m。
（2） 订阅topic"/velodyne_points"，绑定回调函数laserCloudHandler(重要)，对吸收到的点云举行处置处罚。
（3）pubLaserCloud发布原始点云；pubCornerPointsSharp发布曲率大的角点点云；pubCornerPointsLessSharp发布曲率小一些的角点点云；pubSurfPointsFlat发布面点点云；pubSurfPointsLessFlat发布曲率更小面点点云；pubRemovePoints发布移除的点云。
（4）若PUB_EACH_LINE设置的是true，根据laser_scanid发布差别id的点云。
三、点云预处置处罚

1）removeClosedPointCloud()：

1. template <typename PointT>
2. void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
3.                               pcl::PointCloud<PointT> &cloud_out, float thres)
4. {
5.     if (&cloud_in != &cloud_out)
6.     {
7.         cloud_out.header = cloud_in.header;
8.         cloud_out.points.resize(cloud_in.points.size());
9.     }
11.     size_t j = 0;
13.     for (size_t i = 0; i < cloud_in.points.size(); ++i)
14.     {
15.         if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y +
16.            cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
17.             continue;
18.         cloud_out.points[j] = cloud_in.points[i];
19.         j++;
20.     }
21.     if (j != cloud_in.points.size())
22.     {
23.         cloud_out.points.resize(j);
24.     }
26.     cloud_out.height = 1;
27.     cloud_out.width = static_cast<uint32_t>(j);
28.     cloud_out.is_dense = true;
29. }

复制代码

根据设置的参数thres，剔除小于这个距离内过于靠近的点云. 设置点云宽度(当前束激光保留点云个数)，高度1(拆分成了单束)，is_dense = true。
2）laserCloudHandler():

1. void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
2. {
3.     if (!systemInited)
4.     {
5.         systemInitCount++;
6.         if (systemInitCount >= systemDelay)
7.         {
8.             systemInited = true;
9.         }
10.         else
11.             return;
12.     }
14.     TicToc t_whole;
15.     TicToc t_prepare;
16.     std::vector<int> scanStartInd(N_SCANS, 0);
17.     std::vector<int> scanEndInd(N_SCANS, 0);
19.     pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
20.     pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
21.     std::vector<int> indices;
23.     // ���ȶԵ����˲���ȥ��NaNֵ����Ч���ƣ��Լ���Lidar����ϵԭ��MINIMUM_RANGE�������ڵĵ�
24.     pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
25.     removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
28.     int cloudSize = laserCloudIn.points.size();
29.     float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
30.     float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
31.                           laserCloudIn.points[cloudSize - 1].x) +
32.                    2 * M_PI;
34.     if (endOri - startOri > 3 * M_PI)
35.     {
36.         endOri -= 2 * M_PI;
37.     }
38.     else if (endOri - startOri < M_PI)
39.     {
40.         endOri += 2 * M_PI;
41.     }
42.     //printf("end Ori %f\n", endOri);
44.     bool halfPassed = false;
45.     int count = cloudSize;
46.     PointType point;
47.     std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
48.     for (int i = 0; i < cloudSize; i++)
49.     {
50.         point.x = laserCloudIn.points[i].x;
51.         point.y = laserCloudIn.points[i].y;
52.         point.z = laserCloudIn.points[i].z;
54.         float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
55.         int scanID = 0;
57.         if (N_SCANS == 16)
58.         {
59.             scanID = int((angle + 15) / 2 + 0.5);
60.             if (scanID > (N_SCANS - 1) || scanID < 0)
61.             {
62.                 count--;
63.                 continue;
64.             }
65.         }
66.         else if (N_SCANS == 32)
67.         {
68.             scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
69.             if (scanID > (N_SCANS - 1) || scanID < 0)
70.             {
71.                 count--;
72.                 continue;
73.             }
74.         }
75.         else if (N_SCANS == 64)
76.         {   
77.             if (angle >= -8.83)
78.                 scanID = int((2 - angle) * 3.0 + 0.5);
79.             else
80.                 scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
82.             // use [0 50]  > 50 remove outlies
83.             if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
84.             {
85.                 count--;
86.                 continue;
87.             }
88.         }
89.         else
90.         {
91.             printf("wrong scan number\n");
92.             ROS_BREAK();
93.         }
94.         //printf("angle %f scanID %d \n

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。