#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;
using namespace libfreenect2;
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()
{
signal(SIGINT, sigint_handler);
protonect_shutdown = false;
//while (!protonect_shutdown);
// 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();
string serial = freenect2.getDeviceSerialNumber(0);
std::cout << "SERIAL " << serial << std::endl;
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
{
pipeline = new libfreenect2::OpenGLPacketPipeline();
//#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();
}
}
// 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 (int 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((int)rgb->height, (int)rgb->width, CV_8UC4, rgb->data);
cv::Mat irmat((int)ir->height, (int)ir->width, CV_32FC1, ir->data);
cv::Mat depthmat((int)depth->height, (int)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;
//}
//getchar();
std::cout << "Goodbye World!" << std::endl;
return 0;
}