VladyslavUsenko · Aug 7, 2022 · Aug 9, 2022 · Aug 9, 2022 · Aug 9, 2022 · Aug 12, 2022
diff --git a/.gitmodules b/.gitmodules
@@ -19,7 +19,7 @@
 	url = https://github.com/ros/console_bridge.git
 [submodule "thirdparty/basalt-headers"]
 	path = thirdparty/basalt-headers
-	url = https://gitlab.com/VladyslavUsenko/basalt-headers.git
+	url = https://github.com/SpectacularAI/basalt-headers.git
 [submodule "thirdparty/magic_enum"]
 	path = thirdparty/magic_enum
 	url = https://github.com/Neargye/magic_enum.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -292,8 +292,8 @@ add_library(basalt SHARED)
 # To support cmake < 3.13, use absolute paths (see: https://crascit.com/2016/01/31/enhanced-source-file-handling-with-target_sources/)
 target_sources(basalt
   PUBLIC
-    ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/aprilgrid.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/calibration_helper.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/calibration_pattern.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/cam_calib.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/cam_imu_calib.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/vignette.h
@@ -351,8 +351,8 @@ target_sources(basalt
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/vi_estimator/sqrt_keypoint_vo.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/vi_estimator/vio_estimator.h
   PRIVATE
-    ${CMAKE_CURRENT_SOURCE_DIR}/src/calibration/aprilgrid.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/calibration/calibraiton_helper.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/calibration/calibration_pattern.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/calibration/vignette.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/io/dataset_io.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/io/marg_data_io.cpp

diff --git a/include/basalt/calibration/calibration_helper.h b/include/basalt/calibration/calibration_helper.h
@@ -34,12 +34,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
-#include <basalt/calibration/aprilgrid.h>
+#include <basalt/calibration/calibration_pattern.h>
 #include <basalt/io/dataset_io.h>
 #include <basalt/utils/common_types.h>
 #include <basalt/calibration/calibration.hpp>
 
+#define TBB_PREVIEW_CONCURRENT_ORDERED_CONTAINERS 1
 #include <tbb/concurrent_unordered_map.h>
+#include <tbb/concurrent_map.h>
 
 namespace basalt {
 
@@ -72,29 +74,28 @@ using CalibCornerMap = tbb::concurrent_unordered_map<TimeCamId, CalibCornerData,
                                                      std::hash<TimeCamId>>;
 
 using CalibInitPoseMap =
-    tbb::concurrent_unordered_map<TimeCamId, CalibInitPoseData,
-                                  std::hash<TimeCamId>>;
+    tbb::concurrent_map<TimeCamId, CalibInitPoseData>;
 
 class CalibHelper {
  public:
   static void detectCorners(const VioDatasetPtr& vio_data,
-                            const AprilGrid& april_grid,
+                            const CalibrationPattern& calib_pattern,
                             CalibCornerMap& calib_corners,
                             CalibCornerMap& calib_corners_rejected);
 
   static void initCamPoses(
       const Calibration<double>::Ptr& calib,
-      const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
+      const Eigen::aligned_vector<Eigen::Vector4d>& corner_pos_3d,
       CalibCornerMap& calib_corners, CalibInitPoseMap& calib_init_poses);
 
   static bool initializeIntrinsics(
       const Eigen::aligned_vector<Eigen::Vector2d>& corners,
-      const std::vector<int>& corner_ids, const AprilGrid& aprilgrid, int cols,
+      const std::vector<int>& corner_ids, const CalibrationPattern& calib_pattern, int cols,
       int rows, Eigen::Vector4d& init_intr);
 
   static bool initializeIntrinsicsPinhole(
       const std::vector<CalibCornerData*> pinhole_corners,
-      const AprilGrid& aprilgrid, int cols, int rows,
+      const CalibrationPattern& calib_pattern, int cols, int rows,
       Eigen::Vector4d& init_intr);
 
  private:
@@ -106,14 +107,14 @@ class CalibHelper {
 
   static void computeInitialPose(
       const Calibration<double>::Ptr& calib, size_t cam_id,
-      const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
+      const Eigen::aligned_vector<Eigen::Vector4d>& corner_pos_3d,
       const basalt::CalibCornerData& cd, basalt::CalibInitPoseData& cp);
 
   static size_t computeReprojectionError(
       const UnifiedCamera<double>& cam_calib,
       const Eigen::aligned_vector<Eigen::Vector2d>& corners,
       const std::vector<int>& corner_ids,
-      const Eigen::aligned_vector<Eigen::Vector4d>& aprilgrid_corner_pos_3d,
+      const Eigen::aligned_vector<Eigen::Vector4d>& corner_pos_3d,
       const Sophus::SE3d& T_target_camera, double& error);
 };
 

diff --git a/include/basalt/calibration/aprilgrid.h → .../basalt/calibration/calibration_pattern.h b/include/basalt/calibration/aprilgrid.h → .../basalt/calibration/calibration_pattern.h
@@ -35,23 +35,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once
 
 #include <basalt/utils/sophus_utils.hpp>
+#include <vector>
+#include <memory>
 
 namespace basalt {
+struct ImageData;
+struct CalibCornerData;
 
-struct AprilGrid {
-  AprilGrid(const std::string &config_path);
+class CalibrationPattern {
+public:
+  CalibrationPattern(const std::string &config_path);
+  ~CalibrationPattern();
 
-  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_corner_pos_3d;
-  Eigen::aligned_vector<Eigen::Vector4d> aprilgrid_vignette_pos_3d;
+  Eigen::aligned_vector<Eigen::Vector4d> corner_pos_3d;
+  Eigen::aligned_vector<Eigen::Vector4d> vignette_pos_3d;
 
-  inline int getTagCols() const { return tagCols; }
-  inline int getTagRows() const { return tagRows; }
+  void detectCorners(const ImageData &img,
+    CalibCornerData &goodCorners,
+    CalibCornerData &badCorners,
+    const int64_t timestamp_ns,
+    int camIdx) const;
+
+  // Returns a list of lines, each of which consists of a list of corner IDs
+  std::vector<std::vector<int>> getFocalLengthTestLines() const;
 
  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
+  struct Impl;
+  std::unique_ptr<Impl> pImpl;
 };
 
 }  // namespace basalt
diff --git a/include/basalt/calibration/cam_calib.h b/include/basalt/calibration/cam_calib.h
@@ -43,12 +43,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <Eigen/Dense>
 
+#include <functional>
 #include <iostream>
 #include <limits>
 #include <thread>
 
-#include <basalt/calibration/aprilgrid.h>
 #include <basalt/calibration/calibration_helper.h>
+#include <basalt/calibration/calibration_pattern.h>
 #include <basalt/image/image.h>
 #include <basalt/utils/test_utils.h>
 #include <basalt/utils/sophus_utils.hpp>
@@ -60,7 +61,7 @@ class PosesOptimization;
 class CamCalib {
  public:
   CamCalib(const std::string &dataset_path, const std::string &dataset_type,
-           const std::string &aprilgrid_path, const std::string &cache_path,
+           const std::string &calib_pattern_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 = true);
 
@@ -126,7 +127,7 @@ class CamCalib {
   std::string dataset_path;
   std::string dataset_type;
 
-  AprilGrid april_grid;
+  CalibrationPattern calib_pattern;
 
   std::string cache_path;
   std::string cache_dataset_name;
@@ -142,6 +143,7 @@ class CamCalib {
   //////////////////////
 
   pangolin::Var<int> show_frame;
+  pangolin::Var<std::function<void(void)>> next_frame;
 
   pangolin::Var<bool> show_corners;
   pangolin::Var<bool> show_corners_rejected;
@@ -153,6 +155,10 @@ class CamCalib {
   pangolin::Var<double> huber_thresh;
 
   pangolin::Var<bool> opt_intr;
+  pangolin::Var<bool> opt_extrinsic_trans;
+  // maximum number of instrinsic parameters to optimize (including fx,fy,ppx,ppy)
+  // 0 means unlimited
+  pangolin::Var<int> max_intrinsics;
 
   pangolin::Var<bool> opt_until_convg;
   pangolin::Var<double> stop_thresh;

diff --git a/include/basalt/calibration/cam_imu_calib.h b/include/basalt/calibration/cam_imu_calib.h
@@ -43,12 +43,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <Eigen/Dense>
 
+#include <functional>
 #include <iostream>
 #include <limits>
 #include <thread>
 
-#include <basalt/calibration/aprilgrid.h>
 #include <basalt/calibration/calibration_helper.h>
+#include <basalt/calibration/calibration_pattern.h>
 #include <basalt/utils/test_utils.h>
 #include <basalt/utils/sophus_utils.hpp>
 
@@ -60,14 +61,16 @@ class SplineOptimization;
 class CamImuCalib {
  public:
   CamImuCalib(const std::string &dataset_path, const std::string &dataset_type,
-              const std::string &aprilgrid_path, const std::string &cache_path,
+              const std::string &calib_pattern_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);
 
   ~CamImuCalib();
 
   void initGui();
 
+  void resetCalibOpt();
+
   void setNumCameras(size_t n);
 
   void renderingLoop();
@@ -123,7 +126,7 @@ class CamImuCalib {
   std::string dataset_path;
   std::string dataset_type;
 
-  AprilGrid april_grid;
+  CalibrationPattern calib_pattern;
 
   std::string cache_path;
   std::string cache_dataset_name;
@@ -132,13 +135,14 @@ class CamImuCalib {
 
   bool show_gui;
 
-  const size_t MIN_CORNERS = 15;
+  const size_t MIN_CORNERS = 4;
 
   std::vector<double> imu_noise;
 
   //////////////////////
 
   pangolin::Var<int> show_frame;
+  pangolin::Var<std::function<void(void)>> next_frame;
 
   pangolin::Var<bool> show_corners;
   pangolin::Var<bool> show_corners_rejected;
@@ -167,6 +171,8 @@ class CamImuCalib {
   pangolin::Var<bool> opt_mocap;
 
   pangolin::Var<double> huber_thresh;
+  pangolin::Var<int> min_opt_frame;
+  pangolin::Var<int> max_opt_frame;
 
   pangolin::Var<bool> opt_until_convg;
   pangolin::Var<double> stop_thresh;

diff --git a/include/basalt/calibration/vignette.h b/include/basalt/calibration/vignette.h
@@ -34,8 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 #pragma once
 
-#include <basalt/calibration/aprilgrid.h>
 #include <basalt/calibration/calibration_helper.h>
+#include <basalt/calibration/calibration_pattern.h>
 
 #include <basalt/spline/rd_spline.h>
 
@@ -53,7 +53,7 @@ class VignetteEstimator {
       const Eigen::aligned_vector<Eigen::Vector2i> &resolutions,
       const std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
           &reprojected_vignette,
-      const AprilGrid &april_grid);
+      const CalibrationPattern &calib_pattern);
 
   void compute_error(std::map<TimeCamId, std::vector<double>>
                          *reprojected_vignette_error = nullptr);
@@ -80,7 +80,7 @@ class VignetteEstimator {
   Eigen::aligned_vector<Eigen::Vector2i> resolutions;
   std::map<TimeCamId, Eigen::aligned_vector<Eigen::Vector3d>>
       reprojected_vignette;
-  const AprilGrid &april_grid;
+  const CalibrationPattern &calib_pattern;
 
   size_t vign_size;
   std::vector<double> irradiance;

diff --git a/include/basalt/io/dataset_io.h b/include/basalt/io/dataset_io.h
@@ -61,6 +61,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <basalt/image/image.h>
 #include <basalt/utils/assert.h>
 
+#include <basalt/calibration/calibration.hpp>
 #include <basalt/camera/generic_camera.hpp>
 #include <basalt/camera/stereographic_param.hpp>
 
@@ -71,6 +72,7 @@ struct ImageData {
 
   ManagedImage<uint16_t>::Ptr img;
   double exposure;
+  int index = -1;
 };
 
 struct Observations {
@@ -102,7 +104,7 @@ struct MocapPoseData {
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-struct AprilgridCornersData {
+struct CalibrationPatternCornersData {
   int64_t timestamp_ns;
   int cam_id;
 
@@ -126,6 +128,8 @@ class VioDataset {
   virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
   virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;
 
+  virtual Calibration<double> *get_calib() { return nullptr; }
+
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 

diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h
@@ -74,8 +74,8 @@ struct LinearizeBase {
   typedef typename Eigen::aligned_vector<GyroData>::const_iterator GyroDataIter;
   typedef
       typename Eigen::aligned_vector<AccelData>::const_iterator AccelDataIter;
-  typedef typename Eigen::aligned_vector<AprilgridCornersData>::const_iterator
-      AprilgridCornersDataIter;
+  typedef typename Eigen::aligned_vector<CalibrationPatternCornersData>::const_iterator
+      CalibrationPatternCornersDataIter;
 
   template <int INTRINSICS_SIZE>
   struct PoseCalibH {
@@ -99,7 +99,7 @@ struct LinearizeBase {
   struct CalibCommonData {
     const Calibration<Scalar>* calibration = nullptr;
     const MocapCalibration<Scalar>* mocap_calibration = nullptr;
-    const Eigen::aligned_vector<Eigen::Vector4d>* aprilgrid_corner_pos_3d =
+    const Eigen::aligned_vector<Eigen::Vector4d>* calib_corner_pos_3d =
         nullptr;
 
     // Calib data
@@ -115,6 +115,8 @@ struct LinearizeBase {
     const Vector3* g = nullptr;
 
     bool opt_intrinsics;
+    bool opt_extrinsic_trans;
+    int max_intrinsics = 0;
 
     // Cam-IMU options
     bool opt_cam_time_offset;
@@ -140,11 +142,11 @@ struct LinearizeBase {
     Eigen::Matrix<double, 2, CamT::N> d_r_d_param;
 
     BASALT_ASSERT_STREAM(
-        corner_id < int(common_data.aprilgrid_corner_pos_3d->size()),
+        corner_id < int(common_data.calib_corner_pos_3d->size()),
         "corner_id " << corner_id);
 
     Eigen::Vector4d point3d =
-        T_c_w * common_data.aprilgrid_corner_pos_3d->at(corner_id);
+        T_c_w * common_data.calib_corner_pos_3d->at(corner_id);
 
     Eigen::Vector2d proj;
     bool valid;