563 lines
18 KiB
Kotlin
563 lines
18 KiB
Kotlin
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)
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|