rename device
This commit is contained in:
Vladyslav Usenko
parent
ae199327fe
commit
e1a1c3c3d0
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<basalt::Calibration<double>> Device::exportCalibration() {
|
||||
std::shared_ptr<basalt::Calibration<double>> RsT265Device::exportCalibration() {
|
||||
using Scalar = double;
|
||||
|
||||
if (calib.get()) return calib;
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
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<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,
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue