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_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, 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, '+' & '-' - change IR intensity \n");
printf("'f' - change video format, 'm' - mirror video, 'o' - rotate video with accelerometer \n");
printf("'e' - auto exposure, 'b' - white balance, 'r' - raw color, 'n' - near mode (K4W only) \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_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, 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 *freenect_threadfunc(void *arg)
{
printf("Freenect thread\n");
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_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, 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_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, 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 KinectInterface::setVideoMode(
int _mode
)
{
freenect_video_format vm=FREENECT_VIDEO_RGB;
switch(_mode)
{
case 0 : { vm=FREENECT_VIDEO_RGB; break;}
case 1 : { vm=FREENECT_VIDEO_YUV_RGB; break;}
case 2 : { vm=FREENECT_VIDEO_IR_8BIT; break;}
/*
/// had issues with these modes so sticking to the 3 that work
case 1 : { vm=FREENECT_VIDEO_BAYER; break;}
case 2 : { vm=FREENECT_VIDEO_IR_8BIT; break;}
case 3 : { vm=FREENECT_VIDEO_IR_10BIT; break;}
case 4 : { vm=FREENECT_VIDEO_IR_10BIT_PACKED; break;}
case 5 : { vm=FREENECT_VIDEO_YUV_RGB; break;}
case 6 : { vm=FREENECT_VIDEO_YUV_RAW; break;}
*/
default : qDebug()<<"index out of bounds for video mode\n";
vm=FREENECT_VIDEO_RGB;
break;
}
/// stop the video and set to new mode
freenect_stop_video(m_dev);
freenect_set_video_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
//deprecated
// freenect_set_video_format(m_dev, vm);
freenect_start_video(m_dev);
}
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);
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLogVerbose("ofxKinect") << "device " << deviceId << " " << serial << " connection opened";
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
while(isThreadRunning() && freenect_process_events(kinectContext.getContext()) >= 0) {
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
if(bLedNeedsApplying) {
if(currentLed == ofxKinect::LED_DEFAULT) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_GREEN);
}
else {
freenect_set_led(kinectDevice, (freenect_led_options)currentLed);
}
bLedNeedsApplying = false;
}
freenect_update_tilt_state(kinectDevice);
freenect_raw_tilt_state * tilt = freenect_get_tilt_state(kinectDevice);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
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);
}
// finish up a tilt on exit
if(bTiltNeedsApplying) {
freenect_set_tilt_degs(kinectDevice, targetTiltAngleDeg);
bTiltNeedsApplying = false;
}
freenect_stop_depth(kinectDevice);
freenect_stop_video(kinectDevice);
if(currentLed < 0) {
freenect_set_led(kinectDevice, (freenect_led_options)ofxKinect::LED_RED);
}
kinectContext.close(*this);
ofLogVerbose("ofxKinect") << "device " << deviceId << " connection closed";
}
/*
* Class: org_ensor_robots_sensors_kinect_Device
* Method: nativeStartVideo
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Device_nativeStartVideo
(JNIEnv *aJNIEnv, jobject aThisObject, jint aDeviceId)
{
pthread_mutex_lock(&mutex);
freenect_set_video_mode(f_devices[aDeviceId].f_dev,
freenect_find_video_mode(
FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB
)
);
f_devices[aDeviceId].videobuffersize = 640 * 480 * 3;
f_devices[aDeviceId].f_video_buffer = malloc(f_devices[aDeviceId].videobuffersize);
freenect_set_video_buffer(
f_devices[aDeviceId].f_dev,
f_devices[aDeviceId].f_video_buffer
);
f_devices[aDeviceId].videobuffervalue =
(*aJNIEnv)->NewBooleanArray(aJNIEnv, f_devices[aDeviceId].videobuffersize);
f_devices[aDeviceId].videobuffervalue = (*aJNIEnv)->NewGlobalRef(aJNIEnv,
f_devices[aDeviceId].videobuffervalue
);
freenect_set_video_callback(f_devices[aDeviceId].f_dev, video_cb);
pthread_mutex_unlock(&mutex);
freenect_start_video(f_devices[aDeviceId].f_dev);
}
//----------------------------------------------------------------------------------------------------------------------
void KinectInterface::startVideo()
{
if(freenect_start_video(m_dev) < 0)
{
throw std::runtime_error("Cannot start RGB callback");
}
}
void setVideoFormat(freenect_video_format requested_format) {
if (requested_format != m_video_format) {
freenect_stop_video(m_dev);
if (freenect_set_video_format(m_dev, requested_format) < 0) throw std::runtime_error("Cannot set video format");
freenect_start_video(m_dev);
m_video_format = requested_format;
}
}
void KntStart(kntCtx_t * ctx) {
freenect_start_depth(ctx->dev);
freenect_start_video(ctx->dev);
ctx->isRunning = 1;
pthread_create(&ctx->thread, NULL, KntHandleData, ctx);
}
//---------------------------------------------------------------------------
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");
}
//---------------------------------------------------------------------------
void ofxKinect::threadedFunction(){
freenect_set_led(kinectDevice, LED_GREEN);
freenect_frame_mode videoMode = freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, bIsVideoInfrared?FREENECT_VIDEO_IR_8BIT:FREENECT_VIDEO_RGB);
freenect_set_video_mode(kinectDevice, videoMode);
freenect_frame_mode depthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, bUseRegistration?FREENECT_DEPTH_REGISTERED:FREENECT_DEPTH_MM);
freenect_set_depth_mode(kinectDevice, depthMode);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d %s connection opened", deviceId, serial.c_str());
freenect_start_depth(kinectDevice);
if(bGrabVideo) {
freenect_start_video(kinectDevice);
}
// call platform specific processors (needed for Win)
if(freenect_process_events(kinectContext.getContext()) != 0) {
ofLog(OF_LOG_ERROR, "ofxKinect: Device %d freenect_process_events failed!", deviceId);
return;
}
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);
currentTiltAngleDeg = freenect_get_tilt_degs(tilt);
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);
// ... and $0.02 for the scheduler
ofSleepMillis(10);
}
// 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);
kinectContext.close(*this);
ofLog(OF_LOG_VERBOSE, "ofxKinect: Device %d connection closed", deviceId);
}
// the freenect thread.
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_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(f_dev, rgb_back);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
while (!die && freenect_process_events(f_ctx) >= 0) {
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);
}
if (requested_format != current_format) {
freenect_stop_video(f_dev);
freenect_set_video_mode(f_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, requested_format));
freenect_start_video(f_dev);
current_format = requested_format;
}
}
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
return NULL;
}
void setVideoFormat(freenect_video_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) {
if (requested_format != m_video_format || requested_resolution != m_video_resolution) {
freenect_stop_video(m_dev);
freenect_frame_mode mode = freenect_find_video_mode(requested_resolution, requested_format);
if (!mode.is_valid) throw std::runtime_error("Cannot set video format: invalid mode");
if (freenect_set_video_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set video format");
freenect_start_video(m_dev);
m_video_format = requested_format;
m_video_resolution = requested_resolution;
}
}
static int change_video_format(sync_kinect_t *kinect, freenect_video_format fmt)
{
freenect_stop_video(kinect->dev);
free_buffer_ring(&kinect->video);
if (alloc_buffer_ring_video(fmt, &kinect->video))
return -1;
freenect_set_video_format(kinect->dev, fmt);
freenect_set_video_buffer(kinect->dev, kinect->video.bufs[2]);
freenect_start_video(kinect->dev);
return 0;
}
static int change_video_format(sync_kinect_t *kinect, freenect_video_format fmt)
{
freenect_stop_video(kinect->dev);
free_buffer_ring(&kinect->video);
if (alloc_buffer_ring_video(fmt, &kinect->video))
return -1;
freenect_set_video_mode(kinect->dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, fmt));
freenect_set_video_buffer(kinect->dev, kinect->video.bufs[2]);
freenect_start_video(kinect->dev);
return 0;
}
void FreenectGrabber :: setIRMode(bool ir)
{
if (!isConnected()) return;
QWriteLocker locker(&m_lock);
m_ir_mode = ir;
freenect_stop_video(f_dev);
if (!m_ir_mode)
freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
else
freenect_set_video_format(f_dev, FREENECT_VIDEO_IR_8BIT);
freenect_start_video(f_dev);
}
//---------------------------------------------------------------------------
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");
}
/*
* Main method; primarily just initializes our thread and handles Kinect details.
*/
int main(int argc, char **argv)
{
freenect_context *f_ctx;
freenect_device *f_dev;
display = 0;
int res = 0;
int die = 0;
printf("Code Red Kinect Vision init\n");
if (argc > 1 && strcmp(argv[1],"--display") == 0) {
display = 1;
}
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;
}
if (display) { cvNamedWindow( FREENECTOPENCV_WINDOW_N, CV_WINDOW_AUTOSIZE ); }
rgbimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_IR_DEPTH);
tempimg = cvCreateImage(cvSize(FREENECTOPENCV_RGB_WIDTH, FREENECTOPENCV_RGB_HEIGHT), IPL_DEPTH_8U, FREENECTOPENCV_RGB_DEPTH);
// create opencv display thread
res = pthread_create(&cv_thread, NULL, cv_threadfunc, (void*) tempimg);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, FREENECT_VIDEO_IR_8BIT);
printf("init done\n");
freenect_start_video(f_dev);
while(!die && freenect_process_events(f_ctx) >= 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);
}
}
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;
}
int KinectFreenect::start() {
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
int nr_devices = freenect_num_devices(f_ctx);
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device.\n");
return 1;
}
freenect_set_led(f_dev, LED_RED);
freenect_set_depth_callback(f_dev, KinectFreenect::depth_cb);
freenect_set_video_callback(f_dev, KinectFreenect::rgb_cb);
freenect_set_video_mode(f_dev, freenect_find_video_mode(current_resolution, current_format));
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
int res;
res = pthread_create(&fnkt_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
int status = 0;
while (!die && status >= 0) {
char k = cvWaitKey(5);
if( k == 27 ) {
die = 1;
break;
}
}
return 0;
}
void kinect_thread(void* user) {
Kinect* kinect = static_cast<Kinect*>(user);
if(!kinect) {
printf("Error: kinect thread didn't receive a reference to the Kinect instance.\n");
::exit(EXIT_FAILURE);
}
freenect_device* dev = kinect->device;
//freenect_set_tilt_degs(dev, 15);
freenect_set_led(dev, LED_RED);
freenect_set_depth_callback(dev, kinect_depth_callback);
freenect_set_video_callback(dev, kinect_video_callback);
freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(dev, kinect->rgb_back);
freenect_set_depth_buffer(dev, kinect->depth_back);
freenect_start_depth(dev);
freenect_start_video(dev);
bool must_stop = false;
while(freenect_process_events(kinect->ctx) >= 0) {
uv_mutex_lock(&kinect->mutex);
must_stop = kinect->must_stop;
uv_mutex_unlock(&kinect->mutex);
if(must_stop) {
break;
}
}
freenect_set_led(dev, LED_GREEN);
freenect_stop_depth(dev);
freenect_stop_video(dev);
}
void *capture_loop(void *arg) {
freenect_set_depth_callback(device, depth_captured);
freenect_set_depth_mode(device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_MM));
freenect_start_depth(device);
freenect_set_video_callback(device, rgb_captured);
freenect_set_video_mode(device, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_video_buffer(device, kinect_rgb_buffer);
freenect_start_video(device);
while (freenect_process_events(context) >= 0) {
if (die) break;
}
printf("shutting down streams...\n");
freenect_stop_depth(device);
freenect_close_device(device);
freenect_shutdown(context);
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;
}
// fakenect doesn't support audio yet, so don't bother claiming the device
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
if (freenect_open_device(ctx, &dev, 0)) {
printf("Error: Cannot get device\n");
return;
}
print_mode("Depth", freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
print_mode("Video", freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(dev);
freenect_set_video_mode(dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, 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);
}
void* f_threadfunc( void *arg )
{
freenect_set_tilt_degs( f_device, f_angle );
freenect_set_led( f_device, LED_RED );
freenect_set_depth_callback( f_device, depth_cb );
freenect_set_video_callback(f_device, rgb_cb);
if( (W == 640) && H ==( 480 ))
{
freenect_set_video_mode( f_device, freenect_find_video_mode( FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB ) );
printf("Resolution:%d:%d\n",W,H);
fflush(stdout);
}
else
if(( W == 1280) &&( H == 1024) )
{
freenect_set_video_mode( f_device, freenect_find_video_mode( FREENECT_RESOLUTION_HIGH, FREENECT_VIDEO_RGB ) );
printf("Resolution:%d:%d\n",W,H);
fflush(stdout);
}
else
{
printf("Unknown resolution. Shutting down...\n");
CloseAll();
}
freenect_set_depth_mode( f_device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT) );
freenect_start_video( f_device );
freenect_start_depth( f_device );
while ( !shutdown && freenect_process_events( f_context ) >= 0 )
{
}
CloseAll();
printf( "Done. Shutting down...\n" );
fflush(stdout);
return NULL;
}
void jit_freenect_grab_open(t_jit_freenect_grab *x, t_symbol *s, long argc, t_atom *argv)
{
int ndevices, devices_left, dev_ndx;
t_jit_freenect_grab *y;
freenect_device *dev;
postNesa("opening device...\n");//TODO: remove
if(x->device){
error("A device is already open.");
return;
}
x->is_open = FALSE;
if(!f_ctx){
postNesa("!f_ctx is null, opening a new device\n");//TODO: remove
if (jit_freenect_restart_thread(x)!=MAX_ERR_NONE) {
//if (pthread_create(&capture_thread, NULL, capture_threadfunc, NULL)) {
error("Failed to create capture thread.");
return;
}
int bailout=0;
while((!f_ctx)&&(++bailout<1000)){
//systhread_sleep(1);
sleep(0);
//post("deadlocking in the sun %i",bailout);//TODO: remove
}
if (!f_ctx)
{
// TODO: replace with conditionall
error("Failed to init freenect after %i retries.\n",bailout);
return;
}
}
ndevices = freenect_num_devices(f_ctx);
if(!ndevices){
error("Could not find any connected Kinect device. Are you sure the power cord is plugged-in?");
return;
}
devices_left = ndevices;
dev = f_ctx->first;
while(dev){
dev = dev->next;
devices_left--;
}
if(!devices_left){
error("All Kinect devices are currently in use.");
return;
}
if(!argc){
x->index = 0;
}
else{
//Is the device already in use?
x->index = jit_atom_getlong(argv);
dev = f_ctx->first;
while(dev){
y = freenect_get_user(dev);
if(y->index == x->index){
error("Kinect device %d is already in use.", x->index);
x->index = 0;
return;
}
dev = dev->next;
}
}
if(x->index > ndevices){
error("Cannot open Kinect device %d, only %d are connected.", x->index, ndevices);
x->index = 0;
return;
}
//Find out which device to open
dev_ndx = x->index;
if(!dev_ndx){
int found = 0;
while(!found){
found = 1;
dev = f_ctx->first;
while(dev){
y = freenect_get_user(dev);
if(y->index-1 == dev_ndx){
found = 0;
break;
}
dev = dev->next;
}
dev_ndx++;
}
x->index = dev_ndx;
}
if (freenect_open_device(f_ctx, &(x->device), dev_ndx-1) < 0) {
error("Could not open Kinect device %d", dev_ndx);
x->index = 0;
x->device = NULL;
}
else {
postNesa("device open");//TODO: remove
}
//freenect_set_depth_buffer(x->device, x->depth_back);
//freenect_set_video_buffer(x->device, x->rgb_back);
freenect_set_depth_callback(x->device, depth_callback);
freenect_set_video_callback(x->device, rgb_callback);
if(x->format.a_w.w_sym == s_ir){
freenect_set_video_mode(x->device, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_IR_8BIT));
}
else{
freenect_set_video_mode(x->device, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
}
//TODO: add FREENECT_DEPTH_REGISTERED mode
//FREENECT_DEPTH_REGISTERED = 4, /**< processed depth data in mm, aligned to 640x480 RGB */
//FREENECT_DEPTH_11BIT
if (x->aligndepth==1)
{
postNesa("Depth is aligned to color");
freenect_set_depth_mode(x->device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED));
}
else
{
freenect_set_depth_mode(x->device, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
}
//freenect_set_video_buffer(x->device, rgb_back);
//Store a pointer to this object in the freenect device struct (for use in callbacks)
freenect_set_user(x->device, x);
freenect_set_led(x->device,LED_RED);
//freenect_set_tilt_degs(x->device,x->tilt);
freenect_start_depth(x->device);
freenect_start_video(x->device);
x->is_open = TRUE;
open_device_count++;
freenect_active=TRUE;
}
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);
}
void startVideo() {
if(freenect_start_video(m_dev) < 0) throw std::runtime_error("Cannot start RGB callback");
}
int kinect_init(Kinect* kt){
pthread_mutex_init(&kt->mutex, NULL);
kt->depth_mid = (uint8_t*)malloc(640*480*3);
kt->depth_front = (uint8_t*)malloc(640*480*3);
kt->rgb_back = (uint8_t*)malloc(640*480*3);
kt->rgb_mid = (uint8_t*)malloc(640*480*3);
kt->rgb_front = (uint8_t*)malloc(640*480*3);
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3) * 6;
kt->t_gamma[i] = v*6*256;
}
if (freenect_init(&kt->ctx, NULL) < 0) {
fprintf(stderr, "freenect_init() failed\n");
return 1;
}
freenect_set_log_level(kt->ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(kt->ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (kt->ctx);
fprintf(stderr, "Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (nr_devices < 1) {
fprintf(stderr, "No devices detected\n");
freenect_shutdown(kt->ctx);
return 1;
}
if (freenect_open_device(kt->ctx, &kt->dev, user_device_number) < 0) {
fprintf(stderr, "Could not open device\n");
freenect_shutdown(kt->ctx);
return 1;
}
int freenect_angle = 0;
freenect_set_tilt_degs(kt->dev,freenect_angle);
freenect_set_led(kt->dev,LED_RED);
freenect_set_depth_callback(kt->dev, depth_cb);
freenect_set_video_callback(kt->dev, rgb_cb);
freenect_set_video_mode(kt->dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(kt->dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_video_buffer(kt->dev, kt->rgb_back);
freenect_start_depth(kt->dev);
freenect_start_video(kt->dev);
return 0;
}
void qKinect::doStartGrabbing()
{
assert(m_app);
if (!m_app)
return;
f_ctx=0;
f_dev=0;
s_grabIndex=0;
if (m_kDlg)
delete m_kDlg;
m_kDlg=0;
s_max_depth_count = 0;
if (s_depth_data)
delete[] s_depth_data;
s_depth_count = 0;
s_depth_data = 0;
s_wDepth = s_hDepth = 0;
if (s_last_rgb_data)
delete[] s_last_rgb_data;
s_last_rgb_data = 0;
s_rgb_count = 0;
s_wRgb = s_hRgb = 0;
if (freenect_init(&f_ctx, NULL) < 0)
{
m_app->dispToConsole("[qKinect] Failed to initialize kinect driver!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices(f_ctx);
m_app->dispToConsole(qPrintable(QString("[qKinect] Number of devices found: %1").arg(nr_devices)));
if (nr_devices < 1)
return;
if (freenect_open_device(f_ctx, &f_dev, 0) < 0)
{
m_app->dispToConsole("[qKinect] Failed to initialize kinect device!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
//test: try to init high resolution mode
//freenect_frame_mode upDepthMode = freenect_find_depth_mode(FREENECT_RESOLUTION_HIGH, FREENECT_DEPTH_11BIT);
//int success = freenect_set_depth_mode(f_dev,upDepthMode);
/*** Depth information ***/
freenect_frame_mode depthMode = freenect_get_current_depth_mode(f_dev);
if (!depthMode.depth_format == FREENECT_DEPTH_11BIT)
{
depthMode = freenect_find_depth_mode(depthMode.resolution, FREENECT_DEPTH_11BIT);
if (freenect_set_depth_mode(f_dev,depthMode)<0)
{
m_app->dispToConsole("[qKinect] Failed to initialiaze depth mode!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
}
if (!getResolution(depthMode.resolution,s_wDepth,s_hDepth))
{
m_app->dispToConsole("[qKinect] Failed to read depth resolution!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
m_app->dispToConsole(qPrintable(QString("[qKinect] Depth resolution: %1 x %2").arg(s_wDepth).arg(s_hDepth)));
s_depth_data = new uint16_t[s_wDepth*s_hDepth];
if (!s_depth_data)
{
m_app->dispToConsole("[qKinect] Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return;
}
s_max_depth_count = 1;
/*** RGB information ***/
bool grabRGB = true;
{
freenect_frame_mode rgbMode = freenect_get_current_video_mode(f_dev);
if (!rgbMode.video_format == FREENECT_VIDEO_RGB || depthMode.resolution != rgbMode.resolution)
{
rgbMode = freenect_find_video_mode(depthMode.resolution, FREENECT_VIDEO_RGB);
if (freenect_set_video_mode(f_dev,rgbMode)<0)
{
m_app->dispToConsole("[qKinect] Can't find a video mode compatible with current depth mode?!");
grabRGB = false;
}
}
//still want to/can grab RGB info?
if (grabRGB)
{
getResolution(rgbMode.resolution,s_wRgb,s_hRgb);
s_last_rgb_data = new uint8_t[s_wRgb*s_hRgb*3];
if (!s_last_rgb_data) //not enough memory for RGB
{
m_app->dispToConsole("[qKinect] Not enough memory to grab RGB info!");
grabRGB = false;
}
else
{
m_app->dispToConsole(qPrintable(QString("[qKinect] RGB resolution: %1 x %2").arg(s_wRgb).arg(s_hRgb)));
}
}
}
int freenect_angle = 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);
if (grabRGB)
freenect_set_video_buffer(f_dev, s_last_rgb_data);
freenect_start_depth(f_dev);
if (s_last_rgb_data)
freenect_start_video(f_dev);
m_kDlg = new ccKinectDlg(m_app->getMainWindow());
if (grabRGB)
m_kDlg->addMode(QString("%1 x %2").arg(s_wDepth).arg(s_hDepth));
else
m_kDlg->grabRGBCheckBox->setChecked(false);
m_kDlg->grabRGBCheckBox->setEnabled(grabRGB);
m_kDlg->grabPushButton->setEnabled(true);
connect(m_kDlg->grabPushButton, SIGNAL(clicked()), this, SLOT(grabCloud()));
connect(m_kDlg, SIGNAL(finished(int)), this, SLOT(dialogClosed(int)));
//m_kDlg->setModal(false);
//m_kDlg->setWindowModality(Qt::NonModal);
m_kDlg->show();
if (!m_timer)
{
m_timer = new QTimer(this);
connect(m_timer, SIGNAL(timeout()), this, SLOT(updateRTView()));
}
m_timer->start(0);
}
