ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add support for D455 on realsense T265 executable

This commit is contained in:
Mateo de Mayo 2022-05-06 14:57:12 -03:00
parent 35e710c9eb
commit e827866904
4 changed files with 107 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
include/basalt/device/rs_t265.h
View File

@ -70,6 +70,15 @@ struct RsPoseData {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 { class RsT265Device {
public: public:
using Ptr = std::shared_ptr<RsT265Device>; using Ptr = std::shared_ptr<RsT265Device>;
@ -77,8 +86,8 @@ class RsT265Device {
static constexpr int IMU_RATE = 200; static constexpr int IMU_RATE = 200;
static constexpr int NUM_CAMS = 2; static constexpr int NUM_CAMS = 2;
RsT265Device(bool manual_exposure, int skip_frames, int webp_quality, RsT265Device(bool is_d455, RsD455Config d455, bool manual_exposure,
double exposure_value = 10.0); int skip_frames, int webp_quality, double exposure_value = 10.0);
void start(); void start();
void stop(); void stop();
@ -96,6 +105,9 @@ class RsT265Device {
tbb::concurrent_bounded_queue<RsPoseData>* pose_data_queue = nullptr; tbb::concurrent_bounded_queue<RsPoseData>* pose_data_queue = nullptr;
private: private:
void disableLaserEmitters();
bool is_d455; // Just a patch to void doing a proper class hierarchy
bool manual_exposure; bool manual_exposure;
int skip_frames; int skip_frames;
int webp_quality; int webp_quality;

82
src/device/rs_t265.cpp
View File

@ -34,25 +34,42 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/ */
#include <basalt/device/rs_t265.h> #include <basalt/device/rs_t265.h>
#include "basalt/utils/assert.h"
std::string get_date(); std::string get_date();
namespace basalt { 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) int webp_quality, double exposure_value)
: manual_exposure(manual_exposure), : is_d455(is_d455),
manual_exposure(manual_exposure),
skip_frames(skip_frames), skip_frames(skip_frames),
webp_quality(webp_quality) { webp_quality(webp_quality) {
rs2::log_to_console(RS2_LOG_SEVERITY_ERROR); rs2::log_to_console(RS2_LOG_SEVERITY_ERROR);
context = rs2::context();
pipe = rs2::pipeline(context); pipe = rs2::pipeline(context);
config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F); config = rs2::config();
config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F);
config.enable_stream(RS2_STREAM_FISHEYE, 1, RS2_FORMAT_Y8); if (is_d455) {
config.enable_stream(RS2_STREAM_FISHEYE, 2, RS2_FORMAT_Y8); BASALT_ASSERT_MSG(d455.accel_fps < d455.gyro_fps,
if (!manual_exposure) { "Accelerometer frequency must be lower than gyroscope's "
config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF); "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) { if (context.query_devices().size() == 0) {
@ -71,19 +88,36 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
} }
auto device = context.query_devices()[0]; auto device = context.query_devices()[0];
device.hardware_reset();
std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME)
<< " connected" << std::endl; << " connected" << std::endl;
sensor = device.query_sensors()[0]; 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; 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_ENABLE_AUTO_EXPOSURE, 0);
sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000); sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);
} }
} }
void RsT265Device::disableLaserEmitters() {
std::vector<rs2::sensor> sensors =
pipe.get_active_profile().get_device().query_sensors();
for (auto&& sensor : sensors) {
if (sensor.is<rs2::depth_stereo_sensor>()) {
sensor.set_option(RS2_OPTION_EMITTER_ENABLED, 0);
}
}
}
void RsT265Device::start() { void RsT265Device::start() {
auto callback = [&](const rs2::frame& frame) { auto callback = [&](const rs2::frame& frame) {
exportCalibration(); exportCalibration();
@ -152,6 +186,7 @@ void RsT265Device::start() {
for (int i = 0; i < NUM_CAMS; ++i) { for (int i = 0; i < NUM_CAMS; ++i) {
rs2::video_frame vf = fs[i].as<rs2::video_frame>(); rs2::video_frame vf = fs[i].as<rs2::video_frame>();
if (!vf) { if (!vf) {
BASALT_ASSERT(false && "Weird frame");
std::cout << "Weird Frame, skipping" << std::endl; std::cout << "Weird Frame, skipping" << std::endl;
return; return;
} }
@ -231,6 +266,7 @@ void RsT265Device::start() {
}; };
profile = pipe.start(config, callback); profile = pipe.start(config, callback);
if (is_d455) disableLaserEmitters();
} }
void RsT265Device::stop() { void RsT265Device::stop() {
@ -259,8 +295,10 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
auto accel_stream = profile.get_stream(RS2_STREAM_ACCEL); auto accel_stream = profile.get_stream(RS2_STREAM_ACCEL);
auto gyro_stream = profile.get_stream(RS2_STREAM_GYRO); auto gyro_stream = profile.get_stream(RS2_STREAM_GYRO);
auto cam0_stream = profile.get_stream(RS2_STREAM_FISHEYE, 1); auto cam0_stream = is_d455 ? profile.get_stream(RS2_STREAM_INFRARED, 1)
auto cam1_stream = profile.get_stream(RS2_STREAM_FISHEYE, 2); : 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 // get gyro extrinsics
if (auto gyro = gyro_stream.as<rs2::motion_stream_profile>()) { if (auto gyro = gyro_stream.as<rs2::motion_stream_profile>()) {
@ -348,16 +386,22 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
// intrinsics // intrinsics
rs2_intrinsics intrinsics = cam.get_intrinsics(); rs2_intrinsics intrinsics = cam.get_intrinsics();
basalt::KannalaBrandtCamera4<Scalar>::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; // std::cout << "Camera intrinsics: " << params.transpose() << std::endl;
basalt::GenericCamera<Scalar> camera; basalt::GenericCamera<Scalar> camera;
basalt::KannalaBrandtCamera4 kannala_brandt(params); if (is_d455) {
camera.variant = kannala_brandt; basalt::PinholeCamera<Scalar>::VecN params;
params << intrinsics.fx, intrinsics.fy, intrinsics.ppx, intrinsics.ppy;
basalt::PinholeCamera pinhole(params);
camera.variant = pinhole;
} else {
basalt::KannalaBrandtCamera4<Scalar>::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); calib->intrinsics.push_back(camera);
} else { } else {

13
src/rs_t265_record.cpp
View File

@ -321,10 +321,19 @@ int main(int argc, char *argv[]) {
CLI::App app{"Record RealSense T265 Data"}; CLI::App app{"Record RealSense T265 Data"};
std::string dataset_path; std::string dataset_path;
bool is_d455 = false;
basalt::RsD455Config d455{};
app.add_option("--dataset-path", dataset_path, "Path to dataset"); app.add_option("--dataset-path", dataset_path, "Path to dataset");
app.add_flag("--manual-exposure", manual_exposure, app.add_flag("--manual-exposure", manual_exposure,
"If set will enable 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 { try {
app.parse(argc, argv); app.parse(argc, argv);
@ -355,8 +364,8 @@ int main(int argc, char *argv[]) {
pose_data_queue.set_capacity(10000); pose_data_queue.set_capacity(10000);
// realsense // realsense
t265_device.reset(new basalt::RsT265Device(manual_exposure, skip_frames, t265_device.reset(new basalt::RsT265Device(
webp_quality, exposure)); is_d455, d455, manual_exposure, skip_frames, webp_quality, exposure));
t265_device->start(); t265_device->start();
imu_log.reset(new pangolin::DataLog); imu_log.reset(new pangolin::DataLog);

21
src/rs_t265_vio.cpp
View File

@ -125,6 +125,10 @@ int main(int argc, char** argv) {
int num_threads = 0; int num_threads = 0;
bool use_double = false; 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"}; CLI::App app{"RealSense T265 Live Vio"};
app.add_option("--show-gui", show_gui, "Show GUI"); 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("--step-by-step", step_by_step, "Path to config file.");
app.add_option("--use-double", use_double, "Use double not float."); 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 { try {
app.parse(argc, argv); app.parse(argc, argv);
} catch (const CLI::ParseError& e) { } catch (const CLI::ParseError& e) {
@ -160,8 +174,11 @@ int main(int argc, char** argv) {
} }
// realsense // realsense
t265_device.reset( int skip_frames = 1; // Use only one frame for every `skip_frames` frames
new basalt::RsT265Device(false, 1, 90, 10.0)); // TODO: add options? 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 // startup device and load calibration
t265_device->start(); t265_device->start();