rename device

include/basalt/device/rs_t265.h

@@ -73,17 +73,18 @@ struct RsPoseData {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-class Device {
+class RsT265Device {
  public:
-  using Ptr = std::shared_ptr<Device>;
+  using Ptr = std::shared_ptr<RsT265Device>;
 
   static constexpr int IMU_RATE = 200;
   static constexpr int NUM_CAMS = 2;
 
-  Device(bool manual_exposure, int skip_frames, int webp_quality,
+  RsT265Device(bool manual_exposure, int skip_frames, int webp_quality,
                double exposure_value = 10.0);
-  ~Device();
+  ~RsT265Device();
   void start();
+  void stop();
 
   bool setExposure(double exposure);  // in milliseconds
   void setSkipFrames(int skip);
@@ -102,15 +103,16 @@ class Device {
   int skip_frames;
   int webp_quality;
 
-  rs2::config config;
-  rs2::pipeline pipe;
-  rs2::context context;
-  rs2::pipeline_profile profile;
-
   Eigen::deque<RsIMUData> gyro_data_queue;
   std::shared_ptr<RsIMUData> prev_accel_data;
 
   std::shared_ptr<basalt::Calibration<double>> calib;
+
+  rs2::context context;
+  rs2::config config;
+  rs2::pipeline pipe;
+
+  rs2::pipeline_profile profile;
 };
 
 }  // namespace basalt

src/device/rs_t265.cpp

@@ -39,8 +39,8 @@ std::string get_date();
 
 namespace basalt {
 
-Device::Device(bool manual_exposure, int skip_frames, int webp_quality,
+RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
-               double exposure_value)
+                           int webp_quality, double exposure_value)
     : manual_exposure(manual_exposure),
       skip_frames(skip_frames),
       webp_quality(webp_quality) {
@@ -82,9 +82,9 @@ Device::Device(bool manual_exposure, int skip_frames, int webp_quality,
   }
 }
 
-Device::~Device() = default;
+RsT265Device::~RsT265Device(){};
 
-void Device::start() {
+void RsT265Device::start() {
   auto callback = [&](const rs2::frame& frame) {
     exportCalibration();
 
@@ -206,7 +206,12 @@ void Device::start() {
   profile = pipe.start(config, callback);
 }
 
-bool Device::setExposure(double exposure) {
+void RsT265Device::stop() {
+  if (image_data_queue) image_data_queue->push(nullptr);
+  if (imu_data_queue) imu_data_queue->push(nullptr);
+}
+
+bool RsT265Device::setExposure(double exposure) {
   if (!manual_exposure) return false;
 
   auto device = context.query_devices()[0];
@@ -215,11 +220,11 @@ bool Device::setExposure(double exposure) {
   return true;
 }
 
-void Device::setSkipFrames(int skip) { skip_frames = skip; }
+void RsT265Device::setSkipFrames(int skip) { skip_frames = skip; }
 
-void Device::setWebpQuality(int quality) { webp_quality = quality; }
+void RsT265Device::setWebpQuality(int quality) { webp_quality = quality; }
 
-std::shared_ptr<basalt::Calibration<double>> Device::exportCalibration() {
+std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
   using Scalar = double;
 
   if (calib.get()) return calib;

src/rs_t265_record.cpp

@@ -64,7 +64,7 @@ namespace fs = std::experimental::filesystem;
 
 constexpr int UI_WIDTH = 200;
 
-basalt::Device::Ptr t265_device;
+basalt::RsT265Device::Ptr t265_device;
 
 std::shared_ptr<pangolin::DataLog> imu_log;
 
@@ -81,7 +81,7 @@ std::atomic<bool> recording;
 
 std::string dataset_dir;
 
-static constexpr int NUM_CAMS = basalt::Device::NUM_CAMS;
+static constexpr int NUM_CAMS = basalt::RsT265Device::NUM_CAMS;
 static constexpr int NUM_WORKERS = 8;
 
 std::ofstream cam_data[NUM_CAMS], exposure_data[NUM_CAMS], imu0_data, pose_data;
@@ -192,7 +192,7 @@ void pose_save_worker() {
   }
 }
 
-void save_calibration(const basalt::Device::Ptr &device) {
+void save_calibration(const basalt::RsT265Device::Ptr &device) {
   auto calib = device->exportCalibration();
 
   if (calib) {
@@ -317,8 +317,8 @@ int main(int argc, char *argv[]) {
   pose_data_queue.set_capacity(10000);
 
   // realsense
-  t265_device.reset(
+  t265_device.reset(new basalt::RsT265Device(manual_exposure, skip_frames,
-      new basalt::Device(manual_exposure, skip_frames, webp_quality, exposure));
+                                             webp_quality, exposure));
 
   t265_device->image_data_queue = &image_data_queue;
   t265_device->imu_data_queue = &imu_data_queue;
@@ -352,7 +352,7 @@ int main(int argc, char *argv[]) {
                                           pangolin::Attach::Pix(UI_WIDTH));
 
     std::vector<std::shared_ptr<pangolin::ImageView>> img_view;
-    while (img_view.size() < basalt::Device::NUM_CAMS) {
+    while (img_view.size() < basalt::RsT265Device::NUM_CAMS) {
       int idx = img_view.size();
       std::shared_ptr<pangolin::ImageView> iv(new pangolin::ImageView);
 
@@ -414,7 +414,8 @@ int main(int argc, char *argv[]) {
         fmt.scalable_internal_format = GL_LUMINANCE16;
 
         if (t265_device->last_img_data.get())
-          for (size_t cam_id = 0; cam_id < basalt::Device::NUM_CAMS; cam_id++) {
+          for (size_t cam_id = 0; cam_id < basalt::RsT265Device::NUM_CAMS;
+               cam_id++) {
             if (t265_device->last_img_data->img_data[cam_id].img.get())
               img_view[cam_id]->SetImage(
                   t265_device->last_img_data->img_data[cam_id].img->ptr,

src/rs_t265_vio.cpp

@@ -74,7 +74,7 @@ void draw_plots();
 // Pangolin variables
 constexpr int UI_WIDTH = 200;
 
-basalt::Device::Ptr t265_device;
+basalt::RsT265Device::Ptr t265_device;
 
 using Button = pangolin::Var<std::function<void(void)>>;
 
@@ -106,7 +106,6 @@ std::string marg_data_path;
 
 std::mutex m;
 bool step_by_step = false;
-std::atomic<bool> terminate;
 int64_t curr_t_ns = -1;
 
 // VIO variables
@@ -117,7 +116,6 @@ basalt::OpticalFlowBase::Ptr opt_flow_ptr;
 basalt::VioEstimatorBase::Ptr vio;
 
 int main(int argc, char** argv) {
-  terminate = false;
   bool show_gui = true;
   bool print_queue = false;
   std::string cam_calib_path;
@@ -156,7 +154,7 @@ int main(int argc, char** argv) {
 
   // realsense
   t265_device.reset(
-      new basalt::Device(false, 1, 90, 10.0));  // TODO: add options?
+      new basalt::RsT265Device(false, 1, 90, 10.0));  // TODO: add options?
 
   // startup device and load calibration
   t265_device->start();
@@ -193,6 +191,8 @@ int main(int argc, char** argv) {
     t3.reset(new std::thread([&]() {
       while (true) {
         out_vis_queue.pop(curr_vis_data);
+
+        if (!curr_vis_data.get()) break;
       }
 
       std::cout << "Finished t3" << std::endl;
@@ -326,7 +326,8 @@ int main(int argc, char** argv) {
             curr_vis_data->opt_flow_res->input_images.get()) {
           auto& img_data = curr_vis_data->opt_flow_res->input_images->img_data;
 
-          for (size_t cam_id = 0; cam_id < basalt::Device::NUM_CAMS; cam_id++) {
+          for (size_t cam_id = 0; cam_id < basalt::RsT265Device::NUM_CAMS;
+               cam_id++) {
             if (img_data[cam_id].img.get())
               img_view[cam_id]->SetImage(
                   img_data[cam_id].img->ptr, img_data[cam_id].img->w,
@@ -346,7 +347,8 @@ int main(int argc, char** argv) {
     }
   }
 
-  terminate = true;
+  t265_device->stop();
+
   if (t3.get()) t3->join();
   t4.join();