org.openimaj.hardware.kinect.KinectController Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of kinect Show documentation
Show all versions of kinect Show documentation
The OpenIMAJ Kinect Library contains the core classes and native code required interface with the Kinect device.
/**
* Copyright (c) 2011, The University of Southampton and the individual contributors.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* * Neither the name of the University of Southampton nor the names of its
* contributors may be used to endorse or promote products derived from this
* software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
* ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.openimaj.hardware.kinect;
import java.util.ArrayList;
import java.util.List;
import org.bridj.Pointer;
import org.bridj.ValuedEnum;
import org.openimaj.hardware.kinect.freenect.freenect_raw_tilt_state;
import org.openimaj.hardware.kinect.freenect.freenect_registration;
import org.openimaj.hardware.kinect.freenect.libfreenectLibrary;
import org.openimaj.hardware.kinect.freenect.libfreenectLibrary.freenect_context;
import org.openimaj.hardware.kinect.freenect.libfreenectLibrary.freenect_device;
import org.openimaj.hardware.kinect.freenect.libfreenectLibrary.freenect_led_options;
import org.openimaj.hardware.kinect.freenect.libfreenectLibrary.freenect_tilt_status_code;
import org.openimaj.image.FImage;
import org.openimaj.video.VideoDisplay;
/**
* The event thread for Freenect
*
* @author Jonathon Hare ([email protected])
*/
class EventThread extends Thread {
private volatile boolean alive = true;
public EventThread() {
setDaemon(true);
setName("FreenectEventThread");
}
public void kill() {
this.alive = false;
}
@Override
public void run() {
while (alive) {
libfreenectLibrary.freenect_process_events(KinectController.CONTEXT);
}
}
};
/**
* The OpenIMAJ Kinect Interface.
*
* @author Jonathon Hare ([email protected])
*/
public class KinectController {
private static final int DEPTH_X_RES = 640;
private static final int DEPTH_Y_RES = 480;
protected volatile static Pointer CONTEXT;
protected volatile static EventThread EVENT_THREAD;
protected volatile static List ACTIVE_CONTROLLERS = new ArrayList();
protected Pointer device;
/**
* The RGB or IR video stream
*/
public KinectStream videoStream;
/**
* The depth stream
*/
public KinectDepthStream depthStream;
/**
* Construct with given device
*
* @param deviceId
* the device id
* @throws KinectException
*/
public KinectController(int deviceId) throws KinectException {
this(deviceId, false, false);
}
/**
* Construct with the first device in the given mode.
*
* @param irmode
* if true then the camera is set to IR mode; otherwise its in
* RGB mode
* @throws KinectException
*/
public KinectController(boolean irmode) throws KinectException {
this(0, irmode, false);
}
/**
* Default constructor. Uses the first device in RGB mode.
*
* @throws KinectException
*/
public KinectController() throws KinectException {
this(0, false, false);
}
/**
* Construct with the given device and mode.
*
* @param deviceId
* the device identifier. 0 for the first one.
* @param irmode
* whether to use infra-red mode or rgb mode.
* @param registeredDepthMode
* whether to register the depth image. If true, depth
* measurements are in millimeters.
* @throws KinectException
*/
public KinectController(int deviceId, boolean irmode, boolean registeredDepthMode) throws KinectException {
// init the context and start thread if necessary
init();
final int cd = connectedDevices();
if (deviceId >= cd || deviceId < 0) {
throw new IllegalArgumentException("Invalid device id");
}
ACTIVE_CONTROLLERS.add(this);
// init device
final Pointer> devicePtr = Pointer.pointerToPointer(null);
libfreenectLibrary.freenect_open_device(CONTEXT, devicePtr, deviceId);
device = devicePtr.get();
// setup listeners
if (irmode)
videoStream = new KinectIRVideoStream(this);
else
videoStream = new KinectRGBVideoStream(this);
depthStream = new KinectDepthStream(this, registeredDepthMode);
}
@Override
public void finalize() {
close();
}
/**
* Init the freenect library. This only has to be done once.
*
* @throws KinectException
*/
private static synchronized void init() throws KinectException {
if (KinectController.CONTEXT == null) {
final Pointer> ctxPointer = Pointer.pointerToPointer(null);
libfreenectLibrary.freenect_init(ctxPointer, Pointer.NULL);
if (ctxPointer == null)
throw new KinectException("Unable to initialise libfreenect.");
CONTEXT = ctxPointer.get();
if (CONTEXT == null)
throw new KinectException("Unable to initialise libfreenect.");
if (libfreenectLibrary.freenect_num_devices(CONTEXT) == 0) {
libfreenectLibrary.freenect_shutdown(CONTEXT);
throw new KinectException("Unable to initialise libfreenect; No devices found.");
}
EVENT_THREAD = new EventThread();
EVENT_THREAD.start();
// turn off the devices on shutdown
Runtime.getRuntime().addShutdownHook(new Thread() {
@Override
public synchronized void run() {
shutdownFreenect();
}
});
}
}
/**
* Completely shutdown the context. This turns off all cameras. The context
* will be restarted upon creation of a new KinectController.
*/
public synchronized static void shutdownFreenect() {
while (ACTIVE_CONTROLLERS.size() > 0) {
ACTIVE_CONTROLLERS.get(0).close();
}
if (EVENT_THREAD != null) {
EVENT_THREAD.kill();
}
if (CONTEXT != null)
libfreenectLibrary.freenect_shutdown(CONTEXT);
CONTEXT = null;
EVENT_THREAD = null;
}
/**
* Switch the main camera between InfraRed and RGB modes.
*
* @param irmode
* if true, then switches to IR mode, otherwise switches to RGB
* mode.
*/
public void setIRMode(boolean irmode) {
if (device == null)
return;
if (irmode) {
if (!(videoStream instanceof KinectIRVideoStream)) {
videoStream.stop();
videoStream = new KinectIRVideoStream(this);
}
} else {
if (!(videoStream instanceof KinectRGBVideoStream)) {
videoStream.stop();
videoStream = new KinectRGBVideoStream(this);
}
}
}
/**
* Set whether depth should be registered
*
* @param rdepth
* if true, then switches to depth registered mode, otherwise
* depth is not registered
*/
public void setRegisteredDepth(boolean rdepth) {
if (device == null)
return;
if (depthStream.registered != rdepth) {
depthStream.stop();
depthStream = new KinectDepthStream(this, rdepth);
}
}
/**
* Get the number of connected devices.
*
* @return the number of devices connected.
* @throws KinectException
*/
public static synchronized int connectedDevices() throws KinectException {
init();
return libfreenectLibrary.freenect_num_devices(CONTEXT);
}
/**
* Close the device. Closing an already closed device has no effect.
*/
public synchronized void close() {
if (device == null)
return;
videoStream.stop();
depthStream.stop();
libfreenectLibrary.freenect_close_device(device);
device = null;
ACTIVE_CONTROLLERS.remove(this);
}
/**
* Get the current angle in degrees
*
* @return the angle or 0 if the device is closed
*/
public synchronized double getTilt() {
if (device == null)
return 0;
libfreenectLibrary.freenect_update_tilt_state(device);
final Pointer state = libfreenectLibrary.freenect_get_tilt_state(device);
return libfreenectLibrary.freenect_get_tilt_degs(state);
}
/**
* Set the tilt to the given angle in degrees
*
* @param angle
* the angle
*/
public synchronized void setTilt(double angle) {
if (device == null)
return;
if (angle < -30)
angle = -30;
if (angle > 30)
angle = 30;
libfreenectLibrary.freenect_set_tilt_degs(device, angle);
}
/**
* Determine the current tilt status of the device
*
* @return the tilt status or null if the device is closed
*/
public synchronized KinectTiltStatus getTiltStatus() {
if (device == null)
return null;
libfreenectLibrary.freenect_update_tilt_state(device);
final Pointer state = libfreenectLibrary.freenect_get_tilt_state(device);
final ValuedEnum v = libfreenectLibrary.freenect_get_tilt_status(state);
for (final freenect_tilt_status_code c : freenect_tilt_status_code.values())
if (c.value == v.value())
return KinectTiltStatus.valueOf(c.name());
return null;
}
/**
* Set the device status LEDs
*
* @param option
* the LED status to set
*/
public synchronized void setLED(KinectLEDMode option) {
if (device == null)
return;
final ValuedEnum o = freenect_led_options.valueOf(option.name());
libfreenectLibrary.freenect_set_led(device, o);
}
/**
* Sets the LEDs to blink red and yellow for five seconds before resetting
* to green.
*/
public synchronized void identify() {
if (device == null)
return;
setLED(KinectLEDMode.LED_BLINK_RED_YELLOW);
try {
Thread.sleep(5000);
} catch (final InterruptedException e) {
}
setLED(KinectLEDMode.LED_GREEN);
}
/**
* Get the current acceleration values from the device
*
* @return the acceleration or null if the device is closed
*/
public synchronized KinectAcceleration getAcceleration() {
if (device == null)
return null;
final Pointer px = Pointer.pointerToDouble(0);
final Pointer py = Pointer.pointerToDouble(0);
final Pointer pz = Pointer.pointerToDouble(0);
libfreenectLibrary.freenect_update_tilt_state(device);
final Pointer state = libfreenectLibrary.freenect_get_tilt_state(device);
libfreenectLibrary.freenect_get_mks_accel(state, px, py, pz);
return new KinectAcceleration(px.getDouble(), py.getDouble(), pz.getDouble());
}
/**
* Compute the world coordinates from the pixel location and registered
* depth.
*
* @param x
* pixel x-ordinate
* @param y
* pixel y-ordinate
* @param depth
* the depth
* @return the (x,y,z) coordinate in world space
*/
public double[] cameraToWorld(int x, int y, int depth) {
final Pointer wx = Pointer.allocateDouble();
final Pointer wy = Pointer.allocateDouble();
libfreenectLibrary.freenect_camera_to_world(device, x, y, depth, wx, wy);
return new double[] { wx.get(), wy.get(), depth };
}
/**
* Compute the scaling factor for computing world coordinates.
*
* @see #cameraToWorld(int, int, int, double)
*
* @return the scaling factor
*/
public double computeScalingFactor() {
// Struct-by-value isn't currently working in bridj. When it is
// we can do:
// freenect_registration regInfo =
// libfreenectLibrary.freenect_copy_registration(device);
// double ref_pix_size =
// regInfo.zero_plane_info().reference_pixel_size();
// double ref_distance = regInfo.zero_plane_info().reference_distance();
// return 2 * ref_pix_size / ref_distance;
// for now we can work around by calculating the factor from a projected
// point
final int x = (DEPTH_X_RES / 2) + 1;
final double[] xyz = cameraToWorld(x, 0, 1000);
return xyz[0] / 1000.0;
}
/**
* Compute the world coordinates from the pixel location and registered
* depth. This method requires you pre-compute the constant scaling factor
* using {@link #computeScalingFactor()}, but it should be faster than using
* {@link #cameraToWorld(int, int, int)}.
*
* @param x
* pixel x-ordinate
* @param y
* pixel y-ordinate
* @param depth
* the depth
* @param factor
* the scaling factor
* @return the (x,y,z) coordinate in world space
*/
public float[] cameraToWorld(int x, int y, int depth, double factor) {
final float[] pt = new float[3];
pt[0] = (float) ((x - DEPTH_X_RES / 2) * factor * depth);
pt[1] = (float) ((y - DEPTH_Y_RES / 2) * factor * depth);
pt[2] = depth;
return pt;
}
/**
* Compute the world coordinates from the pixel location and registered
* depth. This method requires you pre-compute the constant scaling factor
* using {@link #computeScalingFactor()}, but it should be faster than using
* {@link #cameraToWorld(int, int, int)}.
*
* This method is the same as {@link #cameraToWorld(int, int, int, double)},
* but reuses the point array for efficiency.
*
* @param x
* pixel x-ordinate
* @param y
* pixel y-ordinate
* @param depth
* the depth
* @param factor
* @param pt
* the point to write to
* @return the (x,y,z) coordinate in world space
*/
public float[] cameraToWorld(int x, int y, int depth, double factor, float[] pt) {
pt[0] = (float) ((x - DEPTH_X_RES / 2) * factor * depth);
pt[1] = (float) ((y - DEPTH_Y_RES / 2) * factor * depth);
pt[2] = depth;
return pt;
}
/**
* Compute the world coordinates from the pixel locations in the given
* registered depth image. This method basically gives you a fully
* registered point cloud.
*
* @param regDepth
* the registered depth image
* @param startx
* the starting x-ordinate in the image
* @param stopx
* the stopping x-ordinate in the image
* @param stepx
* the number of pixels in the x direction to skip between
* samples
* @param starty
* the starting y-ordinate in the image
* @param stopy
* the stopping y-ordinate in the image
* @param stepy
* the number of pixels in the y direction to skip between
* samples
* @return an array of 3d vectors
*/
public float[][] cameraToWorld(FImage regDepth, int startx, int stopx, int stepx, int starty, int stopy, int stepy) {
final freenect_registration regInfo = libfreenectLibrary.freenect_copy_registration(device);
final double ref_pix_size = regInfo.zero_plane_info().reference_pixel_size();
final double ref_distance = regInfo.zero_plane_info().reference_distance();
final int nx = (stopx - startx) / stepx;
final int ny = (stopy - starty) / stepy;
final float[][] points = new float[nx * ny][3];
final float[][] depths = regDepth.pixels;
final double factor = 2 * ref_pix_size / ref_distance;
for (int i = 0, y = starty; y < stopy; y += stepy) {
for (int x = startx; x < stopx; x += stepx) {
final float depth = depths[y][x];
points[i][0] = (float) ((x - DEPTH_X_RES / 2) * factor * depth);
points[i][1] = (float) ((y - DEPTH_Y_RES / 2) * factor * depth);
points[i][2] = depth;
}
}
return points;
}
/**
* Test
*
* @param args
* @throws KinectException
*/
public static void main(String[] args) throws KinectException {
VideoDisplay.createVideoDisplay(new KinectController(0).videoStream);
}
}