Skip to content

Commit

Permalink
* Enable by default on RealSense2FrameGrabber.start() all color, d…
Browse files Browse the repository at this point in the history 
…epth, and IR streams as `videoStream` (pull bytedeco#1750)
  • Loading branch information
@cansik
cansik authored Feb 15, 2022
1 parent 42b3e7a commit 48f58d7
Show file tree
Hide file tree
Showing 2 changed files with 127 additions and 16 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,6 @@

* Enable by default on `RealSense2FrameGrabber.start()` all color, depth, and IR streams as `videoStream` ([pull #1750](https://github.com/bytedeco/javacv/pull/1750))

### February 11, 2022 version 1.5.7
* Fix accuracy and latency issues with `FFmpegFrameGrabber.setVideoFrameNumber()` ([pull #1734](https://github.com/bytedeco/javacv/pull/1734))
* Add new `Frame.pictType` field set to `I`, `P`, `B`, etc by `FFmpegFrameGrabber` ([pull #1730](https://github.com/bytedeco/javacv/pull/1730))
Expand Down
141 changes: 125 additions & 16 deletions src/main/java/org/bytedeco/javacv/RealSense2FrameGrabber.java
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2019-2020 Florian Bruggisser, Samuel Audet
* Copyright (C) 2019-2022 Florian Bruggisser, Samuel Audet
*
* Licensed either under the Apache License, Version 2.0, or (at your option)
* under the terms of the GNU General Public License as published by
Expand Down Expand Up @@ -29,6 +29,7 @@
import org.bytedeco.opencv.opencv_core.Size;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

import static org.bytedeco.librealsense2.global.realsense2.*;
Expand All @@ -38,6 +39,10 @@ public class RealSense2FrameGrabber extends FrameGrabber {

private static FrameGrabber.Exception loadingException = null;

private static final int defaultFrameRate = 30;
private static final int defaultWidth = 640;
private static final int defaultHeight = 480;

public static void tryLoad() throws FrameGrabber.Exception {
if (loadingException != null) {
loadingException.printStackTrace();
Expand Down Expand Up @@ -183,8 +188,10 @@ public void start() throws Exception {
this.config = createConfig();

// check if streams is not empty
if (streams.isEmpty())
throw new Exception("No stream has been added to be enabled.");
if (streams.isEmpty()) {
enableAllVideoStreams();
Collections.sort(streams);
}

// enable streams
for (RealSenseStream stream : streams) {
Expand All @@ -199,7 +206,7 @@ public void start() throws Exception {
checkError(error);
}

// set image width & height to largest stream
// set image width & height to the largest stream
RealSenseStream largestStream = getLargestStreamByArea();
this.imageWidth = largestStream.size.width();
this.imageHeight = largestStream.size.height();
Expand Down Expand Up @@ -243,7 +250,8 @@ public void trigger() throws Exception {

@Override
public Frame grab() throws Exception {
RealSenseStream stream = streams.get(0);
int videoStreamId = Math.max(0, videoStream);
RealSenseStream stream = streams.get(videoStreamId);

switch (stream.type) {
case RS2_STREAM_DEPTH:
Expand Down Expand Up @@ -381,14 +389,9 @@ public void setSensorOption(Rs2SensorType sensorType, int optionIndex, boolean v
}

public void setSensorOption(Rs2SensorType sensorType, int optionIndex, float value) throws Exception {
rs2_sensor_list sensorList = rs2_query_sensors(this.device, error);
checkError(error);

int sensorCount = rs2_get_sensors_count(sensorList, error);
checkError(error);
rs2_sensor[] sensors = getSensors(device);

for (int i = 0; i < sensorCount; i++) {
rs2_sensor sensor = rs2_create_sensor(sensorList, i, error);
for (rs2_sensor sensor : sensors) {
checkError(error);

// check if name matches
Expand All @@ -401,8 +404,6 @@ public void setSensorOption(Rs2SensorType sensorType, int optionIndex, float val
// cleanup
rs2_delete_sensor(sensor);
}

rs2_delete_sensor_list(sensorList);
}

private rs2_context createContext() throws Exception {
Expand Down Expand Up @@ -505,7 +506,18 @@ private rs2_stream_profile getStreamProfile(rs2_frame frame) throws Exception {
private StreamProfileData getStreamProfileData(rs2_stream_profile streamProfile) throws Exception {
StreamProfileData profileData = new StreamProfileData();

// check if stream profile matches search type
if (isStreamProfile(streamProfile, RS2_EXTENSION_VIDEO_PROFILE)) {
VideoStreamProfileData videoStreamProfileData = new VideoStreamProfileData();

rs2_get_video_stream_resolution(streamProfile,
videoStreamProfileData.width,
videoStreamProfileData.height,
error);
checkError(error);

profileData = videoStreamProfileData;
}

rs2_get_stream_profile_data(streamProfile,
profileData.nativeStreamIndex,
profileData.nativeFormatIndex,
Expand All @@ -524,6 +536,27 @@ private boolean isSensorExtendableTo(rs2_sensor sensor, int extension) throws Ex
return isExtandable;
}

private boolean isStreamProfile(rs2_stream_profile profile, int type) throws Exception {
boolean isOfType = toBoolean(rs2_stream_profile_is(profile, type, error));
checkError(error);
return isOfType;
}

private boolean matchesVideoStreamProfile(rs2_stream_profile profile,
int streamType,
int streamFormat,
int frameRate,
int width,
int height) throws Exception {
VideoStreamProfileData data = (VideoStreamProfileData) getStreamProfileData(profile);

return streamType == data.nativeStreamIndex.get()
&& streamFormat == data.nativeFormatIndex.get()
&& frameRate == data.frameRate.get()
&& width == data.width.get()
&& height == data.height.get();
}

private void setRs2Option(rs2_options options, int optionIndex, float value) throws Exception {
boolean isSupported = toBoolean(rs2_supports_option(options, optionIndex, error));
checkError(error);
Expand All @@ -536,6 +569,72 @@ private void setRs2Option(rs2_options options, int optionIndex, float value) thr
checkError(error);
}

private rs2_sensor[] getSensors(rs2_device device) throws Exception {
rs2_sensor_list sensorList = rs2_query_sensors(device, error);
checkError(error);

int sensorCount = rs2_get_sensors_count(sensorList, error);
checkError(error);

rs2_sensor[] sensors = new rs2_sensor[sensorCount];

for (int i = 0; i < sensorCount; i++) {
rs2_sensor sensor = rs2_create_sensor(sensorList, i, error);
checkError(error);

sensors[i] = sensor;
}

rs2_delete_sensor_list(sensorList);

return sensors;
}

private rs2_stream_profile[] getStreamProfiles(rs2_sensor sensor) throws Exception {
rs2_stream_profile_list streamList = rs2_get_stream_profiles(sensor, error);
checkError(error);

int streamProfileCount = rs2_get_stream_profiles_count(streamList, error);
checkError(error);

rs2_stream_profile[] profiles = new rs2_stream_profile[streamProfileCount];

for (int i = 0; i < streamProfileCount; i++) {
rs2_stream_profile profile = rs2_get_stream_profile(streamList, i, error);
checkError(error);

profiles[i] = profile;
}

rs2_delete_stream_profiles_list(streamList);
return profiles;
}

private void enableAllVideoStreams() throws Exception {
for (rs2_sensor sensor : getSensors(device)) {
if (!isSensorExtendableTo(sensor, RS2_EXTENSION_VIDEO)) {
rs2_delete_sensor(sensor);
continue;
}

for (rs2_stream_profile profile : getStreamProfiles(sensor)) {
int rsFrameRate = frameRate > 0 ? (int) frameRate : defaultFrameRate;
int rsWidth = imageWidth > 0 ? imageWidth : defaultWidth;
int rsHeight = imageWidth > 0 ? imageWidth : defaultHeight;

if (matchesVideoStreamProfile(profile, RS2_STREAM_DEPTH, RS2_FORMAT_Z16, rsFrameRate, rsWidth, rsHeight)) {
enableDepthStream(imageWidth, imageHeight, rsFrameRate);
} else if (matchesVideoStreamProfile(profile, RS2_STREAM_COLOR, RS2_FORMAT_RGB8, rsFrameRate, rsWidth, rsHeight)) {
enableColorStream(imageWidth, imageHeight, rsFrameRate);
} else if (matchesVideoStreamProfile(profile, RS2_STREAM_INFRARED, RS2_FORMAT_Y8, rsFrameRate, rsWidth, rsHeight)) {
enableIRStream(imageWidth, imageHeight, rsFrameRate);
}
}

rs2_delete_sensor(sensor);
}
}

private static void checkError(rs2_error e) throws Exception {
if (!e.isNull()) {
throw new Exception(String.format("rs_error was raised when calling %s(%s):\n%s\n",
Expand Down Expand Up @@ -564,7 +663,12 @@ static class StreamProfileData {
IntPointer frameRate = new IntPointer(1);
}

public static class RealSenseStream {
static class VideoStreamProfileData extends StreamProfileData {
IntPointer width = new IntPointer(1);
IntPointer height = new IntPointer(1);
}

public static class RealSenseStream implements Comparable<RealSenseStream> {
private int type;
private int index;
private Size size;
Expand Down Expand Up @@ -598,6 +702,11 @@ public int getFrameRate() {
public int getFormat() {
return format;
}

@Override
public int compareTo(RealSenseStream o) {
return Integer.compare(getType(), o.getType());
}
}

public static class RealSense2DeviceInfo {
Expand Down

0 comments on commit 48f58d7

Please sign in to comment.