CMakeLists.txt

@@ -48,6 +48,7 @@ set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME_LOWERCASE}_${CPACK_PACKAGE_VERSION_M
 set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
 include(CPack)
 
+
 # Configure CCache if available
 if (NOT CMAKE_C_COMPILER_LAUNCHER AND NOT CMAKE_CXX_COMPILER_LAUNCHER)
   find_program(CCACHE_PROGRAM ccache)
@@ -288,12 +289,12 @@ add_library(basalt::magic_enum INTERFACE IMPORTED)
 set_property(TARGET basalt::magic_enum PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/magic_enum/include)
 
 # main library
-add_library(basalt_internal STATIC)
+add_library(basalt SHARED)
 
 # List all header and source files with target_sources.
 # This ensures, e.g., that QtCreator >= 6.0 properly recognize these headers to belong to the project.
 # 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_internal
+target_sources(basalt
   PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/aprilgrid.h
     ${CMAKE_CURRENT_SOURCE_DIR}/include/basalt/calibration/calibration_helper.h
@@ -364,6 +365,7 @@ target_sources(basalt_internal
     ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_abs_sc.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_base.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_rel_sc.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/monado/slam_tracker.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/optical_flow/optical_flow.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/utils/keypoints.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/src/utils/system_utils.cpp
@@ -380,48 +382,45 @@ target_sources(basalt_internal
     ${CMAKE_CURRENT_SOURCE_DIR}/src/vi_estimator/vio_estimator.cpp
 )
 
-target_link_libraries(basalt_internal
+target_link_libraries(basalt
   PUBLIC ${STD_CXX_FS} basalt::opencv basalt::basalt-headers TBB::tbb pangolin basalt::cli11 basalt::monado
   PRIVATE basalt::magic_enum rosbag apriltag opengv nlohmann::json fmt::fmt)
-target_include_directories(basalt_internal PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_include_directories(basalt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include)
-target_compile_definitions(basalt_internal PUBLIC ${BASALT_COMPILE_DEFINITIONS})
+target_compile_definitions(basalt PUBLIC ${BASALT_COMPILE_DEFINITIONS})
-#target_compile_definitions(basalt_internal PUBLIC BASALT_DISABLE_ASSERTS)
+#target_compile_definitions(basalt PUBLIC BASALT_DISABLE_ASSERTS)
 
-add_library(basalt SHARED)
-target_sources(basalt PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src/monado/slam_tracker.cpp)
-target_link_libraries(basalt basalt_internal)
 
 add_executable(basalt_calibrate src/calibrate.cpp src/calibration/cam_calib.cpp)
-target_link_libraries(basalt_calibrate basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_calibrate basalt pangolin basalt::cli11)
 
 add_executable(basalt_calibrate_imu src/calibrate_imu.cpp src/calibration/cam_imu_calib.cpp)
-target_link_libraries(basalt_calibrate_imu basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_calibrate_imu basalt pangolin basalt::cli11)
 
 
 add_executable(basalt_vio_sim src/vio_sim.cpp)
-target_link_libraries(basalt_vio_sim basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_vio_sim basalt pangolin basalt::cli11)
 
 add_executable(basalt_mapper_sim src/mapper_sim.cpp)
-target_link_libraries(basalt_mapper_sim basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_mapper_sim basalt pangolin basalt::cli11)
 
 # mapper sim native doesn't use template free interface
 if(BASALT_INSTANTIATIONS_DOUBLE)
   add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
-  target_link_libraries(basalt_mapper_sim_naive basalt_internal pangolin basalt::cli11)
+  target_link_libraries(basalt_mapper_sim_naive basalt pangolin basalt::cli11)
 endif()
 
 add_executable(basalt_mapper src/mapper.cpp)
-target_link_libraries(basalt_mapper basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_mapper basalt pangolin basalt::cli11)
 
 
 add_executable(basalt_opt_flow src/opt_flow.cpp)
-target_link_libraries(basalt_opt_flow basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_opt_flow basalt pangolin basalt::cli11)
 
 add_executable(basalt_vio src/vio.cpp)
-target_link_libraries(basalt_vio basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_vio basalt pangolin basalt::cli11)
 
 add_executable(basalt_time_alignment src/time_alignment.cpp)
-target_link_libraries(basalt_time_alignment basalt_internal pangolin basalt::cli11)
+target_link_libraries(basalt_time_alignment basalt pangolin basalt::cli11)
 
 add_executable(basalt_kitti_eval src/kitti_eval.cpp)
 target_link_libraries(basalt_kitti_eval basalt::basalt-headers basalt::cli11)
@@ -429,24 +428,15 @@ target_link_libraries(basalt_kitti_eval basalt::basalt-headers basalt::cli11)
 find_package(realsense2 QUIET)
 if(realsense2_FOUND)
   add_executable(basalt_rs_t265_record src/rs_t265_record.cpp src/device/rs_t265.cpp)
-  target_link_libraries(basalt_rs_t265_record basalt_internal realsense2::realsense2 pangolin basalt::cli11)
+  target_link_libraries(basalt_rs_t265_record basalt realsense2::realsense2 pangolin basalt::cli11)
 
   add_executable(basalt_rs_t265_vio src/rs_t265_vio.cpp src/device/rs_t265.cpp)
-  target_link_libraries(basalt_rs_t265_vio basalt_internal realsense2::realsense2 pangolin basalt::cli11)
+  target_link_libraries(basalt_rs_t265_vio basalt realsense2::realsense2 pangolin basalt::cli11)
 endif()
 
-# Let's export only the slam_tracker.hpp functions for libORB_SLAM3.so,
-# But will let a libORB_SLAM3_EXAMPLES.so for the regular ORB-SLAM3 examples to use
-# if the user builds with -DBUILD_EXAMPLES=On
-set_target_properties(${PROJECT_NAME} PROPERTIES
-  C_VISIBILITY_PRESET hidden
-  CXX_VISIBILITY_PRESET hidden
-  VISIBILITY_INLINES_HIDDEN YES
-  LINK_DEPENDS ${CMAKE_CURRENT_LIST_DIR}/cmake_modules/basalt.map
-  LINK_FLAGS "-Wl,--version-script=${CMAKE_CURRENT_LIST_DIR}/cmake_modules/basalt.map"
-)
 
-install(TARGETS basalt basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper basalt_opt_flow basalt_vio basalt_kitti_eval basalt_time_alignment
+
+install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper basalt_opt_flow basalt_vio basalt_kitti_eval basalt_time_alignment basalt
   EXPORT BasaltTargets
   RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
   LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib

README.md

@@ -106,8 +106,12 @@ This step is optional but you can try Basalt without Monado with one of the foll
 
 ### Monado Specifics
 
-Note: be careful when manually enabling ASan when building Monado as some
+You'll need to compile Monado with the same Eigen used in Basalt, and with the
-crashes have been reported. I'm still trying to figure out why those happen.
+same flags. For that, set these with CMake:
+`-DEIGEN3_INCLUDE_DIR=$bsltdeps/basalt/thirdparty/basalt-headers/thirdparty/eigen
+-DCMAKE_C_FLAGS="-march=native" -DCMAKE_CXX_FLAGS="-march=native"` otherwise
+Monado will automatically use your system's Eigen, and having mismatched Eigen
+version/flags can cause a lot of headaches.
 
 Run an OpenXR app like `hello_xr` with the following environment variables set
 

cmake_modules/basalt.map

@@ -1,25 +0,0 @@
-LIBXRTSLAM_BASALT_4.0.0 {
-  global:
-    extern "C++" {
-      xrt::auxiliary::tracking::slam::slam_tracker::slam_tracker*;
-      xrt::auxiliary::tracking::slam::slam_tracker::*slam_tracker*;
-
-      xrt::auxiliary::tracking::slam::slam_tracker::initialize*;
-      xrt::auxiliary::tracking::slam::slam_tracker::start*;
-      xrt::auxiliary::tracking::slam::slam_tracker::is_running*;
-      xrt::auxiliary::tracking::slam::slam_tracker::stop*;
-      xrt::auxiliary::tracking::slam::slam_tracker::finalize*;
-
-      xrt::auxiliary::tracking::slam::slam_tracker::push_imu_sample*;
-      xrt::auxiliary::tracking::slam::slam_tracker::push_frame*;
-      xrt::auxiliary::tracking::slam::slam_tracker::try_dequeue_pose*;
-
-      xrt::auxiliary::tracking::slam::slam_tracker::supports_feature*;
-      xrt::auxiliary::tracking::slam::slam_tracker::use_feature*;
-
-      xrt::auxiliary::tracking::slam::IMPLEMENTATION_VERSION_MAJOR*;
-      xrt::auxiliary::tracking::slam::IMPLEMENTATION_VERSION_MINOR*;
-      xrt::auxiliary::tracking::slam::IMPLEMENTATION_VERSION_PATCH*
-    };
-  local: *;
-};

data/monado/index.toml

@@ -4,7 +4,7 @@
 show-gui=1
 
 # Ground-truth camera calibration used for simulation.
-cam-calib="/home/mateo/Documents/apps/bsltdeps/basalt/data/index_calib.json"
+cam-calib="/home/mateo/Documents/apps/bsltdeps/basalt/data/index_calib_oxrimu.json"
 
 # Path to config file.
 config-path="/home/mateo/Documents/apps/bsltdeps/basalt/data/euroc_config.json"

doc/monado/Vive.md

@@ -15,20 +15,20 @@ work very well.
 The general calibration procedure is as follows:
 
 1. Setup the calibration target:
-    1. Download [the one from kalibr](https://drive.google.com/file/d/1DqKWgePodCpAKJCd_Bz-hfiEQOSnn_k0/view)
+  1. Download [the one from kalibr](https://drive.google.com/file/d/1DqKWgePodCpAKJCd_Bz-hfiEQOSnn_k0/view)
-    2. Open the pdf file and measure with a ruler the black square side (e.g., 24 inch 1080p screen, 34% zoom in Okular viewer, gives 3cm)
+  2. Open the pdf file and measure with a ruler the black square side (e.g., 24 inch 1080p screen, 34% zoom in Okular viewer, gives 3cm)
-    3. Update aprlgrid_6x6.json "tagSize" property in meters (e.g., 3cm would be "0.03")
+  3. Update aprlgrid_6x6.json "tagSize" property in meters (e.g., 3cm would be "0.03")
 2. Record camera calibration sequence with the euroc recorder in Monado:
-    - See calib-cam3 example from TUM-VI https://vision.in.tum.de/data/datasets/visual-inertial-dataset
+  - See calib-cam3 example from TUM-VI https://vision.in.tum.de/data/datasets/visual-inertial-dataset
-    - Or this example from ORB-SLAM3: https://www.youtube.com/watch?v=R_K9-O4ool8
+  - Or this example from ORB-SLAM3: https://www.youtube.com/watch?v=R_K9-O4ool8
-    - Note that for stereo calibration you want to cover as much as possible of
+  - Note that for stereo calibration you want to cover as much as possible of
-      both cameras images but always trying to keep >90% of the calibration target
+    both cameras images but always trying to keep >90% of the calibration target
-      visible in both views. This can be a little tricky so practice it beforehand.
+    visible in both views. This can be a little tricky so practice it beforehand.
 3. Record camera-imu calibration sequence: (faster motions)
-    - Similar recommendations as in the previous sequence but this time we want
+  - Similar recommendations as in the previous sequence but this time we want
-      faster motions that excite all IMU axes, see these examples:
+    faster motions that excite all IMU axes, see these examples:
-    - calib-imu1 from TUM-VI https://vision.in.tum.de/data/datasets/visual-inertial-dataset
+  - calib-imu1 from TUM-VI https://vision.in.tum.de/data/datasets/visual-inertial-dataset
-    - Or this from ORB-SLAM3 https://www.youtube.com/watch?v=4XkivVLw5k4
+  - Or this from ORB-SLAM3 https://www.youtube.com/watch?v=4XkivVLw5k4
 4. Now run camera calibration as explained [here](https://gitlab.freedesktop.org/mateosss/basalt/-/blob/xrtslam/doc/Calibration.md#camera-calibration)
 5. Then camera+imu (no mocap) calibration as explained [here](https://gitlab.freedesktop.org/mateosss/basalt/-/blob/xrtslam/doc/Calibration.md#camera-imu-mocap-calibration)
 6. If the datasets are not good, the calibration will likely not be very good as

scripts/install_arch_deps.sh

@@ -1,17 +0,0 @@
-#!/bin/sh
-##
-## BSD 3-Clause License
-##
-## This file includes additional changes by Cathal Flynn to support Arch Linux,
-## targeted at the Basalt for Monado fork of the Basalt project.
-##
-## https://gitlab.com/VladyslavUsenko/basalt.git
-## https://gitlab.freedesktop.org/mateosss/basalt.git
-## 
-##
-## Copyright (c) 2022, Cathal Flynn.
-## All rights reserved.
-##
-
-sudo pacman -Syu
-sudo pacman -Sy --noconfirm gcc cmake git tbb eigen glew ccache libjpeg-turbo libpng lz4 bzip2 boost boost-libs gtest opencv fmt

scripts/install_deps.sh

@@ -17,8 +17,6 @@ else
 	DISTRO=$( awk -F= '/^ID/{print $2}' /etc/os-release )
 	if [ "$DISTRO" == "fedora" ]; then
 		${DIR}/install_fedora_deps.sh
-	elif [ "$DISTRO" == "arch" ]; then
-		${DIR}/install_arch_deps.sh
 	else
 		${DIR}/install_ubuntu_deps.sh
 	fi

src/monado/slam_tracker.cpp

@@ -22,17 +22,11 @@
 #include <basalt/vi_estimator/vio_estimator.h>
 #include "basalt/utils/vis_utils.h"
 
-#if defined(_WIN32) || defined(__CYGWIN__)
-#define EXPORT __declspec(dllexport)
-#else
-#define EXPORT __attribute__((visibility("default")))
-#endif
-
 namespace xrt::auxiliary::tracking::slam {
 
-EXPORT extern const int IMPLEMENTATION_VERSION_MAJOR = HEADER_VERSION_MAJOR;
+const int IMPLEMENTATION_VERSION_MAJOR = HEADER_VERSION_MAJOR;
-EXPORT extern const int IMPLEMENTATION_VERSION_MINOR = HEADER_VERSION_MINOR;
+const int IMPLEMENTATION_VERSION_MINOR = HEADER_VERSION_MINOR;
-EXPORT extern const int IMPLEMENTATION_VERSION_PATCH = HEADER_VERSION_PATCH;
+const int IMPLEMENTATION_VERSION_PATCH = HEADER_VERSION_PATCH;
 
 using std::cout;
 using std::make_shared;
@@ -47,6 +41,28 @@ using std::unordered_set;
 using std::vector;
 using namespace basalt;
 
+string imu2str(const imu_sample &s) {
+  string str = "imu_sample ";
+  str += "t=" + to_string(s.timestamp) + " ";
+  str += "a=[" + to_string(s.ax) + ", " + to_string(s.ay) + ", " + to_string(s.az) + "] ";
+  str += "w=[" + to_string(s.wx) + ", " + to_string(s.wy) + ", " + to_string(s.wz) + "]";
+  return str;
+}
+
+string img2str(const img_sample &s) {
+  string str = "img_sample ";
+  str += s.is_left ? "left " : "right ";
+  str += "t=" + to_string(s.timestamp);
+  return str;
+}
+
+string pose2str(const pose &p) {
+  string str = "pose ";
+  str += "p=[" + to_string(p.px) + ", " + to_string(p.py) + ", " + to_string(p.pz) + "] ";
+  str += "r=[" + to_string(p.rx) + ", " + to_string(p.ry) + ", " + to_string(p.rz) + ", " + to_string(p.rw) + "]";
+  return str;
+}
+
 static const vector<string> timing_titles{
     "frame_ts",
     "tracker_received",
@@ -531,29 +547,29 @@ struct slam_tracker::implementation {
   void enable_pose_ext_features(bool enable) { pose_features_enabled = enable; }
 };
 
-EXPORT slam_tracker::slam_tracker(const string &config_file) { impl = make_unique<slam_tracker::implementation>(config_file); }
+slam_tracker::slam_tracker(const string &config_file) { impl = make_unique<slam_tracker::implementation>(config_file); }
 
-EXPORT slam_tracker::~slam_tracker() = default;
+slam_tracker::~slam_tracker() = default;
 
-EXPORT void slam_tracker::initialize() { impl->initialize(); }
+void slam_tracker::initialize() { impl->initialize(); }
 
-EXPORT void slam_tracker::start() { impl->start(); }
+void slam_tracker::start() { impl->start(); }
 
-EXPORT void slam_tracker::stop() { impl->stop(); }
+void slam_tracker::stop() { impl->stop(); }
 
-EXPORT void slam_tracker::finalize() { impl->finalize(); }
+void slam_tracker::finalize() { impl->finalize(); }
 
-EXPORT bool slam_tracker::is_running() { return impl->is_running(); }
+bool slam_tracker::is_running() { return impl->is_running(); }
 
-EXPORT void slam_tracker::push_imu_sample(const imu_sample &s) { impl->push_imu_sample(s); }
+void slam_tracker::push_imu_sample(const imu_sample &s) { impl->push_imu_sample(s); }
 
-EXPORT void slam_tracker::push_frame(const img_sample &sample) { impl->push_frame(sample); }
+void slam_tracker::push_frame(const img_sample &sample) { impl->push_frame(sample); }
 
-EXPORT bool slam_tracker::try_dequeue_pose(pose &pose) { return impl->try_dequeue_pose(pose); }
+bool slam_tracker::try_dequeue_pose(pose &pose) { return impl->try_dequeue_pose(pose); }
 
-EXPORT bool slam_tracker::supports_feature(int feature_id) { return impl->supports_feature(feature_id); }
+bool slam_tracker::supports_feature(int feature_id) { return impl->supports_feature(feature_id); }
 
-EXPORT bool slam_tracker::use_feature(int feature_id, const shared_ptr<void> &params, shared_ptr<void> &result) {
+bool slam_tracker::use_feature(int feature_id, const shared_ptr<void> &params, shared_ptr<void> &result) {
   return impl->use_feature(feature_id, params, result);
 }
 

src/monado/slam_tracker_ui.hpp

@@ -1,6 +1,5 @@
 #pragma once
 
-#include "pangolin/display/display.h"
 #include <pangolin/display/image_view.h>
 #include <pangolin/pangolin.h>
 
@@ -231,7 +230,7 @@ class slam_tracker_ui {
       pangolin::FinishFrame();
     }
 
-    pangolin::QuitAll();
+    pangolin::DestroyWindow(window_name);
     cout << "Finished ui_runner\n";
   }
 

src/rs_t265_vio.cpp

@@ -391,13 +391,13 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) {
     glEnable(GL_BLEND);
     glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 
-    if (curr_vis_data.get() && cam_id < curr_vis_data->projections->size()) {
+    if (curr_vis_data.get() && cam_id < curr_vis_data->projections.size()) {
-      const auto& points = curr_vis_data->projections->at(cam_id);
+      const auto& points = curr_vis_data->projections[cam_id];
 
       if (!points.empty()) {
         double min_id = points[0][2], max_id = points[0][2];
 
-        for (const auto& points2 : *curr_vis_data->projections)
+        for (const auto& points2 : curr_vis_data->projections)
           for (const auto& p : points2) {
             min_id = std::min(min_id, p[2]);
             max_id = std::max(max_id, p[2]);

src/vi_estimator/sqrt_keypoint_vio.cpp

@@ -230,14 +230,10 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
             prev_frame->t_ns, last_state.getState().bias_gyro,
             last_state.getState().bias_accel));
 
-        BASALT_ASSERT_MSG(prev_frame->t_ns != curr_frame->t_ns,
+        BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns,
                           "duplicate frame timestamps?! zero time delta leads "
                           "to invalid IMU integration.");
 
-        BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns,
-                          "frame timestamps not monotonically increasing?! "
-                          "are we going 88mph and inside a delorean?");
-
         while (data->t_ns <= prev_frame->t_ns) {
           data = popFromImuDataQueue();
           if (!data) break;

test/CMakeLists.txt

@@ -6,22 +6,22 @@ include_directories(../thirdparty/basalt-headers/test/include)
 
 
 add_executable(test_spline_opt src/test_spline_opt.cpp)
-target_link_libraries(test_spline_opt gtest gtest_main basalt_internal)
+target_link_libraries(test_spline_opt gtest gtest_main basalt)
 
 add_executable(test_vio src/test_vio.cpp)
-target_link_libraries(test_vio gtest gtest_main basalt_internal)
+target_link_libraries(test_vio gtest gtest_main basalt)
 
 add_executable(test_nfr src/test_nfr.cpp)
-target_link_libraries(test_nfr gtest gtest_main basalt_internal)
+target_link_libraries(test_nfr gtest gtest_main basalt)
 
 add_executable(test_qr src/test_qr.cpp)
-target_link_libraries(test_qr gtest gtest_main basalt_internal)
+target_link_libraries(test_qr gtest gtest_main basalt)
 
 add_executable(test_linearization src/test_linearization.cpp)
-target_link_libraries(test_linearization gtest gtest_main basalt_internal)
+target_link_libraries(test_linearization gtest gtest_main basalt)
 
 add_executable(test_patch src/test_patch.cpp)
-target_link_libraries(test_patch gtest gtest_main basalt_internal)
+target_link_libraries(test_patch gtest gtest_main basalt)
 
 enable_testing()
 

thirdparty/basalt-headers

@@ -1 +1 @@
-Subproject commit 233c6b901771f50a6a135b583a38885f0c3d6ed9
+Subproject commit ff99f3d040fb6aeb3535338ba69c6300191c9bda