三维点云处理-滤波器

前言：

点云中往往会存在很多噪声，也就是常说的离群点，如下左图中的黑色圈位置，可能会对有效数据的提取分析造成影响，因此在数据分析前通常会考虑采用滤波器（Filter）等手段进行一些预处理的操作。过滤后的点云如下右图所示，这样的点云有助于对其进行更好的数据分析，如平面估计、分类、分割提取等。

常见的滤波算法

噪声去除
1. Radius Outlier Removal 基于半径的异常值去除
2. Statistical Outlier Removal 统计异常值去除
下采样
1. Voxel Grid Downsampling 体素网格下采样
2. Farthest Point Sampling 最远点采样
3. Normal Space Sampling 法线空间采样
上采样/平滑/噪声去除
1. Bilateral Filter 双边滤波

1. Radius Outlier Removal (ROR) 基于半径的异常值去除

基本思想：噪声点一般是离群孤立的，而正常数据点其周围会存在足够多的近邻点。
方法：检查数据点在其指定半径范围内的邻居数量，如果邻居数量少于某个阈值，则视为异常值并去除。

2. Statistical Outlier Removal (SOR) 统计异常值滤除

基本思想：基于点云数据的统计分布（如均值和标准差）来识别并移除异常值。
方法：根据每一点与邻近点距离的分布信息，计算这些点的高斯分布参数，然后使用 3σ 准则进行过滤。

3. Voxel Grid Downsampling (VGD) 体素网格下采样

基本思想：将三维点云空间划分为一系列的体素（Voxel），然后每个体素内选取少量特征点来表征该体素信息，最终生成降采样后的点云数据。
那么如何从每个格子中选出一个点？常见的选取方法如：中心点（Centroid）、随机点（Random select）；

3.1 VGD - Exact

主要流程：根据输入的数据和体素尺寸，计算不同维度上可分割的体素数量，之后将数据映射到对应的体素单元得到索引，并根据点云对应的体素索引进行排序，最后从每个体素格子中选取特征点（中心点、随机点）来代表该体素单元，实现点云的降采样。

3.2 VGD - Approximated

前面提到的VGD - Extra 需要对所有点云的索引进行排序，C++ 中 sort 使用的是快速排序，平均时间复杂度为 $O (N * l o g N)$ ，已经是一种比较优的排序方法了，但是数据量过大时排序效率也会变低，无法满足一些移动计算平台的实时使用。可以采取一些其他的手段来进行加速，这里使用 sort 排序是为了将相同 index 的点放到连续的空间中，也就可以考虑使用 Hash Table来实现这个功能。
主要流程：根据输入的数据和体素尺寸，计算不同维度上可分割的体素数量，之后将数据映射到对应的体素单元得到索引。接着使用 Hash 函数将点索引映射到指定数量的容器中，不断迭代直到得到 M 个点。
当然，如果container_size 小于前面计算得到的 voxel_size时，就有可能出现Hash冲突，即不同区域的数据计算得到的hash值一样。比如container_size 为 100，index-15 和 index-115 通过哈希映射后得到的值均为 15，那么如何处理这种冲突呢？

一种处理方法是：如果我们发现了冲突，可以先将原来容器内的点随机选一个点输出，随后清空容器，并将新hash 值对应的点放入容器。

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

基本思想：从数据集中选择最远的点作为采样点，确保采样点分布均匀。
方法：从数据中随机选取一个点作为初始点，然后不断迭代地选择距离已有采样点集合的最远点。

5. Normal Space Sampling (NSS) 法向空间采样

基本思想：在法向量空间内均匀随机抽样，以保持地物特征。
方法：先在法向量空间构建一系列的容器，然后依据平面法向量将所有点放到对应容器中，接着从所有容器中均匀地选点。

6. Learning to Sample[1]

课程里也介绍了一下利用深度学习降采样的方法。通过几何意义上的限制，使得降采样后的点具有和降采样之前相似的几何状态
主要流程：输入 -> 神经网络 -> 输出降采样后的点云 -> 输入分类网络。
核心思路：不直接基于几何关系，而是基于语义检测任务，要求降采样之后的点能够达到语义分析的效果。

虽然没有直接基于几何关系进行点云降采样，但是在降采样任务训练的时候，也加入了一些几何约束，如：

7. Bilateral Filter (BF) 双边滤波

基本思想：在滤波过程中同时考虑空间邻近度和像素值相似性，以达到保边去噪的效果。
应用场景：图像处理、点云上采样等；
以图像为例：通过两个高斯核来对数据进行双边滤波。如对图像使用距离高斯核和颜色高斯核对图像进行模糊或者平滑。

对点云来说，在与图像融合时可以进行上采样填充。

Voxel Grid Downsampling 练习

1. 代码实现

前面已经简单介绍了VGD-Exact 体素网格降采样算法的基本原理，这部分主要是进行代码实现及简单测试。
首先需要先明确一下体素网格降采样算法的输入输入及主要处理流程。

输入部分：原始三维点云
输出部分：降采样后的三维点云
处理步骤：
1. 根据输入的数据和体素尺寸，计算不同维度上可分割的体素数量;
2. 将数据映射到对应的体素单元得到索引;
3. 根据点云对应的体素索引进行排序;
4. 从每个体素格子中选取特征点（中心点、随机点）来代表该体素单元；
5. 降采样后的点云输出。

#pargma once

#include <iostream>
#include <vector>
#include <string>
#include <Eigen/Dense>

class VoxelGridDownSampling{
public:
    // 使用枚举来定义体素点选取方式
    enum class SelectPointsMethod{
        CENTROID,
        RANDOM,
    };

    bool set_data(const Eigen::MatrixXd& data);		// 输入点云数据
    bool downsampling(double grid_size, SelectPointsMethod select_pts_method = SelectPointMethod::RANDOM);	// 点云降采样

    const std::vector<std::pair<int, Eigen::Vector3d>>& get_downsample_result(); // 返回降采样数据	

private:
    bool select_centroid(std::vector<std::pair<int, Eigen::Vector3d>>& downsample);	// 中心点降采样
    bool select_random(std::vector<std::pair<int, Eigen::Vector3d>>& downsample);	// 随机点降采样

private:
    Eigen::MatrixXd _pointcloud;
    std::vector<std::pair<int, Eigen::Vector3d>> _downsample;
};

#include "voxel_grid_downsampling.h"

#include <algorithm>
#include <cmath>

bool VoxelGridDownsampling::set_data(const Eigen::MatirxXd& data){
    _pointcloud = data;
    return true;
}

bool VoxelGridDownsampling::downsampling(
    double grid_size, SelectPointsMethod select_pts_method){
    // 1.计算点云的最大最小值
    Eigen::Vector3d max = _pointcloud.rowwise().maxCoeff();
    Eigen::Vector3d min = _pointcloud.rowwise().minCoeff();
    // 计算体素数量
    Eigen::Vector3d dim = (max - min) / grid_size;
    
    // 2.将点云投影到网格中
    for(int i = 0; i < _pointcloud.cols(); ++i){
        Eigen::Vector3d pts = _pointcloud.col(i);
        int h_x = floor((pts(0) - min(0)) / grid_size);
        int h_y = floor((pts(1) - min(1)) / grid_size);
        int h_z = floor((pts(2) - min(2)) / grid_size);

        int h = h_x + dim(0) * h_y + dim(0) * dim(1) * h_z;
        _downsample.emplace_back(std::make_piar(h, pts));
    }
    
    std::cout << "_downsample size: " << _downsample.size() << std::endl;
    
    // 3.索引排序
    std::sort(_downsample.begin(), _downsample.end(), 
        [] (const std::pair<int, Eigen::Vector3d>& p1, const std::pair<int, Eigen::Vector3d>& p2) -> bool {return p1.first < p2.first;} );
    // 4.点云降采样
    switch(select_pts_method) {
        case SelectPointsMethod::CENTROID:
            select_centroid(_downsample);
            break;
        case SelectPointsMethod::RANDOM:
            select_random(_downsample);
            break;
        default:
            std::cerr << "select points method error!" << std::endl;
            return false;
    }
    std::cout << "_downsample size after select: " << _downsample.size() << std::endl;
    return true;
}

const std::vector<std::pair<int, Eigen::Vector3d>>& VoxelGridDownsampling::get_downsample_result(){
    return _downsample;
}

bool VoxelGridDownsampling::select_centroid(std::vector<std::pair<int, Eigen::Vector3d>>& downsample){
    // 定义存储网格点云和降采样后点云的变量
    std::vector<std::pair<int, Eigen::Vector3d>> grid_pts;
    std::vector<std::pair<int, Eigen::Vector3d>> res;
    // 依次对每个点云进行处理
    for(size_t i = 0; i < downsample.size(); ++i){
        // grid_pts 为空
        if(grid_pts.size() == 0){
            grid_pts.emplace_back(downsample[i]);
        }else if(grid_pts[grid_pts.size() - 1].first != downsample[i].first){
            // grid_pts点云与downsample[i]网格索引不一致
            // 计算中心点
            double cx = 0, cy = 0, cz = 0;
            for(size_t j = 0; j < grid_pts.size(); ++j){
                cx += grid_pts[j].second(0);
                cy += grid_pts[j].second(1);
                cz += grid_pts[j].second(2);
            }
            cx /= grid_pts.size();
            cy /= grid_pts.size();
            cz /= grid_pts.size();
            res.emplace_back(std::make_pair(grid_pts[0].first, Eigen::Vector3d(cx, cy, cz)));

            // 清空当前网格点云，并添加新点云
            grid_pts.clear();
            grid_pts.emplace_back(downsample[i]);
        }else{
            // downsample[i]与grid_pts点云索引一致
            grid_pts.emplace_back(downsample[i]);
        }     
    }
    // 更新降采样点云
    downsample.swap(res);
    return true;
}

bool VoxelGridDownSampling::select_random(std::vector<std::pair<int, Eigen::Vector3d>>& downsample){
    // 定义存储网格点云和结果点云的变量
    std::vector<std::pair<int, Eigen::Vector3d>> grid_pts;
    std::vector<std::pair<int, Eigen::Vector3d>> res;
    // 处理每个点云并选取random点
    for(size_t i = 0; i < downsample.size(); ++i){
        // 网格内无点云，直接添加
        if(grid_pts.size() == 0){
            grid_pts.emplace_back(downsample[i]);
        }else if(grid_pts[grid_pts.size() - 1].first != downsample[i].first){
            // 网格点云和当前点云归属不同网格，计算均值点
            int rand_idx = rand() % grid_pts.size();
            Eigen::Vector3d pts;
            pts << grid_pts[rand_idx].second(0),
                   grid_pts[rand_idx].second(1),
                   grid_pts[rand_idx].second(2);

            res.emplace_back(std::make_pair(grid_pts[0].first, pts));
            // 清空网格内点云并添加新点云
            grid_pts.clear();
            grid_pts.emplace_back(downsample[i]);
        }else{
            // 正常添加网格点云
            grid_pts.emplace_back(downsample[i]);
        }
    }

    // 更新下采样点云结果
    downsample.swap(res);
    return true;    
}