Update build system

.gitignore

@@ -33,3 +33,6 @@ common
 % Ignore objects
 *.dbg.so
 *.opt.so
+
+.idea
+cmake-build*

CMakeLists.txt

@@ -1,16 +1,17 @@
-cmake_minimum_required (VERSION 3.5)
+cmake_minimum_required (VERSION 3.16)
 project(kimera_vio VERSION 1.0 LANGUAGES CXX)
 
 # ILLIXR requires C++17
 set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)# Make sure that custom modules like FindXXXX are found
 
-# Make sure that custom modules like FindXXXX are found
 # Prefer to use system installation of gflags/glog
 set(GFLAGS_PREFER_EXPORTED_GFLAGS_CMAKE_CONFIGURATION TRUE)
 set(GLOG_PREFER_EXPORTED_GLOG_CMAKE_CONFIGURATION TRUE)
 list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake)
+list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_INSTALL_PREFIX}/lib/cmake)
 
+link_directories(BEFORE ${CMAKE_INSTALL_DIR}/lib)
 IF(APPLE)
     # Fix linking on 10.14+. See https://stackoverflow.com/questions/54068035
     LINK_DIRECTORIES(/usr/local/lib)
@@ -33,18 +34,30 @@ if(NOT TARGET Boost::boost)
   INTERFACE_INCLUDE_DIRECTORIES "${Boost_INCLUDE_DIRS}")
 endif()
 
+find_package(Eigen3 3.3 REQUIRED NO_MODULE)
 find_package(Gflags REQUIRED)
 find_package(Glog 0.3.5 REQUIRED)
 find_package(GTSAM REQUIRED)
+if(${GTSAM_VERSION_MAJOR} EQUAL 4)
+  add_definitions(-DUSING_GTSAM4)
+endif()
 find_package(opengv REQUIRED)
-find_package(OpenCV REQUIRED EXACT 3.4.6)
-find_package(DBoW2 REQUIRED)
-if(NOT TARGET DBoW2::DBoW2)
-  add_library(DBoW2::DBoW2 INTERFACE IMPORTED)
-  set_target_properties(DBoW2::DBoW2 PROPERTIES
-  INTERFACE_LINK_LIBRARIES "${DBoW2_LIBRARIES}"
-  INTERFACE_INCLUDE_DIRECTORIES "${DBoW2_INCLUDE_DIRS}")
+if(DEFINED INTERNAL_OPENCV)
+  find_package(OpenCV REQUIRED PATHS ${INTERNAL_OPENCV} NO_DEFAULT_PATH NO_CMAKE_ENVIRONMENT_PATH NO_SYSTEM_ENVIRONMENT_PATH NO_CMAKE_PACKAGE_REGISTRY NO_CMAKE_SYSTEM_PATH NO_CMAKE_SYSTEM_PACKAGE_REGISTRY)
+else()
+  find_package(OpenCV REQUIRED)
+endif()
+if(${OpenCV_VERSION_MAJOR} EQUAL 4)
+  add_definitions(-DUSING_OPENCV4)
 endif()
+find_package(DBoW2 REQUIRED)
+message(${DBoW2_LIBRARIES})
+#if(NOT TARGET DBoW2::DBoW2)
+#  add_library(DBoW2::DBoW2 INTERFACE IMPORTED)
+#  set_target_properties(DBoW2::DBoW2 PROPERTIES
+#  INTERFACE_LINK_LIBRARIES "${DBoW2_LIBRARIES}"
+#  INTERFACE_INCLUDE_DIRECTORIES "${DBoW2_INCLUDE_DIRS}")
+#endif()
 find_package(KimeraRPGO REQUIRED)
 
 ### External Dependencies
@@ -77,7 +90,7 @@ else()
 endif()
 
 ### Compile the code
-add_library(${PROJECT_NAME} SHARED "")
+add_library(${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX} SHARED "")
 
 ### Add source code for data provider.
 add_subdirectory(include/kimera-vio/dataprovider)
@@ -121,21 +134,20 @@ add_subdirectory(src/imu-frontend)
 ### Add source code for visualizer.
 add_subdirectory(include/kimera-vio/visualizer)
 add_subdirectory(src/visualizer)
-
-target_link_libraries(${PROJECT_NAME}
-  PRIVATE
+target_link_libraries(${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX}
+  PUBLIC
     opengv
-    DBoW2::DBoW2
     KimeraRPGO
-  PUBLIC
+    ${DBoW2_LIBRARIES}
     Boost::boost
     ${OpenCV_LIBRARIES}
     ${GFLAGS_LIBRARIES}
     ${GLOG_LIBRARIES}
+    Eigen3::Eigen
     gtsam
     gtsam_unstable
  )
-target_include_directories(${PROJECT_NAME}
+target_include_directories(${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX}
   PUBLIC
     ${OpenCV_INCLUDE_DIRS}
     ${GFLAGS_INCLUDE_DIRS}
@@ -145,25 +157,26 @@ target_include_directories(${PROJECT_NAME}
     $<INSTALL_INTERFACE:include>
 )
 
-target_compile_options(${PROJECT_NAME}
+target_compile_options(${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX}
   PRIVATE -Wall -pipe
-  PRIVATE -march=native
 )
 
 # We would just need to say cxx_std_11 if we were using cmake 3.8
-target_compile_features(${PROJECT_NAME} PUBLIC
+target_compile_features(${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX} PUBLIC
         cxx_auto_type cxx_constexpr cxx_range_for cxx_nullptr cxx_override)
 
 # Add an alias so that library can be used inside the build tree,
 # e.g. when testing
-add_library(kimera_vio::kimera_vio ALIAS kimera_vio)
+add_library(kimera_vio::kimera_vio ALIAS kimera_vio${ILLIXR_BUILD_SUFFIX})
 
 add_executable(stereoVIOEuroc ./examples/KimeraVIO.cpp)
 target_link_libraries(stereoVIOEuroc PUBLIC kimera_vio::kimera_vio)
+option(ILLIXR_ROOT "" "")
 
-add_library(plugin SHARED ./examples/plugin.cpp)
-target_include_directories(plugin PRIVATE ./common)
-target_link_libraries(plugin PUBLIC kimera_vio::kimera_vio)
+add_library(plugin.kimera_vio${ILLIXR_BUILD_SUFFIX} SHARED ./examples/plugin.cpp)
+target_include_directories(plugin.kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE ${ILLIXR_ROOT})
+target_link_libraries(plugin.kimera_vio${ILLIXR_BUILD_SUFFIX} PUBLIC kimera_vio::kimera_vio)
+target_compile_definitions(plugin.kimera_vio${ILLIXR_BUILD_SUFFIX} PUBLIC -DPARAMS_LOCATION=\"${CMAKE_INSTALL_PREFIX}/etc/kimera_vio/params\")
 
 ############################### TESTS ##########################################
 ### Add testing
@@ -279,13 +292,13 @@ configure_package_config_file(
 
 option(EXPORT_KIMERA "Export kimera_vio instead of installing it." OFF)
 if(EXPORT_KIMERA)
-  export(TARGETS ${PROJECT_NAME}
+  export(TARGETS ${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX}
          FILE kimera_vioTargets.cmake)
-       export(PACKAGE ${PROJECT_NAME})
+       export(PACKAGE ${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX})
 else(EXPORT_KIMERA)
   include(GNUInstallDirs)
   ## First of all, we need to install the library itself.
-  install(TARGETS ${PROJECT_NAME}
+  install(TARGETS ${PROJECT_NAME}${ILLIXR_BUILD_SUFFIX}
       EXPORT kimera_vio-export
       LIBRARY DESTINATION lib
       ARCHIVE DESTINATION lib
@@ -294,6 +307,8 @@ else(EXPORT_KIMERA)
       INCLUDES DESTINATION include # We need this right?
       RUNTIME DESTINATION bin
   )
+  install(TARGETS plugin.kimera_vio${ILLIXR_BUILD_SUFFIX}
+          DESTINATION lib)
   install(EXPORT kimera_vio-export
     FILE
       kimera_vioTargets.cmake
@@ -313,6 +328,8 @@ else(EXPORT_KIMERA)
       ${CMAKE_CURRENT_LIST_DIR}/cmake/FindGlog.cmake
       DESTINATION ${INSTALL_CONFIGDIR}
   )
+  install(DIRECTORY ${CMAKE_SOURCE_DIR}/params
+          DESTINATION etc/kimera_vio)
 endif(EXPORT_KIMERA)
 
 ################################################################################

examples/plugin.cpp

@@ -1,29 +1,32 @@
 #include <functional>
 
-#include <opencv/cv.hpp>
+#include <opencv2/opencv.hpp>
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 
-#include <math.h>
+#include <cmath>
 #include <eigen3/Eigen/Dense>
+#include <filesystem>
 
 #include "kimera-vio/pipeline/Pipeline.h"
 
-#include "../common/plugin.hpp"
-#include "../common/switchboard.hpp"
-#include "../common/data_format.hpp"
-#include "../common/phonebook.hpp"
-#include "../common/error_util.hpp"
+#include "illixr/plugin.hpp"
+#include "illixr/opencv_data_types.hpp"
+#include "illixr/switchboard.hpp"
+#include "illixr/data_format.hpp"
+#include "illixr/phonebook.hpp"
+#include "illixr/error_util.hpp"
 
 using namespace ILLIXR;
 
 std::string get_path() {
-    const char* KIMERA_ROOT_c_str = std::getenv("KIMERA_ROOT");
-	if (!KIMERA_ROOT_c_str) {
-        ILLIXR::abort("Please define KIMERA_ROOT");
-	}
-	std::string KIMERA_ROOT = std::string{KIMERA_ROOT_c_str};
-    return KIMERA_ROOT + "params/ILLIXR";
+    std::string path = std::string{PARAMS_LOCATION} + "/ILLIXR";
+    if(std::filesystem::is_directory(path))
+        return path;
+    const char* KIMERA_ROOT = std::getenv("KIMERA_ROOT");
+    if (!KIMERA_ROOT)
+        ILLIXR::abort("Parameter files not found, please define KIMERA_ROOT");
+    return std::string{KIMERA_ROOT} + "/ILLIXR";
 }
 
 class kimera_vio : public plugin {
@@ -147,22 +150,22 @@ class kimera_vio : public plugin {
 
         const auto& cached_state = vio_output->W_State_Blkf_;
         const auto& w_pose_blkf_trans = cached_state.pose_.translation().transpose();
-        const auto& w_pose_blkf_rot = cached_state.pose_.rotation().quaternion();
+        const auto& w_pose_blkf_rot = cached_state.pose_.rotation().toQuaternion();
         const auto& w_vel_blkf = cached_state.velocity_.transpose();
         const auto& imu_bias_gyro = cached_state.imu_bias_.gyroscope().transpose();
         const auto& imu_bias_acc = cached_state.imu_bias_.accelerometer().transpose();
         // Get the pose returned from SLAM
         Eigen::Quaternionf quat = Eigen::Quaternionf{
-            static_cast<float>(w_pose_blkf_rot(0)),
-            static_cast<float>(w_pose_blkf_rot(1)),
-            static_cast<float>(w_pose_blkf_rot(2)),
-            static_cast<float>(w_pose_blkf_rot(3))
+            static_cast<float>(w_pose_blkf_rot.w()),
+            static_cast<float>(w_pose_blkf_rot.x()),
+            static_cast<float>(w_pose_blkf_rot.y()),
+            static_cast<float>(w_pose_blkf_rot.z())
         };
         Eigen::Quaterniond doub_quat = Eigen::Quaterniond{
-            w_pose_blkf_rot(0),
-            w_pose_blkf_rot(1),
-            w_pose_blkf_rot(2),
-            w_pose_blkf_rot(3)
+            w_pose_blkf_rot.w(),
+            w_pose_blkf_rot.x(),
+            w_pose_blkf_rot.y(),
+            w_pose_blkf_rot.z()
         };
         Eigen::Vector3f pos  = w_pose_blkf_trans.cast<float>();
 

include/kimera-vio/backend/CMakeLists.txt

@@ -1,5 +1,5 @@
 ### Add source code just for IDEs
-target_sources(kimera_vio PRIVATE
+target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE
   "${CMAKE_CURRENT_LIST_DIR}/FactorGraphManagement.h"
   "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd-definitions.h"
   "${CMAKE_CURRENT_LIST_DIR}/RegularVioBackEnd.h"

include/kimera-vio/backend/RegularVioBackEnd.h

@@ -69,7 +69,7 @@ class RegularVioBackEnd : public VioBackEnd {
   // For Stereo and Projection factors.
   gtsam::SharedNoiseModel stereo_noise_;
   gtsam::SharedNoiseModel mono_noise_;
-  boost::shared_ptr<Cal3_S2> mono_cal_;
+  AUTOP::shared_ptr<Cal3_S2> mono_cal_;
 
   // For regularity factors.
   gtsam::SharedNoiseModel point_plane_regularity_noise_;

include/kimera-vio/backend/VioBackEnd.h

@@ -29,9 +29,11 @@
 #include <iostream>
 #include <memory>
 #include <unordered_map>
+#include "kimera-vio/definitions.h"
 
+#ifndef USING_GTSAM4
 #include <boost/foreach.hpp>
-
+#endif
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/StereoCamera.h>
@@ -44,7 +46,11 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#ifdef USING_GTSAM4
+#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
+#else
 #include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
+#endif
 #include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
 
 #include "kimera-vio/backend/VioBackEnd-definitions.h"
@@ -390,31 +396,31 @@ class VioBackEnd {
                          const bool& showDetails = false) const;
 
   /* ------------------------------------------------------------------------ */
-  void printSmartFactor(boost::shared_ptr<SmartStereoFactor> gsf) const;
+  void printSmartFactor(AUTOP::shared_ptr<SmartStereoFactor> gsf) const;
 
   /* ------------------------------------------------------------------------ */
   void printPointPlaneFactor(
-      boost::shared_ptr<gtsam::PointPlaneFactor> ppf) const;
+      AUTOP::shared_ptr<gtsam::PointPlaneFactor> ppf) const;
 
   /* ------------------------------------------------------------------------ */
   void printPlanePrior(
-      boost::shared_ptr<gtsam::PriorFactor<gtsam::OrientedPlane3>> ppp) const;
+      AUTOP::shared_ptr<gtsam::PriorFactor<gtsam::OrientedPlane3>> ppp) const;
 
   /* ------------------------------------------------------------------------ */
   void printPointPrior(
-      boost::shared_ptr<gtsam::PriorFactor<gtsam::Point3>> ppp) const;
+      AUTOP::shared_ptr<gtsam::PriorFactor<gtsam::Point3>> ppp) const;
 
   /* ------------------------------------------------------------------------ */
   void printLinearContainerFactor(
-      boost::shared_ptr<gtsam::LinearContainerFactor> lcf) const;
+      AUTOP::shared_ptr<gtsam::LinearContainerFactor> lcf) const;
 
   /* ------------------------------------------------------------------------ */
   // Provide a nonlinear factor, which will be casted to any of the selected
   // factors, and then printed.
   // Slot argument, is just to print the slot of the factor if you know it.
   // If slot is -1, there is no slot number printed.
   void printSelectedFactors(
-      const boost::shared_ptr<gtsam::NonlinearFactor>& g,
+      const AUTOP::shared_ptr<gtsam::NonlinearFactor>& g,
       const size_t& slot = 0,
       const bool print_smart_factors = true,
       const bool print_point_plane_factors = true,

include/kimera-vio/common/CMakeLists.txt

@@ -1,5 +1,5 @@
 ### Add source code just for IDEs
-target_sources(kimera_vio
+target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX}
   PRIVATE
     "${CMAKE_CURRENT_LIST_DIR}/vio_types.h"
     "${CMAKE_CURRENT_LIST_DIR}/VioNavState.h"

include/kimera-vio/dataprovider/CMakeLists.txt

@@ -1,5 +1,5 @@
 ### Add source code for IDEs
-target_sources(kimera_vio PRIVATE
+target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE
   "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.h"
   "${CMAKE_CURRENT_LIST_DIR}/DataProviderModule.h"
   "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.h"

include/kimera-vio/definitions.h

@@ -0,0 +1,7 @@
+#pragma once
+
+#ifdef USING_GTSAM4
+#define AUTOP std
+#else
+#define AUTOP boost
+#endif

include/kimera-vio/factors/CMakeLists.txt

@@ -1,5 +1,5 @@
 ### Add source code for IDEs
-target_sources(kimera_vio PRIVATE
+target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE
   "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.h"
   "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.h"
 )