orx-kinect refactoring + new general orx-depth-camera (#257)
This commit is contained in:
Kazik Pogoda
GitHub
562
orx-jvm/orx-kinect-v1/src/main/kotlin/Kinect1.kt
Normal file
562
orx-jvm/orx-kinect-v1/src/main/kotlin/Kinect1.kt
Normal file
@@ -0,0 +1,562 @@
|
||||
package org.openrndr.extra.kinect.v1
|
||||
|
||||
import kotlinx.coroutines.Job
|
||||
import kotlinx.coroutines.flow.*
|
||||
import mu.KotlinLogging
|
||||
import org.bytedeco.javacpp.Pointer
|
||||
import org.bytedeco.libfreenect.*
|
||||
import org.bytedeco.libfreenect.global.freenect.*
|
||||
import org.openrndr.Extension
|
||||
import org.openrndr.Program
|
||||
import org.openrndr.draw.*
|
||||
import org.openrndr.extra.depth.camera.DepthMeasurement
|
||||
import org.openrndr.extra.kinect.*
|
||||
import org.openrndr.launch
|
||||
import org.openrndr.math.IntVector2
|
||||
import java.util.*
|
||||
import java.util.concurrent.*
|
||||
import kotlin.concurrent.thread
|
||||
|
||||
class Kinect1Exception(msg: String) : KinectException(msg)
|
||||
|
||||
class Kinect1 : Kinect, Extension {
|
||||
|
||||
override var enabled: Boolean = true
|
||||
|
||||
/**
|
||||
* Without the delay between starting depth camera and
|
||||
* registering depth callback, no frames are transferred
|
||||
* at all. However this problem happens only on the first
|
||||
* try with freshly connected kinect.
|
||||
* Subsequent runs of the same program don't require
|
||||
* this delay at all.
|
||||
*/
|
||||
private var cameraInitializationDelay: Long = 100
|
||||
|
||||
class DeviceInfo(
|
||||
override val serialNumber: String,
|
||||
) : Kinect.Device.Info {
|
||||
override fun toString(): String {
|
||||
return "Kinect1[$serialNumber]"
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Log level for native freenect logging.
|
||||
*
|
||||
* Note: logs will appear on standard out for performance reasons.
|
||||
*
|
||||
* @param code the code of corresponding freenect log level.
|
||||
* @see Kinect1.logLevel
|
||||
*/
|
||||
@Suppress("unused")
|
||||
enum class LogLevel(val code: Int) {
|
||||
|
||||
/** Crashing/non-recoverable errors. */
|
||||
FATAL(FREENECT_LOG_FATAL),
|
||||
|
||||
/** Major errors. */
|
||||
ERROR(FREENECT_LOG_ERROR),
|
||||
|
||||
/** Warning messages. */
|
||||
WARNING(FREENECT_LOG_WARNING),
|
||||
|
||||
/** Important messages. */
|
||||
NOTICE(FREENECT_LOG_NOTICE),
|
||||
|
||||
/** Log for normal messages. */
|
||||
INFO(FREENECT_LOG_INFO),
|
||||
|
||||
/** Log for useful development messages. */
|
||||
DEBUG(FREENECT_LOG_DEBUG),
|
||||
|
||||
/** Log for slightly less useful messages. */
|
||||
SPEW(FREENECT_LOG_SPEW),
|
||||
|
||||
/** Log EVERYTHING. May slow performance. */
|
||||
FLOOD(FREENECT_LOG_FLOOD);
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Kinect native log level, defaults to `INFO`.
|
||||
*/
|
||||
var logLevel: LogLevel
|
||||
get() = freenect.logLevel
|
||||
set(value) { freenect.logLevel = value }
|
||||
|
||||
private val logger = KotlinLogging.logger {}
|
||||
|
||||
private lateinit var program: Program
|
||||
|
||||
private lateinit var freenect: Freenect
|
||||
|
||||
override fun setup(program: Program) {
|
||||
if (!enabled) { return }
|
||||
logger.info("Starting Kinect1 support")
|
||||
this.program = program
|
||||
freenect = Freenect(initialLogLevel = LogLevel.INFO)
|
||||
}
|
||||
|
||||
override fun listDevices(): List<DeviceInfo> = freenect.callBlocking(
|
||||
"listDevices"
|
||||
) { _, _ ->
|
||||
freenect.listDevices()
|
||||
}
|
||||
|
||||
override fun openDevice(index: Int): V1Device {
|
||||
val result = freenect.callBlocking("openDeviceByIndex") { ctx, _ ->
|
||||
val devices = freenect.listDevices()
|
||||
if (devices.isEmpty()) {
|
||||
throw KinectException("No kinect devices detected, cannot open any")
|
||||
} else if (index >= devices.size) {
|
||||
throw KinectException("Invalid device index, number of kinect1 devices: ${devices.size}")
|
||||
}
|
||||
Pair(
|
||||
openFreenectDevice(
|
||||
ctx,
|
||||
devices[index].serialNumber
|
||||
),
|
||||
devices[index]
|
||||
)
|
||||
}
|
||||
val device = V1Device(result.first, result.second)
|
||||
mutableActiveDevices.add(device)
|
||||
return device
|
||||
}
|
||||
|
||||
override fun openDevice(serialNumber: String): V1Device {
|
||||
val dev = freenect.callBlocking("openDeviceBySerial") { ctx, _ ->
|
||||
openFreenectDevice(ctx, serialNumber)
|
||||
}
|
||||
val device = V1Device(dev, DeviceInfo(serialNumber))
|
||||
mutableActiveDevices.add(device)
|
||||
return device
|
||||
}
|
||||
|
||||
private val mutableActiveDevices = LinkedList<V1Device>()
|
||||
|
||||
override val activeDevices: List<Kinect.Device>
|
||||
get() = mutableActiveDevices
|
||||
|
||||
private fun openFreenectDevice(
|
||||
ctx: freenect_context,
|
||||
serialNumber: String,
|
||||
): freenect_device {
|
||||
val dev = freenect_device()
|
||||
freenect.checkReturn(
|
||||
freenect_open_device_by_camera_serial(ctx, dev, serialNumber)
|
||||
)
|
||||
return dev
|
||||
}
|
||||
|
||||
override fun shutdown(program: Program) {
|
||||
if (!enabled) { return }
|
||||
logger.info { "Shutting down Kinect1 support" }
|
||||
logger.debug("Closing active devices, count: ${mutableActiveDevices.size}")
|
||||
mutableActiveDevices.forEach {
|
||||
it.close()
|
||||
}
|
||||
mutableActiveDevices.clear()
|
||||
freenect.close()
|
||||
}
|
||||
|
||||
@Suppress("unused")
|
||||
fun executeInFreenectContext(
|
||||
name: String,
|
||||
block: (ctx: freenect_context, usbCtx: freenect_usb_context) -> Unit
|
||||
) {
|
||||
freenect.call(name) { ctx, usbCtx ->
|
||||
block(ctx, usbCtx)
|
||||
}
|
||||
}
|
||||
|
||||
fun <T> executeInFreenectContextBlocking(
|
||||
name: String,
|
||||
block: (ctx: freenect_context, usbCtx: freenect_usb_context) -> T
|
||||
): T = freenect.callBlocking(name) { ctx, usbCtx ->
|
||||
block(ctx, usbCtx)
|
||||
}
|
||||
|
||||
inner class V1Device(
|
||||
private val dev: freenect_device,
|
||||
override val info: DeviceInfo
|
||||
) : Kinect.Device {
|
||||
|
||||
inner class V1DepthCamera(
|
||||
override val resolution: IntVector2,
|
||||
) : KinectDepthCamera {
|
||||
|
||||
private var firstStart = true
|
||||
private var started = false
|
||||
|
||||
private var bytesFront = kinectRawDepthByteBuffer(resolution)
|
||||
private var bytesBack = kinectRawDepthByteBuffer(resolution)
|
||||
private val bytesFlow = MutableStateFlow(bytesBack) // the first frame will come from bytesFront
|
||||
|
||||
private val rawBuffer = colorBuffer(
|
||||
resolution.x,
|
||||
resolution.y,
|
||||
format = ColorFormat.R,
|
||||
type = ColorType.UINT16_INT
|
||||
).also {
|
||||
it.filter(MinifyingFilter.NEAREST, MagnifyingFilter.NEAREST)
|
||||
}
|
||||
|
||||
private val processedFrameBuffer = colorBuffer(
|
||||
resolution.x,
|
||||
resolution.y,
|
||||
format = ColorFormat.R,
|
||||
type = ColorType.FLOAT16 // in the future we might want to choose the precision here
|
||||
).also {
|
||||
it.filter(MinifyingFilter.LINEAR, MagnifyingFilter.LINEAR)
|
||||
}
|
||||
|
||||
private var mutableCurrentFrame = processedFrameBuffer
|
||||
|
||||
private val depthMappers = Kinect1DepthMappers().apply {
|
||||
update(resolution)
|
||||
}
|
||||
|
||||
override val currentFrame get() = mutableCurrentFrame
|
||||
|
||||
private var onFrameReceived: (frame: ColorBuffer) -> Unit = {}
|
||||
|
||||
// working on rendering thread
|
||||
private val frameReceiverJob: Job = program.launch {
|
||||
bytesFlow.collect { bytes ->
|
||||
rawBuffer.write(bytes)
|
||||
depthMappers.mapper?.apply(rawBuffer, processedFrameBuffer)
|
||||
onFrameReceived(mutableCurrentFrame)
|
||||
}
|
||||
}
|
||||
|
||||
private val freenectDepthCallback = object : freenect_depth_cb() {
|
||||
override fun call(
|
||||
dev: freenect_device,
|
||||
depth: Pointer,
|
||||
timestamp: Int
|
||||
) {
|
||||
bytesFlow.tryEmit(bytesFront)
|
||||
val bytesTmp = bytesBack
|
||||
bytesBack = bytesFront
|
||||
bytesFront = bytesTmp
|
||||
freenect.checkReturn(
|
||||
freenect_set_depth_buffer(dev, Pointer(bytesFront))
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
override var enabled: Boolean = false
|
||||
set(value) {
|
||||
logger.debug { "$info.enabled = $value" }
|
||||
if (value == field) {
|
||||
logger.debug { "$info.enabled: doing nothing, already in state: $value" }
|
||||
return
|
||||
}
|
||||
field = value
|
||||
freenect.call("$info.enabled = $value") { _, _ ->
|
||||
if (value) start() else stop()
|
||||
}
|
||||
}
|
||||
|
||||
override var depthMeasurement: DepthMeasurement
|
||||
get() = depthMappers.depthMeasurement
|
||||
set(value) {
|
||||
logger.debug { "$info.depthMeasurement = $value" }
|
||||
depthMappers.depthMeasurement = value
|
||||
mutableCurrentFrame =
|
||||
if (value == DepthMeasurement.RAW) rawBuffer
|
||||
else processedFrameBuffer
|
||||
}
|
||||
|
||||
override var flipH: Boolean
|
||||
get() = depthMappers.flipH
|
||||
set(value) {
|
||||
logger.debug { "$info.flipH = $value" }
|
||||
depthMappers.flipH = value
|
||||
}
|
||||
|
||||
override var flipV: Boolean
|
||||
get() = depthMappers.flipV
|
||||
set(value) {
|
||||
logger.debug { "$info.flipV = $value" }
|
||||
depthMappers.flipV = value
|
||||
}
|
||||
|
||||
private fun start() {
|
||||
logger.info { "$info.start()" }
|
||||
freenect.checkReturn(freenect_set_depth_mode(
|
||||
dev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT))
|
||||
)
|
||||
freenect.checkReturn(freenect_set_depth_buffer(dev, Pointer(bytesFront)))
|
||||
freenect.checkReturn(freenect_start_depth(dev))
|
||||
if (firstStart) { // workaround, see comments above
|
||||
Thread.sleep(cameraInitializationDelay)
|
||||
firstStart = false
|
||||
}
|
||||
freenect_set_depth_callback(dev, freenectDepthCallback)
|
||||
started = true
|
||||
freenect.expectingEvents = true
|
||||
}
|
||||
|
||||
internal fun stop() {
|
||||
logger.info { "$info.stop()" }
|
||||
if (started) {
|
||||
freenect.expectingEvents = false
|
||||
freenect.checkReturn(freenect_stop_depth(dev))
|
||||
started = false
|
||||
} else {
|
||||
logger.warn { "$info.stop(): cannot stop already stopped depth camera" }
|
||||
}
|
||||
}
|
||||
|
||||
internal fun close() {
|
||||
frameReceiverJob.cancel()
|
||||
}
|
||||
|
||||
override fun onFrameReceived(block: (frame: ColorBuffer) -> Unit) {
|
||||
onFrameReceived = block
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
override val depthCamera: V1DepthCamera = V1DepthCamera(
|
||||
resolution = KINECT1_DEPTH_RESOLUTION
|
||||
)
|
||||
|
||||
fun executeInFreenectDeviceContext(
|
||||
name: String,
|
||||
block: (ctx: freenect_context, usbCtx: freenect_usb_context, dev: freenect_device) -> Unit
|
||||
) {
|
||||
freenect.call("$info: $name") { ctx, usbCtx ->
|
||||
block(ctx, usbCtx, dev)
|
||||
}
|
||||
}
|
||||
|
||||
@Suppress("unused")
|
||||
fun <T> executeInFreenectDeviceContextBlocking(
|
||||
name: String,
|
||||
block: (ctx: freenect_context, usbCtx: freenect_usb_context, dev: freenect_device) -> T
|
||||
): T = freenect.callBlocking("$info: $name") { ctx, usbCtx ->
|
||||
block(ctx, usbCtx, dev)
|
||||
}
|
||||
|
||||
override fun close() {
|
||||
logger.info { "$info.close()" }
|
||||
freenect.callBlocking("$info.closeDevice") { _, _ ->
|
||||
depthCamera.stop()
|
||||
freenect.checkReturn(freenect_close_device(dev))
|
||||
mutableActiveDevices.remove(this)
|
||||
}
|
||||
depthCamera.close()
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* This class provides a low level API for accessing a kinect1 device.
|
||||
* All the operations are executed in a single thread responsible for calling
|
||||
* freenect API.
|
||||
*
|
||||
* @param initialLogLevel the log level to use when freenect is initialized.
|
||||
*/
|
||||
class Freenect(private val initialLogLevel: Kinect1.LogLevel) {
|
||||
|
||||
private val logger = KotlinLogging.logger {}
|
||||
|
||||
var logLevel: Kinect1.LogLevel = initialLogLevel
|
||||
set(value) {
|
||||
call("logLevel[$value]") { ctx, _ ->
|
||||
freenect_set_log_level(ctx, value.code)
|
||||
}
|
||||
field = value
|
||||
}
|
||||
|
||||
internal var expectingEvents: Boolean = false
|
||||
|
||||
private val ctx = freenect_context()
|
||||
|
||||
private val usbCtx = freenect_usb_context()
|
||||
|
||||
private var running: Boolean = true
|
||||
|
||||
private val runner = thread(name = "kinect1", start = true) {
|
||||
logger.info("Starting Kinect1 thread")
|
||||
checkReturn(freenect_init(ctx, usbCtx))
|
||||
freenect_set_log_level(ctx, logLevel.code)
|
||||
val num = checkReturn(freenect_num_devices(ctx))
|
||||
if (num == 0) {
|
||||
logger.warn { "Could not find any Kinect1 devices, calling openDevice() will throw exception" }
|
||||
} else {
|
||||
val devices = listDevices()
|
||||
logger.info { "Kinect1 detected, device count: ${devices.size}" }
|
||||
devices.forEachIndexed { index, info ->
|
||||
logger.info { " |-[$index]: ${info.serialNumber}" }
|
||||
}
|
||||
}
|
||||
|
||||
while (running) {
|
||||
if (expectingEvents) {
|
||||
val ret = freenect_process_events(ctx)
|
||||
if (ret != 0) {
|
||||
logger.error { "freenect_process_events returned non-zero value: $ret" }
|
||||
}
|
||||
val tasks = freenectCallQueue.iterator()
|
||||
for (task in tasks) {
|
||||
tasks.remove()
|
||||
task.run()
|
||||
}
|
||||
} else {
|
||||
freenectCallQueue.pollFirst()?.run()
|
||||
}
|
||||
}
|
||||
|
||||
checkReturn(freenect_shutdown(ctx))
|
||||
}
|
||||
|
||||
private val freenectCallQueue = LinkedBlockingDeque<FutureTask<*>>()
|
||||
|
||||
fun call(
|
||||
name: String,
|
||||
block: (
|
||||
ctx: freenect_context,
|
||||
usbCtx: freenect_usb_context
|
||||
) -> Unit
|
||||
) {
|
||||
logger.debug { "'$name' requested (non-blocking)" }
|
||||
val task = FutureTask {
|
||||
logger.trace { "'$name': started" }
|
||||
try {
|
||||
block(ctx, usbCtx)
|
||||
logger.trace { "'$name': ended" }
|
||||
} catch (e: Exception) {
|
||||
logger.error("'$name': failed", e)
|
||||
}
|
||||
}
|
||||
freenectCallQueue.add(task)
|
||||
}
|
||||
|
||||
fun <T> callBlocking(
|
||||
name: String,
|
||||
block: (
|
||||
ctx: freenect_context,
|
||||
usbCtx: freenect_usb_context
|
||||
) -> T
|
||||
): T {
|
||||
logger.debug { "'$name' requested (blocking)" }
|
||||
val task = FutureTask {
|
||||
logger.trace { "'$name': started" }
|
||||
try {
|
||||
val result = block(ctx, usbCtx)
|
||||
logger.trace { "'$name': ended" }
|
||||
Result.success(result)
|
||||
} catch (e: Exception) {
|
||||
logger.error("'$name': failed", e)
|
||||
Result.failure(e)
|
||||
}
|
||||
}
|
||||
freenectCallQueue.add(task)
|
||||
val result = task.get()
|
||||
logger.trace { "'$name': returned result" }
|
||||
return result.getOrThrow()
|
||||
}
|
||||
|
||||
fun listDevices() : List<Kinect1.DeviceInfo> {
|
||||
val attributes = freenect_device_attributes()
|
||||
freenect_list_device_attributes(ctx, attributes)
|
||||
try {
|
||||
val devices = buildList {
|
||||
var item: freenect_device_attributes? =
|
||||
if (attributes.isNull) null
|
||||
else attributes
|
||||
while (item != null) {
|
||||
val serialNumber = item.camera_serial().string
|
||||
add(Kinect1.DeviceInfo(serialNumber))
|
||||
item = item.next()
|
||||
}
|
||||
}
|
||||
return devices
|
||||
} finally {
|
||||
if (!attributes.isNull) {
|
||||
freenect_free_device_attributes(attributes)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fun close() {
|
||||
logger.debug("Closing Kinect1 runner")
|
||||
running = false
|
||||
logger.debug("Waiting for runner thread to finish")
|
||||
runner.join()
|
||||
}
|
||||
|
||||
fun checkReturn(ret: Int): Int =
|
||||
if (ret >= 0) ret
|
||||
else {
|
||||
throw Kinect1Exception("Freenect error: ret=$ret")
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
internal const val KINECT1_MAX_DEPTH_VALUE: Double = 2047.0
|
||||
|
||||
internal val KINECT1_DEPTH_RESOLUTION: IntVector2 = IntVector2(640, 480)
|
||||
|
||||
internal class Kinect1DepthMappers {
|
||||
|
||||
private val depthToRawNormalized = depthToRawNormalizedMappers().apply {
|
||||
forEach {
|
||||
it.parameters["maxDepthValue"] = KINECT1_MAX_DEPTH_VALUE
|
||||
}
|
||||
}
|
||||
|
||||
private val depthToMeters = KinectDepthMappers(
|
||||
"kinect1-depth-to-meters.frag",
|
||||
Kinect1::class
|
||||
)
|
||||
|
||||
var depthMeasurement: DepthMeasurement = DepthMeasurement.RAW_NORMALIZED
|
||||
set(value) {
|
||||
field = value
|
||||
selectMapper()
|
||||
}
|
||||
|
||||
var flipH: Boolean = false
|
||||
set(value) {
|
||||
field = value
|
||||
selectMapper()
|
||||
}
|
||||
|
||||
var flipV: Boolean = false
|
||||
set(value) {
|
||||
field = value
|
||||
selectMapper()
|
||||
}
|
||||
|
||||
var mapperState: Filter? = depthToRawNormalized.select(
|
||||
flipH = false,
|
||||
flipV = false
|
||||
)
|
||||
val mapper: Filter? get() = mapperState
|
||||
|
||||
fun update(resolution: IntVector2) {
|
||||
depthToRawNormalized.update(resolution)
|
||||
depthToMeters.update(resolution)
|
||||
}
|
||||
|
||||
private fun selectMapper() {
|
||||
mapperState = when (depthMeasurement) {
|
||||
DepthMeasurement.RAW -> null
|
||||
DepthMeasurement.RAW_NORMALIZED -> {
|
||||
depthToRawNormalized.select(flipH, flipV)
|
||||
}
|
||||
DepthMeasurement.METERS -> {
|
||||
depthToMeters.select(flipH, flipV)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,289 +0,0 @@
|
||||
package org.openrndr.extra.kinect.v1
|
||||
|
||||
import mu.KotlinLogging
|
||||
import org.bytedeco.javacpp.Pointer
|
||||
import org.bytedeco.libfreenect.*
|
||||
import org.bytedeco.libfreenect.global.freenect.*
|
||||
import org.bytedeco.libfreenect.presets.freenect
|
||||
import org.openrndr.Program
|
||||
import org.openrndr.extra.kinect.*
|
||||
import org.openrndr.extra.kinect.impl.*
|
||||
import java.nio.ByteBuffer
|
||||
import java.nio.ByteOrder
|
||||
import java.util.*
|
||||
import java.util.concurrent.*
|
||||
import java.util.concurrent.atomic.AtomicBoolean
|
||||
import java.util.concurrent.atomic.AtomicReference
|
||||
import java.util.function.Supplier
|
||||
import kotlin.concurrent.thread
|
||||
|
||||
/**
|
||||
* Returns support for kinect version 1.
|
||||
*
|
||||
* @param depthCameraInitializationDelay defaults to 100 ms. Delay seems to be
|
||||
* necessary due to either my misunderstanding or some weird freenect bug.
|
||||
* Without the delay between starting depth camera and registering
|
||||
* depth callback, no frames are transferred at all. However this
|
||||
* problem happens only on the first try with freshly connected
|
||||
* kinect. Subsequent runs of the same program don't require
|
||||
* this delay at all.
|
||||
*/
|
||||
fun getKinectsV1(program: Program, depthCameraInitializationDelay: Long = 100) : Kinects<Freenect> {
|
||||
return DefaultKinects(program, KinectsV1Manager(depthCameraInitializationDelay))
|
||||
}
|
||||
|
||||
/** Provides low level freenect context for calling native freenect methods. */
|
||||
class Freenect(
|
||||
val fnCtx: freenect_context,
|
||||
val fnUsbCtx: freenect_usb_context,
|
||||
val fnDev: freenect_device? = null // only available for device level commands
|
||||
)
|
||||
|
||||
private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : KinectsManager<Freenect> {
|
||||
private val logger = KotlinLogging.logger {}
|
||||
|
||||
private val fnCtx = freenect_context()
|
||||
|
||||
private val fnUsbCtx = freenect_usb_context()
|
||||
|
||||
private val ctx = Freenect(fnCtx, fnUsbCtx)
|
||||
|
||||
private var taskQueue = LinkedBlockingDeque<FutureTask<*>>()
|
||||
|
||||
private var running = true
|
||||
|
||||
private val runner = thread(
|
||||
name = "Kinect1-runner",
|
||||
start = false,
|
||||
isDaemon = true
|
||||
) {
|
||||
initializeFreenect()
|
||||
while (running) { mainLoop() }
|
||||
shutdownFreenect()
|
||||
}
|
||||
|
||||
private var expectingEvents = false
|
||||
|
||||
private val devices: LinkedList<FreenectDevice> = LinkedList()
|
||||
|
||||
private val timeout = freenect.timeval()
|
||||
init { timeout.tv_sec(1) }
|
||||
|
||||
private inner class KinectV1CommandsExecutor(val context: Freenect): KinectCommandsExecutor<Freenect> {
|
||||
override fun <T> execute(commands: (Freenect) -> T): T {
|
||||
return callSync {
|
||||
logger.trace { "executing native freenect commands" }
|
||||
commands(context)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private val commandsExecutor = KinectV1CommandsExecutor(ctx)
|
||||
|
||||
override fun initialize() {
|
||||
logger.info("Initializing Kinect1 support, set log level to TRACE to see received frames")
|
||||
runner.start()
|
||||
}
|
||||
|
||||
private fun initializeFreenect() {
|
||||
logger.debug("initializing freenect")
|
||||
verify(freenect_init(fnCtx, fnUsbCtx))
|
||||
freenect_set_log_level(fnCtx, FREENECT_LOG_INFO)
|
||||
freenect_select_subdevices(fnCtx, FREENECT_DEVICE_CAMERA)
|
||||
val num = verify(freenect_num_devices(fnCtx))
|
||||
if (num == 0) {
|
||||
logger.warn { "Could not find any Kinect1 device, calling startDevice() will throw exception" }
|
||||
}
|
||||
}
|
||||
|
||||
private fun mainLoop() {
|
||||
if (expectingEvents) {
|
||||
val ret = freenect_process_events(fnCtx)
|
||||
if (ret != 0) { logger.error { "freenect_process_events returned non-zero value: $ret" } }
|
||||
val tasks = taskQueue.iterator()
|
||||
for (task in tasks) {
|
||||
tasks.remove()
|
||||
task.run()
|
||||
}
|
||||
} else {
|
||||
taskQueue.poll(100, TimeUnit.MILLISECONDS)?.run()
|
||||
}
|
||||
}
|
||||
|
||||
private fun shutdownFreenect() {
|
||||
logger.debug("shutting down freenect")
|
||||
if (!fnCtx.isNull) {
|
||||
devices.forEach { device -> device.shutdown() }
|
||||
devices.clear()
|
||||
verifyOnShutdown(freenect_shutdown(fnCtx))
|
||||
}
|
||||
}
|
||||
|
||||
override fun countDevices(): Int {
|
||||
return callSync { verify(freenect_num_devices(fnCtx)) }
|
||||
}
|
||||
|
||||
override fun startDevice(num: Int): KinectDevice<Freenect> {
|
||||
callSync {
|
||||
devices.find { device -> device.num == num }
|
||||
}?.let {
|
||||
throw KinectException("Kinect1 device already started, num: $num")
|
||||
}
|
||||
val count = countDevices()
|
||||
if (num >= count) {
|
||||
throw KinectException(
|
||||
"Trying to start non-existent Kinect1 device, " +
|
||||
"device count: $count, num: $num (index starts with 0)"
|
||||
)
|
||||
}
|
||||
val device = callSync {
|
||||
val device = FreenectDevice(num)
|
||||
devices.add(device)
|
||||
device
|
||||
}
|
||||
return DefaultKinectDevice(
|
||||
DefaultKinectDepthCamera(
|
||||
device.depthCamera.width,
|
||||
device.depthCamera.height,
|
||||
32.0,
|
||||
device.depthCamera.enabler,
|
||||
device.depthCamera.bytesSupplier
|
||||
),
|
||||
KinectV1CommandsExecutor(device.devCtx)
|
||||
)
|
||||
}
|
||||
|
||||
override fun <T> execute(commands: (Freenect) -> T): T {
|
||||
return commandsExecutor.execute(commands)
|
||||
}
|
||||
|
||||
override fun shutdown() {
|
||||
logger.info("Shutting down Kinect1 support")
|
||||
callSync { running = false }
|
||||
runner.join()
|
||||
}
|
||||
|
||||
private inline fun <T> callSync(crossinline block: () -> T): T {
|
||||
val task = FutureTask<T>(Callable { block() })
|
||||
taskQueue.add(task)
|
||||
return task.get()
|
||||
}
|
||||
|
||||
private inner class FreenectDevice(val num: Int) {
|
||||
|
||||
val depthCamera = FreenectDepthCamera()
|
||||
|
||||
val fnDev = freenect_device()
|
||||
|
||||
val devCtx = Freenect(fnCtx, fnUsbCtx, fnDev)
|
||||
|
||||
init {
|
||||
logger.info { "Opening Kinect1 device num: $num" }
|
||||
verify(freenect_open_device(fnCtx, fnDev, num))
|
||||
}
|
||||
|
||||
val expectingEvents: Boolean
|
||||
get() = depthCamera.expectingEvents // or other device in the future
|
||||
|
||||
fun shutdown() {
|
||||
logger.info { "Shutting down Kinect1 device num: $num" }
|
||||
if (!fnDev.isNull) {
|
||||
verifyOnShutdown(freenect_stop_depth(fnDev))
|
||||
verifyOnShutdown(freenect_close_device(fnDev))
|
||||
}
|
||||
}
|
||||
|
||||
inner class FreenectDepthCamera {
|
||||
|
||||
val width: Int = 640
|
||||
val height: Int = 480
|
||||
|
||||
private val bytes = ByteBuffer.allocateDirect(width * height * 2)
|
||||
init { bytes.order(ByteOrder.nativeOrder()) }
|
||||
|
||||
private val currentBytesRef = AtomicReference<ByteBuffer?>()
|
||||
|
||||
private val freenectDepthCb = object : freenect_depth_cb() {
|
||||
override fun call(dev: freenect_device?, depth: Pointer?, timestamp: Int) {
|
||||
logger.trace { "depth frame received for Kinect1 device: $num, at: $timestamp" }
|
||||
currentBytesRef.set(bytes)
|
||||
}
|
||||
}
|
||||
|
||||
val bytesSupplier = Supplier<ByteBuffer?> { currentBytesRef.getAndSet(null) }
|
||||
|
||||
val enabler = object : KinectFeatureEnabler {
|
||||
|
||||
private val atomicEnabled = AtomicBoolean(false)
|
||||
private val inProgress = AtomicBoolean(false)
|
||||
|
||||
override var enabled // usually called from rendering thread
|
||||
get() = atomicEnabled.get()
|
||||
set(value) {
|
||||
if (atomicEnabled.get() == value) {
|
||||
logger.warn { "Current state requested - doing nothing, Kinect1 device: $num, enabled=$value" }
|
||||
return
|
||||
}
|
||||
if (!inProgress.getAndSet(true)) {
|
||||
if (value) {
|
||||
callSync {
|
||||
try {
|
||||
start()
|
||||
atomicEnabled.set(true)
|
||||
updateExpectingEvents()
|
||||
} finally { inProgress.set(false) }
|
||||
}
|
||||
} else {
|
||||
callSync {
|
||||
try {
|
||||
stop()
|
||||
atomicEnabled.set(false)
|
||||
updateExpectingEvents()
|
||||
} finally { inProgress.set(false) }
|
||||
}
|
||||
}
|
||||
} else {
|
||||
logger.warn { "Operation in progress, Kinect1 device: $num, requested enabled=$value" }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
val expectingEvents: Boolean
|
||||
get() = depthCamera.enabler.enabled
|
||||
|
||||
private fun start() {
|
||||
logger.info { "Enabling Kinect1 depth camera, device num: $num" }
|
||||
verify(freenect_set_depth_mode(
|
||||
fnDev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT))
|
||||
)
|
||||
verify(freenect_set_depth_buffer(fnDev, Pointer(bytes)))
|
||||
verify(freenect_start_depth(fnDev))
|
||||
Thread.sleep(depthCameraInitializationDelay) // here is the hack
|
||||
freenect_set_depth_callback(fnDev, freenectDepthCb)
|
||||
}
|
||||
|
||||
private fun stop() {
|
||||
logger.info { "Disabling Kinect1 depth camera, device num: $num" }
|
||||
verify(freenect_stop_depth(fnDev))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private fun updateExpectingEvents() {
|
||||
expectingEvents = devices.any { device -> device.expectingEvents }
|
||||
}
|
||||
|
||||
private fun verifyOnShutdown(ret: Int) {
|
||||
if (ret != 0) {
|
||||
logger.error { "Unexpected return value while shutting down Kinect1 support: $ret" }
|
||||
}
|
||||
}
|
||||
|
||||
private fun verify(ret: Int): Int {
|
||||
if (ret < 0) {
|
||||
throw KinectException("Kinect1 error: ret=$ret")
|
||||
}
|
||||
return ret
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
#ifndef KINECT_FLIPV
|
||||
layout(origin_upper_left) in vec4 gl_FragCoord;
|
||||
#endif
|
||||
|
||||
uniform usampler2D tex0; // kinect raw
|
||||
#ifdef KINECT_FLIPH
|
||||
uniform int resolutionXMinus1;
|
||||
#endif
|
||||
out float outDepth; // measured in meters
|
||||
|
||||
const uint UINT_MAX_KINECT_DEPTH = 2047u;
|
||||
|
||||
void main() {
|
||||
ivec2 uv = ivec2(gl_FragCoord);
|
||||
#ifdef KINECT_FLIPH
|
||||
uv = ivec2(resolutionXMinus1 - uv.x, uv.y);
|
||||
#endif
|
||||
uint uintDepth = texelFetch(tex0, uv, 0).r;
|
||||
float depth = float(uintDepth);
|
||||
outDepth = (uintDepth < UINT_MAX_KINECT_DEPTH)
|
||||
? 1.0 / (depth * -0.0030711016 + 3.3309495161)
|
||||
: 0.0;
|
||||
}
|
||||
Reference in New Issue
Block a user