简单地把vloam中的特征提取线程提取出来，不依赖于ROS运行，适用于初学者对特征提取的理解
使用的时候需要把点云文件改为自己的文件，同时根据自己雷达的线数修改main函数最开始的线数定义。
最后的点云可视化偶尔会崩溃，建议保存点云用cloudcompare查看。
效果如图：
common.h
#pragma once
#include <cmath>
#include <pcl/point_types.h>
typedef pcl::PointXYZI PointType;
inline double rad2deg(double radians)
{
return radians * 180.0 / M_PI;
}
inline double deg2rad(double degrees)
{
return degrees * M_PI / 180.0;
}
tic_toc.h
#pragma once
#include <ctime>
#include <cstdlib>
#include <chrono>
class TicToc
{
public:
TicToc()
{
tic();
}
void tic()
{
start = std::chrono::system_clock::now();
}
double toc()
{
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end - start;
return elapsed_seconds.count() * 1000;
}
private:
std::chrono::time_point<std::chrono::system_clock> start, end;
};
main.cpp
#include <cmath>
#include <vector>
#include <string>
#include "common.h"
#include "tic_toc.h"
// OPENCV
#include <opencv2/opencv.hpp>
// PCL
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
using std::atan2;
using std::cos;
using std::sin;
//扫描周期, velodyne频率10Hz，周期0.1s
const double scanPeriod = 0.1;
//弃用前systemDelay帧初始数据
const int systemDelay = 0;
//systemInitCount用于计数过了多少帧
//超过systemDelay后，systemInited为true即初始化完成
int systemInitCount = 0;
bool systemInited = false;
//激光雷达线数初始化为16
int N_SCANS = 16;
//点云曲率, 400000为一帧点云中点的最大数量
float cloudCurvature[400000];
//曲率点对应的序号
int cloudSortInd[400000];
//点是否筛选过标志：0-未筛选过，1-筛选过
int cloudNeighborPicked[400000];
//点分类标号:2-代表曲率很大，1-代表曲率比较大, 0-曲率比较小, -1-代表曲率很小(其中1包含了2, 0包含了1, 0和1构成了点云全部的点)
int cloudLabel[400000];
//两点曲率比较
bool comp(int i, int j) { return (cloudCurvature[i] < cloudCurvature[j]); }
// 设置发布内容，整体点云，角点，降采样角点，面点，降采样面点，剔除点
//是否发布每行Scan
bool PUB_EACH_LINE = false;
//根据距离去除过远的点，距离的参数
double MINIMUM_RANGE = 0.1;
//点云可视化
void printscreen(const std::string& filename) {
// 加载点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename, *cloud) == -1) {
std::cerr << "Couldn't read file: " << filename << std::endl;
return;
}
std::cout << "Loaded " << cloud->width * cloud->height << " data points from " << filename << std::endl;
// 可视化点云
pcl::visualization::PCLVisualizer viewer(filename );
viewer.setBackgroundColor(0, 0, 0);
viewer.addPointCloud<pcl::PointXYZ>(cloud, "point cloud");
while (!viewer.wasStopped()) {
viewer.spinOnce(100);
}
}
//去除过远点 使用template进行兼容 好象是过近点
template<typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT>& cloud_in,
pcl::PointCloud<PointT>& cloud_out, float thres)
{
// 统一header(时间戳)和size
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)
{ //计算每个点的xyz的平方和是否小于阈值的平方和
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; //如果是则continue
cloud_out.points[j] = cloud_in.points[i]; //如果不是则将这点保留下来
j++;
}
//有点被剔除时，size改变
if (j != cloud_in.points.size())
{
cloud_out.points.resize(j);
}
//收到的lidar点云是一个有序的点云
//数据行数，默认1为无组织的数据，因为要对一些点做去除，所以只能将3维空间的点降到2维，即将线数默认为1，宽度为n的所有值
cloud_out.height = 1; //高度代表线数
//可以理解成点数
cloud_out.width = static_cast<uint32_t>(j); //每一次scan的宽度
//点数是否有限
cloud_out.is_dense = true;
}
//订阅点云句柄 形参叫做收到这个点云的消息
void laserCloudHandler(pcl::PointCloud<pcl::PointXYZ> laserCloudIn)
{
/* ***********************************************3D点云预处理**********************************************************/
//是否剔除前systemDelay帧 如果系统没有初始化的话，就等几帧 通过设置systemDelay大小
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
//registration计时
TicToc t_whole;
//计算曲率前的预处理计时
TicToc t_prepare;
//记录每个scan有曲率的点的开始和结束索引
std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
std::vector<int> indices;
//这个函数目的是去除过远点，第一个参数是输入，第二个参数是输出，第三个参数是列表保存输出的点在输入里的位置
//输出里的第i个点，是输入里的第indices[i]个点，就是
//cloud_out.points[i] = cloud_in.points[indices[i]]
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
//引用上文作者写的去除函数 ，去除距离小于阈值的点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
/* **********************************************以下计算都是为了记录每个点是第几根线和扫描的时间的多少,laserCloudScans***************************************************** */
//计算起始点和结束点的水平角度
//该次sweep的点数
int cloudSize = laserCloudIn.points.size();
//每次扫描是一条线，看作者的数据集激光x向前，y向左，那么下面就是线一端到另一端
//atan2的输出为-pi到pi(PS:atan输出为-pi/2到pi/2)，四象限反三角函数。
//计算旋转角时取负号是因为velodyne是顺时针旋转，取反相当于人为逆时针
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//此处加2pi是为了保证起始点到结束点相差2pi，符合实际，按照原理上起始点应该和结束点在一个位置，所以加2pi区分
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
//总有一些例外，激光间距收束到1pi~3pi，本应相减差2pi
//去除补偿2pi的情况
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
//额外补偿2pi的情况
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[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
//求仰角atan输出为-pi/2到pi/2，实际看scanID应该每条线之间差距是2度
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
//根据不同线数使用不同参数对每个点对应的第几根激光线进行判断，后来的算法不需要这一步可以直接从驱动中获得这个点是第几根线
if (N_SCANS == 16)
{
//16线的激光雷达是30度（-15,15）+15为了补偿，每相邻的scan的夹角是2度，+0.5是为了四舍五入
scanID = int((angle + 15) / 2 + 0.5); //排列是从下往上
//如果判定线在16以上或是负数则忽视该点回到上面for循环
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 [0 50] > 50 remove outlie
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
//只有16,32,64线
else
{
printf("wrong scan number\n");
}
//printf("angle %f scanID %d \n", angle, scanID);
//计算水平角
float ori = -atan2(point.y, point.x);
//根据扫描线是否旋转过半选择与起始位置还是终止位置进行差值计算，从而进行补偿
if (!halfPassed)
{
//确保-pi/2 < ori - startOri < 3*pi/2
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
//这种case不会发生
else if (ori > startOri + M_PI * 3 / 2)
{
ori -= 2 * M_PI;
}
//如果超过180度，就说明过了一半了
if (ori - startOri > M_PI)
{
halfPassed = true;
}
//确保-3*pi/2 < ori - endOri < pi/2
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI; //两种情况，补偿2pi
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI; //去除补偿的2pi
}
}
}
//角度的计算是为了看看旋转多少了，记录比例relTime，占整个扫描时间的比例
float relTime = (ori - startOri) / (endOri - startOri); //当前点与起始点的角度差 比 整条线的起始结束差
//某个点是第几根线和扫描的时间的多少记录在point.intensity 整数和小数
point.intensity = scanID + scanPeriod * relTime;
//按线分类保存 每条线开辟一个数组
laserCloudScans[scanID].push_back(point);
}
cloudSize = count;
printf("points size %d \n", cloudSize);
//也就是把所有线保存在laserCloud（一维数组）一个数据集合里，把每条线的第五个和倒数第五个点在laserCloud中的位置反馈给scanStartInd和scanEndInd
//这10个点的曲率计算不方便，所就不计算，scanStartInd和scanEndInd用来记录每条线的起始和结束的id（去除前五和后五之后的）
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
//预处理部分终于完成了
printf("prepare time %f \n", t_prepare.toc());
/* **************************************************特征值提取********************************************************* */
//开始计算曲率
//十点求曲率，自然是在一条线上的十个点，利用前后5个点计算每个点的曲率（前后5点不计算）
for (int i = 5; i < cloudSize - 5; i++)
{
float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
//曲率，序号，是否筛过标志位，曲率分类
cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
cloudSortInd[i] = i;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
}
//计时用
TicToc t_pts;
//角点，降采样角点，面点，降采样面点
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<pcl::PointXYZI> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;
float t_q_sort = 0;
//开始提取特征
//遍历每条scan
for (int i = 0; i < N_SCANS; i++)
{
//此条扫描线扫描到的点数小于6就退出
if (scanEndInd[i] - scanStartInd[i] < 6)
continue;
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
//将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点,或者说每两行都有
for (int j = 0; j < 6; j++)
{
//每一个等分计算起始和终结的id，sp和ep表示在laserCloud中的位置
//六等份起点：sp = scanStartInd + (scanEndInd - scanStartInd)*j/6
//六等份终点：ep = scanStartInd - 1 + (scanEndInd - scanStartInd)*(j+1)/6
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
TicToc t_tmp;
//返回的是结束地址后面那一个，comp定义的是排序是按照从大到小对cloudSortInd[sp, ep + 1]进行重新排序
//本来cloudSortInd[1,6]分别是12345 排序之后变成245613，现在cloudSortInd[3]的值表示laserCloud中第5个索引
std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();
//挑选每个分段的曲率很大和比较大的点
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--)
{
//排序后cloudSortInd顺序就乱了，这个时候索引的作用就体现出来了
//ind的值表示为在laserCloud中的位置 ind=cloudSortInd[k]=k,laserCloud[ind]表此点,cloudCurvature[ind]表示此点曲率
int ind = cloudSortInd[k];
//如果筛选标志为0，并且曲率较大 阈值为0.1
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
{
//则曲率大的点记数一下
largestPickedNum++;
//满足阈值的点少于等于两个（将满足条件的最大曲率的两个点放入cornerPointsSharp中）
if (largestPickedNum <= 2)
{
cloudLabel[ind] = 2;
cornerPointsSharp.push_back(laserCloud->points[ind]);
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//保存曲率较大的前20个角点放入cornerPointsLessSharp中
else if (largestPickedNum <= 20)
{
cloudLabel[ind] = 1;
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//多了就不要了
else
{
break;
}
//标志位一改
cloudNeighborPicked[ind] = 1;
//将曲率比较大的点的前后各5个连续距离比较近的点做标记不再筛选为特征点，防止特征点聚集，使得特征点在每个方向上尽量分布均匀
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
//应该是决定简单计算不稳定，直接过
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
//前后几个也
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
//前后几个也
cloudNeighborPicked[ind + l] = 1;
}
}
}
//挑选每个分段的曲率很小比较小的点
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
//如果曲率的确比较小，并且未被筛选出
if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1)
{
//-1代表曲率很小的点
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
//只选最小的四个，剩下的Label==0,就都是曲率比较小的
smallestPickedNum++;
if (smallestPickedNum >= 4)
{
break;
}
cloudNeighborPicked[ind] = 1;
}
//同样防止特征点聚集
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
//将小于阈值0.1剩余的点（包括之前被排除的点）全部归入平面点中less flat类别中
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
//由于less flat点最多，对每个分段less flat的点进行提速栅格滤波
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
//less flat点汇总
surfPointsLessFlat += surfPointsLessFlatScanDS;
}
}
// （求出曲率后）降采样和分类的时间
printf("sort q time %f \n", t_q_sort);
printf("seperate points time %f \n", t_pts.toc());
//总时间输出
printf("scan registration time %f ms *************\n", t_whole.toc());
pcl::io::savePCDFileBinary("../corner_points_sharp.pcd", cornerPointsSharp);
pcl::io::savePCDFileBinary("../cornerPointsLessSharp.pcd", cornerPointsLessSharp);
pcl::io::savePCDFileBinary("../surfPointsFlat.pcd", surfPointsFlat);
pcl::io::savePCDFileBinary("../surfPointsLessFlat.pcd", surfPointsLessFlat);
}
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::io::loadPCDFile<pcl::PointXYZ>("../1715855868.10.pcd", laserCloudIn);
//参数，过近去除 (最后一个参数为默认值) 最小有效距离
//nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
printf("scan line number %d \n", N_SCANS);
//只有线数是16 32 64 的才可以继续运行
if (N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
laserCloudHandler(laserCloudIn);
printscreen( "../corner_points_sharp.pcd");
printscreen( "../cornerPointsLessSharp.pcd");
printscreen( "../surfPointsFlat.pcd");
printscreen( "../surfPointsLessFlat.pcd");
return 0;
}
扫一扫
308
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
