rename device

This commit is contained in:
Vladyslav Usenko 2019-06-13 18:21:37 +02:00
parent ae199327fe
commit e1a1c3c3d0
4 changed files with 41 additions and 31 deletions

View File

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

View File

@ -39,8 +39,8 @@ std::string get_date();
namespace basalt { 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), : manual_exposure(manual_exposure),
skip_frames(skip_frames), skip_frames(skip_frames),
webp_quality(webp_quality) { 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) { auto callback = [&](const rs2::frame& frame) {
exportCalibration(); exportCalibration();
@ -206,7 +206,12 @@ void Device::start() {
profile = pipe.start(config, callback); 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; if (!manual_exposure) return false;
auto device = context.query_devices()[0]; auto device = context.query_devices()[0];
@ -215,11 +220,11 @@ bool Device::setExposure(double exposure) {
return true; 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; using Scalar = double;
if (calib.get()) return calib; if (calib.get()) return calib;

View File

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

View File

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