A-LOAM工程笔记（二）：前端点云预处置处罚及特征提取
一、前言
LOAM算法前端主要包含对lidar 点云的预处置处罚及点线特征和点面特征的提取。这些功能实现在scanRegistration.cpp中。
二、主要流程
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>("/velodyne_points", 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;
} 主要流程包含：
（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()：
template <typename PointT>
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.x * cloud_in.points.x + cloud_in.points.y * cloud_in.points.y +
cloud_in.points.z * cloud_in.points.z < thres * thres)
continue;
cloud_out.points = cloud_in.points;
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;
} 根据设置的参数thres，剔除小于这个距离内过于靠近的点云. 设置点云宽度(当前束激光保留点云个数)，高度1(拆分成了单束)，is_dense = true。
2）laserCloudHandler():
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
TicToc t_whole;
TicToc t_prepare;
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
std::vector<int> indices;
// ���ȶԵ����˲���ȥ��NaNֵ����Ч���ƣ��Լ���Lidar����ϵԭ��MINIMUM_RANGE�������ڵĵ�
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
int cloudSize = laserCloudIn.points.size();
float startOri = -atan2(laserCloudIn.points.y, laserCloudIn.points.x);
float endOri = -atan2(laserCloudIn.points.y,
laserCloudIn.points.x) +
2 * M_PI;
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
//printf("end Ori %f\n", endOri);
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++)
{
point.x = laserCloudIn.points.x;
point.y = laserCloudIn.points.y;
point.z = laserCloudIn.points.z;
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
if (N_SCANS == 16)
{
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 64)
{
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。
页:
[1]