/*
* 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 rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
{
bool local_acquire_flag = 0;
pthread_mutex_lock(&acquire_mutex);
local_acquire_flag = !acquired_rgb;
pthread_mutex_unlock(&acquire_mutex);
pthread_mutex_lock(&gl_backbuf_mutex);
// swap buffers
assert (rgb_back == rgb);
rgb_back = rgb_mid;
freenect_set_video_buffer(dev, rgb_back);
if(local_acquire_flag){
saveRGBFrame(rgb_back);
}
rgb_mid = rgb;
got_rgb++;
pthread_cond_signal(&gl_frame_cond);
pthread_mutex_unlock(&gl_backbuf_mutex);
if(acquire_countdown){
acquire_countdown--;
}
}
void rgb_callback(freenect_device *dev, void *pixels, uint32_t timestamp){
t_jit_freenect_grab *x;
x = freenect_get_user(dev);
if(!x)
{
error("Invalid max object supplied in rgb_callback\n");// TODO:should print only in debug mode
return;
}
if (x->is_open)
{
systhread_mutex_lock(x->backbuffer_mutex);
//pthread_mutex_lock(&x->cb_mutex);
/*
x->rgb_data = pixels;
x->rgb_timestamp = timestamp;
x->have_rgb_frames++;
*/
//assert(x->rgb_back == pixels);
x->rgb_back = x->rgb_mid;
freenect_set_video_buffer(dev, x->rgb_back);
x->rgb_mid = (uint8_t*)pixels;
x->got_rgb++;
systhread_mutex_unlock(x->backbuffer_mutex);
}
//pthread_mutex_unlock(&x->cb_mutex);
}
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 ofxKinect::grabVideoFrame(freenect_device *dev, void *video, uint32_t timestamp) {
ofxKinect* kinect = kinectContext.getKinect(dev);
if(kinect->kinectDevice == dev) {
kinect->lock();
swap(kinect->videoPixelsBack,kinect->videoPixelsIntra);
kinect->bNeedsUpdateVideo = true;
kinect->unlock();
freenect_set_video_buffer(kinect->kinectDevice,kinect->videoPixelsBack.getPixels());
}
}
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 rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp)
{
pthread_mutex_lock(&gl_backbuf_mutex);
// swap buffers
assert (rgb_back == rgb);
rgb_back = rgb_mid;
freenect_set_video_buffer(dev, rgb_back);
rgb_mid = (uint8_t*)rgb;
got_rgb++;
pthread_cond_signal(&gl_frame_cond);
pthread_mutex_unlock(&gl_backbuf_mutex);
}
void rgb_cb( freenect_device *dev, void *v_rgb, uint32_t timestamp )
{
pthread_mutex_lock(&gl_mutex);
//assert( rgb_back == rgb);
rgb_back = rgb;
freenect_set_video_buffer(dev, rgb_back);
rgb = (uint8_t*)v_rgb;
for(int i=0; i<W*3; i++)
for(int j=0; j<H; j++)
rgb2d[i][j] = rgb[j*W+i];
printf("asfafsdgsdfgasd");
rgbstarted = true;
pthread_cond_signal(&gl_cond);
pthread_mutex_unlock(&gl_mutex);
}
//--------------------------------------------------------------------
bool ofxKinect::open(int deviceIndex) {
if(!bGrabberInited) {
ofLogWarning("ofxKinect") << "open(): cannot open, init not called";
return false;
}
//we need to find the device id from the device list index.
//as the first device id could be 1 and not 0 when the device list is sorted by serial
int deviceIDFromIndex = -1;
if( deviceIndex > -1 ){
deviceIDFromIndex = kinectContext.getDeviceId(deviceIndex);
}
if(!kinectContext.open(*this, deviceIDFromIndex)) {
return false;
}
if(serial == "0000000000000000") {
bHasMotorControl = false;
//if we do motor control via the audio device ( ie: 1473 or k4w ) and we have firmware uploaded
//then we can do motor stuff! :)
if( kinectDevice->motor_control_with_audio_enabled ){
bHasMotorControl = true;
}else{
ofLogVerbose("ofxKinect") << "open(): device " << deviceId << " does not have motor control";
}
}
else {
bHasMotorControl = true;
}
lastDeviceIndex = deviceIndex;
timeSinceOpen = ofGetElapsedTimef();
bGotDataVideo = false;
bGotDataDepth = false;
bFirstUpdate = true;
freenect_set_user(kinectDevice, this);
freenect_set_depth_buffer(kinectDevice, depthPixelsRawBack.getData());
freenect_set_video_buffer(kinectDevice, videoPixelsBack.getData());
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabVideoFrame);
startThread(); // blocking, not verbose
return true;
}
void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp) {
sendMsgToJava(NULL, "New RGB Frame");
if(need_write)
writeToFile(pathToFile, (uint8_t*) rgb);
pthread_mutex_lock(&pth_backbuf_mutex);
// swap buffers
assert(rgb_back == rgb);
rgb_back = rgb_mid;
freenect_set_video_buffer(dev, rgb_back);
rgb_mid = (uint8_t*) rgb;
pthread_cond_signal(&pth_frame_cond);
pthread_mutex_unlock(&pth_backbuf_mutex);
}
// 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 *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 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);
}
//--------------------------------------------------------------------
bool ofxKinect::open(int id) {
if(!bGrabberInited) {
ofLogWarning("ofxKinect") << "open(): cannot open, init not called";
return false;
}
if(!kinectContext.open(*this, id)) {
return false;
}
if(serial == "0000000000000000") {
bHasMotorControl = false;
//if we do motor control via the audio device ( ie: 1473 or k4w ) and we have firmware uploaded
//then we can do motor stuff! :)
if( kinectDevice->motor_control_with_audio_enabled ){
bHasMotorControl = true;
}else{
ofLogVerbose("ofxKinect") << "open(): device " << deviceId << " does not have motor control";
}
}
else {
bHasMotorControl = true;
}
lastDeviceId = deviceId;
timeSinceOpen = ofGetElapsedTimef();
bGotData = false;
freenect_set_user(kinectDevice, this);
freenect_set_depth_buffer(kinectDevice, depthPixelsRawBack.getPixels());
freenect_set_video_buffer(kinectDevice, videoPixelsBack.getPixels());
freenect_set_depth_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabVideoFrame);
startThread(true, false); // blocking, not verbose
return true;
}
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;
}
}
void rgb_cb(freenect_device *dev, void *rgb, uint32_t timestamp){
//static unsigned char out_rgb[640*480*3];
pthread_mutex_lock(&G_kt.mutex);
// swap buffers
assert (G_kt.rgb_back == rgb);
G_kt.rgb_back = G_kt.rgb_mid;
freenect_set_video_buffer(dev, G_kt.rgb_back);
G_kt.rgb_mid = (uint8_t*)rgb;
/*int i;
for (i=0; i<640*480*3; i++){
out_rgb[i] = G_kt.rgb_mid[i];
}*/
FILE* fd = fopen("image","a+");
assert( fd );
flock( fileno(fd), LOCK_EX );
ftruncate( fileno(fd), 0 );
fprintf(fd, "class=Mat:Cv\n");
fprintf(fd, "cols=640\n");
fprintf(fd, "rows=480\n");
fprintf(fd, "step=%d\n", 640*3);
fprintf(fd, "type=%d\n", 16);
fprintf(fd, "data=<%d|", 640*480*3);
fwrite( rgb, sizeof(uint8_t), 640*480*3, fd);
flock( fileno(fd), LOCK_UN );
fclose(fd);
pthread_mutex_unlock(&G_kt.mutex);
}
void kinect_video_callback(freenect_device* dev, void* rgb, uint32_t timestamp) {
Kinect* kinect = static_cast<Kinect*>(freenect_get_user(dev));
if(!kinect) {
printf("Error: cannot get the Kinect* user ptr.\n");
::exit(EXIT_FAILURE);
}
if(rgb != kinect->rgb_back) {
printf("Error: wrong rgb pointer.\n");
}
kinect->rgb_back = kinect->rgb_mid;
freenect_set_video_buffer(dev, kinect->rgb_back);
kinect->rgb_mid = (uint8_t*)rgb;
uv_mutex_lock(&kinect->mutex);
{
memcpy(kinect->rgb_front, kinect->rgb_mid, kinect->nbytes_rgb);
kinect->has_new_rgb = true;
}
uv_mutex_unlock(&kinect->mutex);
}
void *freenect_threadfunc(void *env) {
int accelCount = 0;
int status = (*gJavaVM)->AttachCurrentThread(gJavaVM, &env, NULL);
freenect_set_tilt_degs(f_dev, current_tilt_angle);
freenect_set_led(f_dev, current_led_option);
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);
int depthS = freenect_start_depth(f_dev);
int videoS = freenect_start_video(f_dev);
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG,
"Depth start = %d Video start = %d ", depthS, videoS);
sprintf(my_log, "Depth start = %d Video start = %d ", depthS, videoS);
sendMsgToJava(env, my_log);
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, "Threaded...\n");
sendMsgToJava(env, "Threaded...");
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);
//
freenect_set_led(f_dev, current_led_option);
freenect_set_tilt_degs(f_dev, current_tilt_angle);
}
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;
}
}
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, "shutting down streams...");
int statusD = (*gJavaVM)->DetachCurrentThread(gJavaVM);
freenect_set_led(f_dev, LED_OFF);
freenect_stop_depth(f_dev);
freenect_stop_video(f_dev);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
closed - 1;
return NULL;
}
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);
}
