5基础点云处理  

      从本章开始，我开启了这本书第二部分的学习（数学好难，终于迷迷糊糊地学完啦）,本章开始激光雷达的定位于建图，激光雷达传感器是自动驾驶和机器人应用中最重要的传感器之一。在本章中我们将学习一些算法来处理最近邻问题和拟合问题，来从激光雷达获取的点云数据来进行简单的几何形状拟合，为后续的2Dslam与3Dslam打下基础。在进行基础点云数据处理时最基本的问题是如何定义空间上的相邻性，也就是最近邻问题。

5.1激光雷法传感器与点云的数学模型

5.1.1激光雷达传感器的数学模型

        自动驾驶使用的激光雷达主要分为机械旋转式激光雷达与固态激光雷达两种形式。机械旋转式激光雷达可以看成系列以固定频率旋转的激光探头。每个探头能够快速探测外部物体离自身的距离，这些探头每旋转一周，就可以完成一次对周围场景的扫描，能探测360°视野范围内的3D信息。特点：为360°扫描特性对定位和建图十分有利，环顾视野只需采集一遍就可以构建整个路段的地图，也让点云定位不易被物体遮挡，但其在价格和寿命方面劣势明显，固态激光雷达不能旋转只能探测120°视野范围内的3D信息，特点价格和寿命方面优势明显，代价是牺牲一定的视野，但是可以用多个雷达来弥补。

        单个激光探头的测量较为简单：它只是测量某个空间点离自身的距离，记为，激光探头本身按照某个倾斜角放置在车辆上，于是可以得到末端点的空间位置，这种模型称为模型。

       距离 ，方位角，俯仰角，由此得到在雷达参考系下的位置：

        探头旋转一周，就得到了方位角从0°到360°的末端点，把这一圈点称为一次扫描数据。

5.1.2点云的表达

        最简单、最原始的点云表达方式就是数组。点云也可以携带其他信息，如反射率、所属的线束、RGB的颜色。来自RGB相机的点云还可以储存每个点在图像中的行数和列数信息。下面通过高翔老师给出的一个程序实例来实现基础点云的读写和可视化。点云地图的数据：

也可使用C++里的PCL接口来改变点云的颜色：

#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_types.h>

int main() {
    // 定义点云类型（支持RGB）
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    // 读取PCD文件
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("map_example.pcd", *cloud) == -1) {
        PCL_ERROR("无法读取文件！\n");
        return -1;
    }
    // 遍历所有点，设置为蓝色
    for (auto& point : *cloud) {
        point.r = 0;
        point.g = 0;
        point.b = 255;
    }
    // 可视化
    pcl::visualization::PCLVisualizer viewer("Colored Point Cloud");
    viewer.addPointCloud(cloud, "cloud");
    viewer.spin();
    // 保存文件
    pcl::io::savePCDFile("cpp_output.pcd", *cloud);
    return 0;
}

单次扫描的数据：

下面将实现如何读取并可视化点云。

using PointType = pcl::PointXYZI;
using PointCloudType = pcl::PointCloud<PointType>;

DEFINE_string(pcd_path, "./data/ch5/map_example.pcd", "点云文件路径");

/// 本程序可用于显示单个点云，演示PCL的基本用法
/// 实际上就是调用了pcl的可视化库，类似于pcl_viewer
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    if (FLAGS_pcd_path.empty()) {
        LOG(ERROR) << "pcd path is empty";
        return -1;
    }

    // 读取点云
    PointCloudType::Ptr cloud(new PointCloudType);
    pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);

    if (cloud->empty()) {
        LOG(ERROR) << "cannot load cloud file";
        return -1;
    }

    LOG(INFO) << "cloud points: " << cloud->size();

    // visualize
    pcl::visualization::PCLVisualizer viewer("cloud viewer");
       pcl::visualization::PointCloudColorHandlerGenericField<PointType> handle(cloud, "z");
    
    // 3. 向可视化窗口添加点云：
    // 参数1：待显示的点云智能指针
    // 参数2：颜色处理器（指定点云的着色方式）
    // 注：若不指定颜色处理器，默认显示白色点云
    viewer.addPointCloud<PointType>(cloud, handle);
    viewer.spin();

    return 0;
}

读取结果：

5.1.3Packet

        在激光传感器中，雷达的旋转速率、各探头相对雷达中心的俯仰角等参数，都是在雷达设计时或者运行时已知的参数，被称为雷达的内参数，属于雷达固定的参数，真正的测量数据是那些运行时变化的部分，对雷达来通常是指物体的探测距离与反射率，在储存点云原始数据时也可以只记录下这些部分来代替单纯的点云，这就是雷达数据包的思路。

5.1.4俯视图和距离图

        如果我们希望用栅格的方式表达激光点云，并且尝试一些以栅格地图为基础的路径规划避障等算法，或者希望在地图数据上做2D的标注，就有必要以俯视的方式来表达点云地图。把三维信息转换为二维通常要丢弃一部分信息，这种俯视图的做法显然就是丢弃了点云的高度信息。

        传感器是水平时点云的可以分别对应水平坐标和高度坐标，而倾斜旋转的传感器则需要额外的地面或者世界坐标系的方向信息。将点云坐标转换为图像坐标，需要定义一个分辨率，以确定每个像素对应多少米的距离。同时，我们希望图像中心正对点云中心，图像的长宽取决于点云的 范围。于是，设点云的中心为 ，图像中心为,，那么，一个坐标为 的点云应该落在图像的 处，它们满足:

        而坐标则可以显示不同颜色，用于区分点云的高度。

        点云图到俯视图：

       由于激光点云覆盖了周围 360°，所以投出来的图像也是环视的 360°。我们取图像的横坐标为激光雷达的方位角，纵坐标则取俯仰角，这种投影被称为距离图。进一步,知道雷达每个线数对应的俯仰鱼，也可以将线数作为纵坐标。用这种方式做出的图像也称为距离图。书中将点云转换为距离图，图像为：

        由上述可知，表达雷达数据的有距离图、俯视图、点云图像，虽然图像表述方式的改变并未改变测量数据本身，但是改变了点和点的相邻距离，这种分布方式的改变会影响一些聚类或者特征提取算法的性能。

5.2最近邻问题

5.2.1暴力最近邻法

        暴力最近邻法(bfnn)是最简单直观的最近邻计算方法。如果我们搜索一个点的最近邻，称为暴力最近邻，那么搜索k个最近邻称为暴力k近邻搜索。

        暴力最近邻搜索(BFNN)：给定点云和待查找点，计算与中每个点的距离，并给出最小距离。当点数为n时，复杂度为。

        暴力k近邻（BF kNN）：

        1. 给定点云和查找点，计算对所有点的距离

        2.对第1步的结果排序。

        3.选择个最近的点。

        4.对所有的重复步骤1~3.

        在进行实例演示中，使用到了GridNN，GridNN 的核心思想是「空间网格化」：将 3D/2D 空间按固定分辨率（如此套代码中 0.1m）划分成一个个互不重叠的网格单元，每个点云会落入某个网格。

        演示实例与单/多线程的暴力匹配各调用5次的实例结果：

 5.2.2栅格与体素方

        栅格近邻法，如果将点云在空间层面划分为栅格，就计算每个点对应的栅格位置，进而对所有点进行空间索引，生成栅格时需要注意：

1.根据点云的疏密程度，需要预先定义栅格的分辨率（超参数，凭借经验来定）。

2.格子的边界是离散的，在查找最近邻时，除了在格内查找，还应该在他的周边查找，周边越多算法效率越低。

3.由于栅格有限很可能找不到某个点的最近邻，所以除了评估栅格法的计算效率，还应评估它的正确性。

        体素最近邻法则将空间分为三维的体素，一个体素与周围6个体素直接相邻，如果加上对角的话，还可以增加8个，共14个相邻体素。

        哈希表是 GridNN 的核心数据结构—— 它用于快速映射「网格索引」和「该网格内的点云数据」，从而避免暴力搜索的全局遍历，大幅提升近邻查找效率，在GridNN中哈希表的作用1.将每个点的「网格索引」作为键（Key），「点的索引 / 坐标」作为值（Value），存入哈希表，来对GridNN中的空间点进行定位；2.对于待查找的点，先计算其所在的网格索引，通过哈希表快速找到该网格（及邻域网格）内的所有点，仅在这些点中遍历查找最近邻。

        空间哈希函数:在空间网格化（如 GridNN）场景中，空间哈希函数（Spatial Hash Function） 的核心作用是将「多维网格索引」（2D 的 (i,j)、3D 的 (i,j,k)）映射为「单一整数键（Key）」，以便哈希表（如unordered_map）快速存储和查询。其设计需满足三个核心要求：

1. 唯一性：尽可能避免不同网格索引映射到同一个 Key（哈希冲突）；
2. 高效性：计算过程简单（无复杂运算），适配实时点云处理；
3. 非负性：哈希表的 Key 通常为无符号整数，需将负网格索引（如 x=-0.2m 对应 i=-2）转为非负值。

        在后续程序实现中哈希函数可以使用各维度数据乘以大质数，再求异或，最后对大整数取模。最近邻查找的逻辑：1.计算给定点所在的栅格。2.根据最近邻的定义，查找附近的栅格。3.根据第二步的结果，使用暴力匹配计算这些栅格的最近邻。其中二维栅格允许取 0、8 个最近邻，三维体素则可以取0、6、14个最近邻。

        栅格法最近邻存在两种情况：

        1.栅格法检测出来的最近邻，实际上并不是最近邻。这种情况称为假阳性(False Positive)。
一次实验中假阳性的次数记作FP。
        2.实际中的某个最近邻，在栅格法中并没有检测到。这种情况称为假阴性(False Negative)。
一次实验中假阴性的次数记作FN。
        利用 FP 和 FN 的定义，可以定义算法的准确率(Precision)和召回率(Recall)。记近邻算法总共计算了m次最近邻，而真值共给出了n个最近邻，那么准确率和召回率可以定义为

        准确率描述了算法检出的最近邻中的正确性，而召回率描述了所有正确结果中，算法检测到的正确结果占所有真实结果的比例。栅格法最近邻在误检和漏检方面的问题主要是因为栅格本质是对空间进行了硬性划分，若点出现在划分边界线附近，那么最近邻就容易出问题。

5.2.3二分树与K-d树

        二分查找的复杂度是，线性查找则是。二分查找过程本身就是树状的:对于给定的元素与容器,先比较与容器中心元素的大小关系。如果比较小，就继续将它与左半部分容器的元素比较;反之，则与右半部分容器的元素比较。二分树唯一的缺点是只对一维数据有效。

        K-d树是二分树的高维版本，通过超平面来分割点云之后进行查找。

        具体讲解请看这位up主的视频：KD树讲解视频

        kd树建树实例：

bool KdTree::BuildTree(const CloudPtr &cloud) {
    if (cloud->empty()) {
        return false;
    }

    cloud_.clear();
    cloud_.resize(cloud->size());
    for (size_t i = 0; i < cloud->points.size(); ++i) {
        cloud_[i] = ToVec3f(cloud->points[i]);
    }

    Clear();
    Reset();

    IndexVec idx(cloud->size());
    for (int i = 0; i < cloud->points.size(); ++i) {
        idx[i] = i;
    }

    Insert(idx, root_.get());
    return true;
}

void KdTree::Insert(const IndexVec &points, KdTreeNode *node) {
    nodes_.insert({node->id_, node});

    if (points.empty()) {
        return;
    }

    if (points.size() == 1) {
        size_++;
        node->point_idx_ = points[0];
        return;
    }

    IndexVec left, right;
    if (!FindSplitAxisAndThresh(points, node->axis_index_, node->split_thresh_, left, right)) {
        size_++;
        node->point_idx_ = points[0];
        return;
    }

    const auto create_if_not_empty = [&node, this](KdTreeNode *&new_node, const IndexVec &index) {
        if (!index.empty()) {
            new_node = new KdTreeNode;
            new_node->id_ = tree_node_id_++;
            Insert(index, new_node);
        }
    };

    create_if_not_empty(node->left_, left);
    create_if_not_empty(node->right_, right);
}

bool KdTree::GetClosestPoint(const PointType &pt, std::vector<int> &closest_idx, int k) {
    if (k > size_) {
        LOG(ERROR) << "cannot set k larger than cloud size: " << k << ", " << size_;
        return false;
    }
    k_ = k;

    std::priority_queue<NodeAndDistance> knn_result;
    Knn(ToVec3f(pt), root_.get(), knn_result);

    // 排序并返回结果
    closest_idx.resize(knn_result.size());
    for (int i = closest_idx.size() - 1; i >= 0; --i) {
        // 倒序插入
        closest_idx[i] = knn_result.top().node_->point_idx_;
        knn_result.pop();
    }
    return true;
}

bool KdTree::GetClosestPointMT(const CloudPtr &cloud, std::vector<std::pair<size_t, size_t>> &matches, int k) {
    matches.resize(cloud->size() * k);

    // 索引
    std::vector<int> index(cloud->size());
    for (int i = 0; i < cloud->points.size(); ++i) {
        index[i] = i;
    }

    std::for_each(std::execution::par_unseq, index.begin(), index.end(), [this, &cloud, &matches, &k](int idx) {
        std::vector<int> closest_idx;
        GetClosestPoint(cloud->points[idx], closest_idx, k);
        for (int i = 0; i < k; ++i) {
            matches[idx * k + i].second = idx;
            if (i < closest_idx.size()) {
                matches[idx * k + i].first = closest_idx[i];
            } else {
                matches[idx * k + i].first = math::kINVALID_ID;
            }
        }
    });

    return true;
}

void KdTree::Knn(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
    if (node->IsLeaf()) {
        // 如果是叶子，检查叶子是否能插入
        ComputeDisForLeaf(pt, node, knn_result);
        return;
    }

    // 看pt落在左还是右，优先搜索pt所在的子树
    // 然后再看另一侧子树是否需要搜索
    KdTreeNode *this_side, *that_side;
    if (pt[node->axis_index_] < node->split_thresh_) {
        this_side = node->left_;
        that_side = node->right_;
    } else {
        this_side = node->right_;
        that_side = node->left_;
    }

    Knn(pt, this_side, knn_result);
    if (NeedExpand(pt, node, knn_result)) {  // 注意这里是跟自己比
        Knn(pt, that_side, knn_result);
    }
}

bool KdTree::NeedExpand(const Vec3f &pt, KdTreeNode *node, std::priority_queue<NodeAndDistance> &knn_result) const {
    if (knn_result.size() < k_) {
        return true;
    }

    if (approximate_) {
        float d = pt[node->axis_index_] - node->split_thresh_;
        if ((d * d) < knn_result.top().distance2_ * alpha_) {
            return true;
        } else {
            return false;
        }
    } else {
        // 检测切面距离，看是否有比现在更小的
        float d = pt[node->axis_index_] - node->split_thresh_;
        if ((d * d) < knn_result.top().distance2_) {
            return true;
        } else {
            return false;
        }
    }
}

void KdTree::ComputeDisForLeaf(const Vec3f &pt, KdTreeNode *node,
                               std::priority_queue<NodeAndDistance> &knn_result) const {
    // 比较与结果队列的差异，如果优于最远距离，则插入
    float dis2 = Dis2(pt, cloud_[node->point_idx_]);
    if (knn_result.size() < k_) {
        // results 不足k
        knn_result.emplace(node, dis2);
    } else {
        // results等于k，比较current与max_dis_iter之间的差异
        if (dis2 < knn_result.top().distance2_) {
            knn_result.emplace(node, dis2);
            knn_result.pop();
        }
    }
}

bool KdTree::FindSplitAxisAndThresh(const IndexVec &point_idx, int &axis, float &th, IndexVec &left, IndexVec &right) {
    // 计算三个轴上的散布情况，我们使用math_utils.h里的函数
    Vec3f var;
    Vec3f mean;
    math::ComputeMeanAndCovDiag(point_idx, mean, var, [this](int idx) { return cloud_[idx]; });
    int max_i, max_j;
    var.maxCoeff(&max_i, &max_j);
    axis = max_i;
    th = mean[axis];

    for (const auto &idx : point_idx) {
        if (cloud_[idx][axis] < th) {
            // 中位数可能向左取整
            left.emplace_back(idx);
        } else {
            right.emplace_back(idx);
        }
    }

    // 边界情况检查：输入的points等于同一个值，上面的判定是>=号，所以都进了右侧
    // 这种情况不需要继续展开，直接将当前节点设为叶子就行
    // if (point_idx.size() > 1 && (left.empty() || right.empty())) {
    //     return false;
    // }

    return true;
}

void KdTree::Reset() {
    tree_node_id_ = 0;
    root_.reset(new KdTreeNode());
    root_->id_ = tree_node_id_++;
    size_ = 0;
}

void KdTree::Clear() {
    for (const auto &np : nodes_) {
        if (np.second != root_.get()) {
            delete np.second;
        }
    }

    nodes_.clear();
    root_ = nullptr;
    size_ = 0;
    tree_node_id_ = 0;
}

void KdTree::PrintAll() {
    for (const auto &np : nodes_) {
        auto node = np.second;
        if (node->left_ == nullptr && node->right_ == nullptr) {
            LOG(INFO) << "leaf node: " << node->id_ << ", idx: " << node->point_idx_;
        } else {
            LOG(INFO) << "node: " << node->id_ << ", axis: " << node->axis_index_ << ", th: " << node->split_thresh_;
        }
    }
}

        建树过程测试代码：

TEST(CH5_TEST, KDTREE_BASICS) {
    sad::CloudPtr cloud(new sad::PointCloudType);
    sad::PointType p1, p2, p3, p4;
    p1.x = 0;
    p1.y = 0;
    p1.z = 0;

    p2.x = 1;
    p2.y = 0;
    p2.z = 0;

    p3.x = 0;
    p3.y = 1;
    p3.z = 0;

    p4.x = 1;
    p4.y = 1;
    p4.z = 0;

    cloud->points.push_back(p1);
    cloud->points.push_back(p2);
    cloud->points.push_back(p3);
    cloud->points.push_back(p4);

    sad::KdTree kdtree;
    kdtree.BuildTree(cloud);
    kdtree.PrintAll();

    SUCCEED();
}

        K-d树在召回率和准确率上都可以做到100%。

5.2.4四叉树与八叉树

        在二维和三维空间中，分别两类对应的处理方法：四叉树和八叉树。

        四叉树讲解

        八叉树讲解

5.2.5小结

ps:其实就想知道到这里算是入门了吗？