当前位置：首页 > article >正文

【点云上采样】最近邻插值上采样算法增加点云密度

article 2024/11/18 0:30:22

看了很多文章都是用CGAL去做的，又是下载安装CGAL的贼麻烦，关键弄好还不能用，气死了。

文章目录

前言
一、最近邻插值上采样算法
- 1、原理：
- 2、步骤：
二、完整代码
三、效果对比

前言

传感器采集到的点云比较稀疏，毕竟价位在那，好的太贵，买便宜的点又太稀，需要增加点云数据。

一、最近邻插值上采样算法

1、原理：

最近邻插值上采样算法的核心思想是在每个点与其最近邻点之间插入一个新点。具体来说，对于点云中的每个点 Pi，找到其最近邻点 Pj，然后在这两个点之间插入一个新点 Pnew，使得 Pnew 的坐标是 Pi和Pj坐标的平均值。

2、步骤：

1、读取点云数据：从文件中读取点云数据，将其加载到内存中。

pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
if (pcl::io::loadPCDFile<PointType>("rabbit.pcd", *cloud) == -1) {
    std::cerr << "Error: cannot read file rabbit.pcd" << std::endl;
    return EXIT_FAILURE;
}

2、构建KD树：使用 KD 树（K-Dimensional Tree）来加速最近邻搜索。【KD 树是一种高效的空间分割数据结构，适用于多维空间中的快速最近邻查询。】

pcl::search::KdTree<PointType>::Ptr kdtree(new pcl::search::KdTree<PointType>);
kdtree->setInputCloud(cloud);

3、最近邻搜索：对点云中的每个点Pi ，使用 KD 树查找其最近邻点 Pj 。

for (size_t i = 0; i < cloud->points.size(); ++i) {
    std::vector<int> pointIdxNKNSearch(1);
    std::vector<float> pointNKNSquaredDistance(1);

    // 寻找最近邻点
    if (kdtree->nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
        // 在原始点和其最近邻点之间插入一个点
        PointType newPoint;
        newPoint.x = (cloud->points[i].x + cloud->points[pointIdxNKNSearch[1]].x) / 2.0;
        newPoint.y = (cloud->points[i].y + cloud->points[pointIdxNKNSearch[1]].y) / 2.0;
        newPoint.z = (cloud->points[i].z + cloud->points[pointIdxNKNSearch[1]].z) / 2.0;
        outputCloud->push_back(newPoint);
    }
}

4、插值新点：计算 Pi 和 Pj 之间的中点 Pnew ，并将 Pnew 添加到新的点云中。

PointType newPoint;
newPoint.x = (cloud->points[i].x + cloud->points[pointIdxNKNSearch[1]].x) / 2.0;
newPoint.y = (cloud->points[i].y + cloud->points[pointIdxNKNSearch[1]].y) / 2.0;
newPoint.z = (cloud->points[i].z + cloud->points[pointIdxNKNSearch[1]].z) / 2.0;
outputCloud->push_back(newPoint);

5、合并点云：将插值后的新点云与原始点云合并。

*cloud += *outputCloud;

6、保存点云：将合并后的点云保存到文件中。

if (pcl::io::savePCDFileBinary("output.pcd", *cloud) == -1) {
    std::cerr << "Error: cannot write file output.pcd" << std::endl;
    return EXIT_FAILURE;
}

二、完整代码

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/search/kdtree.h>
#include <iostream>
#include <vector>

// 类型定义
typedef pcl::PointXYZ PointType;

// 最近邻插值函数实现
pcl::PointCloud<PointType>::Ptr nearestNeighborInterpolation(pcl::PointCloud<PointType>::Ptr inputCloud) 
{
    // 创建KdTree对象进行最近邻搜索
    pcl::search::KdTree<PointType>::Ptr kdtree(new pcl::search::KdTree<PointType>);    
    kdtree->setInputCloud(inputCloud);

    // 新点云，用于存储插值后的点
    pcl::PointCloud<PointType>::Ptr outputCloud(new pcl::PointCloud<PointType>);

    // 对每个点进行最近邻搜索并插值
    for (size_t i = 0; i < inputCloud->points.size(); ++i) {
        std::vector<int> pointIdxNKNSearch(1);
        std::vector<float> pointNKNSquaredDistance(1);

        // 寻找最近邻点
        if (kdtree->nearestKSearch(inputCloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) {
            // 在原始点和其最近邻点之间插入一个点
            PointType newPoint;
            newPoint.x = (inputCloud->points[i].x + inputCloud->points[pointIdxNKNSearch[1]].x) / 2.0;
            newPoint.y = (inputCloud->points[i].y + inputCloud->points[pointIdxNKNSearch[1]].y) / 2.0;
            newPoint.z = (inputCloud->points[i].z + inputCloud->points[pointIdxNKNSearch[1]].z) / 2.0;
            outputCloud->push_back(newPoint);
        }
    }

    return outputCloud;
}

int main()
{
    // 输入文件路径
    const std::string input_file = "rabbit.pcd";
    // 输出文件路径
    const std::string output_file = "output.pcd";

    // 读取 PCD 文件
    pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
    if (pcl::io::loadPCDFile<PointType>(input_file, *cloud) == -1) {
        std::cerr << "Error: cannot read file " << input_file << std::endl;
        return EXIT_FAILURE;
    }

    // 进行最近邻插值
    pcl::PointCloud<PointType>::Ptr interpolatedCloud = nearestNeighborInterpolation(cloud);

    // 将插值后的点云合并到原始点云中
    *cloud += *interpolatedCloud;

    // 保存点云为 PCD 文件
    if (pcl::io::savePCDFileBinary(output_file, *cloud) == -1) {
        std::cerr << "Error: cannot write file " << output_file << std::endl;
        return EXIT_FAILURE;
    }

    std::cout << "Interpolation completed and saved to " << output_file << std::endl;

    return EXIT_SUCCESS;
}