各种滤波算法

这里整理了一些滤波算法，比如用于噪声点去除的Radius Outlier Removal和Statistical Outlier removal；用于降采样的voxel grid downsample，Farthest Point Sampling和Normal Space Sampling；用于上采样和平滑的Bilateral Filter；

1. 半径离群点去除(Radius Outlier Removal，半径滤波)

• 算法思想
  半径离群点去除算法的原理主要涉及以下几个步骤：
  • 设定参数：首先需要设定半径 $r$ 和最小邻居数k作为算法的参数。半径$r$决定了在计算离群点时考虑的范围，最小邻居数 $k$ 决定了一个点周围需要多少个邻居点才能将该点视为非离群点。
  • 建立数据结构：为了快速查找每个点的邻居点，通常会使用 KD 树或者球树等数据结构来存储点云数据，并加速邻居点的搜索。
  • 遍历点云：对于点云中的每个点$P_i$，在给定的半径$r$内查找其邻居点，计算邻居点的数量 $N_i$
  • 判断离群点：对于每个点$P_i$，如果其邻居点的数量小于设定的最小邻居数$k$，则将$P_i$视为离群点。
  • 移除离群点：将被标记为离群点的点从点云中移除，得到去除离群点后的点云。

这种方法主要用于去除点云中的噪声点或者异常点，以提高点云数据的质量和准确性。

• 代码1(使用PCL)

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>

int main() {
    // Load point cloud data
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("input_cloud.pcd", *cloud);

    // Set parameters for radius outlier removal
    float radius = 0.1; // Radius search
    int min_neighbors = 10; // Minimum number of neighbors
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
    outrem.setInputCloud(cloud);
    outrem.setRadiusSearch(radius);
    outrem.setMinNeighborsInRadius(min_neighbors);

    // Apply radius outlier removal
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    outrem.filter(*cloud_filtered);

    // Save filtered point cloud
    pcl::io::savePCDFile("output_cloud.pcd", *cloud_filtered);

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

• 代码2(不使用PCL, 自己构建KDTree)

#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>

struct Point {
    float x;
    float y;
    float z;
    int id;  // 用于标识点的ID

    Point(float x, float y, float z, int id) : x(x), y(y), z(z), id(id) {}
};

struct Node {
    Point point;
    Node* left;
    Node* right;

    Node(Point p) : point(p), left(nullptr), right(nullptr) {}
};

// 定义KD树
class KDTree {
private:
    Node* root;

    Node* buildTree(std::vector<Point>& points, int depth, int start, int end) {
        if (start > end) return nullptr;

        int axis = depth % 3;  // 在三维空间中进行分割

        std::sort(points.begin() + start, points.begin() + end + 1, [axis](const Point& a, const Point& b) {
            if (axis == 0) return a.x < b.x;
            else if (axis == 1) return a.y < b.y;
            else return a.z < b.z;
        });

        int mid = start + (end - start) / 2;
        Node* node = new Node(points[mid]);
        node->left = buildTree(points, depth + 1, start, mid - 1);
        node->right = buildTree(points, depth + 1, mid + 1, end);
        return node;
    }

    void searchNeighbors(Node* node, const Point& query, float radius, std::vector<int>& neighbors) {
        if (node == nullptr) return;

        float dist = calculateDistance(node->point, query);
        if (dist <= radius) {
            neighbors.push_back(node->point.id);
        }

        int axis = 0;
        if (axis == 0) {
            if (query.x - radius <= node->point.x) searchNeighbors(node->left, query, radius, neighbors);
            if (query.x + radius > node->point.x) searchNeighbors(node->right, query, radius, neighbors);
        } else if (axis == 1) {
            if (query.y - radius <= node->point.y) searchNeighbors(node->left, query, radius, neighbors);
            if (query.y + radius > node->point.y) searchNeighbors(node->right, query, radius, neighbors);
        } else {
            if (query.z - radius <= node->point.z) searchNeighbors(node->left, query, radius, neighbors);
            if (query.z + radius > node->point.z) searchNeighbors(node->right, query, radius, neighbors);
        }
    }

public:
    KDTree(std::vector<Point>& points) {
        root = buildTree(points, 0, 0, points.size() - 1);
    }

    std::vector<int> getNeighbors(const Point& query, float radius) {
        std::vector<int> neighbors;
        searchNeighbors(root, query, radius, neighbors);
        return neighbors;
    }

    float calculateDistance(const Point& p1, const Point& p2) {
        float dx = p1.x - p2.x;
        float dy = p1.y - p2.y;
        float dz = p1.z - p2.z;
        return std::sqrt(dx * dx + dy * dy + dz * dz);
    }
};

int main() {
    std::vector<Point> points = {{1, 2, 3, 0}, {4, 5, 6, 1}, {7, 8, 9, 2}, {10, 11, 12, 3}};
    KDTree kd_tree(points);

    Point query_point(3, 4, 5, -1);
    float radius = 2.0;
    std::vector<int> neighbors = kd_tree.getNeighbors(query_point, radius);

    for (int id : neighbors) {
        std::cout << "Neighbor ID: " << id << std::endl;
    }

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

2. 统计离群点剔除(Statistical Outlier Removal, 统计滤波)

统计滤波基于一种假设： 点云中大部分点都是从同一平均分布中采样得到的，而离群点则来自一个不同的分布。

• 算法思想步骤如下:

  • 设定参数：首先需要设定滤波算法的参数，包括点云中每个点的邻域范围（半径或K近邻），以及用于判断离群点的统计参数阈值，如均值和标准差的倍数。
  • 计算邻域统计参数：对于点云中的每个点，计算其邻域内点的统计参数，通常包括均值和标准差。可以选择使用固定大小的邻域或者自适应的 K 近邻来计算统计参数。
  • 判断离群点：根据设定的统计参数阈值，判断每个点是否为离群点。通常采用的判断方式是，如果点的某个统计参数（如距离邻域点的平均距离）超过了设定的阈值，则将该点标记为离群点。
  • 滤除离群点：将被标记为离群点的点从点云中移除，得到去除离群点后的点云。

• 公式表达步骤如下:

  • 设置邻域r，得到点P周围邻域r内的若干点，记为k个(其实不是每个点在周围邻域内的邻居都有k个)
  • 计算所有点与自身邻域邻居的距离值$d_{ij},i\in [1,2,...,m],j\in [1,2,...,k]$。m表示输入的点云个数，k表示每个点的邻居个数(只是方便表达，每个点的邻域邻居个数会有不同)
  • 通过$d_{ij}$，计算该组点云的距离高斯分布$d～N(\mu ,\sigma )$也就是说这组点的每个点与其邻域内邻居的欧式距离均值都要遵循这个分布
    $$\mu =\frac{1}{m\times k}\sum _{i=1}^m\sum _{j=1}^k{d}_{ij},\sigma =\sqrt{\frac{1}{m\times k}\sum _{i=1}^m\sum _{j=1}^k(d_{ij}-\mu {)}^2}$$
  • 计算每个点与其邻域半径范围内点的距离均值$d$
  • 剔除距离均值d不再置信度区间内的点，即如果不满足下边的公式，就要剔除
    $$\sum _{j=1}^{k}d>\mu +3\sigma \text{ or }\sum _{j=1}^{k}d<\mu -3\sigma$$

• 代码

#include <iostream>
#include <vector>
#include <cmath>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

typedef pcl::PointXYZ PointType;
typedef pcl::PointCloud<PointType> PointCloud;

// Statistical outlier removal filter function using KDTree
PointCloud::Ptr statisticalOutlierRemovalKDTree(const PointCloud::Ptr& cloud, float radius) {
    pcl::KdTreeFLANN<PointType> kdtree;
    kdtree.setInputCloud(cloud);

    PointCloud::Ptr filtered_cloud(new PointCloud);
    std::vector<int> pointIdxRadiusSearch;
    std::vector<float> pointRadiusSquaredDistance;

    for (size_t i = 0; i < cloud->points.size(); ++i) {
        if (kdtree.radiusSearch(cloud->points[i], radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
            std::vector<float> distances;
            for (size_t j = 0; j < pointIdxRadiusSearch.size(); ++j) {
                distances.push_back(std::sqrt(pointRadiusSquaredDistance[j]));
            }

            float sum_distances = 0.0;
            for (float dist : distances) {
                sum_distances += dist;
            }

            float mean_distance = sum_distances / distances.size();

            float sum_sq_diff = 0.0;
            for (float dist : distances) {
                sum_sq_diff += (dist - mean_distance) * (dist - mean_distance);
            }
            float std_dev = std::sqrt(sum_sq_diff / distances.size());

            float confidence_interval = 3 * std_dev;

            float sum_filtered_distances = 0.0;
            for (float dist : distances) {
                if (dist <= (mean_distance + confidence_interval) && dist >= (mean_distance - confidence_interval)) {
                    sum_filtered_distances += dist;
                }
            }

            float mean_filtered_distance = sum_filtered_distances / distances.size();

            if (sum_filtered_distances > (mean_distance + confidence_interval) || 
                sum_filtered_distances < (mean_distance - confidence_interval)) {
                // Point is an outlier, do not add to filtered cloud
            } else {
                // Point is within confidence interval, add to filtered cloud
                filtered_cloud->points.push_back(cloud->points[i]);
            }
        }
    }

    filtered_cloud->width = filtered_cloud->points.size();
    filtered_cloud->height = 1;
    return filtered_cloud;
}

int main() {
    PointCloud::Ptr cloud(new PointCloud);
    // Fill 'cloud' with point data (assuming loaded from file)

    float neighborhood_radius = 1.0;

    PointCloud::Ptr filtered_cloud = statisticalOutlierRemovalKDTree(cloud, neighborhood_radius);

    std::cout << "Filtered Cloud:" << std::endl;
    for (size_t i = 0; i < filtered_cloud->points.size(); ++i) {
        std::cout << "(" << filtered_cloud->points[i].x << ", " << filtered_cloud->points[i].y << ", " << filtered_cloud->points[i].z << ")" << std::endl;
    }

    return 0;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

3. 体素网格将采样(voxel grid downsampling)

体素网格降采样的本质就是，设置体素网格的大小，对于包含点云的体素栅格，从栅格中提取一个点。如果只是针对坐标值来说，可以是随机提取，可以求均值，也可以直接使用栅格的中心。但是对于携带的物理信息，比如说反射强度等属性，可以采用投票的方式

• 算法原理

  • 设定体素大小：首先需要设定体素网格的大小，即立方体的边长。这个大小决定了最终采样后点云的密度和分辨率。
  • 创建体素网格：将点云中的所有点按照体素大小划分到对应的立方体网格中。这一步可以使用一个三维数组或者类似的数据结构来表示体素网格。
  • 采样过程：对于每个立方体网格，选择其中包含点的网格作为采样后的点，可以选择点的均值或者其他统计值作为采样点的位置。
  • 降采样：根据体素网格的大小，可以选择保留每个体素网格中的一个采样点，或者按照一定规则保留部分采样点，实现点云的降采样。

• 代码(PCL版本)

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>

int main() {
    // Load point cloud data
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("input_cloud.pcd", *cloud);

    // Voxel grid downsampling
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(cloud);
    voxel_grid.setLeafSize(0.1, 0.1, 0.1);  // Set voxel size
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
    voxel_grid.filter(*cloud_downsampled);

    // Save downsampled cloud
    pcl::io::savePCDFile<pcl::PointXYZ>("output_downsampled.pcd", *cloud_downsampled);

    std::cout << "PointCloud before downsampling: " << cloud->size() << " points" << std::endl;
    std::cout << "PointCloud after downsampling: " << cloud_downsampled->size() << " points" << std::endl;

    return 0;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

4. 最远点采样(Farthest Point Sampling, FPS)

最远点采样（Farthest Point Sampling，FPS）算法是一种常用于点云数据采样的方法，其原理如下：

• 初始化：随机选择一个点作为起始点，并将其加入到采样点集合中。
• 迭代采样：对于剩余的点，计算它们到已选取采样点集合中所有点的最短距离（或者最远距离），选取距离最大的点作为下一个采样点，并将其加入到采样点集合中。
• 重复直到结束：重复上述过程，直到采样点集合达到所需的数量或者所有点都被选取。

这个算法的关键是在每次迭代中高效地找到距离已选取采样点集合最远的点。这通常可以通过优先级队列（如堆）来实现，以快速找到最远点。

5. 正态空间将采样(Normal Space Sampling, NSS)

普通的采样，对于表面非均匀的场景不太友好。如果点云曲面有小的凹陷，如果使用均匀采样，就会遗失这种特征，所以引入了NSS的方法，NSS可以保证所有的法向量范围都可以采样的到。

• NSS算法原理：
  • 在法向量空间中建立一些容器，每个容器对应一个不同的法向量方向。
  • 遍历点云中的每个点，根据点的法向量将点放入对应的法向量容器中。
  • 从每个不同的法向量容器中取出一定数量的点作为采样点，可以根据需求调整采样点的数量。
    这种方法能够有效地在法向量空间内对点云进行采样，保留了不同法向量方向上的特征点，有助于保持点云的形状信息。
    

6. 高斯滤波

这里的S代表像素p的位置邻域空间，q为邻域内的某个位置，$G_{\sigma }(x)$代表像素p和像素q两个像素点在某个尺度上的高斯权重(这里两者在该尺度上的距离越远，权重越小，表示像素点q给像素点p的贡献越小)，$I_q$这里可以指代像素点的RGB通道值。也就是说这里是使用高斯滤波来求像素点p的rgb通道值(借助像素p周围邻域内的所有像素点的rgb通道值完成滤波)

具体来说，高斯滤波器在处理图像或信号时，通过将每个像素点或采样点与周围像素点或采样点的值按照高斯函数的权重进行加权平均，从而达到平滑、去噪或者边缘保留的效果。

高斯滤波的本质可以用以下几点来总结：

加权平均：高斯滤波器对每个像素或采样点的值进行加权平均，权重由高斯函数决定。
平滑效果：高斯函数在中心值附近有较高的权重，而在距离中心值较远的位置权重逐渐减小，因此对信号进行高斯滤波可以实现平滑效果，去除噪声。
边缘保留：由于高斯函数的权重在中心值附近较高，在边缘处权重急剧减小，因此高斯滤波在一定程度上能够保留图像或信号的边缘特征，避免过度平滑导致细节丢失

7. 双边滤波(Bilateral Filter)

双边滤波是说同时使用两个尺度的高斯距离来给权重