diff --git a/CMakeLists.txt b/CMakeLists.txt index 50477b3..cbcfe6c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -202,6 +202,7 @@ include_directories(include) add_library(basalt SHARED src/io/dataset_io.cpp src/io/marg_data_io.cpp + src/calibration/aprilgrid.cpp src/calibration/cam_calib.cpp src/calibration/cam_imu_calib.cpp src/calibration/calibraiton_helper.cpp diff --git a/data/aprilgrid_6x6.json b/data/aprilgrid_6x6.json new file mode 100644 index 0000000..1361c77 --- /dev/null +++ b/data/aprilgrid_6x6.json @@ -0,0 +1,6 @@ +{ + "tagCols": 6, + "tagRows": 6, + "tagSize": 0.088, + "tagSpacing": 0.3 +} diff --git a/doc/Calibration.md b/doc/Calibration.md index 67cae83..5f0e9ff 100644 --- a/doc/Calibration.md +++ b/doc/Calibration.md @@ -15,12 +15,13 @@ wget http://vision.in.tum.de/tumvi/calibrated/512_16/dataset-calib-imu1_512_16.b ### Camera calibration Run the camera calibration: ``` -basalt_calibrate --dataset-path ~/tumvi_calib_data/dataset-calib-cam3_512_16.bag --dataset-type bag --result-path ~/tumvi_calib_result/ --cam-types ds ds +basalt_calibrate --dataset-path ~/tumvi_calib_data/dataset-calib-cam3_512_16.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/tumvi_calib_result/ --cam-types ds ds ``` The command line options have the following meaning: * `--dataset-path` path to the dataset. * `--dataset-type` type of the datset. Currently only `bag` and `euroc` formats of the datasets are supported. * `--result-path` path to the folder where the resulting calibration and intermediate results will be stored. +* `--aprilgrid` path to the configuration file for the aprilgrid. * `--cam-types` camera models for the image streams in the dataset. For more detais see [arXiv:1807.08957](https://arxiv.org/abs/1807.08957). After that, you should see the calibration GUI: @@ -51,7 +52,7 @@ You can also control the process using the following buttons: ### Camera + IMU + Mocap calibration After calibrating the camera you can run the camera + IMU + Mocap calibration. The result path should point to the **same folder as before**: ``` -basalt_calibrate_imu --dataset-path ~/tumvi_calib_data/dataset-calib-imu1_512_16.bag --dataset-type bag --result-path ~/tumvi_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 +basalt_calibrate_imu --dataset-path ~/tumvi_calib_data/dataset-calib-imu1_512_16.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/tumvi_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 ``` The command line options for the IMU noise are continous-time and defined as in [Kalibr](https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model): * `--gyro-noise-std` gyroscope white noise. @@ -106,13 +107,13 @@ wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/calibration_da ### Camera calibration Run the camera calibration: ``` -basalt_calibrate --dataset-path ~/euroc_calib_data/cam_april.bag --dataset-type bag --result-path ~/euroc_calib_result/ --cam-types ds ds +basalt_calibrate --dataset-path ~/euroc_calib_data/cam_april.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/euroc_calib_result/ --cam-types ds ds ``` ![euroc_cam_calib](/doc/img/euroc_cam_calib.png) ### Camera + IMU calibration After calibrating the camera you can run the camera + IMU calibration. The result-path should point to the same folder as before: ``` -basalt_calibrate_imu --dataset-path ~/euroc_calib_data/imu_april.bag --dataset-type bag --result-path ~/euroc_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 +basalt_calibrate_imu --dataset-path ~/euroc_calib_data/imu_april.bag --dataset-type bag --aprilgrid /usr/etc/basalt/aprilgrid_6x6.json --result-path ~/euroc_calib_result/ --gyro-noise-std 0.000282 --accel-noise-std 0.016 --gyro-bias-std 0.0001 --accel-bias-std 0.001 ``` ![euroc_imu_calib](/doc/img/euroc_imu_calib.png) diff --git a/include/basalt/calibration/aprilgrid.h b/include/basalt/calibration/aprilgrid.h index cf1c79e..3c4a885 100644 --- a/include/basalt/calibration/aprilgrid.h +++ b/include/basalt/calibration/aprilgrid.h @@ -39,99 +39,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { struct AprilGrid { - AprilGrid() { - const int tagCols = 6; // number of apriltags - const int tagRows = 6; // number of apriltags - const double tagSize = 0.088; // size of apriltag, edge to edge [m] - const double tagSpacing = 0.3; // ratio of space between tags to tagSize - - double x_corner_offsets[4] = {0, tagSize, tagSize, 0}; - double y_corner_offsets[4] = {0, 0, tagSize, tagSize}; - - aprilgrid_corner_pos_3d.resize(tagCols * tagRows * 4); - - for (int y = 0; y < tagCols; y++) { - for (int x = 0; x < tagRows; x++) { - int tag_id = tagRows * y + x; - double x_offset = x * tagSize * (1 + tagSpacing); - double y_offset = y * tagSize * (1 + tagSpacing); - - for (int i = 0; i < 4; i++) { - int corner_id = (tag_id << 2) + i; - - Eigen::Vector4d &pos_3d = aprilgrid_corner_pos_3d[corner_id]; - - pos_3d[0] = x_offset + x_corner_offsets[i]; - pos_3d[1] = y_offset + y_corner_offsets[i]; - pos_3d[2] = 0; - pos_3d[3] = 1; - } - } - } - - size_t num_vign_points = 5; - size_t num_blocks = tagCols * tagRows * 2; - - aprilgrid_vignette_pos_3d.resize((num_blocks + tagCols + tagRows) * - num_vign_points); - - for (size_t k = 0; k < num_vign_points; k++) { - for (size_t i = 0; i < tagCols * tagRows; i++) { - // const Eigen::Vector3d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; - const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; - const Eigen::Vector4d p2 = aprilgrid_corner_pos_3d[4 * i + 2]; - const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i + 3]; - - double coeff = double(k + 1) / double(num_vign_points + 1); - - aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0] = - (p1 + coeff * (p2 - p1)); - aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1] = - (p2 + coeff * (p3 - p2)); - - aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0][0] += - tagSize * tagSpacing / 2; - aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1][1] += - tagSize * tagSpacing / 2; - } - } - - size_t curr_idx = num_blocks * num_vign_points; - - for (size_t k = 0; k < num_vign_points; k++) { - for (size_t i = 0; i < tagCols; i++) { - const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; - const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; - - double coeff = double(k + 1) / double(num_vign_points + 1); - - aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = - (p0 + coeff * (p1 - p0)); - - aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][1] -= - tagSize * tagSpacing / 2; - } - } - - curr_idx += tagCols * num_vign_points; - - for (size_t k = 0; k < num_vign_points; k++) { - for (size_t i = 0; i < tagRows; i++) { - const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i * tagCols + 0]; - const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i * tagCols + 3]; - - double coeff = double(k + 1) / double(num_vign_points + 1); - - aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = - (p0 + coeff * (p3 - p0)); - - aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][0] -= - tagSize * tagSpacing / 2; - } - } - } + AprilGrid(const std::string &config_path); Eigen::vector aprilgrid_corner_pos_3d; Eigen::vector aprilgrid_vignette_pos_3d; + + private: + int tagCols; // number of apriltags + int tagRows; // number of apriltags + double tagSize; // size of apriltag, edge to edge [m] + double tagSpacing; // ratio of space between tags to tagSize }; + } // namespace basalt diff --git a/include/basalt/calibration/cam_calib.h b/include/basalt/calibration/cam_calib.h index b6cd261..8b02f46 100644 --- a/include/basalt/calibration/cam_calib.h +++ b/include/basalt/calibration/cam_calib.h @@ -60,9 +60,9 @@ class PosesOptimization; class CamCalib { public: CamCalib(const std::string &dataset_path, const std::string &dataset_type, - const std::string &cache_path, const std::string &cache_dataset_name, - int skip_images, const std::vector &cam_types, - bool show_gui = true); + const std::string &aprilgrid_path, const std::string &cache_path, + const std::string &cache_dataset_name, int skip_images, + const std::vector &cam_types, bool show_gui = true); ~CamCalib(); @@ -108,8 +108,6 @@ class CamCalib { // typedef Calibration::Ptr CalibrationPtr; - AprilGrid april_grid; - VioDatasetPtr vio_dataset; // CalibrationPtr calib; @@ -128,6 +126,9 @@ class CamCalib { std::string dataset_path; std::string dataset_type; + + AprilGrid april_grid; + std::string cache_path; std::string cache_dataset_name; diff --git a/include/basalt/calibration/cam_imu_calib.h b/include/basalt/calibration/cam_imu_calib.h index 942757b..7d15107 100644 --- a/include/basalt/calibration/cam_imu_calib.h +++ b/include/basalt/calibration/cam_imu_calib.h @@ -61,7 +61,7 @@ class SplineOptimization; class CamImuCalib { public: CamImuCalib(const std::string &dataset_path, const std::string &dataset_type, - const std::string &cache_path, + const std::string &aprilgrid_path, const std::string &cache_path, const std::string &cache_dataset_name, int skip_images, const std::vector &imu_noise, bool show_gui = true); @@ -109,8 +109,6 @@ class CamImuCalib { private: static constexpr int UI_WIDTH = 300; - AprilGrid april_grid; - VioDatasetPtr vio_dataset; tbb::concurrent_unordered_map calib_corners; @@ -126,6 +124,9 @@ class CamImuCalib { std::string dataset_path; std::string dataset_type; + + AprilGrid april_grid; + std::string cache_path; std::string cache_dataset_name; diff --git a/src/calibrate.cpp b/src/calibrate.cpp index 00dabca..d9999ad 100644 --- a/src/calibrate.cpp +++ b/src/calibrate.cpp @@ -45,6 +45,7 @@ int main(int argc, char **argv) { std::string dataset_path; std::string dataset_type; + std::string aprilgrid_path; std::string result_path; std::vector cam_types; std::string cache_dataset_name = "calib-cam"; @@ -58,6 +59,10 @@ int main(int argc, char **argv) { app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)") ->required(); + app.add_option("--aprilgrid", aprilgrid_path, + "Path to Aprilgrid config file)") + ->required(); + app.add_option("--cache-name", cache_dataset_name, "Name to save cached files"); @@ -72,7 +77,7 @@ int main(int argc, char **argv) { return app.exit(e); } - basalt::CamCalib cv(dataset_path, dataset_type, result_path, + basalt::CamCalib cv(dataset_path, dataset_type, aprilgrid_path, result_path, cache_dataset_name, skip_images, cam_types); cv.renderingLoop(); diff --git a/src/calibrate_imu.cpp b/src/calibrate_imu.cpp index f5add27..4b77805 100644 --- a/src/calibrate_imu.cpp +++ b/src/calibrate_imu.cpp @@ -47,6 +47,7 @@ int main(int argc, char **argv) { std::string dataset_path; std::string dataset_type; + std::string aprilgrid_path; std::string result_path; std::string cache_dataset_name = "calib-cam-imu"; int skip_images = 1; @@ -64,6 +65,10 @@ int main(int argc, char **argv) { app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)") ->required(); + app.add_option("--aprilgrid", aprilgrid_path, + "Path to Aprilgrid config file)") + ->required(); + app.add_option("--gyro-noise-std", gyro_noise_std, "Gyroscope noise std"); app.add_option("--accel-noise-std", accel_noise_std, "Accelerometer noise std"); @@ -85,7 +90,8 @@ int main(int argc, char **argv) { } basalt::CamImuCalib cv( - dataset_path, dataset_type, result_path, cache_dataset_name, skip_images, + dataset_path, dataset_type, aprilgrid_path, result_path, + cache_dataset_name, skip_images, {accel_noise_std, gyro_noise_std, accel_bias_std, gyro_bias_std}); cv.renderingLoop(); diff --git a/src/calibration/aprilgrid.cpp b/src/calibration/aprilgrid.cpp new file mode 100644 index 0000000..fd3a343 --- /dev/null +++ b/src/calibration/aprilgrid.cpp @@ -0,0 +1,144 @@ +/** +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. +*/ + +#include + +#include + +#include + +namespace basalt { + +AprilGrid::AprilGrid(const std::string &config_path) { + std::ifstream is(config_path); + if (is.is_open()) { + cereal::JSONInputArchive ar(is); + ar(cereal::make_nvp("tagCols", tagCols)); + ar(cereal::make_nvp("tagRows", tagRows)); + ar(cereal::make_nvp("tagSize", tagSize)); + ar(cereal::make_nvp("tagSpacing", tagSpacing)); + } else { + std::cerr << "Could not open aprilgrid configuration: " << config_path + << std::endl; + std::abort(); + } + + double x_corner_offsets[4] = {0, tagSize, tagSize, 0}; + double y_corner_offsets[4] = {0, 0, tagSize, tagSize}; + + aprilgrid_corner_pos_3d.resize(tagCols * tagRows * 4); + + for (int y = 0; y < tagCols; y++) { + for (int x = 0; x < tagRows; x++) { + int tag_id = tagRows * y + x; + double x_offset = x * tagSize * (1 + tagSpacing); + double y_offset = y * tagSize * (1 + tagSpacing); + + for (int i = 0; i < 4; i++) { + int corner_id = (tag_id << 2) + i; + + Eigen::Vector4d &pos_3d = aprilgrid_corner_pos_3d[corner_id]; + + pos_3d[0] = x_offset + x_corner_offsets[i]; + pos_3d[1] = y_offset + y_corner_offsets[i]; + pos_3d[2] = 0; + pos_3d[3] = 1; + } + } + } + + int num_vign_points = 5; + int num_blocks = tagCols * tagRows * 2; + + aprilgrid_vignette_pos_3d.resize((num_blocks + tagCols + tagRows) * + num_vign_points); + + for (int k = 0; k < num_vign_points; k++) { + for (int i = 0; i < tagCols * tagRows; i++) { + // const Eigen::Vector3d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; + const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; + const Eigen::Vector4d p2 = aprilgrid_corner_pos_3d[4 * i + 2]; + const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i + 3]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0] = + (p1 + coeff * (p2 - p1)); + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1] = + (p2 + coeff * (p3 - p2)); + + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 0][0] += + tagSize * tagSpacing / 2; + aprilgrid_vignette_pos_3d[k * num_blocks + 2 * i + 1][1] += + tagSize * tagSpacing / 2; + } + } + + size_t curr_idx = num_blocks * num_vign_points; + + for (int k = 0; k < num_vign_points; k++) { + for (int i = 0; i < tagCols; i++) { + const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i + 0]; + const Eigen::Vector4d p1 = aprilgrid_corner_pos_3d[4 * i + 1]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = + (p0 + coeff * (p1 - p0)); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][1] -= + tagSize * tagSpacing / 2; + } + } + + curr_idx += tagCols * num_vign_points; + + for (int k = 0; k < num_vign_points; k++) { + for (int i = 0; i < tagRows; i++) { + const Eigen::Vector4d p0 = aprilgrid_corner_pos_3d[4 * i * tagCols + 0]; + const Eigen::Vector4d p3 = aprilgrid_corner_pos_3d[4 * i * tagCols + 3]; + + double coeff = double(k + 1) / double(num_vign_points + 1); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i] = + (p0 + coeff * (p3 - p0)); + + aprilgrid_vignette_pos_3d[curr_idx + k * tagCols + i][0] -= + tagSize * tagSpacing / 2; + } + } +} + +} // namespace basalt diff --git a/src/calibration/cam_calib.cpp b/src/calibration/cam_calib.cpp index 24bad74..b7f13c3 100644 --- a/src/calibration/cam_calib.cpp +++ b/src/calibration/cam_calib.cpp @@ -50,11 +50,13 @@ namespace basalt { CamCalib::CamCalib(const std::string &dataset_path, const std::string &dataset_type, + const std::string &aprilgrid_path, const std::string &cache_path, const std::string &cache_dataset_name, int skip_images, const std::vector &cam_types, bool show_gui) : dataset_path(dataset_path), dataset_type(dataset_type), + april_grid(aprilgrid_path), cache_path(ensure_trailing_slash(cache_path)), cache_dataset_name(cache_dataset_name), skip_images(skip_images), diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 305d5ef..353d10f 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -45,11 +45,13 @@ namespace basalt { CamImuCalib::CamImuCalib(const std::string &dataset_path, const std::string &dataset_type, + const std::string &aprilgrid_path, const std::string &cache_path, const std::string &cache_dataset_name, int skip_images, const std::vector &imu_noise, bool show_gui) : dataset_path(dataset_path), dataset_type(dataset_type), + april_grid(aprilgrid_path), cache_path(ensure_trailing_slash(cache_path)), cache_dataset_name(cache_dataset_name), skip_images(skip_images),