Skip to content

Instantly share code, notes, and snippets.

@takahub1
Created January 14, 2017 15:43
Show Gist options
  • Save takahub1/f87cc2080f197a84f71d6e52462099f0 to your computer and use it in GitHub Desktop.
Save takahub1/f87cc2080f197a84f71d6e52462099f0 to your computer and use it in GitHub Desktop.
require libfreenect2 and opencv
/*
-- Georgia Tech 2016 Spring
--
-- This is a sample code to show how to use the libfreenet2 with OpenCV
--
-- The code will streams RGB, IR and Depth images from an Kinect sensor.
-- To use multiple Kinect sensor, simply initial other "listener" and "frames"
-- This code refered from sample code provided from libfreenet2: Protonect.cpp
-- https://github.com/OpenKinect/libfreenect2
-- and another discussion from: http://answers.opencv.org/question/76468/opencvkinect-onekinect-for-windows-v2linuxlibfreenect2/
-- Contact: Chih-Yao Ma at <cyma@gatech.edu>
--Build option
g++ KinectOneStream.cpp -std=c++11 -o out `pkg-config opencv --cflags --libs` `pkg-config freenect2 --cflags --libs`
*/
//! [headers]
#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>
//! [headers]
using namespace std;
using namespace cv;
bool protonect_shutdown = false; // Whether the running application should shut down.
void sigint_handler(int s)
{
protonect_shutdown = true;
}
int main()
{
std::cout << "Streaming from Kinect One sensor!" << std::endl;
//! [context]
libfreenect2::Freenect2 freenect2;
libfreenect2::Freenect2Device *dev = 0;
libfreenect2::PacketPipeline *pipeline = 0;
//! [context]
//! [discovery]
if(freenect2.enumerateDevices() == 0)
{
std::cout << "no device connected!" << std::endl;
return -1;
}
string serial = freenect2.getDefaultDeviceSerialNumber();
std::cout << "SERIAL: " << serial << std::endl;
if(pipeline)
{
//! [open]
dev = freenect2.openDevice(serial, pipeline);
//! [open]
} else {
dev = freenect2.openDevice(serial);
}
if(dev == 0)
{
std::cout << "failure opening device!" << std::endl;
return -1;
}
signal(SIGINT, sigint_handler);
protonect_shutdown = false;
//! [listeners]
libfreenect2::SyncMultiFrameListener listener(libfreenect2::Frame::Color |
libfreenect2::Frame::Depth |
libfreenect2::Frame::Ir);
libfreenect2::FrameMap frames;
dev->setColorFrameListener(&listener);
dev->setIrAndDepthFrameListener(&listener);
//! [listeners]
//! [start]
dev->start();
std::cout << "device serial: " << dev->getSerialNumber() << std::endl;
std::cout << "device firmware: " << dev->getFirmwareVersion() << std::endl;
//! [start]
//! [registration setup]
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); // check here (https://github.com/OpenKinect/libfreenect2/issues/337) and here (https://github.com/OpenKinect/libfreenect2/issues/464) why depth2rgb image should be bigger
//! [registration setup]
Mat rgbmat, depthmat, depthmatUndistorted, irmat, rgbd, rgbd2;
//! [loop start]
while(!protonect_shutdown)
{
listener.waitForNewFrame(frames);
libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
libfreenect2::Frame *ir = frames[libfreenect2::Frame::Ir];
libfreenect2::Frame *depth = frames[libfreenect2::Frame::Depth];
//! [loop start]
cv::Mat(rgb->height, rgb->width, CV_8UC4, rgb->data).copyTo(rgbmat);
cv::Mat(ir->height, ir->width, CV_32FC1, ir->data).copyTo(irmat);
cv::Mat(depth->height, depth->width, CV_32FC1, depth->data).copyTo(depthmat);
cv::imshow("rgb", rgbmat);
cv::imshow("ir", irmat / 4096.0f);
cv::imshow("depth", depthmat / 4096.0f);
//! [registration]
registration->apply(rgb, depth, &undistorted, &registered, true, &depth2rgb);
//! [registration]
cv::Mat(undistorted.height, undistorted.width, CV_32FC1, undistorted.data).copyTo(depthmatUndistorted);
cv::Mat(registered.height, registered.width, CV_8UC4, registered.data).copyTo(rgbd);
cv::Mat(depth2rgb.height, depth2rgb.width, CV_32FC1, depth2rgb.data).copyTo(rgbd2);
cv::imshow("undistorted", depthmatUndistorted / 4096.0f);
cv::imshow("registered", rgbd);
cv::imshow("depth2RGB", rgbd2 / 4096.0f);
int key = cv::waitKey(1);
protonect_shutdown = protonect_shutdown || (key > 0 && ((key & 0xFF) == 27)); // shutdown on escape
//! [loop end]
listener.release(frames);
}
//! [loop end]
//! [stop]
dev->stop();
dev->close();
//! [stop]
delete registration;
std::cout << "Streaming Ends!" << std::endl;
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment