From e8278669046329bd5d008c2c58b1fcd486865edf Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Fri, 6 May 2022 14:57:12 -0300 Subject: [PATCH] Add support for D455 on realsense T265 executable --- include/basalt/device/rs_t265.h | 16 ++++++- src/device/rs_t265.cpp | 82 +++++++++++++++++++++++++-------- src/rs_t265_record.cpp | 13 +++++- src/rs_t265_vio.cpp | 21 ++++++++- 4 files changed, 107 insertions(+), 25 deletions(-) diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index 462c6d2..6e607db 100644 --- a/include/basalt/device/rs_t265.h +++ b/include/basalt/device/rs_t265.h @@ -70,6 +70,15 @@ struct RsPoseData { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +// Struct to pass around D455 config params +struct RsD455Config { + int video_width = 640; + int video_height = 360; + int video_fps = 30; + int accel_fps = 63; + int gyro_fps = 200; +}; + class RsT265Device { public: using Ptr = std::shared_ptr; @@ -77,8 +86,8 @@ class RsT265Device { static constexpr int IMU_RATE = 200; static constexpr int NUM_CAMS = 2; - RsT265Device(bool manual_exposure, int skip_frames, int webp_quality, - double exposure_value = 10.0); + RsT265Device(bool is_d455, RsD455Config d455, bool manual_exposure, + int skip_frames, int webp_quality, double exposure_value = 10.0); void start(); void stop(); @@ -96,6 +105,9 @@ class RsT265Device { tbb::concurrent_bounded_queue* pose_data_queue = nullptr; private: + void disableLaserEmitters(); + + bool is_d455; // Just a patch to void doing a proper class hierarchy bool manual_exposure; int skip_frames; int webp_quality; diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index 462da01..f3cda57 100644 --- a/src/device/rs_t265.cpp +++ b/src/device/rs_t265.cpp @@ -34,25 +34,42 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include +#include "basalt/utils/assert.h" std::string get_date(); namespace basalt { -RsT265Device::RsT265Device(bool manual_exposure, int skip_frames, +RsT265Device::RsT265Device(bool is_d455, RsD455Config d455, + bool manual_exposure, int skip_frames, int webp_quality, double exposure_value) - : manual_exposure(manual_exposure), + : is_d455(is_d455), + manual_exposure(manual_exposure), skip_frames(skip_frames), webp_quality(webp_quality) { rs2::log_to_console(RS2_LOG_SEVERITY_ERROR); + context = rs2::context(); pipe = rs2::pipeline(context); - config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); - config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); - config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8); - config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8); - if (!manual_exposure) { - config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); + config = rs2::config(); + + if (is_d455) { + BASALT_ASSERT_MSG(d455.accel_fps < d455.gyro_fps, + "Accelerometer frequency must be lower than gyroscope's " + "because of how IMU interpolation is implemented"); + config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F, + d455.accel_fps); + config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F, + d455.gyro_fps); + config.enable_stream(RS2_STREAM_INFRARED, 1, d455.video_width, + d455.video_height, RS2_FORMAT_Y8, d455.video_fps); + config.enable_stream(RS2_STREAM_INFRARED, 2, d455.video_width, + d455.video_height, RS2_FORMAT_Y8, d455.video_fps); + } else { + config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); + config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F); + config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8); + config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8); } if (context.query_devices().size() == 0) { @@ -71,19 +88,36 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames, } auto device = context.query_devices()[0]; - device.hardware_reset(); std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) << " connected" << std::endl; sensor = device.query_sensors()[0]; - if (manual_exposure) { + if (!manual_exposure) { + if (is_d455) { + // Enable autoexposure on stereo sensor + sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 1); + } else { + // Not sure why this happens here, but it was here from before + config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); + } + } else { std::cout << "Enabling manual exposure control" << std::endl; sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000); } } +void RsT265Device::disableLaserEmitters() { + std::vector sensors = + pipe.get_active_profile().get_device().query_sensors(); + for (auto&& sensor : sensors) { + if (sensor.is()) { + sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0); + } + } +} + void RsT265Device::start() { auto callback = [&](const rs2::frame& frame) { exportCalibration(); @@ -152,6 +186,7 @@ void RsT265Device::start() { for (int i = 0; i < NUM_CAMS; ++i) { rs2::video_frame vf = fs[i].as(); if (!vf) { + BASALT_ASSERT(false && "Weird frame"); std::cout << "Weird Frame, skipping" << std::endl; return; } @@ -231,6 +266,7 @@ void RsT265Device::start() { }; profile = pipe.start(config, callback); + if (is_d455) disableLaserEmitters(); } void RsT265Device::stop() { @@ -259,8 +295,10 @@ std::shared_ptr> RsT265Device::exportCalibration() { auto accel_stream = profile.get_stream(RS2_STREAM_ACCEL); auto gyro_stream = profile.get_stream(RS2_STREAM_GYRO); - auto cam0_stream = profile.get_stream(RS2_STREAM_FISHEYE, 1); - auto cam1_stream = profile.get_stream(RS2_STREAM_FISHEYE, 2); + auto cam0_stream = is_d455 ? profile.get_stream(RS2_STREAM_INFRARED, 1) + : profile.get_stream(RS2_STREAM_FISHEYE, 1); + auto cam1_stream = is_d455 ? profile.get_stream(RS2_STREAM_INFRARED, 2) + : profile.get_stream(RS2_STREAM_FISHEYE, 2); // get gyro extrinsics if (auto gyro = gyro_stream.as()) { @@ -348,16 +386,22 @@ std::shared_ptr> RsT265Device::exportCalibration() { // intrinsics rs2_intrinsics intrinsics = cam.get_intrinsics(); - basalt::KannalaBrandtCamera4::VecN params; - params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy, - intrinsics.coeffs[0], intrinsics.coeffs[1], intrinsics.coeffs[2], - intrinsics.coeffs[3]; - // std::cout << "Camera intrinsics: " << params.transpose() << std::endl; basalt::GenericCamera camera; - basalt::KannalaBrandtCamera4 kannala_brandt(params); - camera.variant = kannala_brandt; + if (is_d455) { + basalt::PinholeCamera::VecN params; + params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy; + basalt::PinholeCamera pinhole(params); + camera.variant = pinhole; + } else { + basalt::KannalaBrandtCamera4::VecN params; + params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy, + intrinsics.coeffs[0], intrinsics.coeffs[1], intrinsics.coeffs[2], + intrinsics.coeffs[3]; + basalt::KannalaBrandtCamera4 kannala_brandt(params); + camera.variant = kannala_brandt; + } calib->intrinsics.push_back(camera); } else { diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp index 7889f28..1b3890c 100644 --- a/src/rs_t265_record.cpp +++ b/src/rs_t265_record.cpp @@ -321,10 +321,19 @@ int main(int argc, char *argv[]) { CLI::App app{"Record RealSense T265 Data"}; std::string dataset_path; + bool is_d455 = false; + basalt::RsD455Config d455{}; app.add_option("--dataset-path", dataset_path, "Path to dataset"); app.add_flag("--manual-exposure", manual_exposure, "If set will enable manual exposure."); + app.add_flag("--is-d455", is_d455, + "If set will work on a D455 (probably on a D435 too)"); + app.add_option("--d455-video-width", d455.video_width, "Frame width"); + app.add_option("--d455-video-height", d455.video_height, "Frame height"); + app.add_option("--d455-video-fps", d455.video_fps, "Video FPS"); + app.add_option("--d455-accel-fps", d455.accel_fps, "Accelerometer FPS"); + app.add_option("--d455-gyro-fps", d455.gyro_fps, "Gyroscope FPS"); try { app.parse(argc, argv); @@ -355,8 +364,8 @@ int main(int argc, char *argv[]) { pose_data_queue.set_capacity(10000); // realsense - t265_device.reset(new basalt::RsT265Device(manual_exposure, skip_frames, - webp_quality, exposure)); + t265_device.reset(new basalt::RsT265Device( + is_d455, d455, manual_exposure, skip_frames, webp_quality, exposure)); t265_device->start(); imu_log.reset(new pangolin::DataLog); diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index e4a6c0b..6efcec7 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -125,6 +125,10 @@ int main(int argc, char** argv) { int num_threads = 0; bool use_double = false; + double exposure = 0.0; // 0 means auto, else use something like 10.0 or 33.0 + bool is_d455 = false; + basalt::RsD455Config d455{}; + CLI::App app{"RealSense T265 Live Vio"}; app.add_option("--show-gui", show_gui, "Show GUI"); @@ -140,6 +144,16 @@ int main(int argc, char** argv) { app.add_option("--step-by-step", step_by_step, "Path to config file."); app.add_option("--use-double", use_double, "Use double not float."); + app.add_option("--exposure", exposure, + "Shutter time in ms, 0 by default to use auto exposure"); + app.add_flag("--is-d455", is_d455, + "If set will work on a D455 (probably on a D435 too)"); + app.add_option("--d455-video-width", d455.video_width, "Frame width"); + app.add_option("--d455-video-height", d455.video_height, "Frame height"); + app.add_option("--d455-video-fps", d455.video_fps, "Video FPS"); + app.add_option("--d455-accel-fps", d455.accel_fps, "Accelerometer FPS"); + app.add_option("--d455-gyro-fps", d455.gyro_fps, "Gyroscope FPS"); + try { app.parse(argc, argv); } catch (const CLI::ParseError& e) { @@ -160,8 +174,11 @@ int main(int argc, char** argv) { } // realsense - t265_device.reset( - new basalt::RsT265Device(false, 1, 90, 10.0)); // TODO: add options? + int skip_frames = 1; // Use only one frame for every `skip_frames` frames + int webp_quality = 90; // This is not being used + bool manual_exposure = exposure != 0.0; + t265_device.reset(new basalt::RsT265Device( + is_d455, d455, manual_exposure, skip_frames, webp_quality, exposure)); // startup device and load calibration t265_device->start();