diff --git a/include/basalt/io/dataset_io_euroc.h b/include/basalt/io/dataset_io_euroc.h index 7ce4f68..f55bea6 100644 --- a/include/basalt/io/dataset_io_euroc.h +++ b/include/basalt/io/dataset_io_euroc.h @@ -67,7 +67,7 @@ class EurocVioDataset : public VioDataset { Eigen::vector device_pose_data; // TODO: change to eigen aligned - int64_t mocap_to_imu_offset_ns; + int64_t mocap_to_imu_offset_ns = 0; std::vector> exposure_times; diff --git a/include/basalt/io/dataset_io_uzh.h b/include/basalt/io/dataset_io_uzh.h new file mode 100644 index 0000000..658e1d6 --- /dev/null +++ b/include/basalt/io/dataset_io_uzh.h @@ -0,0 +1,319 @@ +/** +BSD 3-Clause License + +This file is part of the Basalt project. +https://gitlab.com/VladyslavUsenko/basalt.git + +Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +#pragma once + +#include + +#include +namespace fs = std::experimental::filesystem; + +#include + +namespace basalt { + +class UzhVioDataset : public VioDataset { + size_t num_cams; + + std::string path; + + std::vector image_timestamps; + std::unordered_map left_image_path; + std::unordered_map right_image_path; + + // vector of images for every timestamp + // assumes vectors size is num_cams for every timestamp with null pointers for + // missing frames + // std::unordered_map> image_data; + + Eigen::vector accel_data; + Eigen::vector gyro_data; + + 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 = 0; + + std::vector> exposure_times; + + public: + ~UzhVioDataset(){}; + + size_t get_num_cams() const { return num_cams; } + + std::vector &get_image_timestamps() { return image_timestamps; } + + const Eigen::vector &get_accel_data() const { return accel_data; } + const Eigen::vector &get_gyro_data() const { return gyro_data; } + const std::vector &get_gt_timestamps() const { + return gt_timestamps; + } + 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) { + std::vector res(num_cams); + + for (size_t i = 0; i < num_cams; i++) { + std::string full_image_path = + path + "/" + + (i == 0 ? left_image_path.at(t_ns) : right_image_path.at(t_ns)); + + if (file_exists(full_image_path)) { + cv::Mat img = cv::imread(full_image_path, cv::IMREAD_UNCHANGED); + + if (img.type() == CV_8UC1) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); + + const uint8_t *data_in = img.ptr(); + uint16_t *data_out = res[i].img->ptr; + + size_t full_size = img.cols * img.rows; + for (size_t i = 0; i < full_size; i++) { + int val = data_in[i]; + val = val << 8; + data_out[i] = val; + } + } else if (img.type() == CV_8UC3) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); + + const uint8_t *data_in = img.ptr(); + uint16_t *data_out = res[i].img->ptr; + + size_t full_size = img.cols * img.rows; + for (size_t i = 0; i < full_size; i++) { + int val = data_in[i * 3]; + val = val << 8; + data_out[i] = val; + } + } else if (img.type() == CV_16UC1) { + res[i].img.reset(new ManagedImage(img.cols, img.rows)); + std::memcpy(res[i].img->ptr, img.ptr(), + img.cols * img.rows * sizeof(uint16_t)); + + } else { + std::cerr << "img.fmt.bpp " << img.type() << std::endl; + std::abort(); + } + + auto exp_it = exposure_times[i].find(t_ns); + if (exp_it != exposure_times[i].end()) { + res[i].exposure = exp_it->second; + } + } + } + + return res; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + friend class UzhIO; +}; + +class UzhIO : public DatasetIoInterface { + public: + UzhIO() {} + + void read(const std::string &path) { + if (!fs::exists(path)) + std::cerr << "No dataset found in " << path << std::endl; + + data.reset(new UzhVioDataset); + + data->num_cams = 2; + data->path = path; + + read_image_timestamps(path); + + std::cout << "Loaded " << data->get_image_timestamps().size() + << " timestamps, " << data->left_image_path.size() + << " left images and " << data->right_image_path.size() + << std::endl; + + // { + // int64_t t_ns = data->get_image_timestamps()[0]; + // std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " " + // << data->right_image_path.at(t_ns) << std::endl; + // } + + read_imu_data(path + "/imu.txt"); + + std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs." + << std::endl; + + if (file_exists(path + "/groundtruth.txt")) { + read_gt_data_pose(path + "/groundtruth.txt"); + } + + data->exposure_times.resize(data->num_cams); + } + + void reset() { data.reset(); } + + VioDatasetPtr get_data() { return data; } + + private: + void read_exposure(const std::string &path, + std::unordered_map &exposure_data) { + exposure_data.clear(); + + std::ifstream f(path + "exposure.csv"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + char tmp; + int64_t timestamp, exposure_int; + Eigen::Vector3d gyro, accel; + + ss >> timestamp >> tmp >> exposure_int; + + exposure_data[timestamp] = exposure_int * 1e-9; + } + } + + void read_image_timestamps(const std::string &path) { + { + std::ifstream f(path + "/left_images.txt"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + std::stringstream ss(line); + int tmp; + double t_s; + std::string path; + ss >> tmp >> t_s >> path; + + int64_t t_ns = t_s * 1e9; + + data->image_timestamps.emplace_back(t_ns); + data->left_image_path[t_ns] = path; + } + } + + { + std::ifstream f(path + "/right_images.txt"); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + std::stringstream ss(line); + int tmp; + double t_s; + std::string path; + ss >> tmp >> t_s >> path; + + int64_t t_ns = t_s * 1e9; + + data->right_image_path[t_ns] = path; + } + } + } + + void read_imu_data(const std::string &path) { + data->accel_data.clear(); + data->gyro_data.clear(); + + std::ifstream f(path); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + int tmp; + double timestamp; + Eigen::Vector3d gyro, accel; + + ss >> tmp >> timestamp >> gyro[0] >> gyro[1] >> gyro[2] >> accel[0] >> + accel[1] >> accel[2]; + + int64_t t_ns = timestamp * 1e9; + + data->accel_data.emplace_back(); + data->accel_data.back().timestamp_ns = t_ns; + data->accel_data.back().data = accel; + + data->gyro_data.emplace_back(); + data->gyro_data.back().timestamp_ns = t_ns; + data->gyro_data.back().data = gyro; + } + } + + void read_gt_data_pose(const std::string &path) { + data->gt_timestamps.clear(); + data->gt_pose_data.clear(); + + std::ifstream f(path); + std::string line; + while (std::getline(f, line)) { + if (line[0] == '#') continue; + + std::stringstream ss(line); + + int tmp; + double timestamp; + Eigen::Quaterniond q; + Eigen::Vector3d pos; + + ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >> + q.z() >> q.w(); + + int64_t t_ns = timestamp * 1e9; + t_ns += -99902802; + + data->gt_timestamps.emplace_back(t_ns); + data->gt_pose_data.emplace_back(q, pos); + } + } + + std::shared_ptr data; +}; + +} // namespace basalt diff --git a/src/calibrate_imu.cpp b/src/calibrate_imu.cpp index 4b77805..8588039 100644 --- a/src/calibrate_imu.cpp +++ b/src/calibrate_imu.cpp @@ -73,9 +73,9 @@ int main(int argc, char **argv) { app.add_option("--accel-noise-std", accel_noise_std, "Accelerometer noise std"); - app.add_option("--gyro-bias-std", accel_bias_std, + app.add_option("--gyro-bias-std", gyro_bias_std, "Gyroscope bias random walk std"); - app.add_option("--accel-bias-std", gyro_bias_std, + app.add_option("--accel-bias-std", accel_bias_std, "Accelerometer bias random walk std"); app.add_option("--cache-name", cache_dataset_name, diff --git a/src/io/dataset_io.cpp b/src/io/dataset_io.cpp index f49fbaf..fa9eee6 100644 --- a/src/io/dataset_io.cpp +++ b/src/io/dataset_io.cpp @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace basalt { @@ -46,6 +47,8 @@ DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo( return DatasetIoInterfacePtr(new EurocIO); } else if (dataset_type == "bag") { return DatasetIoInterfacePtr(new RosbagIO(with_images)); + } else if (dataset_type == "uzh") { + return DatasetIoInterfacePtr(new UzhIO); } else { std::cerr << "Dataset type " << dataset_type << " is not supported" << std::endl; diff --git a/src/time_alignment.cpp b/src/time_alignment.cpp index b157fd7..4d8b0d5 100644 --- a/src/time_alignment.cpp +++ b/src/time_alignment.cpp @@ -66,17 +66,15 @@ int main(int argc, char **argv) { std::string output_gyro_path; std::string output_mocap_path; - bool show_gui = false; + bool show_gui = true; 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("--calibration", calibration_path, "Path to calibration file"); app.add_option("--mocap-calibration", mocap_calibration_path, - "Path to mocap calibration file") - ->required(); + "Path to mocap calibration file"); app.add_option("--dataset-type", dataset_type, "Dataset type .") ->required(); @@ -101,27 +99,32 @@ int main(int argc, char **argv) { basalt::VioDatasetPtr vio_dataset; - std::ifstream is(calibration_path); + const bool use_calib = + !(calibration_path.empty() || mocap_calibration_path.empty()); - 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(); - } + if (use_calib) { + std::ifstream is(calibration_path); - std::ifstream mocap_is(mocap_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(); + } - 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(); + 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 = @@ -148,7 +151,11 @@ int main(int argc, char **argv) { 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)); + if (use_calib) { + gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement)); + } else { + gyro_data.push_back(measurement); + } } std::cout << "saturated gyro measurement count: " << saturation_count << std::endl; @@ -156,7 +163,8 @@ int main(int argc, char **argv) { // compute rotational velocity from mocap data { - Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse(); + Sophus::SE3d T_mark_i; + if (use_calib) 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++) { @@ -373,7 +381,8 @@ int main(int argc, char **argv) { 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(); + Sophus::SE3d T_mark_i; + if (use_calib) 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"); diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 9daadc3..95b5a70 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -125,6 +125,13 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, while (true) { vision_data_queue.pop(curr_frame); + if (!curr_frame.get()) { + break; + } + + // Correct camera time offset + // curr_frame->t_ns += calib.cam_time_offset_ns; + if (!initialized) { Eigen::Vector3d vel_w_i_init; vel_w_i_init.setZero(); @@ -150,10 +157,6 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, initialized = true; } - if (!curr_frame.get()) { - break; - } - if (prev_frame) { // preintegrate measurements @@ -179,6 +182,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, } if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { + if (!data.get()) break; int64_t tmp = data->t_ns; data->t_ns = curr_frame->t_ns; meas->integrate(*data, accel_cov, gyro_cov);