利用libfreenect内置KDE算法在kinect2上生成点

• libfreenect内置的KDE算法（参考论文Efficient Multi-Frequency Phase Unwrapping using Kernel Density Estimation）是一种展开多频相位估计飞行时间测距的有效方法。实测效果在近距离使用时，二维图像物体边缘噪声能得到有效抑制，生成点云物体边缘噪声抑制效果不是很明显，但实时渲染效果更加流畅。

将该方法应用于Kinect v2传感器的深度解码，并与Microsoft Kinect SDK和开源驱动程序libfreenect2进行比较。预期的Kinect v2用例是范围小于8m的场景，对于这种情况，我们观察到一致的改进，同时保持实时性能。当深度范围扩展到18.75 m的最大值时，我们得到比libfreenect2更多的52%个有效测量值。效果是，传感器现在可以在大深度场景中使用，以前它不是一个好的选择。

一. 工程配置

在vs上运行本工程（本文所需要的工程为：libfreenect2安装目录下 的build目录下的工程libfreenect2.sln）需要配置：

• cuda

• opencv

• PCL

• opengl

我的运行环境及配置文件版本：

win10 x64, vs2015, opencv3.4.1, PCL1.80, cuda-8.0.44, opengl版本无要求

本文在已经安装好libfreenect2的基础上（如何在win10上安装libfreenect2，点击这里），cuda,opencv,PCL,opengl的详细安装教程网上搜索一大堆，在这里我只记录在安装的过程的一些要点。

• 配置cuda

如果要在vs2015上配置cuda，建议不要安装最新cuda版本（我试装了，有报错，而且该错误可以通过安装低版本cuda来避免），我这里安装cuda-8.0.44最终可用，如何在vs2015中配置cuda参考网上资料。在配置过程中我出现一个问题：MSB3191: Unable to create directory The given path's format is not supported.，该问题解决方法参考该博文。

• 配置opencv

没有发现很难解决的问题，按照网上博文配置即可。

• 配置PCL

我试过很多博文的配置流程总出现各种错误，最好找到一篇好博文（点击这里），需要注意的点有：

1. 安装pcl过程中可能会提示：path too long installer unable to modify path的警告，就目前情况，忽略该警告还没有在后续工程中发现问题。（所以忽略掉往下走...佛系）

2. 下载pcl的注册表时一定要下和pcl完全对应的版本，不然会报错。（参考上面给出的博文，博主贴心的给出所有需要下载的资源链接）

3. 如果想要在Debug 和 Release 模式下都配置附加依赖项，可以在属性管理器中（在“视图”中点击属性管理器（如果没有找到，可以在视图-->其他窗口；中找到）），找到你想要设置的工程，选中在该工程的Debug | x64 和 Release | x64，鼠标右键选择属性，找到附加依赖项，分别设置。（在属性管理器中设置附加依赖项相比于在解决方案资源管理器中设置附加依赖项，可以不用担心Debug 和 Release 的附加依赖项混在一起，而且还删不掉的情况...）

4.配置opengl

貌似没啥需要特别注意的...跟着网上博文走就好

二. 代码实现

- 生成二维图像

在vs2015中打开libfreenect2安装目录下 的build目录下的工程libfreenect2.sln，按照文章第一部分配置好环境，运行项目protonect(需要设置该项目为启动项目：右键protonect-->设置为启动项目)。注意以下部分代码：

    else if(arg == "clkde")
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::OpenCLKdePacketPipeline(deviceId);
#else
      std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }

以及：

    else if(arg == "cudakde")
    {
#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
      if(!pipeline)
        pipeline = new libfreenect2::CudaKdePacketPipeline(deviceId);
#else
      std::cout << "CUDA pipeline is not supported!" << std::endl;
#endif
    }

可见libfreenect2中已经实现了基于opencl的kde算法和基于cuda 的kde 算法，只需要将"cudakde"作为程序入口参数传入即可（至于clkde,可能需要再配置一下opencl）。在解决方案资源管理器中，选中项目"protonect"，右键-->属性，进入"protonect属性页"，点击"调试"-->"命令参数"，将命令参数设置为"cudakde"，点击保存，运行项目即可。

- 生成点云

代码如下：point_cloud.cpp：

#include <iostream>
#include <stdio.h>
#include <iomanip>
#include <time.h>
#include <signal.h>
#include <opencv2/opencv.hpp>
#include <math.h>

#include <libfreenect2/libfreenect2.hpp>
#include <libfreenect2/frame_listener_impl.h>
#include <libfreenect2/registration.h>
#include <libfreenect2/packet_pipeline.h>
#include <libfreenect2/logger.h>


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

using namespace std;
using namespace cv;


typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;



enum
{
    Processor_cl,
    Processor_gl,
    Processor_cpu,
    Processor_clkde,
    Processor_cudakde
};

bool protonect_shutdown = false; // Whether the running application should shut down.

void sigint_handler(int s)
{
    protonect_shutdown = true;
}

int main()
{

    //定义变量
    std::cout << "start!" << std::endl;
    libfreenect2::Freenect2 freenect2;
    libfreenect2::Freenect2Device *dev = 0;
    libfreenect2::PacketPipeline  *pipeline = 0;



    //搜寻并初始化传感器
    if (freenect2.enumerateDevices() == 0)
    {
        std::cout << "no device connected!" << std::endl;
        return -1;
    }
    string serial = freenect2.getDefaultDeviceSerialNumber();
    std::cout << "SERIAL: " << serial << std::endl;

    //配置传输格式
#if 1 // sean
    int depthProcessor = Processor_cudakde;
    if (depthProcessor == Processor_cpu)
    {
        if (!pipeline)
            //! [pipeline]
            pipeline = new libfreenect2::CpuPacketPipeline();
        //! [pipeline]
    }
    else if (depthProcessor == Processor_gl) // if support gl
    {
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
        if (!pipeline)
        {
            pipeline = new libfreenect2::OpenGLPacketPipeline();
        }
#else
        std::cout << "OpenGL pipeline is not supported!" << std::endl;
#endif
    }
    else if (depthProcessor == Processor_cl) // if support cl
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
        if (!pipeline)
            pipeline = new libfreenect2::OpenCLPacketPipeline();
#else
        std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if (depthProcessor == Processor_clkde) // if support gl
    {
#ifdef LIBFREENECT2_WITH_OPENCL_SUPPORT
        if (!pipeline)
        {
            pipeline = new libfreenect2::OpenCLKdePacketPipeline();
            cout << "clkde is avariable" << endl;

        }
#else
        std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
    }
    else if (depthProcessor == Processor_cudakde) // if support gl
    {
#ifdef LIBFREENECT2_WITH_CUDA_SUPPORT
        if (!pipeline)
        {
            pipeline = new libfreenect2::CudaKdePacketPipeline();
            cout << "cudakde is avariable" << endl;
        }
#else
        std::cout << "Cudaked pipeline is not supported!" << std::endl;
#endif
    }

    

    //启动设备
    if (pipeline)
    {
        dev = freenect2.openDevice(serial, pipeline);
    }
    else
    {
        dev = freenect2.openDevice(serial);
    }
    if (dev == 0)
    {
        std::cout << "failure opening device!" << std::endl;
        return -1;
    }
    signal(SIGINT, sigint_handler);
    protonect_shutdown = false;
    libfreenect2::SyncMultiFrameListener listener(
        libfreenect2::Frame::Color |
        libfreenect2::Frame::Depth |
        libfreenect2::Frame::Ir);
    libfreenect2::FrameMap frames;
    dev->setColorFrameListener(&listener);
    dev->setIrAndDepthFrameListener(&listener);


    //启动数据传输
    dev->start();

    std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
    std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;




    //循环接收
    libfreenect2::Registration* registration = new libfreenect2::Registration(dev->getIrCameraParams(), dev->getColorCameraParams());
    libfreenect2::Frame undistorted(512, 424, 4), registered(512, 424, 4), depth2rgb(1920, 1080 + 2, 4);


    Mat rgbmat, depthmat, rgbd, dst;
    float x, y, z, color;

    pcl::visualization::CloudViewer viewer("Viewer");  //创建一个显示点云的窗口



    while (!protonect_shutdown)
    {
        listener.waitForNewFrame(frames);
        libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
        libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
        registration->apply(rgb, depth, &undistorted, &registered, true, &depth2rgb);

        PointCloud::Ptr cloud(new PointCloud); //使用智能指针，创建一个空点云。这种指针用完会自动释放。
        for (int m = 0; m < 512; m++)
        {
            for (int n = 0; n < 424; n++)
            {
                PointT p;
                registration->getPointXYZRGB(&undistorted, &registered, n, m, x, y, z, color);
                const uint8_t *c = reinterpret_cast<uint8_t*>(&color);
                uint8_t b = c[0];
                uint8_t g = c[1];
                uint8_t r = c[2];
                if (z<1.2 && y<0.2)  
                {
                    p.z = -z;
                    p.x = x;
                    p.y = -y;
                    p.b = b;
                    p.g = g;
                    p.r = r;
                }
                cloud->points.push_back(p);
            }
        }
        viewer.showCloud(cloud);
        int key = cv::waitKey(1);
        protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
        listener.release(frames);
    }

    dev->stop();
    dev->close();

    delete registration;

#endif

    std::cout << "stop!" << std::endl;
    return 0;
}

可以直接用“point_cloud.cpp”直接替换掉项目"Protonect"中的“Protonect.cpp”，这样可以不用再次配置环境，或者用Cmake生成VS项目再在vs2015中运行。代码不是我写的，仅作少量修改。代码来源点击这里，有关libfreenect2 API用法，点击这里。

最终效果图: 二维图像

三维点云