Add support for D455 on realsense T265 executable

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<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,
+  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 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;

src/device/rs_t265.cpp

@@ -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 = 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 (!manual_exposure) {
-    config.enable_stream(RS2_STREAM_POSE, RS2_FORMAT_6DOF);
   }
 
   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 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
   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();
+      // std::cout << "Camera intrinsics: " << params.transpose() << std::endl;
+
+      basalt::GenericCamera<Scalar> camera;
+      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];
-
-      // std::cout << "Camera intrinsics: " << params.transpose() << std::endl;
-
-      basalt::GenericCamera<Scalar> camera;
         basalt::KannalaBrandtCamera4 kannala_brandt(params);
         camera.variant = kannala_brandt;
+      }
 
       calib->intrinsics.push_back(camera);
     } else {

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,
+  t265_device.reset(new basalt::RsT265Device(
-                                             webp_quality, exposure));
+      is_d455, d455, manual_exposure, skip_frames, webp_quality, exposure));
 
   t265_device->start();
   imu_log.reset(new pangolin::DataLog);

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(
+  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
   t265_device->start();