diff --git a/Makefile b/Makefile index 2cbf0d2..578b9bd 100644 --- a/Makefile +++ b/Makefile @@ -1,18 +1,92 @@ -CXX = nvcc -CC = nvcc -CPPFLAGS=-I../include -I../include/libfreenect -I/usr/loca/cuda/include -I/opt/local/include -DLIBFREENECT_INTERFACE -CXXFLAGS=-g -m64 -O3 -use_fast_math -## -ptx -src-in-ptx -LDFLAGS=-g -m64 -L../lib -lfreenect -Xlinker -framework,OpenGL,-framework,GLUT +# When editing this Makefile, please make sure all variables apart from +# the KF_* and ALL_* variables stay overridable from outside. +# KF_* variables *may* be set from outside (like KF_DRIVER). -%.o: %.cu - $(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $^ +CC ?= gcc +CXX ?= g++ +NVCC ?= nvcc +# Can be either 'libfreenect' or 'openni2'. +# If not given, it's 'libfreenect' by default. +KF_DRIVER ?= libfreenect + +# You can set the following path variables for this Makefile: +# CUDA_INCLUDE_PATH +# OPENNI2_INCLUDE_PATH +# LIBFREENECT_INCLUDE_PATH + + +# Default flags +KF_CPPFLAGS = -Ithirdparty +KF_CXXFLAGS = -g -m64 -O3 +KF_NVCCFLAGS = -g -m64 -O3 -use_fast_math +KF_LDFLAGS = -g -m64 -lpthread -Xlinker -lcudart + +ifdef CUDA_INCLUDE_PATH + KF_CPPFLAGS += -I$(CUDA_INCLUDE_PATH) +endif + +# Driver dependent flags + +# For using libfreenect +ifeq ($(KF_DRIVER),libfreenect) + KF_CPPFLAGS += -DLIBFREENECT_INTERFACE + ifdef LIBFREENECT_INCLUDE_PATH + KF_CPPFLAGS += -I$(LIBFREENECT_INCLUDE_PATH) + endif + KF_LDFLAGS += -lfreenect +# For using OpenNI2 +else ifeq ($(KF_DRIVER),openni2) + KF_CPPFLAGS += -DOPENNI2_INTERFACE + ifdef OPENNI2_INCLUDE_PATH + KF_CPPFLAGS += -I$(OPENNI2_INCLUDE_PATH) + endif + KF_LDFLAGS += -lOpenNI2 +else + $(error KF_DRIVER is not set to a possible driver) +endif + +# OS dependent flags + +KF_OS := $(shell uname) +ifeq ($(KF_OS),Darwin) + KF_LDFLAGS += -framework,OpenGL,-framework,GLUT +else + KF_LDFLAGS += -lGL -lglut +endif + + +# Concatenate our and user flags +ALL_CPPFLAGS := $(KF_CPPFLAGS) $(CPPFLAGS) +ALL_CXXFLAGS := $(KF_CXXFLAGS) $(CXXFLAGS) +ALL_NVCCFLAGS := $(KF_NVCCFLAGS) $(NVCCFLAGS) +ALL_LDFLAGS := $(KF_LDFLAGS) $(LDFLAGS) + + +# Default target +.PHONY: all all: kinect test -test: kfusion.o helpers.o test.o -kinect: kfusion.o helpers.o kinect.o interface.o +# .cpp files +%.o: %.cpp + $(CXX) $(ALL_CXXFLAGS) $(ALL_CPPFLAGS) -c -o $@ $< + +# .cu files +%.cu_o: %.cu + $(NVCC) $(ALL_NVCCFLAGS) $(ALL_CPPFLAGS) -c -o $@ $< + + +# Executables + +kinect: kinect.cpp kfusion.cu_o helpers.cu_o interface.o + $(CXX) $(ALL_CPPFLAGS) $(ALL_CXXFLAGS) $^ -o $@ $(ALL_LDFLAGS) + +test: test.cpp kfusion.cu_o helpers.cu_o + $(CXX) $(ALL_CPPFLAGS) $(ALL_CXXFLAGS) $^ -o $@ $(ALL_LDFLAGS) + +# Cleanup +.PHONY: clean clean: - rm *.o test kinect + rm -f *.cu_o *.o test kinect diff --git a/README.md b/README.md index d53a6dc..c8b3223 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,9 @@ KFusion is mainly written in CUDA with some interface code to display graphics o Requirements ------------ +You need a depth camera: Either a Microsoft Kinect or any camera supported +by OpenNI 2 (such as the Asus Xtion Pro Live). + KFusion depends on the following libraries: * http://www.edwardrosten.com/cvd/toon.html @@ -25,9 +28,10 @@ On Windows use the MS Kinect SDK: * http://www.microsoft.com/en-us/kinectforwindows/develop/overview.aspx -while on other platforms use libfreenect: +while on other platforms use either: -* http://openkinect.org/ +* libfreenect: http://openkinect.org or +* OpenNI: https://github.com/OpenNI/OpenNI2 and of course the CUDA 5 SDK by NVidia diff --git a/interface.cpp b/interface.cpp index db715b7..4d8a5c8 100644 --- a/interface.cpp +++ b/interface.cpp @@ -139,7 +139,20 @@ DWORD WINAPI run(LPVOID pParam) return (0); } -int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer ){ +int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer, const string & replay_path, const string & simultaneous_recording_path ){ + + if (replay_path != "") + { + cout << "Replaying a recording is not supported with MS_KINECT_INTERFACE!" << endl; + return 1; + } + + if (simultaneous_recording_path != "") + { + cout << "Simultaneous recording is not supported with MS_KINECT_INTERFACE!" << endl; + return 1; + } + buffers[0] = depth_buffer[0]; buffers[1] = depth_buffer[1]; rgb = rgb_buffer; @@ -266,7 +279,20 @@ void *freenect_threadfunc(void *arg) return NULL; } -int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer ){ +int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer, const string & replay_path, const string & simultaneous_recording_path ){ + + if (replay_path != "") + { + cout << "Replaying a recording is not supported with LIBFREENECT_INTERFACE!" << endl; + return 1; + } + + if (simultaneous_recording_path != "") + { + cout << "Simultaneous recording is not supported with LIBFREENECT_INTERFACE!" << endl; + return 1; + } + if (freenect_init(&f_ctx, NULL) < 0) { cout << "freenect_init() failed" << endl; return 1; @@ -276,8 +302,10 @@ int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer ){ freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA)); int nr_devices = freenect_num_devices (f_ctx); - if (nr_devices < 1) + if (nr_devices < 1){ + cout << "libfreenect: No devices found" << endl; return 1; + } if (freenect_open_device(f_ctx, &f_dev, 0) < 0) { cout << "libfreenect: Could not open device" << endl; @@ -323,4 +351,323 @@ int GetKinectFrame(){ return depth_index; } +// This implementation uses the OpenNI2 and pthreads for threading +#elif defined(OPENNI2_INTERFACE) + +#include +#include +#include +#include + +using namespace std; +using namespace openni; + +Device device; +VideoStream depth_stream; +VideoStream color_stream; +Recorder recorder; // for simultaneous recording +bool gotDepth = false; // set to true as soon as the first depth frame is received +int depth_index = 0; // for flipping between the depth double buffers + +pthread_t openni_thread; // thread for the readFrame() loop +volatile bool die = false; // tells the readFrame() loop to stop + +// We use OpenNI frame allocators to let it write new images directly +// into our buffers (one for RGB and a double-buffer for depth) so that +// we don't have to memcpy each frame. + +class KFusionDepthFrameAllocator : public VideoStream::FrameAllocator +{ +private: + uint16_t * depth_buffers_[2]; +public: + KFusionDepthFrameAllocator(uint16_t * depth_buffers[2]) + { + depth_buffers_[0] = depth_buffers[0]; + depth_buffers_[1] = depth_buffers[1]; + } + void *allocateFrameBuffer(int size) + { + if (size != 640*480*2) { + cout << "KFusionDepthFrameAllocator size request of " << size << " (should be " << 640*480*2 << ")" << endl; + throw runtime_error("KFusionDepthFrameAllocator got bad size request, currently only supports 640*480*2"); + } + return depth_buffers_[depth_index]; + } + + // We have static buffers, nothing to do. + void freeFrameBuffer(void *data) {} +}; + +class KFusionColorFrameAllocator : public VideoStream::FrameAllocator +{ +private: + unsigned char * rgb_buffer_; +public: + KFusionColorFrameAllocator(unsigned char * rgb_buffer) + { + rgb_buffer_ = rgb_buffer; + } + void *allocateFrameBuffer(int size) + { + if (size != 640*480*3) { + cout << "KFusionColorFrameAllocator size request of " << size << " (should be " << 640*480*3 << ")" << endl; + throw runtime_error("KFusionColorFrameAllocator got bad size request, currently only supports 640*480*3"); + } + return rgb_buffer_; + } + + // We have static buffers, nothing to do. + void freeFrameBuffer(void *data) {} +}; + +// This thread continuously reads depth and RGB images from the camera +// using the blocking readFrame(). +// We could have used OpenNI::waitForAnyStream() as an alternative, +// but there is no direct benefit of it. +void *openni_threadfunc(void *arg) +{ + while(!die){ + Status status = STATUS_OK; + + // Our FrameAllocators make sure the data lands in our buffers; + // that's why we never have to use the VideoFrameRefs. + + // Next depth frame + VideoFrameRef depthFrame; + status = depth_stream.readFrame(&depthFrame); + if (status != STATUS_OK) { + printf("OpenNI: readFrame failed:\n%s\n", OpenNI::getExtendedError()); + break; + } else { + depth_index = (depth_index+1) % 2; // Flip double buffers + gotDepth = true; + } + + // Next RGB frame + VideoFrameRef colorFrame; + status = color_stream.readFrame(&colorFrame); + if (status != STATUS_OK) { + printf("OpenNI: readFrame failed:\n%s\n", OpenNI::getExtendedError()); + break; + } + } + depth_stream.destroy(); + color_stream.destroy(); + device.close(); + OpenNI::shutdown(); + return NULL; +} + +int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer, const string & replay_path, const string & simultaneous_recording_path ) +{ + // The allocators must survive this initialization function. + KFusionDepthFrameAllocator *depthAlloc = new KFusionDepthFrameAllocator(depth_buffer); + KFusionColorFrameAllocator *colorAlloc = new KFusionColorFrameAllocator(rgb_buffer); + + Status status = STATUS_OK; + + // Initialize OpenNI + status = OpenNI::initialize(); + + if (status != STATUS_OK) { + printf("OpenNI: Initialize failed:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Check if a camera is connected + Array deviceList; + OpenNI::enumerateDevices(&deviceList); + int nr_devices = deviceList.getSize(); + + if(replay_path == "" && nr_devices < 1) { + cout << "OpenNI: No devices found" << endl; + OpenNI::shutdown(); + return 1; + } + + // Open device or .oni file to replay + if (replay_path == "") + { + // Use a camera + status = device.open(ANY_DEVICE); + } else { + // Use an .oni file + status = device.open(replay_path.c_str()); + } + + if (status != STATUS_OK) { + printf("OpenNI: Could not open device:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Create depth stream + if (device.getSensorInfo(SENSOR_DEPTH) != NULL) { + status = depth_stream.create(device, SENSOR_DEPTH); + if (status != STATUS_OK) { + printf("OpenNI: Could not create depth stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + } + + // Create color stream + if (device.getSensorInfo(SENSOR_COLOR) != NULL) { + status = color_stream.create(device, SENSOR_COLOR); + if (status != STATUS_OK) { + printf("OpenNI: Could not create color stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + } + + // Setting video modes is only necessary if we're using a real camera. + if (replay_path == "") { + + // Choose what depth format we want from the camera + VideoMode depth_mode; + depth_mode.setPixelFormat(PIXEL_FORMAT_DEPTH_1_MM); + depth_mode.setResolution(640, 480); + depth_mode.setFps(30); + status = depth_stream.setVideoMode(depth_mode); + if (status != STATUS_OK) { + printf("OpenNI: Could not set depth video mode:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + + // Choose what color format we want from the camera + VideoMode color_mode; + color_mode.setPixelFormat(PIXEL_FORMAT_RGB888); + color_mode.setResolution(640, 480); + color_mode.setFps(30); + status = color_stream.setVideoMode(color_mode); + if (status != STATUS_OK) { + printf("OpenNI: Could not set color video mode:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + } + + // Enable registration mode + status = device.setImageRegistrationMode(IMAGE_REGISTRATION_DEPTH_TO_COLOR); + + if (status != STATUS_OK) { + printf("OpenNI: Could not enable registration mode:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Enable color-to-depth synchronization + status = device.setDepthColorSyncEnabled(true); + + if (status != STATUS_OK) { + printf("OpenNI: Could not enable color sync:\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Disable depth mirroring (we want to see the perspective of the camera) + // (only works with a real camera, not with a replay) + if (replay_path == "") + { + status = depth_stream.setMirroringEnabled(false); + + if (status != STATUS_OK) { + printf("OpenNI: Could enable mirroring on depth stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + } + + // Disable color mirroring (we want to see the perspective of the camera) + // (only works with a real camera, not with a replay) + if (replay_path == "") + { + status = color_stream.setMirroringEnabled(false); + + if (status != STATUS_OK) { + printf("OpenNI: Could enable mirroring on color stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + } + + // Use allocator to have OpenNI write directly into our depth buffers + status = depth_stream.setFrameBuffersAllocator(depthAlloc); + + if (status != STATUS_OK) { + printf("OpenNI: Could not set depth frame buffer allocator\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Use allocator to have OpenNI write directly into our color buffer + status = color_stream.setFrameBuffersAllocator(colorAlloc); + + if (status != STATUS_OK) { + printf("OpenNI: Could not set color frame buffer allocator\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Start depth + status = depth_stream.start(); + + if (status != STATUS_OK) { + printf("OpenNI: Could not start the depth stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Start color + status = color_stream.start(); + + if (status != STATUS_OK) { + printf("OpenNI: Could not start the color stream\n%s\n", OpenNI::getExtendedError()); + OpenNI::shutdown(); + return 1; + } + + // Simultaneous recording + if (simultaneous_recording_path != "") + { + recorder.create(simultaneous_recording_path.c_str()); + recorder.attach(depth_stream); + recorder.attach(color_stream); + recorder.start(); + } + + // Start spawn thread running openni_threadfunc to poll for new frames + int res = pthread_create(&openni_thread, NULL, openni_threadfunc, NULL); + if(res) { + cout << "error starting kinect thread " << res << endl; + OpenNI::shutdown(); + return 1; + } + + return 0; +} + +void CloseKinect() { + die = true; + pthread_join(openni_thread, NULL); +} + +bool KinectFrameAvailable() { + bool result = gotDepth; + gotDepth = false; + return result; +} + +int GetKinectFrame() { + return depth_index; +} + +#else +#error "No camera driver interface specified!" #endif diff --git a/interface.h b/interface.h index e84f1af..f7e7e40 100644 --- a/interface.h +++ b/interface.h @@ -24,7 +24,7 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #ifndef INTERFACE_H #define INTERFACE_H -int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer ); +int InitKinect( uint16_t * depth_buffer[2], unsigned char * rgb_buffer, const std::string & replay_path = "", const std::string & simultaneous_recording_path = "" ); bool KinectFrameAvailable(); int GetKinectFrame(); void CloseKinect(); diff --git a/kinect.cpp b/kinect.cpp index c337ada..8df5a43 100644 --- a/kinect.cpp +++ b/kinect.cpp @@ -26,6 +26,7 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #include "interface.h" #include "perfstats.h" +#include #include #include #include @@ -106,17 +107,17 @@ void display(void){ glClear(GL_COLOR_BUFFER_BIT); glRasterPos2i(0, 0); - glDrawPixels(lightScene); + glDrawPixels(lightScene); // left top glRasterPos2i(0, 240); glPixelZoom(0.5, -0.5); - glDrawPixels(rgbImage); + glDrawPixels(rgbImage); // left bottom glPixelZoom(1,-1); glRasterPos2i(320,0); - glDrawPixels(lightModel); + glDrawPixels(lightModel); // middle top glRasterPos2i(320,240); - glDrawPixels(trackModel); + glDrawPixels(trackModel); // middle bottom glRasterPos2i(640, 0); - glDrawPixels(texModel); + glDrawPixels(texModel); // right const double endProcessing = Stats.sample("draw"); Stats.sample("total", endProcessing - startFrame, PerfStats::TIME); @@ -196,11 +197,53 @@ void exitFunc(void){ cudaDeviceReset(); } +// Look up the parameter to the given flag in argv. +// Returns "" if the flag was not given. +// Example: `string param = getFlag(argc, argv, "--flag")` +// Inspired by: http://stackoverflow.com/a/868894/263061 +string getFlag(vector & args, const string & flag) +{ + vector::iterator itr = std::find(args.begin(), args.end(), flag); + if (itr != args.end() && ++itr != args.end()) + { + return string(*itr); + } + return ""; +} + +// Tell if an argument-less flag was given. +// Example: bool help = getFlag(argc, argv, "--help") +bool haveSwitch(vector & args, const string & flag) +{ + return std::find(args.begin(), args.end(), flag) != args.end(); +} + + int main(int argc, char ** argv) { - const float size = (argc > 1) ? atof(argv[1]) : 2.f; + + // Convert argv to vector + std::vector args(argv + 1, argv + argc); + + const float default_size = 2.f; KFusionConfig config; + // Search for --help argument + if (haveSwitch(args, "--help")) { + cout << "Usage: kinect [--size METERS] [--dist_threshold METERS] [--normal_threshold METERS] [--simultaneous-recording PATH.oni] [--replay PATH.oni]" << endl; + cout << endl; + cout << "Any other argument is ignored." << endl; + cout << endl; + cout << "Defaults:" << endl; + cout << " --size " << default_size << endl; + cout << " --dist_threshold " << config.dist_threshold << endl; + cout << " --normal_threshold " << config.normal_threshold << endl; + return 0; + } + + string size_flag = getFlag(args, "--size"); + const float size = (size_flag != "") ? atof(size_flag.c_str()) : default_size; + // it is enough now to set the volume resolution once. // everything else is derived from that. // config.volumeSize = make_uint3(64); @@ -225,8 +268,15 @@ int main(int argc, char ** argv) { config.iterations[1] = 5; config.iterations[2] = 4; - config.dist_threshold = (argc > 2 ) ? atof(argv[2]) : config.dist_threshold; - config.normal_threshold = (argc > 3 ) ? atof(argv[3]) : config.normal_threshold; + string dist_threshold_flag = getFlag(args, "--dist_threshold"); + config.dist_threshold = (dist_threshold_flag != "") ? atof(dist_threshold_flag.c_str()) : config.dist_threshold; + + string normal_threshold_flag = getFlag(args, "--normal_threshold"); + config.normal_threshold = (normal_threshold_flag != "") ? atof(normal_threshold_flag.c_str()) : config.normal_threshold; + + string replay_path = getFlag(args, "--replay"); // "" for no replay (use camera device) + + string simultaneous_recording_path = getFlag(args, "--simultaneous-recording"); // "" for no recording initPose = SE3(makeVector(size/2, size/2, 0, 0, 0, 0)); @@ -251,12 +301,15 @@ int main(int argc, char ** argv) { return 1; } + cout << "Using depthImage size: " << depthImage[0].size.x*depthImage[0].size.y * sizeof(uint16_t) << " bytes " << endl; + cout << "Using rgbImage size: " << rgbImage.size.x*rgbImage.size.y * sizeof(uchar3) << " bytes " << endl; + memset(depthImage[0].data(), 0, depthImage[0].size.x*depthImage[0].size.y * sizeof(uint16_t)); memset(depthImage[1].data(), 0, depthImage[1].size.x*depthImage[1].size.y * sizeof(uint16_t)); memset(rgbImage.data(), 0, rgbImage.size.x*rgbImage.size.y * sizeof(uchar3)); uint16_t * buffers[2] = {depthImage[0].data(), depthImage[1].data()}; - if(InitKinect(buffers, (unsigned char *)rgbImage.data())){ + if(InitKinect(buffers, (unsigned char *)rgbImage.data(), replay_path, simultaneous_recording_path)){ cudaDeviceReset(); return 1; }