diff --git a/.gitignore b/.gitignore index 228fd7618..894fecd97 100644 --- a/.gitignore +++ b/.gitignore @@ -33,3 +33,6 @@ common % Ignore objects *.dbg.so *.opt.so + +.idea +cmake-build* diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bf5fdd66..55c668406 100644 --- a/CMakeLists.txt +++ b/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} $ ) -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) ################################################################################ diff --git a/examples/plugin.cpp b/examples/plugin.cpp index 56146b352..a227eaa8d 100644 --- a/examples/plugin.cpp +++ b/examples/plugin.cpp @@ -1,29 +1,32 @@ #include -#include +#include #include #include -#include +#include #include +#include #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(w_pose_blkf_rot(0)), - static_cast(w_pose_blkf_rot(1)), - static_cast(w_pose_blkf_rot(2)), - static_cast(w_pose_blkf_rot(3)) + static_cast(w_pose_blkf_rot.w()), + static_cast(w_pose_blkf_rot.x()), + static_cast(w_pose_blkf_rot.y()), + static_cast(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(); diff --git a/include/kimera-vio/backend/CMakeLists.txt b/include/kimera-vio/backend/CMakeLists.txt index 0dd33f2a7..594d79ca6 100644 --- a/include/kimera-vio/backend/CMakeLists.txt +++ b/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" diff --git a/include/kimera-vio/backend/RegularVioBackEnd.h b/include/kimera-vio/backend/RegularVioBackEnd.h index 85ff01b2c..3a9616b5f 100644 --- a/include/kimera-vio/backend/RegularVioBackEnd.h +++ b/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 mono_cal_; + AUTOP::shared_ptr mono_cal_; // For regularity factors. gtsam::SharedNoiseModel point_plane_regularity_noise_; diff --git a/include/kimera-vio/backend/VioBackEnd.h b/include/kimera-vio/backend/VioBackEnd.h index 103585a61..fb661626a 100644 --- a/include/kimera-vio/backend/VioBackEnd.h +++ b/include/kimera-vio/backend/VioBackEnd.h @@ -29,9 +29,11 @@ #include #include #include +#include "kimera-vio/definitions.h" +#ifndef USING_GTSAM4 #include - +#endif #include #include #include @@ -44,7 +46,11 @@ #include #include #include +#ifdef USING_GTSAM4 +#include +#else #include +#endif #include #include "kimera-vio/backend/VioBackEnd-definitions.h" @@ -390,23 +396,23 @@ class VioBackEnd { const bool& showDetails = false) const; /* ------------------------------------------------------------------------ */ - void printSmartFactor(boost::shared_ptr gsf) const; + void printSmartFactor(AUTOP::shared_ptr gsf) const; /* ------------------------------------------------------------------------ */ void printPointPlaneFactor( - boost::shared_ptr ppf) const; + AUTOP::shared_ptr ppf) const; /* ------------------------------------------------------------------------ */ void printPlanePrior( - boost::shared_ptr> ppp) const; + AUTOP::shared_ptr> ppp) const; /* ------------------------------------------------------------------------ */ void printPointPrior( - boost::shared_ptr> ppp) const; + AUTOP::shared_ptr> ppp) const; /* ------------------------------------------------------------------------ */ void printLinearContainerFactor( - boost::shared_ptr lcf) const; + AUTOP::shared_ptr lcf) const; /* ------------------------------------------------------------------------ */ // Provide a nonlinear factor, which will be casted to any of the selected @@ -414,7 +420,7 @@ class VioBackEnd { // 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& g, + const AUTOP::shared_ptr& g, const size_t& slot = 0, const bool print_smart_factors = true, const bool print_point_plane_factors = true, diff --git a/include/kimera-vio/common/CMakeLists.txt b/include/kimera-vio/common/CMakeLists.txt index 37cdfbe0d..4223e847a 100644 --- a/include/kimera-vio/common/CMakeLists.txt +++ b/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" diff --git a/include/kimera-vio/dataprovider/CMakeLists.txt b/include/kimera-vio/dataprovider/CMakeLists.txt index 66555ee56..21b7983e8 100644 --- a/include/kimera-vio/dataprovider/CMakeLists.txt +++ b/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" diff --git a/include/kimera-vio/definitions.h b/include/kimera-vio/definitions.h new file mode 100644 index 000000000..ac1d50069 --- /dev/null +++ b/include/kimera-vio/definitions.h @@ -0,0 +1,7 @@ +#pragma once + +#ifdef USING_GTSAM4 +#define AUTOP std +#else +#define AUTOP boost +#endif \ No newline at end of file diff --git a/include/kimera-vio/factors/CMakeLists.txt b/include/kimera-vio/factors/CMakeLists.txt index e758bc3d9..4cd000320 100644 --- a/include/kimera-vio/factors/CMakeLists.txt +++ b/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" ) diff --git a/include/kimera-vio/factors/ParallelPlaneRegularFactor.h b/include/kimera-vio/factors/ParallelPlaneRegularFactor.h index 8bdc4ea74..c1c135107 100644 --- a/include/kimera-vio/factors/ParallelPlaneRegularFactor.h +++ b/include/kimera-vio/factors/ParallelPlaneRegularFactor.h @@ -69,8 +69,13 @@ class ParallelPlaneRegularFactor: public NoiseModelFactor2 H_plane_1 = {}, + OptionalMatrixTypeT H_plane_2 = {}) const { +#else boost::optional H_plane_1 = boost::none, boost::optional H_plane_2 = boost::none) const { +#endif return doEvaluateError(plane_1, plane_2, H_plane_1, H_plane_2); } @@ -82,8 +87,13 @@ class ParallelPlaneRegularFactor: public NoiseModelFactor2 H_plane_1, + OptionalMatrixTypeT H_plane_2) const = 0; +#else boost::optional H_plane_1, boost::optional H_plane_2) const = 0; +#endif }; /** @@ -114,8 +124,13 @@ class ParallelPlaneRegularTangentSpaceFactor: virtual Vector doEvaluateError( const OrientedPlane3& plane_1, const OrientedPlane3& plane_2, +#ifdef USING_GTSAM4 + OptionalMatrixTypeT H_plane_1, + OptionalMatrixTypeT H_plane_2) const { +#else boost::optional H_plane_1, boost::optional H_plane_2) const { +#endif Unit3 plane_normal_1 = plane_1.normal(); Unit3 plane_normal_2 = plane_2.normal(); Matrix22 H_n_1, H_n_2; @@ -165,8 +180,13 @@ class GeneralParallelPlaneRegularTangentSpaceFactor: virtual Vector doEvaluateError( const OrientedPlane3& plane_1, const OrientedPlane3& plane_2, +#ifdef USING_GTSAM4 + OptionalMatrixTypeT H_plane_1, + OptionalMatrixTypeT H_plane_2) const { +#else boost::optional H_plane_1, boost::optional H_plane_2) const { +#endif Unit3 plane_normal_1 = plane_1.normal(); Unit3 plane_normal_2 = plane_2.normal(); Matrix22 H_n_1, H_n_2; @@ -218,8 +238,13 @@ class ParallelPlaneRegularBasicFactor: virtual Vector doEvaluateError( const OrientedPlane3& plane_1, const OrientedPlane3& plane_2, +#ifdef USING_GTSAM4 + OptionalMatrixTypeT H_plane_1, + OptionalMatrixTypeT H_plane_2) const { +#else boost::optional H_plane_1, boost::optional H_plane_2) const { +#endif const Unit3& plane_normal_1 = plane_1.normal(); const Unit3& plane_normal_2 = plane_2.normal(); Vector3 err (plane_normal_1.unitVector() - plane_normal_2.unitVector()); @@ -272,8 +297,13 @@ class GeneralParallelPlaneRegularBasicFactor: virtual Vector doEvaluateError( const OrientedPlane3& plane_1, const OrientedPlane3& plane_2, +#ifdef USING_GTSAM4 + OptionalMatrixTypeT H_plane_1, + OptionalMatrixTypeT H_plane_2) const { +#else boost::optional H_plane_1, boost::optional H_plane_2) const { +#endif const Unit3& plane_normal_1(plane_1.normal()); const Unit3& plane_normal_2(plane_2.normal()); Vector4 err (0.0, 0.0, 0.0, 0.0); diff --git a/include/kimera-vio/factors/PointPlaneFactor.h b/include/kimera-vio/factors/PointPlaneFactor.h index 1e4b06c4d..57946cba3 100644 --- a/include/kimera-vio/factors/PointPlaneFactor.h +++ b/include/kimera-vio/factors/PointPlaneFactor.h @@ -15,6 +15,7 @@ #pragma once +#include #include #include @@ -52,8 +53,13 @@ class PointPlaneFactor: public NoiseModelFactor2 { /// Hpoint: jacobian of h wrt point landmark /// Hplane: jacobian of h wrt plane virtual Vector evaluateError(const Point3& point, const OrientedPlane3& plane, +#ifdef USING_GTSAM4 + Matrix* H_point = {}, + Matrix* H_plane = {}) const { +#else boost::optional H_point = boost::none, boost::optional H_plane = boost::none) const { +#endif Vector err(1); Unit3 plane_normal = plane.normal(); double plane_distance = plane.distance(); @@ -65,7 +71,11 @@ class PointPlaneFactor: public NoiseModelFactor2 { // computations. H_plane_retract << plane_normal.basis(), Vector3::Zero(), 0, 0, 1; Vector4 p; +#ifdef USING_GTSAM4 + p << point, -1; +#else p << point.vector(), -1; +#endif *H_plane = p.transpose() * H_plane_retract; } diff --git a/include/kimera-vio/frontend/CMakeLists.txt b/include/kimera-vio/frontend/CMakeLists.txt index ea3a0633a..3bdfa8646 100644 --- a/include/kimera-vio/frontend/CMakeLists.txt +++ b/include/kimera-vio/frontend/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}/Camera.h" "${CMAKE_CURRENT_LIST_DIR}/StereoCamera.h" "${CMAKE_CURRENT_LIST_DIR}/UndistorterRectifier.h" diff --git a/include/kimera-vio/frontend/Camera.h b/include/kimera-vio/frontend/Camera.h index dd47972db..588bf7cc6 100644 --- a/include/kimera-vio/frontend/Camera.h +++ b/include/kimera-vio/frontend/Camera.h @@ -14,7 +14,7 @@ #pragma once -#include +#include #include diff --git a/include/kimera-vio/frontend/OpticalFlowPredictor.h b/include/kimera-vio/frontend/OpticalFlowPredictor.h index a1f5266ac..e6ffdcb40 100644 --- a/include/kimera-vio/frontend/OpticalFlowPredictor.h +++ b/include/kimera-vio/frontend/OpticalFlowPredictor.h @@ -93,7 +93,11 @@ class RotationalOpticalFlowPredictor : public OpticalFlowPredictor { // of the image. // Keep because then you save a lot of computation. static constexpr double kSmallRotationTol = 1e-4; +#ifdef USING_GTSAM4 + if (std::abs(1.0 - std::abs(cam1_R_cam2.toQuaternion().w())) < +#else if (std::abs(1.0 - std::abs(cam1_R_cam2.quaternion()[0])) < +#endif kSmallRotationTol) { *next_kps = prev_kps; return true; diff --git a/include/kimera-vio/frontend/StereoCamera.h b/include/kimera-vio/frontend/StereoCamera.h index c69962f24..8160139a0 100644 --- a/include/kimera-vio/frontend/StereoCamera.h +++ b/include/kimera-vio/frontend/StereoCamera.h @@ -14,7 +14,7 @@ #pragma once -#include +#include #include #include diff --git a/include/kimera-vio/frontend/Tracker.h b/include/kimera-vio/frontend/Tracker.h index 5f2096b66..be428c038 100644 --- a/include/kimera-vio/frontend/Tracker.h +++ b/include/kimera-vio/frontend/Tracker.h @@ -16,6 +16,10 @@ // TODO(Toni): put tracker in another folder. #pragma once +#ifdef USING_GTSAM4 +#include +#endif + #include #include diff --git a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/CMakeLists.txt index 207166d2c..2e484cf5d 100644 --- a/include/kimera-vio/frontend/feature-detector/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/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}/FeatureDetector.h" "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.h" "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector-definitions.h" diff --git a/include/kimera-vio/frontend/feature-detector/FeatureDetector.h b/include/kimera-vio/frontend/feature-detector/FeatureDetector.h index 7bc205f79..a1b20e2cc 100644 --- a/include/kimera-vio/frontend/feature-detector/FeatureDetector.h +++ b/include/kimera-vio/frontend/feature-detector/FeatureDetector.h @@ -14,7 +14,7 @@ #pragma once -#include +#include #include diff --git a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt index cc08258db..ec8b8202a 100644 --- a/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt +++ b/include/kimera-vio/frontend/feature-detector/anms/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for IDEs -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/nanoflann.hpp" "${CMAKE_CURRENT_LIST_DIR}/range-tree/lrtypes.h" diff --git a/include/kimera-vio/imu-frontend/CMakeLists.txt b/include/kimera-vio/imu-frontend/CMakeLists.txt index 42ce70bf3..3f287bdad 100644 --- a/include/kimera-vio/imu-frontend/CMakeLists.txt +++ b/include/kimera-vio/imu-frontend/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for kimera_vio -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.h" "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.h" diff --git a/include/kimera-vio/imu-frontend/ImuFrontEnd-definitions.h b/include/kimera-vio/imu-frontend/ImuFrontEnd-definitions.h index e8600b700..9cb7f64a0 100644 --- a/include/kimera-vio/imu-frontend/ImuFrontEnd-definitions.h +++ b/include/kimera-vio/imu-frontend/ImuFrontEnd-definitions.h @@ -14,7 +14,7 @@ #pragma once -#include +#include #include diff --git a/include/kimera-vio/imu-frontend/ImuFrontEnd.h b/include/kimera-vio/imu-frontend/ImuFrontEnd.h index 122d9927a..9ffdab9fd 100644 --- a/include/kimera-vio/imu-frontend/ImuFrontEnd.h +++ b/include/kimera-vio/imu-frontend/ImuFrontEnd.h @@ -20,8 +20,12 @@ #include #include #include +#include "kimera-vio/definitions.h" +#ifdef USING_GTSAM4 +#include +#endif -#include +#include #include #include @@ -188,10 +192,10 @@ class ImuFrontEnd { static gtsam::PreintegrationType::Params convertVioImuParamsToGtsam( const ImuParams& imu_params); - static boost::shared_ptr + static AUTOP::shared_ptr generateCombinedImuParams(const ImuParams& imu_params); - static boost::shared_ptr + static AUTOP::shared_ptr generateRegularImuParams(const ImuParams& imu_params); private: diff --git a/include/kimera-vio/initial/CMakeLists.txt b/include/kimera-vio/initial/CMakeLists.txt index edefa6a3a..088b8b27e 100644 --- a/include/kimera-vio/initial/CMakeLists.txt +++ b/include/kimera-vio/initial/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.h" "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.h" diff --git a/include/kimera-vio/initial/InitializationBackEnd-definitions.h b/include/kimera-vio/initial/InitializationBackEnd-definitions.h index 6f770c7dd..e148a640d 100644 --- a/include/kimera-vio/initial/InitializationBackEnd-definitions.h +++ b/include/kimera-vio/initial/InitializationBackEnd-definitions.h @@ -40,8 +40,13 @@ struct InitializationInputPayload : public FrontendOutput { const ImuFrontEnd::PimPtr& pim, const ImuAccGyrS imu_acc_gyrs, const DebugTrackerInfo& debug_tracker_info, +#ifdef USING_GTSAM4 + const gtsam::PreintegratedAhrsMeasurements& ahrs_pim = + gtsam::PreintegratedAhrsMeasurements()) +#else const gtsam::AHRSFactor::PreintegratedMeasurements& ahrs_pim = gtsam::AHRSFactor::PreintegratedMeasurements()) +#endif : FrontendOutput(is_keyframe, status_stereo_measurements, tracker_status, @@ -52,8 +57,11 @@ struct InitializationInputPayload : public FrontendOutput { cv::Mat(), debug_tracker_info), ahrs_pim_(ahrs_pim) {} - - const gtsam::AHRSFactor::PreintegratedMeasurements ahrs_pim_; +#ifdef USING_GTSAM4 + const gtsam::PreintegratedAhrsMeasurements ahrs_pim_; +#else + const gtsam::AHRSFactor::PreintegratedMeasurements ahrs_pim_; +#endif }; } // namespace VIO diff --git a/include/kimera-vio/initial/OnlineGravityAlignment.h b/include/kimera-vio/initial/OnlineGravityAlignment.h index 26d340285..078b02f88 100644 --- a/include/kimera-vio/initial/OnlineGravityAlignment.h +++ b/include/kimera-vio/initial/OnlineGravityAlignment.h @@ -37,7 +37,11 @@ namespace VIO { typedef std::vector AlignmentPoses; typedef std::vector AlignmentPims; typedef std::vector VisualInertialFrames; +#ifdef USING_GTSAM4 +typedef std::vector +#else typedef std::vector +#endif InitialAHRSPims; // Class with functions for online initialization diff --git a/include/kimera-vio/logging/CMakeLists.txt b/include/kimera-vio/logging/CMakeLists.txt index 5d5bd3392..887f443c6 100644 --- a/include/kimera-vio/logging/CMakeLists.txt +++ b/include/kimera-vio/logging/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Logger.h" ) diff --git a/include/kimera-vio/logging/Logger.h b/include/kimera-vio/logging/Logger.h index b6d4e89b1..60eda68b5 100644 --- a/include/kimera-vio/logging/Logger.h +++ b/include/kimera-vio/logging/Logger.h @@ -20,6 +20,11 @@ #include #include +#ifdef USING_GTSAM4 +#include +#include +#endif + #include "kimera-vio/backend/VioBackEnd-definitions.h" #include "kimera-vio/loopclosure/LoopClosureDetector-definitions.h" #include "kimera-vio/mesh/Mesh.h" diff --git a/include/kimera-vio/loopclosure/CMakeLists.txt b/include/kimera-vio/loopclosure/CMakeLists.txt index e76bf18fe..bef348c0b 100644 --- a/include/kimera-vio/loopclosure/CMakeLists.txt +++ b/include/kimera-vio/loopclosure/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for LoopClosureDetector -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.h" "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetectorParams.h" diff --git a/include/kimera-vio/mesh/CMakeLists.txt b/include/kimera-vio/mesh/CMakeLists.txt index efb824c1f..698dbb021 100644 --- a/include/kimera-vio/mesh/CMakeLists.txt +++ b/include/kimera-vio/mesh/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Mesh.h" "${CMAKE_CURRENT_LIST_DIR}/Mesher.h" "${CMAKE_CURRENT_LIST_DIR}/MesherModule.h" diff --git a/include/kimera-vio/pipeline/CMakeLists.txt b/include/kimera-vio/pipeline/CMakeLists.txt index fbf57dcdc..f6bbc9437 100644 --- a/include/kimera-vio/pipeline/CMakeLists.txt +++ b/include/kimera-vio/pipeline/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Pipeline.h" "${CMAKE_CURRENT_LIST_DIR}/Pipeline-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/PipelinePayload.h" diff --git a/include/kimera-vio/playground/CMakeLists.txt b/include/kimera-vio/playground/CMakeLists.txt index db3f85eaa..6a7757d15 100644 --- a/include/kimera-vio/playground/CMakeLists.txt +++ b/include/kimera-vio/playground/CMakeLists.txt @@ -1,4 +1,4 @@ ### Add source code for IDEs -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/EurocPlayground.h" ) diff --git a/include/kimera-vio/utils/CMakeLists.txt b/include/kimera-vio/utils/CMakeLists.txt index 9187fbbb4..4c8a81976 100644 --- a/include/kimera-vio/utils/CMakeLists.txt +++ b/include/kimera-vio/utils/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add includes for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Accumulator.h" "${CMAKE_CURRENT_LIST_DIR}/Histogram.h" diff --git a/include/kimera-vio/utils/ThreadsafeImuBuffer.h b/include/kimera-vio/utils/ThreadsafeImuBuffer.h index 25b1f4ca3..22c818a9e 100644 --- a/include/kimera-vio/utils/ThreadsafeImuBuffer.h +++ b/include/kimera-vio/utils/ThreadsafeImuBuffer.h @@ -40,7 +40,7 @@ #include #include -#include +#include #include "kimera-vio/common/vio_types.h" #include "kimera-vio/imu-frontend/ImuFrontEnd-definitions.h" diff --git a/include/kimera-vio/utils/UtilsNumerical.h b/include/kimera-vio/utils/UtilsNumerical.h index 16bc0356d..a7d06bd52 100644 --- a/include/kimera-vio/utils/UtilsNumerical.h +++ b/include/kimera-vio/utils/UtilsNumerical.h @@ -15,6 +15,7 @@ #pragma once #include +#include namespace VIO { diff --git a/include/kimera-vio/utils/UtilsOpenCV.h b/include/kimera-vio/utils/UtilsOpenCV.h index 07c7f10cd..981eccf82 100644 --- a/include/kimera-vio/utils/UtilsOpenCV.h +++ b/include/kimera-vio/utils/UtilsOpenCV.h @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -36,8 +36,8 @@ // TODO(Toni): do not forward declare that much!! // Forward declare classes. namespace gtsam { -class Point2; -class Point3; +//class Point2; +//class Point3; class Pose3; class Symbol; typedef Eigen::MatrixXd Matrix; diff --git a/include/kimera-vio/visualizer/CMakeLists.txt b/include/kimera-vio/visualizer/CMakeLists.txt index dcbb5e1ea..8cc81c282 100644 --- a/include/kimera-vio/visualizer/CMakeLists.txt +++ b/include/kimera-vio/visualizer/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add includes -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D-definitions.h" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.h" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DFactory.h" diff --git a/src/backend/CMakeLists.txt b/src/backend/CMakeLists.txt index 2cd79690f..a3ec98974 100644 --- a/src/backend/CMakeLists.txt +++ b/src/backend/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code -target_sources(kimera_vio PRIVATE +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/VioBackEndModule.cpp" "${CMAKE_CURRENT_LIST_DIR}/VioBackEnd.cpp" "${CMAKE_CURRENT_LIST_DIR}/VioBackEndParams.cpp" diff --git a/src/backend/RegularVioBackEnd.cpp b/src/backend/RegularVioBackEnd.cpp index ed8f609da..d0026722b 100644 --- a/src/backend/RegularVioBackEnd.cpp +++ b/src/backend/RegularVioBackEnd.cpp @@ -20,6 +20,9 @@ #include "kimera-vio/backend/RegularVioBackEnd.h" +#ifdef USING_GTSAM4 +#include +#endif #include #include @@ -28,6 +31,7 @@ #include "kimera-vio/factors/PointPlaneFactor.h" #include "kimera-vio/utils/UtilsNumerical.h" +#include "kimera-vio/definitions.h" DEFINE_int32(min_num_of_observations, 2, @@ -140,7 +144,7 @@ RegularVioBackEnd::RegularVioBackEnd( regular_vio_params_.regularityNormType_, regular_vio_params_.regularityNormParam_); - mono_cal_ = boost::make_shared(stereo_cal_->calibration()); + mono_cal_ = AUTOP::make_shared(stereo_cal_->calibration()); CHECK(mono_cal_->equals(stereo_cal_->calibration())) << "Monocular calibration should match Stereo calibration"; } @@ -460,7 +464,7 @@ void RegularVioBackEnd::addLandmarkToGraph(const LandmarkId& lmk_id, // We use a unit pinhole projection camera for the smart factors to be // more efficient. SmartStereoFactor::shared_ptr new_factor = - boost::make_shared( + AUTOP::make_shared( smart_noise_, smart_factors_params_, B_Pose_leftCam_); VLOG(20) << "Adding landmark with id: " << lmk_id @@ -579,7 +583,7 @@ void RegularVioBackEnd::updateExistingSmartFactor( // Clone old factor as a new factor. SmartStereoFactor::shared_ptr new_factor = - boost::make_shared(*old_factor); + AUTOP::make_shared(*old_factor); // Add observation to new factor. VLOG(20) << "Added observation for smart factor of lmk with id: " << lmk_id; @@ -643,7 +647,11 @@ bool RegularVioBackEnd::convertSmartToProjectionFactor( // Check triangulation result is initialized. if (old_factor->point().valid()) { +#ifdef USING_GTSAM4 + CHECK(old_factor->point().has_value()); +#else CHECK(old_factor->point().is_initialized()); +#endif VLOG(10) << "Performing conversion for lmk with id: " << lmk_id << " from " << " smart factor to projection factor."; @@ -656,7 +664,7 @@ bool RegularVioBackEnd::convertSmartToProjectionFactor( // static const gtsam::noiseModel::Diagonal::shared_ptr prior_lmk_noise = // gtsam::noiseModel::Diagonal::Sigmas(Vector3(1, 1, 1)); // new_imu_prior_and_other_factors_.push_back( - // boost::make_shared >( + // AUTOP::make_shared >( // lmk_key, // *(old_factor->point()), // prior_lmk_noise)); @@ -826,7 +834,7 @@ void RegularVioBackEnd::addProjectionFactor( if (parallax < FLAGS_max_parallax) { CHECK_GT(parallax, 0.0); new_imu_prior_and_other_factors->push_back( - boost::make_shared>( + AUTOP::make_shared>( new_obs.second, stereo_noise_, gtsam::Symbol('x', new_obs.first), @@ -845,7 +853,7 @@ void RegularVioBackEnd::addProjectionFactor( // Right pixel has a NAN value for u, use GenericProjectionFactor instead // of stereo. new_imu_prior_and_other_factors->push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Point2(new_obs.second.uL(), new_obs.second.v()), mono_noise_, gtsam::Symbol('x', new_obs.first), @@ -960,7 +968,11 @@ bool RegularVioBackEnd::isSmartFactor3dPointGood( VLOG(20) << "Smart factor is NOT valid."; return false; } else { +#ifdef USING_GTSAM4 + CHECK(factor->point().has_value()); +#else CHECK(factor->point().is_initialized()); +#endif // If the smart factor has less than x number of observations, // then do not consider the landmark as valid. // This param is different from the one counting the track length @@ -1052,7 +1064,7 @@ void RegularVioBackEnd::addRegularityFactors( idx_of_point_plane_factors_to_add->push_back(std::make_pair( new_imu_prior_and_other_factors_.size(), lmk_id)); new_imu_prior_and_other_factors_.push_back( - boost::make_shared( + AUTOP::make_shared( gtsam::Symbol('l', lmk_id), plane_key, point_plane_regularity_noise_)); @@ -1092,7 +1104,7 @@ void RegularVioBackEnd::addRegularityFactors( // = // gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.5, 0.5, 0.5)); // new_imu_prior_and_other_factors_.push_back( - // boost::make_shared + // AUTOP::make_shared // >( // plane_key, // plane_value, @@ -1105,7 +1117,7 @@ void RegularVioBackEnd::addRegularityFactors( idx_of_point_plane_factors_to_add->push_back(std::make_pair( new_imu_prior_and_other_factors_.size(), prev_lmk_id)); new_imu_prior_and_other_factors_.push_back( - boost::make_shared( + AUTOP::make_shared( gtsam::Symbol('l', prev_lmk_id), plane_key, point_plane_regularity_noise_)); @@ -1185,7 +1197,7 @@ void RegularVioBackEnd::addRegularityFactors( idx_of_point_plane_factors_to_add->push_back( std::make_pair(new_imu_prior_and_other_factors_.size(), lmk_id)); new_imu_prior_and_other_factors_.push_back( - boost::make_shared( + AUTOP::make_shared( gtsam::Symbol('l', lmk_id), plane_key, point_plane_regularity_noise_)); @@ -1283,11 +1295,11 @@ void RegularVioBackEnd::removeOldRegularityFactors_Slow( // Loop over current graph. for (const auto& g : graph) { if (g) { - const auto& ppf = boost::dynamic_pointer_cast(g); - const auto& plane_prior = boost::dynamic_pointer_cast< + const auto& ppf = AUTOP::dynamic_pointer_cast(g); + const auto& plane_prior = AUTOP::dynamic_pointer_cast< gtsam::PriorFactor>(g); const auto& lcf = - boost::dynamic_pointer_cast(g); + AUTOP::dynamic_pointer_cast(g); if (ppf) { // We found a PointPlaneFactor. for (const size_t& plane_id : plane_idx_to_clean) { @@ -1489,7 +1501,7 @@ void RegularVioBackEnd::removeOldRegularityFactors_Slow( FLAGS_prior_noise_sigma_normal, FLAGS_prior_noise_sigma_distance)); new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( plane_symbol.key(), plane_estimate, prior_noise)); // Delete just the bad factors. @@ -1534,7 +1546,7 @@ void RegularVioBackEnd::removeOldRegularityFactors_Slow( // = // // gtsam::noiseModel::Diagonal::Sigmas(Vector3(1.0, 1.0, 1.0)); // // new_imu_prior_and_other_factors_.push_back( - // // boost::make_shared>( + // // AUTOP::make_shared>( // // point_symbol.key(), // // point, // // prior_lmk_noise)); @@ -1604,7 +1616,7 @@ void RegularVioBackEnd::deleteNewSlots( // The slot is valid. CHECK(new_imu_prior_and_other_factors->exists(i.first)); - const auto& ppf = boost::dynamic_pointer_cast( + const auto& ppf = AUTOP::dynamic_pointer_cast( new_imu_prior_and_other_factors->at(i.first)); // The factor is really a point plane one. CHECK(ppf); diff --git a/src/backend/VioBackEnd.cpp b/src/backend/VioBackEnd.cpp index c6570db1e..b51a9b86f 100644 --- a/src/backend/VioBackEnd.cpp +++ b/src/backend/VioBackEnd.cpp @@ -34,6 +34,9 @@ #include #include // for make_pair #include +#ifdef USING_GTSAM4 +#include +#endif #include #include @@ -43,6 +46,7 @@ #include "kimera-vio/utils/Statistics.h" #include "kimera-vio/utils/Timer.h" #include "kimera-vio/utils/UtilsNumerical.h" +#include "kimera-vio/definitions.h" DEFINE_bool(debug_graph_before_opt, false, @@ -408,7 +412,7 @@ void VioBackEnd::addLandmarkToGraph(const LandmarkId& lmk_id, // We use a unit pinhole projection camera for the smart factors to be // more efficient. SmartStereoFactor::shared_ptr new_factor = - boost::make_shared( + AUTOP::make_shared( smart_noise_, smart_factors_params_, B_Pose_leftCam_); VLOG(10) << "Adding landmark with: " << ft.obs_.size() @@ -451,7 +455,7 @@ void VioBackEnd::updateLandmarkInGraph( old_smart_factors_it->second.first; // Clone old factor to keep all previous measurements, now append one. SmartStereoFactor::shared_ptr new_factor = - boost::make_shared(*old_factor); + AUTOP::make_shared(*old_factor); gtsam::Symbol pose_symbol('x', new_measurement.first); if (smoother_->getFactors().exists(pose_symbol)) { const StereoPoint2& measurement = new_measurement.second; @@ -564,8 +568,8 @@ PointsWithIdMap VioBackEnd::getMapLmkIdsTo3dPointsInTimeHorizon( // such as the triangulated point, whether it is valid or not // and the number of observations... // Is graph more up to date? - boost::shared_ptr gsf = - boost::dynamic_pointer_cast(graph.at(slot_id)); + AUTOP::shared_ptr gsf = + AUTOP::dynamic_pointer_cast(graph.at(slot_id)); CHECK(gsf) << "Cannot cast factor in graph to a smart stereo factor."; // Get triangulation result from smart factor. @@ -573,7 +577,11 @@ PointsWithIdMap VioBackEnd::getMapLmkIdsTo3dPointsInTimeHorizon( // Check that the boost::optional result is initialized. // Otherwise we will be dereferencing a nullptr and we will head // directly to undefined behaviour wonderland. +#ifdef USING_GTSAM4 + if (result.has_value()) { +#else if (result.is_initialized()) { +#endif if (result.valid()) { if (gsf->measured().size() >= min_age) { // Triangulation result from smart factor is valid and @@ -610,12 +618,21 @@ PointsWithIdMap VioBackEnd::getMapLmkIdsTo3dPointsInTimeHorizon( // Step 2: ////////////// Add landmarks that now are in projection factors. ///////////// +#ifdef USING_GTSAM4 + for (const std::pair& + key_value : state_.extract(gtsam::Symbol::ChrTest('l'))) { + DCHECK_EQ(gtsam::Symbol(key_value.first).chr(), 'l'); + const LandmarkId& lmk_id = gtsam::Symbol(key_value.first).index(); + DCHECK(points_with_id.find(lmk_id) == points_with_id.end()); + points_with_id[lmk_id] = key_value.second; +#else for (const gtsam::Values::Filtered::ConstKeyValuePair& key_value : state_.filter(gtsam::Symbol::ChrTest('l'))) { DCHECK_EQ(gtsam::Symbol(key_value.key).chr(), 'l'); const LandmarkId& lmk_id = gtsam::Symbol(key_value.key).index(); DCHECK(points_with_id.find(lmk_id) == points_with_id.end()); points_with_id[lmk_id] = key_value.value.cast(); +#endif if (lmk_id_to_lmk_type_map) { (*lmk_id_to_lmk_type_map)[lmk_id] = LandmarkType::PROJECTION; } @@ -752,7 +769,7 @@ void VioBackEnd::addImuFactor(const FrameId& from_id, switch (imu_params_.imu_preintegration_type_) { case ImuPreintegrationType::kPreintegratedCombinedMeasurements: { new_imu_prior_and_other_factors_.push_back( - boost::make_shared( + AUTOP::make_shared( gtsam::Symbol('x', from_id), gtsam::Symbol('v', from_id), gtsam::Symbol('x', to_id), @@ -764,7 +781,7 @@ void VioBackEnd::addImuFactor(const FrameId& from_id, } case ImuPreintegrationType::kPreintegratedImuMeasurements: { new_imu_prior_and_other_factors_.push_back( - boost::make_shared( + AUTOP::make_shared( gtsam::Symbol('x', from_id), gtsam::Symbol('v', from_id), gtsam::Symbol('x', to_id), @@ -789,7 +806,7 @@ void VioBackEnd::addImuFactor(const FrameId& from_id, gtsam::noiseModel::Diagonal::Sigmas(biasSigmas); new_imu_prior_and_other_factors_.push_back( - boost::make_shared< + AUTOP::make_shared< gtsam::BetweenFactor>( gtsam::Symbol('b', from_id), gtsam::Symbol('b', to_id), @@ -820,7 +837,7 @@ void VioBackEnd::addBetweenFactor(const FrameId& from_id, gtsam::noiseModel::Diagonal::Precisions(precisions); new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('x', from_id), gtsam::Symbol('x', to_id), from_id_POSE_to_id, @@ -833,7 +850,7 @@ void VioBackEnd::addBetweenFactor(const FrameId& from_id, void VioBackEnd::addNoMotionFactor(const FrameId& from_id, const FrameId& to_id) { new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('x', from_id), gtsam::Symbol('x', to_id), Pose3(), @@ -851,7 +868,7 @@ void VioBackEnd::addNoMotionFactor(const FrameId& from_id, void VioBackEnd::addZeroVelocityPrior(const FrameId& frame_id) { VLOG(10) << "No motion detected, adding zero velocity prior."; new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('v', frame_id), gtsam::Vector3::Zero(), zero_velocity_prior_noise_)); @@ -960,8 +977,12 @@ bool VioBackEnd::optimize( if (VLOG_IS_ON(10)) { // Get state before optimization to compute error. debug_info_.stateBeforeOpt = gtsam::Values(state_); - BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, +#ifdef USING_GTSAM4 + for (auto key_value : new_values_) { +#else + BOOST_FOREACH (const std::pair& key_value, new_values_) { +#endif debug_info_.stateBeforeOpt.insert(key_value.key, key_value.value); } } @@ -993,8 +1014,12 @@ bool VioBackEnd::optimize( std::map timestamps; // Also needs to convert to seconds... double timestamp_kf = static_cast(timestamp_kf_nsec) * 1e-9; +#ifdef USING_GTSAM4 + for (auto key_value : new_values_) { +#else BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, new_values_) { +#endif timestamps[key_value.key] = timestamp_kf; // for the latest pose, velocity, and bias } @@ -1121,7 +1146,7 @@ void VioBackEnd::addInitialPriorFactors(const FrameId& frame_id) { gtsam::SharedNoiseModel noise_init_pose = gtsam::noiseModel::Gaussian::Covariance(pose_prior_covariance); new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('x', frame_id), W_Pose_B_lkf_, noise_init_pose)); // Add initial velocity priors. @@ -1130,7 +1155,7 @@ void VioBackEnd::addInitialPriorFactors(const FrameId& frame_id) { gtsam::noiseModel::Isotropic::Sigma( 3, backend_params_.initialVelocitySigma_); new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('v', frame_id), W_Vel_B_lkf_, noise_init_vel_prior)); // Add initial bias priors: @@ -1145,7 +1170,7 @@ void VioBackEnd::addInitialPriorFactors(const FrameId& frame_id) { imu_bias_lkf_.print(); } new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('b', frame_id), imu_bias_lkf_, imu_bias_prior_noise)); VLOG(2) << "Added initial priors for frame " << frame_id; @@ -1156,7 +1181,7 @@ void VioBackEnd::addConstantVelocityFactor(const FrameId& from_id, const FrameId& to_id) { VLOG(10) << "Adding constant velocity factor."; new_imu_prior_and_other_factors_.push_back( - boost::make_shared>( + AUTOP::make_shared>( gtsam::Symbol('v', from_id), gtsam::Symbol('v', to_id), gtsam::Vector3::Zero(), @@ -1478,12 +1503,12 @@ void VioBackEnd::updateNewSmartFactorsSlots( << "Trying to access unavailable factor."; // CHECK that the factor in the graph at slot position is a smart // factor. - DCHECK(boost::dynamic_pointer_cast( + DCHECK(AUTOP::dynamic_pointer_cast( smoother_->getFactors().at(slot))); // CHECK that shared ptrs point to the same smart factor. // make sure no one is cloning SmartSteroFactors. DCHECK_EQ(it->second.first, - boost::dynamic_pointer_cast( + AUTOP::dynamic_pointer_cast( smoother_->getFactors().at(slot))) << "Non-matching addresses for same factors for lmk with id: " << lmk_ids_of_new_smart_factors.at(i) << " in old_smart_factors_ " @@ -1533,12 +1558,20 @@ void VioBackEnd::setIsam2Params(const BackendParams& vio_params, // TODO (Toni): remove hardcoded // Cache Linearized Factors seems to improve performance. +#ifdef USING_GTSAM4 + isam_param->cacheLinearizedFactors = true; +#else isam_param->setCacheLinearizedFactors(true); +#endif isam_param->relinearizeThreshold = vio_params.relinearizeThreshold_; isam_param->relinearizeSkip = vio_params.relinearizeSkip_; isam_param->findUnusedFactorSlots = true; // isam_param->enablePartialRelinearizationCheck = true; +#ifdef USING_GTSAM4 + isam_param->evaluateNonlinearError = false; // only for debugging +#else isam_param->setEvaluateNonlinearError(false); // only for debugging +#endif isam_param->enableDetailedResults = false; // only for debugging. isam_param->factorization = gtsam::ISAM2Params::CHOLESKY; // QR } @@ -1740,7 +1773,11 @@ void VioBackEnd::printSmootherInfo( //////////////////////// Print all values in state. //////////////////////// LOG(INFO) << "Nr of values in state_ : " << state_.size() << ", with keys:"; std::cout << "[\n\t"; - BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, state_) { +#ifdef USING_GTSAM4 + for (auto key_value : state_) { +#else + //BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, state_) { +#endif std::cout << gtsam::DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; @@ -1750,8 +1787,12 @@ void VioBackEnd::printSmootherInfo( LOG(INFO) << "Nr values in new_values_ : " << new_values_.size() << ", with keys:"; std::cout << "[\n\t"; - BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, +#ifdef USING_GTSAM4 + for (auto key_value : new_values_) { +#else + BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair& key_value, new_values_) { +#endif std::cout << " " << gtsam::DefaultKeyFormatter(key_value.key) << " "; } std::cout << std::endl; @@ -1770,7 +1811,7 @@ void VioBackEnd::printSmootherInfo( } void VioBackEnd::printSmartFactor( - boost::shared_ptr gsf) const { + AUTOP::shared_ptr gsf) const { CHECK(gsf); std::cout << "Smart Factor (valid: " << (gsf->isValid() ? "yes" : "NO!") << ", deg: " << (gsf->isDegenerate() ? "YES!" : "no") @@ -1780,7 +1821,7 @@ void VioBackEnd::printSmartFactor( } void VioBackEnd::printPointPlaneFactor( - boost::shared_ptr ppf) const { + AUTOP::shared_ptr ppf) const { CHECK(ppf); std::cout << "Point Plane Factor: plane key " << gtsam::DefaultKeyFormatter(ppf->getPlaneKey()) << ", point key " @@ -1788,28 +1829,28 @@ void VioBackEnd::printPointPlaneFactor( } void VioBackEnd::printPlanePrior( - boost::shared_ptr> ppp) const { + AUTOP::shared_ptr> ppp) const { CHECK(ppp); std::cout << "Plane Prior: plane key \t"; ppp->printKeys(); } void VioBackEnd::printPointPrior( - boost::shared_ptr> ppp) const { + AUTOP::shared_ptr> ppp) const { CHECK(ppp); std::cout << "Point Prior: point key \t"; ppp->printKeys(); } void VioBackEnd::printLinearContainerFactor( - boost::shared_ptr lcf) const { + AUTOP::shared_ptr lcf) const { CHECK(lcf); std::cout << "Linear Container Factor: \t"; lcf->printKeys(); } void VioBackEnd::printSelectedFactors( - const boost::shared_ptr& g, + const AUTOP::shared_ptr& g, const size_t& slot, const bool print_smart_factors, const bool print_point_plane_factors, @@ -1817,7 +1858,7 @@ void VioBackEnd::printSelectedFactors( const bool print_point_priors, const bool print_linear_container_factors) const { if (print_smart_factors) { - const auto& gsf = boost::dynamic_pointer_cast(g); + const auto& gsf = AUTOP::dynamic_pointer_cast(g); if (gsf) { std::cout << "\tSlot # " << slot << ": "; printSmartFactor(gsf); @@ -1825,7 +1866,7 @@ void VioBackEnd::printSelectedFactors( } if (print_point_plane_factors) { - const auto& ppf = boost::dynamic_pointer_cast(g); + const auto& ppf = AUTOP::dynamic_pointer_cast(g); if (ppf) { std::cout << "\tSlot # " << slot << ": "; printPointPlaneFactor(ppf); @@ -1834,7 +1875,7 @@ void VioBackEnd::printSelectedFactors( if (print_plane_priors) { const auto& ppp = - boost::dynamic_pointer_cast>( + AUTOP::dynamic_pointer_cast>( g); if (ppp) { std::cout << "\tSlot # " << slot << ": "; @@ -1844,7 +1885,7 @@ void VioBackEnd::printSelectedFactors( if (print_point_priors) { const auto& ppp = - boost::dynamic_pointer_cast>(g); + AUTOP::dynamic_pointer_cast>(g); if (ppp) { std::cout << "\tSlot # " << slot << ": "; printPointPrior(ppp); @@ -1853,7 +1894,7 @@ void VioBackEnd::printSelectedFactors( if (print_linear_container_factors) { const auto& lcf = - boost::dynamic_pointer_cast(g); + AUTOP::dynamic_pointer_cast(g); if (lcf) { std::cout << "\tSlot # " << slot << ": "; printLinearContainerFactor(lcf); @@ -1889,7 +1930,7 @@ void VioBackEnd::computeSmartFactorStatistics() { gtsam::NonlinearFactorGraph graph = smoother_->getFactors(); for (const auto& g : graph) { if (g) { - const auto& gsf = boost::dynamic_pointer_cast(g); + const auto& gsf = AUTOP::dynamic_pointer_cast(g); if (gsf) { debug_info_.numSF_ += 1; @@ -1916,7 +1957,11 @@ void VioBackEnd::computeSmartFactorStatistics() { // Check SF status const gtsam::TriangulationResult& result = gsf->point(); +#ifdef USING_GTSAM4 + if (result.has_value()) { +#else if (result.is_initialized()) { +#endif if (result.degenerate()) debug_info_.numDegenerate_ += 1; if (result.farPoint()) debug_info_.numFarPoints_ += 1; if (result.outlier()) debug_info_.numOutliers_ += 1; @@ -2046,12 +2091,12 @@ void VioBackEnd::deleteAllFactorsWithKeyFromFactorGraph( if ((*it)->find(key) != (*it)->end()) { // We found our lmk in the list of keys of the factor. // Sanity check, this lmk has no priors right? - CHECK(!boost::dynamic_pointer_cast>( + CHECK(!AUTOP::dynamic_pointer_cast>( *it)); // We are not deleting a smart factor right? // Otherwise we need to update structure: // lmk_ids_of_new_smart_factors... - CHECK(!boost::dynamic_pointer_cast(*it)); + CHECK(!AUTOP::dynamic_pointer_cast(*it)); // Whatever factor this is, it has our lmk... // Delete it. LOG(WARNING) << "Delete factor in new_factors at slot # " @@ -2114,17 +2159,17 @@ void VioBackEnd::findSlotsOfFactorsWithKey( CHECK_NOTNULL(slots_of_factors_with_key); slots_of_factors_with_key->resize(0); size_t slot = 0; - for (const boost::shared_ptr& g : graph) { + for (const AUTOP::shared_ptr& g : graph) { if (g) { // Found a valid factor. if (g->find(key) != g->end()) { // Whatever factor this is, it has our lmk... // Sanity check, this lmk has no priors right? - CHECK(!boost::dynamic_pointer_cast(g)); + CHECK(!AUTOP::dynamic_pointer_cast(g)); CHECK( - !boost::dynamic_pointer_cast>(g)); + !AUTOP::dynamic_pointer_cast>(g)); // Sanity check that we are not deleting a smart factor. - CHECK(!boost::dynamic_pointer_cast(g)); + CHECK(!AUTOP::dynamic_pointer_cast(g)); // Delete it. LOG(WARNING) << "Delete factor in graph at slot # " << slot << " corresponding to lmk with id: " diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt index b851589c5..66f60e1f7 100644 --- a/src/common/CMakeLists.txt +++ b/src/common/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/VioNavState.cpp" ) diff --git a/src/dataprovider/CMakeLists.txt b/src/dataprovider/CMakeLists.txt index 4a03df329..0713f8d24 100644 --- a/src/dataprovider/CMakeLists.txt +++ b/src/dataprovider/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface-definitions.cpp" "${CMAKE_CURRENT_LIST_DIR}/DataProviderInterface.cpp" diff --git a/src/dataprovider/EurocDataProvider.cpp b/src/dataprovider/EurocDataProvider.cpp index db656b735..3a42b5c09 100644 --- a/src/dataprovider/EurocDataProvider.cpp +++ b/src/dataprovider/EurocDataProvider.cpp @@ -368,7 +368,16 @@ bool EurocDataProvider::parseGtData(const std::string& input_dataset_path, gt_data_raw[3], gt_data_raw[4], gt_data_raw[5], gt_data_raw[6]); // Sanity check. +#ifdef USING_GTSAM4 + auto qt = rot.toQuaternion(); + gtsam::Vector q(4); + q(0) = qt.w(); + q(1) = qt.x(); + q(2) = qt.y(); + q(3) = qt.z(); +#else gtsam::Vector q = rot.quaternion(); +#endif // Figure out sign for quaternion. if (std::fabs(q(0) + gt_data_raw[3]) < std::fabs(q(0) - gt_data_raw[3])) { q = -q; diff --git a/src/factors/CMakeLists.txt b/src/factors/CMakeLists.txt index f106a8b45..37bfa973b 100644 --- a/src/factors/CMakeLists.txt +++ b/src/factors/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/ParallelPlaneRegularFactor.cpp" "${CMAKE_CURRENT_LIST_DIR}/PointPlaneFactor.cpp" diff --git a/src/factors/PointPlaneFactor.cpp b/src/factors/PointPlaneFactor.cpp index b4a7ab337..cb3b47823 100644 --- a/src/factors/PointPlaneFactor.cpp +++ b/src/factors/PointPlaneFactor.cpp @@ -12,9 +12,11 @@ * Created on: Feb 20, 2018 * Author: Antoni Rosinol */ - +#ifdef USING_GTSAM4 +#include +#endif #include "kimera-vio/factors/PointPlaneFactor.h" - +#include "kimera-vio/definitions.h" using namespace std; namespace gtsam { @@ -28,7 +30,7 @@ void PointPlaneFactor::print(const string& s, } gtsam::NonlinearFactor::shared_ptr PointPlaneFactor::clone() const { - return boost::static_pointer_cast( + return AUTOP::static_pointer_cast( gtsam::NonlinearFactor::shared_ptr(new PointPlaneFactor(*this))); } diff --git a/src/frontend/CMakeLists.txt b/src/frontend/CMakeLists.txt index 70fbd627d..ca38808b0 100644 --- a/src/frontend/CMakeLists.txt +++ b/src/frontend/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Camera.cpp" "${CMAKE_CURRENT_LIST_DIR}/StereoCamera.cpp" diff --git a/src/frontend/Camera.cpp b/src/frontend/Camera.cpp index 243d2c5bf..2373119e5 100644 --- a/src/frontend/Camera.cpp +++ b/src/frontend/Camera.cpp @@ -14,7 +14,7 @@ #include "kimera-vio/frontend/Camera.h" -#include +#include #include diff --git a/src/frontend/StereoCamera.cpp b/src/frontend/StereoCamera.cpp index 4223dae94..e720ce5a6 100644 --- a/src/frontend/StereoCamera.cpp +++ b/src/frontend/StereoCamera.cpp @@ -11,16 +11,18 @@ * @brief Class describing a stereo camera. * @author Antoni Rosinol */ +#ifdef USING_GTSAM4 +#include +#include +#endif #include "kimera-vio/frontend/StereoCamera.h" -#include +#include #include #include -#include // for tie - #include #include #include @@ -33,6 +35,7 @@ #include "kimera-vio/frontend/StereoFrame.h" #include "kimera-vio/frontend/StereoMatchingParams.h" #include "kimera-vio/utils/Macros.h" +#include "kimera-vio/definitions.h" namespace VIO { @@ -74,7 +77,7 @@ StereoCamera::StereoCamera(const CameraParams& left_cam_params, //! Create stereo camera calibration after rectification and undistortion. gtsam::Cal3_S2 left_undist_rect_cam_mat = UtilsOpenCV::Cvmat2Cal3_S2(P1_); stereo_calibration_ = - boost::make_shared(left_undist_rect_cam_mat.fx(), + AUTOP::make_shared(left_undist_rect_cam_mat.fx(), left_undist_rect_cam_mat.fy(), left_undist_rect_cam_mat.skew(), left_undist_rect_cam_mat.px(), @@ -274,7 +277,7 @@ void StereoCamera::computeRectificationParameters( // NOTE: openCV pose convention is the opposite, that's why we have to // invert cv::Mat camL_Rot_camR, camL_Tran_camR; - boost::tie(camL_Rot_camR, camL_Tran_camR) = + AUTOP::tie(camL_Rot_camR, camL_Tran_camR) = UtilsOpenCV::Pose2cvmats(camL_Pose_camR.inverse()); // kAlpha is -1 by default, but that introduces invalid keypoints! diff --git a/src/frontend/StereoFrame.cpp b/src/frontend/StereoFrame.cpp index 3a33b100f..8a9c5bfe2 100644 --- a/src/frontend/StereoFrame.cpp +++ b/src/frontend/StereoFrame.cpp @@ -11,9 +11,11 @@ * @brief Class describing a pair of stereo images * @author Antoni Rosinol, Luca Carlone */ - +#ifdef USING_GTSAM4 +#include +#endif #include "kimera-vio/frontend/StereoFrame.h" - +#include "kimera-vio/definitions.h" #include #include @@ -618,7 +620,7 @@ void StereoFrame::computeRectificationParameters( gtsam::Pose3 camL_Pose_camR = (left_cam_params->body_Pose_cam_) .between(right_cam_params->body_Pose_cam_); // NOTE: openCV pose convention is the opposite, that's why we have to invert - boost::tie(L_Rot_R, L_Tran_R) = + AUTOP::tie(L_Rot_R, L_Tran_R) = UtilsOpenCV::Pose2cvmats(camL_Pose_camR.inverse()); ////////////////////////////////////////////////////////////////////////////// diff --git a/src/frontend/StereoVisionFrontEnd.cpp b/src/frontend/StereoVisionFrontEnd.cpp index 89c651a59..a3481a9ef 100644 --- a/src/frontend/StereoVisionFrontEnd.cpp +++ b/src/frontend/StereoVisionFrontEnd.cpp @@ -53,7 +53,11 @@ StereoVisionFrontEnd::StereoVisionFrontEnd( stereoFrame_k_(nullptr), stereoFrame_km1_(nullptr), stereoFrame_lkf_(nullptr), +#ifdef USING_GTSAM4 + keyframe_R_ref_frame_(gtsam::Rot3::Identity()), +#else keyframe_R_ref_frame_(gtsam::Rot3::identity()), +#endif frame_count_(0), keyframe_count_(0), feature_detector_(nullptr), @@ -448,7 +452,10 @@ StatusStereoMeasurementsPtr StereoVisionFrontEnd::processStereoFrame( // Update keyframe to reference frame for next iteration. if (stereoFrame_k_->isKeyframe()) { // Reset relative rotation if we have a keyframe. - keyframe_R_ref_frame_ = gtsam::Rot3::identity(); +#ifdef USING_GTSAM4 + keyframe_R_ref_frame_ = gtsam::Rot3::Identity(); +#else +#endif } else { // Update rotation from keyframe to next iteration reference frame (aka // cur_frame in current iteration). @@ -474,7 +481,11 @@ void StereoVisionFrontEnd::outlierRejectionMono( TrackingStatusPose* status_pose_mono) { CHECK_NOTNULL(status_pose_mono); if (tracker_.tracker_params_.ransac_use_2point_mono_ && +#ifdef USING_GTSAM4 + !calLrectLkf_R_camLrectKf_imu.equals(gtsam::Rot3::Identity()) && +#else !calLrectLkf_R_camLrectKf_imu.equals(gtsam::Rot3::identity()) && +#endif !force_53point_ransac_) { // 2-point RANSAC. *status_pose_mono = tracker_.geometricOutlierRejectionMonoGivenRotation( @@ -512,7 +523,11 @@ void StereoVisionFrontEnd::outlierRejectionStereo( gtsam::Matrix infoMatStereoTranslation = gtsam::Matrix3::Zero(); if (tracker_.tracker_params_.ransac_use_1point_stereo_ && +#ifdef USING_GTSAM4 + !calLrectLkf_R_camLrectKf_imu.equals(gtsam::Rot3::Identity()) && +#else !calLrectLkf_R_camLrectKf_imu.equals(gtsam::Rot3::identity()) && +#endif !force_53point_ransac_) { // 1-point RANSAC. std::tie(*status_pose_stereo, infoMatStereoTranslation) = diff --git a/src/frontend/Tracker.cpp b/src/frontend/Tracker.cpp index 34cf88a3c..4d4e0983d 100644 --- a/src/frontend/Tracker.cpp +++ b/src/frontend/Tracker.cpp @@ -25,6 +25,7 @@ #include "kimera-vio/utils/Timer.h" #include "kimera-vio/utils/UtilsOpenCV.h" #include "kimera-vio/visualizer/Display-definitions.h" +#include "kimera-vio/definitions.h" DEFINE_bool(visualize_feature_predictions, false, @@ -388,7 +389,11 @@ std::pair Tracker::getPoint3AndCovariance( Matrix3 Jac_point3_sp2; // jacobian of the back projection Vector3 point3_i_gtsam = +#ifdef USING_GTSAM4 + stereoCam.backproject2(stereoPoint, {}, Jac_point3_sp2); +#else stereoCam.backproject2(stereoPoint, boost::none, Jac_point3_sp2).vector(); +#endif Vector3 point3_i = stereoFrame.keypoints_3d_.at(pointId); // TODO(Toni): Adapt value of this threshold for different calibration // models! @@ -432,7 +437,7 @@ Tracker::geometricOutlierRejectionStereoGivenRotation( const gtsam::Cal3_S2& left_undist_rect_cam_mat = ref_stereoFrame.getLeftUndistRectCamMat(); gtsam::Cal3_S2Stereo::shared_ptr K = - boost::make_shared(left_undist_rect_cam_mat.fx(), + AUTOP::make_shared(left_undist_rect_cam_mat.fx(), left_undist_rect_cam_mat.fy(), left_undist_rect_cam_mat.skew(), left_undist_rect_cam_mat.px(), diff --git a/src/frontend/feature-detector/CMakeLists.txt b/src/frontend/feature-detector/CMakeLists.txt index 441f32479..2c0f41de8 100644 --- a/src/frontend/feature-detector/CMakeLists.txt +++ b/src/frontend/feature-detector/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/FeatureDetectorParams.cpp" "${CMAKE_CURRENT_LIST_DIR}/FeatureDetector.cpp" diff --git a/src/frontend/feature-detector/anms/CMakeLists.txt b/src/frontend/feature-detector/anms/CMakeLists.txt index 5792cd35e..6077e5e38 100644 --- a/src/frontend/feature-detector/anms/CMakeLists.txt +++ b/src/frontend/feature-detector/anms/CMakeLists.txt @@ -1,6 +1,6 @@ ### Add source code for IDEs -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/anms.cpp" ) diff --git a/src/imu-frontend/CMakeLists.txt b/src/imu-frontend/CMakeLists.txt index 338206f40..b508ffb75 100644 --- a/src/imu-frontend/CMakeLists.txt +++ b/src/imu-frontend/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for kimera_vio -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEnd.cpp" "${CMAKE_CURRENT_LIST_DIR}/ImuFrontEndParams.cpp" diff --git a/src/imu-frontend/ImuFrontEnd.cpp b/src/imu-frontend/ImuFrontEnd.cpp index e4180006c..ab9fe97fa 100644 --- a/src/imu-frontend/ImuFrontEnd.cpp +++ b/src/imu-frontend/ImuFrontEnd.cpp @@ -12,6 +12,9 @@ * @author Antoni Rosinol * @author Luca Carlone */ +#ifdef USING_GTSAM4 +#include +#endif #include "kimera-vio/imu-frontend/ImuFrontEnd.h" @@ -20,6 +23,7 @@ #include "kimera-vio/common/vio_types.h" #include "kimera-vio/imu-frontend/ImuFrontEnd-definitions.h" #include "kimera-vio/utils/UtilsNumerical.h" +#include "kimera-vio/definitions.h" namespace VIO { @@ -170,11 +174,11 @@ gtsam::PreintegrationBase::Params ImuFrontEnd::convertVioImuParamsToGtsam( return preint_imu_params; } -boost::shared_ptr +AUTOP::shared_ptr ImuFrontEnd::generateCombinedImuParams(const ImuParams& imu_params) { - boost::shared_ptr + AUTOP::shared_ptr combined_imu_params = - boost::make_shared( + AUTOP::make_shared( imu_params.n_gravity_); gtsam::PreintegrationParams gtsam_imu_params = ImuFrontEnd::convertVioImuParamsToGtsam(imu_params); @@ -206,11 +210,11 @@ ImuFrontEnd::generateCombinedImuParams(const ImuParams& imu_params) { return combined_imu_params; } -boost::shared_ptr +AUTOP::shared_ptr ImuFrontEnd::generateRegularImuParams(const ImuParams& imu_params) { - boost::shared_ptr + AUTOP::shared_ptr regular_imu_params = - boost::make_shared( + AUTOP::make_shared( imu_params.n_gravity_); gtsam::PreintegrationParams gtsam_imu_params = ImuFrontEnd::convertVioImuParamsToGtsam(imu_params); diff --git a/src/initial/CMakeLists.txt b/src/initial/CMakeLists.txt index 8abbe3909..f9f239160 100644 --- a/src/initial/CMakeLists.txt +++ b/src/initial/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/InitializationFromImu.cpp" "${CMAKE_CURRENT_LIST_DIR}/InitializationBackEnd.cpp" diff --git a/src/initial/InitializationBackEnd.cpp b/src/initial/InitializationBackEnd.cpp index 12951db04..fbf698bd3 100644 --- a/src/initial/InitializationBackEnd.cpp +++ b/src/initial/InitializationBackEnd.cpp @@ -324,8 +324,12 @@ std::vector InitializationBackEnd::optimizeInitialVisualStates( VLOG(10) << "Levenberg Marquardt optimizer done."; // Query optimized poses in body frame (b0_T_bk) std::vector initial_states; +#ifdef USING_GTSAM4 + for (auto key_value : initial_values) { +#else BOOST_FOREACH (const gtsam::Values::ConstKeyValuePair &key_value, initial_values) { +#endif initial_states.push_back(initial_values.at(key_value.key)); } VLOG(10) << "Initialization values retrieved."; diff --git a/src/logging/CMakeLists.txt b/src/logging/CMakeLists.txt index 9345d5827..1209cd755 100644 --- a/src/logging/CMakeLists.txt +++ b/src/logging/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Logger.cpp" ) diff --git a/src/logging/Logger.cpp b/src/logging/Logger.cpp index 0c4ae1ff9..389050660 100644 --- a/src/logging/Logger.cpp +++ b/src/logging/Logger.cpp @@ -150,7 +150,11 @@ void BackendLogger::logBackendResultsCSV(const BackendOutput& vio_output) { } const auto& cached_state = vio_output.W_State_Blkf_; const auto& w_pose_blkf_trans = cached_state.pose_.translation().transpose(); +#ifdef USING_GTSAM4 + const auto& w_pose_blkf_rot = cached_state.pose_.rotation().toQuaternion(); +#else const auto& w_pose_blkf_rot = cached_state.pose_.rotation().quaternion(); +#endif 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(); @@ -158,10 +162,17 @@ void BackendLogger::logBackendResultsCSV(const BackendOutput& vio_output) { << w_pose_blkf_trans.x() << "," // << w_pose_blkf_trans.y() << "," // << w_pose_blkf_trans.z() << "," // +#ifdef USING_GTSAM4 + << w_pose_blkf_rot.w() << "," // q_w + << w_pose_blkf_rot.x() << "," // q_x + << w_pose_blkf_rot.y() << "," // q_y + << w_pose_blkf_rot.z() << "," // q_z +#else << w_pose_blkf_rot(0) << "," // q_w << w_pose_blkf_rot(1) << "," // q_x << w_pose_blkf_rot(2) << "," // q_y << w_pose_blkf_rot(3) << "," // q_z +#endif << w_vel_blkf(0) << "," // << w_vel_blkf(1) << "," // << w_vel_blkf(2) << "," // diff --git a/src/loopclosure/CMakeLists.txt b/src/loopclosure/CMakeLists.txt index d9f035b75..369fcc261 100644 --- a/src/loopclosure/CMakeLists.txt +++ b/src/loopclosure/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for LoopClosureDetector -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/LoopClosureDetector.cpp" "${CMAKE_CURRENT_LIST_DIR}/LcdThirdPartyWrapper.cpp" diff --git a/src/mesh/CMakeLists.txt b/src/mesh/CMakeLists.txt index ba3ef5029..82f480c07 100644 --- a/src/mesh/CMakeLists.txt +++ b/src/mesh/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Mesh.cpp" "${CMAKE_CURRENT_LIST_DIR}/Mesher.cpp" diff --git a/src/pipeline/CMakeLists.txt b/src/pipeline/CMakeLists.txt index ed4ab6f51..18d50457c 100644 --- a/src/pipeline/CMakeLists.txt +++ b/src/pipeline/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Pipeline.cpp" "${CMAKE_CURRENT_LIST_DIR}/PipelineModule.cpp" diff --git a/src/playground/CMakeLists.txt b/src/playground/CMakeLists.txt index 01426e6fa..80a45cad5 100644 --- a/src/playground/CMakeLists.txt +++ b/src/playground/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/EurocPlayground.cpp" ) diff --git a/src/utils/CMakeLists.txt b/src/utils/CMakeLists.txt index b6e0707e5..5be388a00 100644 --- a/src/utils/CMakeLists.txt +++ b/src/utils/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code for stereoVIO -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/ThreadsafeImuBuffer.cpp" "${CMAKE_CURRENT_LIST_DIR}/Statistics.cpp" diff --git a/src/utils/UtilsOpenCV.cpp b/src/utils/UtilsOpenCV.cpp index cd1c82dfc..e1c86f1af 100644 --- a/src/utils/UtilsOpenCV.cpp +++ b/src/utils/UtilsOpenCV.cpp @@ -373,9 +373,13 @@ std::pair UtilsOpenCV::ComputeRotationAndTranslationErrors( (expectedPose.rotation()).between(actualPose.rotation()); gtsam::Vector3 rotErrorVector = gtsam::Rot3::Logmap(rotErrorMat); double rotError = rotErrorVector.norm(); - +#ifdef USING_GTSAM4 + gtsam::Vector3 actualTranslation = actualPose.translation(); + gtsam::Vector3 expectedTranslation = expectedPose.translation(); +#else gtsam::Vector3 actualTranslation = actualPose.translation().vector(); gtsam::Vector3 expectedTranslation = expectedPose.translation().vector(); +#endif if (upToScale) { double normExpected = expectedTranslation.norm(); double normActual = actualTranslation.norm(); diff --git a/src/visualizer/CMakeLists.txt b/src/visualizer/CMakeLists.txt index 8fcfced4f..73ab10483 100644 --- a/src/visualizer/CMakeLists.txt +++ b/src/visualizer/CMakeLists.txt @@ -1,5 +1,5 @@ ### Add source code -target_sources(kimera_vio +target_sources(kimera_vio${ILLIXR_BUILD_SUFFIX} PRIVATE "${CMAKE_CURRENT_LIST_DIR}/Visualizer3D.cpp" "${CMAKE_CURRENT_LIST_DIR}/Visualizer3DModule.cpp" diff --git a/src/visualizer/OpenCvVisualizer3D.cpp b/src/visualizer/OpenCvVisualizer3D.cpp index bab1f821f..463e73ee5 100644 --- a/src/visualizer/OpenCvVisualizer3D.cpp +++ b/src/visualizer/OpenCvVisualizer3D.cpp @@ -31,6 +31,7 @@ #include "kimera-vio/utils/Timer.h" #include "kimera-vio/utils/UtilsGTSAM.h" #include "kimera-vio/utils/UtilsOpenCV.h" +#include "kimera-vio/definitions.h" #include "kimera-vio/factors/PointPlaneFactor.h" // For visualization of constraints. @@ -209,7 +210,7 @@ VisualizerOutput::UniquePtr OpenCvVisualizer3D::spinOnce( LandmarkIds lmk_ids_in_current_pp_factors; for (const auto& g : input.backend_output_->graph_) { const auto& ppf = - boost::dynamic_pointer_cast(g); + AUTOP::dynamic_pointer_cast(g); if (ppf) { // We found a PointPlaneFactor. // Get point key. diff --git a/tests/testFrame.cpp b/tests/testFrame.cpp index 0172e1339..151c1a317 100644 --- a/tests/testFrame.cpp +++ b/tests/testFrame.cpp @@ -126,7 +126,11 @@ TEST(testFrame, CalibratePixel) { camParams.calibration_.uncalibrate(Point2(versor(0), versor(1))); Point2 uncalibrated_px_expected = Point2(iter->x, iter->y); Point2 px_mismatch = uncalibrated_px_actual - uncalibrated_px_expected; +#ifdef USING_GTSAM4 + ASSERT_TRUE(px_mismatch.norm() < 0.5); +#else ASSERT_TRUE(px_mismatch.vector().norm() < 0.5); +#endif } } @@ -164,7 +168,11 @@ TEST(testFrame, DISABLED_CalibratePixel) { camParams.calibration_.uncalibrate(Point2(versor(0), versor(1))); Point2 uncalibrated_px_expected = Point2(iter->x, iter->y); Point2 px_mismatch = uncalibrated_px_actual - uncalibrated_px_expected; +#ifdef USING_GTSAM4 + ASSERT_TRUE(px_mismatch.norm() < 0.5); +#else ASSERT_TRUE(px_mismatch.vector().norm() < 0.5); +#endif } } diff --git a/tests/testGeneralParallelPlaneRegularBasicFactor.cpp b/tests/testGeneralParallelPlaneRegularBasicFactor.cpp index 8c4436a34..0ac222d06 100644 --- a/tests/testGeneralParallelPlaneRegularBasicFactor.cpp +++ b/tests/testGeneralParallelPlaneRegularBasicFactor.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -131,19 +131,19 @@ TEST(testGeneralParallelPlaneRegularFactor, Jacobians) { /// Use the factor to calculate the Jacobians. gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual); + factor.evaluateError(plane_1, plane_2, &H1Actual, &H2Actual); /// Calculate numerical derivatives. Matrix H1Expected = numericalDerivative21( - boost::bind(&GeneralParallelPlaneRegularBasicFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind(boost::mem_fn(&GeneralParallelPlaneRegularBasicFactor::evaluateError), + &factor, boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); Matrix H2Expected = numericalDerivative22( - boost::bind(&GeneralParallelPlaneRegularBasicFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind(boost::mem_fn(&GeneralParallelPlaneRegularBasicFactor::evaluateError), + &factor, boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); /// Verify the Jacobians are correct. diff --git a/tests/testGeneralParallelPlaneRegularTangentSpaceFactor.cpp b/tests/testGeneralParallelPlaneRegularTangentSpaceFactor.cpp index 5dbc02056..d7d9fdb89 100644 --- a/tests/testGeneralParallelPlaneRegularTangentSpaceFactor.cpp +++ b/tests/testGeneralParallelPlaneRegularTangentSpaceFactor.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -130,21 +130,21 @@ TEST(testGeneralParallelPlaneRegularTangentSpaceFactor, Jacobians) { /// Use the factor to calculate the Jacobians. gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual); + factor.evaluateError(plane_1, plane_2, &H1Actual, &H2Actual); /// Calculate numerical derivatives. Matrix H1Expected = numericalDerivative21( - boost::bind( - &GeneralParallelPlaneRegularTangentSpaceFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind( + boost::mem_fn(&GeneralParallelPlaneRegularTangentSpaceFactor::evaluateError), + boost::ref(factor), boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); Matrix H2Expected = numericalDerivative22( - boost::bind( - &GeneralParallelPlaneRegularTangentSpaceFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind( + boost::mem_fn(&GeneralParallelPlaneRegularTangentSpaceFactor::evaluateError), + &factor, boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); /// Verify the Jacobians are correct. diff --git a/tests/testImuFrontEnd.cpp b/tests/testImuFrontEnd.cpp index 83ea9c722..e0b1f2b5f 100644 --- a/tests/testImuFrontEnd.cpp +++ b/tests/testImuFrontEnd.cpp @@ -63,7 +63,7 @@ TEST(ImuFrontEnd, UpdateBias) { imu_frontend.updateBias(updated_imu_bias); EXPECT_TRUE(imu_frontend.getCurrentImuBias().equals(updated_imu_bias)); // Do some random composition with itself. - updated_imu_bias = imu_bias.compose(imu_bias); + updated_imu_bias = imu_bias + imu_bias; imu_frontend.updateBias(updated_imu_bias); EXPECT_TRUE(imu_frontend.getCurrentImuBias().equals(updated_imu_bias)); // Check that the updated bias does not reset pim! @@ -147,7 +147,7 @@ TEST(ImuFrontEnd, ResetPreintegration) { EXPECT_TRUE(reseted_pim->equals(*curr_pim, 1e-8)); // If we update the biases, the new PIM should reflect that. - ImuBias updated_imu_bias = imu_bias.compose(imu_bias); // Random composition. + ImuBias updated_imu_bias = imu_bias + imu_bias; // Random composition. imu_frontend.updateBias(updated_imu_bias); imu_frontend.resetIntegrationWithCachedBias(); reseted_pim = imu_frontend.preintegrateImuMeasurements(imu_timestamps, diff --git a/tests/testParallelPlaneRegularBasicFactor.cpp b/tests/testParallelPlaneRegularBasicFactor.cpp index 523070d99..3cf4509d5 100644 --- a/tests/testParallelPlaneRegularBasicFactor.cpp +++ b/tests/testParallelPlaneRegularBasicFactor.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -122,19 +122,19 @@ TEST(testParallelPlaneRegularFactor, Jacobians) { // Use the factor to calculate the Jacobians gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual); + factor.evaluateError(plane_1, plane_2, &H1Actual, &H2Actual); // Calculate numerical derivatives Matrix H1Expected = numericalDerivative21( - boost::bind(&ParallelPlaneRegularBasicFactor::evaluateError, &factor, - _1, _2, boost::none, boost::none), + boost::bind(boost::mem_fn(&ParallelPlaneRegularBasicFactor::evaluateError), + &factor, boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); Matrix H2Expected = numericalDerivative22( - boost::bind(&ParallelPlaneRegularBasicFactor::evaluateError, &factor, - _1, _2, boost::none, boost::none), + boost::bind(boost::mem_fn(&ParallelPlaneRegularBasicFactor::evaluateError), + &factor, boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); // Verify the Jacobians are correct diff --git a/tests/testParallelPlaneRegularTangentSpaceFactor.cpp b/tests/testParallelPlaneRegularTangentSpaceFactor.cpp index 1790b7866..41095678b 100644 --- a/tests/testParallelPlaneRegularTangentSpaceFactor.cpp +++ b/tests/testParallelPlaneRegularTangentSpaceFactor.cpp @@ -20,10 +20,11 @@ #include #include -#include +#include #include #include +#include #include #include #include @@ -122,19 +123,20 @@ TEST(testParallelPlaneRegularTangentSPaceFactor, Jacobians) { // Use the factor to calculate the Jacobians gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(plane_1, plane_2, H1Actual, H2Actual); + factor.evaluateError(plane_1, plane_2, &H1Actual, &H2Actual); // Calculate numerical derivatives Matrix H1Expected = numericalDerivative21( - boost::bind(&ParallelPlaneRegularTangentSpaceFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind(boost::mem_fn(&ParallelPlaneRegularTangentSpaceFactor::evaluateError), + boost::ref(factor), boost::placeholders::_1, boost::placeholders::_2, + ParallelPlaneRegularTangentSpaceFactor::OptionalMatrixTypeT(), gtsam::OptionalMatrixTypeT()), plane_1, plane_2, der_tol); Matrix H2Expected = numericalDerivative22( - boost::bind(&ParallelPlaneRegularTangentSpaceFactor::evaluateError, - &factor, _1, _2, boost::none, boost::none), + boost::bind(&ParallelPlaneRegularTangentSpaceFactor::evaluateError, + boost::ref(factor), boost::placeholders::_1, boost::placeholders::_2), plane_1, plane_2, der_tol); // Verify the Jacobians are correct diff --git a/tests/testPointPlaneFactor.cpp b/tests/testPointPlaneFactor.cpp index 1d39751b1..a1651430a 100644 --- a/tests/testPointPlaneFactor.cpp +++ b/tests/testPointPlaneFactor.cpp @@ -17,10 +17,11 @@ #include #include #include +#include #include #include -#include +#include #include @@ -67,8 +68,8 @@ void setIsam2Params(const BackendParams& vio_params, } // Here there was commented code about setRelinearizeThreshold. - isam_param->setCacheLinearizedFactors(false); - isam_param->setEvaluateNonlinearError(true); + isam_param->cacheLinearizedFactors = false; + isam_param->evaluateNonlinearError = true; isam_param->relinearizeThreshold = vio_params.relinearizeThreshold_; isam_param->relinearizeSkip = vio_params.relinearizeSkip_; // isam_param->enablePartialRelinearizationCheck = true; @@ -152,17 +153,15 @@ TEST(testPointPlaneFactor, Jacobians) { // Use the factor to calculate the Jacobians gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(point, plane, H1Actual, H2Actual); + factor.evaluateError(point, plane, &H1Actual, &H2Actual); // Calculate numerical derivatives Matrix H1Expected = numericalDerivative21( - boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2, - boost::none, boost::none), + boost::bind(boost::mem_fn(&PointPlaneFactor::evaluateError), boost::ref(factor), boost::placeholders::_1, boost::placeholders::_2, {}, {}), point, plane, delta_value); Matrix H2Expected = numericalDerivative22( - boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2, - boost::none, boost::none), + boost::bind(&PointPlaneFactor::evaluateError, boost::ref(factor), boost::placeholders::_1, boost::placeholders::_2), point, plane, delta_value); // Verify the Jacobians are correct @@ -192,16 +191,16 @@ TEST(testPointPlaneFactor, JacobiansNegative) { // Use the factor to calculate the Jacobians gtsam::Matrix H1Actual, H2Actual; - factor.evaluateError(point, plane, H1Actual, H2Actual); + factor.evaluateError(point, plane, &H1Actual, &H2Actual); // Calculate numerical derivatives Matrix H1Expected = numericalDerivative21( - boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2, + boost::bind(&PointPlaneFactor::evaluateError, &factor, boost::placeholders::_1, boost::placeholders::_2, boost::none, boost::none), point, plane, delta_value); Matrix H2Expected = numericalDerivative22( - boost::bind(&PointPlaneFactor::evaluateError, &factor, _1, _2, + boost::bind(&PointPlaneFactor::evaluateError, &factor, boost::placeholders::_1, boost::placeholders::_2, boost::none, boost::none), point, plane, delta_value); @@ -384,22 +383,22 @@ TEST(testPointPlaneFactor, MultiplePlanesIncrementalOptimization) { noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)); Point3 priorMean1(0.0, 0.0, 1.0); // prior at origin graph.push_back( - boost::make_shared>(1, priorMean1, priorNoise)); + std::make_shared>(1, priorMean1, priorNoise)); Point3 priorMean2(1.0, 0.0, 1.0); // prior at origin graph.push_back( - boost::make_shared>(2, priorMean2, priorNoise)); + std::make_shared>(2, priorMean2, priorNoise)); Point3 priorMean3(0.0, 1.0, 1.0); // prior at origin graph.push_back( - boost::make_shared>(3, priorMean3, priorNoise)); + std::make_shared>(3, priorMean3, priorNoise)); noiseModel::Isotropic::shared_ptr regularityNoise = noiseModel::Isotropic::Sigma(1, 0.5); graph.push_back( - boost::make_shared(1, 4, regularityNoise)); + std::make_shared(1, 4, regularityNoise)); graph.push_back( - boost::make_shared(2, 4, regularityNoise)); + std::make_shared(2, 4, regularityNoise)); graph.push_back( - boost::make_shared(3, 4, regularityNoise)); + std::make_shared(3, 4, regularityNoise)); Values initial; initial.insert(1, Point3(0.0, 19.0, 3.0)); initial.insert(2, Point3(-1.0, 2.0, 2.0)); @@ -459,23 +458,23 @@ TEST(testPointPlaneFactor, MultiplePlanesIncrementalOptimization) { // Add new plane. Point3 priorMeanA(0.0, 0.0, 2.0); // prior at origin graph.push_back( - boost::make_shared>(5, priorMeanA, priorNoise)); + std::make_shared>(5, priorMeanA, priorNoise)); Point3 priorMeanB(1.0, 0.0, 2.0); // prior at origin graph.push_back( - boost::make_shared>(6, priorMeanB, priorNoise)); + std::make_shared>(6, priorMeanB, priorNoise)); Point3 priorMeanC(0.0, 1.0, 2.0); // prior at origin graph.push_back( - boost::make_shared>(7, priorMeanC, priorNoise)); + std::make_shared>(7, priorMeanC, priorNoise)); // graph.emplace_shared(5, 8, regularityNoise); // graph.emplace_shared(6, 8, regularityNoise); // graph.emplace_shared(7, 8, regularityNoise); graph.push_back( - boost::make_shared(5, 8, regularityNoise)); + std::make_shared(5, 8, regularityNoise)); graph.push_back( - boost::make_shared(6, 8, regularityNoise)); + std::make_shared(6, 8, regularityNoise)); graph.push_back( - boost::make_shared(7, 8, regularityNoise)); + std::make_shared(7, 8, regularityNoise)); initial.clear(); initial.insert(5, Point3(0.0, 19.0, 2.0)); diff --git a/tests/testTracker.cpp b/tests/testTracker.cpp index 7b8fca963..42705d9e6 100644 --- a/tests/testTracker.cpp +++ b/tests/testTracker.cpp @@ -57,7 +57,7 @@ class TestTracker : public ::testing::Test { InitializeData(); // Initialize tracker. - tracker_ = VIO::make_unique(tracker_params_, CameraParams()); + tracker_ = VIO::make_unique(tracker_params_, CameraParams()); } protected: @@ -497,7 +497,7 @@ class TestTracker : public ::testing::Test { const StereoCamera stereoCam, const StereoPoint2 stereoPoint, const Matrix3 stereoPtCov) { - Vector3 meanVector = stereoCam.backproject2(stereoPoint).vector(); + Vector3 meanVector = stereoCam.backproject2(stereoPoint); Vector3 sampleMean = Vector3::Zero(); default_random_engine generator; @@ -516,7 +516,7 @@ class TestTracker : public ::testing::Test { stereoPoint.uR() + noiseUR, stereoPoint.v() + noiseV); - Vector3 sample_i = stereoCam.backproject2(perturbedStereoPoint).vector(); + Vector3 sample_i = stereoCam.backproject2(perturbedStereoPoint); sampleMean += sample_i; sampleCovariance += (sample_i - meanVector) * (sample_i - meanVector).transpose(); @@ -529,7 +529,7 @@ class TestTracker : public ::testing::Test { protected: // Perform Ransac FrontendParams tracker_params_; - std::unique_ptr tracker_; + std::unique_ptr tracker_; }; /* ************************************************************************* */ @@ -603,7 +603,7 @@ TEST_F(TestTracker, geometricOutlierRejectionMono) { trackerParams.ransac_max_iterations_ = 1000; // trackerParams.ransac_probability_ = 0.8; trackerParams.ransac_randomize_ = false; - Tracker tracker(trackerParams, CameraParams()); + VIO::Tracker tracker(trackerParams, CameraParams()); TrackingStatus tracking_status; Pose3 estimated_pose; tie(tracking_status, estimated_pose) = @@ -792,7 +792,7 @@ TEST_F(TestTracker, geometricOutlierRejectionStereo) { FrontendParams trackerParams; trackerParams.ransac_threshold_stereo_ = 0.3; - Tracker tracker(trackerParams, CameraParams()); + VIO::Tracker tracker(trackerParams, CameraParams()); TrackingStatus tracking_status; Pose3 estimated_pose; tie(tracking_status, estimated_pose) = @@ -1013,7 +1013,7 @@ TEST_F(TestTracker, getPoint3AndCovariance) { StereoPoint2 stereoPoint(xL, xR, v); // create a 3D point in front of the camera - Vector3 point3 = stereoCam.backproject2(stereoPoint).vector(); + Vector3 point3 = stereoCam.backproject2(stereoPoint); int pointId = 0; // only point Matrix3 stereoPtCov = Matrix3::Identity(); @@ -1026,7 +1026,7 @@ TEST_F(TestTracker, getPoint3AndCovariance) { // use function to get actual answer Vector3 f_ref_i_expected, f_ref_i_actual; Matrix3 cov_ref_i_expected, cov_ref_i_actual; - tie(f_ref_i_actual, cov_ref_i_actual) = Tracker::getPoint3AndCovariance( + tie(f_ref_i_actual, cov_ref_i_actual) = VIO::Tracker::getPoint3AndCovariance( *ref_stereo_frame, stereoCam, pointId, stereoPtCov); // use monte carlo method to get expected answer @@ -1071,7 +1071,7 @@ TEST_F(TestTracker, findOutliers) { random_shuffle(inliers.begin(), inliers.end()); vector outliers_actual; - Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); + VIO::Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); // check that outliers_actual matches outliers_expected EXPECT_EQ(outliers_expected.size(), outliers_actual.size()); @@ -1099,7 +1099,7 @@ TEST_F(TestTracker, findOutliers) { vector> matches_ref_cur(num_outliers); vector outliers_actual; - Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); + VIO::Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); // check that outliers_actual matches outliers_expected EXPECT_EQ(outliers_expected.size(), outliers_actual.size()); @@ -1126,7 +1126,7 @@ TEST_F(TestTracker, findOutliers) { random_shuffle(inliers.begin(), inliers.end()); vector outliers_actual; - Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); + VIO::Tracker::findOutliers(matches_ref_cur, inliers, &outliers_actual); // check that outliers_actual matches outliers_expected EXPECT_EQ(outliers_actual.size(), 0); @@ -1176,7 +1176,7 @@ TEST_F(TestTracker, FindMatchingKeypoints) { random_shuffle(cur_frame->landmarks_.begin(), cur_frame->landmarks_.end()); vector> matches_ref_cur; - Tracker::findMatchingKeypoints(*ref_frame, *cur_frame, &matches_ref_cur); + VIO::Tracker::findMatchingKeypoints(*ref_frame, *cur_frame, &matches_ref_cur); // Check the correctness of matches_ref_cur EXPECT_EQ(matches_ref_cur.size(), num_landmarks_common); @@ -1261,7 +1261,7 @@ TEST_F(TestTracker, FindMatchingStereoKeypoints) { } vector> matches_ref_cur; - Tracker::findMatchingStereoKeypoints( + VIO::Tracker::findMatchingStereoKeypoints( *ref_stereo_frame, *cur_stereo_frame, &matches_ref_cur); // Check the correctness! diff --git a/tests/testUtilsOpenCV.cpp b/tests/testUtilsOpenCV.cpp index b48737bfb..2799285c7 100644 --- a/tests/testUtilsOpenCV.cpp +++ b/tests/testUtilsOpenCV.cpp @@ -539,8 +539,8 @@ TEST_F(UtilsOpenCVFixture, computeRTErrors_upToScale) { { // translation of new pose is the same of the expected, but with unit norm - double normT = pose_gtsam_.translation().vector().norm(); - Point3 T_new = Point3(pose_gtsam_.translation().vector() / normT); + double normT = pose_gtsam_.translation().norm(); + Point3 T_new = Point3(pose_gtsam_.translation() / normT); double rot_error_expected = 0.37416573867; double tran_error_expected = 0.0; @@ -558,8 +558,8 @@ TEST_F(UtilsOpenCVFixture, computeRTErrors_upToScale) { { // translation of new pose is the same of the expected, but with some other // norm - double normT = pose_gtsam_.translation().vector().norm(); - Point3 T_new = 10 * Point3(pose_gtsam_.translation().vector() / normT); + double normT = pose_gtsam_.translation().norm(); + Point3 T_new = 10 * Point3(pose_gtsam_.translation() / normT); double rot_error_expected = 0.37416573867; double tran_error_expected = 0.0;