librfreenect2

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

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

using namespace std;
using namespace cv;

enum
{
	Processor_cl,
	Processor_gl,
	Processor_cpu
};

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

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

int main()
{
	// Define variables
	std::cout << "Hello World!" << std::endl;
	libfreenect2::Freenect2 freenect2;
	std::vector<libfreenect2::Freenect2Device*> devices;
	std::vector<libfreenect2::PacketPipeline*> pipelines;
	std::vector<libfreenect2::SyncMultiFrameListener*> listeners;
	std::vector<libfreenect2::FrameMap> frames;

	// Search and initialize sensors
	int numDevices = freenect2.enumerateDevices();
	if (numDevices < 2)
	{
		std::cerr << "Error: Not enough Kinect devices connected!" << std::endl;
		return -1;
	}

	// Configure transfer format
	int depthProcessor = Processor_cl;
	for (int i = 0; i < 2; ++i)
	{
		string serial = freenect2.getDeviceSerialNumber(i);
		std::cout << "SERIAL " << i + 1 << ": " << serial << std::endl;

		libfreenect2::PacketPipeline* pipeline = nullptr;
		if (depthProcessor == Processor_cpu)
		{
			pipeline = new libfreenect2::CpuPacketPipeline();
		}
		else if (depthProcessor == Processor_gl) // if support gl
		{
#ifdef LIBFREENECT2_WITH_OPENGL_SUPPORT
			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
			pipeline = new libfreenect2::OpenCLPacketPipeline();
#else
			std::cout << "OpenCL pipeline is not supported!" << std::endl;
#endif
		}

		if (pipeline)
		{
			libfreenect2::Freenect2Device* dev = freenect2.openDevice(serial, pipeline);
			if (dev == nullptr)
			{
				std::cerr << "Failure opening device " << i + 1 << "!" << std::endl;
				return -1;
			}
			devices.push_back(dev);
			pipelines.push_back(pipeline);

			libfreenect2::SyncMultiFrameListener* listener = new libfreenect2::SyncMultiFrameListener(
				libfreenect2::Frame::Color |
				libfreenect2::Frame::Depth |
				libfreenect2::Frame::Ir);
			listeners.push_back(listener);
			frames.push_back(libfreenect2::FrameMap());

			dev->setColorFrameListener(listener);
			dev->setIrAndDepthFrameListener(listener);
			dev->start();
		}
	}

	signal(SIGINT, sigint_handler);
	protonect_shutdown = false;

	// Create windows
	cv::namedWindow("rgb1", WND_PROP_ASPECT_RATIO);
	cv::namedWindow("rgb2", WND_PROP_ASPECT_RATIO);
	cv::namedWindow("ir1", WND_PROP_ASPECT_RATIO);
	cv::namedWindow("ir2", WND_PROP_ASPECT_RATIO);
	cv::namedWindow("depth1", WND_PROP_ASPECT_RATIO);
	cv::namedWindow("depth2", WND_PROP_ASPECT_RATIO);

	// Loop to receive frames
	while (!protonect_shutdown)
	{
		for (size_t i = 0; i < devices.size(); ++i)
		{
			listeners[i]->waitForNewFrame(frames[i]);
			libfreenect2::Frame* rgb = frames[i][libfreenect2::Frame::Color];
			libfreenect2::Frame* ir = frames[i][libfreenect2::Frame::Ir];
			libfreenect2::Frame* depth = frames[i][libfreenect2::Frame::Depth];

			cv::Mat rgbmat(rgb->height, rgb->width, CV_8UC4, rgb->data);
			cv::Mat irmat(ir->height, ir->width, CV_32FC1, ir->data);
			cv::Mat depthmat(depth->height, depth->width, CV_32FC1, depth->data);

			cv::imshow("rgb" + std::to_string(i + 1), rgbmat);
			cv::imshow("ir" + std::to_string(i + 1), irmat / 4500.0f);
			cv::imshow("depth" + std::to_string(i + 1), depthmat / 4500.0f);

			listeners[i]->release(frames[i]);
		}

		int key = cv::waitKey(1);
		protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
	}

	// Close devices
	for (auto dev : devices)
	{
		dev->stop();
		dev->close();
	}

	// Clean up resources
	for (auto pipeline : pipelines)
	{
		delete pipeline;
	}
	for (auto listener : listeners)
	{
		delete listener;
	}

	std::cout << "Goodbye World!" << std::endl;
	return 0;
}