Added aprilgrid configuration

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

data/aprilgrid_6x6.json

@@ -0,0 +1,6 @@
+{
+  "tagCols": 6,
+  "tagRows": 6,
+  "tagSize": 0.088,
+  "tagSpacing": 0.3
+}

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)

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() {
+  AprilGrid(const std::string &config_path);
-    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;
-      }
-    }
-  }
 
   Eigen::vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
   Eigen::vector<Eigen::Vector4d> 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

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,
+           const std::string &aprilgrid_path, const std::string &cache_path,
-           int skip_images, const std::vector<std::string> &cam_types,
+           const std::string &cache_dataset_name, int skip_images,
-           bool show_gui = true);
+           const std::vector<std::string> &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;
 

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<double> &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<TimeCamId, CalibCornerData> 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;
 

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<std::string> 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();

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();

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 <fstream>
+
+#include <basalt/calibration/aprilgrid.h>
+
+#include <cereal/archives/json.hpp>
+
+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

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<std::string> &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),

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<double> &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),