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
};
// 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<RsT265Device>;
@ -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<RsPoseData>* 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;

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/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<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() {
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<rs2::video_frame>();
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<basalt::Calibration<double>> 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<rs2::motion_stream_profile>()) {
@ -348,16 +386,22 @@ std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
// 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;
basalt::GenericCamera<Scalar> camera;
basalt::KannalaBrandtCamera4 kannala_brandt(params);
camera.variant = kannala_brandt;
if (is_d455) {
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);
} 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"};
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);

21
src/rs_t265_vio.cpp
View File

@ -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();