int main(int argc, char **argv)
{
int res;
depth_mid = (uint8_t*)malloc(640*480*3);
depth_front = (uint8_t*)malloc(640*480*3);
rgb_back = (uint8_t*)malloc(640*480*3);
rgb_mid = (uint8_t*)malloc(640*480*3);
rgb_front = (uint8_t*)malloc(640*480*3);
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);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | 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 (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1) {
freenect_shutdown(f_ctx);
return 1;
}
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
freenect_shutdown(f_ctx);
return 1;
}
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
freenect_shutdown(f_ctx);
return 1;
}
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
return 0;
}
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);
}
int main( int argc, char **argv )
{
//depth_gl = (uint8_t*)malloc(640*480*3);
if( freenect_init( &f_context, NULL ) < 0 )
{
printf( "Freenect initialization failed.\n" );
return 1;
}
freenect_set_log_level( f_context, FREENECT_LOG_DEBUG );
freenect_select_subdevices( f_context, ( freenect_device_flags )( FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA ) );
if( freenect_num_devices ( f_context ) < 1 )
{
printf( "No devices found. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
if( freenect_open_device( f_context, &f_device, 0 ) <0 )
{
printf( "Couldn't open device. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
if( pthread_create( &f_thread, NULL, f_threadfunc, NULL ) )
{
printf( "Thread creation failed. Shutting down...\n" );
freenect_shutdown( f_context );
return 1;
}
glutInit( &argc, argv );
gl_threadfunc( NULL );
return 0;
}
KinectControl::KinectControl() {
printf("\nKinectControl Init\n");
errorCode = 0;
isInited = false;
if (kinectControlRef != NULL) {
errorCode = 1;
errorString = "KinectControl Already Started.";
printf("%s.\n",errorString.c_str());
return;
}
for (int i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
kinectControlRef = this;
freenect_angle = 0;
user_device_number = -1;
requested_format = FREENECT_VIDEO_RGB;
current_format = FREENECT_VIDEO_RGB;
//depth_mid = (uint8_t*)malloc(640*480*3);
depth_front = (uint8_t*)malloc(640*480*3);
rgb_back = (uint8_t*)malloc(640*480*3);
rgb_mid = (uint8_t*)malloc(640*480*3);
rgb_front = (uint8_t*)malloc(640*480*3);
depth_mid = NULL;
memset(depth_front,0,(640*480*3));
memset(rgb_back,0,(640*480*3));
memset(rgb_mid,0,(640*480*3));
memset(rgb_front,0,(640*480*3));
if (freenect_init(&f_ctx, NULL) < 0) {
errorCode = 2;
errorString = "freenect_init() failed.";
printf("%s\n",errorString.c_str());
return;
} else {
printf("freenect_init() OK\n");
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(f_ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
// check if there were no device found.
if (nr_devices < 0) {
freenect_shutdown(f_ctx);
errorCode = 3;
errorString = "No freenect devices found!";
printf("%s\n", errorString.c_str());
}
}
Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
{
WriteMessage("Using libfreenect v" + to_string(PROJECT_VER));
freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO
DriverServices = &getServices();
}
jboolean openSync(JNIEnv* env) {
__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, "Start Sync");
sendMsgToJava(NULL, "Starting Sync...");
int res;
depth_mid = (uint8_t*) malloc(640 * 480 * 4);
depth_front = (uint8_t*) malloc(640 * 480 * 3);
rgb_back = (uint8_t*) malloc(640 * 480 * 3);
rgb_mid = (uint8_t*) malloc(640 * 480 * 3);
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;
t_gamma[i] = v * 6 * 256;
}
//init kinect context
if (freenect_init(&f_ctx, NULL) < 0) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG,
"Kinect open sync failed\n");
sendMsgToJava(env, "Kinect open sync failed");
return FALSE;
}
//settup
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(
f_ctx,
(freenect_device_flags) (FREENECT_DEVICE_MOTOR
| FREENECT_DEVICE_CAMERA));
//get num dev
int nr_devices = freenect_num_devices(f_ctx);
sprintf(my_log, "Number of Devices found %d\n", nr_devices);
sendMsgToJava(env, my_log);
__android_log_print(ANDROID_LOG_INFO, LOG_TAG, my_log);
int user_device_number = 0;
if (nr_devices < 1)
return FALSE;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG, "Cannot Open device\n");
sendMsgToJava(env, "Cannot Open device");
return FALSE;
}
//create thread
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, env);
if (res) {
__android_log_print(ANDROID_LOG_ERROR, LOG_TAG,
"pthread_create failed...\n");
sendMsgToJava(env, "pthread_create failed...");
return FALSE;
}
return TRUE;
}
bool Kinect::setup() {
if(ctx) {
printf("Error: the freenect context has been setup already.\n");
return false;
}
if(freenect_init(&ctx, NULL) < 0) {
printf("Error: cannot init libfreenect.\n");
return false;
}
freenect_set_log_level(ctx, FREENECT_LOG_DEBUG);
int ndevices = freenect_num_devices(ctx);
if(ndevices < 1) {
printf("Error: cannot find a kinect. @todo cleanup mem.\n");
freenect_shutdown(ctx);
ctx = NULL;
return false;
}
printf("Number of found kinect devices: %d\n", ndevices);
freenect_select_subdevices(ctx, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
//freenect_select_subdevices(ctx, (freenect_device_flags)( FREENECT_DEVICE_CAMERA));
int devnum = 0;
if(freenect_open_device(ctx, &device, devnum) < 0) {
printf("Error: cannot open device: %d\n", devnum);
freenect_shutdown(ctx);
ctx = NULL;
return false;
}
freenect_set_user(device, this);
uint32_t w = 640;
uint32_t h = 480;
uint32_t nbytes = w * h * 3;
nbytes_rgb = nbytes;
rgb_back = new uint8_t[nbytes];
rgb_mid = new uint8_t[nbytes];
rgb_front = new uint8_t[nbytes];
depth_back = new uint8_t[nbytes];
depth_mid = new uint8_t[nbytes];
depth_front = new uint8_t[nbytes];
uv_mutex_lock(&mutex);
must_stop = false;
uv_mutex_unlock(&mutex);
uv_thread_create(&thread, kinect_thread, this);
return true;
}
int main(int argc, char** argv) {
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
freenect_select_subdevices(f_ctx, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1) {
freenect_shutdown(f_ctx);
return 1;
}
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
freenect_shutdown(f_ctx);
return 1;
}
state.max_samples = 256 * 60;
state.current_idx = 0;
state.buffers[0] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[1] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[2] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[3] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
memset(state.buffers[0], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[1], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[2], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[3], 0, state.max_samples * sizeof(int32_t));
freenect_set_user(f_dev, &state);
freenect_set_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
int res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
freenect_shutdown(f_ctx);
return 1;
}
ros::init(argc, argv, "kinect_audio");
MicView mv;
mv.spin();
return 0;
}
int main(int argc, char **argv)
{
int res;
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;
}
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
return 0;
}
int main(int argc, char **argv)
{
int i, res;
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;
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
while(!die && freenect_process_events(f_ctx) >= 0 );
return 0;
}
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;
}
freenect_context* init_kinect() {
freenect_context *context;
if (freenect_init(&context, NULL) < 0) {
printf("failed to initialize kinect\n");
return NULL;
} else {
printf("initialized kinect\n");
if (getenv("DEBUG")) {
freenect_set_log_level(context, FREENECT_LOG_DEBUG);
}
freenect_select_subdevices(context, (freenect_device_flags)FREENECT_DEVICE_CAMERA);
return context;
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "kinect_motor");
freenect_context * context = 0;
if (freenect_init(&context, NULL) < 0) {
ROS_ERROR("freenect_init() failed");
return 1;
}
freenect_set_log_level(context, FREENECT_LOG_DEBUG);
freenect_select_subdevices(context, (freenect_device_flags)(FREENECT_DEVICE_MOTOR));
int devicesCout = freenect_num_devices (context);
ROS_INFO("Number of devices found: %d", devicesCout);
if (devicesCout < 1) {
freenect_shutdown(context);
return 1;
}
ROS_INFO("Connecting to device: 1");
if (freenect_open_device(context, &device, 0) < 0) {
printf("Could not open device\n");
freenect_shutdown(context);
return 1;
}
freenect_set_tilt_degs(device, 0);
ros::NodeHandle nodeHandleForAngle;
ros::Subscriber angleSubscriber = nodeHandleForAngle.subscribe("angle", 10, Angle);
ros::NodeHandle nodeHandleForLed;
ros::Subscriber ledSubscriber = nodeHandleForLed.subscribe("led", 10, Led);
ros::spin();
freenect_close_device(device);
freenect_shutdown(context);
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "kinect_audio");
freenect_context * contex;
if (freenect_init(&contex, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(contex, FREENECT_LOG_SPEW);
freenect_select_subdevices(contex, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices(contex);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1) {
freenect_shutdown(contex);
return 1;
}
freenect_device * device;
int deviceNumber = 0;
if (freenect_open_device(contex, &device, deviceNumber) < 0) {
printf("Could not open device\n");
freenect_shutdown(contex);
return 1;
}
freenect_set_audio_in_callback(device, callback);
freenect_start_audio(device);
signal(SIGINT, cleanup);
audioPublisher = ros::NodeHandle().advertise<kinect_audio::audio>("audio", 10);
while(!die && freenect_process_events(contex) >= 0) {
// If we did anything else, it might go here.
// Alternately, we might split off another thread
// to do this loop while the main thread did something
// interesting.
}
freenect_shutdown(contex);
return 0;
}
int main (int argc, char **argv) {
// Initalize the ROS node
ros::init(argc, argv, "kinect_base_node");
ros::NodeHandle n;
// Initalize the Freenect Context
if (freenect_init (&f_ctx, NULL) < 0) {
ROS_INFO("freenect_init() failed\n");
return 1;
}
freenect_set_log_level (f_ctx, FREENECT_LOG_INFO);
// Scan for kinect devices
int nr_devices = freenect_num_devices (f_ctx);
ROS_INFO("Number of devices found: %d\n", nr_devices);
// Get the device number
if (argc > 1) user_device_number = atoi (argv[1]);
if (nr_devices < 1) return 1;
// Calculate the kinect node base name
std::stringstream kinect_node_base;
kinect_node_base << "/kinect_base_node/" << user_device_number << "/";
// Open the base portion of the Kinect
if (freenect_open_device (f_ctx, &f_dev, user_device_number) < 0) {
ROS_INFO("Could not open device\n");
return 1;
}
// Get the defaults
double tiltDefaultPosition = 0.0;
double imuDefaultDuration = 1.0;
int ledDefaultState = LED_BLINK_GREEN;
n.getParam(kinect_node_base.str() + "tilt", tiltDefaultPosition);
n.getParam(kinect_node_base.str() + "led", ledDefaultState);
n.getParam(kinect_node_base.str() + "imuDuration", imuDefaultDuration);
// Set the default kinect state
freenect_set_tilt_degs(f_dev, tiltDefaultPosition);
freenect_set_led(f_dev, (freenect_led_options) ledDefaultState);
// Create the provided services
ros::Subscriber subTilt = n.subscribe(kinect_node_base.str() + "tilt", 1000, tilt_received_callback);
ros::Subscriber subLED = n.subscribe(kinect_node_base.str() + "led", 1000, led_received_callback);
imu_publisher = n.advertise<sensor_msgs::Imu>(kinect_node_base.str() + "imu", 1000);
ros::Timer imu_publish_timer = n.createTimer(ros::Duration(imuDefaultDuration), imu_publish_data);
ros::spin();
return 0;
}
int audio_setup(){
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return -1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
freenect_select_subdevices(f_ctx, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1)
return -1;
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_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
return 0;
}
//---------------------------------------------------------------------------
bool ofxKinectContext::init() {
if(freenect_init(&kinectContext, NULL) < 0) {
ofLogError("ofxKinect") << "init(): freenect_init failed";
bInited = false;
return false;
}
#ifdef OFX_KINECT_EXTRA_FW
freenect_set_fw_address_nui(kinectContext, ofxKinectExtras::getFWData1473(), ofxKinectExtras::getFWSize1473());
freenect_set_fw_address_k4w(kinectContext, ofxKinectExtras::getFWDatak4w(), ofxKinectExtras::getFWSizek4w());
#endif
freenect_set_log_level(kinectContext, FREENECT_LOG_WARNING);
freenect_select_subdevices(kinectContext, (freenect_device_flags)(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
bInited = true;
ofLogVerbose("ofxKinect") << "context inited";
buildDeviceList();
listDevices(true);
return true;
}
int main(int argc, char **argv)
{
int res;
//initGPIO();
bool pin = pollGPIO();
printf("GPIO pin = %d\n", pin);
int data_dir_number = 0;
sprintf(data_dir, "%s/%d", DATA_DIR, data_dir_number);
struct stat st;
while(stat(data_dir,&st) == 0){
data_dir_number++;
sprintf(data_dir, "%s/%d", DATA_DIR, data_dir_number);
}
mkdir(data_dir, S_IREAD | S_IWRITE | S_IEXEC);
printf("Data dir is: %s\n", data_dir);
depth_back = malloc(640*480*2);
depth_mid = malloc(640*480*2);
depth_front = malloc(640*480*2);
rgb_back = malloc(640*480*3);
rgb_mid = malloc(640*480*3);
rgb_front = malloc(640*480*3);
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_FATAL);
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;
}
res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
// OS X requires GLUT to run on the main thread
gl_threadfunc(NULL);
//closeGPIO();
return 0;
}
Driver(OniDriverServices* pDriverServices) : DriverBase(pDriverServices)
{
freenect_set_log_level(m_ctx, FREENECT_LOG_DEBUG);
freenect_select_subdevices(m_ctx, FREENECT_DEVICE_CAMERA); // OpenNI2 doesn't use MOTOR or AUDIO
DriverServices = &getServices();
}
int main(int argc, char **argv)
{
int res;
freenect_context *f_ctx;
printf("Kinect camera test\n");
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
freenect_set_tilt_degs(f_dev,freenect_angle);
freenect_set_led(f_dev,LED_RED);
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
res = pthread_create(&gl_thread, NULL, gl_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
return 1;
}
freenect_start_depth(f_dev);
freenect_start_rgb(f_dev);
printf("'w'-tilt up, 's'-level, 'x'-tilt down, '0'-'6'-select LED mode\n");
while(!die && freenect_process_events(f_ctx) >= 0 )
{
int16_t ax,ay,az;
freenect_get_raw_accel(f_dev, &ax, &ay, &az);
double dx,dy,dz;
freenect_get_mks_accel(f_dev, &dx, &dy, &dz);
printf("\r raw acceleration: %4d %4d %4d mks acceleration: %4f %4f %4f\r", ax, ay, az, dx, dy, dz);
fflush(stdout);
}
printf("-- done!\n");
pthread_exit(NULL);
}
int main(int argc, char** argv) {
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_INFO);
freenect_select_subdevices(f_ctx, FREENECT_DEVICE_AUDIO);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
if (nr_devices < 1) {
freenect_shutdown(f_ctx);
return 1;
}
int user_device_number = 0;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
freenect_shutdown(f_ctx);
return 1;
}
state.max_samples = 256 * 60;
state.current_idx = 0;
state.buffers[0] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[1] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[2] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
state.buffers[3] = (int32_t*)malloc(state.max_samples * sizeof(int32_t));
memset(state.buffers[0], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[1], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[2], 0, state.max_samples * sizeof(int32_t));
memset(state.buffers[3], 0, state.max_samples * sizeof(int32_t));
freenect_set_user(f_dev, &state);
freenect_set_audio_in_callback(f_dev, in_callback);
freenect_start_audio(f_dev);
int res = pthread_create(&freenect_thread, NULL, freenect_threadfunc, NULL);
if (res) {
printf("pthread_create failed\n");
freenect_shutdown(f_ctx);
return 1;
}
printf("This is the libfreenect microphone waveform viewer. Press 'q' to quit or spacebar to pause/unpause the view.\n");
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_ALPHA );
glutInitWindowSize(800, 600);
glutInitWindowPosition(0, 0);
window = glutCreateWindow("Microphones");
glClearColor(0.0, 0.0, 0.0, 0.0);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
Reshape(800, 600);
glutReshapeFunc(Reshape);
glutDisplayFunc(DrawMicData);
glutIdleFunc(DrawMicData);
glutKeyboardFunc(Keyboard);
glutMainLoop();
return 0;
}
FreenectDriver() {
freenect_init(&driver_, NULL);
freenect_set_log_level(driver_, FREENECT_LOG_FATAL); // Prevent's printing stuff to the screen
freenect_select_subdevices(driver_, (freenect_device_flags)(FREENECT_DEVICE_CAMERA));
thread_running_ = false;
}
void enableDebug() {
freenect_set_log_level(driver_, FREENECT_LOG_SPEW);
}
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);
}
JNIEXPORT void JNICALL Java_org_openkinect_Context_jniSetLogLevel
(JNIEnv *env, jobject obj, jint level)
{
freenect_context *f_ctx = context(env, obj);
freenect_set_log_level(f_ctx, (freenect_loglevel) level);
}
JNIEXPORT void JNICALL Java_org_ensor_robots_sensors_kinect_Driver_nativeSetLogLevel
(JNIEnv *aJNIEnv, jobject aThisObject, jint aLevel)
{
freenect_set_log_level(f_ctx, aLevel);
}
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 *jit_freenect_capture_threadproc()//t_jit_freenect_grab *x)
{
#ifdef NESADEBUG
#warning if crash remove=NULL
#endif
freenect_context *context = NULL;
postNesa("Threadproc called");//TODO:r
if(!f_ctx){
postNesa("f_ctx is null, calling init_Freenect");//TODO:r
if (freenect_init(&context, NULL) < 0) {
error("freenect_init() failed");
goto out;
}
freenect_set_log_level(context, JIT_FREENECT_LOG_LEVEL);
postNesa("freenect_init ok,id");//TODO: remove
}
f_ctx = context;
struct timeval timeout;
timeout.tv_sec = 60;
timeout.tv_usec = 0;
// loop until told to stop
while (1) {
//postNesaFlood("threadid=%i",x->id);
// test if we're being asked to die, and if so return before we do the work
if (x_systhread_cancel)
{
postNesa("stopping thread");
break;
}
// this thread is used only to process freenect events
// no need to lock the mutex
//systhread_mutex_lock(x->x_mutex);
//if(f_ctx->first){
//if(freenect_process_events(f_ctx) < 0){
//if(freenect_active)
//{
if(f_ctx->first)
if(freenect_process_events_timeout(f_ctx, &timeout)<0)
{
error("Could not process events.");
//if (x->x_systhread_cancel)
break;
}
//}
//else {
// postNesa("freenect_active=false\n");
//}
//}
//else {
// post("not first");//TODO:remove
//}
//x->x_foo++; // fiddle with shared data
//systhread_mutex_unlock(x->x_mutex);
//qelem_set(x->x_qelem); // notify main thread using qelem mechanism
//usleep(2500);
//systhread_sleep(x->x_sleeptime); // sleep a bit
}
out:
postNesa("Threadproc exits");//TODO:r
//postNesa("Threadproc calls grab_close ,id=%i\n",x->id);//TODO:r
// TODO: add notification through dupmoutlet
//jit_freenect_grab_close(x, NULL, 0, NULL);
freenect_shutdown(f_ctx);
f_ctx = NULL;
//x->x_systhread_cancel = false; // reset cancel flag for next time, in case
// the thread is created again
systhread_exit(0); // this can return a value to systhread_join();
return NULL;
}
int main(int argc, char **argv)
{
int i;
for (i=0; i<2048; i++) {
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
g_argc = argc;
g_argv = argv;
if (freenect_init(&f_ctx, NULL) < 0) {
printf("freenect_init() failed\n");
return 1;
}
freenect_set_log_level(f_ctx, FREENECT_LOG_DEBUG);
int nr_devices = freenect_num_devices (f_ctx);
printf ("Number of devices found: %d\n", nr_devices);
int user_device_number = 0;
if (argc > 1)
user_device_number = atoi(argv[1]);
if (nr_devices < 1)
return 1;
if (freenect_open_device(f_ctx, &f_dev, user_device_number) < 0) {
printf("Could not open device\n");
return 1;
}
if ( network_init() < 0 )
return -1;
freenect_set_depth_callback(f_dev, depth_cb);
freenect_set_rgb_callback(f_dev, rgb_cb);
freenect_set_rgb_format(f_dev, FREENECT_FORMAT_RGB);
freenect_set_depth_format(f_dev, FREENECT_FORMAT_11_BIT);
//freenect_start_depth(f_dev);
//freenect_set_led(f_dev,LED_RED);
while(!die && freenect_process_events(f_ctx) >= 0 ){
char buffer[6];
int n = read(data_child, buffer, 1024);
//printf("n: %d\n", n);
if(n == 6){
if (buffer[0] == 1) { //MOTOR
if (buffer[1] == 1) { //MOVE
int angle;
memcpy(&angle, &buffer[2], sizeof(int));
freenect_set_tilt_degs(f_dev,ntohl(angle));
}
}
}
}
network_close();
printf("-- done!\n");
pthread_exit(NULL);
}
//----------------------------------------------------------------------------------------------------------------------
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;
}
