diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index a57b6de..f317462 100644 --- a/include/basalt/device/rs_t265.h +++ b/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; + using Ptr = std::shared_ptr; static constexpr int IMU_RATE = 200; static constexpr int NUM_CAMS = 2; - Device(bool manual_exposure, int skip_frames, int webp_quality, - double exposure_value = 10.0); - ~Device(); + RsT265Device(bool manual_exposure, int skip_frames, int webp_quality, + double exposure_value = 10.0); + ~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 gyro_data_queue; std::shared_ptr prev_accel_data; std::shared_ptr> calib; + + rs2::context context; + rs2::config config; + rs2::pipeline pipe; + + rs2::pipeline_profile profile; }; } // namespace basalt diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index 9321e65..bb93ea1 100644 --- a/src/device/rs_t265.cpp +++ b/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, - double exposure_value) +RsT265Device::RsT265Device(bool manual_exposure, int skip_frames, + 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> Device::exportCalibration() { +std::shared_ptr> RsT265Device::exportCalibration() { using Scalar = double; if (calib.get()) return calib; diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp index 0214923..58fbe04 100644 --- a/src/rs_t265_record.cpp +++ b/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 imu_log; @@ -81,7 +81,7 @@ std::atomic 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( - new basalt::Device(manual_exposure, skip_frames, webp_quality, exposure)); + t265_device.reset(new basalt::RsT265Device(manual_exposure, skip_frames, + 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> 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 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, diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 511c9d4..f555d23 100644 --- a/src/rs_t265_vio.cpp +++ b/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>; @@ -106,7 +106,6 @@ std::string marg_data_path; std::mutex m; bool step_by_step = false; -std::atomic 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();