linux上使用libfreenect2从kinect2获取数据

前言

• kinect2，没错就是微软的那个摄像头。如下图：
  
• libfreenect2，是kinect2的开源驱动。github的这个地址有他的详细介绍和安装步骤，有windows的，linux的，mac的
• 本篇博客，主要介绍利用libfreenect2驱动从kinect2获取数据。

准备

1. 安装cmake

$ sudo apt-get install build-essential cmake

1. 安装libfreenect2，教程地址
2. 安装opencv，教程地址

获取数据

下面将给出c++获取数据的源码，是一个简单的程序。在引入相应头文件后，声明了初始化函数和获取数据函数，之后在main函数中进行测试。

在这里我使用的是libfreenect2自带的kinect2标定参数
我们只获取了红外数据和深度数据，都是以cv::Mat存储的

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

#include <pcl/pcl_config.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <map>

/**
 * Get the value of a specified key from a std::map.
 * @param m the map
 * @param key the key
 * @param defaultValue the default value used if the key is not found
 * @return the value
 */
template<class K, class V>
inline V uValue(const std::map<K, V> & m, const K & key, const V & defaultValue = V())
{
    V v = defaultValue;
    typename std::map<K, V>::const_iterator i = m.find(key);
    if(i != m.end())
    {
        v = i->second;
    }
    return v;
}

/********** global variable begin ************/
int deviceId_;
libfreenect2::Freenect2* freenect2_;
libfreenect2::SyncMultiFrameListener* listener_;
libfreenect2::Freenect2Device* dev_;
libfreenect2::Registration * reg_;
//StereoCameraModel stereoModel_;
/********** global variable end ************/

bool init()
{
    libfreenect2::PacketPipeline* pipeline;
    pipeline = new libfreenect2::CpuPacketPipeline();

    std::cout << ">>> opening default device ..." << std::endl;
    dev_ = freenect2_->openDefaultDevice(pipeline);
    pipeline = 0;

    if(dev_)
    {
        std::cout << ">>> Had got default device " << std::endl;
        libfreenect2::Freenect2Device::Config config;
        config.EnableBilateralFilter = true;
        config.EnableEdgeAwareFilter = true;
        config.MinDepth = 0.3f;
        config.MaxDepth = 12.0f;
        dev_->setConfiguration(config);

        dev_->setColorFrameListener(listener_);
        dev_->setIrAndDepthFrameListener(listener_);

        dev_->start();

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

        //default registration params
        libfreenect2::Freenect2Device::IrCameraParams depthParams = dev_->getIrCameraParams();
        libfreenect2::Freenect2Device::ColorCameraParams colorParams = dev_->getColorCameraParams();
        reg_ = new libfreenect2::Registration(depthParams, colorParams);

        // std::string calibrationFolder = "/home/turtlebot/My_project/freenect2Learn_test/config/";
        // std::string calibrationName = dev_->getSerialNumber();
        // if(!stereoModel_.load(calibrationFolder, calibrationName, false))
        // {
        //  std::cout << "*** Missing calibration files for camera "<< calibrationName <<" in "<< calibrationFolder <<" folder, default calibration used.";
        // }

    }

    return true;
}

void captureImage()
{
    if(dev_ && listener_)
    {
        libfreenect2::FrameMap frames;

        if(!listener_->waitForNewFrame(frames,1000))
        {
            std::cout << "*** CameraFreenect2: Failed to get frames!" << std::endl;
        }
        else
        {
            libfreenect2::Frame* rgbFrame = 0;
            libfreenect2::Frame* irFrame = 0;
            libfreenect2::Frame* depthFrame = 0;

            irFrame = uValue(frames, libfreenect2::Frame::Ir, (libfreenect2::Frame*)0);
            depthFrame = uValue(frames, libfreenect2::Frame::Depth, (libfreenect2::Frame*)0);

            cv::Mat ir,depth;
            float fx = 0, fy = 0, cx = 0, cy = 0;

            if(irFrame && depthFrame)
            {
                cv::Mat irMat((int)irFrame->height, (int)irFrame->width, CV_32FC1, irFrame->data);
                //convert to gray scaled
                float maxIr_ = 0x7FFF;
                float minIr_ = 0x0;
                const float factor = 255.0f / float((maxIr_ - minIr_));
                ir = cv::Mat(irMat.rows, irMat.cols, CV_8UC1);
                for(int i=0; i<irMat.rows; ++i)
                {
                    for(int j=0; j<irMat.cols; ++j)
                    {
                        ir.at<unsigned char>(i, j) = (unsigned char)std::min(float(std::max(irMat.at<float>(i,j) - minIr_, 0.0f)) * factor, 255.0f);
                    }
                }

                cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);

                //void flip(InputArray src, OutputArray dst, int flipCode); 参数fipCode: 整数，水平发转；0垂直反转；负数，水平垂直均反转。
                cv::flip(ir, ir, 1);
                cv::flip(depth, depth, 1);

                libfreenect2::Freenect2Device::IrCameraParams params = dev_->getIrCameraParams();
                fx = params.fx;
                fy = params.fy;
                cx = params.cx;
                cy = params.cy;

                /////////////////////////////////////////////////////////////////////
                // show show ir and depth data
                // std::cout << "depth rows = " << depth.rows << std::endl;
                // std::cout << "depth cols = " << depth.cols << std::endl;
                // for (int row = 0; row < depth.rows; row++)
                // {
                //  float* ptr = (float*)(depth.data + row * depth.step); //第row行数据的起始指针

                //  for (int col = 0; col < depth.cols; col++)
                //  {
                //      std::cout << *ptr++ << " ";
                //  }
                //  std::cout << std::endl;
                // }
                /////////////////////////////////////////////////////////////////////

                /////////////////////////////////////////////////////////////////////
                // std::cout << "ir rows = " << ir.rows << std::endl;
                // std::cout << "ir cols = " << ir.cols << std::endl;
                // for (int row = 0; row < ir.rows; row++)
                // {
                //  float* ptr = (float*)(ir.data + row * ir.step); //第row行数据的起始指针

                //  for (int col = 0; col < ir.cols; col++)
                //  {
                //      std::cout << *ptr++ << " ";
                //  }
                //  std::cout << std::endl;
                // }
                /////////////////////////////////////////////////////////////////////

                /////////////////////////////////////////////////////////////////////
                // you should create new thread to do it;
                // // show ir and depth image
                // if (depth.empty())
                // {
                //  namedWindow("can not show image");
                //  waitKey();
                //  return;
                // }
                // //creat image windows named "My Image"
                // namedWindow("Depth image");
                // //show the image on window
                // imshow("Depth image",depth);
                // //wait key for 5000ms
                // waitKey(5000);
                /////////////////////////////////////////////////////////////////////
            }

            //data = SensorData(ir, depth, model, this->getNextSeqID(), stamp);

            listener_->release(frames);
        }
    }
}

int main(int argc, char const *argv[])
{


    freenect2_ = new libfreenect2::Freenect2();
    //listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Color | libfreenect2::Frame::Ir);
    listener_ = new libfreenect2::SyncMultiFrameListener(libfreenect2::Frame::Ir | libfreenect2::Frame::Depth);

    /********** initial begin **********/
    init();
    /********* initial end *************/
    captureImage();


    /********* destroy begin *************/
    delete freenect2_;
    delete listener_;
    /********* destroy end ***************/
    return 0;
}

CMakeLists

使用cmake工具来生成makefile，cmake的简单说明在这里。这里需要注意的就是要声明libfreenect2的include和lib路径，因为在安装libfreenect2的时候的默认安装路径在home。

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project("My Project")

set(CMAKE_CXX_FLAGS "-std=c++11")

# Set cmake prefix path to enable cmake to find freenect2
set(CMAKE_PREFIX_PATH ${CMAKE_PREFIX_PATH} $ENV{HOME}/freenect2/lib/cmake/freenect2)
find_package(freenect2 REQUIRED)
find_package(PCL 1.7 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories("/usr/include/libusb-1.0/")

INCLUDE_DIRECTORIES(
  ${freenect2_INCLUDE_DIR}
  ${PCL_INCLUDE_DIRS}
)

link_directories(
  ${PCL_LIBRARY_DIRS}
)

add_definitions(
  ${PCL_DEFINITIONS}
)

add_executable(main ./main.cpp)
target_link_libraries(main ${freenect2_LIBRARIES} ${OpenCV_LIBS})

NOTE:
在编译程序时，如果在链接提示说未定义的错误，很有可能就是在target_link_libraries中没有链接相应的库

总结

为什么要写这个获取数据的程序？是因为我想跑kinfu（pcl团队实现的开源kinectfusion），但是发现它使用的openni的第一个版本，而我使用的kinect2，在我电脑上试的结果是：使用openni无法获取数据，使用openni2可以获取数据。但是问题是，不再代码中么有找到怎么将其修改为使用openni2来获取数据。最后想的办法就是使用libfreenect2获取数据替换kinfu中原先使用openni获取数据的部分。在编写这个小程序中也遇到了很多问题，在问题出现的时候一定的静下心来细细检查源码和查看日志，也可以上网查找，当然也要想想之前自己的经验可不可以使用（我主要参考的是rtabmap）