/*-------------------------------------
1.基于正态分布的点云离群点检测算法
2.基于VoxelGrid的点云下采样算法
3.作者：pcb
4.日期：2018.10.3
--------------------------------------*/
#include <iostream>
#include <fstream>
#include<algorithm>
#include <math.h>
#include <vector>
#include <Windows.h>
#include<time.h>
typedef long long int64_t;
typedef int int32_t;
using namespace std;
//定义3D点的结构体
struct Point3D
{
float x;
float y;
float z;
};
struct PointKDistance1
{
Point3D point;
float Distance; //存放距离的向量
};
//存放计算每个点云的idx和cloud_point_index的结构体
struct cloud_point_index_idx
{
unsigned int idx;
unsigned int cloud_point_index;
cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_) : idx(idx_), cloud_point_index(cloud_point_index_) {}
bool operator < (const cloud_point_index_idx &p) const { return (idx < p.idx); }
};
struct Array4f
{
float x;
float y;
float z;
float C;
};
/*----------------------------
* 功能 : 读取一个txt中的数据，将数据放入vector中
*----------------------------
* 函数 : ReadData
* 参数 : str [in] 需要读的txt文件名
* 参数 : Data [in] 读取xt文件数据存放在Data中
*/
int ReadData(const char* str, vector<Point3D>&Data)
{
fstream ReadDataTxt;
float X, Y, Z; //用于读取TXT中的数据
Point3D param; //创建一个用于存储X,Y,X的Point3D结构体
ReadDataTxt.open(str);
while (ReadDataTxt >> X >> Y >> Z)
{
if (Z >= 400)
{
param.x = X;
param.y = Y;
param.z = Z;
Data.push_back(param);
}
else
{
continue;
}
}
ReadDataTxt.close();
size_t DataSize = Data.size();
return 0;
}
/*----------------------------
* 功能 : 向一个txt中写数据
*----------------------------
* 函数 :WriteData
* 参数 : str [in] 需要写的txt文件名
* 参数 : Data [in] 需要写的txt文件数据存放在Data中
*/
int WriteData(string str, vector<Point3D>&Data)
{
fstream WriteTXT;
WriteTXT.open("11.txt");
for (int i = 0; i < Data.size(); i++)
{
if (Data[i].x!=0)
{
WriteTXT << Data[i].x << " " << Data[i].y << " " << Data[i].z << endl;
}
}
WriteTXT.close();
return 0;
}
/*----------------------------
*功能：计算两点之间的欧几里得距离
*-----------------------------
*输入：两个Point3D结构体类型的点
*输出：两点之间的欧几里得距离
*/
float XYZDistance(Point3D &point1,Point3D&point2)
{
float Distance_X = (point1.x - point2.x)*(point1.x - point2.x);
float Distance_Y = (point1.y - point2.y)*(point1.y - point2.y);
float Distance_Z = (point1.z - point2.z)*(point1.z - point2.z);
return sqrt(Distance_X+Distance_Y+Distance_Z);
}
/*----------------------------
*功能：冒泡排序法
*-----------------------------
*输入：vector<float>类型的值
*输出：从小到大排列的vector<float>
*/
void BubbleSort(vector <PointKDistance1>&BubbleSortVector)
{
size_t Num = BubbleSortVector.size();
for (int i = 0; i < Num;i++)
{
for (int j = i ; j < Num;j++)
{
if (BubbleSortVector[i].Distance>BubbleSortVector[j].Distance)
{
float temp = BubbleSortVector[i].Distance;
Point3D point12 = BubbleSortVector[i].point;
BubbleSortVector[i] = BubbleSortVector[j];
BubbleSortVector[j].Distance= temp;
BubbleSortVector[j].point = point12;
}
}
}
}
/*----------------------------
*功能：采用高斯分布的方法进行离群点的判别
*-----------------------------
*输入：Piont3D的原始点云数据
*输出：除去离群点之后的Point3D结构的点云数据
*/
void GaussianDistribution_OutlierDetection(vector<Point3D> &InputPointCloud, vector<Point3D>&OutPointCloud)
{
//均值
double X_Ave = 0;
double Y_Ave = 0;
double Z_Ave = 0;
//方差
double X_Var = 0;
double Y_Var = 0;
double Z_Var = 0;
//求均值
for (int i = 0; i <InputPointCloud.size(); i++)
{
X_Ave +=InputPointCloud[i].x;
Y_Ave +=InputPointCloud[i].y;
Z_Ave +=InputPointCloud[i].z;
}
X_Ave = X_Ave /InputPointCloud.size();
Y_Ave = Y_Ave /InputPointCloud.size();
Z_Ave = Z_Ave /InputPointCloud.size();
//求方差
for (int j = 0; j <InputPointCloud.size(); j++)
{
X_Var += (InputPointCloud[j].x - X_Ave)*(InputPointCloud[j].x - X_Ave);
Y_Var += (InputPointCloud[j].y - Y_Ave)*(InputPointCloud[j].y - Y_Ave);
Z_Var += (InputPointCloud[j].z - Z_Ave)*(InputPointCloud[j].z - Z_Ave);
}
X_Var = X_Var /InputPointCloud.size();
Y_Var = Y_Var /InputPointCloud.size();
Z_Var = Z_Var /InputPointCloud.size();
//开始判断是是否是离群点
for (int k = 0; k <InputPointCloud.size(); k++)
{
if (((abs(InputPointCloud[k].z - Z_Ave) / sqrt(Z_Var))<1.5 || (abs(InputPointCloud[k].z + Z_Ave) / sqrt(Z_Var))<1.5)
&& ((abs(InputPointCloud[k].y - Y_Ave) / sqrt(Y_Var))<1.8 || (abs(InputPointCloud[k].y + Y_Ave) / sqrt(Y_Var))<1.8)
&& ((abs(InputPointCloud[k].x - X_Ave) / sqrt(X_Var))<1.8 || (abs(InputPointCloud[k].x + X_Ave) / sqrt(X_Var))<1.))
{
OutPointCloud.push_back(InputPointCloud[k]); //如果不是离群点，则把该点放入CloudPoint中
}
}
return;
}
/*----------------------------
*功能：找到输入点云中的包围盒两个点的值（右上和坐下）
*-----------------------------
*输入：Piont3D的原始点云数据
*输出：找到之后的min_p和max_p
*/
void GetMaxMin(vector<Point3D>&InputCloudPoint, Array4f&min_p, Array4f&max_p)
{
//主要思路是找到x，y,z的最小值,这样就能得到点云立体包围的次村
//找x,y,z最小值
if (InputCloudPoint.size() == 0)
{
cout << "输入点云为空" << endl;
return;
}
float x_min= (*min_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.x < b.x;})).x;
float y_min =(*min_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.y < b.y;})).y;
float z_min =(*min_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.z < b.z;})).z;
//给min_p赋值
min_p.x = x_min;
min_p.y = y_min;
min_p.z = z_min;
min_p.C = 1;
//找x,y,z的最大值
float x_max = (*max_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.x < b.x; })).x;
float y_max = (*max_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.y < b.y; })).y;
float z_max = (*max_element(InputCloudPoint.begin(), InputCloudPoint.end(), [](Point3D& a, Point3D& b){return a.z < b.z; })).z;
//给max_p赋值
max_p.x = x_max;
max_p.y = y_max;
max_p.z = z_max;
max_p.C = 1;
return;
}
/*----------------------------
*功能：体素化网格方法实现下采样（PCL中的源码C++实现）
*-----------------------------
*输入：Piont3D的原始点云数据,下采样的体素大小x,y,z
*输出：采样之后的之后的Point3D结构的点云数据
*/
void VoxelGrid_ApplyFilter(vector<Point3D>&InputCloudPoint, vector<Point3D>&OutPointCloud, float X_Voxel, float Y_Voxel, float Z_Voxel)
{
//先判断输入的点云是否为空
if (InputCloudPoint.size()==0)
{
cout << "输入点云为空！" << endl;
return;
}
//存放输入点云的最大与最小坐标
Array4f min_p, max_p;
GetMaxMin(InputCloudPoint, min_p, max_p);
Array4f inverse_leaf_size_;
inverse_leaf_size_.x = 1 / X_Voxel;
inverse_leaf_size_.y = 1 / Y_Voxel;
inverse_leaf_size_.z = 1 / Z_Voxel;
inverse_leaf_size_.C = 1;
//计算最小和最大边界框值
Array4f min_b_, max_b_, div_b_, divb_mul_;
min_b_.x = static_cast<int> (floor(min_p.x * inverse_leaf_size_.x));
max_b_.x = static_cast<int> (floor(max_p.x * inverse_leaf_size_.x));
min_b_.y = static_cast<int> (floor(min_p.y * inverse_leaf_size_.y));
max_b_.y = static_cast<int> (floor(max_p.y * inverse_leaf_size_.y));
min_b_.z = static_cast<int> (floor(min_p.z * inverse_leaf_size_.z));
max_b_.z = static_cast<int> (floor(max_p.z * inverse_leaf_size_.z));
//计算沿所有轴所需的分割数
div_b_.x = max_b_.x - min_b_.x + 1;
div_b_.y = max_b_.y - min_b_.y + 1;
div_b_.z = max_b_.z - min_b_.z + 1;
div_b_.C= 0;
//设置除法乘数
divb_mul_.x = 1;
divb_mul_.y = div_b_.x;
divb_mul_.z =div_b_.x * div_b_.y;
divb_mul_.C = 0;
//用于计算idx和pointcloud索引的存储
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve(InputCloudPoint.size());
//第一步：遍历所有点并将它们插入到具有计算idx的index_vector向量中;具有相同idx值的点将有助于产生CloudPoint的相同点
for (int i = 0; i < InputCloudPoint.size();i++)
{
int ijk0 = static_cast<int> (floor(InputCloudPoint[i].x * inverse_leaf_size_.x) - static_cast<float> (min_b_.x));
int ijk1 = static_cast<int> (floor(InputCloudPoint[i].y * inverse_leaf_size_.y) - static_cast<float> (min_b_.y));
int ijk2 = static_cast<int> (floor(InputCloudPoint[i].z * inverse_leaf_size_.z) - static_cast<float> (min_b_.z));
//计算质心叶索引
int idx = ijk0 * divb_mul_.x + ijk1 * divb_mul_.y + ijk2 * divb_mul_.z;
index_vector.push_back(cloud_point_index_idx(static_cast<unsigned int> (idx), i));
}
//第二步：使用表示目标单元格的值作为索引对index_vector向量进行排序;实际上属于同一输出单元格的所有点都将彼此相邻
std::sort(index_vector.begin(), index_vector.end(), std::less<cloud_point_index_idx>());
//第三步：计数输出单元格，我们需要跳过所有相同的，相邻的idx值
unsigned int total = 0;
unsigned int index = 0;
unsigned int min_points_per_voxel_ = 0;
//first_and_last_indices_vector [i]表示属于对应于第i个输出点的体素的index_vector中的第一个点的index_vector中的索引，以及不属于第一个点的索引
std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
first_and_last_indices_vector.reserve(index_vector.size()); //分配内存空间
while (index < index_vector.size())
{
unsigned int i = index + 1;
while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx)
++i;
if (i - index >= min_points_per_voxel_)
{
++total;
first_and_last_indices_vector.push_back(std::pair<unsigned int, unsigned int>(index, i));
}
index = i;
}
//第四步：计算质心，将它们插入最终位置
//OutPointCloud.resize(total); //给输出点云分配内存空间
float x_Sum, y_Sum, z_Sum;
Point3D PointCloud;
unsigned int first_index, last_index;
for (unsigned int cp = 0; cp < first_and_last_indices_vector.size(); ++cp)
{
// 计算质心 - 来自所有输入点的和值，这些值在index_vector数组中具有相同的idx值
first_index = first_and_last_indices_vector[cp].first;
last_index = first_and_last_indices_vector[cp].second;
x_Sum = 0;
y_Sum = 0;
z_Sum = 0;
for (unsigned int li = first_index; li < last_index; ++li)
{
x_Sum += InputCloudPoint[index_vector[li].cloud_point_index].x;
y_Sum += InputCloudPoint[index_vector[li].cloud_point_index].y;
z_Sum += InputCloudPoint[index_vector[li].cloud_point_index].z;
}
PointCloud.x = x_Sum / (last_index - first_index);
PointCloud.y = y_Sum / (last_index - first_index);
PointCloud.z = z_Sum / (last_index - first_index);
OutPointCloud.push_back(PointCloud);
}
return;
}
int main()
{
const char*str = "PointCloud11.txt";
string WriteTxt = "11.txt";
vector<Point3D>Data;
vector<Point3D>Data1;
vector<Point3D>Data2;
ReadData(str, Data);
SYSTEMTIME sys;
GetLocalTime(&sys); //得到系统时间
double Time0 = sys.wMinute * 60 * 1000 + sys.wSecond * 1000 + sys.wMilliseconds;
int Point = 0;
VoxelGrid_ApplyFilter(Data,Data1,3,3,3);
GaussianDistribution_OutlierDetection(Data1, Data2);
GetLocalTime(&sys); //得到系统时间
double Time1 = sys.wMinute * 60 * 1000 + sys.wSecond * 1000 + sys.wMilliseconds;
cout << Time1 - Time0 << endl;
WriteData(WriteTxt, Data2);
return 0;
}
基于正态分布的点云离群点检测算法和基于VoxelGrid的点云下采样算法的完整版（C++实现）
猜你喜欢
转载自blog.csdn.net/pcb931126/article/details/82934656
今日推荐
周排行