diff --git a/orx-realsense2/src/demo/kotlin/DemoRS201.kt b/orx-realsense2/src/demo/kotlin/DemoRS201.kt index 9ed9ce4fa..b46020c69 100644 --- a/orx-realsense2/src/demo/kotlin/DemoRS201.kt +++ b/orx-realsense2/src/demo/kotlin/DemoRS201.kt @@ -1,7 +1,9 @@ import org.openrndr.application +import org.openrndr.color.ColorRGBa import org.openrndr.draw.ColorFormat import org.openrndr.draw.ColorType import org.openrndr.draw.colorBuffer +import org.openrndr.draw.tint import org.openrndr.extra.realsense2.RS2Sensor fun main() { @@ -14,11 +16,17 @@ fun main() { } val sensor = RS2Sensor.openFirstOrDummy() println(sensor) + for (stream in sensor.streams) { + println(stream.intrinsics) + } + + sensor.depthFrameReceived.listen { it.copyTo(depthFrame) } extend { sensor.waitForFrames() + drawer.drawStyle.colorMatrix = tint(ColorRGBa.WHITE.shade(20.0)) drawer.image(depthFrame) } } diff --git a/orx-realsense2/src/demo/kotlin/DemoUnproject01.kt b/orx-realsense2/src/demo/kotlin/DemoUnproject01.kt new file mode 100644 index 000000000..c723fb304 --- /dev/null +++ b/orx-realsense2/src/demo/kotlin/DemoUnproject01.kt @@ -0,0 +1,9 @@ +import org.openrndr.application + +fun main() { + application { + program { + + } + } +} \ No newline at end of file diff --git a/orx-realsense2/src/main/kotlin/RS2Sensor.kt b/orx-realsense2/src/main/kotlin/RS2Sensor.kt index 8270334b5..177a72f9d 100644 --- a/orx-realsense2/src/main/kotlin/RS2Sensor.kt +++ b/orx-realsense2/src/main/kotlin/RS2Sensor.kt @@ -5,6 +5,7 @@ import org.bytedeco.librealsense2.* import org.bytedeco.librealsense2.global.realsense2.* import org.openrndr.draw.ColorBuffer import org.openrndr.events.Event +import org.openrndr.math.Vector3 import java.nio.ByteBuffer private val rs2Context by lazy { @@ -26,7 +27,6 @@ enum class RS2DepthFormat { UINT16 } class RS2SensorDescription(private val deviceList: rs2_device_list, private val deviceIndex: Int) { - /** * open realsense sensor from description entry */ @@ -73,12 +73,66 @@ class RS2FrameEvent(private val frame: rs2_frame, val frameWidth: Int, val frame } } +/** + * Distortion model + */ +enum class DistortionModel { + NONE, + FTHETA, + BROWN_CONRADY, + INVERSE_BROWN_CONRADY, + KANNALA_BRANDT4, + MODIFIED_BROWN_CONRADY +} + +/** + * Stream intrinsics + */ +data class Intrinsics( + /** width of the stream image in pixels */ + val width: Int, + /** height of the stream image in pixels */ + val height: Int, + /** horizontal coordinate of the principal point of the image, as a pixel offset from the left edge */ + val ppx: Double, + /** vertical coordinate of the principal point of the image, as a pixel offset from the left edge */ + val ppy: Double, + /** focal length of the image plane, as a multiple of pixel width */ + val fx: Double, + /** focal length of the image plane, as a multiple of pixel height */ + val fy: Double, + /** distortion model of the image */ + val model: DistortionModel + ) { + + fun unproject(x: Double, y: Double, depth: Double) : Vector3 { + if (model == DistortionModel.BROWN_CONRADY) { + return Vector3(((x - ppx) / fx) * depth , ((y - ppy) / fy) * depth, depth) + } else { + error("unsupported distortion model $model") + } + } +} + +/** + * Stream descriptor + */ +data class Stream(val intrinsics: Intrinsics) { + +} + + abstract class Sensor { /** * depth frame received event, triggered from [waitForFrames] */ val depthFrameReceived = Event() + /** + * a list of [Stream]s for the [Sensor] + */ + abstract val streams: List + /** * wait for frames to arrives */ @@ -96,6 +150,9 @@ class DummySensor : Sensor() { override fun destroy() { } + + override val streams: List + get() = emptyList() } class RS2Sensor( @@ -135,6 +192,43 @@ class RS2Sensor( error.check() } + override val streams: List by lazy { + val error = rs2_error() + val streamProfileList = rs2_pipeline_profile_get_streams(pipelineProfile, error) + error.check() + val streamProfileCount = rs2_get_stream_profiles_count(streamProfileList, error) + error.check() + val result = (0 until streamProfileCount).map { + val streamProfile = rs2_get_stream_profile(streamProfileList, it, error) + error.check() + val intrinsics = rs2_intrinsics() + rs2_get_video_stream_intrinsics(streamProfile, intrinsics, error) + val result = Stream( + Intrinsics( + width = intrinsics.width(), + height = intrinsics.height(), + ppx = intrinsics.ppx().toDouble(), + ppy = intrinsics.ppy().toDouble(), + fx = intrinsics.fx().toDouble(), + fy = intrinsics.fy().toDouble(), + model = when (intrinsics.model()) { + RS2_DISTORTION_NONE -> DistortionModel.NONE + RS2_DISTORTION_FTHETA -> DistortionModel.FTHETA + RS2_DISTORTION_BROWN_CONRADY -> DistortionModel.BROWN_CONRADY + RS2_DISTORTION_INVERSE_BROWN_CONRADY -> DistortionModel.INVERSE_BROWN_CONRADY + RS2_DISTORTION_KANNALA_BRANDT4 -> DistortionModel.KANNALA_BRANDT4 + RS2_DISTORTION_MODIFIED_BROWN_CONRADY -> DistortionModel.MODIFIED_BROWN_CONRADY + else -> error("unsupported distortion model for stream") + } + ) + ) + result + } + rs2_delete_stream_profiles_list(streamProfileList) + result + } + + companion object { /** * list all connected Realsense devices