mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-04 06:06:17 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/rs: remove superfluous rs_update_offset; add config options

Browse source
This commit is contained in:
Moses Turner 2021-05-13 17:54:37 -05:00
parent 423084e827
commit 302e72b2fe
3 changed files with 102 additions and 53 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/xrt/drivers/meson.build
View file

@ -127,7 +127,7 @@ lib_drv_rs = static_library(
'realsense/rs_interface.h',
'realsense/rs_prober.c',
),
include_directories: xrt_include,
include_directories: [xrt_include,cjson_include],
dependencies: [aux, rs],
build_by_default: 'rs' in drivers,
)

150
src/xrt/drivers/realsense/rs_6dof.c
View file

@ -4,6 +4,7 @@
/*!
* @file
* @brief RealSense helper driver for 6DOF tracking.
* @author Moses Turner <mosesturner@protonmail.com>
* @author Nova King <technobaboo@gmail.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_rs
@ -22,6 +23,9 @@
#include "util/u_device.h"
#include "util/u_logging.h"
#include "util/u_json.h"
#include "util/u_config_json.h"
#include <librealsense2/rs.h>
#include <librealsense2/h/rs_pipeline.h>
#include <librealsense2/h/rs_option.h>
@ -50,11 +54,14 @@ struct rs_6dof
uint64_t relation_timestamp_ns;
struct xrt_space_relation relation;
// arbitrary offset to apply to the pose the t265 gives us
struct xrt_pose offset;
struct os_thread_helper oth;
bool enable_mapping;
bool enable_pose_jumping;
bool enable_relocalization;
bool enable_pose_prediction;
bool enable_pose_filtering; // Forward compatibility for when that 1-euro filter is working
rs2_context *ctx;
rs2_pipeline *pipe;
rs2_pipeline_profile *profile;
@ -115,6 +122,15 @@ close_6dof(struct rs_6dof *rs)
}
}
#define CHECK_RS2() \
do { \
if (check_error(rs, e) != 0) { \
close_6dof(rs); \
return 1; \
} \
} while (0)
/*!
* Create all RealSense resources needed for 6DOF tracking.
*/
@ -125,22 +141,13 @@ create_6dof(struct rs_6dof *rs)
rs2_error *e = NULL;
rs->ctx = rs2_create_context(RS2_API_VERSION, &e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
rs2_device_list *device_list = rs2_query_devices(rs->ctx, &e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
int dev_count = rs2_get_device_count(device_list, &e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
U_LOG_D("There are %d connected RealSense devices.", dev_count);
if (0 == dev_count) {
@ -149,16 +156,10 @@ create_6dof(struct rs_6dof *rs)
}
rs->pipe = rs2_create_pipeline(rs->ctx, &e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
rs->config = rs2_create_config(&e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
rs2_config_enable_stream(rs->config, //
RS2_STREAM_POSE, // Type
@ -168,29 +169,42 @@ create_6dof(struct rs_6dof *rs)
RS2_FORMAT_6DOF, // Format
200, // FPS
&e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
CHECK_RS2();
rs2_pipeline_profile *prof = rs2_config_resolve(rs->config, rs->pipe, &e);
CHECK_RS2();
rs2_device *cameras = rs2_pipeline_profile_get_device(prof, &e);
CHECK_RS2();
rs2_sensor_list *sensors = rs2_query_sensors(cameras, &e);
CHECK_RS2();
rs2_sensor *sensor = rs2_create_sensor(sensors, 0, &e);
CHECK_RS2();
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_MAPPING, rs->enable_mapping ? 1.0f : 0.0f, &e);
CHECK_RS2();
if (rs->enable_mapping) {
// Neither of these options mean anything if mapping is off; in fact it errors out
// if we mess with these
// with mapping off
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_RELOCALIZATION,
rs->enable_relocalization ? 1.0f : 0.0f, &e);
CHECK_RS2();
rs2_set_option((rs2_options *)sensor, RS2_OPTION_ENABLE_POSE_JUMPING,
rs->enable_pose_jumping ? 1.0f : 0.0f, &e);
CHECK_RS2();
}
rs->profile = rs2_pipeline_start_with_config(rs->pipe, rs->config, &e);
if (check_error(rs, e) != 0) {
close_6dof(rs);
return 1;
}
CHECK_RS2();
return 0;
}
void
rs_update_offset(struct xrt_pose offset, struct xrt_device *xdev)
{
struct rs_6dof *rs = rs_6dof(xdev);
memcpy(&rs->offset, &offset, sizeof(struct xrt_pose));
}
/*!
* Process a frame as 6DOF data, does not assume ownership of the frame.
*/
@ -354,15 +368,13 @@ rs_6dof_get_tracked_pose(struct xrt_device *xdev,
int64_t diff_prediction_ns = 0;
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
struct xrt_space_relation relation;
double delta_s = time_ns_to_s(diff_prediction_ns);
m_predict_relation(&relation_not_predicted, delta_s, &relation);
struct xrt_space_graph xsg = {0};
m_space_graph_add_pose(&xsg, &rs->offset);
m_space_graph_add_relation(&xsg, &relation);
m_space_graph_resolve(&xsg, out_relation);
if (rs->enable_pose_prediction) {
m_predict_relation(&relation_not_predicted, delta_s, out_relation);
} else {
*out_relation = relation_not_predicted;
}
}
static void
rs_6dof_get_view_pose(struct xrt_device *xdev,
@ -390,9 +402,45 @@ struct xrt_device *
rs_6dof_create(void)
{
struct rs_6dof *rs = U_DEVICE_ALLOCATE(struct rs_6dof, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
rs->offset =
(struct xrt_pose){.orientation = {.x = 0, .y = 0, .z = 0, .w = 1}, .position = {.x = 0, .y = 0, .z = 0}};
int ret = 0;
rs->enable_mapping = true;
rs->enable_pose_jumping = true;
rs->enable_relocalization = true;
rs->enable_pose_prediction = true;
rs->enable_pose_filtering = true;
struct u_config_json config_json;
u_config_json_open_or_create_main_file(&config_json);
cJSON *realsense_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_realsense");
if (realsense_config_json != NULL) {
cJSON *mapping = cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_mapping");
cJSON *pose_jumping = cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_jumping");
cJSON *relocalization =
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_relocalization");
cJSON *pose_prediction =
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_prediction");
cJSON *pose_filtering =
cJSON_GetObjectItemCaseSensitive(realsense_config_json, "enable_pose_filtering");
// if json key isn't in the json, default to true. if it is in there, use json value
if (mapping != NULL) {
rs->enable_mapping = cJSON_IsTrue(mapping);
}
if (pose_jumping != NULL) {
rs->enable_pose_jumping = cJSON_IsTrue(pose_jumping);
}
if (relocalization != NULL) {
rs->enable_relocalization = cJSON_IsTrue(relocalization);
}
if (pose_prediction != NULL) {
rs->enable_pose_prediction = cJSON_IsTrue(pose_prediction);
}
if (pose_filtering != NULL) {
rs->enable_pose_filtering = cJSON_IsTrue(pose_filtering);
}
}
U_LOG_D("Realsense opts are %i %i %i %i %i\n", rs->enable_mapping, rs->enable_pose_jumping,
rs->enable_relocalization, rs->enable_pose_prediction, rs->enable_pose_filtering);
rs->base.update_inputs = rs_6dof_update_inputs;
rs->base.get_tracked_pose = rs_6dof_get_tracked_pose;
@ -409,6 +457,10 @@ rs_6dof_create(void)
rs->base.inputs[0].name = XRT_INPUT_GENERIC_TRACKER_POSE;
int ret = 0;
// Thread and other state.
ret = os_thread_helper_init(&rs->oth);
if (ret != 0) {

3
src/xrt/drivers/realsense/rs_interface.h
View file

@ -29,9 +29,6 @@ extern "C" {
struct xrt_device *
rs_6dof_create(void);
void
rs_update_offset(struct xrt_pose offset, struct xrt_device *xdev);
/*!
* Create a auto prober for rs devices.
*