背景：
进行边缘检测时提取出的点云有很多毛刺点，而且，输出的点云也不是按照顺时针或者逆时针排序。
自己写了一个函数，在xy平面，对点云进行逆时针排序，然后去除掉毛刺点。
去除后的效果为：
函数如下：
#include<pcl/io/pcd_io.h>
#include<algorithm>
#include<vector>
using namespace std;
bool is_invector(vector<vector<float>> v_boundary, vector<float> xy_coordinate) {
for (int i = 0; i < v_boundary.size();i++) {
if (v_boundary[i] == xy_coordinate) {
return true;
}
else {
return false;
}
}
}
// 定义函数，传入点云 和 vector 改变的是vector
void Filter_boundary(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, vector<vector<float>> filtered_boundary)
{
//#include<pcl/io/pcd_io.h>
//#include<algorithm>
//#include<vector>
加载待拟合点云
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
//if (pcl::io::loadPCDFile("D:\\C++project\\boss\\BianJie.pcd", *cloud) < 0)
//{
// PCL_ERROR("点云文件不存在！\a\n");
//}
// 定义容器存放原始x,y值
vector<vector<float>> original_boundary;
// 定义容器存放原始x,y值
vector<vector<float>> sorted_boundary;
// 定义存放tan值+索引的容器
vector<vector<float>> angle;
// 创建空点云，存放排序后，剔除毛刺点的 点云
//vector<vector<float>> filtered_boundary;
// 清空边界点的数据重新指定
original_boundary.clear();
// 将所有点的xy坐标存入 original_boundary 中
cout << "->边界所有点的个数：" << cloud->size() << "正在添加到vector中..."<< endl;
for (int i = 0; i < cloud->size();i++) {
cout << i << ": x:" << cloud->points[i].data[0] << " -- y:" << cloud->points[i].data[1] << endl;
vector<float> xy_coordinate{ cloud->points[i].data[0],cloud->points[i].data[1] };
if (is_invector(original_boundary,xy_coordinate)) {
count += 1;
continue;
}
original_boundary.push_back(xy_coordinate);
}
cout << "--------添加完成！跳过了 "<<count << " 个点---------" << endl;
// 找到 original_boundary 中最右边的点，存入right_point中
int max_index = 0; // 最大x对应点的索引
float max_x = original_boundary[max_index][0]; // 最大x值
for (int i = 1; i < original_boundary.size();i++) {
if (original_boundary[i][0]>max_x) {
// 更新最大值和索引
max_index = i;
max_x = original_boundary[i][0];
}
// 如果最大点x相同，选择y最小的那个
else if(original_boundary[i][0] == max_x){
if (original_boundary[i][1] < original_boundary[max_index][1]) {
// 更新最大值和索引
max_index = i;
max_x = original_boundary[i][0];
}
}
}
cout << "最大点的索引为：" << max_index << endl;
vector<float> right_point{ original_boundary[max_index][0], original_boundary[max_index][1]};
// 计算每个点和 right point 的tan
cout << "开始计算每个点的tan值..." << endl;
float ax = right_point[0]; // 得到 A 点的xy坐标
float ay = right_point[1];
for (int i = 0; i < original_boundary.size();i++) {
if (i==max_index) {
continue;
}
// 获取其他点的xy坐标
float bx = original_boundary[i][0];
float by = original_boundary[i][1];
float fi = float(i);
if (bx==ax) {
vector<float> tan{ -999,fi};
angle.push_back(tan); // angle中存放了 索引i 和对应的角度
cout << "第 " << i << " 个点的tan值为： " << tan[0] << endl;
}
else {
// vector怎么存放 int,float 还不会，所以先把i 转换为float类型
vector<float> tan{ (ax - bx) / (ay - by),fi };
cout << "第 " << i << " 个点的tan值为： " << tan[0] << endl;
angle.push_back(tan); // angle中存放了 索引i 和对应的角度
}
}
cout << "-------------tan计算结束--------------------" << endl;
// 通过tan对 angle中的元素进行降序排序--逆时针
cout << "排序后的tan | 索引 | x | y 为：" << endl;
sort(angle.rbegin(),angle.rend());
for (int i = 0; i < angle.size();i++) {
//利用排序后的angle索引，重新排列 original_boundary 中的点
sorted_boundary.push_back(original_boundary[angle[i][1]]);
cout << angle[i][0] << " | " << angle[i][1] <<" | "<< original_boundary[angle[i][1]][0] <<" | "<< original_boundary[angle[i][1]][1] << endl;
}
cout << "----------- 排序结束--------------" << endl;
cout << "->正在去除毛点..." << endl;
// 剔除 凹进去的点
filtered_boundary.push_back(sorted_boundary[0]);
for (int i = 1; i < sorted_boundary.size()-1;i++) {
float d0 = ax - sorted_boundary[i][0];
float d1 = ax - sorted_boundary[i+1][0];
float d2 = ax - sorted_boundary[i-1][0];
float flag = (d1 - d0) * (d2 - d0);
if (flag <0) {
filtered_boundary.push_back(sorted_boundary[i]);
}
}
// 将最右边那个点也加进去，作为逆时针的最后一个点
sorted_boundary.push_back(original_boundary[max_index]);
cout <<"去除前点数：" <<original_boundary.size() << " 去除后剩余："<<filtered_boundary.size() << endl;
}
使用：
传入待处理的点云，一个vector存放处理后的点的xy坐标。
std::vector<std::vector<float> > filtered_boundary;
list_boundary.clear();
Filter_boundary(cloud_boundary,filtered_boundary);
上面的图都是用python绘制的，可以将边界点云的xy存放到一个txt文件中，然后利用python读取并显示：
存放到txt文件的代码为：
std::cout << "一共"<< cloud_boundary->points.size() << "个边界点，正在添加到列表中..." << endl;
std::fstream fs;
fs.open("。/coordinate.txt",ios::out|ios::app);
for(int i = 0;i < cloud_boundary->points.size();i++)
{
fs<<cloud_boundary->points[i].data[0]<<" "<<cloud_boundary->points[i].data[1]<<endl;
std::cout << i << ": x:" <<cloud_boundary->points[i].data[0] << " -- y:" << cloud_boundary->points[i].data[1] << endl;
}
fs.close();
std::cout << "写入完成！" << endl;
python读取绘制部分代码如下：
import matplotlib.pyplot as plt
import numpy as np
import math
point = [] # 创建一个列表存放xy坐标
with open('coordinate.txt') as f:
for line in f.readlines():
x, y = line.split(' ')
x, y = float(x), float(y)
if [x, y] in point: # 存在相同点，不要
continue
point.append([x, y]) # 将点云
x_t = [i[0] for i in point]
y_t = [i[1] for i in point]
# 找x最大的点 假设x最大只有一个
max_x = max(x_t)
index = x_t.index(max_x) # 取出最大的索引值
# print(point[index])
ax, ay = point[index] # 得到最右边点的x,y坐标
a = point[index] # 取出最右边的点
angle = [] # 创建一个列表存放tan值
# 计算每个点和最右边点的tan值，存储到列表中，最右边的点先跳过
for i in range(len(point)):
if i == index:
continue
bx, by = point[i]
if bx == ax:
tan = -90 if by > ay else 90
else:
tan = (ay - by) / (ax - bx)
angle.append([i, tan])
print(angle)
angle.sort(key=lambda x: x[1]) # 按照第二个元素升序排序
angle.append([index, 999]) # 将最右边的点，加到排序后列表的最后即可
print(angle)
# 按照tan排序后的点：
new_point = [point[i[0]] for i in angle]
# 再对新得到的点，看看其有没有凹进去的情况，调整Num可以去掉
new_index = len(new_point) - 1
nnpoint = []
for i in range(new_index):
if i == 0:
nnpoint.append(new_point[i])
continue
if point[i + 1]:
d0 = (ax - new_point[i][0])
d1 = (ax - new_point[i + 1][0])
d2 = (ax - new_point[i - 1][0])
print("{}-{}-{}".format(d0, d1, d2))
if (d1 - d0) * (d2 - d0) < 0:
nnpoint.append(new_point[i])
print(new_point)
print(nnpoint)
x_t = [i[0] for i in nnpoint]
y_t = [i[1] for i in nnpoint]
plt.plot(x_t, y_t)
plt.show()
coordinate.txt展示：
关于如何去除毛刺点，可以参考：
---------------------
作者：Goafan
来源：CSDN
原文：https://blog.csdn.net/weixin_45231460/article/details/128890783
189
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
