From 1dfbc49e921738065073048afd44d9c6e99f51cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=A1bor=20K=C3=B6nyvesi?= Date: Sun, 8 May 2022 18:03:35 +0200 Subject: [PATCH 1/5] Almost compiles, except: time, filesystem --- .gitignore | 1 + CMakeLists.txt | 70 +- cmake/FindEigen3.cmake | 81 -- cmake/FindLibZip.cmake | 37 - cmake/FindSuiteParse.cmake | 128 --- conanfile.txt | 15 + src/GTSAMIntegration/Sim3GTSAM.h | 8 +- src/IMU/BAIMULogic.cpp | 18 +- src/IMU/CoarseIMULogic.cpp | 2 +- src/IMU/CoarseIMULogic.h | 1 - src/IMU/IMUIntegration.cpp | 6 +- src/IMU/IMUIntegration.hpp | 9 +- .../IMUInitializerTransitions.h | 8 +- src/dso/FullSystem/CoarseInitializer.cpp | 2 +- src/dso/FullSystem/CoarseTracker.cpp | 2 +- src/dso/FullSystem/FullSystem.cpp | 60 +- src/dso/FullSystem/FullSystem.h | 6 +- src/dso/FullSystem/HessianBlocks.cpp | 4 +- src/dso/FullSystem/Residuals.h | 4 + .../IOWrapper/Pangolin/KeyFrameDisplay.cpp | 2 +- src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h | 2 +- .../IOWrapper/Pangolin/PangolinDSOViewer.cpp | 2 +- .../IOWrapper/Pangolin/PangolinDSOViewer.h | 2 +- .../AccumulatedTopHessian.cpp | 2 +- .../OptimizationBackend/EnergyFunctional.cpp | 2 +- .../EnergyFunctionalStructs.cpp | 2 +- .../EnergyFunctionalStructs.h | 2 +- .../OptimizationBackend/MatrixAccumulators.h | 2 +- src/dso/util/DatasetReader.h | 2 +- src/main_dmvio_dataset.cpp | 16 +- src/util/GTData.hpp | 5 +- thirdparty/Sophus/CMakeLists.txt | 83 -- thirdparty/Sophus/FindEigen3.cmake | 81 -- thirdparty/Sophus/README | 19 - thirdparty/Sophus/SophusConfig.cmake.in | 11 - .../Sophus/cmake_modules/FindEigen3.cmake | 26 - thirdparty/Sophus/sophus/rxso3.hpp | 838 --------------- thirdparty/Sophus/sophus/se2.hpp | 907 ---------------- thirdparty/Sophus/sophus/se3.hpp | 947 ----------------- thirdparty/Sophus/sophus/sim3.hpp | 976 ------------------ thirdparty/Sophus/sophus/so2.hpp | 701 ------------- thirdparty/Sophus/sophus/so3.hpp | 811 --------------- thirdparty/Sophus/sophus/sophus.hpp | 77 -- thirdparty/Sophus/sophus/test_rxso3.cpp | 92 -- thirdparty/Sophus/sophus/test_se2.cpp | 90 -- thirdparty/Sophus/sophus/test_se3.cpp | 105 -- thirdparty/Sophus/sophus/test_sim3.cpp | 109 -- thirdparty/Sophus/sophus/test_so2.cpp | 91 -- thirdparty/Sophus/sophus/test_so3.cpp | 84 -- thirdparty/Sophus/sophus/tests.hpp | 264 ----- 50 files changed, 143 insertions(+), 6672 deletions(-) create mode 100644 .gitignore delete mode 100644 cmake/FindEigen3.cmake delete mode 100644 cmake/FindLibZip.cmake delete mode 100644 cmake/FindSuiteParse.cmake create mode 100644 conanfile.txt delete mode 100644 thirdparty/Sophus/CMakeLists.txt delete mode 100644 thirdparty/Sophus/FindEigen3.cmake delete mode 100644 thirdparty/Sophus/README delete mode 100644 thirdparty/Sophus/SophusConfig.cmake.in delete mode 100644 thirdparty/Sophus/cmake_modules/FindEigen3.cmake delete mode 100644 thirdparty/Sophus/sophus/rxso3.hpp delete mode 100644 thirdparty/Sophus/sophus/se2.hpp delete mode 100644 thirdparty/Sophus/sophus/se3.hpp delete mode 100644 thirdparty/Sophus/sophus/sim3.hpp delete mode 100644 thirdparty/Sophus/sophus/so2.hpp delete mode 100644 thirdparty/Sophus/sophus/so3.hpp delete mode 100644 thirdparty/Sophus/sophus/sophus.hpp delete mode 100644 thirdparty/Sophus/sophus/test_rxso3.cpp delete mode 100644 thirdparty/Sophus/sophus/test_se2.cpp delete mode 100644 thirdparty/Sophus/sophus/test_se3.cpp delete mode 100644 thirdparty/Sophus/sophus/test_sim3.cpp delete mode 100644 thirdparty/Sophus/sophus/test_so2.cpp delete mode 100644 thirdparty/Sophus/sophus/test_so3.cpp delete mode 100644 thirdparty/Sophus/sophus/tests.hpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..68df772 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +/conan/ \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cea4e4..ad82dc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,24 +1,20 @@ SET(PROJECT_NAME DMVIO) PROJECT(${PROJECT_NAME}) -CMAKE_MINIMUM_REQUIRED(VERSION 2.6) +CMAKE_MINIMUM_REQUIRED(VERSION 3.10) -IF(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE RelWithDebInfo) -ENDIF() -set(BUILD_TYPE RelWithDebInfo) - set(EXECUTABLE_OUTPUT_PATH bin) set(LIBRARY_OUTPUT_PATH lib) -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) # required libraries -find_package(SuiteParse REQUIRED) +#find_package(SuiteParse REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost COMPONENTS system thread filesystem chrono serialization date_time timer) find_package(GTSAM REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(Sophus REQUIRED) + IF(${Boost_VERSION} GREATER_EQUAL 106500) message("Building with stacktrace support.") @@ -40,23 +36,31 @@ ENDIF(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # flags + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + add_definitions("-DENABLE_SSE") -set(CMAKE_CXX_FLAGS - "${SSE_FLAGS} -std=c++14" -) -set(CMAKE_CXX_FLAGS_DEBUG - "-O2 -g -fno-omit-frame-pointer -DEIGEN_INITIALIZE_MATRICES_WITH_NAN -DDEBUG ${STACKTRACE_DEFINES}" -) -set(CMAKE_CXX_FLAGS_RELWITHDEBINFO - "-O3 -g -fno-omit-frame-pointer ${STACKTRACE_DEFINES}" -) -set(CMAKE_CXX_FLAGS_RELEASE - "-O3 -DNDEBUG" -) if (MSVC) - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc") -endif (MSVC) + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP") + add_definitions("-DNOMINMAX") + add_definitions("-DLEAN_AND_MEAN") +else() + set(CMAKE_CXX_FLAGS + "${SSE_FLAGS}" + ) + set(CMAKE_CXX_FLAGS_DEBUG + "-O2 -g -fno-omit-frame-pointer -DEIGEN_INITIALIZE_MATRICES_WITH_NAN -DDEBUG ${STACKTRACE_DEFINES}" + ) + set(CMAKE_CXX_FLAGS_RELWITHDEBINFO + "-O3 -g -fno-omit-frame-pointer ${STACKTRACE_DEFINES}" + ) + set(CMAKE_CXX_FLAGS_RELEASE + "-O3 -DNDEBUG" + ) +endif() set(DSO_SOURCE_DIR ${PROJECT_SOURCE_DIR}/src/dso) @@ -111,17 +115,11 @@ set(dmvio_SOURCE_FILES src/GTSAMIntegration/AugmentedScatter.cpp ) - include_directories( ${PROJECT_SOURCE_DIR}/src ${PROJECT_SOURCE_DIR}/src/dso - ${PROJECT_SOURCE_DIR}/thirdparty/Sophus - ${PROJECT_SOURCE_DIR}/thirdparty/sse2neon - ${EIGEN3_INCLUDE_DIR} - ${GTSAM_INCLUDE_DIR} ) - # decide if we have pangolin if (Pangolin_FOUND) message("--- found PANGOLIN, compiling with pangolin library.") @@ -167,7 +165,14 @@ endif() # compile main library. include_directories( ${CSPARSE_INCLUDE_DIR} ${CHOLMOD_INCLUDE_DIR} ${YAML_CPP_INCLUDE_DIR}) add_library(dmvio ${dso_SOURCE_FILES} ${dmvio_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SOURCE_FILES}) - +target_link_libraries(dmvio PUBLIC + Eigen3::Eigen + GTSAM::gtsam + Boost::system Boost::thread Boost::filesystem Boost::chrono Boost::serialization Boost::date_time Boost::timer + yaml-cpp::yaml-cpp + Sophus::Sophus + opencv::opencv_highgui + ) if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX set(BOOST_THREAD_LIBRARY boost_thread-mt) @@ -180,8 +185,11 @@ endif() if (OpenCV_FOUND AND Pangolin_FOUND) message("--- compiling dmvio_dataset.") add_executable(dmvio_dataset ${PROJECT_SOURCE_DIR}/src/main_dmvio_dataset.cpp) - set(DMVIO_LINKED_LIBRARIES boost_system cxsparse ${BOOST_THREAD_LIBRARY} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS} gtsam ${YAML_CPP_LIBRARIES} ${STACKTRACE_LIBRARIES}) - target_link_libraries(dmvio_dataset dmvio ${DMVIO_LINKED_LIBRARIES}) + target_link_libraries(dmvio_dataset + dmvio + cxsparse ${STACKTRACE_LIBRARIES} ${LIBZIP_LIBRARY} + ${Pangolin_LIBRARIES} + ) else() message("--- not building dmvio_dataset, since either don't have openCV or Pangolin.") endif() diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake deleted file mode 100644 index 9c546a0..0000000 --- a/cmake/FindEigen3.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) - diff --git a/cmake/FindLibZip.cmake b/cmake/FindLibZip.cmake deleted file mode 100644 index 76ab224..0000000 --- a/cmake/FindLibZip.cmake +++ /dev/null @@ -1,37 +0,0 @@ -# Finds libzip. -# -# This module defines: -# LIBZIP_INCLUDE_DIR_ZIP -# LIBZIP_INCLUDE_DIR_ZIPCONF -# LIBZIP_LIBRARY -# - -find_package(PkgConfig) -pkg_check_modules(PC_LIBZIP QUIET libzip) - -find_path(LIBZIP_INCLUDE_DIR_ZIP - NAMES zip.h - HINTS ${PC_LIBZIP_INCLUDE_DIRS}) - -find_path(LIBZIP_INCLUDE_DIR_ZIPCONF - NAMES zipconf.h - HINTS ${PC_LIBZIP_INCLUDE_DIRS}) - -find_library(LIBZIP_LIBRARY - NAMES zip) - -include(FindPackageHandleStandardArgs) -FIND_PACKAGE_HANDLE_STANDARD_ARGS( - LIBZIP DEFAULT_MSG - LIBZIP_LIBRARY LIBZIP_INCLUDE_DIR_ZIP LIBZIP_INCLUDE_DIR_ZIPCONF) - -set(LIBZIP_VERSION 0) - -if (LIBZIP_INCLUDE_DIR_ZIPCONF) - FILE(READ "${LIBZIP_INCLUDE_DIR_ZIPCONF}/zipconf.h" _LIBZIP_VERSION_CONTENTS) - if (_LIBZIP_VERSION_CONTENTS) - STRING(REGEX REPLACE ".*#define LIBZIP_VERSION \"([0-9.]+)\".*" "\\1" LIBZIP_VERSION "${_LIBZIP_VERSION_CONTENTS}") - endif () -endif () - -set(LIBZIP_VERSION ${LIBZIP_VERSION} CACHE STRING "Version number of libzip") diff --git a/cmake/FindSuiteParse.cmake b/cmake/FindSuiteParse.cmake deleted file mode 100644 index b39eaec..0000000 --- a/cmake/FindSuiteParse.cmake +++ /dev/null @@ -1,128 +0,0 @@ -FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h - PATHS - ${SUITE_SPARSE_ROOT}/include - /usr/include/suitesparse - /usr/include/ufsparse - /opt/local/include/ufsparse - /usr/local/include/ufsparse - /sw/include/ufsparse - ) - -FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod - PATHS - ${SUITE_SPARSE_ROOT}/lib - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - -FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd - PATHS - ${SUITE_SPARSE_ROOT}/lib - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - -FIND_LIBRARY(CAMD_LIBRARY NAMES camd - PATHS - ${SUITE_SPARSE_ROOT}/lib - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - -FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig - PATHS - ${SUITE_SPARSE_ROOT}/lib - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - - -# Different platforms seemingly require linking against different sets of libraries -IF(CYGWIN) - FIND_PACKAGE(PkgConfig) - FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd - PATHS - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - PKG_CHECK_MODULES(LAPACK lapack) - - SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) - -# MacPorts build of the SparseSuite requires linking against extra libraries - -ELSEIF(APPLE) - - FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd - PATHS - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - - FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd - PATHS - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - - FIND_LIBRARY(METIS_LIBRARY NAMES metis - PATHS - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - - SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") -ELSE(APPLE) - SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) -ENDIF(CYGWIN) - -IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) - SET(CHOLMOD_FOUND TRUE) -ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) - SET(CHOLMOD_FOUND FALSE) -ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) - -# Look for csparse; note the difference in the directory specifications! -FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h - PATHS - /usr/include/suitesparse - /usr/include - /opt/local/include - /usr/local/include - /sw/include - /usr/include/ufsparse - /opt/local/include/ufsparse - /usr/local/include/ufsparse - /sw/include/ufsparse - ) - -FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse - PATHS - /usr/lib - /usr/local/lib - /opt/local/lib - /sw/lib - ) - -IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) - SET(CSPARSE_FOUND TRUE) -ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) - SET(CSPARSE_FOUND FALSE) -ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) - diff --git a/conanfile.txt b/conanfile.txt new file mode 100644 index 0000000..c18f2b3 --- /dev/null +++ b/conanfile.txt @@ -0,0 +1,15 @@ +[requires] +eigen/3.4.0 +boost/1.79.0 +yaml-cpp/0.7.0 +gtsam/4.0.3 +opencv/4.5.5 +sophus/1.0.0 + +[generators] +cmake_find_package + +[options] +gtsam:pose3_expmap=True +gtsam:rot3_expmap=True +gtsam:pose3_expmap=True diff --git a/src/GTSAMIntegration/Sim3GTSAM.h b/src/GTSAMIntegration/Sim3GTSAM.h index 0b7eedf..4391694 100644 --- a/src/GTSAMIntegration/Sim3GTSAM.h +++ b/src/GTSAMIntegration/Sim3GTSAM.h @@ -24,13 +24,13 @@ #define DMVIO_SIM3GTSAM_H -#include +#include #include #include #include // Class for Sim(3)-transformations based on Sophus for GTSAM. -class Sim3GTSAM : public gtsam::DerivedValue +class Sim3GTSAM { public: @@ -40,9 +40,9 @@ class Sim3GTSAM : public gtsam::DerivedValue Sim3GTSAM(const Sophus::Sim3d& sim); - void print(const std::string& str) const override; + void print(const std::string& str) const; - size_t dim() const override; + size_t dim() const; Sim3GTSAM retract(const gtsam::Vector& inc) const; diff --git a/src/IMU/BAIMULogic.cpp b/src/IMU/BAIMULogic.cpp index 74bca54..bd690f0 100644 --- a/src/IMU/BAIMULogic.cpp +++ b/src/IMU/BAIMULogic.cpp @@ -45,10 +45,10 @@ using namespace dmvio; using gtsam::Symbol; using gtsam::symbol_shorthand::S, gtsam::symbol_shorthand::G; -constexpr char end = '\n'; +constexpr char lineEnd = '\n'; // Version for flushing always (slower, but useful for crashes). //template -//auto end(std::basic_ostream& os) -> decltype(std::endl(os))& +//auto lineEnd(std::basic_ostream& os) -> decltype(std::endl(os))& //{ // return std::endl(os); //} @@ -233,7 +233,7 @@ void dmvio::BAIMULogic::setNextBAVel(const gtsam::Vector3& velocity, int frameId } void dmvio::BAIMULogic::addKeyframe(BAGraphs* baGraphs, gtsam::Values::shared_ptr baValues, int keyframeId, - const Sophus::SE3& keyframePose, std::vector& frames) + const Sophus::SE3d& keyframePose, std::vector& frames) { if(disableFromKF > 0 && keyframeId > disableFromKF) { @@ -396,7 +396,7 @@ void dmvio::BAIMULogic::acceptUpdate(gtsam::Values::shared_ptr values, gtsam::Va if(optimizeScale && !scaleFixed) { double newScale = transformDSOToIMU->getScale(); - std::cout << "Optimized scale: " << newScale << end; + std::cout << "Optimized scale: " << newScale << lineEnd; if(newScale > maxScaleInterval) { maxScaleInterval = newScale; @@ -409,7 +409,7 @@ void dmvio::BAIMULogic::acceptUpdate(gtsam::Values::shared_ptr values, gtsam::Va if(optimizeIMUExtrinsics) { gtsam::Pose3 newT_cam_imu = newValues->at(Symbol('i', 0)); - std::cout << "Optimized T_cam_imu: " << newT_cam_imu.translation().transpose() << end; + std::cout << "Optimized T_cam_imu: " << newT_cam_imu.translation().transpose() << lineEnd; } } @@ -449,7 +449,7 @@ void dmvio::BAIMULogic::finishKeyframeOperations(int keyframeId) // We also have computed bias covariance. baBiasFile << ' ' << biasCovariance.diagonal().transpose(); } - baBiasFile << end; + baBiasFile << lineEnd; if(optimizeScale && !scaleFixed) { @@ -599,15 +599,15 @@ gtsam::LinearContainerFactor::shared_ptr BAIMULogic::computeFactorForCoarseGraph // Print covariances to file. scaleFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' - << transformDSOToIMU->getScale() << ' ' << uncertainties[0](0, 0) << end; + << transformDSOToIMU->getScale() << ' ' << uncertainties[0](0, 0) << lineEnd; Eigen::Vector3d rotLog = transformDSOToIMU->getR_dsoW_metricW().log(); baGravDirFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' - << rotLog.transpose() << ' ' << uncertainties[1].diagonal().transpose() << end; + << rotLog.transpose() << ' ' << uncertainties[1].diagonal().transpose() << lineEnd; baVelFile << std::fixed << std::setprecision(6) << baIntegration->getCurrBaTimestamp() << ' ' << baIntegration->getBaValues()->at(velKey).transpose() << ' ' - << uncertainties[uncertOrdering.size() + 1].diagonal().transpose() << end; + << uncertainties[uncertOrdering.size() + 1].diagonal().transpose() << lineEnd; // Now marginalize everything in uncertOrdering, because these variables shall not be in the factor for the coarse graph. int secondASize = std::accumulate(factorOrdering.begin(), factorOrdering.end(), 0, accumFun); diff --git a/src/IMU/CoarseIMULogic.cpp b/src/IMU/CoarseIMULogic.cpp index e0d538a..7997fed 100644 --- a/src/IMU/CoarseIMULogic.cpp +++ b/src/IMU/CoarseIMULogic.cpp @@ -370,7 +370,7 @@ Sophus::SE3d dmvio::CoarseIMULogic::getCoarseKFPose() return Sophus::SE3d(coarseValues->at(gtsam::Symbol('p', currentKeyframeId)).matrix()); } -void dmvio::CoarseIMULogic::updateCoarsePose(const Sophus::SE3& refToFrame) +void dmvio::CoarseIMULogic::updateCoarsePose(const Sophus::SE3d& refToFrame) { // GTSAM expects currentImu to world, we passed referenceCamera to currentCamera. PoseTransformation& transformIMUToCoarse = *transformIMUToDSOForCoarse; diff --git a/src/IMU/CoarseIMULogic.h b/src/IMU/CoarseIMULogic.h index ba1cff6..c643b6f 100644 --- a/src/IMU/CoarseIMULogic.h +++ b/src/IMU/CoarseIMULogic.h @@ -28,7 +28,6 @@ #include "IMUSettings.h" #include "BAIMULogic.h" -#include #include #include #include diff --git a/src/IMU/IMUIntegration.cpp b/src/IMU/IMUIntegration.cpp index 02a880d..6703041 100644 --- a/src/IMU/IMUIntegration.cpp +++ b/src/IMU/IMUIntegration.cpp @@ -144,7 +144,7 @@ void IMUIntegration::addIMUDataToBA(const IMUData& imuData) } // returns estimated referenceToFrame. -Sophus::SE3 IMUIntegration::addIMUData(const IMUData& imuData, int frameId, double frameTimestamp, +Sophus::SE3d IMUIntegration::addIMUData(const IMUData& imuData, int frameId, double frameTimestamp, bool firstFrameAfterKFChange, int lastFrameId, bool onlyForHint) { @@ -177,13 +177,13 @@ Sophus::SE3 IMUIntegration::addIMUData(const IMUData& imuData, int frameId, doub } -void IMUIntegration::updateCoarsePose(const Sophus::SE3& refToFrame) +void IMUIntegration::updateCoarsePose(const Sophus::SE3d& refToFrame) { if(!coarseInitialized) return; coarseLogic->updateCoarsePose(refToFrame); } -Sophus::SE3 +Sophus::SE3d IMUIntegration::computeCoarseUpdate(const dso::Mat88& H_in, const dso::Vec8& b_in, float extrapFac, float lambda, double& incA, double& incB, double& incNorm) { diff --git a/src/IMU/IMUIntegration.hpp b/src/IMU/IMUIntegration.hpp index 8553715..40d6da8 100644 --- a/src/IMU/IMUIntegration.hpp +++ b/src/IMU/IMUIntegration.hpp @@ -40,7 +40,6 @@ #include #include -#include #include #include "dso/util/NumType.h" @@ -83,7 +82,7 @@ class IMUIntegration : public PreintegrationProviderBA // Called to add IMU data for the coarse tracking (and the IMU initializer). // Adds a new frame with IMU data to the coarse factor graph, marginalizes old variables, and returns an estimate // for the relative pose of the newly added frame. - Sophus::SE3 addIMUData(const IMUData& imuData, + Sophus::SE3d addIMUData(const IMUData& imuData, int frameId, double frameTimestamp, bool firstFrameAfterKFChange, int lastFrameId, bool onlyForHint = false); @@ -95,7 +94,7 @@ class IMUIntegration : public PreintegrationProviderBA void resetBAPreintegration(); // Passes the new coarse tracking pose. - void updateCoarsePose(const Sophus::SE3& pose); + void updateCoarsePose(const Sophus::SE3d& pose); // This method integrates the CoarseTracker optimization with GTSAM. It is called in each iteration, and // will compute the increment for the optimization iteration. @@ -103,7 +102,7 @@ class IMUIntegration : public PreintegrationProviderBA // increment of the affine lightning transforms after the method call, incNorm is the norm of the increment. // b contains the following parameters: 3 for the rotation ref_to_frame, 3 for the translation ref_to_frame, and // 2 for affine lightning parameters. - Sophus::SE3 computeCoarseUpdate(const dso::Mat88& H, const dso::Vec8& b, float extrapFac, float lambda, + Sophus::SE3d computeCoarseUpdate(const dso::Mat88& H, const dso::Vec8& b, float extrapFac, float lambda, double& incA, double& incB, double& incNorm); // Apply the update computed by the last call of computeCoarseUpdate. @@ -145,7 +144,7 @@ class IMUIntegration : public PreintegrationProviderBA // thread marginalization and post BA stuff -> finishKeyframeOperations() void finishKeyframeOperations(int keyframeId); - Sophus::SE3 TS_cam_imu; + Sophus::SE3d TS_cam_imu; // Sets groundtruth data for a frame for printing out information. Should only be used in non-rt mode as it currently // does not handle multiple threads correctly. diff --git a/src/IMUInitialization/IMUInitializerTransitions.h b/src/IMUInitialization/IMUInitializerTransitions.h index 17ed78e..8dc2a8c 100644 --- a/src/IMUInitialization/IMUInitializerTransitions.h +++ b/src/IMUInitialization/IMUInitializerTransitions.h @@ -53,10 +53,14 @@ class StateTransitionModel virtual IMUInitializerState::unique_ptr marginalizationReplacementReady(std::unique_ptr&& optimizedValues) - {}; + { + return {}; + } virtual IMUInitializerState::unique_ptr marginalizationReplaced() - {}; + { + return {}; + } }; // Enumeration for the available transition models. diff --git a/src/dso/FullSystem/CoarseInitializer.cpp b/src/dso/FullSystem/CoarseInitializer.cpp index 7854794..d966b32 100644 --- a/src/dso/FullSystem/CoarseInitializer.cpp +++ b/src/dso/FullSystem/CoarseInitializer.cpp @@ -41,7 +41,7 @@ #include -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/FullSystem/CoarseTracker.cpp b/src/dso/FullSystem/CoarseTracker.cpp index 0e6d3ae..73579c3 100644 --- a/src/dso/FullSystem/CoarseTracker.cpp +++ b/src/dso/FullSystem/CoarseTracker.cpp @@ -40,7 +40,7 @@ #include #include "util/TimeMeasurement.h" -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/FullSystem/FullSystem.cpp b/src/dso/FullSystem/FullSystem.cpp index 68f5b81..2a6d891 100644 --- a/src/dso/FullSystem/FullSystem.cpp +++ b/src/dso/FullSystem/FullSystem.cpp @@ -269,14 +269,14 @@ void FullSystem::printResult(std::string file, bool onlyLogKFPoses, bool saveMet if(onlyLogKFPoses && s->marginalizedAt == s->id) continue; // firstPose is transformFirstToWorld. We actually want camToFirst here -> - Sophus::SE3 camToWorld = s->camToWorld; + Sophus::SE3d camToWorld = s->camToWorld; // Use camToTrackingReference for nonKFs and the updated camToWorld for KFs. if(useCamToTrackingRef && s->keyframeId == -1) { camToWorld = s->trackingRef->camToWorld * s->camToTrackingRef; } - Sophus::SE3 camToFirst = firstPose.inverse() * camToWorld; + Sophus::SE3d camToFirst = firstPose.inverse() * camToWorld; if(saveMetricPoses) { @@ -295,7 +295,7 @@ void FullSystem::printResult(std::string file, bool onlyLogKFPoses, bool saveMet myfile.close(); } -std::pair FullSystem::trackNewCoarse(FrameHessian* fh, Sophus::SE3 *referenceToFrameHint) +std::pair FullSystem::trackNewCoarse(FrameHessian* fh, Sophus::SE3d *referenceToFrameHint) { dmvio::TimeMeasurement timeMeasurement(referenceToFrameHint ? "FullSystem::trackNewCoarse" : "FullSystem::trackNewCoarseNoIMU"); assert(allFrameHistory.size() > 0); @@ -371,32 +371,32 @@ std::pair FullSystem::trackNewCoarse(FrameHessian* fh, Sophus::SE3 * // also, if tracking rails here we loose, so we really, really want to avoid that. for(float rotDelta=0.02; rotDelta < 0.05; rotDelta++) { - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,0,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,-rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. - lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Sophus::Quaterniond(1,rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,0,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,0,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,-rotDelta,0), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,0,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,0,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,-rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,-rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,-rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,rotDelta,-rotDelta), Vec3(0,0,0))); // assume constant motion. + lastF_2_fh_tries.push_back(fh_2_slast.inverse() * lastF_2_slast * SE3(Eigen::Quaterniond(1,rotDelta,rotDelta,rotDelta), Vec3(0,0,0))); // assume constant motion. } if(!slast->poseValid || !sprelast->poseValid || !lastF->shell->poseValid) @@ -927,7 +927,7 @@ void FullSystem::addActiveFrame(ImageAndExposure* image, int id, dmvio::IMUData* if(setting_useIMU) { imuIntegration.addIMUDataToBA(*imuData); - Sophus::SE3 imuToWorld = gravityInit.addMeasure(*imuData, Sophus::SE3d()); + Sophus::SE3d imuToWorld = gravityInit.addMeasure(*imuData, Sophus::SE3d()); if(initDone) { firstPose = imuToWorld * imuIntegration.TS_cam_imu.inverse(); diff --git a/src/dso/FullSystem/FullSystem.h b/src/dso/FullSystem/FullSystem.h index 6c9dd85..2b1be51 100644 --- a/src/dso/FullSystem/FullSystem.h +++ b/src/dso/FullSystem/FullSystem.h @@ -175,7 +175,7 @@ class FullSystem { public: dmvio::IMUIntegration &getImuIntegration(); - Sophus::SE3 firstPose; // contains transform from first to world. + Sophus::SE3d firstPose; // contains transform from first to world. private: CalibHessian Hcalib; @@ -193,7 +193,7 @@ class FullSystem { double linAllPointSinle(PointHessian* point, float outlierTHSlack, bool plot); // mainPipelineFunctions - std::pair trackNewCoarse(FrameHessian* fh, Sophus::SE3 *referenceToFrameHint = 0); + std::pair trackNewCoarse(FrameHessian* fh, Sophus::SE3d *referenceToFrameHint = 0); void traceNewCoarse(FrameHessian* fh); void activatePoints(); void activatePointsMT(); @@ -271,7 +271,7 @@ class FullSystem { // =================== changed by tracker-thread. protected by trackMutex ============ boost::mutex trackMutex; std::vector allFrameHistory; - std::vector gtPoses; + std::vector gtPoses; CoarseInitializer* coarseInitializer; Vec5 lastCoarseRMSE; diff --git a/src/dso/FullSystem/HessianBlocks.cpp b/src/dso/FullSystem/HessianBlocks.cpp index ef801b7..bfb9704 100644 --- a/src/dso/FullSystem/HessianBlocks.cpp +++ b/src/dso/FullSystem/HessianBlocks.cpp @@ -81,8 +81,8 @@ void FrameHessian::setStateZero(const Vec10 &state_zero) for(int i=0;i<6;i++) { Vec6 eps; eps.setZero(); eps[i] = 1e-3; - SE3 EepsP = Sophus::SE3::exp(eps); - SE3 EepsM = Sophus::SE3::exp(-eps); + SE3 EepsP = Sophus::SE3d::exp(eps); + SE3 EepsM = Sophus::SE3d::exp(-eps); SE3 w2c_leftEps_P_x0 = (get_worldToCam_evalPT() * EepsP) * get_worldToCam_evalPT().inverse(); SE3 w2c_leftEps_M_x0 = (get_worldToCam_evalPT() * EepsM) * get_worldToCam_evalPT().inverse(); nullspaces_pose.col(i) = (w2c_leftEps_P_x0.log() - w2c_leftEps_M_x0.log())/(2e-3); diff --git a/src/dso/FullSystem/Residuals.h b/src/dso/FullSystem/Residuals.h index 73e277a..a12b9a1 100644 --- a/src/dso/FullSystem/Residuals.h +++ b/src/dso/FullSystem/Residuals.h @@ -34,6 +34,10 @@ #include "util/globalFuncs.h" #include "OptimizationBackend/RawResidualJacobian.h" +#ifdef IN +#undef IN +#endif + namespace dso { class PointHessian; diff --git a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp index 0890dc5..0774e57 100644 --- a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp +++ b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.cpp @@ -84,7 +84,7 @@ void KeyFrameDisplay::setFromF(FrameShell* frame, CalibHessian* HCalib) needRefresh=true; } -void KeyFrameDisplay::setFromPose(const Sophus::SE3& pose, CalibHessian *HCalib) +void KeyFrameDisplay::setFromPose(const Sophus::SE3d& pose, CalibHessian *HCalib) { id = 0; fx = HCalib->fxl(); diff --git a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h index cadf66e..01f0512 100644 --- a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h +++ b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h @@ -79,7 +79,7 @@ class KeyFrameDisplay // keeping some additional information so we can render it differently. void setFromF(FrameShell* fs, CalibHessian* HCalib); - void setFromPose(const Sophus::SE3& pose, CalibHessian* HCalib); + void setFromPose(const Sophus::SE3d& pose, CalibHessian* HCalib); // copies & filters internal data to GL buffer for rendering. if nothing to do: does nothing. bool refreshPC(bool canRefresh, float scaledTH, float absTH, int mode, float minBS, int sparsity); diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp index cce378f..a7fbe65 100644 --- a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp @@ -580,7 +580,7 @@ void PangolinDSOViewer::publishTransformDSOToIMU(const dmvio::TransformDSOToIMU& std::make_shared(false), std::make_shared(false)); } -void PangolinDSOViewer::addGTCamPose(const Sophus::SE3& gtPose) +void PangolinDSOViewer::addGTCamPose(const Sophus::SE3d& gtPose) { boost::unique_lock lk(model3DMutex); diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h index 0b31e77..c69e1a3 100644 --- a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h @@ -80,7 +80,7 @@ class PangolinDSOViewer : public Output3DWrapper virtual void publishKeyframes( std::vector &frames, bool final, CalibHessian* HCalib) override; virtual void publishCamPose(FrameShell* frame, CalibHessian* HCalib) override; - void addGTCamPose(const Sophus::SE3& gtPose); + void addGTCamPose(const Sophus::SE3d& gtPose); virtual void pushLiveFrame(FrameHessian* image) override; virtual void pushDepthImage(MinimalImageB3* image) override; diff --git a/src/dso/OptimizationBackend/AccumulatedTopHessian.cpp b/src/dso/OptimizationBackend/AccumulatedTopHessian.cpp index e4f9882..7ad1504 100644 --- a/src/dso/OptimizationBackend/AccumulatedTopHessian.cpp +++ b/src/dso/OptimizationBackend/AccumulatedTopHessian.cpp @@ -27,7 +27,7 @@ #include "OptimizationBackend/EnergyFunctionalStructs.h" #include -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/OptimizationBackend/EnergyFunctional.cpp b/src/dso/OptimizationBackend/EnergyFunctional.cpp index 6c18918..3cda1be 100644 --- a/src/dso/OptimizationBackend/EnergyFunctional.cpp +++ b/src/dso/OptimizationBackend/EnergyFunctional.cpp @@ -33,7 +33,7 @@ #include "OptimizationBackend/AccumulatedSCHessian.h" #include "OptimizationBackend/AccumulatedTopHessian.h" -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp b/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp index 31dc873..271f4f2 100644 --- a/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp +++ b/src/dso/OptimizationBackend/EnergyFunctionalStructs.cpp @@ -28,7 +28,7 @@ #include "FullSystem/HessianBlocks.h" #include "FullSystem/Residuals.h" -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/OptimizationBackend/EnergyFunctionalStructs.h b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h index 149aba6..0fa4a5e 100644 --- a/src/dso/OptimizationBackend/EnergyFunctionalStructs.h +++ b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h @@ -34,7 +34,7 @@ namespace dso { class PointFrameResidual; -class CalibHessian; +struct CalibHessian; class FrameHessian; class PointHessian; diff --git a/src/dso/OptimizationBackend/MatrixAccumulators.h b/src/dso/OptimizationBackend/MatrixAccumulators.h index 28ed13b..4da2571 100644 --- a/src/dso/OptimizationBackend/MatrixAccumulators.h +++ b/src/dso/OptimizationBackend/MatrixAccumulators.h @@ -25,7 +25,7 @@ #pragma once #include "util/NumType.h" -#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) +#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__) && !defined(_WIN64) #include "SSE2NEON.h" #endif diff --git a/src/dso/util/DatasetReader.h b/src/dso/util/DatasetReader.h index de17140..a689491 100644 --- a/src/dso/util/DatasetReader.h +++ b/src/dso/util/DatasetReader.h @@ -34,7 +34,7 @@ #include #include -#include +//#include #include #include "util/Undistort.h" diff --git a/src/main_dmvio_dataset.cpp b/src/main_dmvio_dataset.cpp index 283d795..a30f39b 100644 --- a/src/main_dmvio_dataset.cpp +++ b/src/main_dmvio_dataset.cpp @@ -30,7 +30,7 @@ #include #include #include -#include +//#include #include "IOWrapper/Output3DWrapper.h" #include "IOWrapper/ImageDisplay.h" @@ -60,9 +60,9 @@ std::string source = ""; std::string calib = ""; std::string imuFile = ""; std::string imuCalibFile = ""; -bool reverse = false; +bool g_reverse = false; int start = 0; -int end = 100000; +int g_end = 100000; float playbackSpeed = 0; // 0 for linearize (play as fast as possible, while sequentializing tracking & mapping). otherwise, factor on timestamps. bool preload = false; int maxPreloadImages = 0; // If set we only preload if there are less images to be loade. @@ -201,7 +201,7 @@ void parseArgument(char* arg, dmvio::SettingsUtil& settingsUtil) { if(option == 1) { - reverse = true; + g_reverse = true; printf("REVERSE!\n"); } return; @@ -232,7 +232,7 @@ void parseArgument(char* arg, dmvio::SettingsUtil& settingsUtil) } if(1 == sscanf(arg, "end=%d", &option)) { - end = option; + g_end = option; printf("END AT %d!\n", start); return; } @@ -381,13 +381,13 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) int lstart = start; - int lend = end; + int lend = g_end; int linc = 1; - if(reverse) + if(g_reverse) { assert(!setting_useIMU); // Reverse is not supported with IMU data at the moment! printf("REVERSE!!!!"); - lstart = end - 1; + lstart = g_end - 1; if(lstart >= reader->getNumImages()) lstart = reader->getNumImages() - 1; lend = start; diff --git a/src/util/GTData.hpp b/src/util/GTData.hpp index 4727c43..d4b444e 100644 --- a/src/util/GTData.hpp +++ b/src/util/GTData.hpp @@ -27,7 +27,6 @@ #include -#include #include namespace dmvio @@ -39,13 +38,13 @@ class GTData inline GTData() {} - inline GTData(Sophus::SE3 pose, Eigen::Vector3d velocity, Eigen::Vector3d biasRotation, + inline GTData(Sophus::SE3d pose, Eigen::Vector3d velocity, Eigen::Vector3d biasRotation, Eigen::Vector3d biasTranslation) : pose(pose), velocity(velocity), biasRotation(biasRotation), biasTranslation(biasTranslation) {} - Sophus::SE3 pose; + Sophus::SE3d pose; Eigen::Vector3d velocity; // Note: velocities might be in the vicon frame instead of the world frame... Eigen::Vector3d biasRotation; Eigen::Vector3d biasTranslation; diff --git a/thirdparty/Sophus/CMakeLists.txt b/thirdparty/Sophus/CMakeLists.txt deleted file mode 100644 index 49a5657..0000000 --- a/thirdparty/Sophus/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -SET(PROJECT_NAME Sophus) - -PROJECT(${PROJECT_NAME}) -CMAKE_MINIMUM_REQUIRED(VERSION 2.6) - -SET (CMAKE_VERBOSE_MAKEFILE ON) - -# Release by default -# Turn on Debug with "-DCMAKE_BUILD_TYPE=Debug" -IF( NOT CMAKE_BUILD_TYPE ) - SET( CMAKE_BUILD_TYPE Release ) -ENDIF() - -IF (CMAKE_COMPILER_IS_GNUCXX ) - SET(CMAKE_CXX_FLAGS_DEBUG "-O0 -g") - SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG ") - - ADD_DEFINITIONS("-Wall -Werror -Wno-unused-variable - -Wno-unused-but-set-variable -Wno-unknown-pragmas ") -ENDIF() - -################################################################################ -# Add local path for finding packages, set the local version first -set( CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake_modules" ) -list( APPEND CMAKE_MODULE_PATH "${CMAKE_ROOT}/Modules" ) - -################################################################################ -# Create variables used for exporting in SophusConfig.cmake -set( Sophus_LIBRARIES "" ) -set( Sophus_INCLUDE_DIR ${PROJECT_SOURCE_DIR} ) - -################################################################################ - -include(FindEigen3.cmake) -#find_package( Eigen3 REQUIRED ) -INCLUDE_DIRECTORIES( ${EIGEN3_INCLUDE_DIR} ) -SET( Sophus_INCLUDE_DIR ${Sophus_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ) - -SET (SOURCE_DIR "sophus") - -SET (TEMPLATES tests - so2 - se2 - so3 - se3 - rxso3 - sim3 -) - -SET (SOURCES ${SOURCE_DIR}/sophus.hpp) - -FOREACH(templ ${TEMPLATES}) - LIST(APPEND SOURCES ${SOURCE_DIR}/${templ}.hpp) -ENDFOREACH(templ) - - -INCLUDE_DIRECTORIES(${INCLUDE_DIRS}) - -# Added ${SOURCES} to executables so they show up in QtCreator (and possibly -# other IDEs). -# ADD_EXECUTABLE(test_so2 sophus/test_so2.cpp ${SOURCES}) -# ADD_EXECUTABLE(test_se2 sophus/test_se2.cpp ${SOURCES}) -# ADD_EXECUTABLE(test_so3 sophus/test_so3.cpp ${SOURCES}) -# ADD_EXECUTABLE(test_se3 sophus/test_se3.cpp ${SOURCES}) -# ADD_EXECUTABLE(test_rxso3 sophus/test_rxso3.cpp ${SOURCES}) -# ADD_EXECUTABLE(test_sim3 sophus/test_sim3.cpp ${SOURCES}) -# ENABLE_TESTING() -# -# ADD_TEST(test_so2 test_so2) -# ADD_TEST(test_se2 test_se2) -# ADD_TEST(test_so3 test_so3) -# ADD_TEST(test_se3 test_se3) -# ADD_TEST(test_rxso3 test_rxso3) -# ADD_TEST(test_sim3 test_sim3) - -################################################################################ -# Create the SophusConfig.cmake file for other cmake projects. -CONFIGURE_FILE( ${CMAKE_CURRENT_SOURCE_DIR}/SophusConfig.cmake.in - ${CMAKE_CURRENT_BINARY_DIR}/SophusConfig.cmake @ONLY IMMEDIATE ) -export( PACKAGE Sophus ) - -INSTALL(DIRECTORY sophus DESTINATION ${CMAKE_INSTALL_PREFIX}/include - FILES_MATCHING PATTERN "*.hpp" ) \ No newline at end of file diff --git a/thirdparty/Sophus/FindEigen3.cmake b/thirdparty/Sophus/FindEigen3.cmake deleted file mode 100644 index 9c546a0..0000000 --- a/thirdparty/Sophus/FindEigen3.cmake +++ /dev/null @@ -1,81 +0,0 @@ -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) - diff --git a/thirdparty/Sophus/README b/thirdparty/Sophus/README deleted file mode 100644 index 05213ac..0000000 --- a/thirdparty/Sophus/README +++ /dev/null @@ -1,19 +0,0 @@ -Sophus (version 0.9a) - -C++ implementation of Lie Groups using Eigen. - -Thanks to Steven Lovegrove, Sophus is now fully templated - using the Curiously Recurring Template Pattern (CRTP). - -(In order to go back to the non-templated/double-only version "git checkout a621ff".) - -Installation guide: - ->>> -cd Sophus -mkdir build -cd build -cmake .. -make -<<< - - diff --git a/thirdparty/Sophus/SophusConfig.cmake.in b/thirdparty/Sophus/SophusConfig.cmake.in deleted file mode 100644 index 24ba84f..0000000 --- a/thirdparty/Sophus/SophusConfig.cmake.in +++ /dev/null @@ -1,11 +0,0 @@ -################################################################################ -# Sophus source dir -set( Sophus_SOURCE_DIR "@CMAKE_CURRENT_SOURCE_DIR@") - -################################################################################ -# Sophus build dir -set( Sophus_DIR "@CMAKE_CURRENT_BINARY_DIR@") - -################################################################################ -set( Sophus_INCLUDE_DIR "@Sophus_INCLUDE_DIR@" ) -set( Sophus_INCLUDE_DIRS "@Sophus_INCLUDE_DIR@" ) \ No newline at end of file diff --git a/thirdparty/Sophus/cmake_modules/FindEigen3.cmake b/thirdparty/Sophus/cmake_modules/FindEigen3.cmake deleted file mode 100644 index 469c77d..0000000 --- a/thirdparty/Sophus/cmake_modules/FindEigen3.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# - Try to find Eigen3 lib -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib -# EIGEN3_INCLUDE_DIR - the eigen include directory - -# Copyright (c) 2006, 2007 Montel Laurent, -# Redistribution and use is allowed according to the terms of the BSD license. -# For details see the accompanying COPYING-CMAKE-SCRIPTS file. - -if( EIGEN3_INCLUDE_DIR ) - # in cache already - set( EIGEN3_FOUND TRUE ) -else (EIGEN3_INCLUDE_DIR) - find_path( EIGEN3_INCLUDE_DIR NAMES Eigen/Core - PATH_SUFFIXES eigen3/ - HINTS - ${INCLUDE_INSTALL_DIR} - /usr/local/include - ${KDE4_INCLUDE_DIR} - ) - include( FindPackageHandleStandardArgs ) - find_package_handle_standard_args( Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR ) - mark_as_advanced( EIGEN3_INCLUDE_DIR ) -endif(EIGEN3_INCLUDE_DIR) - diff --git a/thirdparty/Sophus/sophus/rxso3.hpp b/thirdparty/Sophus/sophus/rxso3.hpp deleted file mode 100644 index a0262a0..0000000 --- a/thirdparty/Sophus/sophus/rxso3.hpp +++ /dev/null @@ -1,838 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_RXSO3_HPP -#define RXSO3_HPP - -#include "sophus.hpp" -#include "so3.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class RxSO3Group; -typedef RxSO3Group ScSO3 EIGEN_DEPRECATED; -typedef RxSO3Group RxSO3d; /**< double precision RxSO3 */ -typedef RxSO3Group RxSO3f; /**< single precision RxSO3 */ -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Quaternion QuaternionType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> QuaternionType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> QuaternionType; -}; - -} -} - -namespace Sophus { -using namespace Eigen; - -class ScaleNotPositive : public SophusException { -public: - ScaleNotPositive () - : SophusException("Scale factor is not positive") { - } -}; - -/** - * \brief RxSO3 base type - implements RxSO3 class but is storage agnostic - * - * This class implements the group \f$ R^{+} \times SO3 \f$ (RxSO3), the direct - * product of the group of positive scalar matrices (=isomorph to the positive - * real numbers) and the three-dimensional special orthogonal group SO3. - * Geometrically, it is the group of rotation and scaling in three dimensions. - * As a matrix groups, RxSO3 consists of matrices of the form \f$ s\cdot R \f$ - * where \f$ R \f$ is an orthognal matrix with \f$ det(R)=1 \f$ and \f$ s>0 \f$ - * be a positive real number. - * - * Internally, RxSO3 is represented by the group of non-zero quaternions. This - * is a most compact representation since the degrees of freedom (DoF) of RxSO3 - * (=4) equals the number of internal parameters (=4). - * - * [add more detailed description/tutorial] - */ -template -class RxSO3GroupBase { -public: - /** \brief scalar type, use with care since this might be a Map type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion reference type */ - typedef typename internal::traits::QuaternionType & - QuaternionReference; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - - /** \brief degree of freedom of group - * (three for rotation and one for scaling) */ - static const int DoF = 4; - /** \brief number of internal parameters used - * (quaternion for rotation and scaling) */ - static const int num_parameters = 4; - /** \brief group transformations are NxN matrices */ - static const int N = 3; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Matrix Tangent; - /** \brief adjoint transformation type */ - typedef Matrix Adjoint; - - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. - * - * For RxSO3, it simply returns the rotation matrix corresponding to - * \f$ A \f$. - */ - inline - const Adjoint Adj() const { - Adjoint res; - res.setIdentity(); - res.template topLeftCorner<3,3>() = rotationMatrix(); - return res; - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline RxSO3Group cast() const { - return RxSO3Group(quaternion() - .template cast() ); - } - - /** - * \returns pointer to internal data - * - * This provides direct read/write access to internal data. RxSO3 is - * represented by an Eigen::Quaternion (four parameters). - * - * Note: The first three Scalars represent the imaginary parts, while the - * forth Scalar represent the real part. - */ - inline Scalar* data() { - return quaternion().coeffs().data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - inline const Scalar* data() const { - return quaternion().coeffs().data(); - } - - /** - * \brief In-place group multiplication - * - * Same as operator*=() for RxSO3. - * - * \see operator*=() - */ - inline - void fastMultiply(const RxSO3Group& other) { - quaternion() *= other.quaternion(); - } - - - /** - * \returns group inverse of instance - */ - inline - const RxSO3Group inverse() const { - if(quaternion().squaredNorm() <= static_cast(0)) { - throw ScaleNotPositive(); - } - return RxSO3Group(quaternion().inverse()); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation (=rotation vector) of instance - * - * \see log(). - */ - inline - const Tangent log() const { - return RxSO3Group::log(*this); - } - - /** - * \returns 3x3 matrix representation of instance - * - * For RxSO3, the matrix representation is a scaled orthogonal - * matrix \f$ sR \f$ with \f$ det(sR)=s^3 \f$, thus a scaled rotation - * matrix \f$ R \f$ with scale s. - */ - inline - const Transformation matrix() const { - //ToDO: implement this directly! - Scalar scale = quaternion().norm(); - Quaternion norm_quad = quaternion(); - norm_quad.coeffs() /= scale; - return scale*norm_quad.toRotationMatrix(); - } - - /** - * \brief Assignment operator - */ - template inline - RxSO3GroupBase& operator= - (const RxSO3GroupBase & other) { - quaternion() = other.quaternion(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const RxSO3Group operator*(const RxSO3Group& other) const { - RxSO3Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^3 \f$ - * - * \param p point \f$p \in \mathbf{R}^3 \f$ - * \returns point \f$p' \in \mathbf{R}^3 \f$, - * rotated and scaled version of \f$p\f$ - * - * This function rotates and scales a point \f$ p \f$ in \f$ \mathbf{R}^3 \f$ - * by the RxSO3 transformation \f$sR\f$ (=rotation matrix) - * : \f$ p' = sR\cdot p \f$. - */ - inline - const Point operator*(const Point & p) const { - //ToDO: implement this directly! - Scalar scale = quaternion().norm(); - Quaternion norm_quad = quaternion(); - norm_quad.coeffs() /= scale; - return scale*norm_quad._transformVector(p); - } - - /** - * \brief In-place group multiplication - * \see operator*=() - */ - inline - void operator*=(const RxSO3Group& other) { - quaternion() *= other.quaternion(); - } - - /** - * \brief Mutator of quaternion - */ - EIGEN_STRONG_INLINE - QuaternionReference quaternion() { - return static_cast(this)->quaternion(); - } - - /** - * \brief Accessor of quaternion - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference quaternion() const { - return static_cast(this)->quaternion(); - } - - /** - * \returns rotation matrix - */ - inline - Transformation rotationMatrix() const { - Scalar scale = quaternion().norm(); - Quaternion norm_quad = quaternion(); - norm_quad.coeffs() /= scale; - return norm_quad.toRotationMatrix(); - } - - /** - * \returns scale - */ - EIGEN_STRONG_INLINE - const Scalar scale() const { - return quaternion().norm(); - } - - /** - * \brief Setter of quaternion using rotation matrix, leaves scale untouched - * - * \param R a 3x3 rotation matrix - * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 - */ - inline - void setRotationMatrix(const Transformation & R) { - Scalar saved_scale = scale(); - quaternion() = R; - quaternion().coeffs() *= saved_scale; - } - - /** - * \brief Scale setter - */ - EIGEN_STRONG_INLINE - void setScale(const Scalar & scale) { - quaternion().normalize(); - quaternion().coeffs() *= scale; - } - - /** - * \brief Setter of quaternion using scaled rotation matrix - * - * \param sR a 3x3 scaled rotation matrix - * \pre the 3x3 matrix should be "scaled orthogonal" - * and have a positive determinant - */ - inline - void setScaledRotationMatrix - (const Transformation & sR) { - Transformation squared_sR = sR*sR.transpose(); - Scalar squared_scale - = static_cast(1./3.) - *(squared_sR(0,0)+squared_sR(1,1)+squared_sR(2,2)); - if (squared_scale <= static_cast(0)) { - throw ScaleNotPositive(); - } - Scalar scale = std::sqrt(squared_scale); - if (scale <= static_cast(0)) { - throw ScaleNotPositive(); - } - quaternion() = sR/scale; - quaternion().coeffs() *= scale; - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \param b 4-vector representation of Lie algebra element - * \returns derivative of Lie bracket - * - * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{rxso3} \f$ - * with \f$ [a, b]_{rxso3} \f$ being the lieBracket() of the Lie - * algebra rxso3. - * - * \see lieBracket() - */ - inline static - const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { - Adjoint res; - res.setZero(); - res.template topLeftCorner<3,3>() = -SO3::hat(b.template head<3>()); - return res; - } - - /** - * \brief Group exponential - * - * \param a tangent space element - * (rotation vector \f$ \omega \f$ and logarithm of scale) - * \returns corresponding element of the group RxSO3 - * - * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of RxSO3. - * - * \see expAndTheta() - * \see hat() - * \see log() - */ - inline static - const RxSO3Group exp(const Tangent & a) { - Scalar theta; - return expAndTheta(a, &theta); - } - - /** - * \brief Group exponential and theta - * - * \param a tangent space element - * (rotation vector \f$ \omega \f$ and logarithm of scale ) - * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ - * \returns corresponding element of the group RxSO3 - * - * \see exp() for details - */ - inline static - const RxSO3Group expAndTheta(const Tangent & a, - Scalar * theta) { - const Matrix & omega = a.template head<3>(); - Scalar sigma = a[3]; - Scalar scale = std::exp(sigma); - Quaternion quat - = SO3Group::expAndTheta(omega, theta).unit_quaternion(); - quat.coeffs() *= scale; - return RxSO3Group(quat); - } - - /** - * \brief Generators - * - * \pre \f$ i \in \{0,1,2,3\} \f$ - * \returns \f$ i \f$th generator \f$ G_i \f$ of RxSO3 - * - * The infinitesimal generators of RxSO3 - * are \f$ - * G_0 = \left( \begin{array}{ccc} - * 0& 0& 0& \\ - * 0& 0& -1& \\ - * 0& 1& 0& - * \end{array} \right), - * G_1 = \left( \begin{array}{ccc} - * 0& 0& 1& \\ - * 0& 0& 0& \\ - * -1& 0& 0& - * \end{array} \right), - * G_2 = \left( \begin{array}{ccc} - * 0& -1& 0& \\ - * 1& 0& 0& \\ - * 0& 0& 0& - * \end{array} \right), - * G_3 = \left( \begin{array}{ccc} - * 1& 0& 0& \\ - * 0& 1& 0& \\ - * 0& 0& 1& - * \end{array} \right). - * \f$ - * \see hat() - */ - inline static - const Transformation generator(int i) { - if (i<0 || i>3) { - throw SophusException("i is not in range [0,3]."); - } - Tangent e; - e.setZero(); - e[i] = static_cast(1); - return hat(e); - } - - /** - * \brief hat-operator - * - * \param a 4-vector representation of Lie algebra element - * \returns 3x3-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of RxSO3 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^4 \rightarrow \mathbf{R}^{3\times 3}, - * \quad \widehat{a} = \sum_{i=0}^3 G_i a_i \f$ - * with \f$ G_i \f$ being the ith infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & a) { - Transformation A; - A << a(3), -a(2), a(1) - , a(2), a(3), -a(0) - ,-a(1), a(0), a(3); - return A; - } - - /** - * \brief Lie bracket - * - * \param a 4-vector representation of Lie algebra element - * \param b 4-vector representation of Lie algebra element - * \returns 4-vector representation of Lie algebra element - * - * It computes the bracket of RxSO3. To be more specific, it - * computes \f$ [a, 2]_{rxso3} - * := [\widehat{a}, \widehat{b}]^\vee \f$ - * with \f$ [A,B] = AB-BA \f$ being the matrix - * commutator, \f$ \widehat{\cdot} \f$ the - * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of RxSO3. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & a, - const Tangent & b) { - const Matrix & omega1 = a.template head<3>(); - const Matrix & omega2 = b.template head<3>(); - Matrix res; - res.template head<3>() = omega1.cross(omega2); - res[3] = static_cast(0); - return res; - } - - /** - * \brief Logarithmic map - * - * \param other element of the group RxSO3 - * \returns corresponding tangent space element - * (rotation vector \f$ \omega \f$ and logarithm of scale) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of RxSO3. - * - * \see exp() - * \see logAndTheta() - * \see vee() - */ - inline static - const Tangent log(const RxSO3Group & other) { - Scalar theta; - return logAndTheta(other, &theta); - } - - /** - * \brief Logarithmic map and theta - * - * \param other element of the group RxSO3 - * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ - * \returns corresponding tangent space element - * (rotation vector \f$ \omega \f$ and logarithm of scale) - * - * \see log() for details - */ - inline static - const Tangent logAndTheta(const RxSO3Group & other, - Scalar * theta) { - const Scalar & scale = other.quaternion().norm(); - Tangent omega_sigma; - omega_sigma[3] = std::log(scale); - omega_sigma.template head<3>() - = SO3Group::logAndTheta(SO3Group(other.quaternion()), - theta); - return omega_sigma; - } - - /** - * \brief vee-operator - * - * \param Omega 3x3-matrix representation of Lie algebra element - * \returns 4-vector representatin of Lie algebra element - * - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - return Tangent( static_cast(0.5) * (Omega(2,1) - Omega(1,2)), - static_cast(0.5) * (Omega(0,2) - Omega(2,0)), - static_cast(0.5) * (Omega(1,0) - Omega(0,1)), - static_cast(1./3.) - * (Omega(0,0) + Omega(1,1) + Omega(2,2)) ); - } -}; - -/** - * \brief RxSO3 default type - Constructors and default storage for RxSO3 Type - */ -template -class RxSO3Group : public RxSO3GroupBase > { - typedef RxSO3GroupBase > Base; -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief quaternion reference type */ - typedef typename internal::traits > - ::QuaternionType & QuaternionReference; - /** \brief quaternion const reference type */ - typedef const typename internal::traits > - ::QuaternionType & ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize Quaternion to identity rotation and scale. - */ - inline RxSO3Group() - : quaternion_(static_cast(1), static_cast(0), - static_cast(0), static_cast(0)) { - } - - /** - * \brief Copy constructor - */ - template inline - RxSO3Group(const RxSO3GroupBase & other) - : quaternion_(other.quaternion()) { - } - - /** - * \brief Constructor from scaled rotation matrix - * - * \pre matrix need to be "scaled orthogonal" with positive determinant - */ - inline explicit - RxSO3Group(const Transformation & sR) { - this->setScaledRotationMatrix(sR); - } - - /** - * \brief Constructor from scale factor and rotation matrix - * - * \pre rotation matrix need to be orthogonal with determinant of 1 - * \pre scale need to be not zero - */ - inline - RxSO3Group(const Scalar & scale, const Transformation & R) - : quaternion_(R) { - if(scale <= static_cast(0)) { - throw ScaleNotPositive(); - } - quaternion_.normalize(); - quaternion_.coeffs() *= scale; - } - - /** - * \brief Constructor from scale factor and SO3 - * - * \pre scale need to be not zero - */ - inline - RxSO3Group(const Scalar & scale, const SO3Group & so3) - : quaternion_(so3.unit_quaternion()) { - if (scale <= static_cast(0)) { - throw ScaleNotPositive(); - } - quaternion_.normalize(); - quaternion_.coeffs() *= scale; - } - - /** - * \brief Constructor from quaternion - * - * \pre quaternion must not be zero - */ - inline explicit - RxSO3Group(const Quaternion & quat) : quaternion_(quat) { - if(quaternion_.squaredNorm() <= SophusConstants::epsilon()) { - throw ScaleNotPositive(); - } - } - - /** - * \brief Mutator of quaternion - */ - EIGEN_STRONG_INLINE - QuaternionReference quaternion() { - return quaternion_; - } - - /** - * \brief Accessor of quaternion - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference quaternion() const { - return quaternion_; - } - -protected: - Quaternion quaternion_; -}; - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for RxSO3GroupBase - * - * Allows us to wrap RxSO3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::RxSO3GroupBase< - Map,_Options> > { - typedef Sophus::RxSO3GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion reference type */ - typedef typename internal::traits::QuaternionType & - QuaternionReference; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) : quaternion_(coeffs) { - } - - /** - * \brief Mutator of quaternion - */ - EIGEN_STRONG_INLINE - QuaternionReference quaternion() { - return quaternion_; - } - - /** - * \brief Accessor of quaternion - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference quaternion() const { - return quaternion_; - } - -protected: - Map,_Options> quaternion_; -}; - -/** - * \brief Specialisation of Eigen::Map for const RxSO3GroupBase - * - * Allows us to wrap RxSO3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::RxSO3GroupBase< - Map, _Options> > { - typedef Sophus::RxSO3GroupBase< - Map, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) : quaternion_(coeffs) { - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference quaternion() const { - return quaternion_; - } - -protected: - const Map,_Options> quaternion_; -}; - -} - -#endif // SOPHUS_RXSO3_HPP diff --git a/thirdparty/Sophus/sophus/se2.hpp b/thirdparty/Sophus/sophus/se2.hpp deleted file mode 100644 index 32f3ff2..0000000 --- a/thirdparty/Sophus/sophus/se2.hpp +++ /dev/null @@ -1,907 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_SE2_HPP -#define SOPHUS_SE2_HPP - -#include "so2.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class SE2Group; -typedef SE2Group SE2 EIGEN_DEPRECATED; -typedef SE2Group SE2d; /**< double precision SE2 */ -typedef SE2Group SE2f; /**< single precision SE2 */ -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Matrix TranslationType; - typedef Sophus::SO2Group SO2Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> SO2Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> SO2Type; -}; - -} -} - -namespace Sophus { -using namespace Eigen; -using namespace std; - -/** - * \brief SE2 base type - implements SE2 class but is storage agnostic - * - * [add more detailed description/tutorial] - */ -template -class SE2GroupBase { -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO2 reference type */ - typedef typename internal::traits::SO2Type & - SO2Reference; - /** \brief SO2 type */ - typedef const typename internal::traits::SO2Type & - ConstSO2Reference; - - /** \brief degree of freedom of group - * (two for translation, one for in-plane rotation) */ - static const int DoF = 3; - /** \brief number of internal parameters used - * (unit complex number for rotation + translation 2-vector) */ - static const int num_parameters = 4; - /** \brief group transformations are NxN matrices */ - static const int N = 3; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Matrix Tangent; - /** \brief adjoint transformation type */ - typedef Matrix Adjoint; - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. - */ - inline - const Adjoint Adj() const { - const Matrix & R = so2().matrix(); - Transformation res; - res.setIdentity(); - res.template topLeftCorner<2,2>() = R; - res(0,2) = translation()[1]; - res(1,2) = -translation()[0]; - return res; - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline SE2Group cast() const { - return - SE2Group(so2().template cast(), - translation().template cast() ); - } - - /** - * \brief Fast group multiplication - * - * This method is a fast version of operator*=(), since it does not perform - * normalization. It is up to the user to call normalize() once in a while. - * - * \see operator*=() - */ - inline - void fastMultiply(const SE2Group& other) { - translation() += so2()*(other.translation()); - so2().fastMultiply(other.so2()); - } - - /** - * \returns Group inverse of instance - */ - inline - const SE2Group inverse() const { - const SO2Group invR = so2().inverse(); - return SE2Group(invR, invR*(translation() - *static_cast(-1) ) ); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation - * (translational part and rotation angle) of instance - * - * \see log(). - */ - inline - const Tangent log() const { - return log(*this); - } - - /** - * \brief Normalize SO2 element - * - * It re-normalizes the SO2 element. This method only needs to - * be called in conjunction with fastMultiply() or data() write access. - */ - inline - void normalize() { - so2().normalize(); - } - - /** - * \returns 3x3 matrix representation of instance - */ - inline - const Transformation matrix() const { - Transformation homogenious_matrix; - homogenious_matrix.setIdentity(); - homogenious_matrix.block(0,0,2,2) = rotationMatrix(); - homogenious_matrix.col(2).head(2) = translation(); - return homogenious_matrix; - } - - /** - * \returns 2x3 matrix representation of instance - * - * It returns the three first row of matrix(). - */ - inline - const Matrix matrix2x3() const { - Matrix matrix; - matrix.block(0,0,2,2) = rotationMatrix(); - matrix.col(2) = translation(); - return matrix; - } - - /** - * \brief Assignment operator - */ - template inline - SE2GroupBase& operator= (const SE2GroupBase & other) { - so2() = other.so2(); - translation() = other.translation(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const SE2Group operator*(const SE2Group& other) const { - SE2Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^2 \f$ - * - * \param p point \f$p \in \mathbf{R}^2 \f$ - * \returns point \f$p' \in \mathbf{R}^2 \f$, - * rotated and translated version of \f$p\f$ - * - * This function rotates and translates point \f$ p \f$ - * in \f$ \mathbf{R}^2 \f$ by the SE2 transformation \f$R,t\f$ - * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$. - */ - inline - const Point operator*(const Point & p) const { - return so2()*p + translation(); - } - - /** - * \brief In-place group multiplication - * - * \see fastMultiply() - * \see operator*() - */ - inline - void operator*=(const SE2Group& other) { - fastMultiply(other); - normalize(); - } - - - /** - * \returns Rotation matrix - */ - inline - const Matrix rotationMatrix() const { - return so2().matrix(); - } - - /** - * \brief Setter of internal unit complex number representation - * - * \param complex - * \pre the complex number must not be zero - * - * The complex number is normalized to unit length. - */ - inline - void setComplex(const Matrix & complex) { - return so2().setComplex(complex); - } - - /** - * \brief Setter of unit complex number using rotation matrix - * - * \param R a 2x2 matrix - * \pre the 2x2 matrix should be orthogonal and have a determinant of 1 - */ - inline - void setRotationMatrix(const Matrix & R) { - so2().setComplex(static_cast(0.5)*(R(0,0)+R(1,1)), - static_cast(0.5)*(R(1,0)-R(0,1))); - } - - /** - * \brief Mutator of SO2 group - */ - EIGEN_STRONG_INLINE - SO2Reference so2() { - return static_cast(this)->so2(); - } - - /** - * \brief Accessor of SO2 group - */ - EIGEN_STRONG_INLINE - ConstSO2Reference so2() const { - return static_cast(this)->so2(); - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return static_cast(this)->translation(); - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return static_cast(this)->translation(); - } - - /** - * \brief Accessor of unit complex number - * - * No direct write access is given to ensure the complex number stays - * normalized. - */ - inline - typename internal::traits::SO2Type::ConstComplexReference - unit_complex() const { - return so2().unit_complex(); - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \param b 3-vector representation of Lie algebra element - * \returns derivative of Lie bracket - * - * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{se2} \f$ - * with \f$ [a, b]_{se2} \f$ being the lieBracket() of the Lie algebra se2. - * - * \see lieBracket() - */ - inline static - const Transformation d_lieBracketab_by_d_a(const Tangent & b) { - static const Scalar zero = static_cast(0); - Matrix upsilon2 = b.template head<2>(); - Scalar theta2 = b[2]; - - Transformation res; - res << zero, theta2, -upsilon2[1] - , -theta2, zero, upsilon2[0] - , zero, zero, zero; - return res; - } - - /** - * \brief Group exponential - * - * \param a tangent space element (3-vector) - * \returns corresponding element of the group SE2 - * - * The first two components of \f$ a \f$ represent the translational - * part \f$ \upsilon \f$ in the tangent space of SE2, while the last - * components of \f$ a \f$ is the rotation angle \f$ \theta \f$. - * - * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE2. - * - * \see hat() - * \see log() - */ - inline static - const SE2Group exp(const Tangent & a) { - Scalar theta = a[2]; - const SO2Group & so2 = SO2Group::exp(theta); - Scalar sin_theta_by_theta; - Scalar one_minus_cos_theta_by_theta; - - if(std::abs(theta)::epsilon()) { - Scalar theta_sq = theta*theta; - sin_theta_by_theta - = static_cast(1.) - static_cast(1./6.)*theta_sq; - one_minus_cos_theta_by_theta - = static_cast(0.5)*theta - - static_cast(1./24.)*theta*theta_sq; - } else { - sin_theta_by_theta = so2.unit_complex().y()/theta; - one_minus_cos_theta_by_theta - = (static_cast(1.) - so2.unit_complex().x())/theta; - } - Matrix trans - (sin_theta_by_theta*a[0] - one_minus_cos_theta_by_theta*a[1], - one_minus_cos_theta_by_theta * a[0]+sin_theta_by_theta*a[1]); - return SE2Group(so2, trans); - } - - /** - * \brief Generators - * - * \pre \f$ i \in \{0,1,2\} \f$ - * \returns \f$ i \f$th generator \f$ G_i \f$ of SE2 - * - * The infinitesimal generators of SE2 are: \f[ - * G_0 = \left( \begin{array}{ccc} - * 0& 0& 1\\ - * 0& 0& 0\\ - * 0& 0& 0\\ - * \end{array} \right), - * G_1 = \left( \begin{array}{cccc} - * 0& 0& 0\\ - * 0& 0& 1\\ - * 0& 0& 0\\ - * \end{array} \right), - * G_2 = \left( \begin{array}{cccc} - * 0& 0& 0&\\ - * 0& 0& -1&\\ - * 0& 1& 0&\\ - * \end{array} \right), - * \f] - * \see hat() - */ - inline static - const Transformation generator(int i) { - if (i<0 || i>2) { - throw SophusException("i is not in range [0,2]."); - } - Tangent e; - e.setZero(); - e[i] = static_cast(1); - return hat(e); - } - - /** - * \brief hat-operator - * - * \param omega 3-vector representation of Lie algebra element - * \returns 3x3-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of SE2 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{2\times 2}, - * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$ - * with \f$ G_i \f$ being the ith infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & v) { - Transformation Omega; - Omega.setZero(); - Omega.template topLeftCorner<2,2>() = SO2Group::hat(v[2]); - Omega.col(2).template head<2>() = v.template head<2>(); - return Omega; - } - - /** - * \brief Lie bracket - * - * \param a 3-vector representation of Lie algebra element - * \param b 3-vector representation of Lie algebra element - * \returns 3-vector representation of Lie algebra element - * - * It computes the bracket of SE2. To be more specific, it - * computes \f$ [a, b]_{se2} - * := [\widehat{a_1}, \widehat{b_2}]^\vee \f$ - * with \f$ [A,B] = AB-BA \f$ being the matrix - * commutator, \f$ \widehat{\cdot} \f$ the - * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SE2. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & a, - const Tangent & b) { - Matrix upsilon1 = a.template head<2>(); - Matrix upsilon2 = b.template head<2>(); - Scalar theta1 = a[2]; - Scalar theta2 = b[2]; - - return Tangent(-theta1*upsilon2[1] + theta2*upsilon1[1], - theta1*upsilon2[0] - theta2*upsilon1[0], - static_cast(0)); - } - - /** - * \brief Logarithmic map - * - * \param other element of the group SE2 - * \returns corresponding tangent space element - * (translational part \f$ \upsilon \f$ - * and rotation vector \f$ \omega \f$) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of SE2. - * - * \see exp() - * \see vee() - */ - inline static - const Tangent log(const SE2Group & other) { - Tangent upsilon_theta; - const SO2Group & so2 = other.so2(); - Scalar theta = SO2Group::log(so2); - upsilon_theta[2] = theta; - Scalar halftheta = static_cast(0.5)*theta; - Scalar halftheta_by_tan_of_halftheta; - - const Matrix & z = so2.unit_complex(); - Scalar real_minus_one = z.x()-static_cast(1.); - if (std::abs(real_minus_one)::epsilon()) { - halftheta_by_tan_of_halftheta - = static_cast(1.) - - static_cast(1./12)*theta*theta; - } else { - halftheta_by_tan_of_halftheta - = -(halftheta*z.y())/(real_minus_one); - } - Matrix V_inv; - V_inv << halftheta_by_tan_of_halftheta, halftheta - , -halftheta, halftheta_by_tan_of_halftheta; - upsilon_theta.template head<2>() = V_inv*other.translation(); - return upsilon_theta; - } - - /** - * \brief vee-operator - * - * \param Omega 3x3-matrix representation of Lie algebra element - * \returns 3-vector representatin of Lie algebra element - * - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - Tangent upsilon_omega; - upsilon_omega.template head<2>() = Omega.col(2).template head<2>(); - upsilon_omega[2] - = SO2Group::vee(Omega.template topLeftCorner<2,2>()); - return upsilon_omega; - } -}; - -/** - * \brief SE2 default type - Constructors and default storage for SE2 Type - */ -template -class SE2Group : public SE2GroupBase > { - typedef SE2GroupBase > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits > - ::TranslationType & TranslationReference; - typedef const typename internal::traits > - ::TranslationType & ConstTranslationReference; - /** \brief SO2 reference type */ - typedef typename internal::traits > - ::SO2Type & SO2Reference; - /** \brief SO2 const reference type */ - typedef const typename internal::traits > - ::SO2Type & ConstSO2Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize Complex to identity rotation and translation to zero. - */ - inline - SE2Group() - : translation_( Matrix::Zero() ) - { - } - - /** - * \brief Copy constructor - */ - template inline - SE2Group(const SE2GroupBase & other) - : so2_(other.so2()), translation_(other.translation()) { - } - - /** - * \brief Constructor from SO2 and translation vector - */ - template inline - SE2Group(const SO2GroupBase & so2, - const Point & translation) - : so2_(so2), translation_(translation) { - } - - /** - * \brief Constructor from rotation matrix and translation vector - * - * \pre rotation matrix need to be orthogonal with determinant of 1 - */ - inline - SE2Group(const typename SO2Group::Transformation & rotation_matrix, - const Point & translation) - : so2_(rotation_matrix), translation_(translation) { - } - - /** - * \brief Constructor from rotation angle and translation vector - */ - inline - SE2Group(const Scalar & theta, - const Point & translation) - : so2_(theta), translation_(translation) { - } - - /** - * \brief Constructor from complex number and translation vector - * - * \pre complex must not be zero - */ - inline - SE2Group(const std::complex & complex, - const Point & translation) - : so2_(complex), translation_(translation) { - } - - /** - * \brief Constructor from 3x3 matrix - * - * \pre 2x2 sub-matrix need to be orthogonal with determinant of 1 - */ - inline explicit - SE2Group(const Transformation & T) - : so2_(T.template topLeftCorner<2,2>()), - translation_(T.template block<2,1>(0,2)) { - } - - /** - * \returns pointer to internal data - * - * This provides unsafe read/write access to internal data. SE2 is represented - * by a pair of an SO2 element (two parameters) and a translation vector (two - * parameters). The user needs to take care of that the complex - * stays normalized. - * - * /see normalize() - */ - EIGEN_STRONG_INLINE - Scalar* data() { - // so2_ and translation_ are layed out sequentially with no padding - return so2_.data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - EIGEN_STRONG_INLINE - const Scalar* data() const { - // so2_ and translation_ are layed out sequentially with no padding - return so2_.data(); - } - - /** - * \brief Accessor of SO2 - */ - EIGEN_STRONG_INLINE - SO2Reference so2() { - return so2_; - } - - /** - * \brief Mutator of SO2 - */ - EIGEN_STRONG_INLINE - ConstSO2Reference so2() const { - return so2_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Sophus::SO2Group so2_; - Matrix translation_; -}; - - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for SE2GroupBase - * - * Allows us to wrap SE2 Objects around POD array - * (e.g. external c style complex) - */ -template -class Map, _Options> - : public Sophus::SE2GroupBase, _Options> > -{ - typedef Sophus::SE2GroupBase, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO2 reference type */ - typedef typename internal::traits::SO2Type & SO2Reference; - /** \brief SO2 const reference type */ - typedef const typename internal::traits::SO2Type & ConstSO2Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) - : so2_(coeffs), - translation_(coeffs+Sophus::SO2Group::num_parameters) { - } - - /** - * \brief Mutator of SO2 - */ - EIGEN_STRONG_INLINE - SO2Reference so2() { - return so2_; - } - - /** - * \brief Accessor of SO2 - */ - EIGEN_STRONG_INLINE - ConstSO2Reference so2() const { - return so2_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Map,_Options> so2_; - Map,_Options> translation_; -}; - -/** - * \brief Specialisation of Eigen::Map for const SE2GroupBase - * - * Allows us to wrap SE2 Objects around POD array - * (e.g. external c style complex) - */ -template -class Map, _Options> - : public Sophus::SE2GroupBase< - Map, _Options> > { - typedef Sophus::SE2GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO2 const reference type */ - typedef const typename internal::traits::SO2Type & ConstSO2Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) - : so2_(coeffs), - translation_(coeffs+Sophus::SO2Group::num_parameters) { - } - - EIGEN_STRONG_INLINE - Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) - : translation_(trans_coeffs), so2_(rot_coeffs){ - } - - /** - * \brief Accessor of SO2 - */ - EIGEN_STRONG_INLINE - ConstSO2Reference so2() const { - return so2_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - const Map,_Options> so2_; - const Map,_Options> translation_; -}; - -} - -#endif diff --git a/thirdparty/Sophus/sophus/se3.hpp b/thirdparty/Sophus/sophus/se3.hpp deleted file mode 100644 index d2a20cc..0000000 --- a/thirdparty/Sophus/sophus/se3.hpp +++ /dev/null @@ -1,947 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2011-2013 Hauke Strasdat -// 2012-2013 Steven Lovegrove -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_SE3_HPP -#define SOPHUS_SE3_HPP - -#include "so3.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class SE3Group; -typedef SE3Group SE3 EIGEN_DEPRECATED; -typedef SE3Group SE3d; /**< double precision SE3 */ -typedef SE3Group SE3f; /**< single precision SE3 */ -typedef Matrix Vector6d; -typedef Matrix Matrix6d; -typedef Matrix Vector6f; -typedef Matrix Matrix6f; -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Matrix TranslationType; - typedef Sophus::SO3Group SO3Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> SO3Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> SO3Type; -}; - -} -} - -namespace Sophus { -using namespace Eigen; -using namespace std; - -/** - * \brief SE3 base type - implements SE3 class but is storage agnostic - * - * [add more detailed description/tutorial] - */ -template -class SE3GroupBase { -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO3 reference type */ - typedef typename internal::traits::SO3Type & - SO3Reference; - /** \brief SO3 const reference type */ - typedef const typename internal::traits::SO3Type & - ConstSO3Reference; - - /** \brief degree of freedom of group - * (three for translation, three for rotation) */ - static const int DoF = 6; - /** \brief number of internal parameters used - * (unit quaternion for rotation + translation 3-vector) */ - static const int num_parameters = 7; - /** \brief group transformations are NxN matrices */ - static const int N = 4; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Matrix Tangent; - /** \brief adjoint transformation type */ - typedef Matrix Adjoint; - - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. - */ - inline - const Adjoint Adj() const { - const Matrix & R = so3().matrix(); - Adjoint res; - res.block(0,0,3,3) = R; - res.block(3,3,3,3) = R; - res.block(0,3,3,3) = SO3Group::hat(translation())*R; - res.block(3,0,3,3) = Matrix::Zero(3,3); - return res; - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline SE3Group cast() const { - return - SE3Group(so3().template cast(), - translation().template cast() ); - } - - /** - * \brief Fast group multiplication - * - * This method is a fast version of operator*=(), since it does not perform - * normalization. It is up to the user to call normalize() once in a while. - * - * \see operator*=() - */ - inline - void fastMultiply(const SE3Group& other) { - translation() += so3()*(other.translation()); - so3().fastMultiply(other.so3()); - } - - /** - * \returns Group inverse of instance - */ - inline - const SE3Group inverse() const { - const SO3Group invR = so3().inverse(); - return SE3Group(invR, invR*(translation() - *static_cast(-1) ) ); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation - * (translational part and rotation vector) of instance - * - * \see log(). - */ - inline - const Tangent log() const { - return log(*this); - } - - /** - * \brief Normalize SO3 element - * - * It re-normalizes the SO3 element. This method only needs to - * be called in conjunction with fastMultiply() or data() write access. - */ - inline - void normalize() { - so3().normalize(); - } - - /** - * \returns 4x4 matrix representation of instance - */ - inline - const Transformation matrix() const { - Transformation homogenious_matrix; - homogenious_matrix.setIdentity(); - homogenious_matrix.block(0,0,3,3) = rotationMatrix(); - homogenious_matrix.col(3).head(3) = translation(); - return homogenious_matrix; - } - - /** - * \returns 3x4 matrix representation of instance - * - * It returns the three first row of matrix(). - */ - inline - const Matrix matrix3x4() const { - Matrix matrix; - matrix.block(0,0,3,3) = rotationMatrix(); - matrix.col(3) = translation(); - return matrix; - } - - /** - * \brief Assignment operator - */ - template inline - SE3GroupBase& operator= (const SE3GroupBase & other) { - so3() = other.so3(); - translation() = other.translation(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const SE3Group operator*(const SE3Group& other) const { - SE3Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^3 \f$ - * - * \param p point \f$p \in \mathbf{R}^3 \f$ - * \returns point \f$p' \in \mathbf{R}^3 \f$, - * rotated and translated version of \f$p\f$ - * - * This function rotates and translates point \f$ p \f$ - * in \f$ \mathbf{R}^3 \f$ by the SE3 transformation \f$R,t\f$ - * (=rotation matrix, translation vector): \f$ p' = R\cdot p + t \f$. - */ - inline - const Point operator*(const Point & p) const { - return so3()*p + translation(); - } - - /** - * \brief In-place group multiplication - * - * \see fastMultiply() - * \see operator*() - */ - inline - void operator*=(const SE3Group& other) { - fastMultiply(other); - normalize(); - } - - - /** - * \returns Rotation matrix - * - * deprecated: use rotationMatrix() instead. - */ - typedef Transformation M3_marcos_dont_like_commas; - inline - EIGEN_DEPRECATED const M3_marcos_dont_like_commas rotation_matrix() const { - return so3().matrix(); - } - - /** - * \returns Rotation matrix - */ - inline - const Matrix rotationMatrix() const { - return so3().matrix(); - } - - - /** - * \brief Mutator of SO3 group - */ - EIGEN_STRONG_INLINE - SO3Reference so3() { - return static_cast(this)->so3(); - } - - /** - * \brief Accessor of SO3 group - */ - EIGEN_STRONG_INLINE - ConstSO3Reference so3() const { - return static_cast(this)->so3(); - } - - /** - * \brief Setter of internal unit quaternion representation - * - * \param quaternion - * \pre the quaternion must not be zero - * - * The quaternion is normalized to unit length. - */ - inline - void setQuaternion(const Quaternion & quat) { - return so3().setQuaternion(quat); - } - - /** - * \brief Setter of unit quaternion using rotation matrix - * - * \param rotation_matrix a 3x3 rotation matrix - * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 - */ - inline - void setRotationMatrix - (const Matrix & rotation_matrix) { - so3().setQuaternion(Quaternion(rotation_matrix)); - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return static_cast(this)->translation(); - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return static_cast(this)->translation(); - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - inline - typename internal::traits::SO3Type::ConstQuaternionReference - unit_quaternion() const { - return so3().unit_quaternion(); - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \param b 6-vector representation of Lie algebra element - * \returns derivative of Lie bracket - * - * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{se3} \f$ - * with \f$ [a, b]_{se3} \f$ being the lieBracket() of the Lie algebra se3. - * - * \see lieBracket() - */ - inline static - const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { - Adjoint res; - res.setZero(); - - const Matrix & upsilon2 = b.template head<3>(); - const Matrix & omega2 = b.template tail<3>(); - - res.template topLeftCorner<3,3>() = -SO3Group::hat(omega2); - res.template topRightCorner<3,3>() = -SO3Group::hat(upsilon2); - res.template bottomRightCorner<3,3>() = -SO3Group::hat(omega2); - return res; - } - - /** - * \brief Group exponential - * - * \param a tangent space element (6-vector) - * \returns corresponding element of the group SE3 - * - * The first three components of \f$ a \f$ represent the translational - * part \f$ \upsilon \f$ in the tangent space of SE3, while the last three - * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$. - * - * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of SE3. - * - * \see hat() - * \see log() - */ - inline static - const SE3Group exp(const Tangent & a) { - const Matrix & omega = a.template tail<3>(); - - Scalar theta; - const SO3Group & so3 - = SO3Group::expAndTheta(omega, &theta); - - const Matrix & Omega = SO3Group::hat(omega); - const Matrix & Omega_sq = Omega*Omega; - Matrix V; - - if(theta::epsilon()) { - V = so3.matrix(); - //Note: That is an accurate expansion! - } else { - Scalar theta_sq = theta*theta; - V = (Matrix::Identity() - + (static_cast(1)-std::cos(theta))/(theta_sq)*Omega - + (theta-std::sin(theta))/(theta_sq*theta)*Omega_sq); - } - return SE3Group(so3,V*a.template head<3>()); - } - - /** - * \brief Generators - * - * \pre \f$ i \in \{0,1,2,3,4,5\} \f$ - * \returns \f$ i \f$th generator \f$ G_i \f$ of SE3 - * - * The infinitesimal generators of SE3 are: \f[ - * G_0 = \left( \begin{array}{cccc} - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_1 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_2 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * \end{array} \right). - * G_3 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& -1& 0\\ - * 0& 1& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_4 = \left( \begin{array}{cccc} - * 0& 0& 1& 0\\ - * 0& 0& 0& 0\\ - * -1& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_5 = \left( \begin{array}{cccc} - * 0& -1& 0& 0\\ - * 1& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right). - * \f] - * \see hat() - */ - inline static - const Transformation generator(int i) { - if (i<0 || i>5) { - throw SophusException("i is not in range [0,5]."); - } - Tangent e; - e.setZero(); - e[i] = static_cast(1); - return hat(e); - } - - /** - * \brief hat-operator - * - * \param omega 6-vector representation of Lie algebra element - * \returns 4x4-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of SE3 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^6 \rightarrow \mathbf{R}^{4\times 4}, - * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$ - * with \f$ G_i \f$ being the ith infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & v) { - Transformation Omega; - Omega.setZero(); - Omega.template topLeftCorner<3,3>() - = SO3Group::hat(v.template tail<3>()); - Omega.col(3).template head<3>() = v.template head<3>(); - return Omega; - } - - /** - * \brief Lie bracket - * - * \param a 6-vector representation of Lie algebra element - * \param b 6-vector representation of Lie algebra element - * \returns 6-vector representation of Lie algebra element - * - * It computes the bracket of SE3. To be more specific, it - * computes \f$ [a, b]_{se3} - * := [\widehat{a}, \widehat{b}]^\vee \f$ - * with \f$ [A,B] = AB-BA \f$ being the matrix - * commutator, \f$ \widehat{\cdot} \f$ the - * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SE3. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & a, - const Tangent & b) { - Matrix upsilon1 = a.template head<3>(); - Matrix upsilon2 = b.template head<3>(); - Matrix omega1 = a.template tail<3>(); - Matrix omega2 = b.template tail<3>(); - - Tangent res; - res.template head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2); - res.template tail<3>() = omega1.cross(omega2); - - return res; - } - - /** - * \brief Logarithmic map - * - * \param other element of the group SE3 - * \returns corresponding tangent space element - * (translational part \f$ \upsilon \f$ - * and rotation vector \f$ \omega \f$) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of SE3. - * - * \see exp() - * \see vee() - */ - inline static - const Tangent log(const SE3Group & se3) { - Tangent upsilon_omega; - Scalar theta; - upsilon_omega.template tail<3>() - = SO3Group::logAndTheta(se3.so3(), &theta); - - if (std::abs(theta)::epsilon()) { - const Matrix & Omega - = SO3Group::hat(upsilon_omega.template tail<3>()); - const Matrix & V_inv = - Matrix::Identity() - - static_cast(0.5)*Omega - + static_cast(1./12.)*(Omega*Omega); - - upsilon_omega.template head<3>() = V_inv*se3.translation(); - } else { - const Matrix & Omega - = SO3Group::hat(upsilon_omega.template tail<3>()); - const Matrix & V_inv = - ( Matrix::Identity() - static_cast(0.5)*Omega - + ( static_cast(1) - - theta/(static_cast(2)*tan(theta/Scalar(2)))) / - (theta*theta)*(Omega*Omega) ); - upsilon_omega.template head<3>() = V_inv*se3.translation(); - } - return upsilon_omega; - } - - /** - * \brief vee-operator - * - * \param Omega 4x4-matrix representation of Lie algebra element - * \returns 6-vector representatin of Lie algebra element - * - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - Tangent upsilon_omega; - upsilon_omega.template head<3>() = Omega.col(3).template head<3>(); - upsilon_omega.template tail<3>() - = SO3Group::vee(Omega.template topLeftCorner<3,3>()); - return upsilon_omega; - } -}; - -/** - * \brief SE3 default type - Constructors and default storage for SE3 Type - */ -template -class SE3Group : public SE3GroupBase > { - typedef SE3GroupBase > Base; -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief SO3 reference type */ - typedef typename internal::traits > - ::SO3Type & SO3Reference; - /** \brief SO3 const reference type */ - typedef const typename internal::traits > - ::SO3Type & ConstSO3Reference; - /** \brief translation reference type */ - typedef typename internal::traits > - ::TranslationType & TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits > - ::TranslationType & ConstTranslationReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize Quaternion to identity rotation and translation to zero. - */ - inline - SE3Group() - : translation_( Matrix::Zero() ) - { - } - - /** - * \brief Copy constructor - */ - template inline - SE3Group(const SE3GroupBase & other) - : so3_(other.so3()), translation_(other.translation()) { - } - - /** - * \brief Constructor from SO3 and translation vector - */ - template inline - SE3Group(const SO3GroupBase & so3, - const Point & translation) - : so3_(so3), translation_(translation) { - } - - /** - * \brief Constructor from rotation matrix and translation vector - * - * \pre rotation matrix need to be orthogonal with determinant of 1 - */ - inline - SE3Group(const Matrix & rotation_matrix, - const Point & translation) - : so3_(rotation_matrix), translation_(translation) { - } - - /** - * \brief Constructor from quaternion and translation vector - * - * \pre quaternion must not be zero - */ - inline - SE3Group(const Quaternion & quaternion, - const Point & translation) - : so3_(quaternion), translation_(translation) { - } - - /** - * \brief Constructor from 4x4 matrix - * - * \pre top-left 3x3 sub-matrix need to be orthogonal with determinant of 1 - */ - inline explicit - SE3Group(const Eigen::Matrix& T) - : so3_(T.template topLeftCorner<3,3>()), - translation_(T.template block<3,1>(0,3)) { - } - - /** - * \returns pointer to internal data - * - * This provides unsafe read/write access to internal data. SE3 is represented - * by a pair of an SO3 element (4 parameters) and translation vector (three - * parameters). The user needs to take care of that the quaternion - * stays normalized. - * - * Note: The first three Scalars represent the imaginary parts, while the - * forth Scalar represent the real part. - * - * /see normalize() - */ - EIGEN_STRONG_INLINE - Scalar* data() { - // so3_ and translation_ are layed out sequentially with no padding - return so3_.data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - EIGEN_STRONG_INLINE - const Scalar* data() const { - // so3_ and translation_ are layed out sequentially with no padding - return so3_.data(); - } - - /** - * \brief Accessor of SO3 - */ - EIGEN_STRONG_INLINE - SO3Reference so3() { - return so3_; - } - - /** - * \brief Mutator of SO3 - */ - EIGEN_STRONG_INLINE - ConstSO3Reference so3() const { - return so3_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Sophus::SO3Group so3_; - Matrix translation_; -}; - - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for SE3GroupBase - * - * Allows us to wrap SE3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::SE3GroupBase, _Options> > { - typedef Sophus::SE3GroupBase, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO3 reference type */ - typedef typename internal::traits::SO3Type & - SO3Reference; - /** \brief SO3 const reference type */ - typedef const typename internal::traits::SO3Type & - ConstSO3Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) - : so3_(coeffs), - translation_(coeffs+Sophus::SO3Group::num_parameters) { - } - - /** - * \brief Mutator of SO3 - */ - EIGEN_STRONG_INLINE - SO3Reference so3() { - return so3_; - } - - /** - * \brief Accessor of SO3 - */ - EIGEN_STRONG_INLINE - ConstSO3Reference so3() const { - return so3_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Map,_Options> so3_; - Map,_Options> translation_; -}; - -/** - * \brief Specialisation of Eigen::Map for const SE3GroupBase - * - * Allows us to wrap SE3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::SE3GroupBase< - Map, _Options> > { - typedef Sophus::SE3GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief SO3 const reference type */ - typedef const typename internal::traits::SO3Type & - ConstSO3Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) - : so3_(coeffs), - translation_(coeffs+Sophus::SO3Group::num_parameters) { - } - - EIGEN_STRONG_INLINE - Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) - : translation_(trans_coeffs), so3_(rot_coeffs){ - } - - /** - * \brief Accessor of SO3 - */ - EIGEN_STRONG_INLINE - ConstSO3Reference so3() const { - return so3_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - const Map,_Options> so3_; - const Map,_Options> translation_; -}; - -} - -#endif diff --git a/thirdparty/Sophus/sophus/sim3.hpp b/thirdparty/Sophus/sophus/sim3.hpp deleted file mode 100644 index 97087c3..0000000 --- a/thirdparty/Sophus/sophus/sim3.hpp +++ /dev/null @@ -1,976 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_SIM3_HPP -#define SOPHUS_SIM3_HPP - -#include "rxso3.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class Sim3Group; -typedef Sim3Group Sim3 EIGEN_DEPRECATED; -typedef Sim3Group Sim3d; /**< double precision Sim3 */ -typedef Sim3Group Sim3f; /**< single precision Sim3 */ -typedef Matrix Vector7d; -typedef Matrix Matrix7d; -typedef Matrix Vector7f; -typedef Matrix Matrix7f; -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Matrix TranslationType; - typedef Sophus::RxSO3Group RxSO3Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> RxSO3Type; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> TranslationType; - typedef Map,_Options> RxSO3Type; -}; - -} -} - -namespace Sophus { -using namespace Eigen; -using namespace std; - -/** - * \brief Sim3 base type - implements Sim3 class but is storage agnostic - * - * [add more detailed description/tutorial] - */ -template -class Sim3GroupBase { -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief RxSO3 reference type */ - typedef typename internal::traits::RxSO3Type & - RxSO3Reference; - /** \brief RxSO3 const reference type */ - typedef const typename internal::traits::RxSO3Type & - ConstRxSO3Reference; - - - /** \brief degree of freedom of group - * (three for translation, three for rotation, one for scale) */ - static const int DoF = 7; - /** \brief number of internal parameters used - * (quaternion for rotation and scale + translation 3-vector) */ - static const int num_parameters = 7; - /** \brief group transformations are NxN matrices */ - static const int N = 4; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Matrix Tangent; - /** \brief adjoint transformation type */ - typedef Matrix Adjoint; - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. - */ - inline - const Adjoint Adj() const { - const Matrix & R = rxso3().rotationMatrix(); - Adjoint res; - res.setZero(); - res.block(0,0,3,3) = scale()*R; - res.block(0,3,3,3) = SO3Group::hat(translation())*R; - res.block(0,6,3,1) = -translation(); - res.block(3,3,3,3) = R; - res(6,6) = 1; - return res; - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline Sim3Group cast() const { - return - Sim3Group(rxso3().template cast(), - translation().template cast() ); - } - - /** - * \brief In-place group multiplication - * - * Same as operator*=() for Sim3. - * - * \see operator*() - */ - inline - void fastMultiply(const Sim3Group& other) { - translation() += (rxso3() * other.translation()); - rxso3() *= other.rxso3(); - } - - /** - * \returns Group inverse of instance - */ - inline - const Sim3Group inverse() const { - const RxSO3Group invR = rxso3().inverse(); - return Sim3Group(invR, invR*(translation() - *static_cast(-1) ) ); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation - * (translational part and rotation vector) of instance - * - * \see log(). - */ - inline - const Tangent log() const { - return log(*this); - } - - /** - * \returns 4x4 matrix representation of instance - */ - inline - const Transformation matrix() const { - Transformation homogenious_matrix; - homogenious_matrix.setIdentity(); - homogenious_matrix.block(0,0,3,3) = rxso3().matrix(); - homogenious_matrix.col(3).head(3) = translation(); - return homogenious_matrix; - } - - /** - * \returns 3x4 matrix representation of instance - * - * It returns the three first row of matrix(). - */ - inline - const Matrix matrix3x4() const { - Matrix matrix; - matrix.block(0,0,3,3) = rxso3().matrix(); - matrix.col(3) = translation(); - return matrix; - } - - /** - * \brief Assignment operator - */ - template inline - Sim3GroupBase& operator= - (const Sim3GroupBase & other) { - rxso3() = other.rxso3(); - translation() = other.translation(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const Sim3Group operator*(const Sim3Group& other) const { - Sim3Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^3 \f$ - * - * \param p point \f$p \in \mathbf{R}^3 \f$ - * \returns point \f$p' \in \mathbf{R}^3 \f$, - * rotated, scaled and translated version of \f$p\f$ - * - * This function scales, rotates and translates point \f$ p \f$ - * in \f$ \mathbf{R}^3 \f$ by the Sim3 transformation \f$sR,t\f$ - * (=scaled rotation matrix, translation vector): \f$ p' = sR\cdot p + t \f$. - */ - inline - const Point operator*(const Point & p) const { - return rxso3()*p + translation(); - } - - /** - * \brief In-place group multiplication - * - * \see operator*() - */ - inline - void operator*=(const Sim3Group& other) { - translation() += (rxso3() * other.translation()); - rxso3() *= other.rxso3(); - } - - /** - * \brief Mutator of quaternion - */ - inline - typename internal::traits::RxSO3Type::QuaternionReference - quaternion() { - return rxso3().quaternion(); - } - - /** - * \brief Accessor of quaternion - */ - inline - typename internal::traits::RxSO3Type::ConstQuaternionReference - quaternion() const { - return rxso3().quaternion(); - } - - /** - * \returns Rotation matrix - * - * deprecated: use rotationMatrix() instead. - */ - inline - EIGEN_DEPRECATED const Transformation rotation_matrix() const { - return rxso3().rotationMatrix(); - } - - /** - * \returns Rotation matrix - */ - inline - const Matrix rotationMatrix() const { - return rxso3().rotationMatrix(); - } - - /** - * \brief Mutator of RxSO3 group - */ - EIGEN_STRONG_INLINE - RxSO3Reference rxso3() { - return static_cast(this)->rxso3(); - } - - /** - * \brief Accessor of RxSO3 group - */ - EIGEN_STRONG_INLINE - ConstRxSO3Reference rxso3() const { - return static_cast(this)->rxso3(); - } - - /** - * \returns scale - */ - EIGEN_STRONG_INLINE - const Scalar scale() const { - return rxso3().scale(); - } - - /** - * \brief Setter of quaternion using rotation matrix, leaves scale untouched - * - * \param R a 3x3 rotation matrix - * \pre the 3x3 matrix should be orthogonal and have a determinant of 1 - */ - inline - void setRotationMatrix - (const Matrix & R) { - rxso3().setRotationMatrix(R); - } - - /** - * \brief Scale setter - */ - EIGEN_STRONG_INLINE - void setScale(const Scalar & scale) { - rxso3().setScale(scale); - } - - /** - * \brief Setter of quaternion using scaled rotation matrix - * - * \param sR a 3x3 scaled rotation matrix - * \pre the 3x3 matrix should be "scaled orthogonal" - * and have a positive determinant - */ - inline - void setScaledRotationMatrix - (const Matrix & sR) { - rxso3().setScaledRotationMatrix(sR); - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return static_cast(this)->translation(); - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return static_cast(this)->translation(); - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \param b 7-vector representation of Lie algebra element - * \returns derivative of Lie bracket - * - * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{sim3} \f$ - * with \f$ [a, b]_{sim3} \f$ being the lieBracket() of the Lie algebra sim3. - * - * \see lieBracket() - */ - inline static - const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { - const Matrix & upsilon2 = b.template head<3>(); - const Matrix & omega2 = b.template segment<3>(3); - Scalar sigma2 = b[6]; - - Adjoint res; - res.setZero(); - res.template topLeftCorner<3,3>() - = -SO3::hat(omega2)-sigma2*Matrix3d::Identity(); - res.template block<3,3>(0,3) = -SO3::hat(upsilon2); - res.template topRightCorner<3,1>() = upsilon2; - res.template block<3,3>(3,3) = -SO3::hat(omega2); - return res; - } - - /** - * \brief Group exponential - * - * \param a tangent space element (7-vector) - * \returns corresponding element of the group Sim3 - * - * The first three components of \f$ a \f$ represent the translational - * part \f$ \upsilon \f$ in the tangent space of Sim3, while the last three - * components of \f$ a \f$ represents the rotation vector \f$ \omega \f$. - * - * To be more specific, this function computes \f$ \exp(\widehat{a}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of Sim3. - * - * \see hat() - * \see log() - */ - inline static - const Sim3Group exp(const Tangent & a) { - const Matrix & upsilon = a.segment(0,3); - const Matrix & omega = a.segment(3,3); - Scalar sigma = a[6]; - Scalar theta; - RxSO3Group rxso3 - = RxSO3Group::expAndTheta(a.template tail<4>(), &theta); - const Matrix & Omega = SO3Group::hat(omega); - const Matrix & W = calcW(theta, sigma, rxso3.scale(), Omega); - return Sim3Group(rxso3, W*upsilon); - } - - /** - * \brief Generators - * - * \pre \f$ i \in \{0,1,2,3,4,5,6\} \f$ - * \returns \f$ i \f$th generator \f$ G_i \f$ of Sim3 - * - * The infinitesimal generators of Sim3 are: \f[ - * G_0 = \left( \begin{array}{cccc} - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_1 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_2 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 1\\ - * 0& 0& 0& 0\\ - * \end{array} \right). - * G_3 = \left( \begin{array}{cccc} - * 0& 0& 0& 0\\ - * 0& 0& -1& 0\\ - * 0& 1& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_4 = \left( \begin{array}{cccc} - * 0& 0& 1& 0\\ - * 0& 0& 0& 0\\ - * -1& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_5 = \left( \begin{array}{cccc} - * 0& -1& 0& 0\\ - * 1& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right), - * G_6 = \left( \begin{array}{cccc} - * 1& 0& 0& 0\\ - * 0& 1& 0& 0\\ - * 0& 0& 1& 0\\ - * 0& 0& 0& 0\\ - * \end{array} \right). - * \f] - * \see hat() - */ - inline static - const Transformation generator(int i) { - if (i<0 || i>6) { - throw SophusException("i is not in range [0,6]."); - } - Tangent e; - e.setZero(); - e[i] = static_cast(1); - return hat(e); - } - - /** - * \brief hat-operator - * - * \param omega 7-vector representation of Lie algebra element - * \returns 4x4-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of Sim3 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^7 \rightarrow \mathbf{R}^{4\times 4}, - * \quad \widehat{\omega} = \sum_{i=0}^5 G_i \omega_i \f$ - * with \f$ G_i \f$ being the ith infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & v) { - Transformation Omega; - Omega.template topLeftCorner<3,3>() - = RxSO3Group::hat(v.template tail<4>()); - Omega.col(3).template head<3>() = v.template head<3>(); - Omega.row(3).setZero(); - return Omega; - } - - /** - * \brief Lie bracket - * - * \param a 7-vector representation of Lie algebra element - * \param b 7-vector representation of Lie algebra element - * \returns 7-vector representation of Lie algebra element - * - * It computes the bracket of Sim3. To be more specific, it - * computes \f$ [a, b]_{sim3} - * := [\widehat{a}, \widehat{b}]^\vee \f$ - * with \f$ [A,B] = AB-BA \f$ being the matrix - * commutator, \f$ \widehat{\cdot} \f$ the - * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of Sim3. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & a, - const Tangent & b) { - const Matrix & upsilon1 = a.template head<3>(); - const Matrix & upsilon2 = b.template head<3>(); - const Matrix & omega1 = a.template segment<3>(3); - const Matrix & omega2 = b.template segment<3>(3); - Scalar sigma1 = a[6]; - Scalar sigma2 = b[6]; - - Tangent res; - res.template head<3>() = - SO3Group::hat(omega1)*upsilon2 - + SO3Group::hat(upsilon1)*omega2 - + sigma1*upsilon2 - sigma2*upsilon1; - res.template segment<3>(3) = omega1.cross(omega2); - res[6] = static_cast(0); - - return res; - } - - /** - * \brief Logarithmic map - * - * \param other element of the group Sim3 - * \returns corresponding tangent space element - * (translational part \f$ \upsilon \f$ - * and rotation vector \f$ \omega \f$) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of Sim3. - * - * \see exp() - * \see vee() - */ - inline static - const Tangent log(const Sim3Group & other) { - Tangent res; - Scalar theta; - const Matrix & omega_sigma - = RxSO3Group::logAndTheta(other.rxso3(), &theta); - const Matrix & omega = omega_sigma.template head<3>(); - Scalar sigma = omega_sigma[3]; - const Matrix & W - = calcW(theta, sigma, other.scale(), SO3Group::hat(omega)); - res.segment(0,3) = W.partialPivLu().solve(other.translation()); - res.segment(3,3) = omega; - res[6] = sigma; - return res; - } - - /** - * \brief vee-operator - * - * \param Omega 4x4-matrix representation of Lie algebra element - * \returns 7-vector representatin of Lie algebra element - * - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - Tangent upsilon_omega_sigma; - upsilon_omega_sigma.template head<3>() - = Omega.col(3).template head<3>(); - upsilon_omega_sigma.template tail<4>() - = RxSO3Group::vee(Omega.template topLeftCorner<3,3>()); - return upsilon_omega_sigma; - } - -private: - static - Matrix calcW(const Scalar & theta, - const Scalar & sigma, - const Scalar & scale, - const Matrix & Omega){ - static const Matrix I - = Matrix::Identity(); - static const Scalar one = static_cast(1.); - static const Scalar half = static_cast(1./2.); - Matrix Omega2 = Omega*Omega; - - Scalar A,B,C; - if (std::abs(sigma)::epsilon()) { - C = one; - if (std::abs(theta)::epsilon()) { - A = half; - B = static_cast(1./6.); - } else { - Scalar theta_sq = theta*theta; - A = (one-std::cos(theta))/theta_sq; - B = (theta-std::sin(theta))/(theta_sq*theta); - } - } else { - C = (scale-one)/sigma; - if (std::abs(theta)::epsilon()) { - Scalar sigma_sq = sigma*sigma; - A = ((sigma-one)*scale+one)/sigma_sq; - B = ((half*sigma*sigma-sigma+one)*scale)/(sigma_sq*sigma); - } else { - Scalar theta_sq = theta*theta; - Scalar a = scale*std::sin(theta); - Scalar b = scale*std::cos(theta); - Scalar c = theta_sq+sigma*sigma; - A = (a*sigma+ (one-b)*theta)/(theta*c); - B = (C-((b-one)*sigma+a*theta)/(c))*one/(theta_sq); - } - } - return A*Omega + B*Omega2 + C*I; - } -}; - -/** - * \brief Sim3 default type - Constructors and default storage for Sim3 Type - */ -template -class Sim3Group : public Sim3GroupBase > { - typedef Sim3GroupBase > Base; -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief RxSO3 reference type */ - typedef typename internal::traits > - ::RxSO3Type & RxSO3Reference; - /** \brief RxSO3 const reference type */ - typedef const typename internal::traits > - ::RxSO3Type & ConstRxSO3Reference; - /** \brief translation reference type */ - typedef typename internal::traits > - ::TranslationType & TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits > - ::TranslationType & ConstTranslationReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize Quaternion to identity rotation and translation to zero. - */ - inline - Sim3Group() - : translation_( Matrix::Zero() ) - { - } - - /** - * \brief Copy constructor - */ - template inline - Sim3Group(const Sim3GroupBase & other) - : rxso3_(other.rxso3()), translation_(other.translation()) { - } - - /** - * \brief Constructor from RxSO3 and translation vector - */ - template inline - Sim3Group(const RxSO3GroupBase & rxso3, - const Point & translation) - : rxso3_(rxso3), translation_(translation) { - } - - /** - * \brief Constructor from quaternion and translation vector - * - * \pre quaternion must not be zero - */ - inline - Sim3Group(const Quaternion & quaternion, - const Point & translation) - : rxso3_(quaternion), translation_(translation) { - } - - /** - * \brief Constructor from 4x4 matrix - * - * \pre top-left 3x3 sub-matrix need to be "scaled orthogonal" - * with positive determinant of - */ - inline explicit - Sim3Group(const Eigen::Matrix& T) - : rxso3_(T.template topLeftCorner<3,3>()), - translation_(T.template block<3,1>(0,3)) { - } - - /** - * \returns pointer to internal data - * - * This provides unsafe read/write access to internal data. Sim3 is - * represented by a pair of an RxSO3 element (4 parameters) and translation - * vector (three parameters). - * - * Note: The first three Scalars represent the imaginary parts, while the - */ - EIGEN_STRONG_INLINE - Scalar* data() { - // rxso3_ and translation_ are layed out sequentially with no padding - return rxso3_.data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - EIGEN_STRONG_INLINE - const Scalar* data() const { - // rxso3_ and translation_ are layed out sequentially with no padding - return rxso3_.data(); - } - - /** - * \brief Accessor of RxSO3 - */ - EIGEN_STRONG_INLINE - RxSO3Reference rxso3() { - return rxso3_; - } - - /** - * \brief Mutator of RxSO3 - */ - EIGEN_STRONG_INLINE - ConstRxSO3Reference rxso3() const { - return rxso3_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Sophus::RxSO3Group rxso3_; - Matrix translation_; -}; - - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for Sim3GroupBase - * - * Allows us to wrap Sim3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::Sim3GroupBase, _Options> > { - typedef Sophus::Sim3GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation reference type */ - typedef typename internal::traits::TranslationType & - TranslationReference; - /** \brief translation const reference type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief RxSO3 reference type */ - typedef typename internal::traits::RxSO3Type & - RxSO3Reference; - /** \brief RxSO3 const reference type */ - typedef const typename internal::traits::RxSO3Type & - ConstRxSO3Reference; - - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) - : rxso3_(coeffs), - translation_(coeffs+Sophus::RxSO3Group::num_parameters) { - } - - /** - * \brief Mutator of RxSO3 - */ - EIGEN_STRONG_INLINE - RxSO3Reference rxso3() { - return rxso3_; - } - - /** - * \brief Accessor of RxSO3 - */ - EIGEN_STRONG_INLINE - ConstRxSO3Reference rxso3() const { - return rxso3_; - } - - /** - * \brief Mutator of translation vector - */ - EIGEN_STRONG_INLINE - TranslationReference translation() { - return translation_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - Map,_Options> rxso3_; - Map,_Options> translation_; -}; - -/** - * \brief Specialisation of Eigen::Map for const Sim3GroupBase - * - * Allows us to wrap Sim3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::Sim3GroupBase< - Map, _Options> > { - typedef Sophus::Sim3GroupBase< - Map, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief translation type */ - typedef const typename internal::traits::TranslationType & - ConstTranslationReference; - /** \brief RxSO3 const reference type */ - typedef const typename internal::traits::RxSO3Type & - ConstRxSO3Reference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) - : rxso3_(coeffs), - translation_(coeffs+Sophus::RxSO3Group::num_parameters) { - } - - EIGEN_STRONG_INLINE - Map(const Scalar* trans_coeffs, const Scalar* rot_coeffs) - : translation_(trans_coeffs), rxso3_(rot_coeffs){ - } - - /** - * \brief Accessor of RxSO3 - */ - EIGEN_STRONG_INLINE - ConstRxSO3Reference rxso3() const { - return rxso3_; - } - - /** - * \brief Accessor of translation vector - */ - EIGEN_STRONG_INLINE - ConstTranslationReference translation() const { - return translation_; - } - -protected: - const Map,_Options> rxso3_; - const Map,_Options> translation_; -}; - -} - -#endif diff --git a/thirdparty/Sophus/sophus/so2.hpp b/thirdparty/Sophus/sophus/so2.hpp deleted file mode 100644 index d2e8160..0000000 --- a/thirdparty/Sophus/sophus/so2.hpp +++ /dev/null @@ -1,701 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_SO2_HPP -#define SOPHUS_SO2_HPP - -#include - -#include "sophus.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class SO2Group; -typedef SO2Group SO2 EIGEN_DEPRECATED; -typedef SO2Group SO2d; /**< double precision SO2 */ -typedef SO2Group SO2f; /**< single precision SO2 */ -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Matrix ComplexType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> ComplexType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> ComplexType; -}; - -} -} - -namespace Sophus { -using namespace Eigen; - -/** - * \brief SO2 base type - implements SO2 class but is storage agnostic - * - * [add more detailed description/tutorial] - */ -template -class SO2GroupBase { -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief complex number reference type */ - typedef typename internal::traits::ComplexType & - ComplexReference; - /** \brief complex number const reference type */ - typedef const typename internal::traits::ComplexType & - ConstComplexReference; - - /** \brief degree of freedom of group - * (one for in-plane rotation) */ - static const int DoF = 1; - /** \brief number of internal parameters used - * (unit complex number for rotation) */ - static const int num_parameters = 2; - /** \brief group transformations are NxN matrices */ - static const int N = 2; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Scalar Tangent; - /** \brief adjoint transformation type */ - typedef Scalar Adjoint; - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-operator. - * - * For SO2, it simply returns 1. - */ - inline - const Adjoint Adj() const { - return 1; - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline SO2Group cast() const { - return SO2Group(unit_complex() - .template cast() ); - } - - /** - * \returns pointer to internal data - * - * This provides unsafe read/write access to internal data. SO2 is represented - * by a complex number with unit length (two parameters). When using direct - * write access, the user needs to take care of that the complex number stays - * normalized. - * - * \see normalize() - */ - inline Scalar* data() { - return unit_complex_nonconst().data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - inline const Scalar* data() const { - return unit_complex().data(); - } - - /** - * \brief Fast group multiplication - * - * This method is a fast version of operator*=(), since it does not perform - * normalization. It is up to the user to call normalize() once in a while. - * - * \see operator*=() - */ - inline - void fastMultiply(const SO2Group& other) { - Scalar lhs_real = unit_complex().x(); - Scalar lhs_imag = unit_complex().y(); - const Scalar & rhs_real = other.unit_complex().x(); - const Scalar & rhs_imag = other.unit_complex().y(); - // complex multiplication - unit_complex_nonconst().x() = lhs_real*rhs_real - lhs_imag*rhs_imag; - unit_complex_nonconst().y() = lhs_real*rhs_imag + lhs_imag*rhs_real; - } - - /** - * \returns group inverse of instance - */ - inline - const SO2Group inverse() const { - return SO2Group(unit_complex().x(), -unit_complex().y()); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation (=rotation angle) of instance - * - * \see log(). - */ - inline - const Scalar log() const { - return SO2Group::log(*this); - } - - /** - * \brief Normalize complex number - * - * It re-normalizes complex number to unit length. This method only needs to - * be called in conjunction with fastMultiply() or data() write access. - */ - inline - void normalize() { - Scalar length = - std::sqrt(unit_complex().x()*unit_complex().x() - + unit_complex().y()*unit_complex().y()); - if(length < SophusConstants::epsilon()) { - throw SophusException("Complex number is (near) zero!"); - } - unit_complex_nonconst().x() /= length; - unit_complex_nonconst().y() /= length; - } - - /** - * \returns 2x2 matrix representation of instance - * - * For SO2, the matrix representation is an orthogonal matrix R with det(R)=1, - * thus the so-called rotation matrix. - */ - inline - const Transformation matrix() const { - const Scalar & real = unit_complex().x(); - const Scalar & imag = unit_complex().y(); - Transformation R; - R << real, -imag - ,imag, real; - return R; - } - - /** - * \brief Assignment operator - */ - template inline - SO2GroupBase& operator=(const SO2GroupBase & other) { - unit_complex_nonconst() = other.unit_complex(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const SO2Group operator*(const SO2Group& other) const { - SO2Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^2 \f$ - * - * \param p point \f$p \in \mathbf{R}^2 \f$ - * \returns point \f$p' \in \mathbf{R}^2 \f$, rotated version of \f$p\f$ - * - * This function rotates a point \f$ p \f$ in \f$ \mathbf{R}^2 \f$ by the - * SO2 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$. - */ - inline - const Point operator*(const Point & p) const { - const Scalar & real = unit_complex().x(); - const Scalar & imag = unit_complex().y(); - return Point(real*p[0] - imag*p[1], imag*p[0] + real*p[1]); - } - - /** - * \brief In-place group multiplication - * - * \see fastMultiply() - * \see operator*() - */ - inline - void operator*=(const SO2Group& other) { - fastMultiply(other); - normalize(); - } - - /** - * \brief Setter of internal unit complex number representation - * - * \param complex - * \pre the complex number must not be near zero - * - * The complex number is normalized to unit length. - */ - inline - void setComplex(const Point & complex) { - unit_complex() = complex; - normalize(); - } - - /** - * \brief Accessor of unit complex number - * - * No direct write access is given to ensure the complex stays normalized. - */ - EIGEN_STRONG_INLINE - ConstComplexReference unit_complex() const { - return static_cast(this)->unit_complex(); - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \brief Group exponential - * - * \param theta tangent space element (=rotation angle \f$ \theta \f$) - * \returns corresponding element of the group SO2 - * - * To be more specific, this function computes \f$ \exp(\widehat{\theta}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO2. - * - * \see hat() - * \see log() - */ - inline static - const SO2Group exp(const Tangent & theta) { - return SO2Group(std::cos(theta), std::sin(theta)); - } - - /** - * \brief Generator - * - * The infinitesimal generator of SO2 - * is \f$ - * G_0 = \left( \begin{array}{ccc} - * 0& -1& \\ - * 1& 0& - * \end{array} \right). - * \f$ - * \see hat() - */ - inline static - const Transformation generator() { - return hat(1); - } - - /** - * \brief hat-operator - * - * \param theta scalar representation of Lie algebra element - * \returns 2x2-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of SO2 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^2 \rightarrow \mathbf{R}^{2\times 2}, - * \quad \widehat{\theta} = G_0\cdot \theta \f$ - * with \f$ G_0 \f$ being the infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & theta) { - Transformation Omega; - Omega << static_cast(0), -theta - , theta, static_cast(0); - return Omega; - } - - /** - * \brief Lie bracket - * - * \param theta1 scalar representation of Lie algebra element - * \param theta2 scalar representation of Lie algebra element - * \returns zero - * - * It computes the bracket. For the Lie algebra so2, the Lie bracket is - * simply \f$ [\theta_1, \theta_2]_{so2} = 0 \f$ since SO2 is a - * commutative group. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & theta1, - const Tangent & theta2) { - return static_cast(0); - } - - /** - * \brief Logarithmic map - * - * \param other element of the group SO2 - * \returns corresponding tangent space element - * (=rotation angle \f$ \theta \f$) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of SO2. - * - * \see exp() - * \see vee() - */ - inline static - const Tangent log(const SO2Group & other) { - // todo: general implementation for Scalar not being float or double. - return atan2(other.unit_complex_.y(), other.unit_complex().x()); - } - - /** - * \brief vee-operator - * - * \param Omega 2x2-matrix representation of Lie algebra element - * \pre Omega need to be a skew-symmetric matrix - * \returns scalar representatin of Lie algebra element - *s - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - return static_cast(0.5)*(Omega(1,0) - Omega(0,1)); - } - -private: - // Mutator of complex number is private so users are hampered - // from setting non-unit complex numbers. - EIGEN_STRONG_INLINE - ComplexReference unit_complex_nonconst() { - return static_cast(this)->unit_complex_nonconst(); - } - -}; - -/** - * \brief SO2 default type - Constructors and default storage for SO2 Type - */ -template -class SO2Group : public SO2GroupBase > { - typedef SO2GroupBase > Base; -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief complex number reference type */ - typedef typename internal::traits > - ::ComplexType & ComplexReference; - /** \brief complex number const reference type */ - typedef const typename internal::traits > - ::ComplexType & ConstComplexReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - // base is friend so unit_complex_nonconst can be accessed from base - friend class SO2GroupBase >; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize complex number to identity rotation. - */ - inline SO2Group() - : unit_complex_(static_cast(1), static_cast(0)) { - } - - /** - * \brief Copy constructor - */ - template inline - SO2Group(const SO2GroupBase & other) - : unit_complex_(other.unit_complex()) { - } - - /** - * \brief Constructor from rotation matrix - * - * \pre rotation matrix need to be orthogonal with determinant of 1 - */ - inline explicit - SO2Group(const Transformation & R) - : unit_complex_(static_cast(0.5)*(R(0,0)+R(1,1)), - static_cast(0.5)*(R(1,0)-R(0,1))) { - if (std::abs(R.determinant()-static_cast(1)) - > SophusConstants::epsilon()) { - throw SophusException("det(R) is not near 1."); - } - } - - /** - * \brief Constructor from pair of real and imaginary number - * - * \pre pair must not be zero - */ - inline SO2Group(const Scalar & real, const Scalar & imag) - : unit_complex_(real, imag) { - Base::normalize(); - } - - /** - * \brief Constructor from 2-vector - * - * \pre vector must not be zero - */ - inline explicit - SO2Group(const Matrix & complex) - : unit_complex_(complex) { - Base::normalize(); - } - - /** - * \brief Constructor from std::complex - * - * \pre complex number must not be zero - */ - inline explicit - SO2Group(const std::complex & complex) - : unit_complex_(complex.real(), complex.imag()) { - Base::normalize(); - } - - /** - * \brief Constructor from an angle - */ - inline explicit - SO2Group(Scalar theta) { - unit_complex_nonconst() = SO2Group::exp(theta).unit_complex(); - } - - /** - * \brief Accessor of unit complex number - * - * No direct write access is given to ensure the complex number stays - * normalized. - */ - EIGEN_STRONG_INLINE - ConstComplexReference unit_complex() const { - return unit_complex_; - } - -protected: - // Mutator of complex number is protected so users are hampered - // from setting non-unit complex numbers. - EIGEN_STRONG_INLINE - ComplexReference unit_complex_nonconst() { - return unit_complex_; - } - - static bool isNearZero(const Scalar & real, const Scalar & imag) { - return (real*real + imag*imag < SophusConstants::epsilon()); - } - - Matrix unit_complex_; -}; - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for SO2GroupBase - * - * Allows us to wrap SO2 Objects around POD array - * (e.g. external c style complex number) - */ -template -class Map, _Options> - : public Sophus::SO2GroupBase, _Options> > { - typedef Sophus::SO2GroupBase, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief complex number reference type */ - typedef typename internal::traits::ComplexType & ComplexReference; - /** \brief complex number const reference type */ - typedef const typename internal::traits::ComplexType & - ConstComplexReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - // base is friend so unit_complex_nonconst can be accessed from base - friend class Sophus::SO2GroupBase, _Options> >; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) : unit_complex_(coeffs) { - } - - /** - * \brief Accessor of unit complex number - * - * No direct write access is given to ensure the complex number stays - * normalized. - */ - EIGEN_STRONG_INLINE - ConstComplexReference unit_complex() const { - return unit_complex_; - } - -protected: - // Mutator of complex number is protected so users are hampered - // from setting non-unit complex number. - EIGEN_STRONG_INLINE - ComplexReference unit_complex_nonconst() { - return unit_complex_; - } - - Map,_Options> unit_complex_; -}; - -/** - * \brief Specialisation of Eigen::Map for const SO2GroupBase - * - * Allows us to wrap SO2 Objects around POD array - * (e.g. external c style complex number) - */ -template -class Map, _Options> - : public Sophus::SO2GroupBase< - Map, _Options> > { - typedef Sophus::SO2GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief complex number const reference type */ - typedef const typename internal::traits::ComplexType & - ConstComplexReference; - - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) : unit_complex_(coeffs) { - } - - /** - * \brief Accessor of unit complex number - * - * No direct write access is given to ensure the complex number stays - * normalized. - */ - EIGEN_STRONG_INLINE - ConstComplexReference unit_complex() const { - return unit_complex_; - } - -protected: - const Map,_Options> unit_complex_; -}; - -} - - -#endif // SOPHUS_SO2_HPP diff --git a/thirdparty/Sophus/sophus/so3.hpp b/thirdparty/Sophus/sophus/so3.hpp deleted file mode 100644 index 88a362e..0000000 --- a/thirdparty/Sophus/sophus/so3.hpp +++ /dev/null @@ -1,811 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2011-2013 Hauke Strasdat -// Copyrifht 2012-2013 Steven Lovegrove -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_SO3_HPP -#define SOPHUS_SO3_HPP - -#include "sophus.hpp" - -//////////////////////////////////////////////////////////////////////////// -// Forward Declarations / typedefs -//////////////////////////////////////////////////////////////////////////// - -namespace Sophus { -template class SO3Group; -typedef EIGEN_DEPRECATED SO3Group SO3; -typedef SO3Group SO3d; /**< double precision SO3 */ -typedef SO3Group SO3f; /**< single precision SO3 */ -} - -//////////////////////////////////////////////////////////////////////////// -// Eigen Traits (For querying derived types in CRTP hierarchy) -//////////////////////////////////////////////////////////////////////////// - -namespace Eigen { -namespace internal { - -template -struct traits > { - typedef _Scalar Scalar; - typedef Quaternion QuaternionType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> QuaternionType; -}; - -template -struct traits, _Options> > - : traits > { - typedef _Scalar Scalar; - typedef Map,_Options> QuaternionType; -}; - -} -} - -namespace Sophus { -using namespace Eigen; - -/** - * \brief SO3 base type - implements SO3 class but is storage agnostic - * - * [add more detailed description/tutorial] - */ -template -class SO3GroupBase { -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion reference type */ - typedef typename internal::traits::QuaternionType & - QuaternionReference; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - /** \brief degree of freedom of group - * (three for rotation) */ - static const int DoF = 3; - /** \brief number of internal parameters used - * (unit quaternion for rotation) */ - static const int num_parameters = 4; - /** \brief group transformations are NxN matrices */ - static const int N = 3; - /** \brief group transfomation type */ - typedef Matrix Transformation; - /** \brief point type */ - typedef Matrix Point; - /** \brief tangent vector type */ - typedef Matrix Tangent; - /** \brief adjoint transformation type */ - typedef Matrix Adjoint; - - /** - * \brief Adjoint transformation - * - * This function return the adjoint transformation \f$ Ad \f$ of the - * group instance \f$ A \f$ such that for all \f$ x \f$ - * it holds that \f$ \widehat{Ad_A\cdot x} = A\widehat{x}A^{-1} \f$ - * with \f$\ \widehat{\cdot} \f$ being the hat()-Vector4Scalaror. - * - * For SO3, it simply returns the rotation matrix corresponding to \f$ A \f$. - */ - inline - const Adjoint Adj() const { - return matrix(); - } - - /** - * \returns copy of instance casted to NewScalarType - */ - template - inline SO3Group cast() const { - return SO3Group(unit_quaternion() - .template cast() ); - } - - /** - * \returns pointer to internal data - * - * This provides unsafe read/write access to internal data. SO3 is represented - * by an Eigen::Quaternion (four parameters). When using direct write access, - * the user needs to take care of that the quaternion stays normalized. - * - * Note: The first three Scalars represent the imaginary parts, while the - * forth Scalar represent the real part. - * - * \see normalize() - */ - inline Scalar* data() { - return unit_quaternion_nonconst().coeffs().data(); - } - - /** - * \returns const pointer to internal data - * - * Const version of data(). - */ - inline const Scalar* data() const { - return unit_quaternion().coeffs().data(); - } - - /** - * \brief Fast group multiplication - * - * This method is a fast version of operator*=(), since it does not perform - * normalization. It is up to the user to call normalize() once in a while. - * - * \see operator*=() - */ - inline - void fastMultiply(const SO3Group& other) { - unit_quaternion_nonconst() *= other.unit_quaternion(); - } - - /** - * \returns group inverse of instance - */ - inline - const SO3Group inverse() const { - return SO3Group(unit_quaternion().conjugate()); - } - - /** - * \brief Logarithmic map - * - * \returns tangent space representation (=rotation vector) of instance - * - * \see log(). - */ - inline - const Tangent log() const { - return SO3Group::log(*this); - } - - /** - * \brief Normalize quaternion - * - * It re-normalizes unit_quaternion to unit length. This method only needs to - * be called in conjunction with fastMultiply() or data() write access. - */ - inline - void normalize() { - Scalar length = unit_quaternion_nonconst().norm(); - if (length < SophusConstants::epsilon()) { - throw SophusException("Quaternion is (near) zero!"); - } - unit_quaternion_nonconst().coeffs() /= length; - } - - /** - * \returns 3x3 matrix representation of instance - * - * For SO3, the matrix representation is an orthogonal matrix R with det(R)=1, - * thus the so-called rotation matrix. - */ - inline - const Transformation matrix() const { - return unit_quaternion().toRotationMatrix(); - } - - /** - * \brief Assignment operator - */ - template inline - SO3GroupBase& operator=(const SO3GroupBase & other) { - unit_quaternion_nonconst() = other.unit_quaternion(); - return *this; - } - - /** - * \brief Group multiplication - * \see operator*=() - */ - inline - const SO3Group operator*(const SO3Group& other) const { - SO3Group result(*this); - result *= other; - return result; - } - - /** - * \brief Group action on \f$ \mathbf{R}^3 \f$ - * - * \param p point \f$p \in \mathbf{R}^3 \f$ - * \returns point \f$p' \in \mathbf{R}^3 \f$, rotated version of \f$p\f$ - * - * This function rotates a point \f$ p \f$ in \f$ \mathbf{R}^3 \f$ by the - * SO3 transformation \f$R\f$ (=rotation matrix): \f$ p' = R\cdot p \f$. - * - * - * Since SO3 is intenally represented by a unit quaternion \f$q\f$, it is - * implemented as \f$ p' = q\cdot p \cdot q^{*} \f$ - * with \f$ q^{*} \f$ being the quaternion conjugate of \f$ q \f$. - * - * Geometrically, \f$ p \f$ is rotated by angle \f$|\omega|\f$ around the - * axis \f$\frac{\omega}{|\omega|}\f$ with \f$ \omega = \log(R)^\vee \f$. - * - * \see log() - */ - inline - const Point operator*(const Point & p) const { - return unit_quaternion()._transformVector(p); - } - - /** - * \brief In-place group multiplication - * - * \see fastMultiply() - * \see operator*() - */ - inline - void operator*=(const SO3Group& other) { - fastMultiply(other); - normalize(); - } - - /** - * \brief Setter of internal unit quaternion representation - * - * \param quaternion - * \pre the quaternion must not be zero - * - * The quaternion is normalized to unit length. - */ - inline - void setQuaternion(const Quaternion& quaternion) { - unit_quaternion_nonconst() = quaternion; - normalize(); - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference unit_quaternion() const { - return static_cast(this)->unit_quaternion(); - } - - //////////////////////////////////////////////////////////////////////////// - // public static functions - //////////////////////////////////////////////////////////////////////////// - - /** - * \param b 3-vector representation of Lie algebra element - * \returns derivative of Lie bracket - * - * This function returns \f$ \frac{\partial}{\partial a} [a, b]_{so3} \f$ - * with \f$ [a, b]_{so3} \f$ being the lieBracket() of the Lie algebra so3. - * - * \see lieBracket() - */ - inline static - const Adjoint d_lieBracketab_by_d_a(const Tangent & b) { - return -hat(b); - } - - /** - * \brief Group exponential - * - * \param omega tangent space element (=rotation vector \f$ \omega \f$) - * \returns corresponding element of the group SO3 - * - * To be more specific, this function computes \f$ \exp(\widehat{\omega}) \f$ - * with \f$ \exp(\cdot) \f$ being the matrix exponential - * and \f$ \widehat{\cdot} \f$ the hat()-operator of SO3. - * - * \see expAndTheta() - * \see hat() - * \see log() - */ - inline static - const SO3Group exp(const Tangent & omega) { - Scalar theta; - return expAndTheta(omega, &theta); - } - - /** - * \brief Group exponential and theta - * - * \param omega tangent space element (=rotation vector \f$ \omega \f$) - * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ - * \returns corresponding element of the group SO3 - * - * \see exp() for details - */ - inline static - const SO3Group expAndTheta(const Tangent & omega, - Scalar * theta) { - const Scalar theta_sq = omega.squaredNorm(); - *theta = std::sqrt(theta_sq); - const Scalar half_theta = static_cast(0.5)*(*theta); - - Scalar imag_factor; - Scalar real_factor;; - if((*theta)::epsilon()) { - const Scalar theta_po4 = theta_sq*theta_sq; - imag_factor = static_cast(0.5) - - static_cast(1.0/48.0)*theta_sq - + static_cast(1.0/3840.0)*theta_po4; - real_factor = static_cast(1) - - static_cast(0.5)*theta_sq + - static_cast(1.0/384.0)*theta_po4; - } else { - const Scalar sin_half_theta = std::sin(half_theta); - imag_factor = sin_half_theta/(*theta); - real_factor = std::cos(half_theta); - } - - return SO3Group(Quaternion(real_factor, - imag_factor*omega.x(), - imag_factor*omega.y(), - imag_factor*omega.z())); - } - - /** - * \brief Generators - * - * \pre \f$ i \in \{0,1,2\} \f$ - * \returns \f$ i \f$th generator \f$ G_i \f$ of SO3 - * - * The infinitesimal generators of SO3 - * are \f$ - * G_0 = \left( \begin{array}{ccc} - * 0& 0& 0& \\ - * 0& 0& -1& \\ - * 0& 1& 0& - * \end{array} \right), - * G_1 = \left( \begin{array}{ccc} - * 0& 0& 1& \\ - * 0& 0& 0& \\ - * -1& 0& 0& - * \end{array} \right), - * G_2 = \left( \begin{array}{ccc} - * 0& -1& 0& \\ - * 1& 0& 0& \\ - * 0& 0& 0& - * \end{array} \right). - * \f$ - * \see hat() - */ - inline static - const Transformation generator(int i) { - if (i<0 || i>2) { - throw SophusException("i is not in range [0,2]."); - } - Tangent e; - e.setZero(); - e[i] = static_cast(1); - return hat(e); - } - - /** - * \brief hat-operator - * - * \param omega 3-vector representation of Lie algebra element - * \returns 3x3-matrix representatin of Lie algebra element - * - * Formally, the hat-operator of SO3 is defined - * as \f$ \widehat{\cdot}: \mathbf{R}^3 \rightarrow \mathbf{R}^{3\times 3}, - * \quad \widehat{\omega} = \sum_{i=0}^2 G_i \omega_i \f$ - * with \f$ G_i \f$ being the ith infinitesial generator(). - * - * \see generator() - * \see vee() - */ - inline static - const Transformation hat(const Tangent & omega) { - Transformation Omega; - Omega << static_cast(0), -omega(2), omega(1) - , omega(2), static_cast(0), -omega(0) - , -omega(1), omega(0), static_cast(0); - return Omega; - } - - /** - * \brief Lie bracket - * - * \param omega1 3-vector representation of Lie algebra element - * \param omega2 3-vector representation of Lie algebra element - * \returns 3-vector representation of Lie algebra element - * - * It computes the bracket of SO3. To be more specific, it - * computes \f$ [\omega_1, \omega_2]_{so3} - * := [\widehat{\omega_1}, \widehat{\omega_2}]^\vee \f$ - * with \f$ [A,B] = AB-BA \f$ being the matrix - * commutator, \f$ \widehat{\cdot} \f$ the - * hat()-operator and \f$ (\cdot)^\vee \f$ the vee()-operator of SO3. - * - * For the Lie algebra so3, the Lie bracket is simply the - * cross product: \f$ [\omega_1, \omega_2]_{so3} - * = \omega_1 \times \omega_2 \f$. - * - * \see hat() - * \see vee() - */ - inline static - const Tangent lieBracket(const Tangent & omega1, - const Tangent & omega2) { - return omega1.cross(omega2); - } - - /** - * \brief Logarithmic map - * - * \param other element of the group SO3 - * \returns corresponding tangent space element - * (=rotation vector \f$ \omega \f$) - * - * Computes the logarithmic, the inverse of the group exponential. - * To be specific, this function computes \f$ \log({\cdot})^\vee \f$ - * with \f$ \vee(\cdot) \f$ being the matrix logarithm - * and \f$ \vee{\cdot} \f$ the vee()-operator of SO3. - * - * \see exp() - * \see logAndTheta() - * \see vee() - */ - inline static - const Tangent log(const SO3Group & other) { - Scalar theta; - return logAndTheta(other, &theta); - } - - /** - * \brief Logarithmic map and theta - * - * \param other element of the group SO3 - * \param[out] theta angle of rotation \f$ \theta = |\omega| \f$ - * \returns corresponding tangent space element - * (=rotation vector \f$ \omega \f$) - * - * \see log() for details - */ - inline static - const Tangent logAndTheta(const SO3Group & other, - Scalar * theta) { - const Scalar squared_n - = other.unit_quaternion().vec().squaredNorm(); - const Scalar n = std::sqrt(squared_n); - const Scalar w = other.unit_quaternion().w(); - - Scalar two_atan_nbyw_by_n; - - // Atan-based log thanks to - // - // C. Hertzberg et al.: - // "Integrating Generic Sensor Fusion Algorithms with Sound State - // Representation through Encapsulation of Manifolds" - // Information Fusion, 2011 - - if (n < SophusConstants::epsilon()) { - // If quaternion is normalized and n=0, then w should be 1; - // w=0 should never happen here! - if (std::abs(w) < SophusConstants::epsilon()) { - throw SophusException("Quaternion is not normalized!"); - } - const Scalar squared_w = w*w; - two_atan_nbyw_by_n = static_cast(2) / w - - static_cast(2)*(squared_n)/(w*squared_w); - } else { - if (std::abs(w)::epsilon()) { - if (w > static_cast(0)) { - two_atan_nbyw_by_n = M_PI/n; - } else { - two_atan_nbyw_by_n = -M_PI/n; - } - }else{ - two_atan_nbyw_by_n = static_cast(2) * atan(n/w) / n; - } - } - - *theta = two_atan_nbyw_by_n*n; - - return two_atan_nbyw_by_n * other.unit_quaternion().vec(); - } - - /** - * \brief vee-operator - * - * \param Omega 3x3-matrix representation of Lie algebra element - * \pr Omega must be a skew-symmetric matrix - * \returns 3-vector representatin of Lie algebra element - * - * This is the inverse of the hat()-operator. - * - * \see hat() - */ - inline static - const Tangent vee(const Transformation & Omega) { - return static_cast(0.5) * Tangent(Omega(2,1) - Omega(1,2), - Omega(0,2) - Omega(2,0), - Omega(1,0) - Omega(0,1)); - } - -private: - // Mutator of unit_quaternion is private so users are hampered - // from setting non-unit quaternions. - EIGEN_STRONG_INLINE - QuaternionReference unit_quaternion_nonconst() { - return static_cast(this)->unit_quaternion_nonconst(); - } - -}; - -/** - * \brief SO3 default type - Constructors and default storage for SO3 Type - */ -template -class SO3Group : public SO3GroupBase > { - typedef SO3GroupBase > Base; -public: - /** \brief scalar type */ - typedef typename internal::traits > - ::Scalar Scalar; - /** \brief quaternion type */ - typedef typename internal::traits > - ::QuaternionType & QuaternionReference; - typedef const typename internal::traits > - ::QuaternionType & ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - // base is friend so unit_quaternion_nonconst can be accessed from base - friend class SO3GroupBase >; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** - * \brief Default constructor - * - * Initialize Quaternion to identity rotation. - */ - inline - SO3Group() - : unit_quaternion_(static_cast(1), static_cast(0), - static_cast(0), static_cast(0)) { - } - - /** - * \brief Copy constructor - */ - template inline - SO3Group(const SO3GroupBase & other) - : unit_quaternion_(other.unit_quaternion()) { - } - - /** - * \brief Constructor from rotation matrix - * - * \pre rotation matrix need to be orthogonal with determinant of 1 - */ - inline SO3Group(const Transformation & R) - : unit_quaternion_(R) { - } - - /** - * \brief Constructor from quaternion - * - * \pre quaternion must not be zero - */ - inline explicit - SO3Group(const Quaternion & quat) : unit_quaternion_(quat) { - Base::normalize(); - } - - /** - * \brief Constructor from Euler angles - * - * \param alpha1 rotation around x-axis - * \param alpha2 rotation around y-axis - * \param alpha3 rotation around z-axis - * - * Since rotations in 3D do not commute, the order of the individual rotations - * matter. Here, the following convention is used. We calculate a SO3 member - * corresponding to the rotation matrix \f$ R \f$ such - * that \f$ R=\exp\left(\begin{array}{c}\alpha_1\\ 0\\ 0\end{array}\right) - * \cdot \exp\left(\begin{array}{c}0\\ \alpha_2\\ 0\end{array}\right) - * \cdot \exp\left(\begin{array}{c}0\\ 0\\ \alpha_3\end{array}\right)\f$. - */ - inline - SO3Group(Scalar alpha1, Scalar alpha2, Scalar alpha3) { - const static Scalar zero = static_cast(0); - unit_quaternion_ - = ( SO3Group::exp(Tangent(alpha1, zero, zero)) - *SO3Group::exp(Tangent( zero, alpha2, zero)) - *SO3Group::exp(Tangent( zero, zero, alpha3)) ) - .unit_quaternion_; - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference unit_quaternion() const { - return unit_quaternion_; - } - -protected: - // Mutator of unit_quaternion is protected so users are hampered - // from setting non-unit quaternions. - EIGEN_STRONG_INLINE - QuaternionReference unit_quaternion_nonconst() { - return unit_quaternion_; - } - - Quaternion unit_quaternion_; -}; - -} // end namespace - - -namespace Eigen { -/** - * \brief Specialisation of Eigen::Map for SO3GroupBase - * - * Allows us to wrap SO3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::SO3GroupBase, _Options> > { - typedef Sophus::SO3GroupBase, _Options> > Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion reference type */ - typedef typename internal::traits::QuaternionType & - QuaternionReference; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - // base is friend so unit_quaternion_nonconst can be accessed from base - friend class Sophus::SO3GroupBase, _Options> >; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(Scalar* coeffs) : unit_quaternion_(coeffs) { - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - EIGEN_STRONG_INLINE - ConstQuaternionReference unit_quaternion() const { - return unit_quaternion_; - } - -protected: - // Mutator of unit_quaternion is protected so users are hampered - // from setting non-unit quaternions. - EIGEN_STRONG_INLINE - QuaternionReference unit_quaternion_nonconst() { - return unit_quaternion_; - } - - Map,_Options> unit_quaternion_; -}; - -/** - * \brief Specialisation of Eigen::Map for const SO3GroupBase - * - * Allows us to wrap SO3 Objects around POD array - * (e.g. external c style quaternion) - */ -template -class Map, _Options> - : public Sophus::SO3GroupBase< - Map, _Options> > { - typedef Sophus::SO3GroupBase, _Options> > - Base; - -public: - /** \brief scalar type */ - typedef typename internal::traits::Scalar Scalar; - /** \brief quaternion const reference type */ - typedef const typename internal::traits::QuaternionType & - ConstQuaternionReference; - - /** \brief degree of freedom of group */ - static const int DoF = Base::DoF; - /** \brief number of internal parameters used */ - static const int num_parameters = Base::num_parameters; - /** \brief group transformations are NxN matrices */ - static const int N = Base::N; - /** \brief group transfomation type */ - typedef typename Base::Transformation Transformation; - /** \brief point type */ - typedef typename Base::Point Point; - /** \brief tangent vector type */ - typedef typename Base::Tangent Tangent; - /** \brief adjoint transformation type */ - typedef typename Base::Adjoint Adjoint; - - EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map) - using Base::operator*=; - using Base::operator*; - - EIGEN_STRONG_INLINE - Map(const Scalar* coeffs) : unit_quaternion_(coeffs) { - } - - /** - * \brief Accessor of unit quaternion - * - * No direct write access is given to ensure the quaternion stays normalized. - */ - EIGEN_STRONG_INLINE - const ConstQuaternionReference unit_quaternion() const { - return unit_quaternion_; - } - -protected: - const Map,_Options> unit_quaternion_; -}; - -} - -#endif diff --git a/thirdparty/Sophus/sophus/sophus.hpp b/thirdparty/Sophus/sophus/sophus.hpp deleted file mode 100644 index 76e142e..0000000 --- a/thirdparty/Sophus/sophus/sophus.hpp +++ /dev/null @@ -1,77 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#ifndef SOPHUS_HPP -#define SOPHUS_HPP - -#include - -// fix log1p not being found on Android in Eigen -#if defined( ANDROID ) -#include -namespace std { - using ::log1p; -} -#endif - -#include -#include - -namespace Sophus { -using namespace Eigen; - -template -struct SophusConstants { - EIGEN_ALWAYS_INLINE static - const Scalar epsilon() { - return static_cast(1e-10); - } - - EIGEN_ALWAYS_INLINE static - const Scalar pi() { - return static_cast(M_PI); - } -}; - -template<> -struct SophusConstants { - EIGEN_ALWAYS_INLINE static - float epsilon() { - return static_cast(1e-5); - } - - EIGEN_ALWAYS_INLINE static - float pi() { - return static_cast(M_PI); - } -}; - -class SophusException : public std::runtime_error { -public: - SophusException (const std::string& str) - : runtime_error("Sophus exception: " + str) { - } -}; - -} - -#endif diff --git a/thirdparty/Sophus/sophus/test_rxso3.cpp b/thirdparty/Sophus/sophus/test_rxso3.cpp deleted file mode 100644 index 46134d3..0000000 --- a/thirdparty/Sophus/sophus/test_rxso3.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - - -#include "rxso3.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef RxSO3Group RxSO3Type; - typedef typename RxSO3Group::Point Point; - typedef typename RxSO3Group::Tangent Tangent; - - vector rxso3_vec; - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0, 1.))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, -1.0, 1.1))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0., 1.1))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0.00001))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0., 0., 0.00001, 0))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(M_PI, 0, 0, 0.9))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.2, 0.5, 0.0,0)) - *RxSO3Type::exp(Tangent(M_PI, 0, 0,0.0)) - *RxSO3Type::exp(Tangent(-0.2, -0.5, -0.0,0))); - rxso3_vec.push_back(RxSO3Type::exp(Tangent(0.3, 0.5, 0.1,0)) - *RxSO3Type::exp(Tangent(M_PI, 0, 0,0)) - *RxSO3Type::exp(Tangent(-0.3, -0.5, -0.1,0))); - - vector tangent_vec; - Tangent tmp; - tmp << 0,0,0,0; - tangent_vec.push_back(tmp); - tmp << 1,0,0,0; - tangent_vec.push_back(tmp); - tmp << 1,0,0,0.1; - tangent_vec.push_back(tmp); - tmp << 0,1,0,0.1; - tangent_vec.push_back(tmp); - tmp << 0,0,1,-0.1; - tangent_vec.push_back(tmp); - tmp << -1,1,0,-0.1; - tangent_vec.push_back(tmp); - tmp << 20,-1,0,2; - tangent_vec.push_back(tmp); - - vector point_vec; - point_vec.push_back(Point(1,2,4)); - - Tests tests; - tests.setGroupElements(rxso3_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); -} - -int main() { - cerr << "Test RxSO3" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/test_se2.cpp b/thirdparty/Sophus/sophus/test_se2.cpp deleted file mode 100644 index 1d9d293..0000000 --- a/thirdparty/Sophus/sophus/test_se2.cpp +++ /dev/null @@ -1,90 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include -#include "se2.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef SO2Group SO2Type; - typedef SE2Group SE2Type; - typedef typename SE2Group::Point Point; - typedef typename SE2Group::Tangent Tangent; - - vector se2_vec; - se2_vec.push_back(SE2Type(SO2Type(0.0),Point(0,0))); - se2_vec.push_back(SE2Type(SO2Type(0.2),Point(10,0))); - se2_vec.push_back(SE2Type(SO2Type(0.),Point(0,100))); - se2_vec.push_back(SE2Type(SO2Type(-1.),Point(20,-1))); - se2_vec.push_back(SE2Type(SO2Type(0.00001), - Point(-0.00000001,0.0000000001))); - se2_vec.push_back(SE2Type(SO2Type(0.2),Point(0,0)) - *SE2Type(SO2Type(M_PI),Point(0,0)) - *SE2Type(SO2Type(-0.2),Point(0,0))); - se2_vec.push_back(SE2Type(SO2Type(0.3),Point(2,0)) - *SE2Type(SO2Type(M_PI),Point(0,0)) - *SE2Type(SO2Type(-0.3),Point(0,6))); - - vector tangent_vec; - Tangent tmp; - tmp << 0,0,0; - tangent_vec.push_back(tmp); - tmp << 1,0,0; - tangent_vec.push_back(tmp); - tmp << 0,1,1; - tangent_vec.push_back(tmp); - tmp << -1,1,0; - tangent_vec.push_back(tmp); - tmp << 20,-1,-1; - tangent_vec.push_back(tmp); - tmp << 30,5,20; - tangent_vec.push_back(tmp); - - vector point_vec; - point_vec.push_back(Point(1,2)); - - Tests tests; - tests.setGroupElements(se2_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); -} - -int main() { - cerr << "Test SE2" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/test_se3.cpp b/thirdparty/Sophus/sophus/test_se3.cpp deleted file mode 100644 index 4d9313b..0000000 --- a/thirdparty/Sophus/sophus/test_se3.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "se3.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef SO3Group SO3Type; - typedef SE3Group SE3Type; - typedef typename SE3Group::Point Point; - typedef typename SE3Group::Tangent Tangent; - - vector se3_vec; - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), - Point(0,0,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), - Point(10,0,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), - Point(0,100,5))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), - Point(0,0,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), - Point(0,-0.00000001,0.0000000001))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), - Point(0.01,0,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), - Point(4,-5,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), - Point(0,0,0)) - *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), - Point(0,0,0)) - *SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), - Point(0,0,0))); - se3_vec.push_back(SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), - Point(2,0,-7)) - *SE3Type(SO3Type::exp(Point(M_PI, 0, 0)), - Point(0,0,0)) - *SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), - Point(0,6,0))); - vector tangent_vec; - Tangent tmp; - tmp << 0,0,0,0,0,0; - tangent_vec.push_back(tmp); - tmp << 1,0,0,0,0,0; - tangent_vec.push_back(tmp); - tmp << 0,1,0,1,0,0; - tangent_vec.push_back(tmp); - tmp << 0,-5,10,0,0,0; - tangent_vec.push_back(tmp); - tmp << -1,1,0,0,0,1; - tangent_vec.push_back(tmp); - tmp << 20,-1,0,-1,1,0; - tangent_vec.push_back(tmp); - tmp << 30,5,-1,20,-1,0; - tangent_vec.push_back(tmp); - - vector point_vec; - point_vec.push_back(Point(1,2,4)); - - Tests tests; - tests.setGroupElements(se3_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); - cerr << "passed." << endl << endl; -} - -int main() { - cerr << "Test SE3" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/test_sim3.cpp b/thirdparty/Sophus/sophus/test_sim3.cpp deleted file mode 100644 index 97c2c8c..0000000 --- a/thirdparty/Sophus/sophus/test_sim3.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include - -#include "sim3.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef Sim3Group Sim3Type; - typedef RxSO3Group RxSO3Type; - typedef typename Sim3Group::Point Point; - typedef typename Sim3Group::Tangent Tangent; - typedef Matrix Vector4Type; - - vector sim3_vec; - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,1.)), - Point(0,0,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, -1.0,1.1)), - Point(10,0,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.,1.1)), - Point(0,10,5))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0.)), - Point(0,0,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp( - Vector4Type(0., 0., 0.00001, 0.0000001)), - Point(1,-1.00000001,2.0000000001))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0., 0., 0.00001, 0)), - Point(0.01,0,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0.9)), - Point(4,-5,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.2, 0.5, 0.0,0)), - Point(0,0,0)) - *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), - Point(0,0,0)) - *Sim3Type(RxSO3Type::exp(Vector4Type(-0.2, -0.5, -0.0,0)), - Point(0,0,0))); - sim3_vec.push_back(Sim3Type(RxSO3Type::exp(Vector4Type(0.3, 0.5, 0.1,0)), - Point(2,0,-7)) - *Sim3Type(RxSO3Type::exp(Vector4Type(M_PI, 0, 0,0)), - Point(0,0,0)) - *Sim3Type(RxSO3Type::exp(Vector4Type(-0.3, -0.5, -0.1,0)), - Point(0,6,0))); - vector tangent_vec; - Tangent tmp; - tmp << 0,0,0,0,0,0,0; - tangent_vec.push_back(tmp); - tmp << 1,0,0,0,0,0,0; - tangent_vec.push_back(tmp); - tmp << 0,1,0,1,0,0,0.1; - tangent_vec.push_back(tmp); - tmp << 0,0,1,0,1,0,0.1; - tangent_vec.push_back(tmp); - tmp << -1,1,0,0,0,1,-0.1; - tangent_vec.push_back(tmp); - tmp << 20,-1,0,-1,1,0,-0.1; - tangent_vec.push_back(tmp); - tmp << 30,5,-1,20,-1,0,1.5; - tangent_vec.push_back(tmp); - - - vector point_vec; - point_vec.push_back(Point(1,2,4)); - - Tests tests; - tests.setGroupElements(sim3_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); -} - -int main() { - cerr << "Test Sim3" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/test_so2.cpp b/thirdparty/Sophus/sophus/test_so2.cpp deleted file mode 100644 index 1685b2b..0000000 --- a/thirdparty/Sophus/sophus/test_so2.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - - -#include -#include - -#include "so2.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef SO2Group SO2Type; - typedef typename SO2Group::Point Point; - typedef typename SO2Group::Tangent Tangent; - - vector so2_vec; - so2_vec.push_back(SO2Type::exp(0.0)); - so2_vec.push_back(SO2Type::exp(0.2)); - so2_vec.push_back(SO2Type::exp(10.)); - so2_vec.push_back(SO2Type::exp(0.00001)); - so2_vec.push_back(SO2Type::exp(M_PI)); - so2_vec.push_back(SO2Type::exp(0.2) - *SO2Type::exp(M_PI) - *SO2Type::exp(-0.2)); - so2_vec.push_back(SO2Type::exp(-0.3) - *SO2Type::exp(M_PI) - *SO2Type::exp(0.3)); - - vector tangent_vec; - tangent_vec.push_back(Tangent(0)); - tangent_vec.push_back(Tangent(1)); - tangent_vec.push_back(Tangent(M_PI_2)); - tangent_vec.push_back(Tangent(-1)); - tangent_vec.push_back(Tangent(20)); - tangent_vec.push_back(Tangent(M_PI_2+0.0001)); - - vector point_vec; - point_vec.push_back(Point(1,2)); - - Tests tests; - tests.setGroupElements(so2_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); - - cerr << "Exception test: "; - try { - SO2Type so2(0., 0.); - } catch(SophusException & e) { - cerr << "passed." << endl << endl; - return; - } - cerr << "failed!" << endl << endl; - exit(-1); -} - -int main() { - cerr << "Test SO2" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/test_so3.cpp b/thirdparty/Sophus/sophus/test_so3.cpp deleted file mode 100644 index e961e1c..0000000 --- a/thirdparty/Sophus/sophus/test_so3.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// This file is part of Sophus. -// -// Copyright 2012-2013 Hauke Strasdat -// -// Permission is hereby granted, free of charge, to any person obtaining a copy -// of this software and associated documentation files (the "Software"), to -// deal in the Software without restriction, including without limitation the -// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or -// sell copies of the Software, and to permit persons to whom the Software is -// furnished to do so, subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in -// all copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING -// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS -// IN THE SOFTWARE. - -#include -#include - -#include "so3.hpp" -#include "tests.hpp" - -using namespace Sophus; -using namespace std; - -template -void tests() { - - typedef SO3Group SO3Type; - typedef typename SO3Group::Point Point; - typedef typename SO3Group::Tangent Tangent; - - vector so3_vec; - - so3_vec.push_back(SO3Type(Quaternion(0.1e-11, 0., 1., 0.))); - so3_vec.push_back(SO3Type(Quaternion(-1,0.00001,0.0,0.0))); - so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0))); - so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, -1.0))); - so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.))); - so3_vec.push_back(SO3Type::exp(Point(0., 0., 0.00001))); - so3_vec.push_back(SO3Type::exp(Point(M_PI, 0, 0))); - so3_vec.push_back(SO3Type::exp(Point(0.2, 0.5, 0.0)) - *SO3Type::exp(Point(M_PI, 0, 0)) - *SO3Type::exp(Point(-0.2, -0.5, -0.0))); - so3_vec.push_back(SO3Type::exp(Point(0.3, 0.5, 0.1)) - *SO3Type::exp(Point(M_PI, 0, 0)) - *SO3Type::exp(Point(-0.3, -0.5, -0.1))); - - vector tangent_vec; - tangent_vec.push_back(Tangent(0,0,0)); - tangent_vec.push_back(Tangent(1,0,0)); - tangent_vec.push_back(Tangent(0,1,0)); - tangent_vec.push_back(Tangent(M_PI_2,M_PI_2,0.0)); - tangent_vec.push_back(Tangent(-1,1,0)); - tangent_vec.push_back(Tangent(20,-1,0)); - tangent_vec.push_back(Tangent(30,5,-1)); - - vector point_vec; - point_vec.push_back(Point(1,2,4)); - - Tests tests; - tests.setGroupElements(so3_vec); - tests.setTangentVectors(tangent_vec); - tests.setPoints(point_vec); - - tests.runAllTests(); -} - -int main() { - cerr << "Test SO3" << endl << endl; - - cerr << "Double tests: " << endl; - tests(); - - cerr << "Float tests: " << endl; - tests(); - return 0; -} diff --git a/thirdparty/Sophus/sophus/tests.hpp b/thirdparty/Sophus/sophus/tests.hpp deleted file mode 100644 index 0ec3c4a..0000000 --- a/thirdparty/Sophus/sophus/tests.hpp +++ /dev/null @@ -1,264 +0,0 @@ -#ifndef SOPUHS_TESTS_HPP -#define SOPUHS_TESTS_HPP - -#include -#include - -#include "sophus.hpp" - -namespace Sophus { - -using namespace std; -using namespace Eigen; - -template -class Tests { - -public: - typedef typename LieGroup::Scalar Scalar; - typedef typename LieGroup::Transformation Transformation; - typedef typename LieGroup::Tangent Tangent; - typedef typename LieGroup::Point Point; - typedef typename LieGroup::Adjoint Adjoint; - static const int N = LieGroup::N; - static const int DoF = LieGroup::DoF; - - const Scalar SMALL_EPS; - - Tests() : SMALL_EPS(SophusConstants::epsilon()) { - } - - void setGroupElements(const vector & group_vec) { - group_vec_ = group_vec; - } - - void setTangentVectors(const vector & tangent_vec) { - tangent_vec_ = tangent_vec; - } - - void setPoints(const vector & point_vec) { - point_vec_ = point_vec; - } - - bool adjointTest() { - bool passed = true; - for (size_t i=0; i20.*SMALL_EPS) { - cerr << "Adjoint" << endl; - cerr << "Test case: " << i << "," << j <SMALL_EPS) { - cerr << "G - exp(log(G))" << endl; - cerr << "Test case: " << i << endl; - cerr << DiffT <10.*SMALL_EPS) { - cerr << "expmap(hat(x)) - exp(x)" << endl; - cerr << "Test case: " << i << endl; - cerr << exp_x <SMALL_EPS) { - cerr << "Transform vector" << endl; - cerr << "Test case: " << i << endl; - cerr << (res1-res2) <SMALL_EPS) { - cerr << "Lie Bracket Test" << endl; - cerr << "Test case: " << i << ", " < fastmul_res(fastmul_res_raw); - Eigen::Map group_j_constmap(group_vec_[j].data()); - fastmul_res = group_vec_[i]; - fastmul_res.fastMultiply(group_j_constmap); - Transformation diff = mul_resmat-fastmul_res.matrix(); - Scalar nrm = diff.norm(); - if (isnan(nrm) || nrm>SMALL_EPS) { - cerr << "Map & Multiply" << endl; - cerr << "Test case: " << i << "," << j << endl; - cerr << diff <SMALL_EPS) { - cerr << "Hat-vee Test" << endl; - cerr << "Test case: " << i << endl; - cerr << resDiff << endl; - cerr << endl; - passed = false; - } - } - return passed; - } - - - - void runAllTests() { - bool passed = adjointTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = expLogTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = expMapTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = groupActionTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = lieBracketTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = mapAndMultTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - passed = veeHatTest(); - if (!passed) { - cerr << "failed!" << endl << endl; - exit(-1); - } - cerr << "passed." << endl << endl; - } - -private: - Matrix map(const Matrix & T, - const Matrix & p) { - return T.template topLeftCorner()*p - + T.template topRightCorner(); - } - - Matrix map(const Matrix & T, - const Matrix & p) { - return T*p; - } - - Scalar norm(const Scalar & v) { - return std::abs(v); - } - - Scalar norm(const Matrix & T) { - return T.norm(); - } - - std::vector group_vec_; - std::vector tangent_vec_; - std::vector point_vec_; -}; -} -#endif // TESTS_HPP From 69bc357d5d445603bb655bc30dfd172a128c6264 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=A1bor=20K=C3=B6nyvesi?= Date: Sun, 8 May 2022 18:31:32 +0200 Subject: [PATCH 2/5] Fix windows compile errors: bigobj, time handling, remove redundant SIGINT handling --- CMakeLists.txt | 2 +- .../IOWrapper/Pangolin/PangolinDSOViewer.cpp | 10 ++-- .../IOWrapper/Pangolin/PangolinDSOViewer.h | 5 +- src/dso/util/DatasetReader.h | 1 - src/main_dmvio_dataset.cpp | 46 +++++-------------- 5 files changed, 19 insertions(+), 45 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ad82dc8..acab86f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,7 @@ set(CMAKE_CXX_EXTENSIONS OFF) add_definitions("-DENABLE_SSE") if (MSVC) - set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP") + set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /EHsc /MP /bigobj") add_definitions("-DNOMINMAX") add_definitions("-DLEAN_AND_MEAN") else() diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp index a7fbe65..4e9ac7e 100644 --- a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.cpp @@ -517,9 +517,8 @@ void PangolinDSOViewer::publishCamPose(FrameShell* frame, if(disableAllDisplay) return; boost::unique_lock lk(model3DMutex); - struct timeval time_now; - gettimeofday(&time_now, NULL); - lastNTrackingMs.push_back(((time_now.tv_sec-last_track.tv_sec)*1000.0f + (time_now.tv_usec-last_track.tv_usec)/1000.0f)); + auto time_now = std::chrono::high_resolution_clock::now(); + lastNTrackingMs.push_back(std::chrono::duration(time_now.time_since_epoch()).count()); if(lastNTrackingMs.size() > 10) lastNTrackingMs.pop_front(); last_track = time_now; @@ -560,9 +559,8 @@ void PangolinDSOViewer::pushDepthImage(MinimalImageB3* image) boost::unique_lock lk(openImagesMutex); - struct timeval time_now; - gettimeofday(&time_now, NULL); - lastNMappingMs.push_back(((time_now.tv_sec-last_map.tv_sec)*1000.0f + (time_now.tv_usec-last_map.tv_usec)/1000.0f)); + auto time_now = std::chrono::high_resolution_clock::now(); + lastNMappingMs.push_back(std::chrono::duration(time_now.time_since_epoch()).count()); if(lastNMappingMs.size() > 10) lastNMappingMs.pop_front(); last_map = time_now; diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h index c69e1a3..7c288d1 100644 --- a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h @@ -32,6 +32,7 @@ #include #include #include "util/SettingsUtil.h" +#include namespace dmvio @@ -140,8 +141,8 @@ class PangolinDSOViewer : public Output3DWrapper // timings - struct timeval last_track; - struct timeval last_map; + std::chrono::high_resolution_clock::time_point last_track; + std::chrono::high_resolution_clock::time_point last_map; std::deque lastNTrackingMs; diff --git a/src/dso/util/DatasetReader.h b/src/dso/util/DatasetReader.h index a689491..51e0f2f 100644 --- a/src/dso/util/DatasetReader.h +++ b/src/dso/util/DatasetReader.h @@ -34,7 +34,6 @@ #include #include -//#include #include #include "util/Undistort.h" diff --git a/src/main_dmvio_dataset.cpp b/src/main_dmvio_dataset.cpp index a30f39b..e3ac344 100644 --- a/src/main_dmvio_dataset.cpp +++ b/src/main_dmvio_dataset.cpp @@ -30,7 +30,6 @@ #include #include #include -//#include #include "IOWrapper/Output3DWrapper.h" #include "IOWrapper/ImageDisplay.h" @@ -78,23 +77,6 @@ using namespace dso; dmvio::IMUCalibration imuCalibration; dmvio::IMUSettings imuSettings; -void my_exit_handler(int s) -{ - printf("Caught signal %d\n", s); - exit(1); -} - -void exitThread() -{ - struct sigaction sigIntHandler; - sigIntHandler.sa_handler = my_exit_handler; - sigemptyset(&sigIntHandler.sa_mask); - sigIntHandler.sa_flags = 0; - sigaction(SIGINT, &sigIntHandler, NULL); - - while(true) pause(); -} - void settingsDefault(int preset) { @@ -456,8 +438,7 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) } } - struct timeval tv_start; - gettimeofday(&tv_start, NULL); + auto tv_start = std::chrono::high_resolution_clock::now(); clock_t started = clock(); double sInitializerOffset = 0; @@ -469,7 +450,7 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) { if(!fullSystem->initialized) // if not initialized: reset start time. { - gettimeofday(&tv_start, NULL); + tv_start = std::chrono::high_resolution_clock::now(); started = clock(); sInitializerOffset = timesToPlayAt[ii]; } @@ -487,13 +468,13 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) bool skipFrame = false; if(playbackSpeed != 0) { - struct timeval tv_now; - gettimeofday(&tv_now, NULL); - double sSinceStart = sInitializerOffset + ((tv_now.tv_sec - tv_start.tv_sec) + - (tv_now.tv_usec - tv_start.tv_usec) / (1000.0f * 1000.0f)); + auto tv_now = std::chrono::high_resolution_clock::now(); + double sSinceStart = sInitializerOffset + std::chrono::duration>(tv_now.time_since_epoch()).count(); // in seconds - if(sSinceStart < timesToPlayAt[ii]) - usleep((int) ((timesToPlayAt[ii] - sSinceStart) * 1000 * 1000)); + if (sSinceStart < timesToPlayAt[ii]) + { + std::this_thread::sleep_for(std::chrono::duration>(timesToPlayAt[ii] - sSinceStart)); + } else if(sSinceStart > timesToPlayAt[ii] + 0.5 + 0.1 * (ii % 2)) { printf("SKIPFRAME %d (play at %f, now it is %f)!\n", ii, timesToPlayAt[ii], sSinceStart); @@ -567,8 +548,7 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) } fullSystem->blockUntilMappingIsFinished(); clock_t ended = clock(); - struct timeval tv_end; - gettimeofday(&tv_end, NULL); + auto tv_end = std::chrono::high_resolution_clock::now(); fullSystem->printResult(imuSettings.resultsPrefix + "result.txt", false, false, true); @@ -581,8 +561,7 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) int numFramesProcessed = abs(idsToPlay[0] - idsToPlay.back()); double numSecondsProcessed = fabs(reader->getTimestamp(idsToPlay[0]) - reader->getTimestamp(idsToPlay.back())); double MilliSecondsTakenSingle = 1000.0f * (ended - started) / (float) (CLOCKS_PER_SEC); - double MilliSecondsTakenMT = sInitializerOffset + ((tv_end.tv_sec - tv_start.tv_sec) * 1000.0f + - (tv_end.tv_usec - tv_start.tv_usec) / 1000.0f); + double MilliSecondsTakenMT = sInitializerOffset + std::chrono::duration(tv_end.time_since_epoch()).count(); printf("\n======================" "\n%d Frames (%.1f fps)" "\n%.2fms per frame (single core); " @@ -601,7 +580,7 @@ void run(ImageFolderReader* reader, IOWrap::PangolinDSOViewer* viewer) std::ofstream tmlog; tmlog.open("logs/time.txt", std::ios::trunc | std::ios::out); tmlog << 1000.0f * (ended - started) / (float) (CLOCKS_PER_SEC * reader->getNumImages()) << " " - << ((tv_end.tv_sec - tv_start.tv_sec) * 1000.0f + (tv_end.tv_usec - tv_start.tv_usec) / 1000.0f) / + << std::chrono::duration(tv_end - tv_start).count() / (float) reader->getNumImages() << "\n"; tmlog.flush(); tmlog.close(); @@ -662,9 +641,6 @@ int main(int argc, char** argv) } - // hook crtl+C. - boost::thread exThread = boost::thread(exitThread); - ImageFolderReader* reader = new ImageFolderReader(source, calib, gammaCalib, vignette, use16Bit); reader->loadIMUData(imuFile); reader->setGlobalCalibration(); From e76110eefeda431ceb036df5bb7ababf9d52b239 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=A1bor=20K=C3=B6nyvesi?= Date: Sun, 8 May 2022 18:42:16 +0200 Subject: [PATCH 3/5] Fix windows compile error: port from unix dirent.h to std::filesystem --- src/dso/util/DatasetReader.h | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/dso/util/DatasetReader.h b/src/dso/util/DatasetReader.h index 51e0f2f..5de3988 100644 --- a/src/dso/util/DatasetReader.h +++ b/src/dso/util/DatasetReader.h @@ -35,6 +35,7 @@ #include #include #include +#include #include "util/Undistort.h" #include "IOWrapper/ImageRW.h" @@ -51,21 +52,19 @@ using namespace dso; inline int getdir (std::string dir, std::vector &files) { - DIR *dp; - struct dirent *dirp; - if((dp = opendir(dir.c_str())) == NULL) - { - return -1; - } - - while ((dirp = readdir(dp)) != NULL) { - std::string name = std::string(dirp->d_name); + const std::filesystem::path dir_path{ dir }; + for (auto const& dir_entry : std::filesystem::directory_iterator{ dir_path }) + { + std::string name = dir_entry.path().filename().string(); - if(name != "." && name != "..") - files.push_back(name); - } - closedir(dp); + if (name != "." && name != "..") + files.push_back(name); + } + if (files.empty()) + { + return -1; + } std::sort(files.begin(), files.end()); From 920f51970bfef017eeb3eccafb4afd959af89a7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=A1bor=20K=C3=B6nyvesi?= Date: Wed, 11 May 2022 00:09:24 +0200 Subject: [PATCH 4/5] Fix linker errors and some warnings - include , otherwise Eigen::Martix<>::inverse() would be undefined - fix forward declares. if a struct is forward declared as a class, function symbols might be erroneously marked external in the object file --- CMakeLists.txt | 152 +++++++++++++----- src/GTSAMIntegration/AugmentedScatter.hpp | 1 - src/IMUInitialization/IMUInitializerLogic.cpp | 2 +- .../IMUInitializerStates.cpp | 4 +- .../IMUInitializerTransitions.cpp | 1 + .../PoseGraphBundleAdjustment.cpp | 2 +- src/dso/FullSystem/CoarseTracker.h | 5 +- src/dso/FullSystem/PixelSelector2.h | 2 +- src/dso/FullSystem/Residuals.h | 6 +- src/dso/IOWrapper/Output3DWrapper.h | 4 +- .../OutputWrapper/SampleOutputWrapper.h | 4 +- src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h | 4 +- .../IOWrapper/Pangolin/PangolinDSOViewer.h | 4 +- .../OptimizationBackend/EnergyFunctional.h | 6 +- .../EnergyFunctionalStructs.h | 4 +- .../OptimizationBackend/MatrixAccumulators.h | 3 +- src/dso/util/NumType.h | 1 + 17 files changed, 136 insertions(+), 69 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index acab86f..50b5353 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,18 +10,11 @@ set(LIBRARY_OUTPUT_PATH lib) # required libraries #find_package(SuiteParse REQUIRED) find_package(Eigen3 REQUIRED) -find_package(Boost COMPONENTS system thread filesystem chrono serialization date_time timer) +find_package(Boost COMPONENTS system thread filesystem chrono serialization date_time timer stacktrace) find_package(GTSAM REQUIRED) find_package(yaml-cpp REQUIRED) find_package(Sophus REQUIRED) - -IF(${Boost_VERSION} GREATER_EQUAL 106500) - message("Building with stacktrace support.") - set(STACKTRACE_LIBRARIES dl) - set(STACKTRACE_DEFINES "-DSTACKTRACE -DBOOST_STACKTRACE_USE_ADDR2LINE") -ENDIF() - # optional libraries find_package(LibZip QUIET) find_package(Pangolin 0.2 QUIET) @@ -66,53 +59,115 @@ set(DSO_SOURCE_DIR ${PROJECT_SOURCE_DIR}/src/dso) # DSO Source files set(dso_SOURCE_FILES + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedSCHessian.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedTopHessian.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctional.cpp + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctionalStructs.cpp + ${DSO_SOURCE_DIR}/util/Undistort.cpp + ${DSO_SOURCE_DIR}/util/globalCalib.cpp + ${DSO_SOURCE_DIR}/util/settings.cpp + ${DSO_SOURCE_DIR}/FullSystem/CoarseInitializer.cpp + ${DSO_SOURCE_DIR}/FullSystem/CoarseTracker.cpp ${DSO_SOURCE_DIR}/FullSystem/FullSystem.cpp - ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptimize.cpp - ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptPoint.cpp ${DSO_SOURCE_DIR}/FullSystem/FullSystemDebugStuff.cpp ${DSO_SOURCE_DIR}/FullSystem/FullSystemMarginalize.cpp - ${DSO_SOURCE_DIR}/FullSystem/Residuals.cpp - ${DSO_SOURCE_DIR}/FullSystem/CoarseTracker.cpp - ${DSO_SOURCE_DIR}/FullSystem/CoarseInitializer.cpp - ${DSO_SOURCE_DIR}/FullSystem/ImmaturePoint.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptPoint.cpp + ${DSO_SOURCE_DIR}/FullSystem/FullSystemOptimize.cpp ${DSO_SOURCE_DIR}/FullSystem/HessianBlocks.cpp + ${DSO_SOURCE_DIR}/FullSystem/ImmaturePoint.cpp ${DSO_SOURCE_DIR}/FullSystem/PixelSelector2.cpp - ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctional.cpp - ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedTopHessian.cpp - ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedSCHessian.cpp - ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctionalStructs.cpp - ${DSO_SOURCE_DIR}/util/settings.cpp - ${DSO_SOURCE_DIR}/util/Undistort.cpp - ${DSO_SOURCE_DIR}/util/globalCalib.cpp + ${DSO_SOURCE_DIR}/FullSystem/Residuals.cpp + + ${DSO_SOURCE_DIR}/FullSystem/CoarseInitializer.h + ${DSO_SOURCE_DIR}/FullSystem/CoarseTracker.h + ${DSO_SOURCE_DIR}/FullSystem/FullSystem.h + ${DSO_SOURCE_DIR}/FullSystem/HessianBlocks.h + ${DSO_SOURCE_DIR}/FullSystem/ImmaturePoint.h + ${DSO_SOURCE_DIR}/FullSystem/PixelSelector.h + ${DSO_SOURCE_DIR}/FullSystem/PixelSelector2.h + ${DSO_SOURCE_DIR}/FullSystem/ResidualProjections.h + ${DSO_SOURCE_DIR}/FullSystem/Residuals.h + ${DSO_SOURCE_DIR}/IOWrapper/ImageDisplay.h + ${DSO_SOURCE_DIR}/IOWrapper/ImageRW.h + ${DSO_SOURCE_DIR}/IOWrapper/Output3DWrapper.h + ${DSO_SOURCE_DIR}/IOWrapper/OutputWrapper/SampleOutputWrapper.h + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedSCHessian.h + ${DSO_SOURCE_DIR}/OptimizationBackend/AccumulatedTopHessian.h + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctional.h + ${DSO_SOURCE_DIR}/OptimizationBackend/EnergyFunctionalStructs.h + ${DSO_SOURCE_DIR}/OptimizationBackend/MatrixAccumulators.h + ${DSO_SOURCE_DIR}/OptimizationBackend/RawResidualJacobian.h + ${DSO_SOURCE_DIR}/util/DatasetReader.h + ${DSO_SOURCE_DIR}/util/FrameShell.h + ${DSO_SOURCE_DIR}/util/ImageAndExposure.h + ${DSO_SOURCE_DIR}/util/IndexThreadReduce.h + ${DSO_SOURCE_DIR}/util/MinimalImage.h + ${DSO_SOURCE_DIR}/util/NumType.h + ${DSO_SOURCE_DIR}/util/Undistort.h + ${DSO_SOURCE_DIR}/util/globalCalib.h + ${DSO_SOURCE_DIR}/util/globalFuncs.h + ${DSO_SOURCE_DIR}/util/nanoflann.h + ${DSO_SOURCE_DIR}/util/settings.h ) set(dmvio_SOURCE_FILES - src/IMU/IMUIntegration.cpp - src/GTSAMIntegration/Sim3GTSAM.cpp - src/IMUInitialization/GravityInitializer.cpp - src/IMU/IMUTypes.cpp - src/IMU/IMUSettings.cpp - src/util/TimeMeasurement.cpp - src/util/SettingsUtil.cpp + src/GTSAMIntegration/AugmentedScatter.cpp src/GTSAMIntegration/BAGTSAMIntegration.cpp - src/IMU/CoarseIMULogic.cpp - src/IMU/BAIMULogic.cpp - src/GTSAMIntegration/PoseTransformation.cpp + src/GTSAMIntegration/DelayedMarginalization.cpp + src/GTSAMIntegration/GTSAMUtils.cpp src/GTSAMIntegration/Marginalization.cpp - src/GTSAMIntegration/PoseTransformationIMU.cpp + src/GTSAMIntegration/PoseTransformation.cpp src/GTSAMIntegration/PoseTransformationFactor.cpp - src/IMUInitialization/CoarseIMUInitOptimizer.cpp - src/IMUInitialization/IMUInitializer.cpp + src/GTSAMIntegration/PoseTransformationIMU.cpp + src/GTSAMIntegration/Sim3GTSAM.cpp + src/IMU/BAIMULogic.cpp + src/IMU/CoarseIMULogic.cpp + src/IMU/IMUIntegration.cpp + src/IMU/IMUSettings.cpp + src/IMU/IMUTypes.cpp src/IMU/IMUUtils.cpp - src/IMUInitialization/IMUInitSettings.cpp - src/GTSAMIntegration/GTSAMUtils.cpp - src/GTSAMIntegration/DelayedMarginalization.cpp - src/IMUInitialization/PoseGraphBundleAdjustment.cpp + src/util/SettingsUtil.cpp + src/util/TimeMeasurement.cpp src/GTSAMIntegration/FEJValues.cpp - src/IMUInitialization/IMUInitializerStates.cpp + src/IMUInitialization/CoarseIMUInitOptimizer.cpp + src/IMUInitialization/GravityInitializer.cpp + src/IMUInitialization/IMUInitSettings.cpp + src/IMUInitialization/IMUInitializer.cpp src/IMUInitialization/IMUInitializerLogic.cpp + src/IMUInitialization/IMUInitializerStates.cpp src/IMUInitialization/IMUInitializerTransitions.cpp - src/GTSAMIntegration/AugmentedScatter.cpp + src/IMUInitialization/PoseGraphBundleAdjustment.cpp + + src/GTSAMIntegration/AugmentedScatter.hpp + src/GTSAMIntegration/BAGTSAMIntegration.h + src/GTSAMIntegration/DelayedMarginalization.h + src/GTSAMIntegration/ExtUtils.h + src/GTSAMIntegration/FEJNoiseModelFactor.h + src/GTSAMIntegration/FEJValues.h + src/GTSAMIntegration/GTSAMUtils.h + src/GTSAMIntegration/Marginalization.h + src/GTSAMIntegration/PoseTransformation.h + src/GTSAMIntegration/PoseTransformationFactor.h + src/GTSAMIntegration/PoseTransformationIMU.h + src/GTSAMIntegration/Sim3GTSAM.h + src/IMUInitialization/CoarseIMUInitOptimizer.h + src/IMUInitialization/GravityInitializer.h + src/IMUInitialization/IMUInitSettings.h + src/IMUInitialization/IMUInitStateChanger.h + src/IMUInitialization/IMUInitializer.h + src/IMUInitialization/IMUInitializerLogic.h + src/IMUInitialization/IMUInitializerStates.h + src/IMUInitialization/IMUInitializerTransitions.h + src/IMUInitialization/PoseGraphBundleAdjustment.h + src/IMU/BAIMULogic.h + src/IMU/CoarseIMULogic.h + src/IMU/IMUIntegration.hpp + src/IMU/IMUSettings.h + src/IMU/IMUTypes.h + src/IMU/IMUUtils.h + src/util/GTData.hpp + src/util/SettingsUtil.h + src/util/TimeMeasurement.h ) include_directories( @@ -126,7 +181,10 @@ if (Pangolin_FOUND) include_directories( ${Pangolin_INCLUDE_DIRS} ) set(dso_pangolin_SOURCE_FILES ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/KeyFrameDisplay.cpp - ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/PangolinDSOViewer.cpp) + ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/PangolinDSOViewer.cpp + ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/KeyFrameDisplay.h + ${DSO_SOURCE_DIR}/IOWrapper/Pangolin/PangolinDSOViewer.h + ) set(HAS_PANGOLIN 1) else () message("--- could not find PANGOLIN, not compiling with pangolin library.") @@ -169,10 +227,20 @@ target_link_libraries(dmvio PUBLIC Eigen3::Eigen GTSAM::gtsam Boost::system Boost::thread Boost::filesystem Boost::chrono Boost::serialization Boost::date_time Boost::timer + #Boost::stacktrace yaml-cpp::yaml-cpp Sophus::Sophus opencv::opencv_highgui ) +#target_compile_definitions(dmvio PRIVATE +# STACKTRACE +# BOOST_STACKTRACE_USE_ADDR2LINE +#) +if(MSVC) + target_compile_options(dmvio PRIVATE + "/wd4244;/wd4267;/wd4305;/wd4996" + ) +endif() if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") # OSX set(BOOST_THREAD_LIBRARY boost_thread-mt) @@ -187,7 +255,7 @@ if (OpenCV_FOUND AND Pangolin_FOUND) add_executable(dmvio_dataset ${PROJECT_SOURCE_DIR}/src/main_dmvio_dataset.cpp) target_link_libraries(dmvio_dataset dmvio - cxsparse ${STACKTRACE_LIBRARIES} ${LIBZIP_LIBRARY} + ${STACKTRACE_LIBRARIES} ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ) else() diff --git a/src/GTSAMIntegration/AugmentedScatter.hpp b/src/GTSAMIntegration/AugmentedScatter.hpp index f5bc9c5..35af3b3 100644 --- a/src/GTSAMIntegration/AugmentedScatter.hpp +++ b/src/GTSAMIntegration/AugmentedScatter.hpp @@ -26,7 +26,6 @@ #include #include -#include #include "util/NumType.h" #include "OptimizationBackend/EnergyFunctionalStructs.h" diff --git a/src/IMUInitialization/IMUInitializerLogic.cpp b/src/IMUInitialization/IMUInitializerLogic.cpp index 3847f87..9bd546d 100644 --- a/src/IMUInitialization/IMUInitializerLogic.cpp +++ b/src/IMUInitialization/IMUInitializerLogic.cpp @@ -122,7 +122,7 @@ dmvio::IMUInitVariances::IMUInitVariances(const gtsam::Marginals& marginals, gts gtsam::Matrix scaleCovariance = marginals.marginalCovariance(scaleKey); scaleVariance = scaleCovariance(0, 0); biasCovariance = marginals.marginalCovariance(biasKey); - }catch(gtsam::IndeterminantLinearSystemException& exc) + }catch(gtsam::IndeterminantLinearSystemException&) { indetermined = true; } diff --git a/src/IMUInitialization/IMUInitializerStates.cpp b/src/IMUInitialization/IMUInitializerStates.cpp index 973a95c..e559eae 100644 --- a/src/IMUInitialization/IMUInitializerStates.cpp +++ b/src/IMUInitialization/IMUInitializerStates.cpp @@ -283,7 +283,7 @@ dmvio::PGBAState::postBAInit(int keyframeId, gtsam::NonlinearFactor::shared_ptr logic.pgba->optimizationResultNotUsed(); return std::move(pair.second); } - }catch(gtsam::IndeterminantLinearSystemException& exc) + }catch(gtsam::IndeterminantLinearSystemException&) { std::cout << "ERROR during PGBA!" << std::endl; logic.pgba->optimizationResultNotUsed(); @@ -364,7 +364,7 @@ void RealtimePGBAState::threadRun() } newStatePair = transitionModel.pgbaOptimized(optimizedValues, variances); - }catch(gtsam::IndeterminantLinearSystemException& exc) + }catch(gtsam::IndeterminantLinearSystemException&) { std::cout << "ERROR during PGBA!" << std::endl; std::unique_ptr emptyVals; diff --git a/src/IMUInitialization/IMUInitializerTransitions.cpp b/src/IMUInitialization/IMUInitializerTransitions.cpp index 02b3595..e2971ce 100644 --- a/src/IMUInitialization/IMUInitializerTransitions.cpp +++ b/src/IMUInitialization/IMUInitializerTransitions.cpp @@ -40,6 +40,7 @@ dmvio::createTransitionModel(InitTransitionMode mode, IMUInitializerLogic& logic default: std::cout << "ERROR: Trying to use non-implemented transition model!" << std::endl; assert(false); + return {}; } } diff --git a/src/IMUInitialization/PoseGraphBundleAdjustment.cpp b/src/IMUInitialization/PoseGraphBundleAdjustment.cpp index 91544a0..4b6933e 100644 --- a/src/IMUInitialization/PoseGraphBundleAdjustment.cpp +++ b/src/IMUInitialization/PoseGraphBundleAdjustment.cpp @@ -385,7 +385,7 @@ gtsam::NonlinearFactor::shared_ptr dmvio::compensateNegativeEnergy(NonlinearFact GaussNewtonOptimizer optim(graph, values); optim.optimize(); optimError = optim.error(); - }catch(IndeterminantLinearSystemException& exc) + }catch(IndeterminantLinearSystemException&) { std::cout << "WARNING: INDETERMINED LINEAR SYSTEM EXCEPTION" << std::endl; diff --git a/src/dso/FullSystem/CoarseTracker.h b/src/dso/FullSystem/CoarseTracker.h index eb94d38..38aca28 100644 --- a/src/dso/FullSystem/CoarseTracker.h +++ b/src/dso/FullSystem/CoarseTracker.h @@ -28,7 +28,7 @@ #include "util/NumType.h" -#include "vector" +#include #include #include "util/settings.h" #include "OptimizationBackend/MatrixAccumulators.h" @@ -39,9 +39,6 @@ namespace dso { -struct CalibHessian; -struct FrameHessian; -struct PointFrameResidual; class CoarseTracker { public: diff --git a/src/dso/FullSystem/PixelSelector2.h b/src/dso/FullSystem/PixelSelector2.h index fc06d96..35ee34a 100644 --- a/src/dso/FullSystem/PixelSelector2.h +++ b/src/dso/FullSystem/PixelSelector2.h @@ -34,7 +34,7 @@ namespace dso enum PixelSelectorStatus {PIXSEL_VOID=0, PIXSEL_1, PIXSEL_2, PIXSEL_3}; -class FrameHessian; +struct FrameHessian; class PixelSelector { diff --git a/src/dso/FullSystem/Residuals.h b/src/dso/FullSystem/Residuals.h index a12b9a1..4f4aa36 100644 --- a/src/dso/FullSystem/Residuals.h +++ b/src/dso/FullSystem/Residuals.h @@ -40,9 +40,9 @@ namespace dso { -class PointHessian; -class FrameHessian; -class CalibHessian; +struct PointHessian; +struct FrameHessian; +struct CalibHessian; class EFResidual; diff --git a/src/dso/IOWrapper/Output3DWrapper.h b/src/dso/IOWrapper/Output3DWrapper.h index 32f1366..99d6357 100644 --- a/src/dso/IOWrapper/Output3DWrapper.h +++ b/src/dso/IOWrapper/Output3DWrapper.h @@ -46,8 +46,8 @@ class TransformDSOToIMU; namespace dso { -class FrameHessian; -class CalibHessian; +struct FrameHessian; +struct CalibHessian; class FrameShell; namespace IOWrap diff --git a/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h b/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h index d7d1b1a..31293c9 100644 --- a/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h +++ b/src/dso/IOWrapper/OutputWrapper/SampleOutputWrapper.h @@ -35,8 +35,8 @@ namespace dso { -class FrameHessian; -class CalibHessian; +struct FrameHessian; +struct CalibHessian; class FrameShell; diff --git a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h index 01f0512..43d779e 100644 --- a/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h +++ b/src/dso/IOWrapper/Pangolin/KeyFrameDisplay.h @@ -36,8 +36,8 @@ namespace dso { -class CalibHessian; -class FrameHessian; +struct CalibHessian; +struct FrameHessian; class FrameShell; namespace IOWrap diff --git a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h index 7c288d1..9b055b5 100644 --- a/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h +++ b/src/dso/IOWrapper/Pangolin/PangolinDSOViewer.h @@ -43,8 +43,8 @@ class TransformDSOToIMU; namespace dso { -class FrameHessian; -class CalibHessian; +struct FrameHessian; +struct CalibHessian; class FrameShell; diff --git a/src/dso/OptimizationBackend/EnergyFunctional.h b/src/dso/OptimizationBackend/EnergyFunctional.h index b8d9b9b..6a31553 100644 --- a/src/dso/OptimizationBackend/EnergyFunctional.h +++ b/src/dso/OptimizationBackend/EnergyFunctional.h @@ -43,9 +43,9 @@ namespace dso { class PointFrameResidual; -class CalibHessian; -class FrameHessian; -class PointHessian; +struct CalibHessian; +struct FrameHessian; +struct PointHessian; class EFResidual; diff --git a/src/dso/OptimizationBackend/EnergyFunctionalStructs.h b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h index 0fa4a5e..636390f 100644 --- a/src/dso/OptimizationBackend/EnergyFunctionalStructs.h +++ b/src/dso/OptimizationBackend/EnergyFunctionalStructs.h @@ -35,8 +35,8 @@ namespace dso class PointFrameResidual; struct CalibHessian; -class FrameHessian; -class PointHessian; +struct FrameHessian; +struct PointHessian; class EFResidual; class EFPoint; diff --git a/src/dso/OptimizationBackend/MatrixAccumulators.h b/src/dso/OptimizationBackend/MatrixAccumulators.h index 4da2571..64907d0 100644 --- a/src/dso/OptimizationBackend/MatrixAccumulators.h +++ b/src/dso/OptimizationBackend/MatrixAccumulators.h @@ -102,7 +102,8 @@ class Accumulator11 memset(SSEData,0, sizeof(float)*4*1); memset(SSEData1k,0, sizeof(float)*4*1); memset(SSEData1m,0, sizeof(float)*4*1); - num = numIn1 = numIn1k = numIn1m = 0; + num = 0; + numIn1 = numIn1k = numIn1m = 0; } inline void finish() diff --git a/src/dso/util/NumType.h b/src/dso/util/NumType.h index d943b39..5dad323 100644 --- a/src/dso/util/NumType.h +++ b/src/dso/util/NumType.h @@ -25,6 +25,7 @@ #pragma once #include "Eigen/Core" +#include #include "sophus/sim3.hpp" #include "sophus/se3.hpp" From 4557dcfcd49c9221a77c2c3110b229fdaf415dce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=A1bor=20K=C3=B6nyvesi?= Date: Sat, 13 Aug 2022 11:30:21 +0200 Subject: [PATCH 5/5] Update README.md with Windows build instructions --- .gitignore | 4 ++- README.md | 102 +++++++++++++++++++---------------------------------- 2 files changed, 39 insertions(+), 67 deletions(-) diff --git a/.gitignore b/.gitignore index 68df772..c616e74 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ -/conan/ \ No newline at end of file +/build/ +/conan/ +/configs/ diff --git a/README.md b/README.md index 4c04ec0..2cddc45 100644 --- a/README.md +++ b/README.md @@ -22,87 +22,57 @@ When using this project in academic work, please consider citing: * **[Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization](https://vision.in.tum.de/vi-dso)**, L. von Stumberg, V. Usenko and D. Cremers, In International Conference on Robotics and Automation (ICRA), 2018 * **[Direct Sparse Odometry](https://vision.in.tum.de/dso)**, *J. Engel, V. Koltun, D. Cremers*, In TPAMI, vol. 40, 2018 -### 2. Installation +### 2. Installation from source +This branch is the Windows port of DM-VIO. No Windows specific APIs were introduced during porting, only the standard library equivalents of the \*NIX APIs were used. It might still compile under Linux or MacOS, but I haven't verified either. It was only tested with Windows 10, Visual Studio 2019. - git clone https://github.com/lukasvst/dm-vio.git +All dependecies (except for Pangolin) are gathered via Conan. They are listed in the conanfile.txt and all of them are available from the default conancenter repository. All following command line instructions are to be executed in Git Bash which comes with the official Windows Git installer. To install the dependencies, invoke conan from within the DM-VIO folder: -The following instructions have been tested with Ubuntu 20.04. -The system is also known to work well on Ubuntu 16.04, 18.04 and MacOS Big Sur (only Intel Macs have been tested so far). + git clone https://github.com/AltVanguard/dm-vio.git -b windows_support + cd dm-vio + mkdir conan + cd conan + conan install .. -#### 2.1 Required Dependencies - -##### Suitesparse, Eigen3, Boost, yaml-cpp (required). -Required, install with - - sudo apt-get install cmake libsuitesparse-dev libeigen3-dev libboost-all-dev libyaml-cpp-dev - -On MacOS we recommend Homebrew to install the dependencies. It might be necessary -to install boost@1.60 instead of the newest boost, in order for the used GTSAM version to work. - - -##### GTSAM (required). -Build from source with - - sudo apt install libtbb-dev - git clone https://github.com/borglab/gtsam.git - cd gtsam - git checkout a738529af9754c7a085903f90ae8559bbaa82e75 - mkdir build && cd build - cmake -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_USE_SYSTEM_EIGEN=ON .. - make -j - sudo make install - -##### OpenCV. -Used to read, write and display images. -Install with - - sudo apt-get install libopencv-dev +##### Pangolin +Like for DSO, [Pangolin](https://github.com/stevenlovegrove/Pangolin) is used for the GUI. Checkout Pangolin v0.6 to a separate folder (outside DM-VIO). Same process for conan dependencies: + git clone https://github.com/AltVanguard/Pangolin.git -b v0.6 + cd Pangolin + mkdir conan + cd conan + conan install .. -##### Pangolin. -Like for DSO, this is used for the GUI. You should install v0.6. -Install from [https://github.com/stevenlovegrove/Pangolin](https://github.com/stevenlovegrove/Pangolin) +#### 2.3 Build +To build DM-VIO, you have to select installation folders for Pangolin and DM-VIO, then subsitute the folders into the following commands. I recommend not leaving the install folders as the default, and not installing the libraries under system folders or within the source repo folders. This avoids interference and you can easily restart from scratch if something goes wrong. +Build and install Pangolin first: - sudo apt install libgl1-mesa-dev libglew-dev pkg-config libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols - git clone https://github.com/stevenlovegrove/Pangolin.git cd Pangolin - git checkout v0.6 - mkdir build - cd build - cmake .. - cmake --build . - sudo make install + mkdir build + cd build + cmake .. \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_TESTS=OFF \ + -DBUILD_TOOLS=OFF \ + -DDISPLAY_X11=OFF \ + -DMSVC_USE_STATIC_CRT=OFF \ + -DCMAKE_INSTALL_PREFIX= \ + -DCMAKE_MODULE_PATH=/conan/ + # Here open the build/*.sln file, and build and install the solution from Visual Studio - - -#### 2.2 Recommended Dependencies - -##### GTest (optional). -For running tests, install with `git submodule update --init`. - -##### ziplib (optional). -Used to read datasets with images as .zip. -See [src/dso/README.md](src/dso/README.md) for instructions. - -##### sse2neon (required for ARM builds). -After cloning, run `git submodule update --init` to include this. - -#### 2.3 Build +Build DM-VIO: cd dm-vio mkdir build cd build - cmake .. - make -j + cmake .. \ + -DCMAKE_INSTALL_PREFIX= \ + -DCMAKE_PREFIX_PATH= \ + -DCMAKE_MODULE_PATH=/conan/ + # Here open the build/*.sln file, and build the solution from Visual Studio This compiles `dmvio_dataset` to run DM-VIO on datasets (needs both OpenCV and Pangolin installed). -It also compiles the library `libdmvio.a`, which other projects can link to. - -#### Trouble-Shooting -The project is based on DSO and only has two additional dependencies with GTSAM and yaml-cpp. -In case of problems with compilation we recommend trying to compile https://github.com/JakobEngel/dso -first and seeing if it works. +It also compiles the DM-VIO static library, which other projects can link to. ### 3 Running Download a TUM-VI sequence (download in the format `Euroc / DSO 512x512`) at https://vision.in.tum.de/data/datasets/visual-inertial-dataset