//--------------------------------------------------------------------
bool ofxKinect::open(string serial) {
if(!bGrabberInited) {
ofLogVerbose("ofxKinect") << "open(): cannot open, init not called";
return false;
}
if(!kinectContext.open(*this, serial)) {
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_callback(kinectDevice, &grabDepthFrame);
freenect_set_video_callback(kinectDevice, &grabVideoFrame);
startThread(true, false); // blocking, not verbose
return true;
}
int main(int argc, char **argv)
{
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
if ( network_init() < 0 )
return -1;
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
//freenect_start_depth(f_dev);
//freenect_set_led(f_dev,LED_RED);
while(!die && freenect_process_events(f_ctx) >= 0 ){
char buffer[6];
int n = read(data_child, buffer, 1024);
//printf("n: %d\n", n);
if(n == 6){
if (buffer[0] == 1) { //MOTOR
if (buffer[1] == 1) { //MOVE
int angle;
memcpy(&angle, &buffer[2], sizeof(int));
freenect_set_tilt_degs(f_dev,ntohl(angle));
}
}
}
}
network_close();
printf("-- done!\n");
pthread_exit(NULL);
}
int main(int argc, char *argv[])
{
struct kingrid_info data;
freenect_context *kn;
freenect_device *kn_dev;
int rows = 40, cols = 96; // terminal size
int opt;
sigdata = &data;
data.out_of_range = 0;
data.done = 0;
data.divisions = 6;
data.boxwidth = 10;
data.histrows = 8;
data.frame = 0;
data.zmin = 0.5;
data.zmax = 5.0;
data.disp_mode = STATS;
if(getenv("LINES")) {
rows = atoi(getenv("LINES"));
}
if(getenv("COLUMNS")) {
cols = atoi(getenv("COLUMNS"));
}
// Handle command-line options
while((opt = getopt(argc, argv, "shag:z:Z:")) != -1) {
switch(opt) {
case 's':
// Stats mode
data.disp_mode = STATS;
break;
case 'h':
// Histogram mode
data.disp_mode = HISTOGRAM;
break;
case 'a':
// ASCII art mode
data.disp_mode = ASCII;
break;
case 'g':
// Grid divisions
data.divisions = atoi(optarg);
break;
case 'z':
// Near clipping
data.zmin = atof(optarg);
break;
case 'Z':
// Far clipping
data.zmax = atof(optarg);
break;
default:
fprintf(stderr, "Usage: %s -[sha] [-g divisions] [-zZ distance]\n", argv[0]);
fprintf(stderr, "Use up to one of:\n");
fprintf(stderr, "\ts - Stats mode (default)\n");
fprintf(stderr, "\th - Histogram mode\n");
fprintf(stderr, "\ta - ASCII art mode\n");
fprintf(stderr, "Use any of:\n");
fprintf(stderr, "\tg - Set grid divisions for both dimensions\n");
fprintf(stderr, "\tz - Set near clipping plane in meters for ASCII art mode (default 0.5)\n");
fprintf(stderr, "\tZ - Set far clipping plane in meters for ASCII art mode (default 5.0)\n");
return -1;
}
}
data.boxwidth = (cols - 1) / data.divisions - 3;
if(data.boxwidth < 10) {
data.boxwidth = 10;
}
data.histrows = (rows - 2) / data.divisions - 1;
init_lut(data.depth_lut);
if(signal(SIGINT, intr) == SIG_ERR ||
signal(SIGTERM, intr) == SIG_ERR) {
ERROR_OUT("Error setting signal handlers\n");
return -1;
}
if(freenect_init(&kn, NULL) < 0) {
ERROR_OUT("libfreenect init failed.\n");
return -1;
}
INFO_OUT("Found %d Kinect devices.\n", freenect_num_devices(kn));
if(freenect_num_devices(kn) == 0) {
ERROR_OUT("No Kinect devices present.\n");
return -1;
}
if(freenect_open_device(kn, &kn_dev, 0)) {
ERROR_OUT("Error opening Kinect #0.\n");
return -1;
}
freenect_set_user(kn_dev, &data);
freenect_set_tilt_degs(kn_dev, -5);
freenect_set_led(kn_dev, LED_GREEN);
freenect_set_depth_callback(kn_dev, depth);
freenect_set_depth_format(kn_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(kn_dev);
int last_oor = data.out_of_range;
while(!data.done && freenect_process_events(kn) >= 0) {
if(last_oor != data.out_of_range) {
freenect_set_led(kn_dev, data.out_of_range ? LED_BLINK_RED_YELLOW : LED_GREEN);
last_oor = data.out_of_range;
}
}
freenect_stop_depth(kn_dev);
freenect_set_led(kn_dev, LED_OFF);
freenect_close_device(kn_dev);
freenect_shutdown(kn);
return 0;
}
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);
}
void registerDepthCallback(freenect_device* dev) {
extern void depthCallback(freenect_device*, void*, uint32_t);
freenect_set_depth_callback(dev, depthCallback);
}
int main(int argc, char **argv)
{
cv_depth_mat = cvCreateMat(480, 640, CV_16UC1);
cv_rgb_mat = cvCreateMat(480, 640, CV_8UC3);
int res;
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
//freenect_set_tilt_degs(f_dev,15);
freenect_set_tilt_degs(f_dev,0);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_video_callback(f_dev, rgb_cb);
freenect_set_video_format(f_dev, FREENECT_VIDEO_RGB);
freenect_set_depth_format(f_dev, FREENECT_DEPTH_11BIT);
freenect_start_depth(f_dev);
freenect_start_video(f_dev);
cvNamedWindow("rgb", CV_WINDOW_NORMAL);
cvNamedWindow("depth", CV_WINDOW_NORMAL);
cvNamedWindow("depth_th", CV_WINDOW_NORMAL);
cvNamedWindow("contourWin", CV_WINDOW_NORMAL);
CvMat* cv_depth_threshold_mat = cvCreateMat(480,640, CV_8UC1);
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
// Variables for contour finding
CvSeq* contours = NULL;
CvMemStorage* memStorage = cvCreateMemStorage(0);
IplImage* contour_image = cvCreateImage(cvSize(640,480), 8, 1);
//hist_segment_init();
//feature_extract_init();
optflow_init(cvSize(640,480));
while (!die)
{
cvConvertScale(cv_depth_mat, cv_depth_threshold_mat, 255.0/2048.0, 0);
// 120 ~ 2.7 m
cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 120.0, CV_THRESH_TOZERO_INV);
//cvThreshold( cv_depth_threshold_mat, cv_depth_threshold_mat, 120.0, 255.0, CV_THRESH_BINARY_INV);
// Find contours
/*cvClearMemStorage(memStorage);
cvFindContours( cv_depth_threshold_mat, memStorage, &contours, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE, cvPoint(0,0));
cvZero(contour_image);
// Draw the contours
if (contours)
{
cvDrawContours(contour_image, contours, cvScalarAll(255.0), cvScalarAll(255.0), 1, 1, 8, cvPoint(0,0));
}
cvShowImage("contourWin",contour_image);*/
//extractFeatures(cv_depth_threshold_mat);
//hist_segment(cv_depth_threshold_mat, NULL);
optflow_calculate(cv_depth_threshold_mat, NULL);
cvShowImage("rgb", cv_rgb_mat);
cvShowImage("depth_th", cv_depth_threshold_mat);
char k = cvWaitKey(5);
if( k == 27 ) break;
}
optflow_deinit();
//hist_segment_deinit();
//feature_extract_deinit();
printf("-- done!\n");
cvDestroyWindow("rgb");
cvDestroyWindow("depth");
cvDestroyWindow("depth_th");
cvReleaseMat(&cv_depth_mat);
cvReleaseMat(&cv_rgb_mat);
cvReleaseMat(&cv_depth_threshold_mat);
// Release Contour variables
cvDestroyWindow("contourWin");
pthread_join(freenect_thread, NULL);
pthread_exit(NULL);
}
int main(int argc, char **argv)
{
int res;
freenect_context *f_ctx;
printf("Kinect camera test\n");
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("-- done!\n");
pthread_exit(NULL);
}
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;
}
//----------------------------------------------------------------------------------------------------------------------
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;
}
int main( int argc, char** argv ) {
int res;
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
printf("Kinect camera test\n");
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, 0) < 0) {
printf("Could not open device\n");
return 1;
}
cvNamedWindow( "RGB", CV_WINDOW_AUTOSIZE );
cvMoveWindow( "RGB", 0, 0);
rgbBack = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
rgbFront = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
cvNamedWindow( "Depth", CV_WINDOW_AUTOSIZE );
cvMoveWindow("Depth", FREENECT_FRAME_W, 0);
depthBack = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
depthFront = cvCreateImage(cvSize(FREENECT_FRAME_W, FREENECT_FRAME_H), IPL_DEPTH_8U, 3);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&kinect_thread, NULL, kinect_threadFunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
while(1) {
pthread_mutex_lock(&backbuf_mutex);
{
while (got_frames < 2) {
pthread_cond_wait(&framesReady_cond, &backbuf_mutex);
}
cvConvertImage(rgbBack, rgbFront, CV_BGR2GRAY);
cvConvertImage(depthBack, depthFront, CV_BGR2GRAY);
got_frames = 0;
}
pthread_mutex_unlock(&backbuf_mutex);
cvShowImage("RGB", rgbFront);
cvShowImage("Depth", depthFront);
char c = cvWaitKey(10);
if( c == 27 ) break;
}
cvDestroyWindow( "RGB" );
cvDestroyWindow( "Depth" );
}
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 *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;
}
