SHARE
TWEET

Untitled

a guest Nov 19th, 2010 2,506 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*  freenectopencv.cpp
  2.  
  3. Copyright (C) 2010  Arne Bernin <arne@alamut.de>
  4.  
  5. This code is licensed to you under the terms of the GNU GPL, version 2 or version 3;
  6. see:
  7.  http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
  8.  http://www.gnu.org/licenses/gpl-3.0.txt
  9. */
  10.  
  11.  
  12. /*
  13.  * Makefile for ubuntu, assumes that libfreenect.a is in /usr/lib, and libfreenect.h is in /usr/include
  14.  *
  15.  * make sure you have the latest version of freenect from git!
  16.  
  17. ***************************************************************************************************************************
  18. * Makefile
  19. ***************************************************************************************************************************
  20.  CXXFLAGS =     -O2 -g -Wall -fmessage-length=0 `pkg-config opencv --cflags ` -I /usr/include/libusb-1.0
  21.  
  22.  
  23.  
  24.  OBJS =         freenectopencv.o
  25.  
  26. LIBS =    `pkg-config opencv --libs` -lfreenect
  27.  
  28. TARGET =        kinectopencv
  29.  
  30. $(TARGET):      $(OBJS)
  31.         $(CXX) -o $(TARGET) $(OBJS) $(LIBS)
  32.  
  33. all:    $(TARGET)
  34.  
  35. clean:
  36.         rm -f $(OBJS) $(TARGET)
  37.  
  38.  
  39. ***************************************************************************************************************************
  40. * End of Makefile
  41. ***************************************************************************************************************************
  42.  
  43.  
  44. */
  45.  
  46.  
  47.  
  48. #include "highgui.h"
  49. #include <stdio.h>
  50.  
  51. #include <string.h>
  52. #include <math.h>
  53. #include <libusb.h>
  54.  
  55. #include "libfreenect.h"
  56.  
  57. #include <pthread.h>
  58.  
  59.  
  60. #define CV_NO_BACKWARD_COMPATIBILITY
  61.  
  62. #include <cv.h>
  63. #include <highgui.h>
  64.  
  65.  
  66. #define FREENECTOPENCV_WINDOW_D "Depthimage"
  67. #define FREENECTOPENCV_WINDOW_N "Normalimage"
  68. #define FREENECTOPENCV_RGB_DEPTH 3
  69. #define FREENECTOPENCV_DEPTH_DEPTH 1
  70. #define FREENECTOPENCV_RGB_WIDTH 640
  71. #define FREENECTOPENCV_RGB_HEIGHT 480
  72. #define FREENECTOPENCV_DEPTH_WIDTH 640
  73. #define FREENECTOPENCV_DEPTH_HEIGHT 480
  74.  
  75.  
  76.  
  77.  
  78. IplImage* depthimg = 0;
  79. IplImage* rgbimg = 0;
  80. IplImage* tempimg = 0;
  81. pthread_mutex_t mutex_depth = PTHREAD_MUTEX_INITIALIZER;
  82. pthread_mutex_t mutex_rgb = PTHREAD_MUTEX_INITIALIZER;
  83. pthread_t cv_thread;
  84.  
  85.  
  86. // callback for depthimage, called by libfreenect
  87. void depth_cb(freenect_device *dev, freenect_depth *depth, uint32_t timestamp)
  88.  
  89. {
  90.         cv::Mat depth8;
  91.         cv::Mat mydepth = cv::Mat( FREENECTOPENCV_DEPTH_WIDTH,FREENECTOPENCV_DEPTH_HEIGHT, CV_16UC1, depth);
  92.        
  93.         mydepth.convertTo(depth8, CV_8UC1, 1.0/4.0);
  94.         pthread_mutex_lock( &mutex_depth );
  95.         memcpy(depthimg->imageData, depth8.data, 640*480);
  96.         // unlock mutex
  97.         pthread_mutex_unlock( &mutex_depth );
  98.  
  99. }
  100.  
  101.  
  102.  
  103. // callback for rgbimage, called by libfreenect
  104.  
  105. void rgb_cb(freenect_device *dev, freenect_pixel *rgb, uint32_t timestamp)
  106. {
  107.  
  108.  
  109.                 // lock mutex for opencv rgb image
  110.         pthread_mutex_lock( &mutex_rgb );
  111.         memcpy(rgbimg->imageData, rgb, FREENECT_RGB_SIZE);
  112.         // unlock mutex
  113.         pthread_mutex_unlock( &mutex_rgb );
  114. }
  115.  
  116.  
  117. /*
  118.  * thread for displaying the opencv content
  119.  */
  120. void *cv_threadfunc (void *ptr) {
  121.         cvNamedWindow( FREENECTOPENCV_WINDOW_D, CV_WINDOW_AUTOSIZE );
  122.         cvNamedWindow( FREENECTOPENCV_WINDOW_N, CV_WINDOW_AUTOSIZE );
  123.         depthimg = cvCreateImage(cvSize(FREENECTOPENCV_DEPTH_WIDTH, FREENECTOPENCV_DEPTH_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_DEPTH_DEPTH);
  124.         rgbimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);
  125.         tempimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);
  126.  
  127.         // use image polling
  128.         while (1) {
  129.                 //lock mutex for depth image
  130.                 pthread_mutex_lock( &mutex_depth );
  131.                 // show image to window
  132.                 cvCvtColor(depthimg,tempimg,CV_GRAY2BGR);
  133.                 cvCvtColor(tempimg,tempimg,CV_HSV2BGR);
  134.                 cvShowImage(FREENECTOPENCV_WINDOW_D,tempimg);
  135.                 //unlock mutex for depth image
  136.                 pthread_mutex_unlock( &mutex_depth );
  137.  
  138.                 //lock mutex for rgb image
  139.                 pthread_mutex_lock( &mutex_rgb );
  140.                 // show image to window
  141.                 cvCvtColor(rgbimg,tempimg,CV_BGR2RGB);
  142.                 cvShowImage(FREENECTOPENCV_WINDOW_N, tempimg);
  143.                 //unlock mutex
  144.                 pthread_mutex_unlock( &mutex_rgb );
  145.  
  146.                 // wait for quit key
  147.                 if( cvWaitKey( 15 )==27 )
  148.                                 break;
  149.  
  150.         }
  151.         pthread_exit(NULL);
  152.  
  153. }
  154.  
  155.  
  156. int main(int argc, char **argv)
  157. {
  158.  
  159.         freenect_context *f_ctx;
  160.         freenect_device *f_dev;
  161.  
  162.  
  163.         int res = 0;
  164.         int die = 0;
  165.         printf("Kinect camera test\n");
  166.  
  167.         if (freenect_init(&f_ctx, NULL) < 0) {
  168.                         printf("freenect_init() failed\n");
  169.                         return 1;
  170.                 }
  171.  
  172.                 if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
  173.                         printf("Could not open device\n");
  174.                         return 1;
  175.                 }
  176.  
  177.         freenect_set_depth_callback(f_dev, depth_cb);
  178.         freenect_set_rgb_callback(f_dev, rgb_cb);
  179.         freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
  180.  
  181.         // create opencv display thread
  182.         res = pthread_create(&cv_thread, NULL, cv_threadfunc, (void*) depthimg);
  183.         if (res) {
  184.                 printf("pthread_create failed\n");
  185.                 return 1;
  186.         }
  187.  
  188.         printf("init done\n");
  189.  
  190.  
  191.  
  192.         freenect_start_depth(f_dev);
  193.         freenect_start_rgb(f_dev);
  194.  
  195.         while(!die && freenect_process_events(f_ctx) >= 0 );
  196.  
  197.  
  198. }
RAW Paste Data
Top