一. 课题背景

机械臂引导：3D视觉引导机械臂方案被越来越多的行业认可，如：快递分拣供包、汽车配件上下料、仓储拆码垛、仓配货品拣选。本课题使用海康机器人3D相机产品获取视野中多个无序摆放物品的抓取点位与抓取顺序，物品种类不限。

二. 开发环境介绍及目标

1.Visual Studio 2019

2.PCL、Boost

3.RGB-D感知相机MV-EB435i

4.SDK_Mv3dRgbd_V1.0.0_Win

本课题在Windows平台进行开发， 整个项目由两个部分组成：使用海康RGB-D感知相机MV-EB435i采集三维点云数据，存储成pcd格式点云文件；通过PCL对存储的点云文件进行简单的点云处理，包括直通滤波器滤波，体素滤波器下采样，统计滤波器滤波，法向量计算，点云分割，最终输出点云簇的质心位置，得到视野中无序物品的抓取点位。

三. 解析点云数据及存储

由于相机通过USB接入，在设备类型选择DeviceType_USB：

ASSERT_OK(MV3D_RGBD_GetDeviceList(DeviceType_USB, &devs[0], nDevNum, &nDevNum));

参考SDK中给出的结构体参数，可以得到图像的详细信息：

typedef struct _MV3D_RGBD_IMAGE_DATA_  
 {  
 Mv3dRgbdImageType enImageType; // 图像格式  
 uint32_t nWidth; // 图像宽  
 uint32_t nHeight; // 图像高  
 uint8_t* pData; // 相机输出的图像数据  
 uint32_t nDataLen; // 图像数据长度(字节)  
 uint32_t nFrameNum; // 帧号，代表第几帧图像  
 int64_t nTimeStamp; // 设备上报的时间戳 （设备上电从0开始，规则详见设备手册）
 uint8_t nReserved[16]; // 保留字节  
 } MV3D_RGBD_IMAGE_DATA;

在相机图像MV3D_RGBD_IMAGE_DATA数据中，有图像宽、图像高、相机输出的图像数据、图像数据长度(字节)、帧号、设备上报的时间戳等信息，通过图像高、宽、图像数据长度可以计算图像点数，以供后续使用。为了得到图像数据，对pData指针进行解析，得到图像数据。

由于相机不输出点云数据，采用深度图数据转换成点云数据：

nRet = MV3D_RGBD_MapDepthToPointCloud(handle, &stFrameData.stImageData[i], &stPointCloudImage);

采用PCL的点云结构存储解析得到的相机点云数据，并对明显的无效数据进行过滤：

调用PCL存文件接口，将点云文件存储成pcd格式：

pcl::io::savePCDFileASCII(to_string(num)+".pcd", pointCloud);

四. 点云数据处理及物体位置输出

创建点云数据集，读取点云到pcl的点云结构体中：

pcl::PCDReader reader;
reader.read("1.pcd", *cloud);

对点云进行显示。为了同时显示原始点云，直通滤波器滤波，体素滤波器下采样，统计滤波器滤波，法向量计算，点云分割的结果，建立了三个PCLVisualizer渲染窗口，并将其中一个窗口分成四格同时显示不同滤波器的效果：

//第一个窗口左上角显示内容进行设定
int v1(0);
viewer->createViewPort(0.0, 0.5, 0.5, 1.0, v1);
viewer->setBackgroundColor(0, 0, 0, v1);
viewer->addText("sample cloud", 10, 10, "sample cloud", v1);
viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud", v1);

//第一个窗口右上角显示内容进行设定
int v2(0);
viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v2);
viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
viewer->addText("cloud after PassThrough", 10, 10, "cloud after PassThrough", v2);
viewer->addPointCloud<pcl::PointXYZ>(cloud_after_PassThrough, "cloud_after_PassThrough", v2);

//第一个窗口左下角显示内容进行设定
int v3(0);
viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v3);
viewer->setBackgroundColor(0.3, 0.3, 0.3, v3);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
viewer->addText("cloud after voxelgrid", 10, 10, "cloud after voxelgrid", v3);
viewer->addPointCloud<pcl::PointXYZ>(cloud_after_voxelgrid, "cloud_after_voxelgrid", v3);

//第一个窗口右下角显示内容进行设定
int v4(0);
viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v4);
viewer->setBackgroundColor(0.3, 0.3, 0.3, v4);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
viewer->addText("cloud after StatisticalRemoval", 10, 10, "cloud after StatisticalRemoval", v4);
viewer->addPointCloud<pcl::PointXYZ>(cloud_after_StatisticalRemoval, "cloud_after_StatisticalRemoval", v4);

对点云进行滤波。通过直通滤波器滤波过滤z方向上过近或者过远的点云，通过体素滤波器下采样降低点云稠密度，通过统计滤波器滤波过滤离群点。通过上述流水线的串行点云滤波操作，得到更接近期望的点云：

/*直通滤波器对点云进行处理。*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> passthrough;
passthrough.setInputCloud(cloud);//输入点云
passthrough.setFilterFieldName("z");//对z轴进行操作
passthrough.setFilterLimits(500.0, 3000.0);//设置直通滤波器操作范围
//passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
passthrough.filter(*cloud_after_PassThrough);

/*体素滤波器实现下采样*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_voxelgrid(new pcl::PointCloud<pcl::PointXYZ>);//
pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
voxelgrid.setInputCloud(cloud_after_PassThrough);//输入点云数据
voxelgrid.setLeafSize(10.0f, 10.0f, 10.0f);//AABB长宽高
voxelgrid.filter(*cloud_after_voxelgrid);

/*统计滤波器滤波*/
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_StatisticalRemoval(new pcl::PointCloud<pcl::PointXYZ>);//
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> Statistical;
Statistical.setInputCloud(cloud_after_voxelgrid);
Statistical.setMeanK(100);//取平均值的临近点数
Statistical.setStddevMulThresh(0.15);// 设置标准差阈值系数
//Statistical.setStddevMulThresh(100);//临近点数数目少于多少时会被舍弃
Statistical.filter(*cloud_after_StatisticalRemoval);

对滤波后的点云进行法向量计算并显示法向量和点云：

/*法向量计算*/
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud(cloud_after_StatisticalRemoval);
normalEstimation.setRadiusSearch(100);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
normalEstimation.setSearchMethod(kdtree);
normalEstimation.compute(*normals);
viewer1->addPointCloud<pcl::PointXYZ>(cloud_after_StatisticalRemoval, "cloud_after_StatisticalRemoval");
viewer1->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_after_StatisticalRemoval, normals, 2, 100, "normals");
viewer1->addCoordinateSystem(1.0);

对点云进行分割。点云分割是根据空间、几何和纹理等特征对点云进行划分，使得同一划分区域内的点云拥有相似的特征 。从点云中提取簇（集群）,并将点云索引保存在 cluster_indices 中，通过设置临近搜索的搜索半径（搜索容差）、每个簇（集群）的最小大小、每个簇（集群）的最大大小、点云搜索算法，进行点云分割。

需要注意的是：设置一个合适的聚类搜索半径 ClusterTolerance，如果搜索半径取一个非常小的值，那么一个实际独立的对象就会被分割为多个聚类；如果将值设置得太高，那么多个对象就会被分割为一个聚类，所以需要进行测试找出最合适的ClusterTolerance。

std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(20.0); // 设置临近搜索的搜索半径（搜索容差）
ec.setMinClusterSize(2000);    // 每个簇（集群）的最小大小
ec.setMaxClusterSize(250000);  // 每个簇（集群）的最大大小
ec.setSearchMethod(tree);     // 设置点云搜索算法
ec.setInputCloud(cloud_after_StatisticalRemoval);   // 设置输入点云
ec.extract(cluster_indices);   // 设置提取到的簇

输出点云簇质心位置。通过compute3DCentroid计算分割后点云簇的质心，并显示在点云图像中：

Eigen::Vector4f centroid;  //质心
pcl::compute3DCentroid(*cloud_cluster, centroid); // 计算质心
std::cout << "Cluster x y z: " << centroid(0) << " " << centroid(1) << " " << centroid(2) << std::endl;

pcl::PointXYZ center;
center.x = centroid(0);
center.y = centroid(1);
center.z = centroid(2);
viewer2->addSphere(center, 20, 255, 0, 0, "sphere"+to_string(j));

五. 代码

1、解析点云数据及存储：

main.cpp:

#include <iostream>
#include <fstream>
#include <algorithm>
#include <stdio.h>
#include "../common/common.hpp"
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

using namespace std;

int main()
{
    ASSERT_OK(MV3D_RGBD_Initialize());

    unsigned int nDevNum = 0;
    ASSERT_OK(MV3D_RGBD_GetDeviceNumber(DeviceType_USB, &nDevNum));
    LOGD("MV3D_RGBD_GetDeviceNumber success! nDevNum:%d.", nDevNum);
    ASSERT(nDevNum);


    // 查找设备
    std::vector<MV3D_RGBD_DEVICE_INFO> devs(nDevNum);
    ASSERT_OK(MV3D_RGBD_GetDeviceList(DeviceType_USB, &devs[0], nDevNum, &nDevNum));
    for (unsigned int i = 0; i < nDevNum; i++)
    {
        LOG("Index[%d]. SerialNum[%s] IP[%s] name[%s].\r\n", i, devs[i].chSerialNumber, devs[i].SpecialInfo.stNetInfo.chCurrentIp, devs[i].chModelName);
    }

    //打开设备
    void* handle = NULL;
    unsigned int nIndex = 0;
    ASSERT_OK(MV3D_RGBD_OpenDevice(&handle, &devs[nIndex]));
    LOGD("OpenDevice success.");

    // 开始工作流程
    ASSERT_OK(MV3D_RGBD_Start(handle));
    LOGD("Start work success.");

    int num = 0;
    bool exit_flag = TRUE;

    MV3D_RGBD_FRAME_DATA stFrameData = { 0 };
    while (exit_flag)
    {
        pcl::PointCloud<pcl::PointXYZ> pointCloud;

        // 获取图像数据
        int nRet = MV3D_RGBD_FetchFrame(handle, &stFrameData, 5000);
        //获取图像帧
        if (MV3D_RGBD_OK == nRet)
        {
            //图像帧很多类型，当类型为深度时将其转换为点云
            for (int i = 0; i < stFrameData.nImageCount; i++)
            {
                //若是深度格式则开始转换
                if (ImageType_Depth == stFrameData.stImageData[i].enImageType)
                {
                    MV3D_RGBD_IMAGE_DATA stPointCloudImage;
                    //将当前帧转换为点云
                    nRet = MV3D_RGBD_MapDepthToPointCloud(handle, &stFrameData.stImageData[i], &stPointCloudImage);
                    //pPtr 指向 点云存储起始地址
                    float* pPtr = (float*)stPointCloudImage.pData;
                    int nPointNum = stPointCloudImage.nDataLen / (sizeof(float) * 3);

                    //从第一个点云开始向文件流中写入 xyz 坐标
                    LOGD("Start write Points of %d framenum!", stPointCloudImage.nFrameNum);
                    for (int nPntIndex = 0; nPntIndex < nPointNum; ++nPntIndex)
                    {
                        if (pPtr[nPntIndex * 3 + 0] > 6000 || pPtr[nPntIndex * 3 + 1] > 6000 || pPtr[nPntIndex * 3 + 2] > 6000)continue;
                        if (pPtr[nPntIndex * 3 + 0] != 0 || pPtr[nPntIndex * 3 + 1] != 0 || pPtr[nPntIndex * 3 + 2] != 0) {
                            pcl::PointXYZ point;
                            point.x = pPtr[nPntIndex * 3 + 0];
                            point.y = pPtr[nPntIndex * 3 + 1];
                            point.z = pPtr[nPntIndex * 3 + 2];
                            pointCloud.points.push_back(point);
                        }
                    }
                    pointCloud.width = pointCloud.points.size();
                    pointCloud.height = 1;

                    pcl::io::savePCDFileASCII(to_string(num)+".pcd", pointCloud);

                    LOGD("_MapDepthToPointCloud() Run Succeed: framenum (%d) height(%d) width(%d)  len (%d)!", stPointCloudImage.nFrameNum,
                        stPointCloudImage.nHeight, stPointCloudImage.nWidth, stPointCloudImage.nDataLen);
                }
                //若获取图像帧失败则退出
                if (MV3D_RGBD_OK != nRet)
                {
                    break;
                }
            }
        }
        //按键退出
        if (_kbhit())
        {
            exit_flag = FALSE;
        }
        num++;
    }

    ASSERT_OK(MV3D_RGBD_Stop(handle));
    ASSERT_OK(MV3D_RGBD_CloseDevice(&handle));
    ASSERT_OK(MV3D_RGBD_Release());

    LOGD("Main done!");

    return 0;
}

CMakeLists.txt:

cmake_minimum_required(VERSION 3.9)

set(CMAKE_CXX_STANDARD 11)

project(storepointscloud)

set(BOOST_ROOT "C:/PCL 1.12.1/3rdParty/Boost")

find_package(Boost REQUIRED
             COMPONENTS system filesystem thread)
find_package(PCL 1.3 REQUIRED)

set(SOURCE_FILES main.cpp)

include_directories(Include/)
include_directories(common/include)

include_directories(
    ${PCL_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    )

link_directories(Lib/Win64)
link_directories(common/lib/Win64)
link_directories(
    ${PCL_LIBRARY_DIRS}
    ${BOOST_LIBRARY_DIRS}
    )

add_definitions(
    ${PCL_DEFINITIONS}
    )
add_compile_definitions(NOMINMAX)

add_executable(storepointscloud ${SOURCE_FILES})

target_link_libraries(storepointscloud 
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES}
    )

target_link_libraries(storepointscloud Mv3dRgbd glfw3 opengl32 glu32)

2、点云数据处理及物体位置输出：

main.cpp:

#include <iostream>
#include <fstream>
#include <algorithm>
#include <stdio.h>

#include<pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include<pcl/filters/passthrough.h>  //直通滤波器头文件
#include<pcl/filters/voxel_grid.h>  //体素滤波器头文件
#include<pcl/filters/statistical_outlier_removal.h>   //统计滤波器头文件
#include <pcl/filters/conditional_removal.h>    //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>   //半径滤波器头文件

#include <pcl/features/normal_3d.h>

#include <pcl/ModelCoefficients.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <Eigen/Core>
#include <pcl/common/transforms.h>


using namespace std;

int main(int argc, char** argv)
{
    vtkObject::GlobalWarningDisplayOff();//禁用vtk警告信息

    ///****************************************************
    /*创建点云数据集。*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::PCDReader reader;
    reader.read("1.pcd", *cloud);    //读取点云到cloud中

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer:pre-process"));
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("3D Viewer:normals"));
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer2(new pcl::visualization::PCLVisualizer("3D Viewer:segmentation"));
    viewer->initCameraParameters();//初始化相机参数

    //第一个窗口显示内容进行设定
    int v1(0);
    viewer->createViewPort(0.0, 0.5, 0.5, 1.0, v1);
    viewer->setBackgroundColor(0, 0, 0, v1);
    viewer->addText("sample cloud", 10, 10, "sample cloud", v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud", v1);
    std::cout << "原始点云数据点数：" << cloud->points.size() << std::endl;

    ///****************************************************
    /*直通滤波器对点云进行处理。*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PassThrough<pcl::PointXYZ> passthrough;
    passthrough.setInputCloud(cloud);//输入点云
    passthrough.setFilterFieldName("z");//对z轴进行操作
    passthrough.setFilterLimits(500.0, 3000.0);//设置直通滤波器操作范围
    //passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
    passthrough.filter(*cloud_after_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThrough

    //第二个窗口显示内容进行设定
    int v2(0);
    viewer->createViewPort(0.5, 0.5, 1.0, 1.0, v2);
    viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
    viewer->addText("cloud after PassThrough", 10, 10, "cloud after PassThrough", v2);
    viewer->addPointCloud<pcl::PointXYZ>(cloud_after_PassThrough, "cloud_after_PassThrough", v2);
    std::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough->points.size() << std::endl;

    ///****************************************************
    /*体素滤波器实现下采样*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_voxelgrid(new pcl::PointCloud<pcl::PointXYZ>);//
    pcl::VoxelGrid<pcl::PointXYZ> voxelgrid;
    voxelgrid.setInputCloud(cloud_after_PassThrough);//输入点云数据
    voxelgrid.setLeafSize(10.0f, 10.0f, 10.0f);//AABB长宽高
    voxelgrid.filter(*cloud_after_voxelgrid);

    //第三个窗口显示内容进行设定
    int v3(0);
    viewer->createViewPort(0.0, 0.0, 0.5, 0.5, v3);
    viewer->setBackgroundColor(0.3, 0.3, 0.3, v3);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
    viewer->addText("cloud after voxelgrid", 10, 10, "cloud after voxelgrid", v3);
    viewer->addPointCloud<pcl::PointXYZ>(cloud_after_voxelgrid, "cloud_after_voxelgrid", v3);
    std::cout << "体素化网格方法后点云数据点数：" << cloud_after_voxelgrid->points.size() << std::endl;

    ///****************************************************
    /*统计滤波器滤波*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_StatisticalRemoval(new pcl::PointCloud<pcl::PointXYZ>);//
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> Statistical;
    Statistical.setInputCloud(cloud_after_voxelgrid);
    Statistical.setMeanK(100);//取平均值的临近点数
    Statistical.setStddevMulThresh(0.15);// 设置标准差阈值系数
    //Statistical.setStddevMulThresh(100);//临近点数数目少于多少时会被舍弃
    Statistical.filter(*cloud_after_StatisticalRemoval);

    //第四个窗口显示内容进行设定
    int v4(0);
    viewer->createViewPort(0.5, 0.0, 1.0, 0.5, v4);
    viewer->setBackgroundColor(0.3, 0.3, 0.3, v4);//设置v2的背景颜色  背景颜色的设置范围被归化到0-1之间
    viewer->addText("cloud after StatisticalRemoval", 10, 10, "cloud after StatisticalRemoval", v4);
    viewer->addPointCloud<pcl::PointXYZ>(cloud_after_StatisticalRemoval, "cloud_after_StatisticalRemoval", v4);
    std::cout << "统计分析滤波后点云数据点数：" << cloud_after_StatisticalRemoval->points.size() << std::endl;


    ///****************************************************
    /*法向量计算*/
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(cloud_after_StatisticalRemoval);
    // For every point, use all neighbors in a radius of 100cm.
    normalEstimation.setRadiusSearch(100);
    // A kd-tree is a data structure that makes searches efficient. More about it later.
    // The normal estimation object will use it to find nearest neighbors.
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
    normalEstimation.setSearchMethod(kdtree);
    normalEstimation.compute(*normals);

    viewer1->addPointCloud<pcl::PointXYZ>(cloud_after_StatisticalRemoval, "cloud_after_StatisticalRemoval");
    // 参数int level=2 表示每n个点绘制一个法向量，参数float scale=100 表示法向量长度缩放为100倍
    viewer1->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud_after_StatisticalRemoval, normals, 2, 100, "normals");
    viewer1->addCoordinateSystem(1.0);


    // 为提取算法的搜索方法创建一个KdTree对象
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud_after_StatisticalRemoval);
    /**
     * 在这里，我们创建一个PointIndices的vector，该vector在vector <int>中包含实际的索引信息。
     * 每个检测到的簇的索引都保存在这里-请注意，cluster_indices是一个vector，包含多个检测到的簇的PointIndices的实例。
     * 因此，cluster_indices[0]包含我们点云中第一个 cluster(簇)的所有索引。
     *
     * 从点云中提取簇（集群）,并将点云索引保存在 cluster_indices 中。
     */
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(20.0); // 设置临近搜索的搜索半径（搜索容差）
    ec.setMinClusterSize(2000);    // 每个簇（集群）的最小大小
    ec.setMaxClusterSize(250000);  // 每个簇（集群）的最大大小
    ec.setSearchMethod(tree);     // 设置点云搜索算法
    ec.setInputCloud(cloud_after_StatisticalRemoval);   // 设置输入点云
    ec.extract(cluster_indices);        // 设置提取到的簇，将每个簇以索引的形式保存到cluster_indices;


    // 为了从点云索引向量中分割出每个簇，必须迭代访问点云索引
    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin();it != cluster_indices.end(); ++it) {

        // 每次创建一个新的点云数据集，并且将所有当前簇的点写入到点云数据集中。
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
        const std::vector<int>& indices = it->indices;

        for (std::vector<int>::const_iterator pit = indices.begin(); pit != indices.end(); ++pit)
            cloud_cluster->points.push_back(cloud_after_StatisticalRemoval->points[*pit]);

        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points."
            << std::endl;

        std::stringstream ss;
        ss << "cloud_cluster_" << j;

        Eigen::Vector4f centroid;  //质心
        pcl::compute3DCentroid(*cloud_cluster, centroid); // 计算质心
        std::cout << "Cluster x y z: " << centroid(0) << " " << centroid(1) << " " << centroid(2) << std::endl;

        pcl::PointXYZ center;
        center.x = centroid(0);
        center.y = centroid(1);
        center.z = centroid(2);
        viewer2->addSphere(center, 20, 255, 0, 0, "sphere"+to_string(j));

        // Generate a random (bright) color
        pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> single_color(cloud_cluster);
        viewer2->addPointCloud<pcl::PointXYZ>(cloud_cluster, single_color, ss.str());
        viewer2->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, ss.str());

        j++;
    }
    std::cout << "cloud size: " << cluster_indices.size() << std::endl;

    while (!viewer->wasStopped()) {
        viewer->spinOnce(1000);
    }

    return (0);

}

CMakeLists.txt:

cmake_minimum_required(VERSION 3.9)

set(CMAKE_CXX_STANDARD 11)

project(pointscloudprocessing)

set(BOOST_ROOT "C:/PCL 1.12.1/3rdParty/Boost")

find_package(Boost REQUIRED
             COMPONENTS system filesystem thread)

#find_package(PCL 1.3 REQUIRED COMPONENTS common io visualization)
find_package(PCL 1.3 REQUIRED)
find_package(VTK REQUIRED)

set(SOURCE_FILES main.cpp)

include_directories(Include/)
include_directories(common/include)

include_directories(
    ${PCL_INCLUDE_DIRS}
    ${Boost_INCLUDE_DIRS}
    ${VTK_INCLUDE_DIRS}
    )

link_directories(Lib/Win64)
link_directories(common/lib/Win64)
link_directories(
    ${PCL_LIBRARY_DIRS}
    ${BOOST_LIBRARY_DIRS}
    ${VTK_LIBRARY_DIRS}
    )

add_definitions(
    ${PCL_DEFINITIONS}
    )
add_compile_definitions(NOMINMAX)

add_executable(pointscloudprocessing ${SOURCE_FILES})

target_link_libraries(pointscloudprocessing 
    ${PCL_LIBRARIES}
    ${Boost_LIBRARIES}
    ${VTK_LIBRARIES}
    )

六. 运行结果

从控制台查看相机参数和图像信息：

存储得到的点云文件：

第一个渲染窗口的原始点云，直通滤波器滤波，体素滤波器下采样，统计滤波器滤波结果：

法向量计算结果：

点云分割结果：

点云质心位置：

七. 总结

本课题采用PCL对海康机器人3D相机产品野中点云进行处理和获取多个无序摆放物品的抓取点位，但是实际拍摄物体种类较少，实验场景单一，整体还有待优化。

代码地址：https://github.com/hahaha1014/HaikangXiling.git