Merge branch 'niko_devel' into 'master'

ci, cmake, macos compatibility

See merge request slam/basalt!1

.clang-format

@@ -2,5 +2,6 @@
 Language:       Cpp
 BasedOnStyle:   Google
 IndentWidth:    2
+IncludeBlocks:  Preserve
 ...
 

.gitlab-ci.yml

@@ -1,83 +1,168 @@
 image: vladyslavusenko/b_image:latest
 
+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+  BUILD_TYPE: Release
+  CXX_MARCH: native
+  CMAKE_INSTALL_PREFIX: /usr/
+  DEB_DIR: deb
+
 stages:
   - build
   - test
-  - test_results
+  - eval
+  - eval_results
   - deploy
 
-compile:
+# template for docker builds with ccache
-  stage: build
+.prepare_docker_template: &prepare_docker_definition
-  variables:
+  tags:
-    CXX_MARCH: 'corei7-avx'
+    - docker
   before_script:
     - mkdir -p ccache
     - export CCACHE_BASEDIR=${PWD}
     - export CCACHE_DIR=${PWD}/ccache
-  tags: 
-    - docker
   cache:
-    key: bionic
     paths:
     - ccache/
+    key: ${CACHE_KEY}
+
+# template for build & unit test & make deb configurations
+.compile_test_package_template: &compile_test_package_definition
+  stage: build
   script:
-    - ./scripts/update_submodules.sh
     - mkdir build
     - cd build
-    - cmake .. -DCMAKE_BUILD_TYPE=Release -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=/usr/
+    - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
     - make -j3
+    - ctest
     - make package
-    - cd test/
+    - cd ..
-    - ctest -V
+    - mkdir $DEB_DIR
-    - cd ../../
+    - mv build/*.deb $DEB_DIR/
-    - mkdir deb_bionic
+    - dpkg -i $DEB_DIR/*.deb
-    - cp build/*.deb deb_bionic/
+    # smoke test to see if all executables at least start up
+    - basalt_calibrate --help
+    - basalt_calibrate_imu --help
+    - basalt_mapper --help
+    - basalt_mapper_sim --help
+    - basalt_mapper_sim_naive --help
+    - basalt_opt_flow --help
+    - basalt_vio --help
+    - basalt_vio_sim --help
+
+# template for build & unit test configurations (no deb)
+.compile_test_template: &compile_test_definition
+  stage: build
+  script:
+    - mkdir build
+    - cd build
+    - cmake .. -DCMAKE_BUILD_TYPE=${BUILD_TYPE} -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
+    - make -j3
+    - ctest
+    # smoke test to see if all executables at least start up
+    - ./basalt_calibrate --help
+    - ./basalt_calibrate_imu --help
+    - ./basalt_mapper --help
+    - ./basalt_mapper_sim --help
+    - ./basalt_mapper_sim_naive --help
+    - ./basalt_opt_flow --help
+    - ./basalt_vio --help
+    - ./basalt_vio_sim --help
+
+# template to test debian
+.test_deb_template: &test_deb_definition
+  stage: test
+  tags:
+    - docker
+  variables:
+    GIT_STRATEGY: none
+  script:
+    - dpkg -i $DEB_DIR/*.deb
+    # smoke test to see if all executables at least start up
+    - basalt_calibrate --help
+    - basalt_calibrate_imu --help
+    - basalt_mapper --help
+    - basalt_mapper_sim --help
+    - basalt_mapper_sim_naive --help
+    - basalt_opt_flow --help
+    - basalt_vio --help
+    - basalt_vio_sim --help
+
+bionic-release-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  variables:
+    CXX_MARCH: 'corei7-avx'
+    DEB_DIR: deb_bionic
+    CACHE_KEY: bionic-release
   artifacts:
     paths:
     - deb_bionic/*.deb
     - scripts/eval_full/*
     expire_in: 1 week
 
-compile_16_04:
+xenial-release-compile:
-  stage: build
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  image: vladyslavusenko/b_image_xenial:latest
   variables:
     CXX_MARCH: 'corei7-avx'
-  before_script:
+    DEB_DIR: deb_xenial
-    - mkdir -p ccache
+    CACHE_KEY: xenial-release
-    - export CCACHE_BASEDIR=${PWD}
-    - export CCACHE_DIR=${PWD}/ccache
-  tags: 
-    - docker
-  cache:
-    key: xenial
-    paths:
-    - ccache/
-  script:
-    - ./scripts/update_submodules.sh
-    - mkdir build
-    - cd build
-    - cmake .. -DCMAKE_BUILD_TYPE=Release -DCXX_MARCH=${CXX_MARCH} -DCMAKE_INSTALL_PREFIX=/usr/
-    - make -j3
-    - make package
-    - cd test/
-    - ctest -V
-    - cd ../../
-    - mkdir deb_xenial
-    - cp build/*.deb deb_xenial/
   artifacts:
     paths:
     - deb_xenial/*.deb
     expire_in: 1 week
-  only:
-    - master
-  image: vladyslavusenko/b_image_xenial:latest
 
+bionic-debug-compile:
+  <<: *prepare_docker_definition
+  <<: *compile_test_package_definition
+  variables:
+    BUILD_TYPE: Debug
+    CACHE_KEY: bionic-debug
 
-eval_euroc:
+bionic-relwithdebinfo-compile:
-  stage: test
+  <<: *prepare_docker_definition
-  parallel: 10
+  <<: *compile_test_package_definition
+  variables:
+    BUILD_TYPE: RelWithDebInfo
+    CACHE_KEY: bionic-relwithdebinfo 
+
+elcapitan-relwithdebinfo-compile:
+  <<: *compile_test_definition
+  tags: [macos, "10.11"]
+  variables:
+    BUILD_TYPE: RelWithDebInfo
+
+# check if clang-format would make any changes
+clang-format:
   tags:
-    - dataset-eval
+    - docker
+  stage: build
+  variables:
+    GIT_SUBMODULE_STRATEGY: none
+  script:
+    - ./scripts/clang-format-all.sh
+    # check if any files are now modified and error if yes
+    - (if git diff --name-only --diff-filter=M | grep '\..pp$'; then echo $'\n    Some files are not properly formatted. You can use "./scripts/clang-format-all.sh".\n'; git diff --diff-filter=M; false; fi)
+
+bionic-test:
+  <<: *test_deb_definition
+  variables:
+    DEB_DIR: deb_bionic
+
+xenial-test:
+  <<: *test_deb_definition
+  image: vladyslavusenko/b_image_xenial:latest
+  variables:
+    DEB_DIR: deb_xenial
+
+# evaluate on EuRoC sequences
+eval_euroc:
+  stage: eval
+  parallel: 10
+  tags: [docker, dataset-eval]
   variables:
     GIT_STRATEGY: none
   only:
@@ -91,13 +176,14 @@ eval_euroc:
     - cd scripts/eval_full
     - ./run_evaluations.sh
 
+# aggregate results for all EuRoC sequences
 gen_results_euroc:
-  stage: test_results
+  stage: eval_results
   variables:
     GIT_STRATEGY: none
-  when: on_success
   tags: 
     - docker
+  when: on_success
   only:
     - master
   artifacts:
@@ -109,11 +195,15 @@ gen_results_euroc:
     - cat euroc_results.txt
     - mv euroc_results.txt ../../
 
-
+# deploy deb packages
 deploy:
   stage: deploy
+  variables:
+    GIT_STRATEGY: none
   tags: 
     - docker
+  only:
+    - master
   before_script:
     - eval $(ssh-agent -s)
     - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null
@@ -121,10 +211,6 @@ deploy:
     - chmod 700 ~/.ssh
     - echo "$SSH_KNOWN_HOSTS" > ~/.ssh/known_hosts
     - chmod 644 ~/.ssh/known_hosts
-  variables:
-    GIT_STRATEGY: none
-  only:
-    - master
   script:
     - scp $SCP_FLAGS deb_xenial/*.deb $REPOSITORY_URL/xenial/    
     - scp $SCP_FLAGS deb_bionic/*.deb $REPOSITORY_URL/bionic/

CMakeLists.txt

@@ -1,9 +1,14 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.10)
+
+include("thirdparty/basalt-headers/cmake_modules/PreProjectWorkarounds.cmake")
+
 project(basalt)
 
 set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake_modules/" ${CMAKE_MODULE_PATH})
 
+if(NOT EIGEN_ROOT)
   set(EIGEN_ROOT "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/basalt-headers/thirdparty/eigen")
+endif()
 
 string(TOLOWER "${PROJECT_NAME}" PROJECT_NAME_LOWERCASE)
 find_program(DPKG_PROGRAM dpkg DOC "dpkg program of Debian-based systems")
@@ -11,8 +16,7 @@ if(DPKG_PROGRAM)
   execute_process(
     COMMAND ${DPKG_PROGRAM} --print-architecture
     OUTPUT_VARIABLE CPACK_DEBIAN_PACKAGE_ARCHITECTURE
-    OUTPUT_STRIP_TRAILING_WHITESPACE
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
-  )
 endif(DPKG_PROGRAM)
 
 
@@ -20,80 +24,155 @@ find_program(LSB_RELEASE_PROGRAM lsb_release DOC "lsb_release program of Debian-
 if(LSB_RELEASE_PROGRAM)
   execute_process(COMMAND ${LSB_RELEASE_PROGRAM} -rs
     OUTPUT_VARIABLE LSB_RELEASE_ID_SHORT
-    OUTPUT_STRIP_TRAILING_WHITESPACE
+    OUTPUT_STRIP_TRAILING_WHITESPACE)
-)
 
   if(${LSB_RELEASE_ID_SHORT} EQUAL "18.04")
     set(DEBIAN_DEPENDS "libtbb2, liblz4-1, libbz2-1.0, libboost-filesystem1.65.1, libboost-date-time1.65.1, libboost-program-options1.65.1, libboost-regex1.65.1, libopencv-dev, libglew2.0, libjpeg8, libpng16-16")
+
   elseif(${LSB_RELEASE_ID_SHORT} EQUAL "16.04")
     set(DEBIAN_DEPENDS "libtbb2, liblz4-1, libbz2-1.0, libboost-filesystem1.58.0, libboost-date-time1.58.0, libboost-program-options1.58.0, libboost-regex1.58.0, libopencv-dev, libglew1.13, libjpeg8, libpng12-0")
-endif(${LSB_RELEASE_ID_SHORT} EQUAL "18.04")
+  endif()
 
 endif(LSB_RELEASE_PROGRAM)
 
 string(TIMESTAMP PROJECT_VERSION_REVISION "%Y%m%d%H%M")
 
-SET(CPACK_GENERATOR "DEB")
+set(CPACK_GENERATOR "DEB")
-SET(CPACK_DEBIAN_PACKAGE_MAINTAINER "Vladyslav Usenko <vlad.usenko@tum.de>")
+set(CPACK_DEBIAN_PACKAGE_MAINTAINER "Vladyslav Usenko <vlad.usenko@tum.de>")
-SET(CPACK_PACKAGE_VERSION_MAJOR "0")
+set(CPACK_PACKAGE_VERSION_MAJOR "0")
-SET(CPACK_PACKAGE_VERSION_MINOR "1")
+set(CPACK_PACKAGE_VERSION_MINOR "1")
-SET(CPACK_PACKAGE_VERSION_PATCH "0-${PROJECT_VERSION_REVISION}~${LSB_RELEASE_ID_SHORT}")
+set(CPACK_PACKAGE_VERSION_PATCH "0-${PROJECT_VERSION_REVISION}~${LSB_RELEASE_ID_SHORT}")
-SET(CPACK_DEBIAN_PACKAGE_DEPENDS ${DEBIAN_DEPENDS})
+set(CPACK_DEBIAN_PACKAGE_DEPENDS ${DEBIAN_DEPENDS})
-SET(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME_LOWERCASE}_${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}_${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
+set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME_LOWERCASE}_${CPACK_PACKAGE_VERSION_MAJOR}.${CPACK_PACKAGE_VERSION_MINOR}.${CPACK_PACKAGE_VERSION_PATCH}_${CPACK_DEBIAN_PACKAGE_ARCHITECTURE}")
-SET(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
+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)
   if(CCACHE_PROGRAM)
     message(STATUS "Found ccache: ${CCACHE_PROGRAM}")
     set(CMAKE_C_COMPILER_LAUNCHER   ${CCACHE_PROGRAM})
     set(CMAKE_CXX_COMPILER_LAUNCHER ${CCACHE_PROGRAM})
-endif(CCACHE_PROGRAM)
+  else()
+    message(STATUS "Dind't find ccache")
+  endif()
+else()
+  message(STATUS "Compiler launcher already set. Not configuring ccache.")
+  message(STATUS "CMAKE_C_COMPILER_LAUNCHER: ${CMAKE_C_COMPILER_LAUNCHER}")
+  message(STATUS "CMAKE_CXX_COMPILER_LAUNCHER: ${CMAKE_CXX_COMPILER_LAUNCHER}")
+endif()
 
-IF( NOT CMAKE_BUILD_TYPE )
+if( NOT CMAKE_BUILD_TYPE )
-   SET( CMAKE_BUILD_TYPE Release)
+  set( CMAKE_BUILD_TYPE Release)
-ENDIF()
+endif()
+
+if(NOT CXX_MARCH)
+  set(CXX_MARCH native)
+endif()
 
-IF(NOT CXX_MARCH)
-   SET(CXX_MARCH native)
-ENDIF()
 
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
+
 # Flags used for CHECK_CXX_SOURCE_COMPILES
-set(CMAKE_REQUIRED_FLAGS "-Wno-unused-variable -Wno-unused-value")
+set(CMAKE_REQUIRED_FLAGS "-Wno-error")
 
 
-IF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
+# save flags passed by user
-   SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+set(BASALT_PASSED_CXX_FLAGS "${CMAKE_CXX_FLAGS}")
-   SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}")
+
-   SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}")
+set(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g -DEIGEN_INITIALIZE_MATRICES_BY_NAN")          # cmake default: "-g"
-   SET(CMAKE_CXX_FLAGS "-Wall -Werror -Wextra -Wno-deprecated-register -Qunused-arguments -fcolor-diagnostics -fopenmp")
+set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -DEIGEN_INITIALIZE_MATRICES_BY_NAN")  # cmake default: "-O2 -g -DNDEBUG"
-ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "AppleClang")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")                                      # cmake default: "-O3 -DNDEBUG"
-   SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+
-   SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}")
+# base set of compile flags
-   SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}")
+set(BASALT_CXX_FLAGS "-Wall -Wextra -Werror -Wno-unused-parameter -ftemplate-backtrace-limit=0")
-   SET(CMAKE_CXX_FLAGS "-Wall -Wextra -Wno-deprecated-register -Wno-deprecated-declarations -Wno-sign-compare -Wno-exceptions -Wno-missing-field-initializers -Wno-unused-parameter -Wno-unused-private-field -Qunused-arguments -fcolor-diagnostics -nostdinc++")
+
-   include_directories(/usr/local/opt/llvm/include/c++/v1)
+# clang-specific compile flags
-   link_directories(/usr/local/opt/llvm/lib)
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
-   SET(STD_CXX_FS c++fs)
+  set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -Wno-exceptions -fcolor-diagnostics")
-ELSEIF("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
+endif()
-   SET(CMAKE_CXX_FLAGS_DEBUG  "-O0 -g")
+
-   SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -ftree-vectorize -march=${CXX_MARCH}")
+
-   SET(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O3 -g -ftree-vectorize -march=${CXX_MARCH}")
+# Set platform / compiler specific compile flags and checks
-   SET(CMAKE_CXX_FLAGS "-Wall -Werror -Wextra -std=c++11 -Wno-misleading-indentation -Wno-sign-compare -Wno-maybe-uninitialized -Wno-int-in-bool-context -Wno-implicit-fallthrough -Wno-unused-parameter -Wno-deprecated-declarations -ftemplate-backtrace-limit=0 -fopenmp")
+if(APPLE)
-   SET(STD_CXX_FS stdc++fs)
+  # Need to investigate how to reliably detect and use OpenMP on macOS...
-ENDIF()
+  set(USE_OPENMP_DEFAULT Off)
+
+  # Among others, setting CMAKE_FIND_FRAMEWORK to LAST fixed issues
+  # with installed Mono that contains old headers (libpng, ...).
+  # See: https://github.com/openMVG/openMVG/issues/1349#issuecomment-401492811
+  set(CMAKE_FIND_FRAMEWORK LAST)
+
+  # use brewed llvm's libc++
+  add_compile_options("-nostdinc++")
+  include_directories("/usr/local/opt/llvm/include/c++/v1")
+  link_directories("/usr/local/opt/llvm/lib")
+  set(STD_CXX_FS c++fs)
+
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    message(STATUS "Detected macOS with non-Apple clang")
+
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+    message(STATUS "Detected macOS with Apple clang")
+    # Apple clang on macOS < 10.14 Mojave is too old
+    if(CMAKE_SYSTEM_VERSION VERSION_LESS 18.0.0)
+      message(WARNING "Detected Darwin version ${CMAKE_SYSTEM_VERSION}, which is earlier than macos 10.14 Mojave. Apple clang is too old and not supported. Use clang from homebrew.")
+    endif()
+
+  else()
+    message(WARNING "Detected macOS with unsupported compiler ${CMAKE_CXX_COMPILER_ID}")
+  endif()
+
+elseif(UNIX)
+  set(USE_OPENMP_DEFAULT On)
+
+  # assume libstdc++
+  set(STD_CXX_FS stdc++fs)
+
+  if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+    set(LINUX_CLANG 1)
+    message(STATUS "Detected Linux with clang.")
+    message(WARNING "Clang on Linux is currently not fully supported. You'll likely need to get a recent version of TBB.")
+
+  elseif(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+    message(STATUS "Detected Linux with gcc.")
+
+  else()
+    message(WARNING "Detected Linux with unsupported compiler ${CMAKE_CXX_COMPILER_ID}")
+  endif()
+
+else()
+  message(WARNING "Only Linux and macOS are currently supported")
+endif()
+
+
+# OpenMP option and compile flags
+option(USE_OPENMP "Use OpenMP (e.g. for parallel computation in Eigen)" ${USE_OPENMP_DEFAULT})
+if(USE_OPENMP)
+  message(STATUS "OpenMP Enabled")
+  set(BASALT_CXX_FLAGS "${BASALT_CXX_FLAGS} -fopenmp")
+else()
+  message(STATUS "OpenMP Disabled")
+endif()
+
+
+# setup combined compiler flags
+set(CMAKE_CXX_FLAGS "${BASALT_CXX_FLAGS} -march=${CXX_MARCH} ${BASALT_PASSED_CXX_FLAGS}")
+
 
 set(EIGEN_INCLUDE_DIR_HINTS ${EIGEN_ROOT})
 find_package(Eigen3 3.3.7 REQUIRED)
 include_directories(${EIGEN3_INCLUDE_DIR})
 message(STATUS "Found Eigen headers in: ${EIGEN3_INCLUDE_DIR}")
+if(NOT EIGEN3_INCLUDE_DIR MATCHES "^${EIGEN_ROOT}")
+  message(WARNING "Found Eigen headers are outside of specified EIGEN_ROOT '${EIGEN_ROOT}'")
+endif()
 
 find_package(TBB REQUIRED)
 include_directories(${TBB_INCLUDE_DIR})
@@ -103,26 +182,9 @@ include_directories(${OpenCV_INCLUDE_DIR})
 message(STATUS "Found OpenCV headers in: ${OpenCV_INCLUDE_DIR}")
 message(STATUS "Found OpenCV_LIBS: ${OpenCV_LIBS}")
 
-add_subdirectory(thirdparty/ros)
-add_subdirectory(thirdparty/apriltag)
-add_subdirectory(thirdparty/DBoW3)
 
+add_subdirectory(thirdparty)
 
-set(BUILD_SHARED_LIBS OFF CACHE BOOL "Enable BUILD_SHARED_LIBS")
-set(BUILD_TESTS OFF CACHE BOOL "Enable BUILD_TESTS")
-
-set(BUILD_PANGOLIN_LIBOPENEXR OFF CACHE BOOL "Enable BUILD_PANGOLIN_LIBOPENEXR")
-set(BUILD_PANGOLIN_PYTHON OFF CACHE BOOL "Enable BUILD_PANGOLIN_PYTHON")
-set(BUILD_EXAMPLES OFF CACHE BOOL "Enable BUILD_EXAMPLES")
-add_subdirectory(thirdparty/opengv EXCLUDE_FROM_ALL)
-
-# Hack to disable CPack in Pangolin.
-macro(include)
-  if(NOT ${ARGV0} STREQUAL "CPack")
-    _include(${ARGN})
-  endif()
-endmacro()
-add_subdirectory(thirdparty/Pangolin EXCLUDE_FROM_ALL)
 
 include_directories(thirdparty/basalt-headers/thirdparty/Sophus)
 include_directories(thirdparty/basalt-headers/thirdparty/cereal/include)
@@ -151,60 +213,55 @@ add_library(basalt SHARED
   src/utils/keypoints.cpp)
 
 
-target_link_libraries(basalt rosbag pangolin apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${OpenCV_LIBS} opengv ${STD_CXX_FS} DBoW3)
+target_link_libraries(basalt PUBLIC ${TBB_LIBRARIES} ${STD_CXX_FS} ${OpenCV_LIBS} pangolin PRIVATE rosbag apriltag opengv DBoW3)
 
 
 add_executable(basalt_calibrate src/calibrate.cpp)
-target_link_libraries(basalt_calibrate ${Pangolin_LIBRARIES} apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${rosbag_LIBRARIES} basalt)
+target_link_libraries(basalt_calibrate basalt)
 
 add_executable(basalt_calibrate_imu src/calibrate_imu.cpp)
-target_link_libraries(basalt_calibrate_imu ${Pangolin_LIBRARIES} apriltag ${OPENGV_LIBS} ${TBB_LIBRARIES} ${rosbag_LIBRARIES} basalt)
+target_link_libraries(basalt_calibrate_imu basalt)
 
 
 add_executable(basalt_vio_sim src/vio_sim.cpp)
-target_link_libraries(basalt_vio_sim ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} ${TBB_LIBRARIES} basalt)
+target_link_libraries(basalt_vio_sim basalt)
 
 add_executable(basalt_mapper_sim src/mapper_sim.cpp)
-target_link_libraries(basalt_mapper_sim ${Pangolin_LIBRARIES} opengv ${TBB_LIBRARIES} basalt)
+target_link_libraries(basalt_mapper_sim basalt)
 
 add_executable(basalt_mapper_sim_naive src/mapper_sim_naive.cpp)
-target_link_libraries(basalt_mapper_sim_naive ${Pangolin_LIBRARIES} opengv ${TBB_LIBRARIES} basalt)
+target_link_libraries(basalt_mapper_sim_naive basalt)
 
 add_executable(basalt_mapper src/mapper.cpp)
-target_link_libraries(basalt_mapper ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} ${TBB_LIBRARIES} basalt)
+target_link_libraries(basalt_mapper basalt)
 
 
 add_executable(basalt_opt_flow src/opt_flow.cpp)
-target_link_libraries(basalt_opt_flow ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} basalt)
+target_link_libraries(basalt_opt_flow basalt)
 
 add_executable(basalt_vio src/vio.cpp)
-target_link_libraries(basalt_vio ${Pangolin_LIBRARIES} opengv ${STD_CXX_FS} basalt)
+target_link_libraries(basalt_vio basalt)
-
 
 
 install(TARGETS basalt_calibrate basalt_calibrate_imu basalt_vio_sim basalt_mapper_sim basalt_mapper_sim_naive basalt_mapper basalt_opt_flow basalt_vio basalt
   EXPORT BasaltTargets
   RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
   LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
-    ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib
+  ARCHIVE DESTINATION ${CMAKE_INSTALL_PREFIX}/lib)
-)
 
 file(GLOB CONFIG_FILES "${CMAKE_CURRENT_SOURCE_DIR}/data/*.json")
-INSTALL(FILES ${CONFIG_FILES} ${CMAKE_CURRENT_SOURCE_DIR}/data/basalt-data/orbvoc.dbow3
+install(FILES ${CONFIG_FILES} ${CMAKE_CURRENT_SOURCE_DIR}/data/basalt-data/orbvoc.dbow3
-      DESTINATION ${CMAKE_INSTALL_PREFIX}/etc/basalt
+  DESTINATION ${CMAKE_INSTALL_PREFIX}/etc/basalt)
-)
 
 
 # Replace install() to do-nothing macro.
 macro(install)
 endmacro()
 # Include subproject (or any other CMake code) with "disabled" install().
+enable_testing()
 add_subdirectory(thirdparty/basalt-headers/test)
 add_subdirectory(test)
 # Restore original install() behavior.
 macro(install)
   _install(${ARGN})
 endmacro()
-
-
-

README.md

@@ -42,7 +42,7 @@ Update the Ubuntu package index and install Basalt:
 sudo apt-get update
 sudo apt-get install basalt
 ```
-### Source installation for Ubuntu 18.04 and MacOS 10.14 Mojave
+### Source installation for Ubuntu 18.04 and MacOS >= 10.11 El Capitan
 Clone the source code for the project and build it. For MacOS you should have [Homebrew](https://brew.sh/) installed.
 ```
 git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git

include/basalt/calibration/vignette.h

@@ -84,4 +84,4 @@ class VignetteEstimator {
   std::vector<double> irradiance;
   std::vector<basalt::RdSpline<1, SPLINE_N>> vign_param;
 };
-}
+}  // namespace basalt

include/basalt/io/dataset_io.h

@@ -179,8 +179,9 @@ template <class Archive, class _Scalar, int _Rows, int _Cols, int _Options,
 inline
     typename std::enable_if<_Rows == Eigen::Dynamic || _Cols == Eigen::Dynamic,
                             void>::type
-    load(Archive &ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows,
+    load(Archive &ar,
-                                    _MaxCols> &matrix) {
+         Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>
+             &matrix) {
   std::int32_t rows;
   std::int32_t cols;
   ar(rows);

include/basalt/optimization/spline_linearize.h

@@ -135,9 +135,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       // std::cout << "time " << time << std::endl;
       // std::cout << "sline.minTime() " << spline.minTime() << std::endl;
 
-      BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "time " << time_ns << " spline.minTimeNs() "
+          time_ns >= spline->minTimeNs(),
-                                   << spline->minTimeNs());
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
 
       // Residual from current value of spline
       Vector3 residual_pos =
@@ -207,12 +207,12 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       //      std::cout << "time " << t << std::endl;
       //      std::cout << "sline.minTime() " << spline.minTime() << std::endl;
 
-      BASALT_ASSERT_STREAM(t >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "t " << t << " spline.minTime() "
+          t >= spline->minTimeNs(),
-                                << spline->minTimeNs());
+          "t " << t << " spline.minTime() " << spline->minTimeNs());
-      BASALT_ASSERT_STREAM(t <= spline->maxTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "t " << t << " spline.maxTime() "
+          t <= spline->maxTimeNs(),
-                                << spline->maxTimeNs());
+          "t " << t << " spline.maxTime() " << spline->maxTimeNs());
 
       Vector3 residual = spline->accelResidual(
           t, pm.data, this->common_data.calibration->calib_accel_bias,
@@ -255,7 +255,8 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
           BASALT_ASSERT(start_j < opt_size);
 
           accum.template addH<POSE_SIZE, POSE_SIZE>(
-              start_i, start_j, accel_var_inv * J.d_val_d_knot[i].transpose() *
+              start_i, start_j,
+              accel_var_inv * J.d_val_d_knot[i].transpose() *
                   J.d_val_d_knot[j]);
         }
         accum.template addH<ACCEL_BIAS_SIZE, POSE_SIZE>(
@@ -497,9 +498,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
       if (time_ns < spline->minTimeNs() || time_ns >= spline->maxTimeNs())
         continue;
 
-      BASALT_ASSERT_STREAM(time_ns >= spline->minTimeNs(),
+      BASALT_ASSERT_STREAM(
-                           "time " << time_ns << " spline.minTimeNs() "
+          time_ns >= spline->minTimeNs(),
-                                   << spline->minTimeNs());
+          "time " << time_ns << " spline.minTimeNs() " << spline->minTimeNs());
 
       const SE3 T_moc_w = this->common_data.mocap_calibration->T_moc_w;
       const SE3 T_i_mark = this->common_data.mocap_calibration->T_i_mark;
@@ -563,9 +564,9 @@ struct LinearizeSplineOpt : public LinearizeBase<Scalar> {
                          d_res_d_T_w_i.transpose() * residual);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            start_T_moc_w, start_i, mocap_var_inv *
+            start_T_moc_w, start_i,
-                                        d_res_d_T_moc_w.transpose() *
+            mocap_var_inv * d_res_d_T_moc_w.transpose() * d_res_d_T_w_i *
-                                        d_res_d_T_w_i * J_pose.d_val_d_knot[i]);
+                J_pose.d_val_d_knot[i]);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
             start_T_i_mark, start_i,

include/basalt/utils/common_types.h

@@ -238,7 +238,7 @@ using ImageProjections = std::map<TimeCamId, ImageProjection>;
 /// inlier projections indexed per track
 using TrackProjections =
     std::unordered_map<TrackId, std::map<TimeCamId, ProjectedLandmarkConstPtr>>;
-}
+}  // namespace basalt
 
 namespace cereal {
 
@@ -251,4 +251,4 @@ template <class Archive>
 void serialize(Archive& ar, basalt::MatchData& c) {
   ar(c.T_i_j, c.matches, c.inliers);
 }
-}
+}  // namespace cereal

include/basalt/utils/nfr.h

@@ -117,4 +117,4 @@ inline Sophus::Vector2d rollPitchError(
 
   return res.head<2>();
 }
-}
+}  // namespace basalt

include/basalt/utils/sim_utils.h

@@ -43,13 +43,11 @@ struct SimObservations {
   std::vector<int> id;
 };
 
-}
+}  // namespace basalt
 
 namespace cereal {
 template <class Archive>
 void serialize(Archive& ar, basalt::SimObservations& c) {
   ar(c.pos, c.id);
 }
-}
+}  // namespace cereal
-
-

include/basalt/utils/tracks.h

@@ -47,11 +47,8 @@
 #include <basalt/utils/common_types.h>
 #include <basalt/utils/union_find.h>
 
-
 namespace basalt {
 
-
-
 /// TrackBuild class creates feature tracks from matches
 struct TrackBuilder {
   std::map<ImageFeaturePair, TrackId> map_node_to_index;
@@ -222,4 +219,4 @@ bool GetSharedTracks(const TimeCamId& image_id, const FeatureTracks& all_tracks,
   return !track_ids.empty();
 }
 
-}
+}  // namespace basalt

include/basalt/vi_estimator/ba_base.h

@@ -174,6 +174,7 @@ class BundleAdjustmentBase {
 
     if (d_res_d_p) {
       Eigen::Matrix<double, 4, 3> Jpp;
+      Jpp.setZero();
       Jpp.block<3, 2>(0, 0) = T_t_h.topLeftCorner<3, 4>() * Jup;
       Jpp.col(2) = T_t_h.col(3);
 
@@ -215,6 +216,7 @@ class BundleAdjustmentBase {
 
     if (d_res_d_p) {
       Eigen::Matrix<double, 4, 3> Jpp;
+      Jpp.setZero();
       Jpp.block<4, 2>(0, 0) = Jup;
       Jpp.col(2).setZero();
 
@@ -284,27 +286,31 @@ class BundleAdjustmentBase {
           continue;
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_h_idx, abs_h_idx, rld.d_rel_d_h[i].transpose() *
+            abs_h_idx, abs_h_idx,
-                                      rel_H.block<POSE_SIZE, POSE_SIZE>(
+            rld.d_rel_d_h[i].transpose() *
-                                          POSE_SIZE * i, POSE_SIZE * j) *
+                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                  POSE_SIZE * j) *
                 rld.d_rel_d_h[j]);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_ti_idx, abs_h_idx, rld.d_rel_d_t[i].transpose() *
+            abs_ti_idx, abs_h_idx,
-                                       rel_H.block<POSE_SIZE, POSE_SIZE>(
+            rld.d_rel_d_t[i].transpose() *
-                                           POSE_SIZE * i, POSE_SIZE * j) *
+                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                  POSE_SIZE * j) *
                 rld.d_rel_d_h[j]);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_h_idx, abs_tj_idx, rld.d_rel_d_h[i].transpose() *
+            abs_h_idx, abs_tj_idx,
-                                       rel_H.block<POSE_SIZE, POSE_SIZE>(
+            rld.d_rel_d_h[i].transpose() *
-                                           POSE_SIZE * i, POSE_SIZE * j) *
+                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                  POSE_SIZE * j) *
                 rld.d_rel_d_t[j]);
 
         accum.template addH<POSE_SIZE, POSE_SIZE>(
-            abs_ti_idx, abs_tj_idx, rld.d_rel_d_t[i].transpose() *
+            abs_ti_idx, abs_tj_idx,
-                                        rel_H.block<POSE_SIZE, POSE_SIZE>(
+            rld.d_rel_d_t[i].transpose() *
-                                            POSE_SIZE * i, POSE_SIZE * j) *
+                rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
+                                                  POSE_SIZE * j) *
                 rld.d_rel_d_t[j]);
       }
     }
@@ -369,4 +375,4 @@ class BundleAdjustmentBase {
 
   basalt::Calibration<double> calib;
 };
-}
+}  // namespace basalt

scripts/clang-format-all.sh

@@ -0,0 +1,43 @@
+#!/usr/bin/env bash
+
+# Format all source files in the project.
+# Optionally take folder as argument; default is full inlude and src dirs.
+
+set -e
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )"
+
+FOLDER="${1:-$SCRIPT_DIR/../include $SCRIPT_DIR/../src $SCRIPT_DIR/../test/src}"
+
+CLANG_FORMAT_COMMANDS="clang-format-9 clang-format-8 clang-format"
+
+# find the first available command:
+for CMD in $CLANG_FORMAT_COMMANDS; do
+    if hash $CMD 2>/dev/null; then
+        CLANG_FORMAT_CMD=$CMD
+        break
+    fi
+done
+
+if [ -z $CLANG_FORMAT_CMD ]; then
+    echo "clang-format not installed..."
+    exit 1
+fi
+    
+# clang format check version
+MAJOR_VERSION_NEEDED=8
+
+MAJOR_VERSION_DETECTED=`$CLANG_FORMAT_CMD -version | sed -n -E 's/.*version ([0-9]+).*/\1/p'`
+if [ -z $MAJOR_VERSION_DETECTED ]; then
+    echo "Failed to parse major version (`$CLANG_FORMAT_CMD -version`)"
+    exit 1
+fi
+
+echo "clang-format version $MAJOR_VERSION_DETECTED (`$CLANG_FORMAT_CMD -version`)"
+
+if [ $MAJOR_VERSION_DETECTED -lt $MAJOR_VERSION_NEEDED ]; then
+    echo "Looks like your clang format is too old; need at least version $MAJOR_VERSION_NEEDED"
+    exit 1
+fi
+
+find $FOLDER -iname "*.?pp" -or -iname "*.h" | xargs $CLANG_FORMAT_CMD -verbose -i

scripts/install_mac_os_deps.sh

@@ -1,4 +1,15 @@
 #!/bin/sh
 
-brew install clang-format tbb glew eigen ccache 
+brew install \
-brew install --with-toolchain llvm
+     boost \
+     opencv \
+     cmake \
+     pkgconfig \
+     lz4 \
+     clang-format \
+     tbb \
+     glew \
+     eigen \
+     ccache
+
+brew install llvm

src/calibrate.cpp

@@ -33,7 +33,6 @@ 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 <basalt/calibration/cam_calib.h>
 
 #include <tbb/tbb.h>

src/calibration/vignette.cpp

@@ -33,7 +33,6 @@ 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 <basalt/calibration/vignette.h>
 
 namespace basalt {

src/io/dataset_io.cpp

@@ -33,7 +33,6 @@ 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 <basalt/io/dataset_io.h>
 #include <basalt/io/dataset_io_euroc.h>
 #include <basalt/io/dataset_io_rosbag.h>

src/mapper_sim.cpp

@@ -33,7 +33,6 @@ 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 <algorithm>
 #include <chrono>
 #include <iostream>
@@ -120,7 +119,7 @@ double alignButton();
 void setup_points();
 
 constexpr int UI_WIDTH = 200;
-constexpr int NUM_FRAMES = 500;
+// constexpr int NUM_FRAMES = 500;
 
 basalt::Calibration<double> calib;
 

src/mapper_sim_naive.cpp

@@ -33,7 +33,6 @@ 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 <algorithm>
 #include <chrono>
 #include <iostream>
@@ -492,8 +491,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path

src/opt_flow.cpp

@@ -33,7 +33,6 @@ 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 <algorithm>
 #include <chrono>
 #include <iostream>
@@ -321,8 +320,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path

src/optical_flow/optical_flow.cpp

@@ -33,7 +33,6 @@ 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 <basalt/optical_flow/optical_flow.h>
 
 #include <basalt/optical_flow/frame_to_frame_optical_flow.h>

src/vi_estimator/ba_base.cpp

@@ -33,8 +33,6 @@ 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 <basalt/vi_estimator/ba_base.h>
 
 #include <tbb/parallel_for.h>
@@ -354,7 +352,6 @@ void BundleAdjustmentBase::linearizeHelper(
             }
           }
         }
-
       });
 
   for (const auto& rld : rld_vec) error += rld.error;
@@ -522,4 +519,4 @@ void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
   abs_H.resize(0, 0);
   abs_b.resize(0);
 }
-}
+}  // namespace basalt

src/vio_sim.cpp

@@ -33,7 +33,6 @@ 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 <algorithm>
 #include <chrono>
 #include <iostream>
@@ -541,8 +540,8 @@ void load_data(const std::string& calib_path) {
   if (os.is_open()) {
     cereal::JSONInputArchive archive(os);
     archive(calib);
-    std::cout << "Loaded camera with " << calib.intrinsics.size()
+    std::cout << "Loaded camera with " << calib.intrinsics.size() << " cameras"
-              << " cameras" << std::endl;
+              << std::endl;
 
   } else {
     std::cerr << "could not load camera calibration " << calib_path
@@ -595,8 +594,8 @@ void compute_projections() {
 void gen_data() {
   for (size_t i = 0; i < calib.intrinsics.size(); i++) {
     images.emplace_back();
-    images.back() = pangolin::TypedImage(
+    images.back() =
-        calib.resolution[i][0], calib.resolution[i][1],
+        pangolin::TypedImage(calib.resolution[i][0], calib.resolution[i][1],
                              pangolin::PixelFormatFromString("GRAY8"));
 
     images.back().Fill(200);

test/CMakeLists.txt

@@ -1,31 +1,28 @@
-cmake_minimum_required(VERSION 3.2)
+cmake_minimum_required(VERSION 3.10)
-
-find_package(TBB REQUIRED)
-include_directories(${TBB_INCLUDE_DIR})
-
-set(GTEST_MAIN_LIBRARY gtest_main)
-set(GTEST_LIBRARY gtest)
 
+# Note: add_subdirectory(googletest ...) is called in basalt-headers
 
 include_directories(../thirdparty/basalt-headers/test/include)
 
 
 add_executable(test_image src/test_image.cpp)
-target_link_libraries(test_image ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${OpenCV_LIBS} basalt)
+target_link_libraries(test_image gtest gtest_main basalt)
 
 add_executable(test_spline_opt src/test_spline_opt.cpp)
-target_link_libraries(test_spline_opt ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt)
+target_link_libraries(test_spline_opt gtest gtest_main basalt)
 
 add_executable(test_vio src/test_vio.cpp)
-target_link_libraries(test_vio ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt)
+target_link_libraries(test_vio gtest gtest_main basalt)
 
 add_executable(test_nfr src/test_nfr.cpp)
-target_link_libraries(test_nfr ${GTEST_LIBRARY} ${GTEST_MAIN_LIBRARY} ${TBB_LIBRARIES} opengv basalt)
+target_link_libraries(test_nfr gtest gtest_main basalt)
 
 
 enable_testing()
 
-add_test(test_image test_image COMMAND Test)
+include(GoogleTest)
-add_test(test_spline_opt test_spline_opt COMMAND Test)
+
-add_test(test_vio test_vio COMMAND Test)
+gtest_discover_tests(test_image DISCOVERY_TIMEOUT 60)
-add_test(test_nfr test_nfr COMMAND Test)
+gtest_discover_tests(test_spline_opt DISCOVERY_TIMEOUT 60)
+gtest_discover_tests(test_vio DISCOVERY_TIMEOUT 60)
+gtest_discover_tests(test_nfr DISCOVERY_TIMEOUT 60)

test/src/test_image.cpp

@@ -27,7 +27,8 @@ TEST(Pattern, ImageInterp) {
 
   // std::cerr << "vg\n" << vg << std::endl;
 
-  test_jacobian("d_val_d_p", J,
+  test_jacobian(
+      "d_val_d_p", J,
       [&](const Eigen::Vector2d &x) {
         Eigen::Matrix<double, 1, 1> res;
         Eigen::Vector2d p1 = offset + x;

test/src/test_nfr.cpp

@@ -34,13 +34,13 @@ TEST(PreIntegrationTestSuite, RelPoseTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j);
-
         },
         x0);
   }
@@ -48,13 +48,13 @@ TEST(PreIntegrationTestSuite, RelPoseTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_j", d_res_d_T_w_j,
+    test_jacobian(
+        "d_res_d_T_w_j", d_res_d_T_w_j,
         [&](const Sophus::Vector6d& x) {
           auto T_w_j_new = T_w_j;
           basalt::PoseState::incPose(x, T_w_j_new);
 
           return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new);
-
         },
         x0);
   }
@@ -71,13 +71,13 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::absPositionError(T_w_i_new, pos);
-
         },
         x0);
   }
@@ -97,7 +97,8 @@ TEST(PreIntegrationTestSuite, YawTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
@@ -105,7 +106,6 @@ TEST(PreIntegrationTestSuite, YawTest) {
           double res = basalt::yawError(T_w_i_new, yaw_dir_body);
 
           return Eigen::Matrix<double, 1, 1>(res);
-
         },
         x0);
   }
@@ -124,13 +124,13 @@ TEST(PreIntegrationTestSuite, RollPitchTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_T_w_i", d_res_d_T_w_i,
+    test_jacobian(
+        "d_res_d_T_w_i", d_res_d_T_w_i,
         [&](const Sophus::Vector6d& x) {
           auto T_w_i_new = T_w_i;
           basalt::PoseState::incPose(x, T_w_i_new);
 
           return basalt::rollPitchError(T_w_i_new, R_w_i);
-
         },
         x0);
   }

test/src/test_vio.cpp

@@ -372,14 +372,14 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
   {
     Sophus::Vector6d x0;
     x0.setZero();
-    test_jacobian("d_res_d_xi", d_res_d_xi,
+    test_jacobian(
+        "d_res_d_xi", d_res_d_xi,
         [&](const Sophus::Vector6d& x) {
-                    Eigen::Matrix4d T_t_h_new =
+          Eigen::Matrix4d T_t_h_new = (Sophus::expd(x) * T_t_h_sophus).matrix();
-                        (Sophus::expd(x) * T_t_h_sophus).matrix();
 
           Eigen::Vector2d res;
-                    basalt::KeypointVioEstimator::linearizePoint(
+          basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos,
-                        kpt_obs, kpt_pos, T_t_h_new, cam, res);
+                                                       T_t_h_new, cam, res);
 
           return res;
         },
@@ -389,17 +389,17 @@ TEST(PreIntegrationTestSuite, LinearizePointsTest) {
   {
     Eigen::Vector3d x0;
     x0.setZero();
-    test_jacobian("d_res_d_p", d_res_d_p,
+    test_jacobian(
+        "d_res_d_p", d_res_d_p,
         [&](const Eigen::Vector3d& x) {
-                    basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new =
+          basalt::KeypointVioEstimator::KeypointPosition kpt_pos_new = kpt_pos;
-                        kpt_pos;
 
           kpt_pos_new.dir += x.head<2>();
           kpt_pos_new.id += x[2];
 
           Eigen::Vector2d res;
-                    basalt::KeypointVioEstimator::linearizePoint(
+          basalt::KeypointVioEstimator::linearizePoint(kpt_obs, kpt_pos_new,
-                        kpt_obs, kpt_pos_new, T_t_h, cam, res);
+                                                       T_t_h, cam, res);
 
           return res;
         },

thirdparty/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.10)
+
+add_subdirectory(ros EXCLUDE_FROM_ALL)
+add_subdirectory(apriltag EXCLUDE_FROM_ALL)
+add_subdirectory(DBoW3 EXCLUDE_FROM_ALL)
+
+
+set(BUILD_SHARED_LIBS OFF CACHE BOOL "Enable BUILD_SHARED_LIBS")
+set(BUILD_TESTS OFF CACHE BOOL "Enable BUILD_TESTS")
+set(BUILD_TOOLS OFF CACHE BOOL "Enable BUILD_TOOLS")
+set(CMAKE_EXPORT_NO_PACKAGE_REGISTRY ON)
+
+set(BUILD_PANGOLIN_LIBOPENEXR OFF CACHE BOOL "Enable BUILD_PANGOLIN_LIBOPENEXR")
+set(BUILD_PANGOLIN_PYTHON OFF CACHE BOOL "Enable BUILD_PANGOLIN_PYTHON")
+set(BUILD_EXAMPLES OFF CACHE BOOL "Enable BUILD_EXAMPLES")
+
+set(EIGEN_INCLUDE_DIR "${EIGEN3_INCLUDE_DIR}")
+set(EIGEN_INCLUDE_DIRS "${EIGEN3_INCLUDE_DIR}")
+
+add_subdirectory(opengv EXCLUDE_FROM_ALL)
+
+# Hack to disable CPack in Pangolin.
+macro(include)
+  if(NOT ${ARGV0} STREQUAL "CPack")
+    _include(${ARGN})
+  endif()
+endmacro()
+add_subdirectory(Pangolin EXCLUDE_FROM_ALL)
+
+
+# fix aprilgrid
+target_compile_options(apriltag PRIVATE "-Wno-unused-private-field")
+
+# fix opengv: c++17 and debug postfix
+set_target_properties(opengv PROPERTIES
+  CXX_STANDARD 17
+  DEBUG_POSTFIX "")
+
+# fix opengv: compile options (CMAKE_CXX_FLAGS is overwritten by Opengv)
+target_compile_options(opengv PRIVATE
+  -Wno-unused-private-field
+  -march=${CXX_MARCH}
+  ${BASALT_PASSED_CXX_FLAGS})
+
+# TODO: enable once mpark issue is fixed upstream pangolin
+#set_target_properties(pangolin PROPERTIES
+#  CXX_STANDARD 17)
+
+# fix pangolin: gcc
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  target_compile_options(pangolin PRIVATE "-Wno-implicit-fallthrough")
+endif()
+
+# fix pangolin: macOS
+if(APPLE)
+  target_compile_options(pangolin PRIVATE "-Wno-objc-missing-super-calls")
+endif()
+
+# fix pangolin: macOS >= 10.14 Mojave
+if(APPLE AND CMAKE_SYSTEM_VERSION VERSION_GREATER_EQUAL 18.0.0)
+  target_compile_options(pangolin PRIVATE "-Wno-deprecated-declarations")
+endif()
+
+# fix pangolin: clang
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
+  target_compile_options(pangolin PRIVATE "-Wno-null-pointer-arithmetic")
+endif()
+
+# fix pangolin: clang >= 8.0
+if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 8.0.0)
+  target_compile_options(pangolin PRIVATE "-Wno-defaulted-function-deleted")
+endif()
+
+# check here the directories for the pangolin and opengv targets, and
+# confirm that the eigen-related include dirs match.
+foreach(_target opengv pangolin)
+  get_target_property(_include_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
+  foreach(_dir IN LISTS _include_dirs)
+    if("${_dir}" MATCHES ".*/eigen3?(/unsupported)?$")
+      if(NOT _dir MATCHES "^${EIGEN3_INCLUDE_DIR}.*")
+      endif()
+    endif()
+  endforeach()
+endforeach()
+
+# opengv and pangolin assume that eigen is found outside the source
+# directory and thus include it in INTERFACE_INCLUDE_DIRECTORIES,
+# which makes cmake complain that that property contains paths in the
+# source folder. Thus, we update the property to only include these
+# eigen paths in the "BUILD_INTERFACE" (not "INSTALL").
+if (EIGEN3_INCLUDE_DIR MATCHES "^${CMAKE_SOURCE_DIR}.*")
+  foreach(_target opengv pangolin)
+    get_target_property(_include_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
+    set(_include_dirs_new "")
+    foreach(_dir IN LISTS _include_dirs)
+      if(_dir MATCHES ".*/eigen(/unsupported)?$")
+        string(REGEX REPLACE "(^${CMAKE_SOURCE_DIR}.*$)" "$<BUILD_INTERFACE:\\1>" _dir "${_dir}")
+      endif()
+      list(APPEND _include_dirs_new "${_dir}")
+    endforeach()
+    set_target_properties(${_target} PROPERTIES INTERFACE_INCLUDE_DIRECTORIES "${_include_dirs_new}")
+  endforeach()
+endif()
+
+
+

thirdparty/basalt-headers

@@ -1 +1 @@
-Subproject commit 4a771353447a0dd4e47a0041ada6385363a34f3b
+Subproject commit 292ee346506b73f691b96108cafc53d589e8b7b3