3D相机获取点云代码

以下是使用OpenCV和PCL库获取3D相机点云的C++代码示例：

#include <iostream>
#include <opencv2/opencv.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>

using namespace std;
using namespace cv;

int main()
{
    // 初始化OpenCV窗口
    namedWindow("RGB Image", WINDOW_NORMAL);
    namedWindow("Depth Image", WINDOW_NORMAL);

    // 初始化PCL可视化窗口
    pcl::visualization::PCLVisualizer viewer("Point Cloud Viewer");

    // 创建PCL点云对象
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

    // 创建OpenCV深度图
    Mat depth;

    // 创建OpenCV相机对象
    VideoCapture cap(0);

    // 设置深度图宽度和高度
    cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);

    // 创建PCL统计滤波器对象
    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
    sor.setMeanK(50);
    sor.setStddevMulThresh(1.0);

    while (true)
    {
        // 读取RGB和深度图
        Mat color;
        cap >> color;
        cap.retrieve(depth, CV_CAP_OPENNI_DEPTH_MAP);

        // 将深度图转化为PCL点云对象
        cloud->width = depth.cols;
        cloud->height = depth.rows;
        cloud->is_dense = false;
        cloud->points.resize(cloud->width * cloud->height);

        for (int y = 0; y < depth.rows; y++)
        {
            for (int x = 0; x < depth.cols; x++)
            {
                // 获取深度值
                float d = depth.at<ushort>(y, x);

                if (d == 0)
                {
                    // 如果深度值为0，说明无法获取该点的位置，跳过
                    continue;
                }

                // 计算点云坐标
                pcl::PointXYZRGB& p = cloud->at(x, y);
                p.z = d / 1000.0f;
                p.x = (x - cloud->width / 2) * p.z / 525.0f;
                p.y = (y - cloud->height / 2) * p.z / 525.0f;

                // 获取RGB值
                p.b = color.at<Vec3b>(y, x)[0];
                p.g = color.at<Vec3b>(y, x)[1];
                p.r = color.at<Vec3b>(y, x)[2];
            }
        }

        // 应用统计滤波器
        sor.setInputCloud(cloud);
        sor.filter(*cloud);

        // 显示RGB和深度图
        imshow("RGB Image", color);
        imshow("Depth Image", depth);

        // 显示点云
        viewer.removePointCloud("cloud");
        viewer.addPointCloud(cloud, "cloud");
        viewer.spinOnce();
    }

    return 0;
}

这个C++代码示例中，我们使用OpenCV和PCL库获取3D相机点云。在主循环中，我们不断读取RGB和深度图，并将深度图转化为PCL点云对象。然后，我们使用PCL库中的统计滤波器进行滤波，并在OpenCV和PCL中显示点云。需要注意的是，该代码示例使用了OpenNI驱动程序，因此需要安装OpenNI库。