//send depth ARGB to client
void sendRawDepth(){
uint32_t ts, x, y, i, j;
freenect_sync_get_depth(&buf_depth_temp, &ts, 0, FREENECT_DEPTH_11BIT);
uint16_t *depth = (uint16_t*) buf_depth_temp;
freenect_frame_mode depth_mode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT);
uint16_t *tmp_depth = (uint16_t*) malloc(depth_mode.bytes);
unsigned char *char_depth = (unsigned char *) malloc(depth_mode.bytes);
if(_depth_mirrored){ //MIRROR DEPTH DATA
for(x = 0; x < depth_mode.width; x++){
for(y = 0; y < depth_mode.height; y++){
i = x + (y * depth_mode.width);
j = (depth_mode.width - x - 1) + (y * depth_mode.width);
tmp_depth[i] = depth[j];
}
}
depth = tmp_depth;
}
for (i=0; i<640 * 480; i++) {
memcpy(char_depth + (i*2), &depth[i], 2);
}
int n = freenect_network_sendMessage(0, 1, char_depth, depth_mode.bytes);
if (n < 0)
{
printf("Error sending raw depth\n");
client_connected = 0;
}
free(char_depth);
free(tmp_depth);
}
//---------------------------------------------------------------------------
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";
}
//send depth ARGB to client
void sendDepth(){
int n;
uint32_t ts, x, y, i, j;
freenect_sync_get_depth(&buf_depth_temp, &ts, 0, FREENECT_DEPTH_11BIT);
uint16_t *depth = (uint16_t*) buf_depth_temp;
freenect_frame_mode depth_mode = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT);
//uint16_t *tmp_depth = (uint16_t*) malloc(FREENECT_DEPTH_11BIT_SIZE);
uint16_t *tmp_depth = (uint16_t*) malloc(depth_mode.bytes);
if(_depth_mirrored){ //MIRROR DEPTH DATA
for(x = 0; x < depth_mode.width; x++){
for(y = 0; y < depth_mode.height; y++){
i = x + (y * depth_mode.width);
j = (depth_mode.width - x - 1) + (y * depth_mode.width);
tmp_depth[i] = depth[j];
}
}
depth = tmp_depth;
}
for (i=0; i< depth_mode.width * depth_mode.height; i++) {
if(_depth_compression != 0) {
buf_depth[3 * i + 0] = 0x00;
buf_depth[3 * i + 1] = 0x00;
buf_depth[3 * i + 2] = 0x00;
} else {
buf_depth[4 * i + 0] = 0x00;
buf_depth[4 * i + 1] = 0x00;
buf_depth[4 * i + 2] = 0x00;
buf_depth[4 * i + 3] = 0xFF;
}
if(depth[i] < _max_depth && depth[i] > _min_depth){
unsigned char l = 0xFF - ((depth[i] - _min_depth) & 0xFF);
if(_depth_compression != 0) {
buf_depth[3 * i + 0] = l;
buf_depth[3 * i + 1] = l;
buf_depth[3 * i + 2] = l;
} else {
buf_depth[4 * i + 0] = l;
buf_depth[4 * i + 1] = l;
buf_depth[4 * i + 2] = l;
buf_depth[4 * i + 3] = 0xFF;
}
}
}
if(_depth_compression != 0) {
unsigned char *compressed_buff = (unsigned char *)malloc(AS3_BITMAPDATA_LEN);
unsigned long len = 0;
RGB_2_JPEG(buf_depth, &compressed_buff, &len, _depth_compression);
n = freenect_network_sendMessage(0, 0, compressed_buff, (int)len);
free(compressed_buff);
} else {
n = freenect_network_sendMessage(0, 0, buf_depth, AS3_BITMAPDATA_LEN);
}
if (n < 0)
{
printf("Error sending depth\n");
client_connected = 0;
}
free(tmp_depth);
}
bool initCamera()
{
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return false;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (nr_devices < 1)
return false;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return false;
}
int accelCount = 0;
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_user(f_dev, (void*)this);
}
/*
* Class: org_ensor_robots_sensors_kinect_Device
* Method: nativeStartDepth
* Signature: (I)V
*/
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Device_nativeStartDepth
(JNIEnv *aJNIEnv, jobject aThisObject, jint aDeviceId)
{
pthread_mutex_lock(&mutex);
freenect_set_depth_mode(f_devices[aDeviceId].f_dev,
freenect_find_depth_mode(
FREENECT_RESOLUTION_MEDIUM,
FREENECT_DEPTH_11BIT
)
);
f_devices[aDeviceId].depthbuffersize = 640 * 480 * 2;
f_devices[aDeviceId].f_depth_buffer = malloc(f_devices[aDeviceId].depthbuffersize);
freenect_set_depth_buffer(
f_devices[aDeviceId].f_dev,
f_devices[aDeviceId].f_depth_buffer
);
f_devices[aDeviceId].depthbuffervalue =
(*aJNIEnv)->NewBooleanArray(aJNIEnv, f_devices[aDeviceId].depthbuffersize);
f_devices[aDeviceId].depthbuffervalue = (*aJNIEnv)->NewGlobalRef(aJNIEnv,
f_devices[aDeviceId].depthbuffervalue
);
freenect_set_depth_callback(f_devices[aDeviceId].f_dev, depth_cb);
pthread_mutex_unlock(&mutex);
freenect_start_depth(f_devices[aDeviceId].f_dev);
}
MyFreenectDevice(freenect_context *_ctx, int _index):
Freenect::FreenectDevice(_ctx, _index),
depth(freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_DEPTH_REGISTERED).bytes),
m_buffer_video(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_VIDEO_RGB).bytes),
m_new_rgb_frame(false), m_new_depth_frame(false)
{
}
void depth_cb_ffmpeg(freenect_device *dev, void *depth, uint32_t timestamp)
{
double cur_time = get_time();
fprintf(index_fp, "d-%f-%u\n", cur_time, timestamp);
dump_ffmpeg_pad16(depth_stream, timestamp, depth,
freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,
FREENECT_DEPTH_11BIT).bytes);
}
int main() {
freenect_context *f_ctx;
freenect_device *f_dev;
if (freenect_init(&f_ctx, NULL) < 0) {
fprintf(stderr, "freenect_init() failed\n");
return 1;
}
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
if (freenect_num_devices(f_ctx) < 1) {
fprintf(stderr, "no devices found\n");
freenect_shutdown(f_ctx);
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
fprintf(stderr, "can't open device\n");
freenect_shutdown(f_ctx);
return 1;
}
depth_image = image_create(640, 480);
freenect_set_led(f_dev, LED_GREEN);
freenect_set_depth_callback(f_dev, capture_depth_image);
freenect_set_depth_mode(f_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_start_depth(f_dev);
if (signal(SIGINT, handle_interrupt) == SIG_IGN) {
signal(SIGINT, SIG_IGN);
}
if (signal(SIGTERM, handle_interrupt) == SIG_IGN) {
signal(SIGTERM, SIG_IGN);
}
fprintf(stdout, "\x1B[2J");
while (running && freenect_process_events(f_ctx) >= 0) {
struct winsize w;
ioctl(STDOUT_FILENO, TIOCGWINSZ, &w);
fprintf(stdout, "\x1B[1;1H");
draw_depth_image(stdout, w.ws_col, w.ws_row - 1);
}
freenect_stop_depth(f_dev);
freenect_set_led(f_dev, LED_OFF);
freenect_close_device(f_dev);
freenect_shutdown(f_ctx);
image_destroy(depth_image);
return 0;
}
/*!
* \brief constructor: reserve memory and set initial flags.
* \param [out] _ctx return usb context to communicate with kinect.
* \param [in] _index selected kinect index.
*/
Apikinect::Apikinect(freenect_context *_ctx, int _index)
: Freenect::FreenectDevice(_ctx, _index),
m_buffer_video(freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes),
m_buffer_depth(freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_REGISTERED).bytes / 2),
m_new_rgb_frame(false), m_new_depth_frame(false)
{
myself = const_cast<freenect_device*>(this->getDevice());//las vueltas que da la vida :o)
setDepthFormat(FREENECT_DEPTH_REGISTERED);
}
FreenectDevice(freenect_context *_ctx, int _index)
: m_video_resolution(FREENECT_RESOLUTION_MEDIUM), m_depth_resolution(FREENECT_RESOLUTION_MEDIUM)
{
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_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(m_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
freenect_set_depth_callback(m_dev, freenect_depth_callback);
freenect_set_video_callback(m_dev, freenect_video_callback);
}
//---------------------------------------------------------------------------
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);
}
FreenectDevice(freenect_context *_ctx, char *_serial)
: m_video_resolution(FREENECT_RESOLUTION_MEDIUM), m_depth_resolution(FREENECT_RESOLUTION_MEDIUM)
{
if(freenect_open_device_by_camera_serial(_ctx, &m_dev, _serial)) throw std::runtime_error("Cannot open Kinect");
///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_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(m_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, /*FREENECT_DEPTH_11BIT*/FREENECT_DEPTH_REGISTERED));//FREENECT_DEPTH_REGISTERED
freenect_set_depth_callback(m_dev, freenect_depth_callback);
freenect_set_video_callback(m_dev, freenect_video_callback);
}
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_mode(kinect->dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, fmt));
freenect_set_depth_buffer(kinect->dev, kinect->depth.bufs[2]);
freenect_start_depth(kinect->dev);
return 0;
}
void setDepthFormat(freenect_depth_format requested_format, freenect_resolution requested_resolution = FREENECT_RESOLUTION_MEDIUM) {
if (requested_format != m_depth_format || requested_resolution != m_depth_resolution) {
freenect_stop_depth(m_dev);
freenect_frame_mode mode = freenect_find_depth_mode(requested_resolution, requested_format);
if (!mode.is_valid) throw std::runtime_error("Cannot set depth format: invalid mode");
if (freenect_set_depth_mode(m_dev, mode) < 0) throw std::runtime_error("Cannot set depth format");
freenect_start_depth(m_dev);
m_depth_format = requested_format;
m_depth_resolution = requested_resolution;
}
}
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 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);
}
// 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;
}
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 *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);
}
static int alloc_buffer_ring_depth(freenect_depth_format fmt, buffer_ring_t *buf)
{
int sz, i;
switch (fmt) {
case FREENECT_DEPTH_11BIT:
case FREENECT_DEPTH_10BIT:
case FREENECT_DEPTH_11BIT_PACKED:
case FREENECT_DEPTH_10BIT_PACKED:
case FREENECT_DEPTH_REGISTERED:
case FREENECT_DEPTH_MM:
sz = freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, fmt).bytes;
break;
default:
printf("Invalid depth format %d\n", fmt);
return -1;
}
for (i = 0; i < 3; ++i)
buf->bufs[i] = malloc(sz);
buf->timestamp = 0;
buf->valid = 0;
buf->fmt = fmt;
return 0;
}
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;
}
int freenect_process_events(freenect_context *ctx)
{
/* This is where the magic happens. We read 1 update from the index
per call, so this needs to be called in a loop like usual. If the
index line is a Depth/RGB image the provided callback is called. If
the index line is accelerometer data, then it is used to update our
internal state. If you query for the accelerometer data you get the
last sensor reading that we have. The time delays are compensated as
best as we can to match those from the original data and current run
conditions (e.g., if it takes longer to run this code then we wait less).
*/
if (!index_fp)
open_index();
char type;
double record_cur_time;
unsigned int timestamp, data_size;
char *data = NULL;
if (parse_line(&type, &record_cur_time, ×tamp, &data_size, &data))
return -1;
// Sleep an amount that compensates for the original and current delays
// playback_ is w.r.t. the current time
// record_ is w.r.t. the original time period during the recording
if (record_prev_time != 0. && playback_prev_time != 0.)
sleep_highres((record_cur_time - record_prev_time) - (get_time() - playback_prev_time));
record_prev_time = record_cur_time;
switch (type) {
case 'd':
if (cur_depth_cb && depth_running) {
void *cur_depth = skip_line(data);
if (depth_buffer) {
memcpy(depth_buffer, cur_depth, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT).bytes);
cur_depth = depth_buffer;
}
cur_depth_cb(fake_dev, cur_depth, timestamp);
}
break;
case 'r':
if (cur_rgb_cb && rgb_running) {
void *cur_rgb = skip_line(data);
if (rgb_buffer) {
memcpy(rgb_buffer, cur_rgb, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB).bytes);
cur_rgb = rgb_buffer;
}
cur_rgb_cb(fake_dev, cur_rgb, timestamp);
}
break;
case 'a':
if (data_size == sizeof(state)) {
memcpy(&state, data, sizeof(state));
} else if (!already_warned) {
already_warned = 1;
printf("\n\nWarning: Accelerometer data has an unexpected"
" size [%u] instead of [%u]. The acceleration "
"and tilt data will be substituted for dummy "
"values. This data was probably made with an "
"older version of record (the upstream interface "
"changed).\n\n",
data_size, (unsigned int)sizeof state);
}
break;
}
free(data);
playback_prev_time = get_time();
return 0;
}
/*
* Get a depth frame mode.
*/
freenect_frame_mode* find_depth_mode_freenect(uint32_t res,
uint32_t fmt){
freenect_frame_mode* mode = malloc(sizeof (*mode));
*mode = freenect_find_depth_mode(res,fmt);
return mode;
};
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);
}
freenect_frame_mode freenect_get_depth_mode(int mode_num)
{
return freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT);
}
//----------------------------------------------------------------------------------------------------------------------
bool KinectInterface::init()
{
// first see if we can init the kinect
if (freenect_init(&m_ctx, NULL) < 0)
{
qDebug()<<"freenect_init() failed\n";
exit(EXIT_FAILURE);
}
/// set loggin level make this programmable at some stage
freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
/// see how many devices we have
int nr_devices = freenect_num_devices (m_ctx);
qDebug()<<"Number of devices found: "<<nr_devices<<"\n";
if(nr_devices < 1)
{
//delete s_instance;
//s_instance = 0;
return false;
}
/// now allocate the buffers so we can fill them
m_userDeviceNumber = 0;
// grab the buffer size and store for later use
m_resolutionRGBBytes=freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_VIDEO_RGB).bytes;
m_bufferDepth=cvCreateMat(480,640,CV_8UC3);
//m_bufferVideo.resize(m_resolutionRGBBytes);
m_bufferVideo = cvCreateMat(480,640,CV_8UC3);
// m_nextBuffer = cvCreateMat(480,640,CV_8UC1);
// m_prevBuffer = cvCreateMat(480,640,CV_8UC1);
// m_diffBuffer = cvCreateMat(480,640,CV_8UC1);
m_resolutionDepthBytes=freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM,FREENECT_DEPTH_11BIT).bytes;
//m_bufferDepthRaw.resize(m_resolutionDepthBytes);
m_bufferDepthRaw16=cvCreateMat(480,640,CV_8UC1);
m_bufferDepthRaw=cvCreateMat(480,640,CV_8UC1);
// m_originalFrameDepth=NULL;
m_gamma.resize(2048);
/// open the device at present hard coded to device 0 as I only
/// have 1 kinect
/// \todo make this support multiple devices at some stage
if (freenect_open_device(m_ctx, &m_dev, m_userDeviceNumber) < 0)
{
qDebug()<<"Could not open device\n";
exit(EXIT_FAILURE);
}
/// build the gamma table used for the depth to rgb conversion
/// taken from the demo programs
// for (int i=0; i<2048; ++i)
// {
// float v = i/2048.0;
// v = std::pow(v, 3)* 6;
// m_gamma[i] = v*6*256;
// }
// from opencv imaging imformation wiki page http://openkinect.org/wiki/Imaging_Information
const float k1 = 1.1863;
const float k2 = 2842.5;
const float k3 = 0.1236;
const float offset = 0.037;
float depth = 0;
for (size_t i=0; i<2048; i++)
{
depth = k3 * tanf(i/k2 + k1) - offset;
m_gamma[i] = depth;
}
/// init our flags
m_newRgbFrame=false;
m_newDepthFrame=false;
m_deviceActive=true;
m_threshValue = 100;
// set our video formats to RGB by default
/// @todo make this more flexible at some stage
freenect_set_video_mode(m_dev, freenect_find_video_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_VIDEO_RGB));
freenect_set_depth_mode(m_dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT));
// deprecated
//freenect_set_video_format(m_dev, FREENECT_VIDEO_RGB);
//freenect_set_depth_format(m_dev, FREENECT_DEPTH_11BIT);
/// hook in the callbacks
freenect_set_depth_callback(m_dev, depthCallback);
freenect_set_video_callback(m_dev, videoCallback);
// start the video and depth sub systems
startVideo();
startDepth();
// set the thread to be active and start
m_process = new QKinectProcessEvents(m_ctx);
m_process->setActive();
m_process->start();
m_depthLower = 0.02;
m_depthHigher = 1.02; // has to be just above the table (in meteres)
//m_selectedBoxCoords = NULL;
m_selectedBoxCoords = cv::Rect(0,0,0,0);
m_toggleTracking = false;
m_setBounds = false;
return true;
}
/**
* Reallocate the depth buffer if the depth format or resolution changes
*/
void allocateBufferDepth(
ImageBuffer& buffer,
const freenect_depth_format& format,
const freenect_resolution& resolution,
const freenect_registration& registration) {
// Obtain a lock on the buffer. This is mostly for debugging, as allocate
// buffer should only be called when the buffer is not being used by the
// freenect thread
boost::lock_guard<boost::mutex> buffer_lock(buffer.mutex);
// Deallocate the buffer incase an exception happens (the buffer should no
// longer be valid)
buffer.image_buffer.reset();
switch (format) {
case FREENECT_DEPTH_11BIT:
case FREENECT_DEPTH_10BIT:
case FREENECT_DEPTH_11BIT_PACKED:
case FREENECT_DEPTH_10BIT_PACKED:
case FREENECT_DEPTH_REGISTERED:
case FREENECT_DEPTH_MM:
switch (resolution) {
case FREENECT_RESOLUTION_MEDIUM:
buffer.metadata =
freenect_find_depth_mode(resolution, format);
if (!buffer.metadata.is_valid) {
throw std::runtime_error("libfreenect: Invalid depth fmt, res: " +
boost::lexical_cast<std::string>(format) + "," +
boost::lexical_cast<std::string>(resolution));
}
break;
default:
throw std::runtime_error("libfreenect: Invalid depth resolution: " +
boost::lexical_cast<std::string>(resolution));
}
break;
default:
throw std::runtime_error("libfreenect: Invalid depth format: " +
boost::lexical_cast<std::string>(format));
}
// All is good, reallocate the buffer and calculate other pieces of info
buffer.image_buffer.reset(new unsigned char[buffer.metadata.bytes]);
switch(format) {
case FREENECT_DEPTH_11BIT:
case FREENECT_DEPTH_10BIT:
case FREENECT_DEPTH_11BIT_PACKED:
case FREENECT_DEPTH_10BIT_PACKED:
case FREENECT_DEPTH_MM:
buffer.focal_length =
getDepthFocalLength(registration, buffer.metadata.width);
buffer.is_registered = false;
break;
case FREENECT_DEPTH_REGISTERED:
buffer.focal_length = getRGBFocalLength(buffer.metadata.width);
buffer.is_registered = true;
break;
default:
throw std::runtime_error("libfreenect: shouldn't reach here");
}
}
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;
}
