void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
if ( pthread_create(&data_in_thread, NULL, data_in, NULL) )
{
fprintf(stderr, "Error on pthread_create() for data_in\n");
}
if ( pthread_create(&data_out_thread, NULL, data_out, NULL) )
{
fprintf(stderr, "Error on pthread_create() for data_out\n");
}
while(!die && freenect_process_events(f_ctx) >= 0 );
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
network_close();
printf("-- done!\n");
return NULL;
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
printf("-- done!\n");
return NULL;
}
void init()
{
freenect_context *ctx;
freenect_device *dev;
if (freenect_init(&ctx, 0)) {
printf("Error: Cannot get context\n");
return;
}
if (freenect_open_device(ctx, &dev, 0)) {
printf("Error: Cannot get device\n");
return;
}
freenect_set_depth_format(dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(dev);
freenect_set_video_format(dev, FREENECT_VIDEO_RGB);
freenect_start_video(dev);
if (use_ffmpeg) {
init_ffmpeg_streams();
freenect_set_depth_callback(dev, depth_cb_ffmpeg);
freenect_set_video_callback(dev, rgb_cb_ffmpeg);
} else {
freenect_set_depth_callback(dev, depth_cb);
freenect_set_video_callback(dev, rgb_cb);
}
while (running && freenect_process_events(ctx) >= 0)
snapshot_accel(dev);
freenect_stop_depth(dev);
freenect_stop_video(dev);
freenect_close_device(dev);
freenect_shutdown(ctx);
}
FreenectDevice(freenect_context *_ctx, int _index) {
if(freenect_open_device(_ctx, &m_dev, _index) < 0) throw std::runtime_error("Cannot open Kinect");
freenect_set_user(m_dev, this);
freenect_set_video_format(m_dev, FREENECT_VIDEO_RGB);
freenect_set_depth_format(m_dev, FREENECT_DEPTH_11BIT);
freenect_set_depth_callback(m_dev, freenect_depth_callback);
freenect_set_video_callback(m_dev, freenect_video_callback);
}
FreenectDevice(freenect_context *_ctx, int _index) {
if(freenect_open_device(_ctx, &m_dev, _index) != 0) throw std::runtime_error("Cannot open Kinect");
freenect_set_user(m_dev, this);
freenect_set_rgb_format(m_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(m_dev, FREENECT_FORMAT_11_BIT);
freenect_set_depth_callback(m_dev, freenect_depth_callback);
freenect_set_rgb_callback(m_dev, freenect_rgb_callback);
}
void setDepthFormat(freenect_depth_format requested_format) {
if (requested_format != m_depth_format) {
freenect_stop_depth(m_dev);
if (freenect_set_depth_format(m_dev, requested_format) < 0) throw std::runtime_error("Cannot set depth format");
freenect_start_depth(m_dev);
m_depth_format = requested_format;
}
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_YUV_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext) != 0){
ofLog(OF_LOG_ERROR, "ofxKinect: freenect_process_events failed!");
return;
}
while(isThreadRunning()){
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying){
freenect_set_led(kinectDevice, targetLedStatus);
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
ofSleepMillis(10);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
// finish up a tilt on exit
if(bTiltNeedsApplying){
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
freenect_close_device(kinectDevice);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
static int change_depth_format(sync_kinect_t *kinect, freenect_depth_format fmt)
{
freenect_stop_depth(kinect->dev);
free_buffer_ring(&kinect->depth);
if (alloc_buffer_ring_depth(fmt, &kinect->depth))
return -1;
freenect_set_depth_format(kinect->dev, fmt);
freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]);
freenect_start_depth(kinect->dev);
return 0;
}
void *freenect_threadfunc(void *arg)
{
int accelCount = 0;
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, current_format);
freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode, 'f'-video format\n");
while (!die && freenect_process_events(f_ctx) >= 0) {
//Throttle the text output
if (accelCount++ >= 2000)
{
accelCount = 0;
freenect_raw_tilt_state* state;
freenect_update_tilt_state(f_dev);
state = freenect_get_tilt_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_format(f_dev, requested_format);
freenect_start_video(f_dev);
current_format = requested_format;
}
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction() {
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_video_format(kinectDevice, bInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_DEPTH_11BIT);
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabRgbFrame);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
freenect_start_video(kinectDevice);
while (isThreadRunning()) {
if( bTiltNeedsApplying ) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
rawAccel.set(tilt->accelerometer_x, tilt->accelerometer_y, tilt->accelerometer_z);
double dx,dy,dz;
freenect_get_mks_accel(tilt, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
ofSleepMillis(10);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
//TODO: uncomment these when they are implemented in freenect
freenect_set_tilt_degs(kinectDevice, 0);
freenect_update_tilt_state(kinectDevice);
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
freenect_set_led(kinectDevice, LED_YELLOW);
freenect_close_device(kinectDevice);
freenect_shutdown(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection closed");
}
int initFreenect() {
// int res = 0;
//setup Freenect...
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_ERROR);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
// if (argc > 1)
// user_device_number = atoi(argv[1]);
//
// if (nr_devices < 1)
// return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
//freenect_set_rgb_callback(f_dev, rgb_cb);
//freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
//start the freenect thread to poll for events
// res = pthread_create(&ocv_thread, NULL, freenect_threadfunc, NULL);
// if (res) {
// printf("pthread_create failed\n");
// return 1;
// }
return 0;
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
SS_SimMode mode= ssGetSimMode(S);
if(mode==0)
{
kinFail=0;
kinRunning=1;
newFrame=0;
//change
memset (kinR,'1',307200);
memset (kinG,'1',307200);
memset (kinB,'1',307200);
memset (kinD,'1',307200);
/*-----INIT---------------*/
if (freenect_init(&kinCtx, 0)) {
printf("Error: Cannot get context\n");
kinFail=1;
return;
}
if (freenect_open_device(kinCtx, &kinDev, 0)) {
printf("Error: Cannot get device\n");
kinFail=1;
return;
}
freenect_set_depth_format(kinDev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kinDev);
//change
freenect_set_video_format(kinDev, FREENECT_VIDEO_YUV_RGB);
freenect_start_video(kinDev);
freenect_set_depth_callback(kinDev, depth_cb);
freenect_set_video_callback(kinDev, rgb_cb);
/*-----END INIT------------*/
pthread_create( &kinThread, NULL, runLoop, NULL);
}
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
if (freenect_init(&kinectContext, NULL) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: freenet_init failed");
}
int number_devices = freenect_num_devices(kinectContext);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Number of Devices found: " + ofToString(number_devices));
if (number_devices < 1) {
ofLog(OF_LOG_ERROR, "ofxKinect: didnt find a device");
return;
}
if (freenect_open_device(kinectContext, &kinectDevice, 0) < 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: could not open device");
return;
}
freenect_set_led(kinectDevice, LED_GREEN);
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_rgb_callback(kinectDevice, &grabRgbFrame);
freenect_set_rgb_format(kinectDevice, FREENECT_FORMAT_RGB);
freenect_set_depth_format(kinectDevice, FREENECT_FORMAT_11_BIT);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Connection opened");
freenect_start_depth(kinectDevice);
freenect_start_rgb(kinectDevice);
while (isThreadRunning()) {
int16_t ax,ay,az;
freenect_get_raw_accelerometers(kinectDevice, &ax, &ay, &az);
rawAccel.set(ax, ay, az);
double dx,dy,dz;
freenect_get_mks_accelerometers(kinectDevice, &dx, &dy, &dz);
mksAccel.set(dx, dy, dz);
// printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", ax, ay, az, dx, dy, dz);
}
}
void *freenect_threadfunc(void *arg)
{
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
freenect_raw_device_state* state;
freenect_update_device_state(f_dev);
state = freenect_get_device_state(f_dev);
double dx,dy,dz;
freenect_get_mks_accel(state, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f", state->accelerometer_x, state->accelerometer_y, state->accelerometer_z, dx, dy, dz);
fflush(stdout);
}
printf("\nshutting down streams...\n");
freenect_stop_depth(f_dev);
freenect_stop_rgb(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
printf("-- done!\n");
return NULL;
}
int KntInit(kntCtx_t * const ctx, int devNum) {
if (freenect_init(&ctx->fn, NULL))
return 0;
if (freenect_open_device(ctx->fn, &ctx->dev, devNum))
return 0;
freenect_set_depth_format(ctx->dev, FREENECT_DEPTH_11BIT);
ctx->dbb = malloc(640*480*2);
ctx->dfb = malloc(640*480*2);
ctx->depthBuf = malloc(640*480*4);
ctx->ibb = malloc(640*480*3);
ctx->imageBuf = malloc(640*480*3);
freenect_set_depth_buffer(ctx->dev, ctx->dbb);
freenect_set_video_buffer(ctx->dev, ctx->ibb);
freenect_set_depth_callback(ctx->dev, KntDepthHandler);
freenect_set_video_callback(ctx->dev, KntImageHandler);
if (!contexts) {
contexts = (ctxList_t *)malloc(sizeof(*contexts));
contexts->next = NULL;
contexts->ctx = *ctx;
} else {
ctxList_t * ptr = contexts;
while (ptr->next)
ptr = ptr->next;
ptr->next = (ctxList_t *)malloc(sizeof(*contexts));
ptr = ptr->next;
ptr->ctx = *ctx;
}
}
int main(int argc, char *argv[])
{
struct kingrid_info data;
freenect_context *kn;
freenect_device *kn_dev;
int rows = 40, cols = 96; // terminal size
int opt;
sigdata = &data;
data.out_of_range = 0;
data.done = 0;
data.divisions = 6;
data.boxwidth = 10;
data.histrows = 8;
data.frame = 0;
data.zmin = 0.5;
data.zmax = 5.0;
data.disp_mode = STATS;
if(getenv("LINES")) {
rows = atoi(getenv("LINES"));
}
if(getenv("COLUMNS")) {
cols = atoi(getenv("COLUMNS"));
}
// Handle command-line options
while((opt = getopt(argc, argv, "shag:z:Z:")) != -1) {
switch(opt) {
case 's':
// Stats mode
data.disp_mode = STATS;
break;
case 'h':
// Histogram mode
data.disp_mode = HISTOGRAM;
break;
case 'a':
// ASCII art mode
data.disp_mode = ASCII;
break;
case 'g':
// Grid divisions
data.divisions = atoi(optarg);
break;
case 'z':
// Near clipping
data.zmin = atof(optarg);
break;
case 'Z':
// Far clipping
data.zmax = atof(optarg);
break;
default:
fprintf(stderr, "Usage: %s -[sha] [-g divisions] [-zZ distance]\n", argv[0]);
fprintf(stderr, "Use up to one of:\n");
fprintf(stderr, "\ts - Stats mode (default)\n");
fprintf(stderr, "\th - Histogram mode\n");
fprintf(stderr, "\ta - ASCII art mode\n");
fprintf(stderr, "Use any of:\n");
fprintf(stderr, "\tg - Set grid divisions for both dimensions\n");
fprintf(stderr, "\tz - Set near clipping plane in meters for ASCII art mode (default 0.5)\n");
fprintf(stderr, "\tZ - Set far clipping plane in meters for ASCII art mode (default 5.0)\n");
return -1;
}
}
data.boxwidth = (cols - 1) / data.divisions - 3;
if(data.boxwidth < 10) {
data.boxwidth = 10;
}
data.histrows = (rows - 2) / data.divisions - 1;
init_lut(data.depth_lut);
if(signal(SIGINT, intr) == SIG_ERR ||
signal(SIGTERM, intr) == SIG_ERR) {
ERROR_OUT("Error setting signal handlers\n");
return -1;
}
if(freenect_init(&kn, NULL) < 0) {
ERROR_OUT("libfreenect init failed.\n");
return -1;
}
INFO_OUT("Found %d Kinect devices.\n", freenect_num_devices(kn));
if(freenect_num_devices(kn) == 0) {
ERROR_OUT("No Kinect devices present.\n");
return -1;
}
if(freenect_open_device(kn, &kn_dev, 0)) {
ERROR_OUT("Error opening Kinect #0.\n");
return -1;
}
freenect_set_user(kn_dev, &data);
freenect_set_tilt_degs(kn_dev, -5);
freenect_set_led(kn_dev, LED_GREEN);
freenect_set_depth_callback(kn_dev, depth);
freenect_set_depth_format(kn_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kn_dev);
int last_oor = data.out_of_range;
while(!data.done && freenect_process_events(kn) >= 0) {
if(last_oor != data.out_of_range) {
freenect_set_led(kn_dev, data.out_of_range ? LED_BLINK_RED_YELLOW : LED_GREEN);
last_oor = data.out_of_range;
}
}
freenect_stop_depth(kn_dev);
freenect_set_led(kn_dev, LED_OFF);
freenect_close_device(kn_dev);
freenect_shutdown(kn);
return 0;
}
int main(int argc, char **argv)
{
cv_depth_mat = cvCreateMat(480, 640, CV_16UC1);
cv_rgb_mat = cvCreateMat(480, 640, CV_8UC3);
int res;
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
//freenect_set_tilt_degs(f_dev,15);
freenect_set_tilt_degs(f_dev,0);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
cvNamedWindow("rgb", CV_WINDOW_NORMAL);
cvNamedWindow("depth", CV_WINDOW_NORMAL);
cvNamedWindow("depth_th", CV_WINDOW_NORMAL);
cvNamedWindow("contourWin", CV_WINDOW_NORMAL);
CvMat* cv_depth_threshold_mat = cvCreateMat(480,640, CV_8UC1);
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
// Variables for contour finding
CvSeq* contours = NULL;
CvMemStorage* memStorage = cvCreateMemStorage(0);
IplImage* contour_image = cvCreateImage(cvSize(640,480), 8, 1);
//hist_segment_init();
//feature_extract_init();
optflow_init(cvSize(640,480));
while (!die)
{
cvConvertScale(cv_depth_mat, cv_depth_threshold_mat, 255.0/2048.0, 0);
// 120 ~ 2.7 m
cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 120.0, CV_THRESH_TOZERO_INV);
//cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 255.0, CV_THRESH_BINARY_INV);
// Find contours
/*cvClearMemStorage(memStorage);
cvFindContours( cv_depth_threshold_mat, memStorage, &contours, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
cvZero(contour_image);
// Draw the contours
if (contours)
{
cvDrawContours(contour_image, contours, cvScalarAll(255.0), cvScalarAll(255.0), 1, 1, 8, cvPoint(0,0));
}
cvShowImage("contourWin",contour_image);*/
//extractFeatures(cv_depth_threshold_mat);
//hist_segment(cv_depth_threshold_mat, NULL);
optflow_calculate(cv_depth_threshold_mat, NULL);
cvShowImage("rgb", cv_rgb_mat);
cvShowImage("depth_th", cv_depth_threshold_mat);
char k = cvWaitKey(5);
if( k == 27 ) break;
}
optflow_deinit();
//hist_segment_deinit();
//feature_extract_deinit();
printf("-- done!\n");
cvDestroyWindow("rgb");
cvDestroyWindow("depth");
cvDestroyWindow("depth_th");
cvReleaseMat(&cv_depth_mat);
cvReleaseMat(&cv_rgb_mat);
cvReleaseMat(&cv_depth_threshold_mat);
// Release Contour variables
cvDestroyWindow("contourWin");
pthread_join(freenect_thread, NULL);
pthread_exit(NULL);
}
int main(int argc, char **argv)
{
int res;
freenect_context *f_ctx;
printf("Kinect camera test\n");
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("-- done!\n");
pthread_exit(NULL);
}
int main(int argc, char **argv)
{
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
if ( network_init() < 0 )
return -1;
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
//freenect_start_depth(f_dev);
//freenect_set_led(f_dev,LED_RED);
while(!die && freenect_process_events(f_ctx) >= 0 ){
char buffer[6];
int n = read(data_child, buffer, 1024);
//printf("n: %d\n", n);
if(n == 6){
if (buffer[0] == 1) { //MOTOR
if (buffer[1] == 1) { //MOVE
int angle;
memcpy(&angle, &buffer[2], sizeof(int));
freenect_set_tilt_degs(f_dev,ntohl(angle));
}
}
}
}
network_close();
printf("-- done!\n");
pthread_exit(NULL);
}
int main( int argc, char** argv ) {
int res;
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
printf("Kinect camera test\n");
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
printf("Could not open device\n");
return 1;
}
cvNamedWindow( "RGB", CV_WINDOW_AUTOSIZE );
cvMoveWindow( "RGB", 0, 0);
rgbBack = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
rgbFront = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
cvNamedWindow( "Depth", CV_WINDOW_AUTOSIZE );
cvMoveWindow("Depth", FREENECT_FRAME_W, 0);
depthBack = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
depthFront = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&kinect_thread, NULL, kinect_threadFunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
while(1) {
pthread_mutex_lock(&backbuf_mutex);
{
while (got_frames < 2) {
pthread_cond_wait(&framesReady_cond, &backbuf_mutex);
}
cvConvertImage(rgbBack, rgbFront, CV_BGR2GRAY);
cvConvertImage(depthBack, depthFront, CV_BGR2GRAY);
got_frames = 0;
}
pthread_mutex_unlock(&backbuf_mutex);
cvShowImage("RGB", rgbFront);
cvShowImage("Depth", depthFront);
char c = cvWaitKey(10);
if( c == 27 ) break;
}
cvDestroyWindow( "RGB" );
cvDestroyWindow( "Depth" );
}
