├── .clang-format ├── .gitignore ├── .linterconfig.yaml ├── LICENSE ├── README.md ├── colmap_groundtruth ├── README.md ├── bus_outside.txt ├── cave.txt └── cemetery.txt ├── config ├── config_fpga_p2_euroc.yaml ├── config_gopro2_uw_radtan.yaml ├── config_gopro_uw_radtan.yaml ├── config_stereorig_v1.yaml ├── config_stereorig_v2.yaml └── config_stereorig_v3.yaml ├── install.md ├── okvis_ros ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake │ ├── CeresConfig.cmake │ ├── FindEigen.cmake │ ├── FindGlog.cmake │ ├── FindSuiteSparse.cmake │ └── opengv │ │ └── CMakeLists.txt ├── doxygen.config ├── include │ └── okvis │ │ ├── Publisher.hpp │ │ ├── RosParametersReader.hpp │ │ └── Subscriber.hpp ├── launch │ ├── flir_stereo.launch │ ├── okvis_node.xml │ ├── svin_flir_stereo_air.launch │ ├── svin_gopro2_uw.launch │ ├── svin_gopro2_uw.xml │ ├── svin_gopro_uw.xml │ ├── svin_speedo1_bbdo2019.launch │ ├── svin_speedo1_downsize_coral_square.launch │ ├── svin_speedo1_water_downsize.launch │ ├── svin_stereo_rig_cropped.launch │ ├── svin_stereorig_mexico.launch │ ├── svin_stereorig_v1.xml │ └── svin_stereorig_v2.xml ├── meshes │ └── firefly.dae ├── msg │ └── SvinHealth.msg ├── okvis │ ├── .gitignore │ ├── CMakeLists.txt │ ├── LICENSE │ ├── README.md │ ├── cmake │ │ ├── CeresConfig.cmake │ │ ├── FindCXSparse.cmake │ │ ├── FindEigen.cmake │ │ ├── FindGlog.cmake │ │ ├── FindMETIS.cmake │ │ ├── FindSuiteSparse.cmake │ │ ├── okvisConfig.cmake.in │ │ ├── okvisConfig.hpp.in │ │ ├── okvisConfigVersion.cmake.in │ │ └── opengv │ │ │ ├── CMakeLists.txt │ │ │ └── opengvConfig.cmake.in │ ├── config │ │ ├── config_bbdos2019_speedo1.yaml │ │ ├── config_flir_air.yaml │ │ ├── config_flir_mono.yaml │ │ ├── config_flir_stereo.yaml │ │ ├── config_fpga_p2_euroc.yaml │ │ ├── config_gopro2_uw_radtan.yaml │ │ ├── config_gopro_uw_radtan.yaml │ │ ├── config_speedo1.yaml │ │ ├── config_speedo1_water_downsize.yaml │ │ ├── config_stereo_rig_cropped.yaml │ │ ├── config_stereorig_v1.yaml │ │ ├── config_stereorig_v2.yaml │ │ └── config_stereorig_v3.yaml │ ├── contributors.txt │ ├── doxygen.config │ ├── okvis_apps │ │ └── src │ │ │ └── okvis_app_synchronous.cpp │ ├── okvis_ceres │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── Estimator.hpp │ │ │ │ ├── IdProvider.hpp │ │ │ │ ├── ceres │ │ │ │ ├── CeresIterationCallback.hpp │ │ │ │ ├── DepthError.hpp │ │ │ │ ├── ErrorInterface.hpp │ │ │ │ ├── HomogeneousPointError.hpp │ │ │ │ ├── HomogeneousPointManifold.hpp │ │ │ │ ├── HomogeneousPointParameterBlock.hpp │ │ │ │ ├── ImuError.hpp │ │ │ │ ├── MagneticSyncPreintegrationError.hpp │ │ │ │ ├── ManifoldAdditionalInterfaces.hpp │ │ │ │ ├── Map.hpp │ │ │ │ ├── MarginalizationError.hpp │ │ │ │ ├── ParameterBlock.hpp │ │ │ │ ├── ParameterBlockSized.hpp │ │ │ │ ├── PoseError.hpp │ │ │ │ ├── PoseManifold.hpp │ │ │ │ ├── PoseParameterBlock.hpp │ │ │ │ ├── RelativePoseError.hpp │ │ │ │ ├── ReprojectionError.hpp │ │ │ │ ├── ReprojectionErrorBase.hpp │ │ │ │ ├── SonarError.hpp │ │ │ │ ├── SpeedAndBiasError.hpp │ │ │ │ ├── SpeedAndBiasParameterBlock.hpp │ │ │ │ ├── implementation │ │ │ │ │ ├── MarginalizationError.hpp │ │ │ │ │ └── ReprojectionError.hpp │ │ │ │ └── ode │ │ │ │ │ └── ode.hpp │ │ │ │ └── implementation │ │ │ │ └── Estimator.hpp │ │ ├── src │ │ │ ├── DepthError.cpp │ │ │ ├── Estimator.cpp │ │ │ ├── HomogeneousPointError.cpp │ │ │ ├── HomogeneousPointManifold.cpp │ │ │ ├── HomogeneousPointParameterBlock.cpp │ │ │ ├── IdProvider.cpp │ │ │ ├── ImuError.cpp │ │ │ ├── MagneticSyncPreintegrationError.cpp │ │ │ ├── ManifoldAdditionalInterfaces.cpp │ │ │ ├── Map.cpp │ │ │ ├── MarginalizationError.cpp │ │ │ ├── PoseError.cpp │ │ │ ├── PoseManifold.cpp │ │ │ ├── PoseParameterBlock.cpp │ │ │ ├── RelativePoseError.cpp │ │ │ ├── SonarError.cpp │ │ │ ├── SonarParameterBlock.cpp │ │ │ ├── SpeedAndBiasError.cpp │ │ │ └── SpeedAndBiasParameterBlock.cpp │ │ └── test │ │ │ ├── TestEstimator.cpp │ │ │ ├── TestHomogeneousPointError.cpp │ │ │ ├── TestImuError.cpp │ │ │ ├── TestJacobians.cpp │ │ │ ├── TestMap.cpp │ │ │ ├── TestMarginalization.cpp │ │ │ ├── TestReprojectionError.cpp │ │ │ └── test_main.cpp │ ├── okvis_common │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── FrameTypedefs.hpp │ │ │ │ ├── Measurements.hpp │ │ │ │ ├── Parameters.hpp │ │ │ │ ├── Variables.hpp │ │ │ │ ├── VioBackendInterface.hpp │ │ │ │ ├── VioFrontendInterface.hpp │ │ │ │ ├── VioInterface.hpp │ │ │ │ └── VioParametersReader.hpp │ │ └── src │ │ │ ├── VioInterface.cpp │ │ │ └── VioParametersReader.cpp │ ├── okvis_cv │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── Frame.hpp │ │ │ │ ├── MultiFrame.hpp │ │ │ │ ├── cameras │ │ │ │ ├── CameraBase.hpp │ │ │ │ ├── DistortionBase.hpp │ │ │ │ ├── EquidistantDistortion.hpp │ │ │ │ ├── NCameraSystem.hpp │ │ │ │ ├── NoDistortion.hpp │ │ │ │ ├── PinholeCamera.hpp │ │ │ │ ├── RadialTangentialDistortion.hpp │ │ │ │ ├── RadialTangentialDistortion8.hpp │ │ │ │ └── implementation │ │ │ │ │ ├── CameraBase.hpp │ │ │ │ │ ├── EquidistantDistortion.hpp │ │ │ │ │ ├── NCameraSystem.hpp │ │ │ │ │ ├── PinholeCamera.hpp │ │ │ │ │ ├── RadialTangentialDistortion.hpp │ │ │ │ │ └── RadialTangentialDistortion8.hpp │ │ │ │ └── implementation │ │ │ │ ├── Frame.hpp │ │ │ │ └── MultiFrame.hpp │ │ ├── src │ │ │ ├── CameraBase.cpp │ │ │ └── NCameraSystem.cpp │ │ └── test │ │ │ ├── TestFrame.cpp │ │ │ ├── TestMultiFrame.cpp │ │ │ ├── TestNCameraSystem.cpp │ │ │ ├── TestPinholeCamera.cpp │ │ │ └── runTests.cpp │ ├── okvis_frontend │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ ├── okvis │ │ │ │ ├── Frontend.hpp │ │ │ │ ├── VioKeyframeWindowMatchingAlgorithm.hpp │ │ │ │ └── triangulation │ │ │ │ │ ├── ProbabilisticStereoTriangulator.hpp │ │ │ │ │ └── stereo_triangulation.hpp │ │ │ └── opengv │ │ │ │ ├── absolute_pose │ │ │ │ └── FrameNoncentralAbsoluteAdapter.hpp │ │ │ │ ├── relative_pose │ │ │ │ └── FrameRelativeAdapter.hpp │ │ │ │ └── sac_problems │ │ │ │ ├── absolute_pose │ │ │ │ └── FrameAbsolutePoseSacProblem.hpp │ │ │ │ └── relative_pose │ │ │ │ ├── FrameRelativePoseSacProblem.hpp │ │ │ │ └── FrameRotationOnlySacProblem.hpp │ │ └── src │ │ │ ├── FrameNoncentralAbsoluteAdapter.cpp │ │ │ ├── FrameRelativeAdapter.cpp │ │ │ ├── Frontend.cpp │ │ │ ├── ProbabilisticStereoTriangulator.cpp │ │ │ ├── VioKeyframeWindowMatchingAlgorithm.cpp │ │ │ └── stereo_triangulation.cpp │ ├── okvis_kinematics │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ └── kinematics │ │ │ │ ├── Transformation.hpp │ │ │ │ ├── implementation │ │ │ │ └── Transformation.hpp │ │ │ │ └── operators.hpp │ │ ├── src │ │ │ └── dependency-tracker.cc │ │ └── test │ │ │ ├── TestTransformation.cpp │ │ │ └── runTests.cpp │ ├── okvis_matcher │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── DenseMatcher.hpp │ │ │ │ ├── MatchingAlgorithm.hpp │ │ │ │ ├── ThreadPool.hpp │ │ │ │ └── implementation │ │ │ │ └── DenseMatcher.hpp │ │ ├── src │ │ │ ├── DenseMatcher.cpp │ │ │ ├── MatchingAlgorithm.cpp │ │ │ └── ThreadPool.cpp │ │ └── test │ │ │ ├── testMatcher.cpp │ │ │ └── test_main.cpp │ ├── okvis_multisensor_processing │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── DepthFrameSynchronizer.hpp │ │ │ │ ├── FrameSynchronizer.hpp │ │ │ │ ├── ImuFrameSynchronizer.hpp │ │ │ │ ├── MagnetometerFrameSynchronizer.hpp │ │ │ │ ├── RelocFrameSynchronizer.hpp │ │ │ │ ├── SonarFrameSynchronizer.hpp │ │ │ │ ├── ThreadedKFVio.hpp │ │ │ │ ├── VioVisualizer.hpp │ │ │ │ └── threadsafe │ │ │ │ └── ThreadsafeQueue.hpp │ │ ├── src │ │ │ ├── DepthFrameSynchronizer.cpp │ │ │ ├── FrameSynchronizer.cpp │ │ │ ├── ImuFrameSynchronizer.cpp │ │ │ ├── MagnetometerFrameSynchronizer.cpp │ │ │ ├── RelocFrameSynchronizer.cpp │ │ │ ├── SonarFrameSynchronizer.cpp │ │ │ ├── ThreadedKFVio.cpp │ │ │ └── VioVisualizer.cpp │ │ └── test │ │ │ ├── FrameSynchronizer_test.cpp │ │ │ ├── ImuFrameSynchronizer_test.cpp │ │ │ ├── MockVioBackendInterface.hpp │ │ │ ├── MockVioFrontendInterface.hpp │ │ │ ├── VioVisualizer_test.cpp │ │ │ ├── testDataFlow.cpp │ │ │ ├── testDataGenerators.hpp │ │ │ ├── testImage.jpg │ │ │ ├── testSynchronizer.cpp │ │ │ ├── testThreading.cpp │ │ │ └── test_main.cpp │ ├── okvis_time │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ ├── Duration.hpp │ │ │ │ ├── Time.hpp │ │ │ │ └── implementation │ │ │ │ ├── Duration.hpp │ │ │ │ └── Time.hpp │ │ └── src │ │ │ ├── Duration.cpp │ │ │ └── Time.cpp │ ├── okvis_timing │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── okvis │ │ │ │ └── timing │ │ │ │ ├── NsecTimeUtilities.hpp │ │ │ │ └── Timer.hpp │ │ ├── src │ │ │ ├── NsecTimeUtilities.cpp │ │ │ └── Timer.cpp │ │ └── test │ │ │ ├── TestNsecTimeUtilities.cpp │ │ │ └── test_main.cpp │ └── okvis_util │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── okvis │ │ │ ├── assert_macros.hpp │ │ │ └── source_file_pos.hpp │ │ └── src │ │ └── dependency-tracker.cc ├── package.xml ├── release.sh ├── src │ ├── Publisher.cpp │ ├── RosParametersReader.cpp │ ├── Subscriber.cpp │ ├── dataset_convertor.cpp │ ├── okvis_node.cpp │ ├── okvis_node_synchronous.cpp │ ├── stereo_sync.cpp │ └── uncompress_image.cpp └── srv │ └── OdometryTrigger.srv ├── pose_graph ├── CMakeLists.txt ├── ThirdParty │ ├── DBoW │ │ ├── BowVector.cpp │ │ ├── BowVector.h │ │ ├── DBoW2.h │ │ ├── FBrief.cpp │ │ ├── FBrief.h │ │ ├── FClass.h │ │ ├── FeatureVector.cpp │ │ ├── FeatureVector.h │ │ ├── QueryResults.cpp │ │ ├── QueryResults.h │ │ ├── ScoringObject.cpp │ │ ├── ScoringObject.h │ │ ├── TemplatedDatabase.h │ │ └── TemplatedVocabulary.h │ ├── DUtils │ │ ├── DException.h │ │ ├── DUtils.h │ │ ├── Random.cpp │ │ ├── Random.h │ │ ├── Timestamp.cpp │ │ └── Timestamp.h │ ├── DVision │ │ ├── BRIEF.cpp │ │ ├── BRIEF.h │ │ ├── BRIEF256.h │ │ └── DVision.h │ ├── VocabularyBinary.cpp │ └── VocabularyBinary.hpp ├── Vocabulary │ ├── brief_k10L6.bin │ └── brief_pattern.yml ├── cmake │ └── FindGlog.cmake ├── config │ ├── gopro │ │ ├── gopro.yaml │ │ ├── gopro2_uw.yaml │ │ └── gopro_uw.yaml │ ├── speedo1 │ │ ├── config_speedo1_downsize.yaml │ │ ├── speedo1.yaml │ │ ├── speedo1_bbdos2019_config.yaml │ │ ├── speedo1_bbdos2019_coral_lmw.yaml │ │ ├── speedo1_cropped.yaml │ │ └── speedo1_downsize_coral_square.yaml │ └── stereo_rig │ │ ├── flir_air.yaml │ │ ├── flir_config.yaml │ │ ├── stereo_rig_v1_config.yaml │ │ ├── stereo_rig_v2_config.yaml │ │ └── stereo_rig_v3_config.yaml ├── include │ ├── common │ │ └── Definitions.h │ ├── pose_graph │ │ ├── GlobalMapping.h │ │ ├── Keyframe.h │ │ ├── LoopClosure.h │ │ ├── Parameters.h │ │ ├── Pose3DError.h │ │ ├── PoseGraph.h │ │ ├── Publisher.h │ │ ├── Subscriber.h │ │ └── SwitchingEstimator.h │ └── utils │ │ ├── Accumulator.h │ │ ├── CameraPoseVisualization.h │ │ ├── Statistics.h │ │ ├── ThreadSafeQueue.h │ │ ├── ThreadsafeTemporalBuffer-inl.h │ │ ├── ThreadsafeTemporalBuffer.h │ │ ├── Timer.h │ │ ├── Utils.h │ │ └── UtilsOpenCV.h ├── output_logs │ └── .gitignore ├── package.xml ├── rviz │ └── svin2.rviz ├── scripts │ ├── histogram_equalize.py │ └── save_okvis_kf_pose.py └── src │ ├── pose_graph │ ├── GlobalMapping.cpp │ ├── Keyframe.cpp │ ├── LoopClosure.cpp │ ├── Parameters.cpp │ ├── PoseGraph.cpp │ ├── Publisher.cpp │ ├── Subscriber.cpp │ └── SwitchingEstimator.cpp │ ├── pose_graph_node.cpp │ └── utils │ ├── CameraPoseVisualization.cpp │ ├── LoopClosureUtils.cpp │ ├── Statistics.cpp │ ├── Utils.cpp │ └── UtilsOpenCV.cpp └── rviz_config ├── dataset_converter.yaml ├── rviz.rviz ├── rviz_mesh.rviz ├── rviz_svin.rviz ├── rviz_uber.rviz └── svin.rviz /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -1 4 | AlignAfterOpenBracket: Align 5 | AlignConsecutiveAssignments: false 6 | AlignConsecutiveDeclarations: false 7 | AlignOperands: true 8 | AlignTrailingComments: true 9 | AllowAllParametersOfDeclarationOnNextLine: false 10 | AllowShortBlocksOnASingleLine: false 11 | AllowShortCaseLabelsOnASingleLine: false 12 | AllowShortFunctionsOnASingleLine: All 13 | AllowShortIfStatementsOnASingleLine: true 14 | AllowShortLoopsOnASingleLine: true 15 | AlwaysBreakAfterDefinitionReturnType: None 16 | AlwaysBreakAfterReturnType: None 17 | AlwaysBreakBeforeMultilineStrings: true 18 | AlwaysBreakTemplateDeclarations: true 19 | BinPackArguments: false 20 | BinPackParameters: false 21 | BraceWrapping: 22 | AfterClass: false 23 | AfterControlStatement: false 24 | AfterEnum: false 25 | AfterFunction: false 26 | AfterNamespace: false 27 | AfterObjCDeclaration: false 28 | AfterStruct: false 29 | AfterUnion: false 30 | BeforeCatch: false 31 | BeforeElse: false 32 | IndentBraces: false 33 | BreakBeforeBinaryOperators: None 34 | BreakBeforeBraces: Attach 35 | BreakBeforeTernaryOperators: true 36 | BreakConstructorInitializersBeforeComma: false 37 | ColumnLimit: 120 38 | CommentPragmas: '^ IWYU pragma:' 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | ConstructorInitializerIndentWidth: 4 41 | ContinuationIndentWidth: 4 42 | Cpp11BracedListStyle: true 43 | DerivePointerAlignment: false 44 | DisableFormat: false 45 | ExperimentalAutoDetectBinPacking: false 46 | ForEachMacros: 47 | - foreach 48 | - Q_FOREACH 49 | - BOOST_FOREACH 50 | IncludeCategories: 51 | - Regex: '^' 52 | Priority: 2 53 | - Regex: '^<.*\.h>' 54 | Priority: 1 55 | - Regex: '^<.*' 56 | Priority: 2 57 | - Regex: '.*' 58 | Priority: 3 59 | IndentCaseLabels: true 60 | IndentWidth: 2 61 | IndentWrappedFunctionNames: false 62 | KeepEmptyLinesAtTheStartOfBlocks: false 63 | MacroBlockBegin: '' 64 | MacroBlockEnd: '' 65 | MaxEmptyLinesToKeep: 1 66 | NamespaceIndentation: None 67 | ObjCBlockIndentWidth: 2 68 | ObjCSpaceAfterProperty: false 69 | ObjCSpaceBeforeProtocolList: false 70 | PenaltyBreakBeforeFirstCallParameter: 1 71 | PenaltyBreakComment: 300 72 | PenaltyBreakFirstLessLess: 120 73 | PenaltyBreakString: 1000 74 | PenaltyExcessCharacter: 1000000 75 | PenaltyReturnTypeOnItsOwnLine: 200 76 | PointerAlignment: Left 77 | ReflowComments: true 78 | SortIncludes: true 79 | SpaceAfterCStyleCast: false 80 | SpaceBeforeAssignmentOperators: true 81 | SpaceBeforeParens: ControlStatements 82 | SpaceInEmptyParentheses: false 83 | SpacesBeforeTrailingComments: 2 84 | SpacesInAngles: false 85 | SpacesInContainerLiterals: true 86 | SpacesInCStyleCastParentheses: false 87 | SpacesInParentheses: false 88 | SpacesInSquareBrackets: false 89 | Standard: Auto 90 | TabWidth: 8 91 | UseTab: Never -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | pose_graph/svin_results/* 2 | pose_graph/reconstruction_results/* 3 | pose_graph/debug_output/* 4 | .idea 5 | -------------------------------------------------------------------------------- /.linterconfig.yaml: -------------------------------------------------------------------------------- 1 | # Turn the components of the linter individualy on / off. 2 | use_clangformat: on 3 | use_cpplint: on 4 | use_yapf: on 5 | use_pylint: on 6 | 7 | # If on, the linter blocks commits after detecting an error. Otherwise, the 8 | # user is asked whether to proceed with the commit. 9 | block_commits: off 10 | 11 | # If this whitelist block is present, the linter only operates on the files and 12 | # directories listed in this whitelist. 13 | whitelist: -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | SVIN is a tightly coupled Sonar-Visual-Inertial-Depth formulation of Simultaneous Localization and Mapping (SLAM) 2 | algorithm for real-time underwater navigation. The package contains two modules: 3 | 4 | 1. okvis_ros: Adaption of OKVIS () to fuse sonar,depth information in the tightly 5 | coupled formulation. 6 | 2. pose_graph: Loop-closing module to enable real-time loop detection and pose-graph optimization based on the 7 | bag-of-binary-words library DBoW2. 8 | 9 | ### !!! Note !!! ## 10 | 11 | **The main branch now uses ROS 2. Please use the ros1 branch if you need to work with ROS1. We are in process of 12 | figuring out how to use old sonar and data in ROS2 as they are custom topic types. Sonar and depth modes will not work 13 | and are disabled by default.** 14 | 15 | ### Setup Instructions ### 16 | 17 | The setup instructions are tested on Ubuntu 24.04 with ROS Jazzy. 18 | 19 | **Prerequisites** 20 | 21 | - [Glog](http://rpg.ifi.uzh.ch/docs/glog.html), [Gflags](https://gflags.github.io/gflags/) 22 | - [BLAS](https://www.netlib.org/blas/), [LAPACK](https://www.netlib.org/lapack/) 23 | - [OpenCV](https://github.com/opencv/opencv) >= 3.4 24 | - [Suitesparse](https://people.engr.tamu.edu/davis/suitesparse.html) 25 | - [ceres-solver](https://github.com/ceres-solver/ceres-solver/tree/master) 26 | - [Brisk](https://ieeexplore.ieee.org/document/6126542) 27 | - [ROS2 Jazzy](https://docs.ros.org/en/jazzy/index.html) 28 | 29 | Please follow [installation page](install.md) for detailed instructions on building SVIN. 30 | 31 | ### Running the project ### 32 | 33 | Running it on our publicly available datasets: . If you follow "Datasets for 34 | Visual-Inertial-Based State Estimation Algorithms" link you will be directed to a google drive directory, under the ' 35 | Bus' and 'Cave' you will find ROS bagfile with Sonar topic named as '/imagenex831l/range' and ' 36 | /imagenex831l/range_raw'. 37 | 38 | ### !!!Note !!!: Any changes in config/launch files are not reflected unless you build the repo again. All the config/launch files are saved inside install folder and will be updated as part of build. ### 39 | 40 | To build again use 41 | 42 | ```bash 43 | colcon build --event-handlers console_direct+ 44 | ``` 45 | 46 | ## Converting between ROS2 and ROS1 bags 47 | 48 | The easiest way to convert between ROS1 and ROS2 bag is using 49 | rosbags-convert [rosbags](https://gitlab.com/ternaris/rosbags). 50 | 51 | To install 52 | 53 | ```bash 54 | sudo apt install pipx 55 | pipx install rosbags 56 | ``` 57 | 58 | To convert ROS1 bag to ROS2 use 59 | 60 | ```bash 61 | rosbags-convert --src --dst 62 | ``` 63 | 64 | ## Running with GoPro Dataset ## 65 | 66 | Run the launch file for Cave: 67 | 68 | ```bash 69 | source install/setup.bash 70 | ros2 launch okvis_ros svin_gopro_uw.xml 71 | ``` 72 | 73 | ## Running on AFRL Datasets ## 74 | 75 | Run the launch file for Cave: 76 | 77 | ```bash 78 | source install/setup.bash 79 | ros2 launch okvis_ros svin_stereorig_v2.xml 80 | ``` 81 | 82 | Or, run the launch file for Bus: 83 | 84 | ```bash 85 | source install/setup.bash 86 | roslaunch okvis_ros svin_stereorig_v1.xml 87 | ``` 88 | 89 | In different terminal, run the bag file 90 | 91 | ```bash 92 | ros2 bag play bagfile_name --clock 93 | ``` 94 | 95 | ### Ground Truth ### 96 | 97 | The pseudo ground truth trajectories obtained using COLMAP are in colmap_groundtruth folder. These trajectories are only 98 | accurate up to scale and evaluation should be done after scaling only. 99 | 100 | Note: We plan to release the scale accurate trajectory using rig constraints soon. 101 | -------------------------------------------------------------------------------- /colmap_groundtruth/README.md: -------------------------------------------------------------------------------- 1 | ## AFRL Underwater Dataset 2 | 3 | This folder contains COLMAP trajectories for underwater dataset discussed in the visual-inertial comparison paper. The dataset is available at [AFRL Underwater Dataset](https://drive.google.com/drive/folders/14LEMMigHxHRGESOZNudUM4LuxXFGLVmE). Link to paper: [Experimental Comparison of Open Source Visual-Inertial-Based State Estimation Algorithms in the Underwater Domain](https://ieeexplore.ieee.org/document/8968049). arxiv link: [arxiv](https://arxiv.org/abs/1904.02215). 4 | 5 | The COLMAP trajectories for Bus and Cemetry dataset are partial as it was not able to register all the images. The trajectories are correct upto scale only. 6 | 7 | If you use the datasets, please cite the following paper: 8 | 9 | ``` 10 | @inproceedings{joshi_iros2019, 11 | author={Joshi, Bharat and Rahman, Sharmin and Kalaitzakis, Michail and Cain, Brennan and Johnson, James and Xanthidis, Marios and Karapetyan, Nare and Hernandez, Alan and Li, Alberto Quattrini and Vitzilaios, Nikolaos and Rekleitis, Ioannis}, 12 | booktitle={2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 13 | title={Experimental Comparison of Open Source Visual-Inertial-Based State Estimation Algorithms in the Underwater Domain}, 14 | year={2019}, 15 | pages={7227-7233}, 16 | doi={10.1109/IROS40897.2019.8968049}} 17 | ``` -------------------------------------------------------------------------------- /install.md: -------------------------------------------------------------------------------- 1 | Current instructions are tested in Ubuntu 24.04 with ROS Jazzy. It should work with Ubuntu 22.04 and ROS Humble. 2 | 3 | **PCL** 4 | 5 | Install pcl libraries 6 | 7 | ```bash 8 | sudo apt install libpcl-dev 9 | ``` 10 | 11 | **Glog, Gflags, BLAS & LAPACK, Eigen3** 12 | 13 | Install Glog, Gflags, BLAS & LAPACK, and Eigen3 14 | 15 | ```bash 16 | sudo apt install libgoogle-glog-dev libgflags-dev libblas-dev liblapack-dev libatlas-base-dev libeigen3-dev 17 | ``` 18 | 19 | **SuiteSparse and OpenCV** 20 | 21 | ```bash 22 | sudo apt install libsuitesparse-dev libopencv-dev 23 | ``` 24 | 25 | **ROS Jazzy** 26 | 27 | Please follow installation instructions for ROS Jazzy. Follow the [ROS Jazzy installation guide](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debians.html) and install ROS Jazzy full desktop. 28 | 29 | You will need the additional package pcl-ros, cv-bridge, image-transport and tf2-sensor-msgs. 30 | 31 | ```bash 32 | sudo apt install ros-jazzy-pcl-ros ros-jazzy-tf2-sensor-msgs ros-jazzy-compressed-image-transport 33 | 34 | ``` 35 | 36 | **Brisk** 37 | 38 | Install brisk features 39 | 40 | ```bash 41 | wget https://www.doc.ic.ac.uk/~sleutene/software/brisk-2.0.8.zip 42 | unzip brisk-2.0.8.zip 43 | cd brisk 44 | mkdir build 45 | cd build 46 | cmake -DCMAKE_BUILD_TYPE=Release .. 47 | make -j8 48 | sudo make install 49 | ``` 50 | 51 | ### Build SVIN ### 52 | 53 | Create ros workspace and then build project: 54 | 55 | ```bash 56 | mkdir -p ~/svin_ws/src 57 | cd ~/svin_ws/src 58 | git clone https://github.com/AutonomousFieldRoboticsLab/SVIn.git 59 | cd .. 60 | colcon build --event-handlers console_direct+ 61 | ``` 62 | You can just use 63 | ```bash 64 | colcon build 65 | ``` 66 | for silent build. 67 | -------------------------------------------------------------------------------- /okvis_ros/LICENSE: -------------------------------------------------------------------------------- 1 | OKVIS - Open Keyframe-based Visual-Inertial SLAM 2 | Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 13 | its contributors may be used to endorse or promote products derived from 14 | this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | 28 | Authors and contributors: 29 | - Stefan Leutenegger 30 | - Andreas Forster 31 | - Paul Furgale 32 | - Pascal Gohl 33 | - Simon Lynen 34 | -------------------------------------------------------------------------------- /okvis_ros/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | 84 | -------------------------------------------------------------------------------- /okvis_ros/launch/flir_stereo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /okvis_ros/launch/okvis_node.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_flir_stereo_air.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_gopro2_uw.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_gopro2_uw.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_gopro_uw.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_speedo1_bbdo2019.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_speedo1_downsize_coral_square.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_speedo1_water_downsize.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_stereo_rig_cropped.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_stereorig_mexico.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_stereorig_v1.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /okvis_ros/launch/svin_stereorig_v2.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /okvis_ros/msg/SvinHealth.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | 3 | bool is_tracking_ok 4 | int32 num_tracked_kps 5 | int32 new_kps 6 | int32[] kps_per_quadrant 7 | geometry_msgs/Point32[] points_3d 8 | int32[] covisibilities 9 | float32[] response_strengths 10 | float32[] quality -------------------------------------------------------------------------------- /okvis_ros/okvis/.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | 30 | # Docu 31 | documentation/html/ 32 | -------------------------------------------------------------------------------- /okvis_ros/okvis/LICENSE: -------------------------------------------------------------------------------- 1 | OKVIS - Open Keyframe-based Visual-Inertial SLAM 2 | Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright notice, 10 | this list of conditions and the following disclaimer in the documentation 11 | and/or other materials provided with the distribution. 12 | * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 13 | its contributors may be used to endorse or promote products derived from 14 | this software without specific prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. 27 | 28 | Authors and contributors: 29 | - Stefan Leutenegger 30 | - Andreas Forster 31 | - Paul Furgale 32 | - Pascal Gohl 33 | - Simon Lynen 34 | -------------------------------------------------------------------------------- /okvis_ros/okvis/cmake/FindEigen.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN_FOUND - system has eigen lib with correct version 10 | # EIGEN_INCLUDE_DIR - the eigen include directory 11 | # EIGEN_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen_FIND_VERSION) 19 | if(NOT Eigen_FIND_VERSION_MAJOR) 20 | set(Eigen_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen_FIND_VERSION_MAJOR) 22 | if(NOT Eigen_FIND_VERSION_MINOR) 23 | set(Eigen_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen_FIND_VERSION_MINOR) 25 | if(NOT Eigen_FIND_VERSION_PATCH) 26 | set(Eigen_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen_FIND_VERSION_PATCH) 28 | 29 | set(Eigen_FIND_VERSION "${Eigen_FIND_VERSION_MAJOR}.${Eigen_FIND_VERSION_MINOR}.${Eigen_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN_VERSION ${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}) 43 | if(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 44 | set(EIGEN_VERSION_OK FALSE) 45 | else(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 46 | set(EIGEN_VERSION_OK TRUE) 47 | endif(${EIGEN_VERSION} VERSION_LESS ${Eigen_FIND_VERSION}) 48 | 49 | if(NOT EIGEN_VERSION_OK) 50 | 51 | message(STATUS "Eigen version ${EIGEN_VERSION} found in ${EIGEN_INCLUDE_DIR}, " 52 | "but at least version ${Eigen_FIND_VERSION} is required") 53 | endif(NOT EIGEN_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN_INCLUDE_DIRS) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN_FOUND ${EIGEN_VERSION_OK}) 61 | 62 | else () 63 | 64 | find_path(EIGEN_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 65 | PATHS 66 | ${CMAKE_INSTALL_PREFIX}/include 67 | ${KDE4_INCLUDE_DIR} 68 | PATH_SUFFIXES eigen3 eigen 69 | ) 70 | 71 | if(EIGEN_INCLUDE_DIR) 72 | _eigen3_check_version() 73 | endif(EIGEN_INCLUDE_DIR) 74 | 75 | include(FindPackageHandleStandardArgs) 76 | find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR EIGEN_VERSION_OK) 77 | 78 | mark_as_advanced(EIGEN_INCLUDE_DIR) 79 | SET(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR} CACHE PATH "The Eigen include path.") 80 | 81 | endif() 82 | 83 | 84 | -------------------------------------------------------------------------------- /okvis_ros/okvis/cmake/okvisConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the OKVIS package 2 | # It defines the following variables 3 | # OKVIS_INCLUDE_DIRS - include directories for FooBar 4 | # OKVIS_LIBRARIES - libraries to link against 5 | # OKVIS_EXECUTABLE - the okvis_app_synchronous executable 6 | # OKVIS_CERES_CONFIG - path to CeresConfig.cmake, to use find_package(ceres) 7 | 8 | set(OKVIS_CERES_CONFIG "@OKVIS_CERES_CONFIG@") 9 | 10 | # Compute paths 11 | get_filename_component(OKVIS_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 12 | set(OKVIS_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 13 | 14 | # Our library dependencies (contains definitions for IMPORTED targets) 15 | if(NOT TARGET okvis AND NOT OKVIS_BINARY_DIR) 16 | include("${OKVIS_CMAKE_DIR}/okvisTargets.cmake") 17 | endif() 18 | 19 | # These are IMPORTED targets created by okvisTargets.cmake 20 | set(OKVIS_LIBRARIES 21 | okvis_util 22 | okvis_kinematics 23 | okvis_time 24 | okvis_cv 25 | okvis_common 26 | okvis_ceres 27 | okvis_timing 28 | okvis_matcher 29 | okvis_frontend 30 | okvis_multisensor_processing ) 31 | set(OKVIS_EXECUTABLE okvis_app_synchronous) 32 | -------------------------------------------------------------------------------- /okvis_ros/okvis/cmake/okvisConfig.hpp.in: -------------------------------------------------------------------------------- 1 | #define OKVIS_VERSION_MAJOR @OKVIS_MAJOR_VERSION @ 2 | #define OKVIS_VERSION_MINOR @OKVIS_MINOR_VERSION @ 3 | #define OKVIS_PATCH_VERSION @OKVIS_PATCH_VERSION @ 4 | -------------------------------------------------------------------------------- /okvis_ros/okvis/cmake/okvisConfigVersion.cmake.in: -------------------------------------------------------------------------------- 1 | set(PACKAGE_VERSION "@OKVIS_VERSION@") 2 | 3 | # Check whether the requested PACKAGE_FIND_VERSION is compatible 4 | if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") 5 | set(PACKAGE_VERSION_COMPATIBLE FALSE) 6 | else() 7 | set(PACKAGE_VERSION_COMPATIBLE TRUE) 8 | if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") 9 | set(PACKAGE_VERSION_EXACT TRUE) 10 | endif() 11 | endif() 12 | -------------------------------------------------------------------------------- /okvis_ros/okvis/cmake/opengv/opengvConfig.cmake.in: -------------------------------------------------------------------------------- 1 | # - Config file for the OPENGV package 2 | # It defines the following variables 3 | # OPENGV_INCLUDE_DIRS - include directories for FooBar 4 | # OPENGV_LIBRARIES - libraries to link against 5 | # OPENGV_EXECUTABLE - the opengv executable - none available 6 | 7 | # Compute paths 8 | get_filename_component(OPENGV_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) 9 | 10 | # Our library dependencies (contains definitions for IMPORTED targets) 11 | if(NOT TARGET opengv AND NOT OPENGV_BINARY_DIR) 12 | include("${OPENGV_CMAKE_DIR}/opengvTargets.cmake") 13 | endif() 14 | 15 | set(OPENGV_LIBRARIES opengv) 16 | set(OPENGV_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@") 17 | -------------------------------------------------------------------------------- /okvis_ros/okvis/contributors.txt: -------------------------------------------------------------------------------- 1 | Stefan Leutenegger 2 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | project(okvis_ceres) 3 | 4 | # require Eigen 5 | find_package( Eigen REQUIRED ) 6 | include_directories(${EIGEN_INCLUDE_DIR}) 7 | 8 | # build the library 9 | add_library(${PROJECT_NAME} 10 | src/PoseParameterBlock.cpp 11 | src/SpeedAndBiasParameterBlock.cpp 12 | src/HomogeneousPointParameterBlock.cpp 13 | src/HomogeneousPointManifold.cpp 14 | # src/SonarParameterBlock.cpp # @Sharmin 15 | # src/SonarManifold.cpp # @Sharmin 16 | src/PoseManifold.cpp 17 | src/ImuError.cpp 18 | src/PoseError.cpp 19 | src/RelativePoseError.cpp 20 | src/SpeedAndBiasError.cpp 21 | src/IdProvider.cpp 22 | src/Map.cpp 23 | src/MarginalizationError.cpp 24 | src/HomogeneousPointError.cpp 25 | src/SonarError.cpp # @Sharmin 26 | src/DepthError.cpp # @Sharmin 27 | src/Estimator.cpp 28 | src/ManifoldAdditionalInterfaces.cpp 29 | include/okvis/Estimator.hpp 30 | include/okvis/ceres/CeresIterationCallback.hpp 31 | ) 32 | 33 | # and link it 34 | target_link_libraries(${PROJECT_NAME} 35 | PUBLIC okvis_util 36 | PUBLIC okvis_cv 37 | PUBLIC okvis_common 38 | PRIVATE ${CERES_LIBRARIES} 39 | PRIVATE ${OpenCV_LIBRARIES} 40 | ) 41 | 42 | # installation if required 43 | install(TARGETS ${PROJECT_NAME} 44 | EXPORT okvisTargets 45 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 46 | ) 47 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 48 | 49 | # testing 50 | if(BUILD_TESTS) 51 | if(APPLE) 52 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 53 | else() 54 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 55 | endif(APPLE) 56 | enable_testing() 57 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 58 | add_executable(${PROJECT_TEST_NAME} 59 | test/test_main.cpp 60 | test/TestEstimator.cpp 61 | test/TestHomogeneousPointError.cpp 62 | test/TestReprojectionError.cpp 63 | test/TestImuError.cpp 64 | test/TestMap.cpp 65 | test/TestMarginalization.cpp 66 | test/TestJacobians.cpp 67 | ) 68 | target_link_libraries(${PROJECT_TEST_NAME} 69 | ${PROJECT_NAME} 70 | gtest 71 | gtest_main 72 | gmock 73 | gmock_main 74 | pthread) 75 | add_test(test ${PROJECT_TEST_NAME}) 76 | endif() 77 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/include/okvis/IdProvider.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file IdProvider.hpp 35 | * @brief Header file for the IdProvider struct emulating a singleton. This provides 36 | Unique IDs. 37 | * @author Stefan Leutenegger 38 | */ 39 | 40 | #ifndef INCLUDE_OKVIS_IDPROVIDER_HPP_ 41 | #define INCLUDE_OKVIS_IDPROVIDER_HPP_ 42 | 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | /// \brief okvis Main namespace of this package. 49 | namespace okvis { 50 | 51 | /// \brief Provides IDs. 52 | namespace IdProvider { 53 | // emulating singleton syntax 54 | struct instance { 55 | /// \brief get a unique new ID. 56 | static uint64_t newId(); 57 | 58 | /** 59 | * \brief Get the last generated ID. 60 | * \warning Use with caution. This ID is almost surely in use. 61 | */ 62 | static uint64_t currentId(); 63 | }; 64 | 65 | } // namespace IdProvider 66 | } // namespace okvis 67 | 68 | #endif /* INCLUDE_OKVIS_IDPROVIDER_HPP_ */ 69 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/src/IdProvider.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file IdProvider.cpp 35 | * @brief Source file for the IdProvider struct. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | 44 | /// \brief Provides IDs. 45 | namespace IdProvider { 46 | std::atomic _id(0); 47 | 48 | // get a unique new ID. 49 | uint64_t instance::newId() { 50 | const uint64_t retVal = ++_id; 51 | return retVal; 52 | } 53 | 54 | // Get the last generated ID. 55 | uint64_t instance::currentId() { return _id; } 56 | } // namespace IdProvider 57 | } // namespace okvis 58 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/src/ManifoldAdditionalInterfaces.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LocalParamizationAdditionalInterfaces.cpp 3 | * 4 | * Created on: 27 Jul 2015 5 | * Author: sleutene 6 | */ 7 | 8 | #include 9 | 10 | namespace okvis { 11 | namespace ceres { 12 | 13 | // Verifies the correctness of a implementation. 14 | bool ManifoldAdditionalInterfaces::verify(const double* x_raw, double purturbation_magnitude) const { 15 | const ::ceres::Manifold* casted = dynamic_cast(this); 16 | if (!casted) { 17 | return false; 18 | } 19 | // verify plus/minus 20 | Eigen::VectorXd x(casted->AmbientSize()); 21 | memcpy(x.data(), x_raw, sizeof(double) * casted->AmbientSize()); 22 | Eigen::VectorXd delta_x(casted->TangentSize()); 23 | Eigen::VectorXd x_plus_delta(casted->AmbientSize()); 24 | Eigen::VectorXd delta_x2(casted->TangentSize()); 25 | delta_x.setRandom(); 26 | delta_x *= purturbation_magnitude; 27 | casted->Plus(x.data(), delta_x.data(), x_plus_delta.data()); 28 | casted->Minus(x_plus_delta.data(), x.data(), delta_x2.data()); 29 | if ((delta_x2 - delta_x).norm() > 1.0e-12) { 30 | return false; 31 | } 32 | 33 | // plusJacobian numDiff 34 | Eigen::Matrix J_plus_num_diff(casted->AmbientSize(), casted->TangentSize()); 35 | const double dx = 1.0e-9; 36 | for (int i = 0; i < casted->TangentSize(); ++i) { 37 | Eigen::VectorXd delta_p(casted->TangentSize()); 38 | delta_p.setZero(); 39 | delta_p[i] = dx; 40 | Eigen::VectorXd delta_m(casted->TangentSize()); 41 | delta_m.setZero(); 42 | delta_m[i] = -dx; 43 | 44 | // reset 45 | Eigen::VectorXd x_p(casted->AmbientSize()); 46 | Eigen::VectorXd x_m(casted->AmbientSize()); 47 | memcpy(x_p.data(), x_raw, sizeof(double) * casted->AmbientSize()); 48 | memcpy(x_m.data(), x_raw, sizeof(double) * casted->AmbientSize()); 49 | casted->Plus(x.data(), delta_p.data(), x_p.data()); 50 | casted->Plus(x.data(), delta_m.data(), x_m.data()); 51 | J_plus_num_diff.col(i) = (x_p - x_m) / (2 * dx); 52 | } 53 | 54 | // verify lift 55 | Eigen::Matrix J_plus(casted->AmbientSize(), casted->TangentSize()); 56 | Eigen::Matrix J_lift(casted->TangentSize(), casted->AmbientSize()); 57 | casted->PlusJacobian(x_raw, J_plus.data()); 58 | ComputeLiftJacobian(x_raw, J_lift.data()); 59 | Eigen::MatrixXd identity(casted->TangentSize(), casted->TangentSize()); 60 | identity.setIdentity(); 61 | if (((J_lift * J_plus) - identity).norm() > 1.0e-6) { 62 | return false; 63 | } 64 | 65 | // verify numDiff jacobian 66 | if ((J_plus - J_plus_num_diff).norm() > 1.0e-6) { 67 | return false; 68 | } 69 | 70 | // everything fine... 71 | return true; 72 | } 73 | 74 | } // namespace ceres 75 | } // namespace okvis 76 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/src/SonarParameterBlock.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file SonarParameterBlock.cpp 35 | * @brief Source file for the SonarParameterBlock class. 36 | * @author Sharmin Rahman 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Default constructor (assumes not fixed). 47 | SonarParameterBlock::SonarParameterBlock() : base_t::ParameterBlockSized(), initialized_(false) { setFixed(false); } 48 | // Trivial destructor. 49 | SonarParameterBlock::~SonarParameterBlock() {} 50 | 51 | // Constructor with estimate and time. 52 | SonarParameterBlock::SonarParameterBlock(const Eigen::Vector4d& point, uint64_t id, bool initialized) { 53 | setEstimate(point); 54 | setId(id); 55 | setInitialized(initialized); 56 | setFixed(false); 57 | } 58 | 59 | // Constructor with estimate and time. 60 | SonarParameterBlock::SonarParameterBlock(const Eigen::Vector3d& point, uint64_t id, bool initialized) { 61 | setEstimate(Eigen::Vector4d(point[0], point[1], point[2], 1.0)); 62 | setId(id); 63 | setInitialized(initialized); 64 | setFixed(false); 65 | } 66 | 67 | // setters 68 | // Set estimate of this parameter block. 69 | void SonarParameterBlock::setEstimate(const Eigen::Vector4d& point) { 70 | // hack: only do "Euclidean" points for now... 71 | for (int i = 0; i < base_t::Dimension; ++i) parameters_[i] = point[i]; 72 | } 73 | 74 | // getters 75 | // Get estimate. 76 | Eigen::Vector4d SonarParameterBlock::estimate() const { 77 | return Eigen::Vector4d(Eigen::Vector4d(parameters_[0], parameters_[1], parameters_[2], parameters_[3])); 78 | } 79 | 80 | } // namespace ceres 81 | } // namespace okvis 82 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/src/SpeedAndBiasParameterBlock.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file SpeedAndBiasParameterBlock.cpp 35 | * @brief Source file for the SpeedAndBiasParameterBlock class. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #include 40 | 41 | /// \brief okvis Main namespace of this package. 42 | namespace okvis { 43 | /// \brief ceres Namespace for ceres-related functionality implemented in okvis. 44 | namespace ceres { 45 | 46 | // Default constructor (assumes not fixed). 47 | SpeedAndBiasParameterBlock::SpeedAndBiasParameterBlock() : base_t::ParameterBlockSized() { setFixed(false); } 48 | 49 | // Trivial destructor. 50 | SpeedAndBiasParameterBlock::~SpeedAndBiasParameterBlock() {} 51 | 52 | // Constructor with estimate and time. 53 | SpeedAndBiasParameterBlock::SpeedAndBiasParameterBlock(const SpeedAndBias& speedAndBias, 54 | uint64_t id, 55 | const okvis::Time& timestamp) { 56 | setEstimate(speedAndBias); 57 | setId(id); 58 | setTimestamp(timestamp); 59 | setFixed(false); 60 | } 61 | 62 | // setters 63 | // Set estimate of this parameter block. 64 | void SpeedAndBiasParameterBlock::setEstimate(const SpeedAndBias& speedAndBias) { 65 | for (int i = 0; i < base_t::Dimension; ++i) parameters_[i] = speedAndBias[i]; 66 | } 67 | 68 | // getters 69 | // Get estimate. 70 | SpeedAndBias SpeedAndBiasParameterBlock::estimate() const { 71 | SpeedAndBias speedAndBias; 72 | for (int i = 0; i < base_t::Dimension; ++i) speedAndBias[i] = parameters_[i]; 73 | return speedAndBias; 74 | } 75 | 76 | } // namespace ceres 77 | } // namespace okvis 78 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_ceres/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 30, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | 35 | #include "glog/logging.h" 36 | 37 | /// Run all the tests that were declared with TEST() 38 | int main(int argc, char** argv) { 39 | testing::InitGoogleTest(&argc, argv); 40 | google::InitGoogleLogging(argv[0]); 41 | FLAGS_logtostderr = true; 42 | FLAGS_colorlogtostderr = true; 43 | FLAGS_stderrthreshold = 0; 44 | 45 | return RUN_ALL_TESTS(); 46 | } 47 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | 3 | project(okvis_common) 4 | 5 | 6 | # require Eigen 7 | find_package( Eigen REQUIRED ) 8 | find_package(OpenCV REQUIRED) 9 | 10 | include_directories(${EIGEN_INCLUDE_DIR}) 11 | include_directories(${OpenCV_INCLUDE_DIRS}) 12 | 13 | # build the library 14 | add_library(${PROJECT_NAME} STATIC 15 | src/VioInterface.cpp 16 | src/VioParametersReader.cpp 17 | include/okvis/FrameTypedefs.hpp 18 | include/okvis/Measurements.hpp 19 | include/okvis/Parameters.hpp 20 | include/okvis/Variables.hpp 21 | include/okvis/VioBackendInterface.hpp 22 | include/okvis/VioFrontendInterface.hpp 23 | include/okvis/VioInterface.hpp 24 | include/okvis/VioParametersReader.hpp) 25 | 26 | # and link it 27 | target_link_libraries(${PROJECT_NAME} 28 | PUBLIC okvis_util 29 | PUBLIC okvis_kinematics 30 | PUBLIC okvis_time 31 | PUBLIC okvis_cv 32 | ${OpenCV_LIBS} 33 | ) 34 | 35 | # installation if required 36 | install(TARGETS ${PROJECT_NAME} 37 | EXPORT okvisTargets 38 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 39 | ) 40 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 41 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_common/include/okvis/Variables.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Apr 22, 2012 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file Variables.hpp 35 | * @brief This file contains some typedefs related to state variables. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_VARIABLES_HPP_ 40 | #define INCLUDE_OKVIS_VARIABLES_HPP_ 41 | 42 | #include 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | 47 | typedef Eigen::Matrix SpeedAndBias; 48 | 49 | typedef Eigen::Matrix Speed; 50 | 51 | typedef Eigen::Matrix GyroBias; 52 | typedef Eigen::Matrix AccBias; 53 | typedef Eigen::Matrix ImuBias; 54 | 55 | typedef Eigen::Matrix MagnetometerBias; 56 | typedef Eigen::Matrix MagnetometerWorldZBias; 57 | 58 | typedef Eigen::Matrix Wind; 59 | typedef Eigen::Matrix Qff; 60 | 61 | } // namespace okvis 62 | 63 | #endif /* INCLUDE_OKVIS_VARIABLES_HPP_ */ 64 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_cv/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | 3 | project(okvis_cv) 4 | 5 | # require Eigen 6 | find_package( Eigen REQUIRED ) 7 | include_directories(${EIGEN_INCLUDE_DIR}) 8 | 9 | # require OpenCV 10 | find_package( OpenCV COMPONENTS core highgui imgproc features2d REQUIRED ) 11 | include_directories(BEFORE ${OpenCV_INCLUDE_DIRS}) 12 | 13 | # build the library 14 | add_library(${PROJECT_NAME} STATIC 15 | src/CameraBase.cpp 16 | src/NCameraSystem.cpp 17 | ) 18 | 19 | # and link it 20 | target_link_libraries(${PROJECT_NAME} 21 | PUBLIC okvis_util 22 | PUBLIC okvis_kinematics 23 | PUBLIC okvis_time 24 | PUBLIC okvis_frontend #Sharmin 25 | PRIVATE ${OpenCV_LIBRARIES} 26 | PRIVATE ${BRISK_LIBRARIES} 27 | ) 28 | 29 | # installation if required 30 | install(TARGETS ${PROJECT_NAME} 31 | EXPORT okvisTargets 32 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 33 | ) 34 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 35 | 36 | # testing 37 | if(BUILD_TESTS) 38 | if(APPLE) 39 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 40 | else() 41 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 42 | endif(APPLE) 43 | enable_testing() 44 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 45 | add_executable(${PROJECT_TEST_NAME} 46 | test/runTests.cpp 47 | test/TestPinholeCamera.cpp 48 | test/TestFrame.cpp 49 | test/TestNCameraSystem.cpp 50 | test/TestMultiFrame.cpp 51 | ) 52 | target_link_libraries(${PROJECT_TEST_NAME} 53 | ${PROJECT_NAME} 54 | gtest 55 | gtest_main 56 | gmock 57 | gmock_main 58 | pthread) 59 | add_test(test ${PROJECT_TEST_NAME}) 60 | endif() 61 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_cv/test/runTests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | 35 | #include "gtest/gtest.h" 36 | 37 | int main(int argc, char** argv) { 38 | testing::InitGoogleTest(&argc, argv); 39 | return RUN_ALL_TESTS(); 40 | } 41 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_frontend/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | project(okvis_frontend) 3 | 4 | if(NOT DO_TIMING) 5 | add_definitions(-DDEACTIVATE_TIMERS) 6 | message(STATUS "Deactivating timers.") 7 | endif() 8 | 9 | # require Eigen 10 | find_package( Eigen REQUIRED ) 11 | include_directories(${EIGEN_INCLUDE_DIR}) 12 | 13 | # build the library 14 | add_library(${PROJECT_NAME} 15 | src/Frontend.cpp 16 | src/VioKeyframeWindowMatchingAlgorithm.cpp 17 | src/stereo_triangulation.cpp 18 | src/ProbabilisticStereoTriangulator.cpp 19 | src/FrameNoncentralAbsoluteAdapter.cpp 20 | src/FrameRelativeAdapter.cpp 21 | include/okvis/Frontend.hpp 22 | include/okvis/VioKeyframeWindowMatchingAlgorithm.hpp 23 | include/okvis/triangulation/stereo_triangulation.hpp 24 | include/okvis/triangulation/ProbabilisticStereoTriangulator.hpp 25 | include/opengv/absolute_pose/FrameNoncentralAbsoluteAdapter.hpp 26 | include/opengv/relative_pose/FrameRelativeAdapter.hpp 27 | include/opengv/sac_problems/absolute_pose/FrameAbsolutePoseSacProblem.hpp 28 | include/opengv/sac_problems/relative_pose/FrameRelativePoseSacProblem.hpp 29 | include/opengv/sac_problems/relative_pose/FrameRotationOnlySacProblem.hpp) 30 | 31 | # and link it 32 | target_link_libraries(${PROJECT_NAME} 33 | PRIVATE ${BRISK_LIBRARIES} 34 | PRIVATE ${OpenGV_LIBRARIES} 35 | PRIVATE ${CERES_LIBRARIES} 36 | PUBLIC okvis_util 37 | PUBLIC okvis_cv 38 | PUBLIC okvis_ceres 39 | PUBLIC okvis_timing 40 | PUBLIC okvis_matcher) 41 | 42 | # installation if required 43 | install(TARGETS ${PROJECT_NAME} 44 | EXPORT okvisTargets 45 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 46 | ) 47 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 48 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_frontend/include/okvis/triangulation/stereo_triangulation.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Mar 10, 2013 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | /** 34 | * @file stereo_triangulation.hpp 35 | * @brief File containing the triangulateFast method definition. 36 | * @author Stefan Leutenegger 37 | */ 38 | 39 | #ifndef INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ 40 | #define INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ 41 | 42 | #include 43 | 44 | /// \brief okvis Main namespace of this package. 45 | namespace okvis { 46 | /// \brief 47 | namespace triangulation { 48 | 49 | /** 50 | * @brief Triangulate the intersection of two rays. 51 | * @warning The rays e1 and e2 need to be normalized! 52 | * @param[in] p1 Camera center position of frame A in coordinate frame A 53 | * @param[in] e1 Ray through keypoint of frame A in coordinate frame A. 54 | * @param[in] p2 Camera center position of frame B in coordinate frame A. 55 | * @param[in] e2 Ray through keypoint of frame B in coordinate frame A. 56 | * @param[in] sigma Ray uncertainty. 57 | * @param[out] isValid Is the triangulation valid. 58 | * @param[out] isParallel Are the rays parallel? 59 | * @return Homogeneous coordinates of triangulated point. 60 | */ 61 | Eigen::Vector4d triangulateFast(const Eigen::Vector3d& p1, 62 | const Eigen::Vector3d& e1, 63 | const Eigen::Vector3d& p2, 64 | const Eigen::Vector3d& e2, 65 | double sigma, 66 | bool& isValid, // NOLINT 67 | bool& isParallel); // NOLINT 68 | } // namespace triangulation 69 | } // namespace okvis 70 | 71 | #endif /* INCLUDE_OKVIS_TRIANGULATION_STEREO_TRIANGULATION_HPP_ */ 72 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_kinematics/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | 3 | project(okvis_kinematics) 4 | 5 | # require Eigen3 6 | find_package( Eigen REQUIRED ) 7 | include_directories(${EIGEN_INCLUDE_DIR}) 8 | 9 | # build the library 10 | add_library(${PROJECT_NAME} STATIC src/dependency-tracker.cc) 11 | 12 | # and link it 13 | target_link_libraries(${PROJECT_NAME} 14 | PUBLIC okvis_util 15 | ) 16 | 17 | # installation if required 18 | install(TARGETS ${PROJECT_NAME} 19 | EXPORT okvisTargets 20 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 21 | ) 22 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 23 | 24 | # testing 25 | if(BUILD_TESTS) 26 | if(APPLE) 27 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 28 | else() 29 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 30 | endif(APPLE) 31 | enable_testing() 32 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 33 | add_executable(${PROJECT_TEST_NAME} 34 | test/runTests.cpp 35 | test/TestTransformation.cpp 36 | ) 37 | target_link_libraries(${PROJECT_TEST_NAME} 38 | ${PROJECT_NAME} 39 | gtest 40 | gtest_main 41 | gmock 42 | gmock_main 43 | pthread) 44 | add_test(test ${PROJECT_TEST_NAME}) 45 | endif() 46 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_kinematics/src/dependency-tracker.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AutonomousFieldRoboticsLab/SVIn/a6fe7011e3502ae199a34a085bb002ee7d9aad66/okvis_ros/okvis/okvis_kinematics/src/dependency-tracker.cc -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_kinematics/test/runTests.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Feb 3, 2015 30 | * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 31 | *********************************************************************************/ 32 | 33 | #include 34 | 35 | #include "gtest/gtest.h" 36 | 37 | int main(int argc, char** argv) { 38 | testing::InitGoogleTest(&argc, argv); 39 | return RUN_ALL_TESTS(); 40 | } 41 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_matcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.3) 2 | project(okvis_matcher) 3 | 4 | # build the library 5 | add_library(${PROJECT_NAME} 6 | src/DenseMatcher.cpp 7 | src/MatchingAlgorithm.cpp 8 | src/ThreadPool.cpp 9 | include/okvis/DenseMatcher.hpp 10 | include/okvis/MatchingAlgorithm.hpp 11 | include/okvis/ThreadPool.hpp 12 | ) 13 | 14 | # and link it 15 | target_link_libraries(${PROJECT_NAME} 16 | PUBLIC okvis_util 17 | ) 18 | 19 | # installation if required 20 | install(TARGETS ${PROJECT_NAME} 21 | EXPORT okvisTargets 22 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 23 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 24 | ) 25 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 26 | 27 | # testing 28 | if(BUILD_TESTS) 29 | if(APPLE) 30 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 31 | else() 32 | add_definitions(-DGTEST_HAS_TR1_TUPLE=0) 33 | endif(APPLE) 34 | enable_testing() 35 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 36 | add_executable(${PROJECT_TEST_NAME} 37 | test/test_main.cpp 38 | test/testMatcher.cpp 39 | ) 40 | target_link_libraries(${PROJECT_TEST_NAME} 41 | ${PROJECT_NAME} 42 | gtest 43 | gtest_main 44 | gmock 45 | gmock_main 46 | ${GLOG_LIBRARY} 47 | pthread 48 | ) 49 | add_test(test ${PROJECT_TEST_NAME}) 50 | endif() 51 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_matcher/src/MatchingAlgorithm.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: 2013 30 | * Author: Simon Lynen 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file MatchingAlgorithm.cpp 36 | * @brief Source file for the MatchingAlgorithm class. 37 | * @author Simon Lynen 38 | * @author Stefan Leutenegger 39 | */ 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | /// \brief okvis Main namespace of this package. 46 | namespace okvis { 47 | 48 | MatchingAlgorithm::MatchingAlgorithm() {} 49 | MatchingAlgorithm::~MatchingAlgorithm() {} 50 | 51 | MatchingAlgorithm::listB_tree_structure_t::iterator MatchingAlgorithm::getListBStartIterator(size_t /* indexA */) { 52 | OKVIS_THROW(std::runtime_error, 53 | "To use the listB iterators, implement this interface in your matching algorithm subclass."); 54 | return dummy_.end(); 55 | } 56 | MatchingAlgorithm::listB_tree_structure_t::iterator MatchingAlgorithm::getListBEndIterator(size_t /* indexA */) { 57 | OKVIS_THROW(std::runtime_error, 58 | "To use the listB iterators, implement this interface in your matching algorithm subclass."); 59 | return dummy_.end(); 60 | } 61 | 62 | } // namespace okvis 63 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_matcher/src/ThreadPool.cpp: -------------------------------------------------------------------------------- 1 | // Adapted from https://github.com/progschj/ThreadPool on September 3, 2014 2 | // Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 3 | 4 | // Original copyright: 5 | // Copyright (c) 2012 Jakob Progsch 6 | // 7 | // This software is provided 'as-is', without any express or implied 8 | // warranty. In no event will the authors be held liable for any damages 9 | // arising from the use of this software. 10 | // 11 | // Permission is granted to anyone to use this software for any purpose, 12 | // including commercial applications, and to alter it and redistribute it 13 | // freely, subject to the following restrictions: 14 | // 15 | // 1. The origin of this software must not be misrepresented; you must not 16 | // claim that you wrote the original software. If you use this software 17 | // in a product, an acknowledgment in the product documentation would be 18 | // appreciated but is not required. 19 | // 20 | // 2. Altered source versions must be plainly marked as such, and must not be 21 | // misrepresented as being the original software. 22 | // 23 | // 3. This notice may not be removed or altered from any source 24 | // distribution. 25 | 26 | /* 27 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 28 | */ 29 | 30 | /** 31 | * @file ThreadPool.cpp 32 | * @brief Source file for the ThreadPool class. 33 | * @author Jakob Progsch 34 | * @author Stefan Leutenegger 35 | */ 36 | 37 | #include 38 | 39 | /// \brief okvis Main namespace of this package. 40 | namespace okvis { 41 | 42 | // The constructor just launches some amount of workers. 43 | ThreadPool::ThreadPool(size_t threads) : active_threads_(0), stop_(false) { 44 | for (size_t i = 0; i < threads; ++i) workers_.emplace_back(std::bind(&ThreadPool::run, this)); 45 | } 46 | 47 | // Destructor. This joins all threads. 48 | ThreadPool::~ThreadPool() { 49 | { 50 | std::unique_lock lock(tasks_mutex_); 51 | stop_ = true; 52 | } 53 | tasks_condition_.notify_all(); 54 | for (size_t i = 0; i < workers_.size(); ++i) { 55 | workers_[i].join(); 56 | } 57 | } 58 | 59 | // Run a single thread. 60 | void ThreadPool::run() { 61 | while (true) { 62 | std::unique_lock lock(this->tasks_mutex_); 63 | while (!this->stop_ && this->tasks_.empty()) { 64 | this->tasks_condition_.wait(lock); 65 | } 66 | if (this->stop_ && this->tasks_.empty()) { 67 | return; 68 | } 69 | std::function task(this->tasks_.front()); 70 | this->tasks_.pop(); 71 | ++active_threads_; 72 | // Unlock the queue while we execute the task. 73 | lock.unlock(); 74 | task(); 75 | lock.lock(); 76 | --active_threads_; 77 | // This is the secret to making the waitForEmptyQueue() function work. 78 | // After finishing a task, notify that this work is done. 79 | wait_condition_.notify_all(); 80 | } 81 | } 82 | 83 | // This method blocks until the queue is empty. 84 | void ThreadPool::waitForEmptyQueue() const { 85 | std::unique_lock lock(this->tasks_mutex_); 86 | // Only exit if all tasks are complete by tracking the number of 87 | // active threads. 88 | while (active_threads_ || !tasks_.empty()) { 89 | this->wait_condition_.wait(lock); 90 | } 91 | } 92 | } // namespace okvis 93 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_matcher/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char** argv) { 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | project(okvis_multisensor_processing) 3 | 4 | if(NOT DO_TIMING) 5 | add_definitions(-DDEACTIVATE_TIMERS) 6 | message(STATUS "Deactivating timers.") 7 | endif() 8 | 9 | # generate the configure header file 10 | configure_file(../cmake/okvisConfig.hpp.in "${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp" @ONLY) 11 | message(STATUS ${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp) 12 | 13 | # require Eigen 14 | find_package( Eigen REQUIRED ) 15 | include_directories(${EIGEN_INCLUDE_DIR}) 16 | 17 | # require OpenCV 18 | find_package( OpenCV COMPONENTS core highgui imgproc features2d REQUIRED ) 19 | include_directories(BEFORE ${OpenCV_INCLUDE_DIRS}) 20 | 21 | add_library(${PROJECT_NAME} 22 | src/ThreadedKFVio.cpp 23 | src/ImuFrameSynchronizer.cpp 24 | src/SonarFrameSynchronizer.cpp # @Sharmin 25 | src/DepthFrameSynchronizer.cpp # @Sharmin 26 | src/RelocFrameSynchronizer.cpp # @Sharmin 27 | src/FrameSynchronizer.cpp 28 | src/VioVisualizer.cpp 29 | include/okvis/ThreadedKFVio.hpp 30 | include/okvis/ImuFrameSynchronizer.hpp 31 | include/okvis/SonarFrameSynchronizer.hpp # @Sharmin 32 | include/okvis/DepthFrameSynchronizer.hpp # @Sharmin 33 | include/okvis/RelocFrameSynchronizer.hpp # @Sharmin 34 | include/okvis/FrameSynchronizer.hpp 35 | include/okvis/VioVisualizer.hpp 36 | include/okvis/threadsafe/ThreadsafeQueue.hpp 37 | ../cmake/okvisConfig.hpp.in 38 | okvisConfig.hpp 39 | ) 40 | 41 | # and link it 42 | target_link_libraries(${PROJECT_NAME} 43 | PUBLIC okvis_time 44 | PUBLIC okvis_util 45 | PUBLIC okvis_kinematics 46 | PUBLIC okvis_cv 47 | PUBLIC okvis_common 48 | PUBLIC okvis_ceres 49 | PUBLIC okvis_frontend 50 | PRIVATE ${GLOG_LIBRARIES} 51 | ) 52 | 53 | # export config 54 | set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER ${CMAKE_CURRENT_BINARY_DIR}/okvisConfig.hpp) 55 | 56 | # installation if required 57 | install(TARGETS ${PROJECT_NAME} 58 | EXPORT okvisTargets 59 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 60 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 61 | ) 62 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 63 | 64 | # testing 65 | if(BUILD_TESTS) 66 | if(APPLE) 67 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 68 | else() 69 | add_definitions(-DGTEST_HAS_TR1_TUPLE=1) 70 | enable_testing() 71 | set(PROJECT_TEST_NAME ${PROJECT_NAME}_test) 72 | add_executable(${PROJECT_TEST_NAME} 73 | test/test_main.cpp 74 | test/FrameSynchronizer_test.cpp 75 | test/ImuFrameSynchronizer_test.cpp 76 | test/test_main.cpp 77 | test/testThreading.cpp 78 | test/testDataFlow.cpp 79 | test/testSynchronizer.cpp 80 | ) 81 | target_link_libraries(${PROJECT_TEST_NAME} 82 | gtest 83 | gtest_main 84 | gmock 85 | gmock_main 86 | ${PROJECT_NAME} 87 | ${CERES_LIBRARIES} 88 | ${OpenCV_LIBRARIES} 89 | pthread) 90 | add_test(test ${PROJECT_TEST_NAME} ) 91 | endif(APPLE) 92 | endif() 93 | 94 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/src/DepthFrameSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Nov 14, 2017 30 | * Author: Sharmin Rahman 31 | 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file DepthFrameSynchronizer.cpp 36 | * @brief Source file for the DepthFrameSynchronizer class. 37 | * @author Sharmin Rahman 38 | */ 39 | 40 | #include "okvis/DepthFrameSynchronizer.hpp" 41 | 42 | /// \brief okvis Main namespace of this package. 43 | namespace okvis { 44 | 45 | DepthFrameSynchronizer::DepthFrameSynchronizer() : shutdown_(false) {} 46 | 47 | DepthFrameSynchronizer::~DepthFrameSynchronizer() { 48 | if (!shutdown_) shutdown(); 49 | } 50 | 51 | // Tell the synchronizer that a new Depth measurement has been registered. 52 | void DepthFrameSynchronizer::gotDepthData(const okvis::Time& stamp) { 53 | newestDepthDataStamp_ = stamp; 54 | if (depthDataNeededUntil_ < stamp) gotNeededDepthData_.notify_all(); 55 | } 56 | 57 | // Wait until a Depth measurement with a timestamp equal or newer to the supplied one is registered. 58 | bool DepthFrameSynchronizer::waitForUpToDateDepthData(const okvis::Time& frame_stamp) { 59 | // if the newest depth data timestamp is smaller than frame_stamp, wait until 60 | // depth_data newer than frame_stamp arrives 61 | if (newestDepthDataStamp_ <= frame_stamp && !shutdown_) { 62 | depthDataNeededUntil_ = frame_stamp; 63 | std::unique_lock lock(mutex_); 64 | gotNeededDepthData_.wait(lock); 65 | } 66 | if (shutdown_) return false; 67 | return true; 68 | } 69 | 70 | // Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 71 | void DepthFrameSynchronizer::shutdown() { 72 | shutdown_ = true; 73 | gotNeededDepthData_.notify_all(); 74 | } 75 | 76 | } /* namespace okvis */ 77 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/src/ImuFrameSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 13, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | * Modified: Andreas Forster (an.forster@gmail.com) 33 | *********************************************************************************/ 34 | 35 | /** 36 | * @file ImuFrameSynchronizer.cpp 37 | * @brief Source file for the ImuFrameSynchronizer class. 38 | * @author Pascal Gohl 39 | * @author Stefan Leutenegger 40 | * @author Andreas Forster 41 | */ 42 | 43 | #include "okvis/ImuFrameSynchronizer.hpp" 44 | 45 | /// \brief okvis Main namespace of this package. 46 | namespace okvis { 47 | 48 | ImuFrameSynchronizer::ImuFrameSynchronizer() : shutdown_(false) {} 49 | 50 | ImuFrameSynchronizer::~ImuFrameSynchronizer() { 51 | if (!shutdown_) shutdown(); 52 | } 53 | 54 | // Tell the synchronizer that a new IMU measurement has been registered. 55 | void ImuFrameSynchronizer::gotImuData(const okvis::Time& stamp) { 56 | newestImuDataStamp_ = stamp; 57 | if (imuDataNeededUntil_ < stamp) gotNeededImuData_.notify_all(); 58 | } 59 | 60 | // Wait until a IMU measurement with a timestamp equal or newer to the supplied one is registered. 61 | bool ImuFrameSynchronizer::waitForUpToDateImuData(const okvis::Time& frame_stamp) { 62 | // if the newest imu data timestamp is smaller than frame_stamp, wait until 63 | // imu_data newer than frame_stamp arrives 64 | if (newestImuDataStamp_ <= frame_stamp && !shutdown_) { 65 | imuDataNeededUntil_ = frame_stamp; 66 | std::unique_lock lock(mutex_); 67 | gotNeededImuData_.wait(lock); 68 | } 69 | if (shutdown_) return false; 70 | return true; 71 | } 72 | 73 | // Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 74 | void ImuFrameSynchronizer::shutdown() { 75 | shutdown_ = true; 76 | gotNeededImuData_.notify_all(); 77 | } 78 | 79 | } /* namespace okvis */ 80 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/src/RelocFrameSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Nov 14, 2017 30 | * Author: Sharmin Rahman 31 | 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file RelocFrameSynchronizer.cpp 36 | * @brief Source file for the DepthFrameSynchronizer class. 37 | * @author Sharmin Rahman 38 | */ 39 | 40 | #include "okvis/RelocFrameSynchronizer.hpp" 41 | 42 | /// \brief okvis Main namespace of this package. 43 | namespace okvis { 44 | 45 | RelocFrameSynchronizer::RelocFrameSynchronizer() : shutdown_(false) {} 46 | 47 | RelocFrameSynchronizer::~RelocFrameSynchronizer() { 48 | if (!shutdown_) shutdown(); 49 | } 50 | 51 | // Tell the synchronizer that a new Reloc measurement has been registered. 52 | void RelocFrameSynchronizer::gotRelocData(const okvis::Time& stamp) { 53 | newestRelocDataStamp_ = stamp; 54 | if (relocDataNeededUntil_ < stamp) gotNeededRelocData_.notify_all(); 55 | } 56 | 57 | // Wait until a Reloc measurement with a timestamp equal or newer to the supplied one is registered. 58 | bool RelocFrameSynchronizer::waitForUpToDateRelocData(const okvis::Time& frame_stamp) { 59 | // if the newest depth data timestamp is smaller than frame_stamp, wait until 60 | // depth_data newer than frame_stamp arrives 61 | if (newestRelocDataStamp_ <= frame_stamp && !shutdown_) { 62 | relocDataNeededUntil_ = frame_stamp; 63 | std::unique_lock lock(mutex_); 64 | gotNeededRelocData_.wait(lock); 65 | } 66 | if (shutdown_) return false; 67 | return true; 68 | } 69 | 70 | // Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 71 | void RelocFrameSynchronizer::shutdown() { 72 | shutdown_ = true; 73 | gotNeededRelocData_.notify_all(); 74 | } 75 | 76 | } /* namespace okvis */ 77 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/src/SonarFrameSynchronizer.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Aug 28, 2017 30 | * Author: Sharmin Rahman 31 | 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file SonarFrameSynchronizer.cpp 36 | * @brief Source file for the SonarFrameSynchronizer class. 37 | * @author Sharmin Rahman 38 | */ 39 | 40 | #include "okvis/SonarFrameSynchronizer.hpp" 41 | 42 | /// \brief okvis Main namespace of this package. 43 | namespace okvis { 44 | 45 | SonarFrameSynchronizer::SonarFrameSynchronizer() : shutdown_(false) {} 46 | 47 | SonarFrameSynchronizer::~SonarFrameSynchronizer() { 48 | if (!shutdown_) shutdown(); 49 | } 50 | 51 | // Tell the synchronizer that a new Sonar measurement has been registered. 52 | void SonarFrameSynchronizer::gotSonarData(const okvis::Time& stamp) { 53 | newestSonarDataStamp_ = stamp; 54 | if (sonarDataNeededUntil_ < stamp) gotNeededSonarData_.notify_all(); 55 | } 56 | 57 | // Wait until a Sonar measurement with a timestamp equal or newer to the supplied one is registered. 58 | bool SonarFrameSynchronizer::waitForUpToDateSonarData(const okvis::Time& frame_stamp) { 59 | // if the newest sonar data timestamp is smaller than frame_stamp, wait until 60 | // sonar_data newer than frame_stamp arrives 61 | if (newestSonarDataStamp_ <= frame_stamp && !shutdown_) { 62 | sonarDataNeededUntil_ = frame_stamp; 63 | std::unique_lock lock(mutex_); 64 | gotNeededSonarData_.wait(lock); 65 | } 66 | if (shutdown_) return false; 67 | return true; 68 | } 69 | 70 | // Tell the synchronizer to shutdown. This will notify all waiting threads to wake up. 71 | void SonarFrameSynchronizer::shutdown() { 72 | shutdown_ = true; 73 | gotNeededSonarData_.notify_all(); 74 | } 75 | 76 | } /* namespace okvis */ 77 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/ImuFrameSynchronizer_test.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: Sep 13, 2014 30 | * Author: Pascal Gohl 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | #include "okvis/ImuFrameSynchronizer.hpp" 35 | 36 | /// \brief okvis Main namespace of this package. 37 | namespace okvis {} /* namespace okvis */ 38 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/VioVisualizer_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * VioVisualizer_test.cpp 3 | * 4 | * Created on: Sep 15, 2014 5 | * Author: pascal 6 | */ 7 | 8 | #include "VioVisualizer.hpp" 9 | 10 | /// \brief okvis Main namespace of this package. 11 | namespace okvis {} /* namespace okvis */ 12 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/testDataFlow.cpp: -------------------------------------------------------------------------------- 1 | #define GTEST_USE_OWN_TR1_TUPLE 0 2 | 3 | #include "gmock/gmock-matchers.h" 4 | #include "gmock/gmock.h" 5 | #include "gtest/gtest.h" 6 | 7 | #pragma GCC diagnostic push 8 | #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 9 | #include 10 | #pragma GCC diagnostic pop 11 | #include using ::testing::_; 12 | #include 13 | #include 14 | 15 | #include "MockVioBackendInterface.hpp" 16 | #include "MockVioFrontendInterface.hpp" 17 | #include "testDataGenerators.hpp" 18 | 19 | using ::testing::AnyNumber; 20 | using ::testing::AtLeast; 21 | using ::testing::Between; 22 | using ::testing::Return; 23 | TEST(OkvisVioInterfaces, testDataFlow) { 24 | using namespace okvis; // NOLINT 25 | 26 | okvis::VioParameters parameters; 27 | parameters.nCameraSystem = TestDataGenerator::getTestCameraSystem(2); 28 | parameters.visualization.displayImages = false; 29 | parameters.imu.a_max = 1; 30 | parameters.imu.g_max = 1; 31 | parameters.optimization.numImuFrames = 2; 32 | parameters.publishing.publishImuPropagatedState = true; 33 | 34 | // configure mock object 35 | MockVioBackendInterface dummy; 36 | EXPECT_CALL(dummy, optimize).Times(Between(5, 10)); 37 | EXPECT_CALL(dummy, get_T_WS).Times(Between(12, 21)); // 1 per matching, 1 per optimization and in destructor 38 | EXPECT_CALL(dummy, addCamera).Times(2); 39 | EXPECT_CALL(dummy, addImu).Times(1); 40 | EXPECT_CALL(dummy, addStates).Times(Between(5, 10)); 41 | EXPECT_CALL(dummy, applyMarginalizationStrategy).Times(Between(5, 10)); 42 | EXPECT_CALL(dummy, setOptimizationTimeLimit).Times(1); 43 | 44 | EXPECT_CALL(dummy, multiFrame).Times(AnyNumber()); 45 | EXPECT_CALL(dummy, getSpeedAndBias).Times(AnyNumber()); 46 | EXPECT_CALL(dummy, numFrames).Times(AnyNumber()); 47 | ON_CALL(dummy, numFrames).WillByDefault(Return(1)); 48 | ON_CALL(dummy, addStates).WillByDefault(Return(true)); 49 | // to circumvent segfault 50 | ON_CALL(dummy, multiFrame) 51 | .WillByDefault(Return( 52 | std::shared_ptr(new okvis::MultiFrame(parameters.nCameraSystem, okvis::Time::now())))); 53 | 54 | MockVioFrontendInterface mock_frontend; 55 | EXPECT_CALL(mock_frontend, detectAndDescribe).Times(Between(18, 20)); 56 | EXPECT_CALL(mock_frontend, dataAssociationAndInitialization).Times(Between(7, 10)); 57 | EXPECT_CALL(mock_frontend, propagation).Times(Between(99, 100)); 58 | EXPECT_CALL(mock_frontend, setBriskDetectionOctaves).Times(1); 59 | EXPECT_CALL(mock_frontend, setBriskDetectionThreshold).Times(1); 60 | 61 | // start with mock 62 | ThreadedKFVio vio(parameters); 63 | vio.setBlocking(true); 64 | 65 | double now = okvis::Time::now().toSec(); 66 | 67 | // test Observation 68 | cv::Mat image_cam; 69 | image_cam = cv::imread("testImage.jpg", 0); 70 | 71 | if (image_cam.data == NULL) { 72 | std::cout << "testImage.jpg not found" << std::endl; 73 | } 74 | 75 | ASSERT_TRUE(image_cam.data != NULL); 76 | 77 | okvis::ImuMeasurement imu_data; 78 | imu_data.measurement.accelerometers.Zero(); 79 | imu_data.measurement.gyroscopes.Zero(); 80 | 81 | // simulate 100Hz IMU and 10Hz images 82 | for (int i = 0; i < 10; ++i) { 83 | for (int j = 0; j < 5; ++j) { 84 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 85 | now += 0.01; 86 | } 87 | vio.addImage(okvis::Time(now), 0, image_cam); 88 | vio.addImage(okvis::Time(now), 1, image_cam); 89 | for (int j = 0; j < 5; ++j) { 90 | vio.addImuMeasurement(okvis::Time(now), imu_data.measurement.accelerometers, imu_data.measurement.gyroscopes); 91 | now += 0.01; 92 | } 93 | } 94 | } 95 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/testDataGenerators.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TEST_DATA_GENERATOR_HPP 2 | #define TEST_DATA_GENERATOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | /// \brief okvis Main namespace of this package. 9 | namespace okvis { 10 | class TestDataGenerator { 11 | public: 12 | static const okvis::cameras::NCameraSystem getTestCameraSystem(int num_cams) { 13 | okvis::cameras::NCameraSystem nCameraSystem; 14 | for (int i = 0; i < num_cams; ++i) { 15 | Eigen::Matrix4d T_SC_e; 16 | T_SC_e << 1, 0, 0, i * 0.01, // TODO(gohlp): does that make sense? 17 | 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0; 18 | std::shared_ptr T_SC_ok_ptr( 19 | new okvis::kinematics::Transformation(T_SC_e)); 20 | nCameraSystem.addCamera( 21 | T_SC_ok_ptr, 22 | std::shared_ptr( 23 | new okvis::cameras::PinholeCamera( 24 | 752, 25 | 480, 26 | 615.14, 27 | 613.03, 28 | 378.27, 29 | 220.05, 30 | okvis::cameras::RadialTangentialDistortion( 31 | -0.40675345816443564, 0.1685454248925922, -0.00046944024734783504, 0.00026638950478762756))), 32 | okvis::cameras::NCameraSystem::RadialTangential); 33 | } 34 | return nCameraSystem; 35 | } 36 | }; 37 | } // namespace okvis 38 | 39 | #endif // TEST_DATA_GENERATOR_HPP 40 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/testImage.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AutonomousFieldRoboticsLab/SVIn/a6fe7011e3502ae199a34a085bb002ee7d9aad66/okvis_ros/okvis/okvis_multisensor_processing/test/testImage.jpg -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_multisensor_processing/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include "gmock/gmock.h" 5 | #include "gtest/gtest.h" 6 | 7 | /// Run all the tests that were declared with TEST() 8 | int main(int argc, char** argv) { 9 | testing::InitGoogleTest(&argc, argv); 10 | 11 | google::InitGoogleLogging(argv[0]); 12 | FLAGS_stderrthreshold = 0; // INFO: 0, WARNING: 1, ERROR: 2, FATAL: 3 13 | FLAGS_colorlogtostderr = 1; 14 | 15 | return RUN_ALL_TESTS(); 16 | } 17 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_time/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | 3 | project(okvis_time) 4 | 5 | # build the library 6 | add_library(${PROJECT_NAME} 7 | src/Time.cpp 8 | src/Duration.cpp) 9 | 10 | # installation if required 11 | install(TARGETS ${PROJECT_NAME} 12 | EXPORT okvisTargets 13 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 14 | ) 15 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 16 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_time/src/Duration.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Modified and adapted by 5 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 6 | * 7 | * Original Copyright (c) 2008, Willow Garage, Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | /* 38 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 39 | */ 40 | 41 | /** 42 | * @file Duration.cpp 43 | * @brief This file contains osme helper function for the Duration class. 44 | * @author Willow Garage Inc. 45 | * @author Stefan Leutenegger 46 | * @author Andreas Forster 47 | */ 48 | 49 | #include 50 | #include 51 | 52 | /// \brief okvis Main namespace of this package. 53 | namespace okvis { 54 | 55 | void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec) { // NOLINT 56 | int64_t nsec_part = nsec; 57 | int64_t sec_part = sec; 58 | 59 | while (nsec_part > 1000000000L) { 60 | nsec_part -= 1000000000L; 61 | ++sec_part; 62 | } 63 | while (nsec_part < 0) { 64 | nsec_part += 1000000000L; 65 | --sec_part; 66 | } 67 | 68 | if (sec_part < INT_MIN || sec_part > INT_MAX) throw std::runtime_error("Duration is out of dual 32-bit range"); 69 | 70 | sec = sec_part; 71 | nsec = nsec_part; 72 | } 73 | 74 | void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec) { // NOLINT 75 | int64_t sec64 = sec; 76 | int64_t nsec64 = nsec; 77 | 78 | normalizeSecNSecSigned(sec64, nsec64); 79 | 80 | sec = (int32_t)sec64; 81 | nsec = (int32_t)nsec64; 82 | } 83 | 84 | template class DurationBase; 85 | template class DurationBase; 86 | } // namespace okvis 87 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_timing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | project(okvis_timing) 3 | 4 | add_library(${PROJECT_NAME} 5 | src/Timer.cpp 6 | src/NsecTimeUtilities.cpp 7 | ) 8 | 9 | target_link_libraries(${PROJECT_NAME} PUBLIC okvis_util) 10 | 11 | # installation if required 12 | install(TARGETS ${PROJECT_NAME} 13 | EXPORT okvisTargets 14 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 15 | ) 16 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 17 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_timing/include/okvis/timing/NsecTimeUtilities.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Copyright (c) 2011-2013, Paul Furgale and others. 6 | * All rights reserved. 7 | * 8 | * Unlike otherwise stated in source files, the code in this repository is 9 | * published under the Revised BSD (New BSD) license. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in the 17 | * documentation and/or other materials provided with the distribution. 18 | * * Neither the name of the nor the 19 | * names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 26 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 29 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file NsecTimeUtilities.hpp 36 | * @author Paul Furgale 37 | * @date Sat Jul 20 12:39:54 2013 38 | * 39 | * @brief Functions to support the use of nanosecond epoch time. 40 | * 41 | */ 42 | #ifndef INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ 43 | #define INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ 44 | #include 45 | #include 46 | 47 | namespace okvis { 48 | namespace timing { 49 | 50 | /// \brief Nanoseconds since the epoch. 51 | typedef boost::int64_t NsecTime; 52 | 53 | /// \brief Convert nanoseconds since the epoch to std::chrono 54 | std::chrono::system_clock::time_point nsecToChrono(const NsecTime& time); 55 | 56 | /// \brief Convert std::chrono to nanoseconds since the epoch. 57 | NsecTime chronoToNsec(const std::chrono::system_clock::time_point& time); 58 | 59 | /// \brief Get the epoch time as nanoseconds since the epoch. 60 | NsecTime nsecNow(); 61 | 62 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 63 | double nsecToSec(const NsecTime& time); 64 | 65 | /// \brief Convert the time (in seconds) to integer nanoseconds 66 | NsecTime secToNsec(const double& time); 67 | 68 | /// \brief return a magic number representing an invalid timestamp 69 | constexpr NsecTime getInvalidTime(); 70 | 71 | /// \brief Is the time valid? This uses a magic number 72 | /// std::numeric_limits::min() to represent an invalid time 73 | bool isValid(const NsecTime& time); 74 | 75 | } // namespace timing 76 | } // namespace okvis 77 | 78 | #endif /* INCLUDE_OKVIS_TIMING_NSECTIMEUTILITIES_HPP_ */ 79 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_timing/src/NsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Copyright (c) 2011-2013, Paul Furgale and others. 6 | * All rights reserved. 7 | * 8 | * Unlike otherwise stated in source files, the code in this repository is 9 | * published under the Revised BSD (New BSD) license. 10 | * 11 | * Redistribution and use in source and binary forms, with or without 12 | * modification, are permitted provided that the following conditions are met: 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in the 17 | * documentation and/or other materials provided with the distribution. 18 | * * Neither the name of the nor the 19 | * names of its contributors may be used to endorse or promote products 20 | * derived from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 23 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 24 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 25 | * DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 26 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 29 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 31 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | *********************************************************************************/ 33 | 34 | #include 35 | #include 36 | 37 | namespace okvis { 38 | namespace timing { 39 | 40 | /// \brief Convert nanoseconds since the epoch to std::chrono 41 | std::chrono::system_clock::time_point nsecToChrono(const NsecTime& time) { 42 | std::chrono::nanoseconds tt(time); 43 | std::chrono::system_clock::time_point tp(std::chrono::duration_cast(tt)); 44 | return tp; 45 | } 46 | 47 | /// \brief Convert std::chrono to nanoseconds since the epoch. 48 | NsecTime chronoToNsec(const std::chrono::system_clock::time_point& time) { 49 | return std::chrono::duration_cast(time.time_since_epoch()).count(); 50 | } 51 | 52 | /// \brief Get the epoch time as nanoseconds since the epoch. 53 | NsecTime nsecNow() { 54 | return std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()) 55 | .count(); 56 | } 57 | 58 | /// \brief Convert the time (in integer nanoseconds) to decimal seconds. 59 | double nsecToSec(const NsecTime& time) { return static_cast(time) * 1e-9; } 60 | 61 | /// \brief Convert the time (in seconds) to integer nanoseconds 62 | NsecTime secToNsec(const double& time) { return boost::int64_t(time * 1e9); } 63 | 64 | constexpr NsecTime getInvalidTime() { return std::numeric_limits::min(); } 65 | 66 | bool isValid(const NsecTime& time) { return getInvalidTime() == time; } 67 | 68 | } // namespace timing 69 | } // namespace okvis 70 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_timing/test/TestNsecTimeUtilities.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | TEST(NsetTimeTestSuite, testChronoConversion) { 6 | std::chrono::system_clock::time_point tp1 = std::chrono::system_clock::now(); 7 | okvis::timing::NsecTime ns1 = okvis::timing::chronoToNsec(tp1); 8 | std::chrono::system_clock::time_point tp2 = okvis::timing::nsecToChrono(ns1); 9 | ASSERT_TRUE(tp1 == tp2); 10 | } 11 | 12 | TEST(NsetTimeTestSuite, testSecConversion) { 13 | okvis::timing::NsecTime ns1 = okvis::timing::nsecNow(); 14 | double s2 = okvis::timing::nsecToSec(ns1); 15 | okvis::timing::NsecTime ns2 = okvis::timing::secToNsec(s2); 16 | 17 | ASSERT_LT(abs(ns1 - ns2), 1000000); 18 | } 19 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_timing/test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char** argv) { 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8.11) 2 | project(okvis_util) 3 | 4 | # nothing to build for now... 5 | add_library(${PROJECT_NAME} STATIC src/dependency-tracker.cc) 6 | 7 | # installation if required 8 | install(TARGETS ${PROJECT_NAME} 9 | EXPORT okvisTargets 10 | ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT lib 11 | PUBLIC_HEADER DESTINATION "${INSTALL_INCLUDE_DIR}/okvis" COMPONENT dev 12 | ) 13 | install(DIRECTORY include/ DESTINATION ${INSTALL_INCLUDE_DIR} COMPONENT dev FILES_MATCHING PATTERN "*.hpp") 14 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_util/include/okvis/source_file_pos.hpp: -------------------------------------------------------------------------------- 1 | /********************************************************************************* 2 | * OKVIS - Open Keyframe-based Visual-Inertial SLAM 3 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of 14 | * its contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | * 29 | * Created on: 30 | * Author: Paul Furgale 31 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 32 | *********************************************************************************/ 33 | 34 | /** 35 | * @file source_file_pos.hpp 36 | * @brief This file contains some helper functions for the assert macros. 37 | * @author Paul Furgale 38 | * @author Stefan Leutenegger 39 | */ 40 | 41 | #ifndef OKVIS_SOURCE_FILE_POS_HPP 42 | #define OKVIS_SOURCE_FILE_POS_HPP 43 | 44 | #include 45 | #include 46 | #include 47 | // A class and macro that gives you the current file position. 48 | 49 | /// \brief okvis Main namespace of this package. 50 | namespace okvis { 51 | 52 | class source_file_pos { 53 | public: 54 | std::string function; 55 | std::string file; 56 | int line; 57 | 58 | source_file_pos(std::string function, std::string file, int line) : function(function), file(file), line(line) {} 59 | 60 | operator std::string() { return toString(); } 61 | 62 | std::string toString() const { 63 | std::stringstream s; 64 | s << file << ":" << line << ": " << function << "()"; 65 | return s.str(); 66 | } 67 | }; 68 | 69 | } // namespace okvis 70 | 71 | inline std::ostream& operator<<(std::ostream& out, const okvis::source_file_pos& sfp) { 72 | out << sfp.file << ":" << sfp.line << ": " << sfp.function << "()"; 73 | return out; 74 | } 75 | 76 | #define OKVIS_SOURCE_FILE_POS okvis::source_file_pos(__FUNCTION__, __FILE__, __LINE__) 77 | 78 | #endif // OKVIS_SOURCE_FILE_POS_HPP 79 | -------------------------------------------------------------------------------- /okvis_ros/okvis/okvis_util/src/dependency-tracker.cc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AutonomousFieldRoboticsLab/SVIn/a6fe7011e3502ae199a34a085bb002ee7d9aad66/okvis_ros/okvis/okvis_util/src/dependency-tracker.cc -------------------------------------------------------------------------------- /okvis_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | okvis_ros 5 | 2.0.0 6 | The okvis_ros package 7 | 8 | Stefan Leutenegger 9 | Bharat Joshi 10 | Stefan Leutenegger 11 | Sharmin Rahman 12 | Bharat Joshi 13 | 14 | BSD 15 | ament_cmake 16 | rosidl_default_generators 17 | 18 | 19 | ament_index_cpp 20 | geometry_msgs 21 | rclcpp 22 | std_msgs 23 | std_srvs 24 | pcl_ros 25 | roscpp 26 | sensor_msgs 27 | cv_bridge 28 | pcl_conversions 29 | 30 | tf 31 | tf2 32 | tf2_ros 33 | tf2_geometry_msgs 34 | tf2_sensor_msgs 35 | nav_msgs 36 | visualization_msgs 37 | image_transport 38 | 39 | rosidl_default_runtime 40 | 41 | rosidl_interface_packages 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /okvis_ros/release.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "Enter release version x.y.z [ENTER]:" 3 | read version 4 | 5 | # first do the submodule 6 | cd okvis 7 | git archive --format=zip --prefix=okvis/ --output=okvis-$version.zip master 8 | #doxygen doxygen.config # regenerate docu 9 | #scp -rq documentation/html/* sleutene@shell1.doc.ic.ac.uk:public_html/software/docs/okvis/$version/ 10 | #if [$? -ne 0]; then 11 | # echo "upload to server failed" 12 | # exit 1 13 | #fi 14 | scp okvis-$version.zip sleutene@shell1.doc.ic.ac.uk:public_html/software/ 15 | if [$? -ne 0]; then 16 | echo "upload to server failed" 17 | exit 1 18 | fi 19 | 20 | # now the whole thing 21 | cd .. 22 | #doxygen doxygen.config # regenerate docu 23 | git archive --format=zip --prefix=okvis_ros/ --output=okvis_ros.zip master 24 | unzip okvis_ros.zip -d . 25 | rmdir okvis_ros/okvis 26 | rm okvis_ros/.gitmodules 27 | unzip okvis/okvis-$version.zip -d okvis_ros/ 28 | rm okvis_ros/release.sh # don't want to release this script... 29 | zip -r okvis_ros-$version.zip okvis_ros/* 30 | rm okvis_ros.zip 31 | rm -rf okvis_ros 32 | #scp -rq documentation/html/* sleutene@shell1.doc.ic.ac.uk:public_html/software/docs/okvis_ros/$version/ 33 | #if [ $? -ne 0 ]; then 34 | # echo "upload to server failed" 35 | # exit 1 36 | #fi 37 | scp okvis_ros-$version.zip sleutene@shell1.doc.ic.ac.uk:public_html/software/ 38 | #if [ $? -ne 0 ]; then 39 | # echo "upload to server failed" 40 | # exit 1 41 | #fi 42 | rm okvis/okvis-$version.zip 43 | rm okvis_ros-$version.zip 44 | -------------------------------------------------------------------------------- /okvis_ros/src/uncompress_image.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #if defined(ROS2_JAZZY) 5 | #include 6 | #elif defined(ROS2_HUMBLE) 7 | #include 8 | #else 9 | #include 10 | #endif 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | class UncompressImage : public rclcpp::Node { 18 | public: 19 | UncompressImage(const rclcpp::NodeOptions& options) : Node("uncompress_image", options) { 20 | this->get_parameter("compressed_img_topic", compressed_img_topic_); 21 | this->get_parameter("ouput_img_topic", uncompressed_img_topic_); 22 | 23 | compressed_img_sub_ = this->create_subscription( 24 | compressed_img_topic_, 1000, std::bind(&UncompressImage::compressedImageCallback, this, std::placeholders::_1)); 25 | output_img_pub_ = this->create_publisher(uncompressed_img_topic_, 100); 26 | } 27 | 28 | private: 29 | std::string compressed_img_topic_, uncompressed_img_topic_; 30 | rclcpp::Subscription::SharedPtr compressed_img_sub_; 31 | rclcpp::Publisher::SharedPtr output_img_pub_; 32 | 33 | void compressedImageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr compressed_img_msg_ptr) { 34 | RCLCPP_INFO_ONCE(this->get_logger(), "Got Compressed image"); 35 | 36 | rclcpp::Time stamp = compressed_img_msg_ptr->header.stamp; 37 | std_msgs::msg::Header header; 38 | header.stamp = stamp; 39 | header.frame_id = compressed_img_msg_ptr->header.frame_id; 40 | 41 | cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvCopy(compressed_img_msg_ptr); 42 | 43 | output_img_pub_->publish(*cv_ptr->toImageMsg()); 44 | } 45 | }; 46 | 47 | int main(int argc, char** argv) { 48 | rclcpp::init(argc, argv); 49 | rclcpp::NodeOptions options; 50 | options.allow_undeclared_parameters(true); 51 | options.automatically_declare_parameters_from_overrides(true); 52 | rclcpp::spin(std::make_shared(options)); 53 | 54 | return 0; 55 | } -------------------------------------------------------------------------------- /okvis_ros/srv/OdometryTrigger.srv: -------------------------------------------------------------------------------- 1 | nav_msgs/Odometry pose 2 | --- 3 | # Return borrowed from std_srvs/Trigger 4 | bool success # indicate successful run of triggered service 5 | string message # informational, e.g. for error messages -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/BowVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.cpp 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | // -------------------------------------------------------------------------- 21 | 22 | BowVector::BowVector(void) {} 23 | 24 | // -------------------------------------------------------------------------- 25 | 26 | BowVector::~BowVector(void) {} 27 | 28 | // -------------------------------------------------------------------------- 29 | 30 | void BowVector::addWeight(WordId id, WordValue v) { 31 | BowVector::iterator vit = this->lower_bound(id); 32 | 33 | if (vit != this->end() && !(this->key_comp()(id, vit->first))) { 34 | vit->second += v; 35 | } else { 36 | this->insert(vit, BowVector::value_type(id, v)); 37 | } 38 | } 39 | 40 | // -------------------------------------------------------------------------- 41 | 42 | void BowVector::addIfNotExist(WordId id, WordValue v) { 43 | BowVector::iterator vit = this->lower_bound(id); 44 | 45 | if (vit == this->end() || (this->key_comp()(id, vit->first))) { 46 | this->insert(vit, BowVector::value_type(id, v)); 47 | } 48 | } 49 | 50 | // -------------------------------------------------------------------------- 51 | 52 | void BowVector::normalize(LNorm norm_type) { 53 | double norm = 0.0; 54 | BowVector::iterator it; 55 | 56 | if (norm_type == DBoW2::L1) { 57 | for (it = begin(); it != end(); ++it) norm += fabs(it->second); 58 | } else { 59 | for (it = begin(); it != end(); ++it) norm += it->second * it->second; 60 | norm = sqrt(norm); 61 | } 62 | 63 | if (norm > 0.0) { 64 | for (it = begin(); it != end(); ++it) it->second /= norm; 65 | } 66 | } 67 | 68 | // -------------------------------------------------------------------------- 69 | 70 | std::ostream& operator<<(std::ostream& out, const BowVector& v) { 71 | BowVector::const_iterator vit; 72 | std::vector::const_iterator iit; 73 | unsigned int i = 0; 74 | const unsigned int N = v.size(); 75 | for (vit = v.begin(); vit != v.end(); ++vit, ++i) { 76 | out << "<" << vit->first << ", " << vit->second << ">"; 77 | 78 | if (i < N - 1) out << ", "; 79 | } 80 | return out; 81 | } 82 | 83 | // -------------------------------------------------------------------------- 84 | 85 | void BowVector::saveM(const std::string& filename, size_t W) const { 86 | std::fstream f(filename.c_str(), std::ios::out); 87 | 88 | WordId last = 0; 89 | BowVector::const_iterator bit; 90 | for (bit = this->begin(); bit != this->end(); ++bit) { 91 | for (; last < bit->first; ++last) { 92 | f << "0 "; 93 | } 94 | f << bit->second << " "; 95 | 96 | last = bit->first + 1; 97 | } 98 | for (; last < (WordId)W; ++last) f << "0 "; 99 | 100 | f.close(); 101 | } 102 | 103 | // -------------------------------------------------------------------------- 104 | 105 | } // namespace DBoW2 106 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/BowVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: BowVector.h 3 | * Date: March 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: bag of words vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_BOW_VECTOR__ 11 | #define __D_T_BOW_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Id of words 20 | typedef unsigned int WordId; 21 | 22 | /// Value of a word 23 | typedef double WordValue; 24 | 25 | /// Id of nodes in the vocabulary treee 26 | typedef unsigned int NodeId; 27 | 28 | /// L-norms for normalization 29 | enum LNorm { L1, L2 }; 30 | 31 | /// Weighting type 32 | enum WeightingType { TF_IDF, TF, IDF, BINARY }; 33 | 34 | /// Scoring type 35 | enum ScoringType { L1_NORM, L2_NORM, CHI_SQUARE, KL, BHATTACHARYYA, DOT_PRODUCT }; 36 | 37 | /// Vector of words to represent images 38 | class BowVector : public std::map { 39 | public: 40 | /** 41 | * Constructor 42 | */ 43 | BowVector(void); 44 | 45 | /** 46 | * Destructor 47 | */ 48 | ~BowVector(void); 49 | 50 | /** 51 | * Adds a value to a word value existing in the vector, or creates a new 52 | * word with the given value 53 | * @param id word id to look for 54 | * @param v value to create the word with, or to add to existing word 55 | */ 56 | void addWeight(WordId id, WordValue v); 57 | 58 | /** 59 | * Adds a word with a value to the vector only if this does not exist yet 60 | * @param id word id to look for 61 | * @param v value to give to the word if this does not exist 62 | */ 63 | void addIfNotExist(WordId id, WordValue v); 64 | 65 | /** 66 | * L1-Normalizes the values in the vector 67 | * @param norm_type norm used 68 | */ 69 | void normalize(LNorm norm_type); 70 | 71 | /** 72 | * Prints the content of the bow vector 73 | * @param out stream 74 | * @param v 75 | */ 76 | friend std::ostream& operator<<(std::ostream& out, const BowVector& v); 77 | 78 | /** 79 | * Saves the bow vector as a vector in a matlab file 80 | * @param filename 81 | * @param W number of words in the vocabulary 82 | */ 83 | void saveM(const std::string& filename, size_t W) const; 84 | }; 85 | 86 | } // namespace DBoW2 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/DBoW2.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DBoW2.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: Generic include file for the DBoW2 classes and 6 | * the specialized vocabularies and databases 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DBoW2 Library 12 | * 13 | * DBoW2 library for C++: 14 | * Bag-of-word image database for image retrieval. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://doriangalvez.com 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils, DUtilsCV, DVision and OpenCV libraries, 23 | * as well as the boost::dynamic_bitset class. 24 | * 25 | * \section citation Citation 26 | * If you use this software in academic works, please cite: 27 | 
28 |    @@ARTICLE{GalvezTRO12,
29 |     author={Galvez-Lopez, Dorian and Tardos, J. D.},
30 |     journal={IEEE Transactions on Robotics},
31 |     title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
32 |     year={2012},
33 |     month={October},
34 |     volume={28},
35 |     number={5},
36 |     pages={1188--1197},
37 |     doi={10.1109/TRO.2012.2197158},
38 |     ISSN={1552-3098}
39 |   }
40 |
41 | * 42 | * \section license License 43 | * This file is licensed under a Creative Commons 44 | * Attribution-NonCommercial-ShareAlike 3.0 license. 45 | * This file can be freely used and users can use, download and edit this file 46 | * provided that credit is attributed to the original author. No users are 47 | * permitted to use this file for commercial purposes unless explicit permission 48 | * is given by the original author. Derivative works must be licensed using the 49 | * same or similar license. 50 | * Check http://creativecommons.org/licenses/by-nc-sa/3.0/ to obtain further 51 | * details. 52 | * 53 | */ 54 | 55 | #ifndef __D_T_DBOW2__ 56 | #define __D_T_DBOW2__ 57 | 58 | /// Includes all the data structures to manage vocabularies and image databases 59 | namespace DBoW2 {} 60 | 61 | #include "BowVector.h" 62 | #include "FBrief.h" 63 | #include "FeatureVector.h" 64 | #include "QueryResults.h" 65 | #include "TemplatedDatabase.h" 66 | #include "TemplatedVocabulary.h" 67 | 68 | /// BRIEF Vocabulary 69 | typedef DBoW2::TemplatedVocabulary BriefVocabulary; 70 | 71 | /// BRIEF Database 72 | typedef DBoW2::TemplatedDatabase BriefDatabase; 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/FBrief.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FBrief.h" 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace DBoW2 { 17 | 18 | // -------------------------------------------------------------------------- 19 | 20 | void FBrief::meanValue(const std::vector& descriptors, FBrief::TDescriptor& mean) { 21 | mean.reset(); 22 | 23 | if (descriptors.empty()) return; 24 | 25 | const int N2 = descriptors.size() / 2; 26 | 27 | std::vector counters(FBrief::L, 0); 28 | 29 | std::vector::const_iterator it; 30 | for (it = descriptors.begin(); it != descriptors.end(); ++it) { 31 | const FBrief::TDescriptor& desc = **it; 32 | for (int i = 0; i < FBrief::L; ++i) { 33 | if (desc[i]) counters[i]++; 34 | } 35 | } 36 | 37 | for (int i = 0; i < FBrief::L; ++i) { 38 | if (counters[i] > N2) mean.set(i); 39 | } 40 | } 41 | 42 | // -------------------------------------------------------------------------- 43 | 44 | double FBrief::distance(const FBrief::TDescriptor& a, const FBrief::TDescriptor& b) { return (double)(a ^ b).count(); } 45 | 46 | // -------------------------------------------------------------------------- 47 | 48 | std::string FBrief::toString(const FBrief::TDescriptor& a) { 49 | return a.to_string(); // reversed 50 | } 51 | 52 | // -------------------------------------------------------------------------- 53 | 54 | void FBrief::fromString(FBrief::TDescriptor& a, const std::string& s) { 55 | // from boost::bitset 56 | std::stringstream ss(s); 57 | ss >> a; 58 | } 59 | 60 | // -------------------------------------------------------------------------- 61 | 62 | void FBrief::toMat32F(const std::vector& descriptors, cv::Mat& mat) { 63 | if (descriptors.empty()) { 64 | mat.release(); 65 | return; 66 | } 67 | 68 | const int N = descriptors.size(); 69 | 70 | mat.create(N, FBrief::L, CV_32F); 71 | 72 | for (int i = 0; i < N; ++i) { 73 | const TDescriptor& desc = descriptors[i]; 74 | float* p = mat.ptr(i); 75 | for (int j = 0; j < FBrief::L; ++j, ++p) { 76 | *p = (desc[j] ? 1.f : 0.f); 77 | } 78 | } 79 | } 80 | 81 | // -------------------------------------------------------------------------- 82 | 83 | } // namespace DBoW2 84 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/FBrief.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FBrief.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: functions for BRIEF descriptors 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_F_BRIEF__ 11 | #define __D_T_F_BRIEF__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include "../DVision/DVision.h" 18 | #include "FClass.h" 19 | 20 | namespace DBoW2 { 21 | 22 | /// Functions to manipulate BRIEF descriptors 23 | class FBrief : protected FClass { 24 | public: 25 | static const int L = 256; // Descriptor length (in bits) 26 | typedef std::bitset TDescriptor; 27 | typedef const TDescriptor* pDescriptor; 28 | 29 | /** 30 | * Calculates the mean value of a set of descriptors 31 | * @param descriptors 32 | * @param mean mean descriptor 33 | */ 34 | static void meanValue(const std::vector& descriptors, TDescriptor& mean); 35 | 36 | /** 37 | * Calculates the distance between two descriptors 38 | * @param a 39 | * @param b 40 | * @return distance 41 | */ 42 | static double distance(const TDescriptor& a, const TDescriptor& b); 43 | 44 | /** 45 | * Returns a string version of the descriptor 46 | * @param a descriptor 47 | * @return string version 48 | */ 49 | static std::string toString(const TDescriptor& a); 50 | 51 | /** 52 | * Returns a descriptor from a string 53 | * @param a descriptor 54 | * @param s string version 55 | */ 56 | static void fromString(TDescriptor& a, const std::string& s); 57 | 58 | /** 59 | * Returns a mat with the descriptors in float format 60 | * @param descriptors 61 | * @param mat (out) NxL 32F matrix 62 | */ 63 | static void toMat32F(const std::vector& descriptors, cv::Mat& mat); 64 | }; 65 | 66 | } // namespace DBoW2 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/FClass.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FClass.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: generic FClass to instantiate templated classes 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FCLASS__ 11 | #define __D_T_FCLASS__ 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | namespace DBoW2 { 18 | 19 | /// Generic class to encapsulate functions to manage descriptors. 20 | /** 21 | * This class must be inherited. Derived classes can be used as the 22 | * parameter F when creating Templated structures 23 | * (TemplatedVocabulary, TemplatedDatabase, ...) 24 | */ 25 | class FClass { 26 | class TDescriptor; 27 | typedef const TDescriptor* pDescriptor; 28 | 29 | /** 30 | * Calculates the mean value of a set of descriptors 31 | * @param descriptors 32 | * @param mean mean descriptor 33 | */ 34 | virtual void meanValue(const std::vector& descriptors, TDescriptor& mean) = 0; 35 | 36 | /** 37 | * Calculates the distance between two descriptors 38 | * @param a 39 | * @param b 40 | * @return distance 41 | */ 42 | static double distance(const TDescriptor& a, const TDescriptor& b); 43 | 44 | /** 45 | * Returns a string version of the descriptor 46 | * @param a descriptor 47 | * @return string version 48 | */ 49 | static std::string toString(const TDescriptor& a); 50 | 51 | /** 52 | * Returns a descriptor from a string 53 | * @param a descriptor 54 | * @param s string version 55 | */ 56 | static void fromString(TDescriptor& a, const std::string& s); 57 | 58 | /** 59 | * Returns a mat with the descriptors in float format 60 | * @param descriptors 61 | * @param mat (out) NxL 32F matrix 62 | */ 63 | static void toMat32F(const std::vector& descriptors, cv::Mat& mat); 64 | }; 65 | 66 | } // namespace DBoW2 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/FeatureVector.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.cpp 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "FeatureVector.h" 11 | #include 12 | #include 13 | #include 14 | 15 | namespace DBoW2 { 16 | 17 | // --------------------------------------------------------------------------- 18 | 19 | FeatureVector::FeatureVector(void) {} 20 | 21 | // --------------------------------------------------------------------------- 22 | 23 | FeatureVector::~FeatureVector(void) {} 24 | 25 | // --------------------------------------------------------------------------- 26 | 27 | void FeatureVector::addFeature(NodeId id, unsigned int i_feature) { 28 | FeatureVector::iterator vit = this->lower_bound(id); 29 | 30 | if (vit != this->end() && vit->first == id) { 31 | vit->second.push_back(i_feature); 32 | } else { 33 | vit = this->insert(vit, FeatureVector::value_type(id, std::vector())); 34 | vit->second.push_back(i_feature); 35 | } 36 | } 37 | 38 | // --------------------------------------------------------------------------- 39 | 40 | std::ostream& operator<<(std::ostream& out, const FeatureVector& v) { 41 | if (!v.empty()) { 42 | FeatureVector::const_iterator vit = v.begin(); 43 | 44 | const std::vector* f = &vit->second; 45 | 46 | out << "<" << vit->first << ": ["; 47 | if (!f->empty()) out << (*f)[0]; 48 | for (unsigned int i = 1; i < f->size(); ++i) { 49 | out << ", " << (*f)[i]; 50 | } 51 | out << "]>"; 52 | 53 | for (++vit; vit != v.end(); ++vit) { 54 | f = &vit->second; 55 | 56 | out << ", <" << vit->first << ": ["; 57 | if (!f->empty()) out << (*f)[0]; 58 | for (unsigned int i = 1; i < f->size(); ++i) { 59 | out << ", " << (*f)[i]; 60 | } 61 | out << "]>"; 62 | } 63 | } 64 | 65 | return out; 66 | } 67 | 68 | // --------------------------------------------------------------------------- 69 | 70 | } // namespace DBoW2 71 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/FeatureVector.h: -------------------------------------------------------------------------------- 1 | /** 2 | * File: FeatureVector.h 3 | * Date: November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: feature vector 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #ifndef __D_T_FEATURE_VECTOR__ 11 | #define __D_T_FEATURE_VECTOR__ 12 | 13 | #include 14 | #include 15 | #include 16 | #include "BowVector.h" 17 | 18 | namespace DBoW2 { 19 | 20 | /// Vector of nodes with indexes of local features 21 | class FeatureVector : public std::map > { 22 | public: 23 | /** 24 | * Constructor 25 | */ 26 | FeatureVector(void); 27 | 28 | /** 29 | * Destructor 30 | */ 31 | ~FeatureVector(void); 32 | 33 | /** 34 | * Adds a feature to an existing node, or adds a new node with an initial 35 | * feature 36 | * @param id node id to add or to modify 37 | * @param i_feature index of feature to add to the given node 38 | */ 39 | void addFeature(NodeId id, unsigned int i_feature); 40 | 41 | /** 42 | * Sends a string versions of the feature vector through the stream 43 | * @param out stream 44 | * @param v feature vector 45 | */ 46 | friend std::ostream& operator<<(std::ostream& out, const FeatureVector& v); 47 | }; 48 | 49 | } // namespace DBoW2 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DBoW/QueryResults.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * File: QueryResults.cpp 3 | * Date: March, November 2011 4 | * Author: Dorian Galvez-Lopez 5 | * Description: structure to store results of database queries 6 | * License: see the LICENSE.txt file 7 | * 8 | */ 9 | 10 | #include "QueryResults.h" 11 | #include 12 | #include 13 | 14 | namespace DBoW2 { 15 | 16 | // --------------------------------------------------------------------------- 17 | 18 | std::ostream& operator<<(std::ostream& os, const Result& ret) { 19 | os << ""; 20 | return os; 21 | } 22 | 23 | // --------------------------------------------------------------------------- 24 | 25 | std::ostream& operator<<(std::ostream& os, const QueryResults& ret) { 26 | if (ret.size() == 1) 27 | os << "1 result:" << std::endl; 28 | else 29 | os << ret.size() << " results:" << std::endl; 30 | 31 | QueryResults::const_iterator rit; 32 | for (rit = ret.begin(); rit != ret.end(); ++rit) { 33 | os << *rit; 34 | if (rit + 1 != ret.end()) os << std::endl; 35 | } 36 | return os; 37 | } 38 | 39 | // --------------------------------------------------------------------------- 40 | 41 | void QueryResults::saveM(const std::string& filename) const { 42 | std::fstream f(filename.c_str(), std::ios::out); 43 | 44 | QueryResults::const_iterator qit; 45 | for (qit = begin(); qit != end(); ++qit) { 46 | f << qit->Id << " " << qit->Score << std::endl; 47 | } 48 | 49 | f.close(); 50 | } 51 | 52 | // --------------------------------------------------------------------------- 53 | 54 | } // namespace DBoW2 55 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DUtils/DException.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DException.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: general exception of the library 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #pragma once 12 | 13 | #ifndef __D_EXCEPTION__ 14 | #define __D_EXCEPTION__ 15 | 16 | #include 17 | #include 18 | 19 | namespace DUtils { 20 | 21 | /// General exception 22 | class DException : public std::exception { 23 | public: 24 | /** 25 | * Creates an exception with a general error message 26 | */ 27 | DException(void) throw() : m_message("DUtils exception") {} 28 | 29 | /** 30 | * Creates an exception with a custom error message 31 | * @param msg: message 32 | */ 33 | DException(const char* msg) throw() : m_message(msg) {} 34 | 35 | /** 36 | * Creates an exception with a custom error message 37 | * @param msg: message 38 | */ 39 | DException(const std::string& msg) throw() : m_message(msg) {} 40 | 41 | /** 42 | * Destructor 43 | */ 44 | virtual ~DException(void) throw() {} 45 | 46 | /** 47 | * Returns the exception message 48 | */ 49 | virtual const char* what() const throw() { return m_message.c_str(); } 50 | 51 | protected: 52 | /// Error message 53 | std::string m_message; 54 | }; 55 | 56 | } // namespace DUtils 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DUtils/DUtils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DUtils.h 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 6, 2009 6 | * Description: include file for including all the library functionalities 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DUtils Library 12 | * 13 | * DUtils library for C++: 14 | * Collection of classes with general utilities for C++ applications. 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section license License 22 | * This program is free software: you can redistribute it and/or modify 23 | * it under the terms of the GNU Lesser General Public License (LGPL) as 24 | * published by the Free Software Foundation, either version 3 of the License, 25 | * or any later version. 26 | * 27 | */ 28 | 29 | #pragma once 30 | 31 | #ifndef __D_UTILS__ 32 | #define __D_UTILS__ 33 | 34 | /// Several utilities for C++ programs 35 | namespace DUtils {} 36 | 37 | // Exception 38 | #include "DException.h" 39 | 40 | #include "Timestamp.h" 41 | // Random numbers 42 | #include "Random.h" 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DUtils/Random.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * File: Random.cpp 3 | * Project: DUtils library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: April 2010 6 | * Description: manages pseudo-random numbers 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | #include "Random.h" 12 | #include 13 | #include "Timestamp.h" 14 | 15 | bool DUtils::Random::m_already_seeded = false; 16 | 17 | void DUtils::Random::SeedRand() { 18 | Timestamp time; 19 | time.setToCurrentTime(); 20 | srand((unsigned)time.getFloatTime()); 21 | } 22 | 23 | void DUtils::Random::SeedRandOnce() { 24 | if (!m_already_seeded) { 25 | DUtils::Random::SeedRand(); 26 | m_already_seeded = true; 27 | } 28 | } 29 | 30 | void DUtils::Random::SeedRand(int seed) { srand(seed); } 31 | 32 | void DUtils::Random::SeedRandOnce(int seed) { 33 | if (!m_already_seeded) { 34 | DUtils::Random::SeedRand(seed); 35 | m_already_seeded = true; 36 | } 37 | } 38 | 39 | int DUtils::Random::RandomInt(int min, int max) { 40 | int d = max - min + 1; 41 | return int(((double)rand() / ((double)RAND_MAX + 1.0)) * d) + min; 42 | } 43 | 44 | // --------------------------------------------------------------------------- 45 | // --------------------------------------------------------------------------- 46 | 47 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max) { 48 | if (min <= max) { 49 | m_min = min; 50 | m_max = max; 51 | } else { 52 | m_min = max; 53 | m_max = min; 54 | } 55 | 56 | createValues(); 57 | } 58 | 59 | // --------------------------------------------------------------------------- 60 | 61 | DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(const DUtils::Random::UnrepeatedRandomizer& rnd) { 62 | *this = rnd; 63 | } 64 | 65 | // --------------------------------------------------------------------------- 66 | 67 | int DUtils::Random::UnrepeatedRandomizer::get() { 68 | if (empty()) createValues(); 69 | 70 | DUtils::Random::SeedRandOnce(); 71 | 72 | int k = DUtils::Random::RandomInt(0, m_values.size() - 1); 73 | int ret = m_values[k]; 74 | m_values[k] = m_values.back(); 75 | m_values.pop_back(); 76 | 77 | return ret; 78 | } 79 | 80 | // --------------------------------------------------------------------------- 81 | 82 | void DUtils::Random::UnrepeatedRandomizer::createValues() { 83 | int n = m_max - m_min + 1; 84 | 85 | m_values.resize(n); 86 | for (int i = 0; i < n; ++i) m_values[i] = m_min + i; 87 | } 88 | 89 | // --------------------------------------------------------------------------- 90 | 91 | void DUtils::Random::UnrepeatedRandomizer::reset() { 92 | if ((int)m_values.size() != m_max - m_min + 1) createValues(); 93 | } 94 | 95 | // --------------------------------------------------------------------------- 96 | 97 | DUtils::Random::UnrepeatedRandomizer& DUtils::Random::UnrepeatedRandomizer::operator=( 98 | const DUtils::Random::UnrepeatedRandomizer& rnd) { 99 | if (this != &rnd) { 100 | this->m_min = rnd.m_min; 101 | this->m_max = rnd.m_max; 102 | this->m_values = rnd.m_values; 103 | } 104 | return *this; 105 | } 106 | 107 | // --------------------------------------------------------------------------- 108 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/DVision/DVision.h: -------------------------------------------------------------------------------- 1 | /* 2 | * File: DVision.h 3 | * Project: DVision library 4 | * Author: Dorian Galvez-Lopez 5 | * Date: October 4, 2010 6 | * Description: several functions for computer vision 7 | * License: see the LICENSE.txt file 8 | * 9 | */ 10 | 11 | /*! \mainpage DVision Library 12 | * 13 | * DVision library for C++ and OpenCV: 14 | * Collection of classes with computer vision functionality 15 | * 16 | * Written by Dorian Galvez-Lopez, 17 | * University of Zaragoza 18 | * 19 | * Check my website to obtain updates: http://webdiis.unizar.es/~dorian 20 | * 21 | * \section requirements Requirements 22 | * This library requires the DUtils and DUtilsCV libraries and the OpenCV library. 23 | * 24 | * \section license License 25 | * This program is free software: you can redistribute it and/or modify 26 | * it under the terms of the GNU Lesser General Public License (LGPL) as 27 | * published by the Free Software Foundation, either version 3 of the License, 28 | * or any later version. 29 | * 30 | */ 31 | 32 | #ifndef __D_VISION__ 33 | #define __D_VISION__ 34 | 35 | /// Computer vision functions 36 | namespace DVision {} 37 | 38 | #include "BRIEF.h" 39 | #include "BRIEF256.h" 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/VocabularyBinary.cpp: -------------------------------------------------------------------------------- 1 | #include "VocabularyBinary.hpp" 2 | #include 3 | 4 | SVINLoop::Vocabulary::Vocabulary() : nNodes(0), nodes(nullptr), nWords(0), words(nullptr) {} 5 | 6 | SVINLoop::Vocabulary::~Vocabulary() { 7 | if (nodes != nullptr) { 8 | delete[] nodes; 9 | nodes = nullptr; 10 | } 11 | 12 | if (words != nullptr) { 13 | delete[] words; 14 | words = nullptr; 15 | } 16 | } 17 | 18 | void SVINLoop::Vocabulary::serialize(std::ofstream& stream) { 19 | stream.write((const char*)this, staticDataSize()); 20 | stream.write((const char*)nodes, sizeof(Node) * nNodes); 21 | stream.write((const char*)words, sizeof(Word) * nWords); 22 | } 23 | 24 | void SVINLoop::Vocabulary::deserialize(std::ifstream& stream) { 25 | stream.read((char*)this, staticDataSize()); 26 | 27 | nodes = new Node[nNodes]; 28 | stream.read((char*)nodes, sizeof(Node) * nNodes); 29 | 30 | words = new Word[nWords]; 31 | stream.read((char*)words, sizeof(Word) * nWords); 32 | } 33 | -------------------------------------------------------------------------------- /pose_graph/ThirdParty/VocabularyBinary.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VocabularyBinary_hpp 2 | #define VocabularyBinary_hpp 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace SVINLoop { 9 | 10 | struct Node { 11 | int32_t nodeId; 12 | int32_t parentId; 13 | double weight; 14 | uint64_t descriptor[4]; 15 | }; 16 | 17 | struct Word { 18 | int32_t nodeId; 19 | int32_t wordId; 20 | }; 21 | 22 | struct Vocabulary { 23 | int32_t k; 24 | int32_t L; 25 | int32_t scoringType; 26 | int32_t weightingType; 27 | 28 | int32_t nNodes; 29 | int32_t nWords; 30 | 31 | Node* nodes; 32 | Word* words; 33 | 34 | Vocabulary(); 35 | ~Vocabulary(); 36 | 37 | void serialize(std::ofstream& stream); 38 | void deserialize(std::ifstream& stream); 39 | 40 | inline static size_t staticDataSize() { return sizeof(Vocabulary) - sizeof(Node*) - sizeof(Word*); } 41 | }; 42 | 43 | } // namespace SVINLoop 44 | 45 | #endif /* VocabularyBinary_hpp */ 46 | -------------------------------------------------------------------------------- /pose_graph/Vocabulary/brief_k10L6.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AutonomousFieldRoboticsLab/SVIn/a6fe7011e3502ae199a34a085bb002ee7d9aad66/pose_graph/Vocabulary/brief_k10L6.bin -------------------------------------------------------------------------------- /pose_graph/config/gopro/gopro.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: slave1 6 | # Note: considers only projection matrix assuming that the images (keyframes from okvis) are pre-rectifed 7 | projection_matrix: 8 | fx: 440.65324069487116 9 | fy: 439.4834334719291 10 | cx: 488.1804598032361 11 | cy: 266.6755685262641 12 | 13 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 14 | resizeFactor: 1.0 15 | 16 | #optimization parameters 17 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 18 | max_num_iterations: 8 # max solver itrations, to guarantee real time 19 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 20 | 21 | #loop closure parameters 22 | loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 23 | #also give the camera calibration file same as feature_tracker node 24 | fast_relocalization: 1 25 | min_loop_num: 50 26 | -------------------------------------------------------------------------------- /pose_graph/config/gopro/gopro2_uw.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: gopro2 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 35 23 | pnp_reprojection_threshold: 8.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_map_params: 33 | enable: 1 34 | min_landmark_quality: 0.001 35 | 36 | debug_image: 1 37 | 38 | image_height: 540 39 | image_width: 960 40 | focal_length: [583.16116134, 581.292988] 41 | principal_point: [481.92859774, 270.2504108] 42 | distortion_coefficients: [-0.10973803, 0.09313709, 0.00143941, 0.0004831] -------------------------------------------------------------------------------- /pose_graph/config/gopro/gopro_uw.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: gopro 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.05 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 20 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | #loop closure 16 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 17 | #also give the camera calibration file same as feature_tracker node 18 | loop_closure_params: 19 | enable: 1 20 | min_correspondences: 25 21 | pnp_reprojection_threshold: 10.0 22 | 23 | fast_relocalization: 1 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | 33 | # Added by Bharat 34 | # Parameters for color mapping as we need to subscribe to original color image 35 | debug_image: 0 36 | image_height: 960 37 | image_width: 540 38 | distortion_coefficients: [-0.1150974141409347, 0.10292772047643643, 0.001419836324816038, -0.0018379214527896284] 39 | focal_length: [582.7585362687838, 580.8446349178992] 40 | principal_point: [479.95702656023155, 271.8298929212255] 41 | -------------------------------------------------------------------------------- /pose_graph/config/speedo1/config_speedo1_downsize.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | # Note: considers only projection matrix assuming that the images (keyframes from okvis) are pre-rectifed 7 | projection_matrix: 8 | fx: 583.0115339295946 9 | fy: 583.0115339295946 10 | cx: 335.9927291870117 11 | cy: 310.6872386932373 12 | 13 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 14 | resizeFactor: 1.0 15 | 16 | #optimization parameters 17 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 18 | max_num_iterations: 8 # max solver itrations, to guarantee real time 19 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 20 | 21 | #loop closure parameters 22 | loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 23 | #also give the camera calibration file same as feature_tracker node 24 | fast_relocalization: 1 25 | min_loop_num: 25 26 | 27 | #For uber estimator 28 | use_health: 0 29 | keypoints_threshold: 20 30 | consecutive_good_keyframes: 5 31 | switching_time: 1.75 32 | 33 | # if enabled will save debug images in pose_graph/output_logs/ 34 | debug_image: 0 35 | 36 | # For undistorting original color image 37 | # R is rectification matrix 38 | # K is camera matrix 39 | is_stereo: 1 40 | cam0: 41 | height: 600 42 | width: 800 43 | D: [-0.16289394362063583, 0.08351833356682732, 0.012276483503048642, 0.0006858966792660155] 44 | K: [569.416384875, 0.0, 354.08646869, 45 | 0.0, 569.7973490352, 308.564486915, 46 | 0.0, 0.0, 1.0] 47 | R: [0.99876085, -0.02473601, 0.04318445, 48 | 0.02417981, 0.99961842, 0.01335487, 49 | -0.04349831, -0.01229413, 0.99897785] 50 | 51 | 52 | # To transform pose between uber and okvis coordinate systems 53 | T_SC: [-0.05832554, -0.49729442, 0.86561908, 0.4404166, 54 | 0.99825455, -0.02099889, 0.05519876, -0.04791664, 55 | -0.009273, 0.86732768, 0.49765118, -0.02872042, 56 | 0.0, 0.0, 0.0, 1.0] 57 | T_BS: [1.0, 0.0, 0.0, 0.0, 58 | 0.0, -1.0, 0.0, 0.0, 59 | 0.0, 0.0, -1.0, 0.0, 60 | 0.0, 0.0, 0.0, 1.0] 61 | -------------------------------------------------------------------------------- /pose_graph/config/speedo1/speedo1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 20 23 | pnp_reprojection_threshold: 10.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_map_params: 33 | enable: 1 34 | min_landmark_quality: 0.001 35 | 36 | debug_image: 0 37 | 38 | image_height: 1200 39 | image_width: 1600 40 | focal_length: [1138.83276975408, 1139.594698074792] 41 | principal_point: [708.1729373830466, 617.1289738266081] 42 | distortion_coefficients: [-0.16289394362063583, 0.08351833356682732, 0.012276483503048642, 0.0006858966792660155] -------------------------------------------------------------------------------- /pose_graph/config/speedo1/speedo1_bbdos2019_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | #loop closure parameters 16 | loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 17 | #also give the camera calibration file same as feature_tracker node 18 | fast_relocalization: 1 19 | min_loop_num: 30 20 | ransac_reproj_threshold: 10.0 21 | 22 | image_height: 600 23 | image_width: 800 24 | focal_length: [1138.83276975408, 1139.594698074792] 25 | principal_point: [308.1729373830466, 317.1289738266081] 26 | distortion_coefficients: [-0.16289394362063583, 0.08351833356682732, 0.012276483503048642, 0.0006858966792660155] 27 | -------------------------------------------------------------------------------- /pose_graph/config/speedo1/speedo1_bbdos2019_coral_lmw.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | # Note: considers only projection matrix assuming that the images (keyframes from okvis) are pre-rectifed 7 | projection_matrix: 8 | fx: 1240.64067844 9 | fy: 1240.64067844 10 | cx: 272.78517151 11 | cy: 310.37955475 12 | 13 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 14 | resizeFactor: 1.0 15 | 16 | #optimization parameters 17 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 18 | max_num_iterations: 8 # max solver itrations, to guarantee real time 19 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 20 | 21 | fast_relocalization: 1 22 | 23 | #loop closure 24 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 25 | #also give the camera calibration file same as feature_tracker node 26 | loop_closure_params: 27 | enable: 1 28 | min_correspondences: 25 29 | pnp_reprojection_threshold: 25.0 30 | 31 | 32 | #For uber estimator 33 | health: 34 | enable: 0 35 | min_keypoints: 15 36 | consecutive_keyframes: 7 37 | keyframe_wait_time: 1.8 38 | kps_per_quadrant: 1 39 | 40 | # if enabled will save debug images in pose_graph/output_logs/ 41 | debug_image: 1 42 | 43 | #minimum triangulated point quality to be added to the global map 44 | min_landmark_quality: 0.015 45 | 46 | # For undistorting original color image 47 | # R is rectification matrix 48 | # K is camera matrix 49 | is_stereo: 1 50 | cam0: 51 | height: 600 52 | width: 800 53 | D: [-0.16289394, 0.08351833, 0.01227648, 0.0006859] 54 | K: [1138.83276975, 0.0, 308.17293738, 55 | 0.0, 1139.59469807, 317.12897383, 56 | 0.0, 0.0, 1.0] 57 | R: [0.9987608453733716, -0.024735976749058985, 0.043184548201154145, 58 | 0.02417977658530305, 0.9996184201648292, 0.013354866957629642, 59 | -0.043498415526920325, -0.012294125485009934, 0.9989778487660305] 60 | 61 | # To transform pose between uber and okvis coordinate systems 62 | T_SC: [-0.05832554, -0.49729442, 0.86561908, 0.4404166, 63 | 0.99825455, -0.02099889, 0.05519876, -0.04791664, 64 | -0.009273, 0.86732768, 0.49765118, -0.02872042, 65 | 0.0, 0.0, 0.0, 1.0] 66 | T_BS: [1.0, 0.0, 0.0, 0.0, 67 | 0.0, -1.0, 0.0, 0.0, 68 | 0.0, 0.0, -1.0, 0.0, 69 | 0.0, 0.0, 0.0, 1.0] 70 | #to synchronize keyframe and raw images. This should be same as okvis config file 71 | image_delay: -0.02 72 | -------------------------------------------------------------------------------- /pose_graph/config/speedo1/speedo1_cropped.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 20 23 | pnp_reprojection_threshold: 10.0 24 | 25 | health: 26 | enable: 1 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_map_params: 33 | enable: 1 34 | min_landmark_quality: 0.001 35 | 36 | debug: 37 | enable: 1 38 | output_dir: /home/bjoshi/ros_workspaces/svin_ws/src/SVIn/pose_graph/output_logs 39 | 40 | image_height: 600 41 | image_width: 800 42 | focal_length: [1138.83276975408, 1139.594698074792] 43 | principal_point: [308.1729373830466, 317.1289738266081] 44 | distortion_coefficients: [-0.16289394362063583, 0.08351833356682732, 0.012276483503048642, 0.0006858966792660155] 45 | 46 | # To transform pose between uber and okvis coordinate systems 47 | T_SC: [-0.05832554, -0.49729442, 0.86561908, 0.4404166, 48 | 0.99825455, -0.02099889, 0.05519876, -0.04791664, 49 | -0.009273, 0.86732768, 0.49765118, -0.02872042, 50 | 0.0, 0.0, 0.0, 1.0] 51 | T_BS: [1.0, 0.0, 0.0, 0.0, 52 | 0.0, -1.0, 0.0, 0.0, 53 | 0.0, 0.0, -1.0, 0.0, 54 | 0.0, 0.0, 0.0, 1.0] -------------------------------------------------------------------------------- /pose_graph/config/speedo1/speedo1_downsize_coral_square.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: cam_fl 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 20 23 | pnp_reprojection_threshold: 10.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_map_params: 33 | enable: 1 34 | min_landmark_quality: 0.001 35 | 36 | debug_image: 0 37 | 38 | image_height: 600 39 | image_width: 800 40 | focal_length: [569.41638488, 569.79734904] 41 | principal_point: [354.08646869, 308.56448691] 42 | distortion_coefficients: [0.0, 0.0, 0.0, 0.0] -------------------------------------------------------------------------------- /pose_graph/config/stereo_rig/flir_air.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: left 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 0.5 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | #loop closure parameters 16 | loop_closure: 1 #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 17 | #also give the camera calibration file same as feature_tracker node 18 | fast_relocalization: 1 19 | min_loop_num: 35 20 | ransac_reproj_threshold: 10.0 21 | 22 | image_height: 1100 23 | image_width: 1600 24 | focal_length: [875.8760546337106, 874.3320431510673] 25 | principal_point: [795.6991969348003, 559.9630752248089] 26 | distortion_coefficients: [-0.076681019896292, 0.05597074463269014, -6.58597206142547e-05, -0.00036397743435735637] 27 | -------------------------------------------------------------------------------- /pose_graph/config/stereo_rig/flir_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | 6 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 7 | resizeFactor: 0.5 8 | 9 | #optimization parameters 10 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 11 | max_num_iterations: 8 # max solver itrations, to guarantee real time 12 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 13 | 14 | fast_relocalization: 1 15 | 16 | #loop closure 17 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 18 | #also give the camera calibration file same as feature_tracker node 19 | loop_closure_params: 20 | enable: 1 21 | min_correspondences: 25 22 | pnp_reprojection_threshold: 8.0 23 | pnp_ransac_iterations: 100 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_map_params: 33 | enable: 1 34 | min_landmark_quality: 0.01 35 | 36 | debug_image: 1 37 | 38 | image_height: 1100 39 | image_width: 1600 40 | distortion_coefficients: [0.1462139249855356, 0.3775304191882089, 0.007835564538475424, 0.002760598455182088] 41 | focal_length: [1159.2905564436558, 1157.0215864700963] 42 | principal_point: [804.4293114224579, 561.884540048856] 43 | -------------------------------------------------------------------------------- /pose_graph/config/stereo_rig/stereo_rig_v1_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: slave1 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 25 23 | pnp_reprojection_threshold: 8.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | debug_image: 1 33 | 34 | image_height: 800 35 | image_width: 600 36 | focal_length: [582.1271700458135, 581.6144453607703] 37 | principal_point: [381.7135426375258, 293.5048450307876] 38 | distortion_coefficients: [0.0, 0.0, 0.0, 0.0] -------------------------------------------------------------------------------- /pose_graph/config/stereo_rig/stereo_rig_v2_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: slave1 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 1.0 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 35 23 | pnp_reprojection_threshold: 8.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | global_mapping: 33 | enable: 0 34 | min_lmk_quality: 0.001 35 | 36 | debug_image: 1 37 | 38 | image_height: 600 39 | image_width: 800 40 | focal_length: [578.25942673, 578.28863331] 41 | principal_point: [381.60416582, 308.63899269] 42 | distortion_coefficients: [0.0, 0.0, 0.0, 0.0] -------------------------------------------------------------------------------- /pose_graph/config/stereo_rig/stereo_rig_v3_config.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | 3 | #camera calibration 4 | model_type: PINHOLE 5 | camera_name: slave1 6 | 7 | #image resize factor. Note that, this factor MUST be same as OKVIS config file 8 | resizeFactor: 0.5 9 | 10 | #optimization parameters 11 | max_solver_time: 0.04 # max solver itration time (ms), to guarantee real time 12 | max_num_iterations: 8 # max solver itrations, to guarantee real time 13 | keyframe_parallax: 10.0 # keyframe selection threshold (pixel) 14 | 15 | fast_relocalization: 1 16 | 17 | #loop closure 18 | #if you want to use loop closure to minimize the drift, set loop_closure true and give your brief pattern file path and vocabulary file path accordingly; 19 | #also give the camera calibration file same as feature_tracker node 20 | loop_closure_params: 21 | enable: 1 22 | min_correspondences: 35 23 | pnp_reprojection_threshold: 8.0 24 | 25 | health: 26 | enable: 0 27 | min_keypoints: 15 28 | consecutive_keyframes: 3 29 | keyframe_wait_time: 2.0 30 | kps_per_quadrant: 1 31 | 32 | debug_image: 0 33 | 34 | image_height: 1200 35 | image_width: 1600 36 | focal_length: [1149.52000993, 1150.57923685] 37 | principal_point: [798.75530924, 577.20248548] 38 | distortion_coefficients: [-0.16012168, 0.1020015, 0.00369027, 0.0034287] -------------------------------------------------------------------------------- /pose_graph/include/pose_graph/GlobalMapping.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "common/Definitions.h" 11 | 12 | struct Observation { 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | Observation(uint64_t kf_indx, const Eigen::Vector3d& local_pos, double quality, const Eigen::Vector3d& color) 16 | : kf_indx_(kf_indx), local_pos_(local_pos), quality_(quality), color_(color) {} 17 | 18 | Observation() : kf_indx_(0), local_pos_(Eigen::Vector3d::Zero()), quality_(0), color_(Eigen::Vector3d::Zero()) {} 19 | 20 | uint64_t kf_indx_; 21 | Eigen::Vector3d local_pos_; 22 | double quality_; 23 | Eigen::Vector3d color_; 24 | }; 25 | 26 | struct Landmark { 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | /** 29 | * @brief Constructor. 30 | * @param id ID of the point. E.g. landmark ID. 31 | * @param point Homogeneous coordinate of the point. 32 | * @param quality Quality of the point. Usually between 0 and 1. 33 | */ 34 | Landmark(uint64_t id, const Eigen::Vector3d& point, double quality, const Eigen::Vector3d& color) 35 | : id_(id), point_(point), quality_(quality), color_(color) {} 36 | Landmark() : id_(0), point_(Eigen::Vector3d::Zero()), quality_(0), color_(Eigen::Vector3d::Zero()) {} 37 | 38 | uint64_t id_; //< ID of the point. E.g. landmark ID. 39 | Eigen::Vector3d point_; //< coordinate of the point. 40 | double quality_; //< Quality of the point. Usually between 0 and 1. 41 | Eigen::Vector3d color_; //< Color of the point. 42 | 43 | std::unordered_map keyframe_observations_; // < Keyframe index, observation 44 | 45 | uint64_t latest_kf_obs_; 46 | 47 | void updateObservation(uint64_t keyframe_id, 48 | const Eigen::Vector3d& position, 49 | double quality, 50 | const Eigen::Vector3d& color); 51 | }; 52 | 53 | std::ostream& operator<<(std::ostream& os, const Landmark& landmark); 54 | 55 | typedef std::unordered_map, 58 | std::equal_to, 59 | Eigen::aligned_allocator>> 60 | LandmarkMap; 61 | 62 | class GlobalMap { 63 | private: 64 | // Map of all points. 65 | 66 | LandmarkMap map_points_; 67 | uint64_t last_loop_closure_optimization_time_; 68 | 69 | public: 70 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 71 | 72 | GlobalMap(); 73 | virtual ~GlobalMap() = default; 74 | 75 | void addLandmark(const Eigen::Vector3d& global_pos, 76 | uint64_t landmark_id, 77 | double quality, 78 | uint64_t keyframe_id, 79 | const Eigen::Vector3d& local_pos, 80 | const Eigen::Vector3d& color); 81 | 82 | void updateLandmark(uint64_t landmark_id, 83 | const Eigen::Vector3d& global_pos, 84 | double quality, 85 | const Eigen::Vector3d& color); 86 | 87 | LandmarkMap getMapPoints() const { return map_points_; } 88 | 89 | void loopClosureOptimizationFinishCallback(const Timestamp time); 90 | bool loop_closure_optimization_finished_; // this flag is set to true when loop closure optimization is done 91 | }; 92 | -------------------------------------------------------------------------------- /pose_graph/include/pose_graph/Parameters.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include "common/Definitions.h" 8 | 9 | struct LoopClosureParams { 10 | bool enabled; 11 | double pnp_reprojection_thresh; 12 | double pnp_ransac_iterations; 13 | int min_correspondences; 14 | }; 15 | 16 | struct HealthParams { 17 | bool enabled = false; // by default, health monitoring is disabled 18 | uint16_t consecutive_keyframes; 19 | uint16_t min_tracked_keypoints; 20 | uint16_t kps_per_quadrant; 21 | float kf_wait_time; 22 | }; 23 | 24 | struct GlobalMappingParams { 25 | bool enabled = false; // by default global mapping is disabled 26 | double min_lmk_quality = 0.001; 27 | }; 28 | 29 | class Parameters { 30 | public: 31 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 32 | 33 | Parameters(); 34 | ~Parameters() = default; 35 | 36 | Eigen::Vector3d tic_; 37 | Eigen::Matrix3d qic_; 38 | 39 | std::string brief_pattern_file_; 40 | 41 | std::string svin_w_loop_path_; 42 | 43 | int fast_relocalization_; 44 | 45 | std::string vocabulary_file_; 46 | 47 | // for visulization 48 | double camera_visual_size_; 49 | 50 | double resize_factor_; 51 | Eigen::Matrix4d T_body_imu_; 52 | 53 | // The first camera in the camera is used as refrence 54 | CameraCalibration camera_calibration_; 55 | 56 | bool debug_mode_ = false; 57 | std::string output_path_; 58 | std::string debug_output_path_; 59 | 60 | double image_delay_; 61 | 62 | // loop closure parameters 63 | LoopClosureParams loop_closure_params_; 64 | 65 | // health monitoring parameters 66 | HealthParams health_params_; 67 | 68 | // Global Mapping Parameters 69 | GlobalMappingParams global_mapping_params_; 70 | 71 | public: 72 | void loadParameters(const std::string& config_file); 73 | bool getCalibrationViaConfig(CameraCalibration& calibration, cv::FileNode camera_node); 74 | }; -------------------------------------------------------------------------------- /pose_graph/include/pose_graph/Publisher.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "common/Definitions.h" 15 | #include "pose_graph/Parameters.h" 16 | #include "utils/CameraPoseVisualization.h" 17 | 18 | class Publisher { 19 | public: 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | Publisher(std::shared_ptr node, bool debug_mode); 23 | 24 | ~Publisher() = default; 25 | 26 | void kfMatchedPointCloudCallback(const sensor_msgs::msg::PointCloud& pointcloud); 27 | void publishGlobalMap(const sensor_msgs::msg::PointCloud2& pointcloud); 28 | void publishPath(const std::vector& poses, 29 | const rclcpp::Publisher::SharedPtr pub) const; 30 | void publishPath(const nav_msgs::msg::Path& path, const rclcpp::Publisher::SharedPtr pub) const; 31 | void publishOdometry(const nav_msgs::msg::Odometry& odometry, 32 | const rclcpp::Publisher::SharedPtr pub) const; 33 | 34 | void publishKeyframePath(const std::pair& keyframe_pose, 35 | const std::pair& loop_closure_edge); 36 | void publishLoopClosurePath(const std::vector>& loop_closure_poses, 37 | const std::vector>& loop_closure_edges); 38 | void updatePublishGlobalMap(); 39 | 40 | void setGlobalPointCloudFunction(const PointCloudCallback& global_pointcloud_callback); 41 | bool savePointCloud(const std::shared_ptr request_header, 42 | const std::shared_ptr request, 43 | const std::shared_ptr response); // NOLINT 44 | 45 | void saveTrajectory(const std::string& filename) const; 46 | void publishPrimitiveEstimator(const std::pair& primitive_estimator_pose); 47 | 48 | private: 49 | std::shared_ptr node_; 50 | 51 | bool debug_mode_; // If true, publish additional topics for debugging 52 | rclcpp::Publisher::SharedPtr 53 | pub_gloal_map_; // Publishes sparse global map deformed after loop closure 54 | rclcpp::Publisher::SharedPtr 55 | pub_primitive_estimator_path_; // Publishes the path of the primitive estimator (debugging) 56 | rclcpp::Publisher::SharedPtr 57 | pub_primitive_odometry_; // Publishes primitive estimator odometry (debugging) 58 | 59 | rclcpp::Publisher::SharedPtr 60 | pub_robust_path_; // Publishes the path of the robust estimator without loop closure (debugging) 61 | rclcpp::Publisher::SharedPtr 62 | pub_robust_odometry_; // Publishes robust switching odometry (debugging) 63 | rclcpp::Publisher::SharedPtr pub_loop_closure_path_; // Publishes loop closure path 64 | rclcpp::Publisher::SharedPtr 65 | pub_visualization_; // Publishes visualization markers for rviz 66 | 67 | nav_msgs::msg::Path loop_closure_traj_; // Stores the loop closure path 68 | nav_msgs::msg::Path primitive_estimator_traj_; // Stores the primitive estimator path 69 | 70 | std::unique_ptr camera_pose_visualizer_; 71 | 72 | PointCloudCallback pointcloud_callback_; 73 | }; 74 | -------------------------------------------------------------------------------- /pose_graph/include/pose_graph/SwitchingEstimator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include "common/Definitions.h" 6 | #include "pose_graph/Parameters.h" 7 | /* 8 | This class contains functionality related to switching estimator. 9 | */ 10 | class SwitchingEstimator { 11 | private: 12 | TrackingStatus tracking_status_; 13 | HealthParams health_params_; 14 | Eigen::Matrix4d T_body_imu_; // Transformation from IMU to body frame 15 | Eigen::Matrix4d T_imu_cam0_; // Transformation from left camera (used for Keyframes) to IMU frame 16 | 17 | double forward_velocity_; // Forward velocity of the vehicle 18 | double lateral_velocity_; // Lateral velocity of the vehicle 19 | 20 | uint16_t consecutive_tracking_failures_; 21 | uint16_t consecutive_tracking_successes_; 22 | 23 | Eigen::Matrix4d init_t_w_prim_; 24 | Eigen::Matrix4d init_t_w_vio_; 25 | 26 | // All the pose are in Body Frame for uniformity 27 | Eigen::Matrix4d switch_t_w_prim_; 28 | Eigen::Matrix4d switch_t_w_vio_; 29 | Eigen::Matrix4d switch_t_w_robust_; 30 | 31 | Eigen::Matrix4d current_primitive_pose_; 32 | Eigen::Matrix4d current_vio_pose_; 33 | Eigen::Matrix4d current_robust_pose_; 34 | 35 | Timestamp last_vio_keyframe_time_ = -1; 36 | Timestamp last_primitive_estimator_time_ = -1; 37 | uint32_t primitive_estimator_kfs_ = 0; 38 | uint32_t latest_kf_index = 0; 39 | 40 | bool update_vio_keyframe_pose_ = false; 41 | bool update_primitive_estimator_pose_ = false; 42 | 43 | std::deque> primitive_estimator_poses_; 44 | 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | SwitchingEstimator(Parameters& params); 49 | ~SwitchingEstimator() = default; 50 | 51 | void addKeyframeInfo(KeyframeInfo& keyframe_info); 52 | 53 | void performHealthCheck(TrackingInfo& tracking_info, std::string& status_message); 54 | bool checkTrackingInfo(TrackingInfo& tracking_info, std::string& status_message); 55 | 56 | void updateVIOKeyframePose(Timestamp time, Eigen::Vector3d& translation, Eigen::Matrix3d& rotation); 57 | void addPrimitiveEstimatorPose(Timestamp timestamp, Eigen::Matrix4d& primitive_estimator_pose); 58 | 59 | bool getRobustPose(Timestamp& stamp, Eigen::Vector3d& translation, Eigen::Matrix3d& rotation); 60 | bool getLatestPrimitiveEstimatorPose(std::pair& primitive_pose); 61 | uint32_t getPrimitiveKFCount(); 62 | 63 | void getPrimitiveEstimatorPoses(std::vector>& primitive_poses); 64 | }; 65 | -------------------------------------------------------------------------------- /pose_graph/include/utils/CameraPoseVisualization.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "pose_graph/Parameters.h" 15 | class CameraPoseVisualization { 16 | public: 17 | std::string m_marker_ns; 18 | 19 | CameraPoseVisualization(float r, float g, float b, float a); 20 | 21 | void setImageBoundaryColor(float r, float g, float b, float a = 1.0); 22 | void setOpticalCenterConnectorColor(float r, float g, float b, float a = 1.0); 23 | void setScale(double s); 24 | void setLineWidth(double width); 25 | 26 | void add_pose(const Eigen::Vector3d& p, const Eigen::Quaterniond& q); 27 | void reset(); 28 | 29 | void publish_by(const rclcpp::Publisher::SharedPtr pub, 30 | const std_msgs::msg::Header& header); // NOLINT 31 | void add_edge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 32 | void add_loopedge(const Eigen::Vector3d& p0, const Eigen::Vector3d& p1); 33 | // void add_image(const Eigen::Vector3d& T, const Eigen::Matrix3d& R, const cv::Mat &src); 34 | // void publish_image_by(rclcpp::Publisher& pub, 35 | // const std_msgs::msg::Header& header); // NOLINT 36 | 37 | void clearCameraPoseMarkers(); 38 | 39 | private: 40 | std::vector camera_pose_markers; 41 | std::vector loop_edge_markers; 42 | 43 | std_msgs::msg::ColorRGBA m_image_boundary_color; 44 | std_msgs::msg::ColorRGBA m_optical_center_connector_color; 45 | double m_scale; 46 | double m_line_width; 47 | // visualization_msgs::msg::Marker image; 48 | int LOOP_EDGE_NUM; 49 | int tmp_loop_edge_num; 50 | 51 | static const Eigen::Vector3d imlt; 52 | static const Eigen::Vector3d imlb; 53 | static const Eigen::Vector3d imrt; 54 | static const Eigen::Vector3d imrb; 55 | static const Eigen::Vector3d oc; 56 | static const Eigen::Vector3d lt0; 57 | static const Eigen::Vector3d lt1; 58 | static const Eigen::Vector3d lt2; 59 | }; 60 | -------------------------------------------------------------------------------- /pose_graph/include/utils/Timer.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace utils { 7 | 8 | class Timer { 9 | public: 10 | static std::chrono::high_resolution_clock::time_point tic() { return std::chrono::high_resolution_clock::now(); } 11 | 12 | // Stop timer and report duration in given time. 13 | // Returns duration in milliseconds by default. 14 | // call .count() on returned duration to have number of ticks. 15 | template 16 | static T toc(const std::chrono::high_resolution_clock::time_point& start) { 17 | return std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - start); 18 | } 19 | }; 20 | 21 | // Usage: measure<>::execution(function, arguments) 22 | template 23 | struct Measure { 24 | template 25 | static typename T::rep execution(F&& func, Args&&... args) { 26 | auto start = std::chrono::steady_clock::now(); 27 | std::forward(func)(std::forward(args)...); 28 | auto duration = std::chrono::duration_cast(std::chrono::steady_clock::now() - start); 29 | return duration.count(); 30 | } 31 | }; 32 | 33 | } // namespace utils 34 | -------------------------------------------------------------------------------- /pose_graph/output_logs/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AutonomousFieldRoboticsLab/SVIn/a6fe7011e3502ae199a34a085bb002ee7d9aad66/pose_graph/output_logs/.gitignore -------------------------------------------------------------------------------- /pose_graph/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pose_graph 5 | 1.0.0 6 | The pose_graph package 7 | 8 | Bharat Joshi 9 | Sharmin Rahman 10 | 11 | Bharat Joshi 12 | BSD 13 | 14 | ament_cmake_ros 15 | 16 | ament_index_cpp 17 | okvis_ros 18 | rclcpp 19 | pcl_ros 20 | sensor_msgs 21 | pcl_conversions 22 | cv_bridge 23 | nav_msgs 24 | std_msgs 25 | geometry_msgs 26 | std_srvs 27 | visualization_msgs 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /pose_graph/scripts/histogram_equalize.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | from __future__ import print_function 4 | 5 | import rospy 6 | from sensor_msgs.msg import Image 7 | from cv_bridge import CvBridge 8 | 9 | import message_filters 10 | 11 | import cv2 12 | 13 | 14 | class histogram_equalize: 15 | def __init__(self, approximate_sync=True): 16 | 17 | self.left_subscriber = message_filters.Subscriber("/left/image_rect", 18 | Image, 19 | queue_size=1000) 20 | self.right_subscriber = message_filters.Subscriber("/right/image_rect", 21 | Image, 22 | queue_size=1000) 23 | 24 | self.left_image_pub = rospy.Publisher("/left_hist/image_rect", 25 | Image, 26 | queue_size=1000) 27 | self.right_image_pub = rospy.Publisher("/right_hist/image_rect", 28 | Image, 29 | queue_size=1000) 30 | 31 | # for speedo1 Lake Jocassee: clipLimit=1.5, titleGridSize=(2,2) 32 | # for stereo rig2 Florida cave: don't use this node 33 | # for stereo Rig Bus: clipLimit=2.0, tileGridSize=(5, 5) 34 | # these parameters can be changed to obtain best results for a particluar package 35 | 36 | # For more documentation see: https://docs.opencv.org/3.1.0/d5/daf/tutorial_py_histogram_equalization.html 37 | 38 | self.histenhance = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(5, 5)) 39 | 40 | Synchronizer = message_filters.ApproximateTimeSynchronizer if approximate_sync else message_filters.TimeSynchronizer 41 | slop = {1} if approximate_sync else {} 42 | self.time_sync = Synchronizer( 43 | [self.left_subscriber, self.right_subscriber], 100, *slop) 44 | 45 | self.time_sync.registerCallback(self.img_callback) 46 | 47 | def img_callback(self, left_msg, right_msg): 48 | 49 | #rospy.loginfo(" Received Image message") 50 | ## image msg to CV image## 51 | 52 | left_image_cv = bridge.imgmsg_to_cv2(left_msg, "mono8") 53 | right_image_cv = bridge.imgmsg_to_cv2(right_msg, "mono8") 54 | 55 | ##Contrast Enhancement ## 56 | left_enhanced_img = self.histenhance.apply(left_image_cv) 57 | right_enhanced_img = self.histenhance.apply(right_image_cv) 58 | 59 | #print (type(left_enhanced_img)) 60 | 61 | #### Create Image Msg #### 62 | enhanced_left_msg = bridge.cv2_to_imgmsg(left_enhanced_img, "mono8") 63 | enhanced_left_msg.header.stamp = left_msg.header.stamp 64 | enhanced_left_msg.height = left_msg.height 65 | enhanced_left_msg.width = left_msg.width 66 | enhanced_right_msg = bridge.cv2_to_imgmsg(right_enhanced_img, "mono8") 67 | enhanced_right_msg.header.stamp = right_msg.header.stamp 68 | enhanced_right_msg.height = right_msg.height 69 | enhanced_right_msg.width = right_msg.width 70 | 71 | # Publish new image 72 | self.left_image_pub.publish(enhanced_left_msg) 73 | self.right_image_pub.publish(enhanced_right_msg) 74 | 75 | 76 | if __name__ == "__main__": 77 | approximate_sync = rospy.get_param("~approximate_sync", True) 78 | histogram_equalize = histogram_equalize(approximate_sync) 79 | bridge = CvBridge() 80 | rospy.init_node('histogram_equalize', anonymous=True) 81 | while (not rospy.is_shutdown()): 82 | rospy.spin() 83 | -------------------------------------------------------------------------------- /pose_graph/scripts/save_okvis_kf_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | from email import header 5 | import rospy 6 | from nav_msgs.msg import Odometry 7 | import rospkg 8 | import os 9 | 10 | package_path = rospkg.RosPack().get_path("pose_graph") 11 | file_path = os.path.join(package_path, "okvis_kf_pose.txt") 12 | 13 | 14 | def odom_callback(odom_msg: Odometry): 15 | trans = odom_msg.pose.pose.position 16 | quat = odom_msg.pose.pose.orientation 17 | stamp = str(odom_msg.header.stamp) 18 | stamp = stamp[:10] + "." + stamp[10:] 19 | with open(file_path, "a") as file: 20 | file.write( 21 | f"{stamp} {trans.x} {trans.y} {trans.z} {quat.x} {quat.y} {quat.z} {quat.w}\n" 22 | ) 23 | 24 | 25 | if __name__ == "__main__": 26 | 27 | rospy.init_node("record_odom", anonymous=True) 28 | file = open(file_path, "w") 29 | file.close() 30 | rospy.Subscriber("/okvis_node/keyframe_pose", Odometry, odom_callback) 31 | 32 | while not rospy.is_shutdown(): 33 | rospy.spin() 34 | -------------------------------------------------------------------------------- /pose_graph/src/pose_graph/GlobalMapping.cpp: -------------------------------------------------------------------------------- 1 | #include "pose_graph/GlobalMapping.h" 2 | 3 | #include 4 | 5 | #include 6 | 7 | std::ostream& operator<<(std::ostream& os, const Landmark& landmark) { 8 | os << "Landmark: " << landmark.id_ << " at " << landmark.point_.transpose() << " with quality: " << landmark.quality_; 9 | os << "Total obs: " << landmark.keyframe_observations_.size() 10 | << "with latest at kf_indx: " << landmark.latest_kf_obs_; 11 | return os; 12 | } 13 | 14 | void Landmark::updateObservation(uint64_t keyframe_id, 15 | const Eigen::Vector3d& position, 16 | double quality, 17 | const Eigen::Vector3d& color) { 18 | if (keyframe_observations_.find(keyframe_id) != keyframe_observations_.end()) { 19 | LOG(WARNING) << "Multiple observation of landmark: " << id_ << " in keyframe: " << keyframe_id; 20 | return; 21 | } 22 | 23 | Observation observation(keyframe_id, position, quality, color); 24 | keyframe_observations_.insert(std::make_pair(keyframe_id, observation)); 25 | latest_kf_obs_ = keyframe_id; 26 | } 27 | 28 | GlobalMap::GlobalMap() { 29 | loop_closure_optimization_finished_ = false; 30 | last_loop_closure_optimization_time_ = 0; 31 | } 32 | 33 | void GlobalMap::addLandmark(const Eigen::Vector3d& global_pos, 34 | uint64_t landmark_id, 35 | double quality, 36 | uint64_t keyframe_id, 37 | const Eigen::Vector3d& local_pos, 38 | const Eigen::Vector3d& color) { 39 | if (!map_points_.count(landmark_id)) { // If the point is not in the map, add it. 40 | Landmark point_landmark = Landmark(landmark_id, global_pos, quality, color); 41 | point_landmark.updateObservation(keyframe_id, local_pos, quality, color); 42 | map_points_.insert(std::make_pair(landmark_id, point_landmark)); 43 | } else { 44 | Landmark& point_landmark = map_points_.at(landmark_id); 45 | point_landmark.color_ = color; 46 | point_landmark.point_ = global_pos; 47 | point_landmark.quality_ = quality; 48 | point_landmark.updateObservation(keyframe_id, local_pos, quality, color); 49 | } 50 | } 51 | 52 | void GlobalMap::updateLandmark(uint64_t landmark_id, 53 | const Eigen::Vector3d& global_pos, 54 | double quality, 55 | const Eigen::Vector3d& color) { 56 | if (map_points_.count(landmark_id)) { 57 | Landmark& point_landmark = map_points_.at(landmark_id); 58 | point_landmark.color_ = color; 59 | point_landmark.point_ = global_pos; 60 | point_landmark.quality_ = quality; 61 | } 62 | } 63 | 64 | void GlobalMap::loopClosureOptimizationFinishCallback(const Timestamp optimization_finish_time) { 65 | last_loop_closure_optimization_time_ = optimization_finish_time; 66 | loop_closure_optimization_finished_ = true; 67 | // LOG(WARN) << "Loop Closure Optimization finished at: " << optimization_finish_time); 68 | } 69 | -------------------------------------------------------------------------------- /pose_graph/src/utils/LoopClosureUtils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils/LoopClosureUtils.h" 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | bool LoopClosureUtils::geometricVerificationNister(const std::vector& matched_2d_cur, 9 | const std::vector& matched_2d_old, 10 | std::vector& status, 11 | int min_correspondences, 12 | opengv::transformation_t* camMatch_T_camQuery_mono) { 13 | BearingVectors query_versors, match_versors; 14 | 15 | query_versors.resize(matched_2d_cur.size()); 16 | match_versors.resize(matched_2d_old.size()); 17 | status.resize(matched_2d_cur.size()); 18 | 19 | for (size_t i = 0; i < match_versors.size(); i++) { 20 | Eigen::Vector3d p_cur(matched_2d_cur[i].pt.x, matched_2d_cur[i].pt.y, 1.0); 21 | Eigen::Vector3d p_old(matched_2d_old[i].x, matched_2d_old[i].y, 1.0); 22 | query_versors[i] = p_cur.normalized(); 23 | match_versors[i] = p_old.normalized(); 24 | status[i] = 0; 25 | } 26 | 27 | // Recover relative pose between frames, with translation up to a scalar. 28 | if (static_cast(match_versors.size()) >= min_correspondences) { 29 | AdapterMono adapter(match_versors, query_versors); 30 | 31 | // Use RANSAC to solve the central-relative-pose problem. 32 | opengv::sac::Ransac ransac; 33 | 34 | ransac.sac_model_ = std::make_shared(adapter, SacProblemMono::Algorithm::NISTER, true); 35 | ransac.max_iterations_ = 500; 36 | ransac.probability_ = 0.995; 37 | ransac.threshold_ = 1e-5; 38 | 39 | // Compute transformation via RANSAC. 40 | bool ransac_success = ransac.computeModel(); 41 | // ROS_INFO_STREAM("ransac 5pt size of input: " << query_versors.size() 42 | // << "\nransac 5pt inliers: " << ransac.inliers_.size() 43 | // << "\nransac 5pt iterations: " << ransac.iterations_); 44 | if (!ransac_success) { 45 | // ROS_WARN_STREAM("LoopClosureDetector Failure: RANSAC 5pt could not solve."); 46 | } else { 47 | double inlier_percentage = static_cast(ransac.inliers_.size()) / query_versors.size(); 48 | 49 | if (inlier_percentage >= 0.20 && ransac.iterations_ < 500) { 50 | *camMatch_T_camQuery_mono = ransac.model_coefficients_; 51 | // Remove the outliers based on ransac.inliers (modify i_query and 52 | // i_match AND pass the result out) 53 | for (const auto i : ransac.inliers_) { 54 | status[i] = 1; 55 | } 56 | return true; 57 | } 58 | } 59 | } 60 | 61 | return false; 62 | } 63 | -------------------------------------------------------------------------------- /pose_graph/src/utils/Utils.cpp: -------------------------------------------------------------------------------- 1 | #include "utils/Utils.h" 2 | 3 | #include 4 | 5 | Eigen::Matrix3d Utils::g2R(const Eigen::Vector3d& g) { 6 | Eigen::Matrix3d R0; 7 | Eigen::Vector3d ng1 = g.normalized(); 8 | Eigen::Vector3d ng2{0, 0, 1.0}; 9 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 10 | double yaw = Utils::R2ypr(R0).x(); 11 | R0 = Utils::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 12 | // R0 = Utils::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 13 | return R0; 14 | } 15 | 16 | std::string Utils::getTimeStr() { 17 | std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); 18 | 19 | char s[100]; 20 | std::strftime(s, sizeof(s), "%Y_%m_%d_%H_%M_%S", std::localtime(&now)); 21 | return s; 22 | } 23 | 24 | Eigen::Matrix4d Utils::rosPoseToMatrix(const geometry_msgs::msg::Pose& pose) { 25 | Eigen::Matrix4d m; 26 | m.setIdentity(); 27 | m.block<3, 3>(0, 0) = 28 | Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z) 29 | .toRotationMatrix(); 30 | m.block<3, 1>(0, 3) = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z); 31 | return m; 32 | } 33 | 34 | geometry_msgs::msg::Pose Utils::matrixToRosPose(const Eigen::Matrix4d& trasform) { 35 | geometry_msgs::msg::Pose pose; 36 | pose.position.x = trasform(0, 3); 37 | pose.position.y = trasform(1, 3); 38 | pose.position.z = trasform(2, 3); 39 | 40 | Eigen::Quaterniond q(trasform.block<3, 3>(0, 0)); 41 | pose.orientation.w = q.w(); 42 | pose.orientation.x = q.x(); 43 | pose.orientation.y = q.y(); 44 | pose.orientation.z = q.z(); 45 | 46 | return pose; 47 | } 48 | 49 | void Utils::printPoseAsEulerAngles(const Eigen::Matrix4d& pose) { 50 | Eigen::Vector3d trans = pose.block<3, 1>(0, 3); 51 | Eigen::Matrix3d rotm = pose.block<3, 3>(0, 0); 52 | std::cout << "trans: " << trans.transpose() << "\teul: " << Utils::R2ypr(rotm).transpose() << std::endl; 53 | } 54 | 55 | // Converts doulbe to sting with desired number of digits (total number of 56 | // digits) 57 | std::string Utils::To_string_with_precision(const double a_value, const int n) { 58 | std::ostringstream out; 59 | out << std::setprecision(n) << a_value; 60 | return out.str(); 61 | } 62 | 63 | std::string Utils::healthMsgToString(const okvis_ros::msg::SvinHealth::ConstSharedPtr health) { 64 | std::stringstream ss; 65 | std::setprecision(5); 66 | ss << "#keypoints: " << health->num_tracked_kps << "," 67 | << "#newkps: " << health->new_kps << "\n"; 68 | ss << "keyframes_per_quartile: " << health->kps_per_quadrant[0] << "," << health->kps_per_quadrant[1] << "," 69 | << health->kps_per_quadrant[2] << "," << health->kps_per_quadrant[3] << ","; 70 | 71 | return ss.str(); 72 | } 73 | 74 | rclcpp::Time Utils::toRosTime(const Timestamp t) { 75 | uint32_t nsec_part = t % 1000000000UL; 76 | uint32_t sec_part = static_cast(t) / 1000000000UL; 77 | return rclcpp::Time(sec_part, nsec_part); 78 | } 79 | 80 | Timestamp Utils::getHeaderStamp(const std_msgs::msg::Header& header) { 81 | return header.stamp.sec * 1000000000UL + header.stamp.nanosec; 82 | } 83 | -------------------------------------------------------------------------------- /rviz_config/dataset_converter.yaml: -------------------------------------------------------------------------------- 1 | #this parameter file stores the topic names of the bag file that has to 2 | # be converted 3 | 4 | dataset: ['okvis_dataset'] 5 | sensors: ['cam0','cam1','imu0','vicon'] 6 | data_file: 'data.csv' 7 | 8 | info: 9 | #type: type of sensor 10 | #topic: topic on which the sensor publishes its data, some times the initial '/' is not required 11 | #dir(optional): subdirectory in which the data has to be stored 12 | cam0: {type: camera, topic: '/cam0/image_raw', data_dir: 'data'} 13 | cam1: {type: camera, topic: 'cam1/image_raw', data_dir: 'data'} 14 | imu0: {type: imu, topic: '/imu0'} 15 | vicon: {type: vicon, topic: '/vicon/sensor_cross/sensor_cross'} 16 | --------------------------------------------------------------------------------