T265 record manual exposure control, compare calib script

include/basalt/device/rs_t265.h

@@ -103,6 +103,8 @@ class RsT265Device {
   int skip_frames;
   int webp_quality;
 
+  int frame_counter = 0;
+
   Eigen::deque<RsIMUData> gyro_data_queue;
   std::shared_ptr<RsIMUData> prev_accel_data;
 
@@ -111,6 +113,7 @@ class RsT265Device {
   rs2::context context;
   rs2::config config;
   rs2::pipeline pipe;
+  rs2::sensor sensor;
 
   rs2::pipeline_profile profile;
 };

scripts/compare_calib.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python3
+
+
+import argparse
+import json
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def print_abs_rel(info, v_0, v_1):
+    diff = np.abs(np.linalg.norm(v_0 - v_1))
+    out = f'{info}:\t{diff:.5f}'
+
+    if diff < 10e-7:
+        out += ' (0.0%)'
+    else:
+        out += f' ({diff / (np.abs(np.linalg.norm(v_0)) * 100.0):.7f}%)'
+
+    print(out)
+
+
+def main(calib_path_1, calib_path_2):
+    with open(calib_path_1, 'r') as c_1, open(calib_path_2, 'r') as c_2:
+        calib0 = json.load(c_1)
+        calib1 = json.load(c_2)
+
+    for i, (t_imu_cam_0, t_imu_cam_1) in enumerate(
+            zip(calib0['value0']['T_imu_cam'], calib1['value0']['T_imu_cam'])):
+        print(f'\nCamera {i} transformation differences')
+        t_0 = np.array(list(t_imu_cam_0.values())[0:2])
+        t_1 = np.array(list(t_imu_cam_1.values())[0:2])
+        r_0 = Rotation(list(t_imu_cam_0.values())[3:7])
+        r_1 = Rotation(list(t_imu_cam_1.values())[3:7])
+
+        print_abs_rel(f'Transformation', t_0, t_1)
+        print_abs_rel(f'Rotation', r_0.as_rotvec(), r_1.as_rotvec())
+
+    for i, (intrinsics0, intrinsics1) in enumerate(
+            zip(calib0['value0']['intrinsics'], calib1['value0']['intrinsics'])):
+        print(f'\nCamera {i} intrinsics differences')
+
+        for (
+                k_0, v_0), (_, v_1) in zip(
+                intrinsics0['intrinsics'].items(), intrinsics1['intrinsics'].items()):
+            print_abs_rel(f'Difference for {k_0}', v_0, v_1)
+
+    print_abs_rel('\nAccel Bias Difference',
+                  np.array(calib0['value0']['calib_accel_bias'][0:2]),
+                  np.array(calib1['value0']['calib_accel_bias'][0:2]))
+
+    print_abs_rel('Accel Scale Difference',
+                  np.array(calib0['value0']['calib_accel_bias'][3:9]),
+                  np.array(calib1['value0']['calib_accel_bias'][3:9]))
+
+    print_abs_rel('Gyro Bias Difference',
+                  np.array(calib0['value0']['calib_gyro_bias'][0:2]),
+                  np.array(calib1['value0']['calib_gyro_bias'][0:2]))
+
+    print_abs_rel('Gyro Scale Difference',
+                  np.array(calib0['value0']['calib_gyro_bias'][3:12]),
+                  np.array(calib1['value0']['calib_gyro_bias'][3:12]))
+
+    print_abs_rel(
+        '\nAccel Noise Std Difference',
+        calib0['value0']['accel_noise_std'],
+        calib1['value0']['accel_noise_std'])
+    print_abs_rel(
+        'Gyro Noise Std Difference',
+        calib0['value0']['gyro_noise_std'],
+        calib1['value0']['gyro_noise_std'])
+    print_abs_rel(
+        'Accel Bias Std Difference',
+        calib0['value0']['accel_bias_std'],
+        calib1['value0']['accel_bias_std'])
+    print_abs_rel(
+        'Gyro Bias Std Difference',
+        calib0['value0']['gyro_bias_std'],
+        calib1['value0']['gyro_bias_std'])
+
+    print_abs_rel(
+        '\nCam Time Offset Difference',
+        calib0['value0']['cam_time_offset_ns'],
+        calib0['value0']['cam_time_offset_ns'])
+
+
+def create_parser():
+    parser = argparse.ArgumentParser()
+    parser.add_argument('calib_path_1')
+    parser.add_argument('calib_path_2')
+
+    return parser
+
+
+if __name__ == '__main__':
+    args = create_parser().parse_args()
+
+    main(args.calib_path_1, args.calib_path_2)

src/device/rs_t265.cpp

@@ -70,15 +70,15 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
     }
   }
 
-  if (manual_exposure) {
+  auto device = context.query_devices()[0];
-    auto device = context.query_devices()[0];
+  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];
-    auto sens = device.query_sensors()[0];
 
+  if (manual_exposure) {
     std::cout << "Enabling manual exposure control" << std::endl;
-    sens.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
+    sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0);
-    sens.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);
+    sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000);
   }
 }
 
@@ -153,6 +153,8 @@ void RsT265Device::start() {
         }
       }
     } else if (auto fs = frame.as<rs2::frameset>()) {
+      if (frame_counter++ % skip_frames != 0) return;
+
       OpticalFlowInput::Ptr data(new OpticalFlowInput);
       data->img_data.resize(NUM_CAMS);
 
@@ -214,9 +216,7 @@ void RsT265Device::stop() {
 bool RsT265Device::setExposure(double exposure) {
   if (!manual_exposure) return false;
 
-  auto device = context.query_devices()[0];
+  sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000);
-  auto sens = device.query_sensors()[0];
-  sens.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000);
   return true;
 }
 

src/rs_t265_record.cpp

@@ -97,7 +97,7 @@ void image_save_worker() {
 
   while (!stop_workers) {
     if (image_data_queue.try_pop(img)) {
-      if (recording)
+      if (recording) {
         for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
 #if CV_MAJOR_VERSION >= 3
           cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp"
@@ -112,38 +112,39 @@ void image_save_worker() {
                                 << std::endl;
         }
 
-      for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
+        for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
-        basalt::ManagedImage<uint16_t>::Ptr image_raw =
+          basalt::ManagedImage<uint16_t>::Ptr image_raw =
-            img->img_data[cam_id].img;
+              img->img_data[cam_id].img;
 
-        if (!image_raw.get()) continue;
+          if (!image_raw.get()) continue;
 
-        cv::Mat image(image_raw->h, image_raw->w, CV_8U);
+          cv::Mat image(image_raw->h, image_raw->w, CV_8U);
 
-        uint8_t *dst = image.ptr();
+          uint8_t *dst = image.ptr();
-        const uint16_t *src = image_raw->ptr;
+          const uint16_t *src = image_raw->ptr;
 
-        for (size_t i = 0; i < image_raw->size(); i++) {
+          for (size_t i = 0; i < image_raw->size(); i++) {
-          dst[i] = (src[i] >> 8);
+            dst[i] = (src[i] >> 8);
-        }
+          }
 
 #if CV_MAJOR_VERSION >= 3
-        std::string filename = dataset_dir + "mav0/cam" +
+          std::string filename = dataset_dir + "mav0/cam" +
-                               std::to_string(cam_id) + "/data/" +
+                                 std::to_string(cam_id) + "/data/" +
-                               std::to_string(img->t_ns) + ".webp";
+                                 std::to_string(img->t_ns) + ".webp";
 
-        std::vector<int> compression_params = {cv::IMWRITE_WEBP_QUALITY,
+          std::vector<int> compression_params = {cv::IMWRITE_WEBP_QUALITY,
-                                               webp_quality};
+                                                 webp_quality};
-        cv::imwrite(filename, image, compression_params);
+          cv::imwrite(filename, image, compression_params);
 #else
-        std::string filename = dataset_dir + "mav0/cam" +
+          std::string filename = dataset_dir + "mav0/cam" +
-                               std::to_string(cam_id) + "/data/" +
+                                 std::to_string(cam_id) + "/data/" +
-                               std::to_string(img->t_ns) + ".jpg";
+                                 std::to_string(img->t_ns) + ".jpg";
 
-        std::vector<int> compression_params = {cv::IMWRITE_JPEG_QUALITY,
+          std::vector<int> compression_params = {cv::IMWRITE_JPEG_QUALITY,
-                                               webp_quality};
+                                                 webp_quality};
-        cv::imwrite(filename, image, compression_params);
+          cv::imwrite(filename, image, compression_params);
 #endif
+        }
       }
     } else {
       std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -272,6 +273,7 @@ void stopRecording() {
     exposure_data[0].close();
     exposure_data[1].close();
     imu0_data.close();
+    pose_data.close();
 
     std::cout << "Stopped recording dataset in " << dataset_dir << std::endl;
   }
@@ -300,6 +302,10 @@ int main(int argc, char *argv[]) {
     return app.exit(e);
   }
 
+  if (dataset_path[dataset_path.length() - 1] != '/') {
+    dataset_path += '/';
+  }
+
   bool show_gui = true;
 
   stop_workers = false;
@@ -379,6 +385,17 @@ int main(int argc, char *argv[]) {
         }
       };
 
+      iv->OnSelectionCallback =
+          [&](pangolin::ImageView::OnSelectionEventData o) {
+            int64_t curr_t_ns = std::chrono::high_resolution_clock::now()
+                                    .time_since_epoch()
+                                    .count();
+            if (std::abs(record_t_ns - curr_t_ns) > int64_t(2e9)) {
+              toggleRecording(dataset_path);
+              record_t_ns = curr_t_ns;
+            }
+          };
+
       img_view.push_back(iv);
       img_view_display.AddDisplay(*iv);
     }

src/rs_t265_vio.cpp

@@ -116,6 +116,7 @@ basalt::OpticalFlowBase::Ptr opt_flow_ptr;
 basalt::VioEstimatorBase::Ptr vio;
 
 int main(int argc, char** argv) {
+  bool terminate = false;
   bool show_gui = true;
   bool print_queue = false;
   std::string cam_calib_path;
@@ -239,7 +240,7 @@ int main(int argc, char** argv) {
 
   if (print_queue) {
     t5.reset(new std::thread([&]() {
-      while (true) {
+      while (!terminate) {
         std::cout << "opt_flow_ptr->input_queue "
                   << opt_flow_ptr->input_queue.size()
                   << " opt_flow_ptr->output_queue "
@@ -348,9 +349,11 @@ int main(int argc, char** argv) {
   }
 
   t265_device->stop();
+  terminate = true;
 
   if (t3.get()) t3->join();
   t4.join();
+  if (t5.get()) t5->join();
 
   return 0;
 }