From 85872d521990fad07e94cb6645dc94cc2ec174aa Mon Sep 17 00:00:00 2001 From: Michael Loipfuehrer Date: Mon, 29 Jul 2019 13:08:54 +0000 Subject: [PATCH] Add device pose data to vio analysis --- CMakeLists.txt | 3 + include/basalt/io/dataset_io.h | 2 + include/basalt/io/dataset_io_euroc.h | 40 +++ include/basalt/io/dataset_io_rosbag.h | 10 + src/time_alignment.cpp | 468 ++++++++++++++++++++++++++ src/vio.cpp | 26 +- 6 files changed, 548 insertions(+), 1 deletion(-) create mode 100644 src/time_alignment.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a75d7a9..3e9688d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -247,6 +247,9 @@ target_link_libraries(basalt_opt_flow basalt pangolin) add_executable(basalt_vio src/vio.cpp) target_link_libraries(basalt_vio basalt pangolin) +add_executable(basalt_time_alignment src/time_alignment.cpp) +target_link_libraries(basalt_time_alignment basalt pangolin) + find_package(realsense2 QUIET) if(realsense2_FOUND) add_executable(basalt_rs_t265_record src/rs_t265_record.cpp src/device/rs_t265.cpp) diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h index 4b6008f..70138b2 100644 --- a/include/basalt/io/dataset_io.h +++ b/include/basalt/io/dataset_io.h @@ -127,6 +127,8 @@ class VioDataset { virtual const Eigen::vector &get_gyro_data() const = 0; virtual const std::vector &get_gt_timestamps() const = 0; virtual const Eigen::vector &get_gt_pose_data() const = 0; + virtual const std::vector &get_device_pose_timestamps() const = 0; + virtual const Eigen::vector &get_device_pose_data() const = 0; virtual int64_t get_mocap_to_imu_offset_ns() const = 0; virtual std::vector get_image_data(int64_t t_ns) = 0; diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index 66878bb..7ce4f68 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -63,6 +63,10 @@ class EurocVioDataset : public VioDataset { std::vector gt_timestamps; // ordered gt timestamps Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector device_pose_timestamps; // ordered gt timestamps + Eigen::vector + device_pose_data; // TODO: change to eigen aligned + int64_t mocap_to_imu_offset_ns; std::vector> exposure_times; @@ -82,6 +86,12 @@ class EurocVioDataset : public VioDataset { const Eigen::vector &get_gt_pose_data() const { return gt_pose_data; } + const std::vector &get_device_pose_timestamps() const { + return device_pose_timestamps; + } + const Eigen::vector &get_device_pose_data() const { + return device_pose_data; + } int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } std::vector get_image_data(int64_t t_ns) { @@ -164,10 +174,16 @@ class EurocIO : public DatasetIoInterface { if (file_exists(path + "/mav0/state_groundtruth_estimate0/data.csv")) { read_gt_data_state(path + "/mav0/state_groundtruth_estimate0/"); + } else if (file_exists(path + "/mav0/gt/data.csv")) { + read_gt_data_pose(path + "/mav0/gt/"); } else if (file_exists(path + "/mav0/mocap0/data.csv")) { read_gt_data_pose(path + "/mav0/mocap0/"); } + if (file_exists(path + "/mav0/realsense0/data.csv")) { + read_device_data_pose(path + "/mav0/realsense0/"); + } + data->exposure_times.resize(data->num_cams); if (file_exists(path + "/mav0/cam0/exposure.csv")) { std::cout << "Loading exposure times for cam0" << std::endl; @@ -300,6 +316,30 @@ class EurocIO : public DatasetIoInterface { } } + void read_device_data_pose(const std::string &path) { + data->device_pose_timestamps.clear(); + data->device_pose_data.clear(); + + std::ifstream f(path + "data.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + uint64_t timestamp; + Eigen::Quaterniond q; + Eigen::Vector3d pos; + + ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >> + tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z(); + + data->device_pose_timestamps.emplace_back(timestamp); + data->device_pose_data.emplace_back(q, pos); + } + } + std::shared_ptr data; }; diff --git a/include/basalt/io/dataset_io_rosbag.h b/include/basalt/io/dataset_io_rosbag.h index 76b58a2..38dc39c 100644 --- a/include/basalt/io/dataset_io_rosbag.h +++ b/include/basalt/io/dataset_io_rosbag.h @@ -76,6 +76,10 @@ class RosbagVioDataset : public VioDataset { std::vector gt_timestamps; // ordered gt timestamps Eigen::vector gt_pose_data; // TODO: change to eigen aligned + std::vector device_pose_timestamps; // ordered gt timestamps + Eigen::vector + device_pose_data; // TODO: change to eigen aligned + int64_t mocap_to_imu_offset_ns; public: @@ -93,6 +97,12 @@ class RosbagVioDataset : public VioDataset { const Eigen::vector &get_gt_pose_data() const { return gt_pose_data; } + const std::vector &get_device_pose_timestamps() const { + return device_pose_timestamps; + } + const Eigen::vector &get_device_pose_data() const { + return device_pose_data; + } int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; } std::vector get_image_data(int64_t t_ns) { diff --git a/src/time_alignment.cpp b/src/time_alignment.cpp new file mode 100644 index 0000000..b157fd7 --- /dev/null +++ b/src/time_alignment.cpp @@ -0,0 +1,468 @@ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include + +namespace fs = std::experimental::filesystem; + +basalt::Calibration calib; +basalt::MocapCalibration mocap_calib; + +// Linear time version +double compute_error(int64_t offset, + const std::vector &gyro_timestamps, + const Eigen::vector &gyro_data, + const std::vector &mocap_rot_vel_timestamps, + const Eigen::vector &mocap_rot_vel_data) { + double error = 0; + int num_points = 0; + + size_t j = 0; + + for (size_t i = 0; i < mocap_rot_vel_timestamps.size(); i++) { + int64_t corrected_time = mocap_rot_vel_timestamps[i] + offset; + + while (gyro_timestamps[j] < corrected_time) j++; + if (j >= gyro_timestamps.size()) break; + + int64_t dist_j = gyro_timestamps[j] - corrected_time; + int64_t dist_j_m1 = corrected_time - gyro_timestamps[j - 1]; + + BASALT_ASSERT(dist_j >= 0); + BASALT_ASSERT(dist_j_m1 >= 0); + + int idx = dist_j < dist_j_m1 ? j : j - 1; + + if (std::min(dist_j, dist_j_m1) > 1e9 / 120) continue; + + error += (gyro_data[idx] - mocap_rot_vel_data[i]).norm(); + num_points++; + } + return error / num_points; +} + +int main(int argc, char **argv) { + tbb::task_scheduler_init init( + tbb::task_scheduler_init::default_num_threads()); + + std::string dataset_path; + std::string calibration_path; + std::string mocap_calibration_path; + std::string dataset_type; + std::string output_path; + std::string output_error_path; + std::string output_gyro_path; + std::string output_mocap_path; + + bool show_gui = false; + + CLI::App app{"Calibrate time offset"}; + + app.add_option("-d,--dataset-path", dataset_path, "Path to dataset") + ->required(); + app.add_option("--calibration", calibration_path, "Path to calibration file") + ->required(); + app.add_option("--mocap-calibration", mocap_calibration_path, + "Path to mocap calibration file") + ->required(); + app.add_option("--dataset-type", dataset_type, "Dataset type .") + ->required(); + + app.add_option("--output", output_path, + "Path to output file with time-offset result"); + app.add_option("--output-error", output_error_path, + "Path to output file with error time-series for plotting"); + app.add_option( + "--output-gyro", output_gyro_path, + "Path to output file with gyro rotational velocities for plotting"); + app.add_option( + "--output-mocap", output_mocap_path, + "Path to output file with mocap rotational velocities for plotting"); + + app.add_flag("--show-gui", show_gui, "Show GUI for debugging"); + + try { + app.parse(argc, argv); + } catch (const CLI::ParseError &e) { + return app.exit(e); + } + + basalt::VioDatasetPtr vio_dataset; + + std::ifstream is(calibration_path); + + if (is.good()) { + cereal::JSONInputArchive archive(is); + archive(calib); + std::cout << "Loaded calibration from: " << calibration_path << std::endl; + } else { + std::cerr << "No calibration found" << std::endl; + std::abort(); + } + + std::ifstream mocap_is(mocap_calibration_path); + + if (mocap_is.good()) { + cereal::JSONInputArchive archive(mocap_is); + archive(mocap_calib); + std::cout << "Loaded mocap calibration from: " << mocap_calibration_path + << std::endl; + } else { + std::cerr << "No mocap calibration found" << std::endl; + std::abort(); + } + + basalt::DatasetIoInterfacePtr dataset_io = + basalt::DatasetIoFactory::getDatasetIo(dataset_type); + + dataset_io->read(dataset_path); + vio_dataset = dataset_io->get_data(); + + std::vector gyro_timestamps; + Eigen::vector gyro_data; + + std::vector mocap_rot_vel_timestamps; + Eigen::vector mocap_rot_vel_data; + + // Apply calibration to gyro + { + int saturation_count = 0; + for (size_t i = 0; i < vio_dataset->get_gyro_data().size(); i++) { + if (vio_dataset->get_gyro_data()[i].data.array().abs().maxCoeff() > + 499.0 * M_PI / 180) { + ++saturation_count; + continue; + } + gyro_timestamps.push_back(vio_dataset->get_gyro_data()[i].timestamp_ns); + + Eigen::Vector3d measurement = vio_dataset->get_gyro_data()[i].data; + gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement)); + } + std::cout << "saturated gyro measurement count: " << saturation_count + << std::endl; + } + + // compute rotational velocity from mocap data + { + Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse(); + + int saturation_count = 0; + for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size() - 1; i++) { + Sophus::SE3d p0, p1; + + // compute central differences, to have no timestamp bias + p0 = vio_dataset->get_gt_pose_data()[i - 1] * T_mark_i; + p1 = vio_dataset->get_gt_pose_data()[i + 1] * T_mark_i; + + double dt = (vio_dataset->get_gt_timestamps()[i + 1] - + vio_dataset->get_gt_timestamps()[i - 1]) * + 1e-9; + + // only compute difference, if measurements are really 2 consecutive + // measurements apart (assuming 120 Hz data) + if (dt > 2.5 / 120) continue; + + Eigen::Vector3d rot_vel = (p0.so3().inverse() * p1.so3()).log() / dt; + + // Filter outliers + if (rot_vel.array().abs().maxCoeff() > 500 * M_PI / 180) { + ++saturation_count; + continue; + } + + mocap_rot_vel_timestamps.push_back(vio_dataset->get_gt_timestamps()[i]); + mocap_rot_vel_data.push_back(rot_vel); + } + std::cout << "outlier mocap rotation velocity count: " << saturation_count + << std::endl; + } + + std::cout << "gyro_data.size() " << gyro_data.size() << std::endl; + std::cout << "mocap_rot_vel_data.size() " << mocap_rot_vel_data.size() + << std::endl; + + std::vector offsets_vec; + std::vector errors_vec; + + int best_offset_ns = 0; + double best_error = std::numeric_limits::max(); + int best_error_idx = -1; + + int64_t max_offset_ns = 10000000000; + int64_t offset_inc_ns = 100000; + + for (int64_t offset_ns = -max_offset_ns; offset_ns <= max_offset_ns; + offset_ns += offset_inc_ns) { + double error = compute_error(offset_ns, gyro_timestamps, gyro_data, + mocap_rot_vel_timestamps, mocap_rot_vel_data); + + offsets_vec.push_back(offset_ns * 1e-6); + errors_vec.push_back(error); + + if (error < best_error) { + best_error = error; + best_offset_ns = offset_ns; + best_error_idx = errors_vec.size() - 1; + } + } + + std::cout << "Best error: " << best_error << std::endl; + std::cout << "Best error idx : " << best_error_idx << std::endl; + std::cout << "Best offset: " << best_offset_ns << std::endl; + + pangolin::DataLog error_log; + + int best_offset_refined_ns = best_offset_ns; + + // Subpixel accuracy + Eigen::Vector3d coeff(0, 0, 0); + { + const static int SAMPLE_INTERVAL = 10; + + if (best_error_idx - SAMPLE_INTERVAL >= 0 && + best_error_idx + SAMPLE_INTERVAL < int(errors_vec.size())) { + Eigen::MatrixXd pol(2 * SAMPLE_INTERVAL + 1, 3); + Eigen::VectorXd err(2 * SAMPLE_INTERVAL + 1); + + for (int i = 0; i < 2 * SAMPLE_INTERVAL + 1; i++) { + int idx = i - SAMPLE_INTERVAL; + pol(i, 0) = idx * idx; + pol(i, 1) = idx; + pol(i, 2) = 1; + + err(i) = errors_vec[best_error_idx + idx]; + } + + coeff = + pol.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(err); + + double a = coeff[0]; + double b = coeff[1]; + + if (a > 1e-9) { + best_offset_refined_ns -= offset_inc_ns * b / (2 * a); + } + } + + for (size_t i = 0; i < errors_vec.size(); i++) { + const double idx = + static_cast(static_cast(i) - best_error_idx); + + const Eigen::Vector3d pol(idx * idx, idx, 1); + + error_log.Log(offsets_vec[i], errors_vec[i], pol.transpose() * coeff); + } + } + + std::cout << "Best error refined: " + << compute_error(best_offset_refined_ns, gyro_timestamps, gyro_data, + mocap_rot_vel_timestamps, mocap_rot_vel_data) + << std::endl; + std::cout << "Best offset refined: " << best_offset_refined_ns << std::endl; + + std::cout << "Total mocap offset: " + << vio_dataset->get_mocap_to_imu_offset_ns() + + best_offset_refined_ns + << std::endl; + + if (output_path != "") { + std::ofstream os(output_path); + cereal::JSONOutputArchive archive(os); + archive(cereal::make_nvp("mocap_to_imu_initial_offset_ns", + vio_dataset->get_mocap_to_imu_offset_ns())); + archive(cereal::make_nvp("mocap_to_imu_additional_offset_refined_ns", + best_offset_refined_ns)); + archive(cereal::make_nvp( + "mocap_to_imu_total_offset_ns", + vio_dataset->get_mocap_to_imu_offset_ns() + best_offset_refined_ns)); + } + + if (output_error_path != "") { + std::cout << "Writing error time series to '" << output_error_path << "'" + << std::endl; + + std::ofstream os(output_error_path); + os << "#TIME_MS,ERROR,ERROR_FITTED" << std::endl; + os << "# best_offset_ms: " << best_offset_ns * 1e-6 + << ", best_offset_refined_ms: " << best_offset_refined_ns * 1e-6 + << std::endl; + + for (size_t i = 0; i < errors_vec.size(); ++i) { + const double idx = + static_cast(static_cast(i) - best_error_idx); + const Eigen::Vector3d pol(idx * idx, idx, 1); + const double fitted = pol.transpose() * coeff; + os << offsets_vec[i] << "," << errors_vec[i] << "," << fitted + << std::endl; + } + } + + int64_t min_time = vio_dataset->get_gyro_data().front().timestamp_ns; + int64_t max_time = vio_dataset->get_gyro_data().back().timestamp_ns; + + if (output_gyro_path != "") { + std::cout << "Writing gyro values to '" << output_gyro_path << "'" + << std::endl; + + std::ofstream os(output_gyro_path); + os << "#TIME_M, GX, GY, GZ" << std::endl; + + for (size_t i = 0; i < gyro_timestamps.size(); ++i) { + os << (gyro_timestamps[i] - min_time) * 1e-9 << " " + << gyro_data[i].transpose() << std::endl; + } + } + + if (output_mocap_path != "") { + std::cout << "Writing mocap rotational velocity values to '" + << output_mocap_path << "'" << std::endl; + + std::ofstream os(output_mocap_path); + os << "#TIME_M, GX, GY, GZ" << std::endl; + + for (size_t i = 0; i < gyro_timestamps.size(); ++i) { + os << (mocap_rot_vel_timestamps[i] + best_offset_ns - min_time) * 1e-9 + << " " << mocap_rot_vel_data[i].transpose() << std::endl; + } + } + + if (show_gui) { + static constexpr int UI_WIDTH = 280; + + pangolin::CreateWindowAndBind("Main", 1280, 800); + + pangolin::Plotter *plotter; + + pangolin::DataLog data_log, mocap_log; + + pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds( + 0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, + pangolin::Attach::Pix(UI_WIDTH)); + + plotter = new pangolin::Plotter(&data_log, 0, (max_time - min_time) * 1e-9, + -10.0, 10.0, 0.01, 0.01); + + plot_display.AddDisplay(*plotter); + pangolin::Var show_gyro("ui.show_gyro", true, false, true); + pangolin::Var show_mocap_rot_vel("ui.show_mocap_rot_vel", true, false, + true); + + pangolin::Var show_error("ui.show_error", false, false, true); + + pangolin::Var> save_aligned_dataset( + "ui.save_aligned_dataset", [&]() { + if (fs::exists(fs::path(dataset_path + "mav0/gt/data.csv"))) { + std::cout << "Aligned grount truth data already exists, skipping." + << std::endl; + return; + } + std::cout << "Saving aligned dataset in " + << dataset_path + "mav0/gt/data.csv" << std::endl; + // output corrected mocap data + Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse(); + fs::create_directory(dataset_path + "mav0/gt/"); + std::ofstream gt_out_stream; + gt_out_stream.open(dataset_path + "mav0/gt/data.csv"); + gt_out_stream + << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], " + "q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n"; + + for (size_t i = 0; i < vio_dataset->get_gt_timestamps().size(); i++) { + gt_out_stream << vio_dataset->get_gt_timestamps()[i] + + best_offset_refined_ns + << ","; + Sophus::SE3d pose_corrected = + vio_dataset->get_gt_pose_data()[i] * T_mark_i; + gt_out_stream << pose_corrected.translation().x() << "," + << pose_corrected.translation().y() << "," + << pose_corrected.translation().z() << "," + << pose_corrected.unit_quaternion().w() << "," + << pose_corrected.unit_quaternion().x() << "," + << pose_corrected.unit_quaternion().y() << "," + << pose_corrected.unit_quaternion().z() << std::endl; + } + gt_out_stream.close(); + }); + + auto recompute_logs = [&]() { + data_log.Clear(); + mocap_log.Clear(); + + for (size_t i = 0; i < gyro_timestamps.size(); i++) { + data_log.Log((gyro_timestamps[i] - min_time) * 1e-9, gyro_data[i][0], + gyro_data[i][1], gyro_data[i][2]); + } + + for (size_t i = 0; i < mocap_rot_vel_timestamps.size(); i++) { + mocap_log.Log( + (mocap_rot_vel_timestamps[i] + best_offset_ns - min_time) * 1e-9, + mocap_rot_vel_data[i][0], mocap_rot_vel_data[i][1], + mocap_rot_vel_data[i][2]); + } + }; + + auto drawPlots = [&]() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_gyro) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour::Red(), "g x"); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour::Green(), "g y"); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, + pangolin::Colour::Blue(), "g z"); + } + + if (show_mocap_rot_vel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour(1, 1, 0), "pv x", &mocap_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour(1, 0, 1), "pv y", &mocap_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, + pangolin::Colour(0, 1, 1), "pv z", &mocap_log); + } + + if (show_error) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, + pangolin::Colour(1, 1, 1), "error", &error_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, + pangolin::Colour(0.3, 1, 0.8), "fitted error", + &error_log); + plotter->AddMarker(pangolin::Marker::Vertical, + best_offset_refined_ns * 1e-6, + pangolin::Marker::Equal, pangolin::Colour(1, 0, 0)); + } + }; + + recompute_logs(); + drawPlots(); + + while (!pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (show_gyro.GuiChanged() || show_mocap_rot_vel.GuiChanged() || + show_error.GuiChanged()) { + drawPlots(); + } + + pangolin::FinishFrame(); + } + } + + return 0; +} diff --git a/src/vio.cpp b/src/vio.cpp index c9fcf05..fb10c63 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -71,6 +71,7 @@ bool next_step(); bool prev_step(); void draw_plots(); void alignButton(); +void alignDeviceButton(); // Pangolin variables constexpr int UI_WIDTH = 200; @@ -91,6 +92,7 @@ pangolin::Var show_est_bg("ui.show_est_bg", false, false, true); pangolin::Var show_est_ba("ui.show_est_ba", false, false, true); pangolin::Var show_gt("ui.show_gt", true, false, true); +pangolin::Var show_device_gt("ui.show_device_gt", true, false, true); Button next_step_btn("ui.next_step", &next_step); Button prev_step_btn("ui.prev_step", &prev_step); @@ -99,6 +101,7 @@ pangolin::Var continue_btn("ui.continue", false, false, true); pangolin::Var continue_fast("ui.continue_fast", true, false, true); Button align_svd_btn("ui.align_svd", &alignButton); +Button align_device_svd_btn("ui.align_device_svd", &alignDeviceButton); pangolin::Var follow("ui.follow", true, false, true); @@ -114,6 +117,9 @@ Eigen::vector vio_t_w_i; std::vector gt_t_ns; Eigen::vector gt_t_w_i; +std::vector device_pose_t_ns; +Eigen::vector device_pose_t_w_i; + std::string marg_data_path; size_t last_frame_processed = 0; @@ -173,6 +179,7 @@ void feed_imu() { int main(int argc, char** argv) { bool show_gui = true; bool print_queue = false; + bool terminate = false; std::string cam_calib_path; std::string dataset_path; std::string dataset_type; @@ -239,6 +246,13 @@ int main(int argc, char** argv) { gt_t_ns.push_back(vio_dataset->get_gt_timestamps()[i]); gt_t_w_i.push_back(vio_dataset->get_gt_pose_data()[i].translation()); } + + for (size_t i = 0; i < vio_dataset->get_device_pose_data().size(); i++) { + device_pose_t_ns.push_back(vio_dataset->get_device_pose_timestamps()[i]); + device_pose_t_w_i.push_back( + vio_dataset->get_device_pose_data()[i].translation()); + } + std::cout << "Len " << device_pose_t_ns.size() << std::endl; } const int64_t start_t_ns = vio_dataset->get_image_timestamps().front(); @@ -335,7 +349,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 " @@ -464,10 +478,13 @@ int main(int argc, char** argv) { } } + terminate = true; + t1.join(); t2.join(); if (t3.get()) t3->join(); t4.join(); + if (t5.get()) t5->join(); if (!result_path.empty()) { double error = basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); @@ -545,6 +562,10 @@ void draw_scene() { glColor3ubv(gt_color); if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i); + u_int8_t device_color[3]{150, 150, 0}; + glColor3ubv(device_color); + if (show_device_gt) pangolin::glDrawLineStrip(device_pose_t_w_i); + size_t frame_id = show_frame; int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; auto it = vis_map.find(t_ns); @@ -653,3 +674,6 @@ void draw_plots() { } void alignButton() { basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); } +void alignDeviceButton() { + basalt::alignSVD(device_pose_t_ns, device_pose_t_w_i, gt_t_ns, gt_t_w_i); +}