机器人视觉仿真软件：PCL (Point Cloud Library)_（9）.三维物体识别与分类

三维物体识别与分类

在机器人视觉领域，三维物体识别与分类是一项核心技术。通过三维点云数据，机器人能够更准确地感知周围环境，识别出物体并对其进行分类。PCL（Point Cloud Library）提供了丰富的算法和工具，使得这项任务变得更加可行和高效。本节将详细介绍如何使用PCL进行三维物体识别与分类，包括点云预处理、特征提取、匹配和分类算法等。

点云预处理

在进行三维物体识别与分类之前，点云数据通常需要进行预处理，以去除噪声、填补空洞、对齐坐标系等。PCL提供了多种预处理方法，如滤波、降采样、配准等。

滤波

滤波是点云预处理中的一种常见方法，用于去除噪声和异常点。PCL支持多种滤波器，如VoxelGrid滤波器、StatisticalOutlierRemoval滤波器等。

VoxelGrid滤波器

VoxelGrid滤波器通过将点云数据划分成体素网格（voxels），然后在每个体素内保留一个代表点，从而实现降采样和噪声去除。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;

    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);

    pcl::PCLPointCloud2 cloud_filtered;



    pcl::io::loadPCDFile ("input_cloud.pcd", *cloud);



    // 创建VoxelGrid滤波器

    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;

    sor.setInputCloud (cloudPtr);

    sor.setLeafSize (0.05f, 0.05f, 0.05f); // 设置体素大小

    sor.filter (cloud_filtered);



    // 保存滤波后的点云

    pcl::io::savePCDFile ("filtered_cloud.pcd", cloud_filtered);



    return 0;

}

StatisticalOutlierRemoval滤波器

StatisticalOutlierRemoval滤波器通过统计方法去除点云中的离群点。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/filters/statistical_outlier_removal.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);



    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // 创建StatisticalOutlierRemoval滤波器

    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;

    sor.setInputCloud (cloud);

    sor.setMeanK (50); // 设置邻域点数

    sor.setStddevMulThresh (1.0); // 设置标准差倍数阈值

    sor.filter (*cloud_filtered);



    // 保存滤波后的点云

    pcl::io::savePCDFile<pcl::PointXYZ> ("filtered_cloud.pcd", *cloud_filtered);



    return 0;

}

特征提取

特征提取是从点云数据中提取能够描述物体的特征，这些特征是后续匹配和分类的基础。PCL提供了多种特征提取方法，如法线估计、FPFH特征、SHOT特征等。

法线估计

法线估计是点云特征提取中的基本步骤，用于计算每个点的表面法线。这些法线信息可以用于后续的特征描述和匹配。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // 创建法线估计对象

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);



    ne.setInputCloud (cloud);

    ne.setRadiusSearch (0.03); // 设置搜索半径



    // 创建KdTree对象

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setSearchMethod (tree);



    // 计算法线

    ne.compute (*cloud_normals);



    // 保存法线估计结果

    pcl::io::savePCDFile<pcl::Normal> ("cloud_normals.pcd", *cloud_normals);



    return 0;

}

FPFH特征

FPFH（Fast Point Feature Histograms）特征是一种高效的局部特征描述子，广泛用于三维物体识别和匹配。FPFH特征通过计算点的局部几何特征来描述点云。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/features/fpfh.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // 计算法线

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud (cloud);

    ne.setRadiusSearch (0.03);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setSearchMethod (tree);

    ne.compute (*cloud_normals);



    // 创建FPFH特征估计对象

    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);



    fpfh.setInputCloud (cloud);

    fpfh.setInputNormals (cloud_normals);

    fpfh.setRadiusSearch (0.05);

    fpfh.setSearchMethod (tree);



    // 计算FPFH特征

    fpfh.compute (*fpfhs);



    // 保存FPFH特征

    pcl::io::savePCDFile<pcl::FPFHSignature33> ("cloud_fpfhs.pcd", *fpfhs);



    return 0;

}

匹配算法

匹配算法用于将提取的特征与已知的特征库进行比较，从而识别出相似的物体。PCL提供了多种匹配算法，如ICP（Iterative Closest Point）、SAC-IA（Sample Consensus Initial Alignment）等。

ICP算法

ICP算法通过迭代最近点方法，将两个点云对齐。对齐后的点云可以用于后续的物体识别和分类。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/registration/icp.h>



int main (int argc, char** argv)

{

    // 读取源点云和目标点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("source_cloud.pcd", *source);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("target_cloud.pcd", *target);



    // 创建ICP对齐对象

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZ>);



    icp.setInputSource (source);

    icp.setInputTarget (target);



    // 执行ICP对齐

    icp.align (*final_cloud);



    // 输出对齐结果

    std::cout << "ICP has converged: " << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl;



    // 保存对齐后的点云

    pcl::io::savePCDFile<pcl::PointXYZ> ("final_cloud.pcd", *final_cloud);



    return 0;

}

SAC-IA算法

SAC-IA算法通过样本一致初始对齐方法，将两个点云对齐。这种方法在点云数据不完全重合的情况下表现较好。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/registration/sac_ia.h>



int main (int argc, char** argv)

{

    // 读取源点云和目标点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr source (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PointCloud<pcl::PointXYZ>::Ptr target (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("source_cloud.pcd", *source);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("target_cloud.pcd", *target);



    // 计算法线

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr source_normals (new pcl::PointCloud<pcl::Normal>);

    pcl::PointCloud<pcl::Normal>::Ptr target_normals (new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud (source);

    ne.setRadiusSearch (0.03);

    ne.setSearchMethod (tree);

    ne.compute (*source_normals);



    ne.setInputCloud (target);

    ne.compute (*target_normals);



    // 创建SAC-IA对齐对象

    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::Normal> sac_ia;

    sac_ia.setInputSource (source);

    sac_ia.setSourceNormals (source_normals);

    sac_ia.setInputTarget (target);

    sac_ia.setTargetNormals (target_normals);



    // 设置参数

    sac_ia.setMinSampleDistance (0.05);

    sac_ia.setCorrespondenceRandomness (5);

    sac_ia.setMaximumIterations (50);



    // 创建结果点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZ>);

    sac_ia.align (*final_cloud);



    // 输出对齐结果

    std::cout << "SAC-IA has converged: " << sac_ia.hasConverged () << " score: " << sac_ia.getFitnessScore () << std::endl;



    // 保存对齐后的点云

    pcl::io::savePCDFile<pcl::PointXYZ> ("final_cloud.pcd", *final_cloud);



    return 0;

}

分类算法

分类算法用于将点云数据中的物体归类到已知的类别中。PCL提供了多种分类算法，如SVM（Support Vector Machine）、KNN（K-Nearest Neighbors）、决策树等。

SVM分类

SVM是一种常用的分类算法，通过支持向量机将点云特征分类到不同的类别中。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/features/fpfh.h>

#include <pcl/ml/svm.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // 计算法线

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud (cloud);

    ne.setRadiusSearch (0.03);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setSearchMethod (tree);

    ne.compute (*cloud_normals);



    // 计算FPFH特征

    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);

    fpfh.setInputCloud (cloud);

    fpfh.setInputNormals (cloud_normals);

    fpfh.setRadiusSearch (0.05);

    fpfh.setSearchMethod (tree);

    fpfh.compute (*fpfhs);



    // 创建SVM分类器

    pcl::ml::SVM<pcl::FPFHSignature33, int> svm;

    svm.train (fpfhs, class_labels); // 假设class_labels是已知的类别标签



    // 预测新点云的类别

    std::vector<int> predicted_labels;

    svm.predict (new_fpfhs, predicted_labels); // 假设new_fpfhs是新的点云特征



    // 输出预测结果

    for (size_t i = 0; i < predicted_labels.size (); ++i)

    {

        std::cout << "Point " << i << " is classified as: " << predicted_labels[i] << std::endl;

    }



    return 0;

}

KNN分类

KNN是一种基于最近邻的分类算法，通过查找最近的K个邻居来确定点云特征的类别。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/features/fpfh.h>

#include <pcl/kdtree/kdtree_flann.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // 计算法线

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud (cloud);

    ne.setRadiusSearch (0.03);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setSearchMethod (tree);

    ne.compute (*cloud_normals);



    // 计算FPFH特征

    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);

    fpfh.setInputCloud (cloud);

    fpfh.setInputNormals (cloud_normals);

    fpfh.setRadiusSearch (0.05);

    fpfh.setSearchMethod (tree);

    fpfh.compute (*fpfhs);



    // 创建KdTree对象

    pcl::KdTreeFLANN<pcl::FPFHSignature33> kdtree;

    kdtree.setInputCloud (fpfhs);



    // 预测新点云的类别

    std::vector<int> indices;

    std::vector<float> distances;

    int K = 3; // 设置邻居数



    for (size_t i = 0; i < new_fpfhs.size (); ++i)

    {

        kdtree.nearestKSearch (new_fpfhs[i], K, indices, distances);



        // 统计最近邻的类别

        std::map<int, int> class_count;

        for (size_t j = 0; j < indices.size (); ++j)

        {

            class_count[class_labels[indices[j]]]++;

        }



        // 选择出现次数最多的类别

        int predicted_label = -1;

        int max_count = 0;

        for (const auto& pair : class_count)

        {

            if (pair.second > max_count)

            {

                max_count = pair.second;

                predicted_label = pair.first;

            }

        }



        std::cout << "Point " << i << " is classified as: " << predicted_label << std::endl;

    }



    return 0;

}

实际应用示例

为了更好地理解三维物体识别与分类的流程，下面是一个完整的示例，展示如何从点云数据中提取特征、进行匹配和分类。

读取和预处理点云

首先，读取点云数据并进行预处理，包括滤波和法线估计。

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/filters/voxel_grid.h>

#include <pcl/features/normal_3d.h>



int main (int argc, char** argv)

{

    // 读取点云数据

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

    pcl::io::loadPCDFile<pcl::PointXYZ> ("input_cloud.pcd", *cloud);



    // VoxelGrid滤波

    pcl::VoxelGrid<pcl::PointXYZ> sor;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

    sor.setInputCloud (cloud);

    sor.setLeafSize (0.05f, 0.05f, 0.05f);

    sor.filter (*cloud_filtered);



    // 计算法线

    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;

    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

    ne.setInputCloud (cloud_filtered);

    ne.setRadiusSearch (0.03);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);

    ne.setSearchMethod (tree);

    ne.compute (*cloud_normals);



    // 保存预处理后的点云和法线

    pcl::io::savePCDFile<pcl::PointXYZ> ("filtered_cloud.pcd", *cloud_filtered);

    pcl::io::savePCDFile<pcl::Normal> ("cloud_normals.pcd", *cloud_normals);



    return 0;

}