First commit of the code

CMakeLists.txt

@@ -0,0 +1,147 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(msckf_vio)
+
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+
+# Modify cmake module path if new .cmake files are required
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  tf
+  nav_msgs
+  sensor_msgs
+  geometry_msgs
+  eigen_conversions
+  tf_conversions
+  random_numbers
+  message_generation
+  image_transport
+  cv_bridge
+  message_filters
+  pcl_conversions
+  pcl_ros
+)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(Cholmod REQUIRED)
+find_package(SPQR REQUIRED)
+
+add_message_files(
+  FILES
+
+  FeatureMeasurement.msg
+  CameraMeasurement.msg
+  TrackingInfo.msg
+
+  #DebugImuState.msg
+  #DebugCamState.msg
+  #DebugState.msg
+  #DebugMsckfInfo.msg
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+#  LIBRARIES msckf_vio
+  CATKIN_DEPENDS
+    roscpp std_msgs tf nav_msgs sensor_msgs geometry_msgs
+    eigen_conversions tf_conversions message_runtime
+    image_transport cv_bridge message_filters pcl_conversions
+    pcl_ros std_srvs
+  DEPENDS
+    Boost EIGEN3 OpenCV
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+  ${Boost_INCLUDE_DIR}
+  ${OpenCV_INCLUDE_DIRS}
+  ${CHOLMOD_INCLUDES}
+  ${SPQR_INCLUDES}
+)
+
+link_directories(
+  ${catkin_LIBRARY_DIRS}
+)
+
+# Msckf Vio
+add_library(msckf_vio
+  src/msckf_vio.cpp
+)
+add_dependencies(msckf_vio
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(msckf_vio
+  ${catkin_LIBRARIES}
+  ${CHOLMOD_LIBRARIES}
+  ${SPQR_LIBRARIES}
+)
+
+# Msckf Vio nodelet
+add_library(msckf_vio_nodelet
+  src/msckf_vio_nodelet.cpp
+)
+add_dependencies(msckf_vio_nodelet
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(msckf_vio_nodelet
+  msckf_vio
+  ${catkin_LIBRARIES}
+)
+
+# Image processor
+add_library(image_processor
+  src/image_processor.cpp
+)
+add_dependencies(image_processor
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(image_processor
+  ${catkin_LIBRARIES}
+  ${OpenCV_LIBRARIES}
+)
+
+# Image processor nodelet
+add_library(image_processor_nodelet
+  src/image_processor_nodelet.cpp
+)
+add_dependencies(image_processor_nodelet
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(image_processor_nodelet
+  image_processor
+  ${catkin_LIBRARIES}
+)
+
+# Feature initialization test
+catkin_add_gtest(test_feature_init
+  test/feature_initialization_test.cpp
+)
+add_dependencies(test_feature_init
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+)
+target_link_libraries(test_feature_init
+  ${catkin_LIBRARIES}
+)
+
+# Math utils test
+catkin_add_gtest(test_math_utils
+  test/math_utils_test.cpp
+)

README.md

@@ -5,7 +5,7 @@ The `MSCKF_VIO` package is a stereo version of MSCKF. The software takes in sync
 
 The software is tested on Ubuntu 16.04 with ROS Kinetic.
 
-Video: [https://www.youtube.com/watch?v=OXSB8Bze0cY](https://www.youtube.com/watch?v=OXSB8Bze0cY)
+Video: [https://www.youtube.com/watch?v=OXSB8Bze0cY](https://www.youtube.com/watch?v=OXSB8Bze0cY)  
 Paper Draft: [https://arxiv.org/abs/1712.00036](https://arxiv.org/abs/1712.00036)
 
 ## License

cmake/FindCholmod.cmake

@@ -0,0 +1,89 @@
+# Cholmod lib usually requires linking to a blas and lapack library.
+# It is up to the user of this module to find a BLAS and link to it.
+
+if (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES)
+  set(CHOLMOD_FIND_QUIETLY TRUE)
+endif (CHOLMOD_INCLUDES AND CHOLMOD_LIBRARIES)
+
+find_path(CHOLMOD_INCLUDES
+  NAMES
+  cholmod.h
+  PATHS
+  $ENV{CHOLMODDIR}
+  ${INCLUDE_INSTALL_DIR}
+  PATH_SUFFIXES
+  suitesparse
+  ufsparse
+)
+
+find_library(CHOLMOD_LIBRARIES cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+
+if(CHOLMOD_LIBRARIES)
+
+  get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
+
+  find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (AMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (COLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CCOLAMD_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
+  else ()
+    set(CHOLMOD_LIBRARIES FALSE)
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (CHOLMOD_METIS_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
+  endif ()
+
+endif(CHOLMOD_LIBRARIES)
+
+if(CHOLMOD_LIBRARIES)
+
+  find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
+  if (SUITESPARSE_LIBRARY)
+    set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${SUITESPARSE_LIBRARY})
+  endif (SUITESPARSE_LIBRARY)
+
+endif(CHOLMOD_LIBRARIES)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
+                                  CHOLMOD_INCLUDES CHOLMOD_LIBRARIES)
+
+mark_as_advanced(CHOLMOD_INCLUDES CHOLMOD_LIBRARIES AMD_LIBRARY COLAMD_LIBRARY SUITESPARSE_LIBRARY)

cmake/FindSPQR.cmake

@@ -0,0 +1,36 @@
+# SPQR lib usually requires linking to a blas and lapack library.
+# It is up to the user of this module to find a BLAS and link to it.
+
+# SPQR lib requires Cholmod, colamd and amd as well. 
+# FindCholmod.cmake can be used to find those packages before finding spqr
+
+if (SPQR_INCLUDES AND SPQR_LIBRARIES)
+  set(SPQR_FIND_QUIETLY TRUE)
+endif (SPQR_INCLUDES AND SPQR_LIBRARIES)
+
+find_path(SPQR_INCLUDES
+  NAMES
+  SuiteSparseQR.hpp
+  PATHS
+  $ENV{SPQRDIR}
+  ${INCLUDE_INSTALL_DIR}
+  PATH_SUFFIXES
+  suitesparse
+  ufsparse
+)
+
+find_library(SPQR_LIBRARIES spqr $ENV{SPQRDIR} ${LIB_INSTALL_DIR})
+
+if(SPQR_LIBRARIES)
+
+  find_library(SUITESPARSE_LIBRARY SuiteSparse PATHS $ENV{SPQRDIR} ${LIB_INSTALL_DIR})
+  if (SUITESPARSE_LIBRARY)
+    set(SPQR_LIBRARIES ${SPQR_LIBRARIES} ${SUITESPARSE_LIBRARY})
+  endif (SUITESPARSE_LIBRARY)
+
+endif(SPQR_LIBRARIES)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(SPQR DEFAULT_MSG SPQR_INCLUDES SPQR_LIBRARIES)
+
+mark_as_advanced(SPQR_INCLUDES SPQR_LIBRARIES)

config/camchain-imucam-euroc.yaml

@@ -0,0 +1,33 @@
+# The modifications of the output file from Kalibr:
+# 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix.
+# 2. Add the T_imu_body at the end of the calibration file (usually set to identity).
+cam0:
+  T_cam_imu:
+    [0.014865542981794,   0.999557249008346,  -0.025774436697440,   0.065222909535531,
+    -0.999880929698575,   0.014967213324719,   0.003756188357967,  -0.020706385492719,
+     0.004140296794224,   0.025715529947966,   0.999660727177902,  -0.008054602460030,
+                     0,                   0,                   0,   1.000000000000000]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
+  distortion_model: radtan
+  intrinsics: [458.654, 457.296, 367.215, 248.375]
+  resolution: [752, 480]
+  timeshift_cam_imu: 0.0
+cam1:
+  T_cam_imu:
+    [0.012555267089103,   0.999598781151433,  -0.025389800891747,  -0.044901980682509,
+    -0.999755099723116,   0.013011905181504,   0.017900583825251,  -0.020569771258915,
+     0.018223771455443,   0.025158836311552,   0.999517347077547,  -0.008638135126028,
+                     0,                   0,                   0,   1.000000000000000]
+  camera_model: pinhole
+  distortion_coeffs: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05]
+  distortion_model: radtan
+  intrinsics: [457.587, 456.134, 379.999, 255.238]
+  resolution: [752, 480]
+  timeshift_cam_imu: 0.0
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]
+

config/camchain-imucam-fla.yaml

@@ -0,0 +1,37 @@
+# The modifications of the output file from Kalibr:
+# 1. For each matrix (e.g. cam0/T_cam_imu), remove the brackets and minus sign for each line. Use one pair of brackets for each matrix.
+# 2. Add the T_imu_body at the end of the calibration file (usually set to identity).
+cam0:
+  T_cam_imu:
+    [0.9999354187625251, -0.008299416281612066, 0.007763890364879687, 0.1058254769257812,
+    -0.008323202950480819, -0.9999607512746784, 0.003036478688450292, -0.017679034754829417,
+    0.0077383846414136375, -0.0031009030240912025, -0.9999652502980172, -0.008890355463642424,
+    0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.0147, -0.00584, 0.00715, -0.00464]
+  distortion_model: equidistant
+  intrinsics: [606.57934, 606.73233, 474.93081, 402.27722]
+  resolution: [960, 800]
+  rostopic: /sync/cam0/image_raw
+cam1:
+  T_cam_imu:
+    [0.9999415499962613, -0.003314180239269491, 0.010291394483555276, -0.09412457355264209,
+    -0.0029741561536762054, -0.9994548489877669, -0.032880985843089204, -0.016816910884051604,
+    0.010394757632964142, 0.03284845573511056, -0.9994062877376598, -0.008908185988981819,
+    0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    [0.9999843700393054, -0.004977416649937181, -0.0025428275521419763, -0.200059,
+    0.005065643350062822, 0.9993405242433947, 0.03595604029590122, 0.000634053,
+    0.002362182447858024, -0.03596835970409878, 0.9993501279159637, -0.000909473,
+    0.0, 0.0, 0.0, 1.0]
+  camera_model: pinhole
+  distortion_coeffs: [-0.01565, -0.00026, -0.00454, 0.00311]
+  distortion_model: equidistant
+  intrinsics: [605.7278, 605.75661, 481.98865, 417.83476]
+  resolution: [960, 800]
+  rostopic: /sync/cam1/image_raw
+T_imu_body:
+  [1.0000, 0.0000, 0.0000, 0.0000,
+  0.0000, 1.0000, 0.0000, 0.0000,
+  0.0000, 0.0000, 1.0000, 0.0000,
+  0.0000, 0.0000, 0.0000, 1.0000]