├── .clang-format ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── EQVIO_config_template.yaml ├── LICENSE ├── README.md ├── configs ├── EQVIO_config_EuRoC_stationary.yaml ├── EQVIO_config_UZHFPV.yaml ├── EQVIO_results_EuRoC_stationary.yaml └── EQVIO_results_UZHFPV.yaml ├── docs ├── intrin.png ├── main.md └── run.png ├── include └── eqvio │ ├── LoopTimer.h │ ├── VIOFilter.h │ ├── VIOFilterSettings.h │ ├── VIOSimulator.h │ ├── VIOVisualiser.h │ ├── VIOWriter.h │ ├── common │ ├── LieYaml.h │ ├── aofstream.h │ └── safeConfig.h │ ├── csv │ ├── CSVFile.h │ ├── CSVLine.h │ └── CSVReader.h │ ├── dataserver │ ├── APDatasetReader.h │ ├── ASLDatasetReader.h │ ├── DataServerBase.h │ ├── DatasetReaderBase.h │ ├── HiltiDatasetReader.h │ ├── RosbagDatasetReader.h │ ├── SimpleDataServer.h │ ├── SimulationDataServer.h │ ├── ThreadedDataServer.h │ ├── UZHFPVDatasetReader.h │ └── dataservers.h │ └── mathematical │ ├── EqFMatrices.h │ ├── Geometry.h │ ├── IMUVelocity.h │ ├── VIOGroup.h │ ├── VIOState.h │ ├── VIO_eqf.h │ └── VisionMeasurement.h ├── intrinsics.yaml ├── libs ├── CMakeLists.txt └── visualisation │ ├── CMakeLists.txt │ ├── include │ ├── PlotDataTypes.h │ └── Plotter.h │ └── src │ ├── Plotter.cpp │ └── main.cpp ├── scripts ├── DatasetInfo.py ├── analyse_timing_data.py ├── analysis_tools.py ├── euroc_sequences.yaml ├── run_and_analyse_dataset.py └── summarise_results.py ├── src ├── LoopTimer.cpp ├── VIOFilter.cpp ├── VIOSimulator.cpp ├── VIOVisualiser.cpp ├── VIOWriter.cpp ├── dataserver │ ├── APDatasetReader.cpp │ ├── ASLDatasetReader.cpp │ ├── DataServerBase.cpp │ ├── HiltiDatasetReader.cpp │ ├── RosbagDatasetReader.cpp │ ├── SimpleDataServer.cpp │ ├── SimulationDataServer.cpp │ ├── ThreadedDataServer.cpp │ └── UZHFPVDatasetReader.cpp ├── main_opt.cpp ├── main_sim.cpp └── mathematical │ ├── EqFMatrices.cpp │ ├── Geometry.cpp │ ├── IMUVelocity.cpp │ ├── VIOGroup.cpp │ ├── VIOState.cpp │ ├── VIO_eqf.cpp │ ├── VisionMeasurement.cpp │ └── coordinateSuite │ ├── euclid.cpp │ ├── invdepth.cpp │ └── normal.cpp └── test ├── CMakeLists.txt ├── GTestCMakeLists.txt.in ├── PrepareGTest.cmake ├── test_CSVReader.cpp ├── test_CoordinateCharts.cpp ├── test_EqFMatrices.cpp ├── test_FilterStatistics.cpp ├── test_VIOGroup.cpp ├── test_VIOGroupActions.cpp ├── test_VIOLift.cpp ├── test_settings.cpp ├── testing_utilities.cpp └── testing_utilities.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: LLVM 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: AlwaysBreak 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Right 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: Never 20 | AllowShortLoopsOnASingleLine: false 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: false 24 | AlwaysBreakTemplateDeclarations: MultiLine 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 120 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: false 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Preserve 70 | IncludeCategories: 71 | - Regex: '^"(llvm|llvm-c|clang|clang-c)/' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^(<|"(gtest|gmock|isl|json)/)' 75 | Priority: 3 76 | SortPriority: 0 77 | - Regex: '.*' 78 | Priority: 1 79 | SortPriority: 0 80 | IncludeIsMainRegex: '(Test)?$' 81 | IncludeIsMainSourceRegex: '' 82 | IndentCaseLabels: false 83 | IndentGotoLabels: true 84 | IndentPPDirectives: None 85 | IndentWidth: 4 86 | IndentWrappedFunctionNames: false 87 | JavaScriptQuotes: Leave 88 | JavaScriptWrapImports: true 89 | KeepEmptyLinesAtTheStartOfBlocks: true 90 | MacroBlockBegin: '' 91 | MacroBlockEnd: '' 92 | MaxEmptyLinesToKeep: 1 93 | NamespaceIndentation: None 94 | ObjCBinPackProtocolList: Auto 95 | ObjCBlockIndentWidth: 2 96 | ObjCSpaceAfterProperty: false 97 | ObjCSpaceBeforeProtocolList: true 98 | PenaltyBreakAssignment: 2 99 | PenaltyBreakBeforeFirstCallParameter: 19 100 | PenaltyBreakComment: 300 101 | PenaltyBreakFirstLessLess: 120 102 | PenaltyBreakString: 1000 103 | PenaltyBreakTemplateDeclaration: 10 104 | PenaltyExcessCharacter: 1000000 105 | PenaltyReturnTypeOnItsOwnLine: 60 106 | PointerAlignment: Left 107 | ReflowComments: true 108 | SortIncludes: true 109 | SortUsingDeclarations: true 110 | SpaceAfterCStyleCast: false 111 | SpaceAfterLogicalNot: false 112 | SpaceAfterTemplateKeyword: true 113 | SpaceBeforeAssignmentOperators: true 114 | SpaceBeforeCpp11BracedList: false 115 | SpaceBeforeCtorInitializerColon: true 116 | SpaceBeforeInheritanceColon: true 117 | SpaceBeforeParens: ControlStatements 118 | SpaceBeforeRangeBasedForLoopColon: true 119 | SpaceInEmptyBlock: false 120 | SpaceInEmptyParentheses: false 121 | SpacesBeforeTrailingComments: 1 122 | SpacesInAngles: false 123 | SpacesInConditionalStatement: false 124 | SpacesInContainerLiterals: true 125 | SpacesInCStyleCastParentheses: false 126 | SpacesInParentheses: false 127 | SpacesInSquareBrackets: false 128 | SpaceBeforeSquareBrackets: false 129 | Standard: Latest 130 | StatementMacros: 131 | - Q_UNUSED 132 | - QT_REQUIRE_VERSION 133 | TabWidth: 8 134 | UseCRLF: false 135 | UseTab: Never 136 | ... 137 | 138 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | build/* 3 | build_stable/* 4 | .cache/* 5 | cmake-build-Release/* 6 | *.swp 7 | EQVIO_output* 8 | *.zip 9 | *.bag 10 | !*.yaml 11 | data/euroc/* 12 | **.pyc 13 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "external/GIFT"] 2 | path = external/GIFT 3 | url = git@github.com:pvangoor/GIFT.git 4 | [submodule "external/argparse"] 5 | path = external/argparse 6 | url = git@github.com:p-ranav/argparse.git 7 | [submodule "external/LiePP"] 8 | path = external/LiePP 9 | url = git@github.com:pvangoor/LiePP 10 | branch = main 11 | -------------------------------------------------------------------------------- /EQVIO_config_template.yaml: -------------------------------------------------------------------------------- 1 | eqf: 2 | initialVariance: 3 | attitude: 1.0 4 | position: 1.0 5 | velocity: 1.0 6 | point: 5000.0 7 | pointDepth: -1.0 8 | cameraAttitude: 0.1 9 | cameraPosition: 0.1 10 | biasGyr: 1.0 11 | biasAcc: 1.0 12 | processVariance: 13 | cameraPosition: 0.0001 14 | cameraAttitude: 0.0001 15 | biasGyr: 0.0001 16 | biasAcc: 0.0001 17 | attitude: 0.01 18 | position: 0.01 19 | velocity: 0.1 20 | point: 0.001 21 | initialValue: 22 | sceneDepth: 1.0 23 | cameraOffset: 24 | - xw 25 | - -0.0216401454975 26 | - -0.064676986768 27 | - 0.00981073058949 28 | - 0.7123014606690344 29 | - -0.007707179755538301 30 | - 0.010499323370588468 31 | - 0.7017528002920512 32 | measurementNoise: 33 | feature: 0.003 34 | featureOutlierAbs: 0.01 35 | featureOutlierProb: 3.0 36 | featureRetention: 0.2 37 | velocityNoise: 38 | gyr: 0.0001 39 | acc: 0.0001 40 | gyrBias: 0.0001 41 | accBias: 0.0001 42 | settings: 43 | fastRiccati: false 44 | useDiscreteInnovationLift: true 45 | useDiscreteVelocityLift: true 46 | coordinateChoice: Euclidean 47 | useMedianDepth: true 48 | useFeaturePredictions: false 49 | useEquivariantOutput: true 50 | removeLostLandmarks: true 51 | useDiscreteStateMatrix: false 52 | GIFT: 53 | intrinsicsFile: intrinsics_example.yaml 54 | maxFeatures: 30 55 | featureDist: 30 56 | minHarrisQuality: 0.05 57 | featureSearchThreshold: 0.8 58 | maxError: 1e8 59 | winSize: 21 60 | maxLevel: 3 61 | trackedFeatureDist: 20.0 62 | equaliseImageHistogram: false 63 | main: 64 | writeState: false 65 | showVisualisation: true 66 | limitRate: 20.0 # Hz -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EqVIO (Equivariant Visual Inertial Odometry) 2 | 3 | This repository contains the implementation of EqVIO: An Equivariant Filter (EqF) for Visual Inertial Odometry (VIO). 4 | 5 | Please see https://pvangoor.github.io/eqvio_docs/ for the documentation. 6 | 7 | ## Dependencies 8 | 9 | - Eigen 3: `sudo apt install libeigen3-dev` 10 | - Yaml-cpp: `sudo apt install libyaml-cpp-dev` 11 | - GIFT: https://github.com/pvangoor/GIFT 12 | 13 | ### Optional Dependencis 14 | 15 | - FreeGLUT (for visualisations): `sudo apt install freeglut3-dev` 16 | - ROS (for reading ROS-Bags): http://wiki.ros.org/ROS/Installation 17 | - Doxygen (for documentation): `sudo apt install doxygen` 18 | 19 | ### Build Guide (for ros users) 20 | 21 | Modify the following settings in cmake if you want ROS compatibility. 22 | You can use command-line cmake, use cmake-gui for convenience, or edit the CMakeLists.txt directly. 23 | 24 | ``` 25 | EQVIO_BUILD_TESTS ON 26 | EQVIO_BUILD_VISUALISATION ON 27 | EQVIO_BUILD_ROSBAG ON 28 | EQVIO_SUPPORT_CONCEPTS OFF 29 | EXTRA_WARNINGS ON 30 | ``` 31 | 32 | Then do the following commands from a terminal in the main directory to build EqVIO: 33 | 34 | ``` 35 | mkdir build 36 | cd build 37 | cmake .. 38 | cmake --build . -j8 39 | ``` 40 | 41 | ### Running Guide 42 | 43 | #### ASL Dataset Format 44 | 45 | The file `intrinsics.yaml` is not needed as the required data is included in each sensor folder. Run EqVIO with: 46 | 47 | ``` 48 | ./build/eqvio_opt ~/datasets/euroc_asl/V1_01_easy/ configs/EQVIO_config_EuRoC_stationary.yaml --display --mode asl 49 | ``` 50 | 51 | You will need to replace the dataset directory with a directory of your choice. 52 | 53 | 54 | #### ROS bag 55 | 56 | The `intrinsics.yaml` file is extracted from the sensors'.yaml files. 57 | Since all of the sensors in the EuRoC dataset are identical, one intrinsics.yaml suits all. 58 | If you plan to use a different dataset, you will need to change the intrinsics to match accordingly. 59 | The example file is located in the root folder. 60 | To use it, you should place the `intrinsics.yaml` file in the same folder as the ROSbags you wish to use: 61 | 62 | ![run](docs/intrin.png) 63 | 64 | Run EqVIO on ROSbags using: 65 | 66 | ``` 67 | ./build/eqvio_opt ./data/euroc/MH_03_medium.bag configs/EQVIO_config_EuRoC_stationary.yaml --display --mode ros 68 | 69 | ./build/eqvio_opt ./data/euroc/V1_02_medium.bag configs/EQVIO_config_EuRoC_stationary.yaml --display --mode ros 70 | ``` 71 | 72 | ![run](docs/run.png) 73 | 74 | And result can be seen in the output file. 75 | -------------------------------------------------------------------------------- /configs/EQVIO_config_EuRoC_stationary.yaml: -------------------------------------------------------------------------------- 1 | GIFT: 2 | equaliseImageHistogram: false 3 | useFastFeatures: false 4 | featureDist: 79.80937096082073 5 | featureSearchThreshold: 0.8854861727179565 6 | maxError: 76.21556706799433 7 | maxFeatures: 40 8 | maxLevel: 3 9 | minHarrisQuality: 0.0792713927794865 10 | ransacParams: 11 | inlierThreshold: 0.0023121620935037416 12 | maxIterations: 34 13 | minDataPoints: 5 14 | minInliers: 30 15 | trackedFeatureDist: 30.79127938908608 16 | winSize: 21 17 | eqf: 18 | initialValue: 19 | sceneDepth: 5.00028218320243 20 | initialVariance: 21 | attitude: 0.13565029126052572 22 | biasAcc: 1.5813333765300104 23 | biasGyr: 97162.79515771076 24 | cameraAttitude: 0.0010228558965517584 25 | cameraPosition: 0.023501400846134893 26 | point: 129.90415638150924 27 | position: 0.1 28 | velocity: 8.974852995731e-08 29 | measurementNoise: 30 | feature: 1.9297839969591413 31 | featureOutlierAbs: 4.852186665580312 32 | featureOutlierProb: 0.03229809583062128 33 | featureRetention: 0.18594708334486176 34 | processVariance: 35 | attitude: 6.025875320811407e-05 36 | biasAcc: 0.0 37 | biasGyr: 0.0 38 | cameraAttitude: 5.075382174045239e-06 39 | cameraPosition: 1.2188313140115635e-05 40 | point: 0.00029845436136043135 41 | position: 9.981466095928483e-06 42 | velocity: 0.025317333863551263 43 | settings: 44 | coordinateChoice: InvDepth 45 | fastRiccati: true 46 | useDiscreteInnovationLift: false 47 | useDiscreteVelocityLift: true 48 | useEquivariantOutput: true 49 | useFeaturePredictions: false 50 | useInnovationLift: true 51 | useMedianDepth: false 52 | velocityNoise: 53 | acc: 0.012438843268295521 54 | accBias: 0.004462289865453429 55 | gyr: 0.000243153572917808 56 | gyrBias: 0.00013372703521098622 57 | main: 58 | limitRate: 0.0 59 | startTime: 0.0 60 | writeState: true 61 | cameraLag: 0.00 62 | 63 | 64 | sim: 65 | cameraOffset: 66 | - xw 67 | - -0.04844625116694773 68 | - -0.08142350220051031 69 | - 0.003102425417307478 70 | - 0.7021240268103595 71 | - -0.006484783602096129 72 | - 0.013217864997878212 73 | - 0.7017528002920512 74 | fieldOfView: 60 75 | maxFeatures: 40 76 | numPoints: 5000 77 | randomSeed: 0 78 | wallDistance: 1.0 -------------------------------------------------------------------------------- /configs/EQVIO_config_UZHFPV.yaml: -------------------------------------------------------------------------------- 1 | GIFT: 2 | equaliseImageHistogram: true 3 | useFastFeatures: false 4 | featureDist: 25.91373395034039 5 | featureSearchThreshold: 0.7 6 | maxError: 100.08998519259788 7 | maxFeatures: 40 8 | maxLevel: 3 9 | minHarrisQuality: 0.08859465154404257 10 | ransacParams: 11 | inlierThreshold: 0.000991045856248861 12 | maxIterations: 20 13 | minDataPoints: 10 14 | minInliers: 37 15 | trackedFeatureDist: 9.995503774595479 16 | winSize: 21 17 | eqf: 18 | initialValue: 19 | sceneDepth: 8.891397050194614 20 | initialVariance: 21 | attitude: 0.10282752317467045 22 | biasAcc: 1.2232071190499316 23 | biasGyr: 1.1673134780260075 24 | cameraAttitude: 1.727825980507864e-07 25 | cameraPosition: 3.349654391578276e-07 26 | point: 100.0 27 | position: 0.00011220184543019634 28 | velocity: 3.6517412725483775e-06 29 | measurementNoise: 30 | feature: 3.7583740428844425 31 | featureOutlierAbs: 5.4509224619256385 32 | featureOutlierProb: 0.23374912831534894 33 | featureRetention: 0.2 34 | processVariance: 35 | attitude: 6.219421634147766e-08 36 | biasAcc: 0.0 37 | biasGyr: 0.0 38 | cameraAttitude: 2.2630153511576583e-06 39 | cameraPosition: 6.853895838650084e-07 40 | point: 0.000530103448340995 41 | position: 1.2589961848499808e-05 42 | velocity: 0.012232071190499315 43 | settings: 44 | coordinateChoice: InvDepth 45 | fastRiccati: true 46 | useDiscreteInnovationLift: false 47 | useDiscreteVelocityLift: true 48 | useEquivariantOutput: true 49 | useFeaturePredictions: false 50 | useInnovationLift: true 51 | useMedianDepth: false 52 | velocityNoise: 53 | acc: 3.262345818455677e-05 54 | accBias: 0.0063404671195099425 55 | gyr: 0.0011913242870580211 56 | gyrBias: 0.00020008996495836354 57 | main: 58 | cameraLag: 0.012232071190499317 59 | limitRate: 0.0 60 | startTime: 0.0 61 | writeState: true 62 | sim: 63 | cameraOffset: 64 | - xw 65 | - -0.04844625116694773 66 | - -0.08142350220051031 67 | - 0.003102425417307478 68 | - 0.7021240268103595 69 | - -0.006484783602096129 70 | - 0.013217864997878212 71 | - 0.7017528002920512 72 | fieldOfView: 60 73 | maxFeatures: 40 74 | numPoints: 5000 75 | randomSeed: 0 76 | wallDistance: 1.0 -------------------------------------------------------------------------------- /configs/EQVIO_results_EuRoC_stationary.yaml: -------------------------------------------------------------------------------- 1 | Early Finish flag: 2 | MH_01_easy: false 3 | MH_02_easy: false 4 | MH_03_medium: false 5 | MH_04_difficult: false 6 | MH_05_difficult: false 7 | V1_01_easy: false 8 | V1_02_medium: false 9 | V1_03_difficult: false 10 | V2_01_easy: false 11 | V2_02_medium: false 12 | V2_03_difficult: false 13 | NaN flag: 14 | MH_01_easy: false 15 | MH_02_easy: false 16 | MH_03_medium: false 17 | MH_04_difficult: false 18 | MH_05_difficult: false 19 | V1_01_easy: false 20 | V1_02_medium: false 21 | V1_03_difficult: false 22 | V2_01_easy: false 23 | V2_02_medium: false 24 | V2_03_difficult: false 25 | Trajectory length: 26 | MH_01_easy: 73.0157421651321 27 | MH_02_easy: 63.70540216156182 28 | MH_03_medium: 127.35526466112435 29 | MH_04_difficult: 88.6148895748826 30 | MH_05_difficult: 94.27476179359579 31 | V1_01_easy: 58.56120400739347 32 | V1_02_medium: 75.85980647296816 33 | V1_03_difficult: 78.92019389844371 34 | V2_01_easy: 36.43699718581205 35 | V2_02_medium: 83.15172072264541 36 | V2_03_difficult: 85.98779430736637 37 | attitude (d)/rmse: 38 | MH_01_easy: 2.153860998826013 39 | MH_02_easy: 1.2320652677435886 40 | MH_03_medium: 1.1419450564319433 41 | MH_04_difficult: 1.0272909031453876 42 | MH_05_difficult: 1.2972947523513327 43 | V1_01_easy: 5.832888183647437 44 | V1_02_medium: 2.9020137064070517 45 | V1_03_difficult: 3.63610384089588 46 | V2_01_easy: 1.265665311915787 47 | V2_02_medium: 2.9879028298524353 48 | V2_03_difficult: 1.9263612537525863 49 | position (m)/rmse: 50 | MH_01_easy: 0.14338760371731546 51 | MH_02_easy: 0.20395310086981192 52 | MH_03_medium: 0.09429728852586221 53 | MH_04_difficult: 0.2191503660789769 54 | MH_05_difficult: 0.27743250446226775 55 | V1_01_easy: 0.056094933186874404 56 | V1_02_medium: 0.13614408692258764 57 | V1_03_difficult: 0.19352257604677903 58 | V2_01_easy: 0.10735199924969296 59 | V2_02_medium: 0.16664523361920217 60 | V2_03_difficult: 0.19504174366877391 61 | scale: 62 | MH_01_easy: 0.9927134620156396 63 | MH_02_easy: 0.9810458679656806 64 | MH_03_medium: 0.998029274494798 65 | MH_04_difficult: 0.9920449030297775 66 | MH_05_difficult: 0.9928695773268286 67 | V1_01_easy: 1.0026951383404439 68 | V1_02_medium: 1.0107298413927654 69 | V1_03_difficult: 0.9781572295343105 70 | V2_01_easy: 0.9720218391155905 71 | V2_02_medium: 0.9888353848354212 72 | V2_03_difficult: 0.9858589849207583 73 | velocity (m/s)/rmse: 74 | MH_01_easy: 0.035755731812501454 75 | MH_02_easy: 0.03441372962325925 76 | MH_03_medium: 0.07522252391388955 77 | MH_04_difficult: 0.0906762792011823 78 | MH_05_difficult: 0.07318917693516612 79 | V1_01_easy: 0.04847196341227278 80 | V1_02_medium: 0.0672761836181115 81 | V1_03_difficult: 0.08224278489042906 82 | V2_01_easy: 0.039575215434914275 83 | V2_02_medium: 0.065392077310591 84 | V2_03_difficult: 0.09388560858743868 -------------------------------------------------------------------------------- /configs/EQVIO_results_UZHFPV.yaml: -------------------------------------------------------------------------------- 1 | Early Finish flag: 2 | indoor_45_12_snapdragon_with_gt: false 3 | indoor_45_13_snapdragon_with_gt: false 4 | indoor_45_14_snapdragon_with_gt: false 5 | indoor_45_2_snapdragon_with_gt: false 6 | indoor_45_4_snapdragon_with_gt: false 7 | indoor_forward_10_snapdragon_with_gt: false 8 | indoor_forward_5_snapdragon_with_gt: false 9 | indoor_forward_6_snapdragon_with_gt: false 10 | indoor_forward_7_snapdragon_with_gt: false 11 | indoor_forward_9_snapdragon_with_gt: false 12 | NaN flag: 13 | indoor_45_12_snapdragon_with_gt: false 14 | indoor_45_13_snapdragon_with_gt: false 15 | indoor_45_14_snapdragon_with_gt: false 16 | indoor_45_2_snapdragon_with_gt: false 17 | indoor_45_4_snapdragon_with_gt: false 18 | indoor_forward_10_snapdragon_with_gt: false 19 | indoor_forward_5_snapdragon_with_gt: false 20 | indoor_forward_6_snapdragon_with_gt: false 21 | indoor_forward_7_snapdragon_with_gt: false 22 | indoor_forward_9_snapdragon_with_gt: false 23 | Trajectory length: 24 | indoor_45_12_snapdragon_with_gt: 110.66648335124499 25 | indoor_45_13_snapdragon_with_gt: 160.80586367285747 26 | indoor_45_14_snapdragon_with_gt: 215.75054657248796 27 | indoor_45_2_snapdragon_with_gt: 207.50865347649187 28 | indoor_45_4_snapdragon_with_gt: 164.7956481424701 29 | indoor_forward_10_snapdragon_with_gt: 129.66475213255444 30 | indoor_forward_5_snapdragon_with_gt: 52.816463619413106 31 | indoor_forward_6_snapdragon_with_gt: 208.33873697441672 32 | indoor_forward_7_snapdragon_with_gt: 314.45629158089594 33 | indoor_forward_9_snapdragon_with_gt: 123.47363034199418 34 | attitude (d)/rmse: 35 | indoor_45_12_snapdragon_with_gt: 0.7601312429661122 36 | indoor_45_13_snapdragon_with_gt: 2.0837812604451416 37 | indoor_45_14_snapdragon_with_gt: 1.43865736956725 38 | indoor_45_2_snapdragon_with_gt: 1.1780986394178516 39 | indoor_45_4_snapdragon_with_gt: 1.0921351182624517 40 | indoor_forward_10_snapdragon_with_gt: 1.1774945260559924 41 | indoor_forward_5_snapdragon_with_gt: 0.8617136493307842 42 | indoor_forward_6_snapdragon_with_gt: 1.8975488742169193 43 | indoor_forward_7_snapdragon_with_gt: 5.056901162447073 44 | indoor_forward_9_snapdragon_with_gt: 1.0673693477289414 45 | position (m)/rmse: 46 | indoor_45_12_snapdragon_with_gt: 0.26043674696194713 47 | indoor_45_13_snapdragon_with_gt: 0.28277875503947436 48 | indoor_45_14_snapdragon_with_gt: 0.2693560738772749 49 | indoor_45_2_snapdragon_with_gt: 0.310390329275013 50 | indoor_45_4_snapdragon_with_gt: 0.33019160528450936 51 | indoor_forward_10_snapdragon_with_gt: 0.1486559165293003 52 | indoor_forward_5_snapdragon_with_gt: 0.29492016107382074 53 | indoor_forward_6_snapdragon_with_gt: 0.22684750795762812 54 | indoor_forward_7_snapdragon_with_gt: 0.40450255719168765 55 | indoor_forward_9_snapdragon_with_gt: 0.16848383797122396 56 | scale: 57 | indoor_45_12_snapdragon_with_gt: 1.009918826995742 58 | indoor_45_13_snapdragon_with_gt: 1.0016408668046348 59 | indoor_45_14_snapdragon_with_gt: 1.00342718254213 60 | indoor_45_2_snapdragon_with_gt: 1.0194040400257687 61 | indoor_45_4_snapdragon_with_gt: 1.0085324036746115 62 | indoor_forward_10_snapdragon_with_gt: 1.0022815498812814 63 | indoor_forward_5_snapdragon_with_gt: 1.0194272432803142 64 | indoor_forward_6_snapdragon_with_gt: 0.9976690435759287 65 | indoor_forward_7_snapdragon_with_gt: 1.0010156159255423 66 | indoor_forward_9_snapdragon_with_gt: 0.9999800108982774 67 | velocity (m/s)/rmse: 68 | indoor_45_12_snapdragon_with_gt: 0.10819024224232712 69 | indoor_45_13_snapdragon_with_gt: 0.08060784439205018 70 | indoor_45_14_snapdragon_with_gt: 0.1320164605501463 71 | indoor_45_2_snapdragon_with_gt: 0.1575524213252414 72 | indoor_45_4_snapdragon_with_gt: 0.10704801174250202 73 | indoor_forward_10_snapdragon_with_gt: 0.12157834907261307 74 | indoor_forward_5_snapdragon_with_gt: 0.09570644246660577 75 | indoor_forward_6_snapdragon_with_gt: 0.15619279482207263 76 | indoor_forward_7_snapdragon_with_gt: 0.16267985083909958 77 | indoor_forward_9_snapdragon_with_gt: 0.15820046094497361 -------------------------------------------------------------------------------- /docs/intrin.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pvangoor/eqvio/3bcc01a9501885d98d88c272229cf12ea7c7d3f3/docs/intrin.png -------------------------------------------------------------------------------- /docs/main.md: -------------------------------------------------------------------------------- 1 | # EqVIO: An Equivariant Filter for Visual Inertial Odometry {#mainpage} 2 | 3 | This repository contains the implementation of EqVIO: An Equivariant Filter (EqF) for Visual Inertial Odometry (VIO). 4 | 5 | ## Dependencies 6 | 7 | - Eigen 3: `sudo apt install libeigen3-dev` 8 | - Yaml-cpp: `sudo apt install libyaml-cpp-dev` 9 | - GIFT: https://github.com/pvangoor/GIFT 10 | 11 | ### Optional Dependencis 12 | 13 | - FreeGLUT (for visualisations): `sudo apt install freeglut3-dev` 14 | - ROS (for reading ROS-Bags): http://wiki.ros.org/ROS/Installation 15 | - Doxygen (for documentation): `sudo apt install doxygen` 16 | 17 | ## Build Overview 18 | 19 | EqVIO is designed to be built as a cmake project rather than a ROS package. 20 | Assuming all prerequisites are installed and you are in the root folder of the repository, then you can follow these steps to build: 21 | 22 | ``` 23 | mkdir build 24 | cd build 25 | cmake .. 26 | cmake --build . -j8 27 | ``` 28 | 29 | Note: on older machines, it may be better to use `-j4` or even `-j2` instead of `-j8`. 30 | There are a number of flags that can be passed to cmake to influence the build process. 31 | The key flags related to eqvio are all prefixed `EQVIO_*`. 32 | For example, if you wish to build the documentation yourself, you could use `cmake .. -DEQVIO_BUILD_DOCS=1` instead of `cmake ..`. 33 | 34 | ### Common Issues 35 | 36 | Some of the geometry functions use c++20 concepts. 37 | If you cannot install a c++20 compatible compiler, then your other option is to disable concepts with `-DEQVIO_SUPPORT_CONCEPTS=0`. 38 | Note that this project still uses c++17 features, and there are no plans to create workarounds. 39 | 40 | ## Usage Overview 41 | 42 | The build process will create an executable called `eqvio_opt` in the build directory. 43 | To see the help, simply run `./build/eqvio_opt --help`. 44 | The required arguments are the dataset file/directory name, and the eqvio configuration file. 45 | There are also many optional arguments to adjust how the program runs. 46 | Different types of datasets are distinguished by providing a value to `--mode` (see the datasetReader page). 47 | An example command is 48 | 49 | ``` 50 | ./build/eqvio_opt ~/datasets/V1_01_easy/ configs/EQVIO_config_EuRoC_stationary.yaml --display --mode asl 51 | ``` 52 | 53 | This will create an output directory with a timestamp in the current directory, containing csv files with all the different states of eqvio. 54 | The `--display` flag is required to create live animations of the VIO state estimate. 55 | 56 | ### Running and Analysing Datasets 57 | 58 | There are a few python scripts provided to help with running and evaluating datasets. 59 | These are meant for evaluation of EqVIO over common robotics datasets, particularly over the EuRoC and UZHFPV datasets. 60 | They can also be used for some other datasets compatible with EqVIO. 61 | 62 | Suppose you want to run EqVIO over the EuRoC sequence V1_01_easy and analyse the results you could use: 63 | 64 | ``` 65 | python3 scripts/run_and_analyse_dataset.py --mode=asl configs/EQVIO_config_EuRoC_stationary.yaml ~/datasets/V1_01_easy/ 66 | ``` 67 | 68 | This will first run EqVIO over the dataset and then analyse the results. 69 | Note that the `--display` option is not used and the usual EqVIO commandline output is suppressed, so there is no output until the running and analysis are complete. 70 | The results are then saved to a new directory created in the dataset directory. In this case, `~/datasets/V1_01_easy/results/`. 71 | The results directory includes a range of useful and interesting figures and information: 72 | 73 | - **biases.pdf**: The estimated gyroscope and accelerometer biases over time. 74 | - **camera_offset.pdf**: The estimated rotation and position of the camera frame relative to the IMU frame over time. 75 | - **features.pdf**: The number of features at any given time and the mean lifetime of all the features in the state over time. 76 | - **gravity_and_velocity.pdf**: The acceleration due to gravity and the linear velocity of the IMU, both expressed in the IMU frame, over time. 77 | - **results.yaml**: The numeric results of the algorithm, including position and attitude error statistics, scale error, mean processing time, etc. 78 | - **timing_boxplots.pdf**: A boxplot of the time taken for each step in the algorithm. 79 | - **timing_flamegraph.pdf**: A flamegraph showing how much time was spent on each step in the algorithm over time. 80 | - **timing_histograms.pdf**: Histograms showing the distribution of time spent on each step of the algorithm. 81 | - **trajectory_error.pdf**: The attitude and position errors over time, including x,y,z components as well as the norm. 82 | - **trajectory.pdf**: The true and estimated trajectories over time in terms of x,y,z position and Euler angles (roll, pitch, yaw). 83 | - **trajectory_xy.pdf**: A top-down (with respect to the z-axis of the true trajectory) view of the true and estimated trajectories. 84 | 85 | If no groundtruth is available, then the plots are all still generated, except for the trajectory_error.pdf. 86 | The true trajectory is obviously not shown in this case, since it is not known. 87 | 88 | #### Running Multiple Datasets 89 | 90 | The python scripts can also handle multiple sequences from the same dataset. 91 | Simply put the names of all the sequences you want to test in the command: 92 | 93 | ``` 94 | python3 scripts/run_and_analyse_dataset.py --mode=asl configs/EQVIO_config_EuRoC_stationary.yaml ~/datasets/V1_01_easy/ ~/datasets/V1_02_medium/ 95 | ``` 96 | 97 | This will generate results for each dataset. 98 | These results can then be summarised into one file: 99 | 100 | ``` 101 | python3 scripts/summarise_results.py --mode=asl ~/datasets/V1_01_easy/ ~/datasets/V1_02_medium/ 102 | ``` 103 | 104 | This generates a new file which collects all the most important results in one place for each dataset, such as position and attitude RMSE, mean processing time, and scale error. 105 | 106 | The other way to run multiple datasets is to create a yaml file with information on the datasets you wish to analyse. 107 | An example is provided in `scripts/euroc_sequences.yaml`. 108 | As shown, this also allows you to add information such as the desired dataset mode and start time. 109 | To run over all the EuRoC datasets and analyse the results: 110 | 111 | ``` 112 | python3 scripts/run_and_analyse_dataset.py configs/EQVIO_config_EuRoC_stationary.yaml scripts/euroc_sequences.yaml 113 | python3 scripts/summarise_results.py scripts/euroc_sequences.yaml 114 | ``` 115 | -------------------------------------------------------------------------------- /docs/run.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pvangoor/eqvio/3bcc01a9501885d98d88c272229cf12ea7c7d3f3/docs/run.png -------------------------------------------------------------------------------- /include/eqvio/LoopTimer.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/common/aofstream.h" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | /** @brief A class for timing the processing speed of code sections in a loop. 28 | * 29 | * This class is intended to be instantiated as a global loop timer variable. It is set up with some initial timing 30 | * category labels. Each loop should be denoted with a call to startLoop. Then, the user can call startTiming and 31 | * endTiming with any of the category labels to start and stop a timer for a given section of code. At the end of each 32 | * loop, the total time spent on each category of code can be obtained from getLoopTimingData. 33 | */ 34 | class LoopTimer { 35 | public: 36 | /// Shorthand for the clock used 37 | using timer_clock = std::chrono::steady_clock; 38 | /// Shorthand for the duration type 39 | using timer_duration = std::chrono::duration; 40 | 41 | /// @brief a struct to contain a time stamp and map from labels to timer duration 42 | struct LoopTimingData { 43 | timer_duration loopTimeStart; ///< The time when the loop was started 44 | std::map timings; ///< The time taken for each section of code in the loop 45 | }; 46 | 47 | protected: 48 | /// The time at which the loop timer is created. 49 | const timer_clock::time_point timerOrigin = timer_clock::now(); 50 | 51 | /// The initial time for each label. 52 | std::map timerStartPoints; 53 | /// The latest loop timing data. 54 | LoopTimingData currentLoopTimingData; 55 | 56 | public: 57 | /** @brief start a timing loop. 58 | * 59 | * Reset all the current elapsed times to zero, and all the current timer start points to now. This should be called 60 | * at the start of every loop that is to be timed. 61 | */ 62 | void startLoop(); 63 | 64 | /** @brief start timing the code associated with the given label. 65 | * 66 | * @param label The label of the relevant category. 67 | * 68 | * Records the current time in the timerStartPoints. 69 | */ 70 | 71 | void startTiming(const std::string& label); 72 | /** @brief stop timing the code associated with the given label. 73 | * 74 | * @param label The label of the relevant category. 75 | * 76 | * Subtracts the timer start point of this label from the current time. 77 | */ 78 | void endTiming(const std::string& label); 79 | 80 | /** @brief set the labels that will be used later for recording data. 81 | * 82 | * @param headers A std::vector of labels for timing data. 83 | * 84 | * Once initialised, no additional labels should be added. 85 | */ 86 | void initialise(const std::vector& headers); 87 | 88 | /** @brief return the current loop timing data. 89 | * 90 | * Should be used at the end of each loop that is to be timed. 91 | */ 92 | const LoopTimingData& getLoopTimingData() const { return currentLoopTimingData; } 93 | }; 94 | 95 | inline LoopTimer loopTimer; -------------------------------------------------------------------------------- /include/eqvio/VIOSimulator.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/VIOFilterSettings.h" 21 | #include "eqvio/mathematical/VIOState.h" 22 | 23 | /** @brief A simulator for VIO measurements from a given groundtruth trajectory. 24 | * 25 | * This class is able to simulate IMU and feature measurements from a given groundtruth VIO trajectory. The simulation 26 | * world has a large number of landmark points, and these are tracked between requests for vision measurements, 27 | * similarly to a real-world feature tracking output. This code is mostly used for testing purposes, and can be used 28 | * alongside real data to provide just IMU or just feature measurements. 29 | */ 30 | class VIOSimulator { 31 | protected: 32 | uint randomSeed = 0; ///< The value used to seed the random number generation 33 | 34 | std::vector poses; ///< All the ground truth poses with time stamps. 35 | std::vector inertialPoints; ///< All the points generated for the simulation world. 36 | 37 | size_t maxFeatures = 30; ///< The maximum number of features tracked at any given time. 38 | bool initialNoise = false; 39 | bool inputNoise = false; 40 | bool outputNoise = false; 41 | VIOFilter::Settings filterSettings; 42 | 43 | /** @brief Generate a number of points for the simulator world. 44 | * 45 | * @param num The number of points to generate in the world. 46 | * @param distance The distance of the world's walls from the trajectory. 47 | * @return The generated world points. 48 | * 49 | * The world points are generated by determining simulation world walls, and then placing an even number of points 50 | * on each of the six walls. The walls are determined as the extremities of the provided trajectory plus the given 51 | * distance. 52 | */ 53 | std::vector 54 | generateWorldPoints(const int num = 1000, const double distance = 1.0, const int numWalls = 1) const; 55 | 56 | Eigen::Matrix3d getInertialStates(std::vector::const_iterator it, const double& ct) const; 57 | 58 | public: 59 | /** @brief Get a vision measurement at the given time. 60 | * 61 | * @param time The time of the vision measurement. 62 | * @return The tracked features at the given time. 63 | * 64 | * The currently tracked features are tracked to the given time, if possible. Then, if this is not the maximum 65 | * number of features, new features are added from the world points. 66 | */ 67 | VisionMeasurement getVision(const double& time) const; 68 | 69 | /** @brief Get an IMU measurement at the given time. 70 | * 71 | * @param time The time of the IMU measurement. 72 | * @param samplingFrequency The frequency of IMU sampling (relevant for noise addition). 73 | * @return The IMU velocity at the given time. 74 | * 75 | * The IMU measurement at the given time is obtained by numerical differentiation using four groundtruth poses. 76 | */ 77 | IMUVelocity getIMU(const double& time, const double& samplingFrequency = -1) const; 78 | 79 | /** @brief Get a const reference to the vector of ground truth poses. 80 | * 81 | * @return The ground truth stamped poses. 82 | */ 83 | const std::vector& viewPoses() const; 84 | 85 | /** @brief Get the full VIO state at the desired time. 86 | * 87 | * @param time The time at which the VIO state should be provided. 88 | * @param allowNoise Whether noise should be allowed to be added to the state. 89 | * @return The (potentially noisy) VIO state at the given time. 90 | */ 91 | VIOState getFullState(const double& time = -1, const bool& allowNoise = false) const; 92 | 93 | VIOSimulator() = default; 94 | 95 | /** @brief Create a simulator from a groundtruth file and some settings 96 | * 97 | * @param poses The sequence of poses to use in generating measurements 98 | * @param settings The simulator settings YAML node 99 | */ 100 | VIOSimulator( 101 | const std::vector& poses, const GIFT::GICameraPtr& camPtr, 102 | const YAML::Node& settings = YAML::Node(), const VIOFilter::Settings& filterSettings = VIOFilter::Settings()); 103 | 104 | GIFT::GICameraPtr cameraPtr; 105 | liepp::SE3d cameraOffset = liepp::SE3d::Identity(); ///< The chosen offset between IMU and camera. 106 | }; 107 | -------------------------------------------------------------------------------- /include/eqvio/VIOVisualiser.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "GIFT/Feature.h" 21 | #include "Plotter.h" 22 | #include "eqvio/mathematical/VIOState.h" 23 | 24 | /** @brief A class to handle visualisation of VIO state 25 | * 26 | * This includes drawing feature points onto the current video frame and also showing the current VIO state in a 3D 27 | * rendering. The 3D rendering is only available if the program is built with support enabled. 28 | */ 29 | class VIOVisualiser { 30 | protected: 31 | #if EQVIO_BUILD_VISUALISATION 32 | std::map pointLifetimeCounter; ///< The number of frames each point has been seen by id number. 33 | std::map persistentPoints; ///< The points that have existed for a sufficiently long time. 34 | std::vector estimatedTrajectory; ///< The history of all robot poses over time. 35 | std::vector groundTruthTrajectory; ///< The ground truth trajectory if provided. 36 | liepp::SE3d alignmentMatrix = liepp::SE3d::Identity(); ///< The matrix used to align estimated poses to the groundtruth. 37 | 38 | std::unique_ptr plotter; ///< A plotter instance to use when drawing the VIO state. 39 | #endif 40 | 41 | public: 42 | /** @brief Update the 3D render of the VIO system from the current state estimate. 43 | * 44 | * @param state The state to be added to the map display. 45 | */ 46 | void updateMapDisplay(const VIOState& state, const double& time); 47 | 48 | /** @brief Set the groundtruth trajectory to compare with the estimated VIO state. 49 | * 50 | * @param groundTruthTrajectory The groundtruth trajectory obtained from the dataset. 51 | */ 52 | void setGroundTruthTrajectory(const std::vector& groundTruthTrajectory); 53 | 54 | /** @brief Displays the features as points on an image. 55 | * 56 | * @param features The image feature points to be displayed. 57 | * @param baseImage The image on which features should be drawn. 58 | */ 59 | void displayFeatureImage(const std::vector& features, const cv::Mat& baseImage); 60 | 61 | /** @brief Construct the VIO visualiser with optional display 62 | * 63 | * @param displayFlag Create displays if true. 64 | * 65 | * @todo The display flag is not sensible. It was introduced to make display optional, but really, if the user does 66 | * not want a display then they should not instantiate a visualiser in the first place. 67 | */ 68 | VIOVisualiser(const bool& displayFlag = false) { 69 | if (displayFlag) { 70 | plotter = std::make_unique(); 71 | } 72 | } 73 | 74 | /** @brief Destroy the visualiser, but first join the plotter thread if one exists. 75 | */ 76 | ~VIOVisualiser(); 77 | }; -------------------------------------------------------------------------------- /include/eqvio/VIOWriter.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/LoopTimer.h" 21 | #include "eqvio/common/aofstream.h" 22 | #include "eqvio/mathematical/VIOState.h" 23 | #include "eqvio/mathematical/VIO_eqf.h" 24 | 25 | /** @brief A class to handle writing output for the VIO system 26 | * 27 | * The output is distributed over a number of files, which are organised in a given directory. The use of aofstream 28 | * rather than ofstream means that the VIO system usually does not have to slow down or wait for the OS to write to 29 | * files. 30 | */ 31 | class VIOWriter { 32 | protected: 33 | std::string outputDir; 34 | 35 | aofstream IMUStateFile; ///< The file for recording the IMU states 36 | aofstream cameraFile; ///< The file for recording the camera offset 37 | aofstream biasFile; ///< The file for recording the IMU biases 38 | aofstream pointsFile; ///< The file for recording the landmark points 39 | 40 | aofstream landmarkErrorFile; ///< The file for recording landmark depth errors 41 | aofstream trueStateFile; ///< The file for recording the true states (from simulation) 42 | aofstream neesFile; ///< The file for recording NEES 43 | aofstream poseConsistencyFile; ///< The file for recording pose consistency 44 | aofstream cameraConsistencyFile; ///< The file for recording camera consistency 45 | aofstream biasConsistencyFile; ///< The file for recording bias consistency 46 | 47 | aofstream featuresFile; ///< The file for recording the image features 48 | 49 | aofstream timingFile; ///< The file for recording processing times 50 | 51 | public: 52 | /** @brief Create a number of files in the output directory to record VIO data. 53 | * 54 | * @param outputDir The output directory where files should be stored. 55 | * 56 | * This method creates the output directory if it does not already exist, and then creates output files for the VIO 57 | * data to be written to. It also creates header rows in each of the files to make them human-readable. 58 | */ 59 | VIOWriter(const std::string& outputDir); 60 | 61 | /** @brief Write the VIO states to the relevant files. 62 | * 63 | * @param stamp The timestamp associated with the given state. 64 | * @param xi The current VIO system state estimate. 65 | * 66 | * The VIO states are divided into IMU states, IMU biases, camera offset, and landmark points. All four of these are 67 | * written to separate output files and time stamped. 68 | */ 69 | void writeStates(const double& stamp, const VIOState& xi); 70 | 71 | /** @brief Write the feature measurements to file. 72 | * 73 | * @param y The vision measurement to be written. 74 | */ 75 | void writeFeatures(const VisionMeasurement& y); 76 | 77 | /** @brief Write the system processing time to file. 78 | * 79 | * @param timingData The timing information for the latest loop of the VIO system. 80 | * 81 | * Note that, on the first call to this function, the timing data is also used to create a header for the timing 82 | * data file. 83 | */ 84 | void writeTiming(const LoopTimer::LoopTimingData& timingData); 85 | 86 | void writeLandmarkError(const double& stamp, const VIOState& trueState, const VIOState& estState); 87 | void writeConsistency(const double& stamp, const VIOState& trueState, const VIO_eqf& filter); 88 | }; -------------------------------------------------------------------------------- /include/eqvio/common/LieYaml.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "yaml-cpp/yaml.h" 21 | 22 | #include "liepp/SE3.h" 23 | 24 | namespace YAML { 25 | 26 | template <> struct convert { 27 | static Node encode(const liepp::SE3d& rhs) { 28 | const Eigen::Vector3d& position = rhs.x; 29 | const Eigen::Quaterniond& attitude = rhs.R.asQuaternion(); 30 | Node node; 31 | node.push_back("xw"); 32 | node.push_back(position.x()); 33 | node.push_back(position.y()); 34 | node.push_back(position.z()); 35 | node.push_back(attitude.w()); 36 | node.push_back(attitude.x()); 37 | node.push_back(attitude.y()); 38 | node.push_back(attitude.z()); 39 | return node; 40 | } 41 | 42 | static bool decode(const Node& node, liepp::SE3d& rhs) { 43 | if (!node.IsSequence() || node.size() != 8) { 44 | return false; 45 | } 46 | Eigen::Vector3d position; 47 | Eigen::Quaterniond attitude; 48 | position.x() = node[1].as(); 49 | position.y() = node[2].as(); 50 | position.z() = node[3].as(); 51 | attitude.w() = node[4].as(); 52 | attitude.x() = node[5].as(); 53 | attitude.y() = node[6].as(); 54 | attitude.z() = node[7].as(); 55 | 56 | rhs.x = position; 57 | rhs.R = attitude.normalized(); 58 | 59 | return true; 60 | } 61 | }; 62 | } // namespace YAML -------------------------------------------------------------------------------- /include/eqvio/common/aofstream.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | /** @brief asynchronous output file stream 28 | * 29 | * Uses a std thread to write data to an output stream asynchronously. When data is received through << by an aofstream, 30 | * it is buffered into a stringstream. A separate thread (started upon opening the output file) is used to flush the 31 | * buffer to the output file every few seconds. Upon destruction, the buffer is flushed completely and the thread is 32 | * destroyed. By using this class instead of a std::ofstream to write output, the program can mostly avoid waiting on 33 | * the OS. 34 | */ 35 | class aofstream { 36 | protected: 37 | /// The output file to which data is written 38 | std::ofstream outputFile; 39 | 40 | /// The buffer of data to write to the file 41 | std::stringstream streamBuffer; 42 | /// A flag to break the output thread loop when the destructor is called. 43 | bool destructorCalled = false; 44 | /// A condition variable that starts the output writing thread when new data is available. 45 | std::condition_variable buffer_cv; 46 | 47 | /// The thread that writes the streamBuffer to the outputFile. 48 | std::thread writerThread; 49 | /// A mutex to govern access to the stream buffer. 50 | std::mutex streamMutex; 51 | 52 | /** @brief Regularly flush the output buffer to the output file. 53 | * 54 | * Every few seconds, the stream buffer is flushed to the output file. This loop is paused until new data is 55 | * available and the condition variable is triggered. This function is intended to be run from the writerThread 56 | * only. 57 | */ 58 | void flushStreamToFile() { 59 | std::chrono::steady_clock::time_point lastFlush = std::chrono::steady_clock::now(); 60 | while (true) { 61 | { 62 | std::unique_lock lck(streamMutex); 63 | buffer_cv.wait(lck, [this]() { return !(streamBuffer.rdbuf()->in_avail() == 0) || destructorCalled; }); 64 | outputFile << streamBuffer.rdbuf(); 65 | } 66 | 67 | // Flush every 5 seconds 68 | if ((std::chrono::steady_clock::now() - lastFlush) > std::chrono::duration(5.0)) { 69 | lastFlush = std::chrono::steady_clock::now(); 70 | outputFile << std::flush; 71 | } 72 | 73 | if (this->destructorCalled) { 74 | break; 75 | } 76 | } 77 | if (!(streamBuffer.rdbuf()->in_avail() == 0)) { 78 | outputFile << streamBuffer.rdbuf(); 79 | } 80 | outputFile << std::flush; 81 | } 82 | 83 | public: 84 | aofstream() = default; 85 | /** @brief Create an aofstream and immediately open fname for writing. */ 86 | aofstream(const std::string& fname) { 87 | outputFile.open(fname); 88 | writerThread = std::thread(&aofstream::flushStreamToFile, this); 89 | } 90 | /** @brief Empty the stream buffer and finish the writing thread before destruction. */ 91 | ~aofstream() { 92 | { 93 | std::unique_lock lck(streamMutex); 94 | this->destructorCalled = true; 95 | } 96 | buffer_cv.notify_one(); 97 | if (writerThread.joinable()) { 98 | writerThread.join(); 99 | } 100 | outputFile.close(); 101 | } 102 | 103 | /** @brief Opens the given file for writing. 104 | * 105 | * @param fname The name of the file to open. 106 | */ 107 | void open(const std::string& fname) { 108 | outputFile.open(fname); 109 | writerThread = std::thread(&aofstream::flushStreamToFile, this); 110 | } 111 | 112 | /** @brief Checks if the associated output file is open. 113 | * 114 | * @return true if the file is open. 115 | */ 116 | bool is_open() { return outputFile.is_open(); } 117 | 118 | /** @brief Write data to the stream buffer to pass to the output file. 119 | * 120 | * @param data The data to be written to the output file. 121 | * @return this instance. 122 | * 123 | * Acquires the buffer mutex and writes data to the streambuffer. This data is flushed to the output file by the 124 | * writer thread. This function returns *this so that operator<< can be used in typical c++ fashion: 125 | * 126 | * myAofstream << data1 << data2; 127 | */ 128 | aofstream& operator<<(const auto& data) { 129 | std::unique_lock lck(streamMutex); 130 | streamBuffer << data; 131 | buffer_cv.notify_one(); 132 | return *this; 133 | } 134 | }; -------------------------------------------------------------------------------- /include/eqvio/common/safeConfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "yaml-cpp/yaml.h" 21 | #include 22 | 23 | template bool safeConfig(const YAML::Node& cfg, T& var) { 24 | if (cfg) { 25 | var = cfg.as(); 26 | return true; 27 | } else { 28 | return false; 29 | } 30 | } 31 | 32 | template bool safeConfig(const YAML::Node& cfg, const std::string& itm, T& var) { 33 | std::stringstream ss(itm); 34 | std::string tmp; 35 | YAML::Node subNode = YAML::Clone(cfg); 36 | while (getline(ss, tmp, ':')) { 37 | if (subNode[tmp]) { 38 | subNode = subNode[tmp]; 39 | } else { 40 | std::cout << itm << " was not found in the configuration." << std::endl; 41 | std::cout << "The following items were found in the configuration provided:\n"; 42 | for (const auto& sn : cfg) { 43 | std::cout << sn.first.as() << " "; 44 | } 45 | std::cout << std::endl; 46 | return false; 47 | } 48 | } 49 | var = subNode.as(); 50 | return true; 51 | } 52 | 53 | template T configOrDefault(const YAML::Node& cfg, const T& defaultValue) { 54 | if (cfg) { 55 | return cfg.as(); 56 | } else { 57 | return defaultValue; 58 | } 59 | } 60 | 61 | template T configOrDefault(const YAML::Node& cfg, const std::string& itm, const T& defaultValue) { 62 | T temp; 63 | if (safeConfig(cfg, itm, temp)) { 64 | return temp; 65 | } else { 66 | return defaultValue; 67 | } 68 | } 69 | -------------------------------------------------------------------------------- /include/eqvio/csv/CSVFile.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVReader.h" 21 | #include 22 | 23 | /** @brief A class for opening and reading CSV files 24 | * 25 | * @todo Consider a simpler interface for creating a view of a file as a csv file, similar to python csv. 26 | */ 27 | class CSVFile { 28 | protected: 29 | std::shared_ptr filePtr; ///< The file to be read. 30 | 31 | public: 32 | CSVReader reader; ///< The iterator used to read the file. 33 | 34 | CSVFile() : reader(){}; 35 | 36 | /** @brief Open the file at filename as a csv file with the chosen delimiter. 37 | * 38 | * @param fileName The name of the file to open. 39 | * @param delim The delimiter separating values in the file. 40 | */ 41 | CSVFile(const std::string& fileName, const char& delim = ',') { 42 | filePtr = std::make_shared(fileName); 43 | std::istream& file = *filePtr.get(); 44 | reader = CSVReader(file, delim); 45 | } 46 | 47 | /** @brief Return true if there are more lines to read in the file. 48 | */ 49 | operator bool() const { return reader != CSVReader() && filePtr->good(); } 50 | 51 | /** @brief Read the next line in the csv file. 52 | * 53 | * @return the line that was read. 54 | */ 55 | CSVLine nextLine() { 56 | CSVLine line = *reader; 57 | ++reader; 58 | return line; 59 | } 60 | }; 61 | -------------------------------------------------------------------------------- /include/eqvio/csv/CSVReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVLine.h" 21 | 22 | /// @todo decide if this is necessary or maybe rewrite as a viewer class 23 | 24 | class CSVReader { 25 | protected: 26 | std::istream* csvPtr; 27 | CSVLine csvLine; 28 | char delim; 29 | 30 | public: 31 | CSVReader() { csvPtr = NULL; } 32 | CSVReader(std::istream& f, const char& delim = ',') : delim(delim) { 33 | csvPtr = f.good() ? &f : NULL; 34 | readNextLine(); 35 | } 36 | 37 | CSVReader begin() { return *this; } 38 | CSVReader end() { return CSVReader(); } 39 | CSVLine operator*() { return csvLine; } 40 | bool operator==(const CSVReader& other) const { 41 | return (this->csvPtr == other.csvPtr) || ((this->csvPtr == NULL) && (other.csvPtr == NULL)); 42 | } 43 | bool operator!=(const CSVReader& other) const { return !(*this == other); } 44 | 45 | CSVReader& operator++() { 46 | if (csvPtr) { 47 | // Try to read the next line 48 | if (!readNextLine()) { 49 | csvPtr = NULL; 50 | } 51 | } 52 | return *this; 53 | } 54 | 55 | bool readNextLine() { 56 | std::string lineString; 57 | bool fileNotEmpty = (bool)std::getline(*csvPtr, lineString, '\n'); 58 | if (fileNotEmpty) { 59 | std::stringstream lineStream(lineString); 60 | csvLine.readLine(lineStream, delim); 61 | } else { 62 | csvLine = CSVLine(); 63 | } 64 | return fileNotEmpty; 65 | } 66 | }; 67 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/APDatasetReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVFile.h" 21 | #include "eqvio/dataserver/DatasetReaderBase.h" 22 | 23 | /** @brief The ardupilot dataset reader. 24 | * 25 | * This class can be used to read datasets in the ArduPilot (AP) format. 26 | * 27 | * @todo Add a clear description of this format. 28 | */ 29 | class APDatasetReader : public DatasetReaderBase { 30 | protected: 31 | std::string groundtruthFileName; ///< The file containing groundtruth data 32 | std::string cam_dir; ///< The directory where camera images are stored. 33 | CSVFile IMUCSVFile; ///< The CSV file containing IMU velocities 34 | CSVFile ImageCSVFile; ///< The CSV file containing image stamps and relative file names. 35 | 36 | public: 37 | virtual std::unique_ptr nextImage() override; 38 | virtual std::unique_ptr nextIMU() override; 39 | virtual std::vector groundtruth() override; 40 | 41 | APDatasetReader() = default; 42 | 43 | /** @brief Construct the AP dataset reader from the given dataset directory and simulation settings.*/ 44 | APDatasetReader(const std::string& datasetFileName); 45 | virtual void readCamera(const std::string& cameraFileName) override; 46 | }; 47 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/ASLDatasetReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVFile.h" 21 | #include "eqvio/dataserver/DatasetReaderBase.h" 22 | 23 | /** @brief The ASL dataset reader. 24 | * 25 | * This class can be used to read datasets in the EuRoC (ASL) format. For details, please see 26 | * EuRoC 27 | */ 28 | class ASLDatasetReader : public DatasetReaderBase { 29 | protected: 30 | std::string groundtruthFileName; ///< The file containing groundtruth data 31 | std::string cam_dir; ///< The directory where camera images are stored. 32 | CSVFile IMUCSVFile; ///< The CSV file containing IMU velocities 33 | CSVFile ImageCSVFile; ///< The CSV file containing image stamps and relative file names. 34 | 35 | public: 36 | virtual std::unique_ptr nextImage() override; 37 | virtual std::unique_ptr nextIMU() override; 38 | virtual void readCamera(const std::string& cameraFileName) override; 39 | virtual std::vector groundtruth() override; 40 | 41 | /** @brief Construct the ASL dataset reader from the given dataset directory and simulation settings. */ 42 | ASLDatasetReader(const std::string& datasetFileName); 43 | }; -------------------------------------------------------------------------------- /include/eqvio/dataserver/DataServerBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/VIOSimulator.h" 21 | #include "eqvio/dataserver/DatasetReaderBase.h" 22 | 23 | /** @brief The common interface of data servers. 24 | * 25 | * This abstraction helps to create a common interface between different data server implementations. A data server 26 | * provides the visual and inertial measurements in order, and can be queried to find out what the next measurement type 27 | * is. It is able to do this using data from any dataset reader. 28 | */ 29 | class DataServerBase { 30 | protected: 31 | std::unique_ptr datasetReaderPtr; ///< The dataset reader providing measurements. 32 | VIOSimulator simulator; ///< The simulator providing substitutions for measurements 33 | 34 | public: 35 | /** @brief Read a camera by using the dataset reader 36 | * 37 | * @param cameraFileName The name of the file with camera information. 38 | */ 39 | void readCamera(const std::string& cameraFileName); 40 | 41 | /** @brief Get a pointer to the dataset camera intrinsics*/ 42 | virtual GIFT::GICameraPtr camera() const; 43 | /** @brief Get a pointer to the dataset camera extrinsics*/ 44 | virtual std::shared_ptr cameraExtrinsics() const; 45 | /** @brief Get the full groundtruth trajectory if it is available. Otherwise the returned vector is empty. */ 46 | virtual std::vector getTrajectory() {return datasetReaderPtr->groundtruth();} 47 | 48 | virtual std::shared_ptr groundTruthPose(const double stamp = -1) const; 49 | 50 | /** @brief Get the type of the next measurement in the dataset.*/ 51 | virtual MeasurementType nextMeasurementType() const = 0; 52 | /** @brief Get the next image data. */ 53 | virtual StampedImage getImage() = 0; 54 | /** @brief Get the next IMU data. */ 55 | virtual IMUVelocity getIMU() = 0; 56 | /** @brief Get the timestamp of the next data, IMU or image.*/ 57 | virtual double nextTime() const = 0; 58 | 59 | /** @brief get a simulated vision measurement at the time of the next real image data.*/ 60 | virtual VisionMeasurement getSimVision(); 61 | /** @brief get a simulated IMU measurement at the time of the next real IMU data.*/ 62 | virtual IMUVelocity getSimIMU(); 63 | 64 | /** @brief Construct a data server with a new dataset reader 65 | * 66 | * @param datasetReader An rvalue reference to a dataset reader unique_ptr. 67 | * @param simSettings The settings to use for the simulator, in case simulated measurements are requested. 68 | */ 69 | DataServerBase(std::unique_ptr&& datasetReader, const YAML::Node& simSettings); 70 | DataServerBase() = default; 71 | }; 72 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/DatasetReaderBase.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/mathematical/IMUVelocity.h" 21 | #include "eqvio/mathematical/VIOState.h" 22 | #include "eqvio/mathematical/VisionMeasurement.h" 23 | 24 | #include "opencv4/opencv2/core.hpp" 25 | #include "opencv4/opencv2/highgui.hpp" 26 | #include "yaml-cpp/yaml.h" 27 | 28 | /** @brief The possible measurement types encountered in a VIO dataset */ 29 | enum class MeasurementType { Image, IMU, None }; 30 | 31 | /** @brief A struct carrying an openCV image and a timestamp */ 32 | struct StampedImage { 33 | cv::Mat image; ///< The image data 34 | double stamp; ///< The time stamp of the image 35 | }; 36 | 37 | /** @brief The common interface of all dataset readers. 38 | * 39 | * This abstraction helps to create a common interface between different dataset formats like EuRoC and UZH-FPV. It is 40 | * an abstract base class and cannot be instantiated itself, although pointers are allowed. 41 | */ 42 | class DatasetReaderBase { 43 | public: 44 | /** @brief get the next image data. 45 | * 46 | * @return a pointer to the next image data in the dataset. The pointer is null if no data remains 47 | */ 48 | virtual std::unique_ptr nextImage() = 0; 49 | 50 | /** @brief get the next IMU data. 51 | * 52 | * @return a pointer to the next IMU data in the dataset. The pointer is null if no data remains 53 | */ 54 | virtual std::unique_ptr nextIMU() = 0; 55 | 56 | /** @brief get a list of groundtruth poses 57 | * 58 | * @return a vector of stamped poses of the groundtruth. The vector is empty if no groundtruth is available. 59 | */ 60 | virtual std::vector groundtruth() = 0; 61 | 62 | /** @brief Read a camera file. 63 | * 64 | * Many datasets have default camera file locations, but some do not (e.g. ROSBags, UZH-FPV). This method lets the 65 | * user pass different camera parameters easily. 66 | */ 67 | virtual void readCamera(const std::string& cameraFileName) = 0; 68 | 69 | GIFT::GICameraPtr camera; ///< A shared pointer to the camera associated with the dataset. 70 | std::shared_ptr cameraExtrinsics; ///< A shared pointer to the camera pose w.r.t. the IMU. 71 | double cameraLag = 0.0; ///< The lag of the image data in the dataset w.r.t. the IMU data. 72 | }; 73 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/HiltiDatasetReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/RosbagDatasetReader.h" 21 | 22 | /** @brief The HILTI dataset reader. 23 | * 24 | * This class can be used to read datasets in the ROSBag format used by the HILTI SLAM challenge. For details, please 25 | * see HILTI Challenge 26 | * 27 | */ 28 | class HiltiDatasetReader : public RosbagDatasetReader { 29 | protected: 30 | HiltiDatasetReader() = default; 31 | 32 | public: 33 | /** @brief Construct a HILTI dataset reader from the given rosbag filename. 34 | * 35 | * The only real difference with the rosbag dataserver is that the IMU and image data topics are set to those found 36 | * in the HILTI dataset. 37 | */ 38 | HiltiDatasetReader(const std::string& rosbagFileName) 39 | : RosbagDatasetReader(rosbagFileName, "/alphasense/imu", "/alphasense/cam0/image_raw"){}; 40 | 41 | virtual void readCamera(const std::string& cameraFileName) override; 42 | }; -------------------------------------------------------------------------------- /include/eqvio/dataserver/RosbagDatasetReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/DatasetReaderBase.h" 21 | #include "rosbag/bag.h" 22 | #include "rosbag/view.h" 23 | 24 | /** @brief The ROSBag dataset reader. 25 | * 26 | * This class can be used to read datasets in the ROSBag format. 27 | */ 28 | class RosbagDatasetReader : public DatasetReaderBase { 29 | protected: 30 | rosbag::Bag bag; ///< The rosbag containing the visual and inertial data. 31 | rosbag::View imuBagView; ///< A rosbag view of IMU data 32 | rosbag::View::iterator imuViewIt; ///< The IMU data view iterator 33 | rosbag::View imageBagView; ///< A rosbag view of image data 34 | rosbag::View::iterator imageViewIt; ///< The image data view iterator 35 | 36 | RosbagDatasetReader() = default; 37 | 38 | public: 39 | /** @brief construct a rosbag dataset reader with the given IMU and Image topic. */ 40 | RosbagDatasetReader( 41 | const std::string& rosbagFileName, const std::string& imuTopic = "/imu0", 42 | const std::string& imageTopic = "/cam0/image_raw"); 43 | 44 | virtual std::unique_ptr nextImage() override; 45 | virtual std::unique_ptr nextIMU() override; 46 | virtual std::vector groundtruth() override{return {};}; 47 | virtual void readCamera(const std::string& cameraFileName) override; 48 | }; -------------------------------------------------------------------------------- /include/eqvio/dataserver/SimpleDataServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/DataServerBase.h" 21 | 22 | /** @brief The simplest implementation of a data server 23 | * 24 | * This data server reads the next image and IMU data, and then passes them on as requested. When the image data is 25 | * requested, the currently stored data is provided and the next data is read in anticipation of the next request. 26 | */ 27 | class SimpleDataServer : public DataServerBase { 28 | protected: 29 | std::unique_ptr nextImageData; ///< The next image measurement 30 | std::unique_ptr nextIMUData; ///< The next IMU measurement 31 | 32 | public: 33 | virtual MeasurementType nextMeasurementType() const override; 34 | virtual StampedImage getImage() override; 35 | virtual IMUVelocity getIMU() override; 36 | virtual double nextTime() const override; 37 | 38 | /** @brief construct the simple data server from a dataset reader. 39 | * 40 | * @param datasetReader An rvalue reference to a dataset reader unique_ptr. 41 | */ 42 | SimpleDataServer(std::unique_ptr&& datasetReader, const YAML::Node& simSettings = YAML::Node()); 43 | }; 44 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/SimulationDataServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/DataServerBase.h" 21 | 22 | /** @brief A data server that provides purely simulated data 23 | * 24 | * This data server creates a simulator instance using an artificial trajectory. It provides data at whatever 25 | * frequencies are chosen. 26 | */ 27 | class SimulationDataServer : public DataServerBase { 28 | protected: 29 | double imageFreq = 20.0; /// The frequency of image measurements in Hz 30 | double imuFreq = 200.0; /// The frequency of IMU measurements in Hz 31 | double maxSimulationTime = 100.0; /// The time at which the simulation should end in s. 32 | 33 | int imuMeasCount = 0; /// The number of IMU measurements provided. 34 | int imageMeasCount = 0; /// The number of image measurements provided. 35 | 36 | double nextImageTime() const; 37 | double nextIMUTime() const; 38 | 39 | public: 40 | virtual MeasurementType nextMeasurementType() const override; 41 | virtual StampedImage getImage() override; 42 | virtual IMUVelocity getIMU() override; 43 | virtual double nextTime() const override; 44 | 45 | virtual VisionMeasurement getSimVision() override; 46 | virtual IMUVelocity getSimIMU() override; 47 | 48 | /** @brief Get the exact initial condition of the VIO system. */ 49 | virtual VIOState getInitialCondition() const; 50 | 51 | /** @brief Get the true state of the VIO State at a time 52 | * 53 | * @param stamp The time stamp for the desired state. 54 | * @param withNoise True if noise should be added to the requested state. 55 | * @return The VIO state at the time stamp with noise if requested. 56 | */ 57 | virtual VIOState getTrueState(const double& stamp, const bool& withNoise = false) const; 58 | 59 | virtual std::shared_ptr cameraExtrinsics() const; 60 | 61 | std::vector generateTrajectory(const YAML::Node& choice) const; 62 | 63 | /** @brief construct the simulation data server without provided trajectory. 64 | * 65 | * @param simSettings The settings to provide to the simulator. 66 | */ 67 | SimulationDataServer( 68 | const YAML::Node& simSettings = YAML::Node(), 69 | const VIOFilter::Settings& filterSettings = VIOFilter::Settings()); 70 | }; 71 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/ThreadedDataServer.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/DataServerBase.h" 21 | 22 | #include "eqvio/csv/CSVReader.h" 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /** @brief A threaded implementation of a data server 31 | * 32 | * This data server uses a thread to continuously buffer IMU and image data. This can greatly increase program speed as 33 | * there is rarely a need to wait for new measurements to be read in the main thread. 34 | */ 35 | class ThreadedDataServer : public DataServerBase { 36 | protected: 37 | const size_t maxImageQueueSize = 200; ///< The maximum image data buffer size 38 | const size_t maxIMUQueueSize = 2000; ///< The maximum IMU data buffer size 39 | 40 | mutable std::condition_variable queuesReady_cv; ///< A CV that is triggered when image and IMU data is ready. 41 | mutable std::mutex ioMutex; ///< A mutex to govern access to the image and IMU buffers. 42 | std::thread ioThread; ///< The thread that reads image and IMU data. 43 | std::atomic_bool destructorCalled = false; ///< A flag to break the thread when the destructor is called. 44 | 45 | bool IMUDataFinished = false; ///< A flag that is false so long more IMU data remains to be read. 46 | bool imageDataFinished = false; ///< A flag that is false so long more image data remains to be read. 47 | 48 | std::queue IMUQueue; ///< The buffer of IMU data. 49 | std::queue imageQueue; ///< The buffer of image data. 50 | 51 | /** @brief Return true if the queues are at their maximum size or the data is finished.*/ 52 | bool queuesFilled() const; 53 | 54 | /** @brief Fill the image and IMU data buffers. 55 | * 56 | * This function is intended to be run by the ioThread. 57 | */ 58 | void fillQueues(); 59 | 60 | public: 61 | /** @brief Construct a threaded data server from a dataset reader 62 | * 63 | * @param datasetReader An rvalue reference to a dataset reader unique_ptr. 64 | */ 65 | ThreadedDataServer( 66 | std::unique_ptr&& datasetReader, const YAML::Node& simSettings = YAML::Node()); 67 | 68 | /** @brief Destroy the dataset reader, but ensure the io thread is stopped gracefully. */ 69 | ~ThreadedDataServer(); 70 | 71 | virtual MeasurementType nextMeasurementType() const override; 72 | virtual StampedImage getImage() override; 73 | virtual IMUVelocity getIMU() override; 74 | virtual double nextTime() const override; 75 | }; 76 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/UZHFPVDatasetReader.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVFile.h" 21 | #include "eqvio/dataserver/DatasetReaderBase.h" 22 | 23 | /** @brief The UZH-FPV dataset reader. 24 | * 25 | * This class can be used to read datasets in the UZH-FPV format. For details, please see 26 | * UZH-FPV 27 | */ 28 | class UZHFPVDatasetReader : public DatasetReaderBase { 29 | protected: 30 | std::string baseDir; ///< The base directory of the dataset. 31 | CSVFile IMUCSVFile; ///< The CSV file containing IMU velocities 32 | CSVFile ImageCSVFile; ///< The CSV file containing image stamps and relative file names. 33 | 34 | public: 35 | virtual std::unique_ptr nextImage() override; 36 | virtual std::unique_ptr nextIMU() override; 37 | virtual void readCamera(const std::string& cameraFileName) override; 38 | virtual std::vector groundtruth() override; 39 | 40 | /** @brief Construct the UZH FPV dataset reader from the given dataset directory and simulation settings. */ 41 | UZHFPVDatasetReader(const std::string& datasetFileName); 42 | }; 43 | -------------------------------------------------------------------------------- /include/eqvio/dataserver/dataservers.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/dataserver/APDatasetReader.h" 21 | #include "eqvio/dataserver/ASLDatasetReader.h" 22 | #include "eqvio/dataserver/DatasetReaderBase.h" 23 | #include "eqvio/dataserver/UZHFPVDatasetReader.h" 24 | 25 | #include "eqvio/dataserver/SimpleDataServer.h" 26 | #include "eqvio/dataserver/SimulationDataServer.h" 27 | #include "eqvio/dataserver/ThreadedDataServer.h" 28 | 29 | #if EQVIO_BUILD_ROSBAG 30 | #include "eqvio/dataserver/HiltiDatasetReader.h" 31 | #include "eqvio/dataserver/RosbagDatasetReader.h" 32 | #endif 33 | 34 | inline std::unique_ptr 35 | createDatasetReader(const std::string& dataMode, const std::string& datasetFileName, const double& cameraLag = 0.0) { 36 | std::unique_ptr datasetReader; 37 | 38 | if (dataMode == "asl") { 39 | datasetReader = std::make_unique(datasetFileName); 40 | } else if (dataMode == "anu") { 41 | datasetReader = std::make_unique(datasetFileName); 42 | } else if (dataMode == "uzhfpv") { 43 | datasetReader = std::make_unique(datasetFileName); 44 | } 45 | #if EQVIO_BUILD_ROSBAG 46 | else if (dataMode == "ros") { 47 | datasetReader = std::make_unique(datasetFileName); 48 | } else if (dataMode == "hilti") { 49 | datasetReader = std::make_unique(datasetFileName); 50 | } 51 | #endif 52 | 53 | datasetReader->cameraLag = cameraLag; 54 | return datasetReader; 55 | } -------------------------------------------------------------------------------- /include/eqvio/mathematical/EqFMatrices.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/mathematical/VIOGroup.h" 21 | #include 22 | 23 | /** @file */ 24 | 25 | /** @brief The local coordinate charts available to the EqF 26 | */ 27 | enum class CoordinateChoice { Euclidean, InvDepth, Normal }; 28 | 29 | /** @brief The suite of functions associated with a choice of coordinates for the EqF. 30 | * 31 | * Given the symmetry action of the VIO group, the matrices of the EqF are determined by the choice of local coordinates 32 | * about the fixed origin. This struct provides pointers to each of those functions, and can be instantiated based on a 33 | * particular choice of coordinates. 34 | */ 35 | struct EqFCoordinateSuite { 36 | /** @brief The coordinate chart for the VIO State space, \f$ \vartheta : \mathcal{T}^{\text{VI}}_n(3) \to 37 | * \mathbb{R}^m \f$. 38 | */ 39 | const CoordinateChart& stateChart; 40 | 41 | /// The EqF state matrix \f$ \mathring{A}_t \f$ 42 | const std::function stateMatrixA; 43 | /// The EqF input matrix \f$ B_t \f$ 44 | const std::function inputMatrixB; 45 | /// The EqF equivariant output matrix block \f$ C^\star_t \f$ 46 | const std::function( 47 | const Eigen::Vector3d& q0, const liepp::SOT3d& QHat, const GIFT::GICameraPtr& camPtr, const Eigen::Vector2d& y)> 48 | outputMatrixCiStar; 49 | 50 | /// The output matrix with equivariance \f$ C_t^\star \f$ or without \f$ C_t \f$. 51 | const Eigen::MatrixXd outputMatrixC( 52 | const VIOState& xi0, const VIOGroup& X, const VisionMeasurement& y, const bool useEquivariance = true) const; 53 | /// The discrete EqF state transition matrix 54 | Eigen::MatrixXd 55 | stateMatrixADiscrete(const VIOGroup& X, const VIOState& xi0, const IMUVelocity& imuVel, const double& dt) const; 56 | 57 | /// The standard (not equivariant) output matrix block \f$ C_i \f$. 58 | const Eigen::Matrix 59 | outputMatrixCi(const Eigen::Vector3d& q0, const liepp::SOT3d& QHat, const GIFT::GICameraPtr& camPtr) const; 60 | 61 | /// The continuous-time lift of the correction term \f$ \Delta \f$ from the tangent space at \f$ \mathring{\xi} \f$ 62 | /// to the Lie algebra \f$ \mathfrak{g} \f$. 63 | const std::function liftInnovation; 64 | /// The discrete-time lift of the correction term \f$ \Delta \f$ from the tangent space at \f$ \mathring{\xi} \f$ to 65 | /// the Lie algebra \f$ \mathfrak{g} \f$. 66 | const std::function liftInnovationDiscrete; 67 | }; 68 | 69 | /// A suite of functions for the EqF using Euclidean coordinates 70 | extern const EqFCoordinateSuite EqFCoordinateSuite_euclid; 71 | /// A suite of functions for the EqF using Inverse Depth coordinates 72 | extern const EqFCoordinateSuite EqFCoordinateSuite_invdepth; 73 | /// A suite of functions for the EqF using Normal coordinates 74 | extern const EqFCoordinateSuite EqFCoordinateSuite_normal; 75 | 76 | /** @brief Provide coordinates based on the coordinate choice 77 | * 78 | * @param coordinateChoice The chosen coordinates 79 | * @return A pointer to the coordinate implementation 80 | */ 81 | inline const EqFCoordinateSuite* getCoordinates(const CoordinateChoice& coordinateChoice) { 82 | if (coordinateChoice == CoordinateChoice::Euclidean) { 83 | return &EqFCoordinateSuite_euclid; 84 | } else if (coordinateChoice == CoordinateChoice::InvDepth) { 85 | return &EqFCoordinateSuite_invdepth; 86 | } else if (coordinateChoice == CoordinateChoice::Normal) { 87 | return &EqFCoordinateSuite_normal; 88 | } 89 | return nullptr; 90 | } -------------------------------------------------------------------------------- /include/eqvio/mathematical/Geometry.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | /** @file */ 21 | 22 | #include "eigen3/Eigen/Core" 23 | 24 | #include 25 | #include 26 | 27 | #if EIGEN_MAJOR_VERSION == 3 && EIGEN_MINOR_VERSION <= 9 28 | namespace Eigen { 29 | template using Vector = Matrix<_Scalar, _Rows, 1>; 30 | } 31 | #endif 32 | 33 | #if EQVIO_SUPPORT_CONCEPTS 34 | #include 35 | // Concept of manifold requires that the class has an int member called `CompDim'. 36 | template 37 | concept Manifold = requires(T t) { std::is_same::value; }; 38 | #endif 39 | 40 | /** @brief A Coordiante chart for a given manifold M. 41 | * 42 | * The manifold M must have a static int CompDim that indicates the dimension of the manifold at compile time (-1 for 43 | * variable dimension). The coordinate chart must be implemented with a forward and an inverse method. 44 | */ 45 | #if EQVIO_SUPPORT_CONCEPTS 46 | template struct CoordinateChart { 47 | #else 48 | template struct CoordinateChart { 49 | #endif 50 | /// Function pointer to the implementation of the forward chart 51 | const std::function(const M&, const M&)> chart; 52 | /// Function pointer to the implementation of the inverse chart 53 | const std::function&, const M&)> chartInv; 54 | 55 | /** @brief Apply the forward coordinate chart 56 | * 57 | * @param xi The manifold point requiring coordinates. 58 | * @param xi0 The manifold point that serves as the chart origin. 59 | * @return the local coordinate of xi about xi0. 60 | * 61 | * A coordinate chart maps a point xi near xi0 to an Eigen::Vector. xi0 is the origin of the chart in the sense that 62 | * chart(xi0, xi0) = 0. 63 | */ 64 | Eigen::Vector operator()(const M& xi, const M& xi0) const { return chart(xi, xi0); }; 65 | 66 | /** @brief Apply the inverse coordinate chart 67 | * 68 | * @param x The Vector to be mapped to the manifold. 69 | * @param xi0 The manifold point that serves as the chart origin. 70 | * @return the manifold point xi such that chart(xi, xi0) = x. 71 | * 72 | * The inverse coordinate chart maps a real vector x to a point xi near xi0 on the manifold. xi0 is the origin of 73 | * the chart in the sense that chart^{-1}(0, xi0) = xi0. 74 | */ 75 | M inv(const Eigen::Vector& x, const M& xi0) const { return chartInv(x, xi0); }; 76 | }; 77 | 78 | /** @brief A Coordinate chart for an embedded manifold. 79 | * 80 | * Given a manifold embedded in R^E, this struct is used to implement coordinate charts R^E -> R^m, where m is the 81 | * dimension of the manifold itself. 82 | */ 83 | template struct EmbeddedCoordinateChart { 84 | using EManifold = Eigen::Matrix; ///< The embedded manifold type 85 | /// Function pointer to the implementation of the forward chart 86 | const std::function(const EManifold&, const EManifold&)> chart; 87 | /// Function pointer to the implementation of the inverse chart 88 | const std::function&, const EManifold&)> chartInv; 89 | /// Function pointer to the implementation of the forward chart differential at 0 90 | const std::function(const EManifold&)> chartDiff0; 91 | /// Function pointer to the implementation of the inverse chart differential at 0 92 | const std::function(const EManifold&)> chartInvDiff0; 93 | 94 | /** @brief Apply the forward coordinate chart 95 | * 96 | * @param xi The embedded manifold point requiring coordinates. 97 | * @param xi0 The embedded manifold point that serves as the chart origin. 98 | * @return the local coordinate of xi about xi0. 99 | * 100 | * A coordinate chart maps a point xi near xi0 to an Eigen::Vector. xi0 is the origin of the chart in the sense that 101 | * chart(xi0, xi0) = 0. 102 | */ 103 | Eigen::Vector operator()(const EManifold& xi, const EManifold& xi0) const { return chart(xi, xi0); }; 104 | 105 | /** @brief Apply the inverse coordinate chart 106 | * 107 | * @param x The Vector to be mapped to the manifold. 108 | * @param xi0 The embedded manifold point that serves as the chart origin. 109 | * @return the embedded manifold point xi such that chart(xi, xi0) = x. 110 | * 111 | * The inverse coordinate chart maps a real vector x to a point xi near xi0 on the manifold. xi0 is the origin of 112 | * the chart in the sense that chart^{-1}(0, xi0) = xi0. 113 | */ 114 | EManifold inv(const Eigen::Vector& x, const EManifold& xi0) const { return chartInv(x, xi0); }; 115 | 116 | /** @brief Obtain the differential of the forward chart at a given origin. 117 | * 118 | * @param xi0 The origin at which the differential is to be computed. 119 | * @return the differential of the forward chart about xi0. 120 | */ 121 | Eigen::Matrix diff0(const EManifold& xi0) const { return chartDiff0(xi0); }; 122 | /** @brief Obtain the differential of the inverse chart at a given origin. 123 | * 124 | * @param xi0 The origin at which the differential is to be computed. 125 | * @return the differential of the inverse chart about xi0. 126 | */ 127 | Eigen::Matrix invDiff0(const EManifold& xi0) const { return chartInvDiff0(xi0); }; 128 | }; 129 | 130 | /** @brief Compute the numerical differential of a multivariate function. 131 | * 132 | * @param f The function R^n -> R^m to be differentiated. 133 | * @param x The point in R^n where the derivative is to be taken. 134 | * @param h The step size used to calculate the differential. Optional. 135 | * @return The differential Df(x) as an m by n matrix. 136 | * 137 | * This function computes the numerical differential of a function f at a point x by using the central difference 138 | * approximation. By default, the step size is computed as the cube root of std::numeric_limits::epsilon(). The 139 | * optimal step size will depend on the exact function being differentiated, but this is a good starting point most of 140 | * the time. 141 | */ 142 | Eigen::MatrixXd numericalDifferential( 143 | std::function f, const Eigen::VectorXd& x, double h = -1.0); 144 | 145 | /** @brief Draw a sample from a multivariate Gaussian distribution 146 | * 147 | * @param covariance The covariance of the Gaussian distribution. 148 | * @return A sample drawn from a zero-mean Gaussian distribution with the given covariance. 149 | */ 150 | Eigen::VectorXd sampleGaussianDistribution(const Eigen::MatrixXd& covariance); 151 | 152 | /** @brief Draw a number of samples from a multivariate Gaussian distribution 153 | * 154 | * @param covariance The covariance of the Gaussian distribution. 155 | * @param numSamples The number of samples to draw. 156 | * @return numSamples samples drawn from a zero-mean Gaussian distribution with the given covariance. 157 | */ 158 | std::vector sampleGaussianDistribution(const Eigen::MatrixXd& covariance, const size_t& numSamples); -------------------------------------------------------------------------------- /include/eqvio/mathematical/IMUVelocity.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eigen3/Eigen/Eigen" 21 | #include "eqvio/csv/CSVReader.h" 22 | 23 | /** @file */ 24 | 25 | /// The approximate value of acceleration due to gravity. 26 | constexpr double GRAVITY_CONSTANT = 9.80665; 27 | 28 | /** @brief An Inertial Measurement Unit (IMU) reading. 29 | * 30 | * IMUVelocity objects contain a gyroscope and accelerometer measurement, along with a timestamp. Additionally, they can 31 | * also carry bias velocities, although these are typically left as zero. 32 | */ 33 | struct IMUVelocity { 34 | /// The timestamp of the measurements 35 | double stamp; 36 | 37 | Eigen::Vector3d gyr; ///< The angular velocity measured by the gyroscope 38 | Eigen::Vector3d acc; ///< The linear acceleration measured by the accelerometer 39 | Eigen::Vector3d gyrBiasVel = Eigen::Vector3d::Zero(); ///< The velocity of the gyroscope bias. Usually zero. 40 | Eigen::Vector3d accBiasVel = Eigen::Vector3d::Zero(); ///< The velocity of the accelerometer bias. Usually zero. 41 | 42 | /// @brief An IMUVelocity with zero acc and gyr. 43 | static IMUVelocity Zero(); 44 | 45 | IMUVelocity() = default; 46 | /// @brief Construct the IMU velocity from a vector containing (gyr, acc) 47 | IMUVelocity(const Eigen::Matrix& vec); 48 | /// @brief Construct the IMU velocity from a vector containing (gyr, acc, gBiasVel, aBiasVel) 49 | IMUVelocity(const Eigen::Matrix& vec); 50 | 51 | /** @brief add an IMU velocity to this one. 52 | * 53 | * @param other The other velocity to add to this one. 54 | * @return the sum of IMU velocities. 55 | * 56 | * @note The stamp is taken from whichever summand has a positive stamp. The left summand is preferred. 57 | */ 58 | IMUVelocity operator+(const IMUVelocity& other) const; 59 | /// @brief add anything by converting it to an IMU velocity 60 | IMUVelocity operator+(const auto& other) const { return this->operator+(IMUVelocity(other)); } 61 | 62 | /** @brief subtract a vector from the velocity components 63 | * 64 | * @param vec The vector to subtract, assumed to be of the form (gyr, acc, gBiasVel, aBiasVel) 65 | * @return the resulting IMU velocity. 66 | */ 67 | IMUVelocity operator-(const Eigen::Matrix& vec) const; 68 | /** @brief subtract a vector from the velocity components 69 | * 70 | * @param vec The vector to subtract, assumed to be of the form (gyr, acc) 71 | * @return the resulting IMU velocity. The gBiasVel and aBiasVel are left unchanged. 72 | */ 73 | IMUVelocity operator-(const Eigen::Matrix& vec) const; 74 | 75 | /** @brief scale the IMU velocity by a constant. 76 | * 77 | * @param c The parameter by which to scale. 78 | * @return An IMU velocity with each velocity component multiplied by c. 79 | */ 80 | IMUVelocity operator*(const double& c) const; 81 | 82 | /// The dimension of the IMU velocity vector space. 83 | constexpr static int CompDim = 12; 84 | }; 85 | 86 | /** @brief Write an IMU velocity to a CSV line 87 | * 88 | * @param line A reference to the CSV line where the IMU velocity should be written. 89 | * @param imu The IMU data to write to the CSV line. 90 | * @return A reference to the CSV line with the IMU velocity written. 91 | * 92 | * The data is formatted in the CSV line as stamp, gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z 93 | */ 94 | CSVLine& operator<<(CSVLine& line, const IMUVelocity& imu); 95 | 96 | /** @brief Read an IMU velocity from a CSV line 97 | * 98 | * @param line A reference to the CSV line from which the IMU velocity should be read. 99 | * @param imu A reference to an IMU velocity where data is to be stored. 100 | * @return A reference to the CSV line with the IMU velocity removed during reading. 101 | * 102 | * The data is formatted in the CSV line as stamp, gyr_x, gyr_y, gyr_z, acc_x, acc_y, acc_z 103 | */ 104 | CSVLine& operator>>(CSVLine& line, IMUVelocity& imu); -------------------------------------------------------------------------------- /include/eqvio/mathematical/VIOGroup.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "eqvio/csv/CSVReader.h" 21 | #include "eqvio/mathematical/VIOState.h" 22 | #include "liepp/SE3.h" 23 | #include "liepp/SOT3.h" 24 | 25 | /** @file */ 26 | 27 | /** @brief The Lie group used for VIO. 28 | * 29 | * This is the Lie group used for VIO in the EqF. Note that the dimension is variable since there may be an arbitrary 30 | * number of SOT(3) elements. 31 | */ 32 | struct VIOGroup { 33 | Eigen::Matrix beta; ///< The symmetry component related to IMU biases 34 | liepp::SE3d A; ///< The symmetry component related to robot pose 35 | Eigen::Vector3d w; ///< The symmetry component related to robot velocity 36 | liepp::SE3d B; ///< The symmetry component related to camera offset 37 | std::vector Q; ///< The symmetry components related to landmark positions 38 | std::vector id; ///< The id numbers associated with the SOT(3) components 39 | 40 | /** @brief The group multiplication operator 41 | * 42 | * @param other The right factor in the group product. 43 | * @return The result of multiplication 44 | * 45 | * The order of multiplication is very important here. It is given by: 46 | * result = (*this) * (other) 47 | */ 48 | [[nodiscard]] VIOGroup operator*(const VIOGroup& other) const; 49 | 50 | /** @brief Create the group identity from the ids 51 | * 52 | * @param id The id numbers required in the group identity. 53 | * @return The group identity with the provided id numbers 54 | */ 55 | [[nodiscard]] static VIOGroup Identity(const std::vector& id = {}); 56 | 57 | /** @brief The group inverse operator 58 | * 59 | * @return The inverse of the group element used to call this method. 60 | */ 61 | [[nodiscard]] VIOGroup inverse() const; 62 | 63 | /** @brief Check if any components of the group are NaN. 64 | * 65 | * @return true if any components have NaN values. 66 | * 67 | * This is primarily useful for debugging. 68 | */ 69 | [[nodiscard]] bool hasNaN() const; 70 | }; 71 | 72 | /** @brief Write a group element to a CSV line 73 | * 74 | * @param line A reference to the CSV line where the data should be written. 75 | * @param X The group element to write to the CSV line. 76 | * @return A reference to the CSV line with the data written. 77 | */ 78 | CSVLine& operator<<(CSVLine& line, const VIOGroup& X); 79 | 80 | /** @brief The Lie algebra of the VIOGroup 81 | * 82 | * This is the Lie algebra associated with the Lie group used for VIO in the EqF. Note that the dimension is variable 83 | * since there may be an arbitrary number of sot(3) elements. 84 | */ 85 | struct VIOAlgebra { 86 | Eigen::Matrix u_beta; ///< The algebra component related to IMU biases 87 | liepp::se3d U_A; ///< The algebra component related to robot pose 88 | liepp::se3d U_B; ///< The algebra component related to robot velocity 89 | Eigen::Vector3d u_w; ///< The algebra component related to camera offset 90 | std::vector W; ///< The algebra components related to landmark positions 91 | std::vector id; ///< The id numbers associated with the sot(3) components 92 | 93 | /** @brief Scale an algebra element by a constant 94 | * 95 | * @param c The scaling factor 96 | * @return The result of scaling 97 | */ 98 | [[nodiscard]] VIOAlgebra operator*(const double& c) const; 99 | 100 | /** @brief Get the negative version of an algebra element. 101 | * 102 | * @return The negative version of *this with the same id numbers 103 | */ 104 | [[nodiscard]] VIOAlgebra operator-() const; 105 | 106 | /** @brief Add another Lie algebra element 107 | * 108 | * @param other the Lie algebra element to add. 109 | * @return The sum of this with other as vectors. 110 | */ 111 | [[nodiscard]] VIOAlgebra operator+(const VIOAlgebra& other) const; 112 | 113 | /** @brief Subtract another Lie algebra element 114 | * 115 | * @param other the Lie algebra element to subtract. 116 | * @return The difference of this with other as vectors. 117 | */ 118 | [[nodiscard]] VIOAlgebra operator-(const VIOAlgebra& other) const; 119 | }; 120 | 121 | /** @brief Scale an algebra element by a constant 122 | * 123 | * @param c The scaling factor 124 | * @param lambda The Lie algebra element to be scaled. 125 | * @return The result of scaling 126 | */ 127 | [[nodiscard]] VIOAlgebra operator*(const double& c, const VIOAlgebra& lambda); 128 | 129 | /** @brief The group action of the VIO group on the sensor states. 130 | * 131 | * @param X The VIO group element 132 | * @param sensor The sensor states 133 | * @return The result of applying the right group action of X on sensor. 134 | */ 135 | [[nodiscard]] VIOSensorState sensorStateGroupAction(const VIOGroup& X, const VIOSensorState& sensor); 136 | 137 | /** @brief The group action of the VIO group on the a VIO system state. 138 | * 139 | * @param X The VIO group element 140 | * @param state The VIO system state 141 | * @return The transformed VIO system state. 142 | */ 143 | [[nodiscard]] VIOState stateGroupAction(const VIOGroup& X, const VIOState& state); 144 | 145 | /** @brief The group action of the VIO group on the a visual output measurements. 146 | * 147 | * @param X The VIO group element. 148 | * @param measurement The feature measurements. 149 | * @return The transformed measurements. 150 | * 151 | * Note that this action differs from the one presented in the paper as it takes into account an arbitrary camera model. 152 | * Importantly, this group action is compatible with the VIO state group action, such that the measurement function is 153 | * equivariant. 154 | */ 155 | [[nodiscard]] VisionMeasurement outputGroupAction(const VIOGroup& X, const VisionMeasurement& measurement); 156 | 157 | /** @brief Lift the VIO system velocity from the state space to the VIO Lie algebra 158 | * 159 | * @param state The VIO state at which to lift. 160 | * @param velocity The IMU velocity to be lifted. 161 | * @return The lifted velocity on the VIO Lie algebra. 162 | */ 163 | [[nodiscard]] VIOAlgebra liftVelocity(const VIOState& state, const IMUVelocity& velocity); 164 | 165 | /** @brief A discrete version of the VIO system lift. 166 | * 167 | * @param state The VIO state at which to lift. 168 | * @param velocity The IMU velocity to be lifted. 169 | * @param dt The time period over which to compute the discrete lift. 170 | * @return The lifted velocity on the VIO Lie algebra. 171 | */ 172 | [[nodiscard]] VIOGroup liftVelocityDiscrete(const VIOState& state, const IMUVelocity& velocity, const double& dt); 173 | 174 | /** @brief The Lie theoretic exponential map from the VIO Lie algebra to the VIO group. 175 | * 176 | * @param lambda The algebra element to which the exponential should be applied. 177 | * @return The resulting group element. 178 | */ 179 | [[nodiscard]] VIOGroup VIOExp(const VIOAlgebra& lambda); -------------------------------------------------------------------------------- /include/eqvio/mathematical/VIO_eqf.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | #include "eqvio/csv/CSVReader.h" 24 | #include "eqvio/mathematical/EqFMatrices.h" 25 | #include "eqvio/mathematical/IMUVelocity.h" 26 | #include "eqvio/mathematical/VIOGroup.h" 27 | #include "eqvio/mathematical/VIOState.h" 28 | #include "eqvio/mathematical/VisionMeasurement.h" 29 | 30 | /** @brief The implementation of the EqF for VIO. 31 | * 32 | * This struct implements the basic equivariant filter for visual inertial odometry. 33 | */ 34 | struct VIO_eqf { 35 | 36 | EqFCoordinateSuite const* coordinateSuite = &EqFCoordinateSuite_euclid; ///< The suite of EqF matrix functions. 37 | VIOState xi0; ///< The fixed origin configuration. 38 | VIOGroup X = VIOGroup::Identity(); ///< The EqF's observer state 39 | Eigen::MatrixXd Sigma = 40 | Eigen::MatrixXd::Identity(VIOSensorState::CompDim, VIOSensorState::CompDim); ///< The EqF's Riccati matrix 41 | 42 | double currentTime = -1; ///< The time of the filter states. 43 | 44 | /** @brief Add new landmarks to the EqF state from the provided vector. 45 | * 46 | * @param newLandmarks The landmarks to be added to the state. 47 | * @param newLandmarkCov The covariance of the new landmarks to be added. 48 | * 49 | * This method adds new landmarks to the state exactly as they are provided, and augments the Riccati matrix as 50 | * appropriate. 51 | */ 52 | void addNewLandmarks(std::vector& newLandmarks, const Eigen::MatrixXd& newLandmarkCov); 53 | 54 | /** @brief Remove a landmark from the EqF states based on its index in the EqF state vector. 55 | * 56 | * @param idx The index of the landmark to remove. 57 | */ 58 | void removeLandmarkByIndex(const int& idx); 59 | 60 | /** @brief Remove a landmark from the EqF states based on its id number. 61 | * 62 | * @param id The id number of the landmark to remove. 63 | */ 64 | void removeLandmarkById(const int& id); 65 | 66 | /** @brief Remove all landmarks with depth values that are too small or too large. 67 | */ 68 | void removeInvalidLandmarks(); 69 | 70 | /** @brief Get the marginalised covariance associated with a landmark by its id number 71 | * 72 | * @param id The id number of the landmark with the desired covariance. 73 | */ 74 | Eigen::Matrix3d getLandmarkCovById(const int& id) const; 75 | 76 | /** @brief Get the covariance associated with the measurement of a particular value. 77 | * 78 | * @param id The id number to which the measurement covariance is associated. 79 | * @param y The real measurement of pixel coordinates associated with this id number. 80 | * @param camPtr The camera model for the camera used to make the measurement y. 81 | * 82 | * The availability of the true measurement y allows us to use an equivariant output approximation here. 83 | */ 84 | Eigen::Matrix2d getOutputCovById(const int& id, const Eigen::Vector2d& y, const GIFT::GICameraPtr& camPtr) const; 85 | 86 | void integrateObserverState(const IMUVelocity& imuVelocity, const double& dt, const bool& discreteLift = true); 87 | void integrateRiccatiStateFast( 88 | const IMUVelocity& imuVelocity, const double& dt, 89 | const Eigen::Matrix& inputGainMatrix, 90 | const Eigen::MatrixXd& stateGainMatrix); 91 | void integrateRiccatiStateAccurate( 92 | const IMUVelocity& imuVelocity, const double& dt, 93 | const Eigen::Matrix& inputGainMatrix, 94 | const Eigen::MatrixXd& stateGainMatrix); 95 | void integrateRiccatiStateDiscrete( 96 | const IMUVelocity& imuVelocity, const double& dt, 97 | const Eigen::Matrix& inputGainMatrix, 98 | const Eigen::MatrixXd& stateGainMatrix); 99 | 100 | /** @brief Process a sequence of IMU measurements 101 | * 102 | * @param measurement The measurements to use for the update. 103 | * @param outputGainMatrix The covariance associated with uncertainty in the measurements. 104 | * @param useEquivariantOutput Set true to use the equivariant output approximation (recommended true). 105 | * @param discreteCorrection Set true to use a discrete-time correction (recommended false). 106 | */ 107 | void performVisionUpdate( 108 | const VisionMeasurement& measurement, const Eigen::MatrixXd& outputGainMatrix, 109 | const bool& useEquivariantOutput = true, const bool& discreteCorrection = false); 110 | 111 | /** @brief Provide the current state estimate. 112 | * 113 | * This is computed by applying the group action with the observer state X to the fixed origin configuration xi0. 114 | */ 115 | VIOState stateEstimate() const; 116 | 117 | /** @brief Provide a prediction of the state at the given time stamp. 118 | * 119 | * This is calculated by computing the current state estimate and then integrating the dynamics. 120 | */ 121 | VIOState predictState(const double& stamp, const std::vector imuVelocities) const; 122 | 123 | double computeNEES(const VIOState& trueState) const; 124 | 125 | /** @brief Write an the filter states to a file. 126 | * 127 | * @param line A reference to the CSV line where the data should be written. 128 | * @param filter The filter states to write to the CSV line. 129 | * @return A reference to the CSV line with the data written. 130 | * 131 | * The data is formatted in the CSV line as [xi0, X, Sigma] 132 | */ 133 | friend CSVLine& operator<<(CSVLine& line, const VIO_eqf& filter); 134 | }; 135 | 136 | CSVLine& operator<<(CSVLine& line, const VIO_eqf& filter); -------------------------------------------------------------------------------- /include/eqvio/mathematical/VisionMeasurement.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "Geometry.h" 21 | #include "eqvio/csv/CSVReader.h" 22 | 23 | #include "GIFT/camera/camera.h" 24 | #include "eigen3/Eigen/Eigen" 25 | 26 | #include 27 | #include 28 | 29 | /** @brief A measurement of features from an image. 30 | * 31 | * This is the VIO system measurement, along with a time stamp and a pointer to a camera model used to generate the 32 | * measurement. The camera model is important, as it provides all the necessary information to replicate the measurement 33 | * function in the VIO system. 34 | */ 35 | struct VisionMeasurement { 36 | double stamp; ///< The time stamp of the image features. 37 | std::map camCoordinates; ///< The pixel coordinates of the features, by id number. 38 | GIFT::GICameraPtr cameraPtr; ///< A pointer to the camera model associated with the image features. 39 | 40 | constexpr static int CompDim = Eigen::Dynamic; ///< The dimension of the space of feature measurements. 41 | 42 | /** @brief get the id numbers associated with this measurement. 43 | * 44 | * @return All the id numbers present in the camCoordinates map. 45 | */ 46 | std::vector getIds() const; 47 | 48 | /** @brief cast the measurement to an Eigen Vector 49 | * 50 | * @return The cam coordinates as a vector 51 | * 52 | * The order in which feature coordinates are listed in the vector depends on their ordering in the camCoordinates 53 | * map. In principle, this means they will be in ascending order of id numbers. 54 | */ 55 | operator Eigen::VectorXd() const; 56 | 57 | /** @brief cast the feature coordinates to OpenCV Point2f 58 | * 59 | * @return The map of camera coordinates as Point2f 60 | */ 61 | std::map ocvCoordinates() const; 62 | }; 63 | 64 | /** @brief Compute the difference between two vision measurements 65 | * 66 | * @param y1 The left-hand term in the subtraction 67 | * @param y2 The right-hand term in the subtraction 68 | * @return A vision measurement with the difference between each feature in both y1 and y2 69 | * 70 | * It is expected that y1 and y2 share the same camera pointer. This method is primarily useful for subtracting the 71 | * expected measurement from the true measurement in the EqF framework. 72 | */ 73 | VisionMeasurement operator-(const VisionMeasurement& y1, const VisionMeasurement& y2); 74 | 75 | /** @brief Add a vector to the vision measurement 76 | * 77 | * @param y The original measurement 78 | * @param eta The noise sample to add 79 | * @return A vision measurement with all the pixel coordinates shifted by the noise sample 80 | */ 81 | VisionMeasurement operator+(const VisionMeasurement& y, const Eigen::VectorXd& eta); 82 | 83 | /** @brief Write a vision measurement to a CSV line 84 | * 85 | * @param line A reference to the CSV line where the data should be written. 86 | * @param vision The vision measurement data to write to the CSV line. 87 | * @return A reference to the CSV line with the data written. 88 | */ 89 | CSVLine& operator<<(CSVLine& line, const VisionMeasurement& vision); 90 | 91 | /** @brief Read an IMU velocity from a CSV line 92 | * 93 | * @param line A reference to the CSV line from which the data should be read. 94 | * @param vision A reference to a vision measurement where data is to be stored. 95 | * @return A reference to the CSV line with the data removed during reading. 96 | */ 97 | CSVLine& operator>>(CSVLine& line, VisionMeasurement& vision); 98 | -------------------------------------------------------------------------------- /intrinsics.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | camera_matrix: !!opencv-matrix 4 | rows: 3 5 | cols: 3 6 | dt: d 7 | data: [ 458.654, 0., 367.215, 0., 457.296, 248.375, 0., 0., 1. ] 8 | distortion: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0] 9 | 10 | 11 | # Sensor extrinsics wrt. the body-frame. 12 | T_BS: 13 | cols: 4 14 | rows: 4 15 | data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 16 | 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 17 | -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 18 | 0.0, 0.0, 0.0, 1.0] 19 | 20 | # Camera specific definitions. 21 | rate_hz: 20 22 | resolution: [752, 480] 23 | camera_model: pinhole 24 | intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv 25 | distortion_model: radial-tangential 26 | distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 27 | 28 | # inertial sensor noise model parameters (static) 29 | gyroscope_noise_density: 1.6968e-04 # [ rad / s / sqrt(Hz) ] ( gyro "white noise" ) 30 | gyroscope_random_walk: 1.9393e-05 # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion ) 31 | accelerometer_noise_density: 2.0000e-3 # [ m / s^2 / sqrt(Hz) ] ( accel "white noise" ) 32 | accelerometer_random_walk: 3.0000e-3 # [ m / s^3 / sqrt(Hz) ]. ( accel bias diffusion ) 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /libs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | # The order of modules is important! 3 | if (EQVIO_BUILD_VISUALISATION) 4 | set(module visualisation) 5 | 6 | set(EQVIO_MODULE_INCLUDE_DIRS "") 7 | set(EQVIO_MODULE_LIBS "") 8 | 9 | add_subdirectory(${module}) 10 | list(APPEND EQVIO_MODULE_INCLUDE_DIRS "${CMAKE_SOURCE_DIR}/libs/${module}/include") 11 | list(APPEND EQVIO_MODULE_LIBS "eqviolib_${module}") 12 | 13 | set(EQVIO_MODULE_INCLUDE_DIRS ${EQVIO_MODULE_INCLUDE_DIRS} PARENT_SCOPE) 14 | set(EQVIO_MODULE_LIBS ${EQVIO_MODULE_LIBS} PARENT_SCOPE) 15 | endif() -------------------------------------------------------------------------------- /libs/visualisation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(GLUT REQUIRED) 2 | find_package(Threads REQUIRED) 3 | set(OpenGL_GL_PREFERENCE LEGACY) 4 | find_package(OpenGL REQUIRED) 5 | 6 | set(eqviolib_visualisation_SOURCE_FILES 7 | src/Plotter.cpp 8 | ) 9 | 10 | set(eqviolib_visualisation_HEADER_FILES 11 | include/Plotter.h 12 | include/PlotDataTypes.h 13 | ) 14 | 15 | add_library(eqviolib_visualisation 16 | ${eqviolib_visualisation_HEADER_FILES} 17 | ${eqviolib_visualisation_SOURCE_FILES} 18 | ) 19 | 20 | target_include_directories(eqviolib_visualisation 21 | PRIVATE include 22 | PRIVATE ${EIGEN_INCLUDE_DIRS} 23 | PRIVATE ${GLUT_INCLUDE_DIRS} 24 | PRIVATE ${OPENGL_INCLUDE_DIR} 25 | ) 26 | 27 | target_link_libraries(eqviolib_visualisation 28 | ${GLUT_LIBRARY} 29 | ${OPENGL_LIBRARIES} 30 | ${CMAKE_THREAD_LIBS_INIT} 31 | ) 32 | -------------------------------------------------------------------------------- /libs/visualisation/include/PlotDataTypes.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "GL/gl.h" 21 | #include "eigen3/Eigen/Core" 22 | #include 23 | 24 | class PlotData3 { 25 | private: 26 | std::vector coords; 27 | GLenum drawType = GL_POINTS; 28 | Eigen::Vector4d color = Eigen::Vector4d(0, 0, 1, 1); 29 | std::vector colors; 30 | bool useMultipleColors = false; 31 | int size = 2; 32 | 33 | public: 34 | PlotData3( 35 | const std::vector& dataPoints, const GLenum drawType = GL_POINTS, 36 | const Eigen::Vector4d& color = Eigen::Vector4d(0, 0, 1, 1), const int size = 2) { 37 | this->coords = dataPoints; 38 | this->drawType = drawType; 39 | this->color = color; 40 | this->useMultipleColors = false; 41 | this->size = size; 42 | } 43 | 44 | PlotData3( 45 | const std::vector& dataPoints, const GLenum drawType = GL_POINTS, 46 | const std::vector& colors = {}, const int size = 2) { 47 | assert(colors.size() == dataPoints.size()); 48 | this->coords = dataPoints; 49 | this->drawType = drawType; 50 | this->colors = colors; 51 | this->useMultipleColors = true; 52 | this->size = size; 53 | } 54 | 55 | void draw() const { 56 | glPointSize(size); 57 | glLineWidth(size); 58 | glBegin(drawType); 59 | if (!useMultipleColors) { 60 | glColor4f(color[0], color[1], color[2], color[3]); 61 | for (const Eigen::Vector3d& coord : coords) { 62 | glVertex3f(coord[0], coord[1], coord[2]); 63 | } 64 | } else { 65 | for (size_t i = 0; i < coords.size(); ++i) { 66 | glColor4f(colors[i][0], colors[i][1], colors[i][2], colors[i][3]); 67 | glVertex3f(coords[i][0], coords[i][1], coords[i][2]); 68 | } 69 | } 70 | 71 | glEnd(); 72 | } 73 | }; 74 | -------------------------------------------------------------------------------- /libs/visualisation/include/Plotter.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "GL/glut.h" 21 | #include 22 | #include 23 | 24 | #include "eigen3/Eigen/Eigen" 25 | #include 26 | 27 | #include "PlotDataTypes.h" 28 | 29 | enum class PlotMotion { NONE, ROTATE, TRANSLATE, SCALE }; 30 | 31 | class Plotter { 32 | protected: 33 | // Technical 34 | static Plotter* currentInstance; 35 | std::thread plottingThread; 36 | std::mutex plottingMutex; 37 | 38 | // Plot view interaction 39 | float ratio = 1; 40 | float angleAzimuth = 0; 41 | float angleIncline = 0; 42 | float zoom = 10.0; 43 | 44 | int mouseOriginX = -1; 45 | int mouseOriginY = -1; 46 | 47 | float deltaAngle = 0; 48 | 49 | PlotMotion plotMotionType = PlotMotion::NONE; 50 | 51 | Eigen::Vector3d plotOrigin = Eigen::Vector3d::Zero(); 52 | bool allowTranslation = true; 53 | 54 | // Data 55 | std::vector plotsData; 56 | std::vector savedData; 57 | 58 | public: 59 | Plotter(); 60 | ~Plotter() { plottingThread.join(); }; 61 | 62 | void drawPoints( 63 | const std::vector& newPoints, const Eigen::Vector4d& color = Eigen::Vector4d(0, 0, 1, 1), 64 | const int& size = 1); 65 | void drawPoints( 66 | const std::vector& newPoints, const std::vector& color, const int& size = 1); 67 | void drawLine( 68 | const std::vector& newLine, const Eigen::Vector4d& color = Eigen::Vector4d(0, 0, 1, 1), 69 | const int& size = 1); 70 | void drawAxes(const Eigen::Matrix4d& pose, const double& length = 1.0, const int& size = 1); 71 | void updatePlotData(const PlotData3& plotData); 72 | 73 | bool hold = false; 74 | bool pause = false; 75 | 76 | void lockOrigin(const Eigen::Vector3d& origin = Eigen::Vector3d::Zero()) { 77 | allowTranslation = false; 78 | plotOrigin = origin; 79 | }; 80 | void unlockOrigin() { allowTranslation = true; }; 81 | 82 | protected: 83 | static void startThread(); 84 | 85 | // Plotting functionality wrappers 86 | static void wrap_renderPoints() { currentInstance->renderPoints(); }; 87 | static void wrap_changeWindowSize(int w, int h) { currentInstance->changeWindowSize(w, h); }; 88 | static void wrap_mouseButton(int button, int state, int x, int y) { 89 | currentInstance->mouseButton(button, state, x, y); 90 | }; 91 | static void wrap_processNormalKeys(unsigned char key, int x, int y) { 92 | currentInstance->processNormalKeys(key, x, y); 93 | }; 94 | static void wrap_mouseMove(int x, int y) { currentInstance->mouseMove(x, y); }; 95 | 96 | // Plotting functionality functions 97 | void changeWindowSize(int w, int h); 98 | void renderPoints(); 99 | void mouseButton(int button, int state, int x, int y); 100 | void mouseMove(int x, int y); 101 | virtual void processNormalKeys(unsigned char key, int x, int y); 102 | 103 | // Utility 104 | template T clamp(const T& value, const T& minValue, const T& maxValue) { 105 | T result = value; 106 | result = (result < minValue) ? minValue : result; 107 | result = (result > maxValue) ? maxValue : result; 108 | return result; 109 | } 110 | }; -------------------------------------------------------------------------------- /libs/visualisation/src/main.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "Plotter.h" 19 | 20 | #include "eigen3/Eigen/Core" 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | using namespace Eigen; 29 | 30 | void startGlutWindow(); 31 | void renderPoints(); 32 | 33 | vector pts; 34 | thread plottingThread; 35 | 36 | int main(int argc, char** argv) { 37 | cout << "Hello GLUT!" << endl; 38 | 39 | // plot some random points 40 | int n = 50; 41 | for (int i = 0; i < n; ++i) { 42 | pts.emplace_back(3 * Vector3d::Random() + Vector3d(1, 1, 1)); 43 | } 44 | 45 | vector line(5 * n); 46 | for (int i = 0; i < 5 * n; ++i) { 47 | line[i] = Vector3d(cos(0.1 * i), 0.02 * sqrt(i), sin(0.1 * i)); 48 | } 49 | 50 | Plotter p; 51 | p.hold = true; 52 | p.drawPoints(pts, Vector4d(0, 0, 1, 0), 3); 53 | p.drawAxes(Matrix4d::Identity(), 1, 4); 54 | p.drawLine(line, Vector4d(0, 0, 0, 1), 2); 55 | p.hold = false; 56 | 57 | p.maintain(); 58 | 59 | // startGlutWindow(); 60 | // plottingThread.join(); 61 | 62 | return 0; 63 | } 64 | 65 | void startGlutWindow() { 66 | // Initialise glut 67 | char* my_argv[1]; 68 | int my_argc = 1; 69 | my_argv[0] = strdup("Plotter"); 70 | 71 | glutInit(&my_argc, my_argv); 72 | glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGBA); 73 | glutInitWindowPosition(100, 100); 74 | glutInitWindowSize(1280, 720); 75 | glutCreateWindow("Plotter window"); 76 | 77 | // register callbacks 78 | glutDisplayFunc(renderPoints); 79 | glutIdleFunc(renderPoints); 80 | 81 | // Enter glut main loop 82 | plottingThread = thread(glutMainLoop); 83 | } 84 | 85 | void renderPoints() { 86 | glClearColor(0.1f, 0.5f, 0.5f, 1.0f); 87 | glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 88 | glutSwapBuffers(); 89 | } -------------------------------------------------------------------------------- /scripts/analyse_timing_data.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import csv 3 | import matplotlib.pyplot as plt 4 | # plt.rc('text', usetex=True) 5 | plt.rc('font', family='serif') 6 | plt.rc('lines', linewidth=1.0) 7 | import math 8 | import numpy as np 9 | 10 | flamegraph_presets = { 11 | 'eqvio': [ 12 | "features", 13 | "preprocessing", 14 | "propagation", 15 | "correction", 16 | ], 17 | 'openvins': [ 18 | "tracking", 19 | "propagation", 20 | "msckf update", 21 | "slam update", 22 | "slam delayed", 23 | "re-tri & marg", 24 | ] 25 | } 26 | 27 | 28 | def collect_timing_info(fname, ignore_entries=0): 29 | with open(fname, 'r') as f: 30 | reader = csv.reader(f) 31 | labels = [label.strip() for label in next(reader)] 32 | 33 | for _ in range(ignore_entries): 34 | next(reader) # skip header and first few entries 35 | 36 | timing_dict = {label: [] for label in labels} 37 | for row in reader: 38 | for (label, entry) in zip(labels, row): 39 | timing_dict[label].append(float(entry)*1.0e3) 40 | 41 | return labels, timing_dict 42 | 43 | 44 | def plot_timing_histograms(labels, timing_dict): 45 | cols = math.ceil(len(labels)**0.5) 46 | rows = math.ceil(len(labels)/cols) 47 | fig, axs = plt.subplots(rows, cols) 48 | 49 | for (i, label) in enumerate(labels): 50 | ax = axs.ravel()[i] 51 | ax.hist(timing_dict[label], 100) 52 | # ax.boxplot(timing_dict[label]) 53 | ax.set_xlabel("Timing (ms)") 54 | ax.set_ylabel("Count") 55 | ax.set_title(label.capitalize()) 56 | 57 | fig.tight_layout() 58 | 59 | return {"timing_histograms": fig} 60 | 61 | 62 | def plot_flame_graph(labels, timing_dict, keys): 63 | flame_len = None 64 | for k in keys: 65 | assert(k in labels) 66 | 67 | flame_len = min(len(timing_dict[k]) for k in keys) 68 | 69 | # Set up colours 70 | my_cmap = plt.get_cmap("rainbow") 71 | 72 | fig, ax = plt.subplots() 73 | cumulative_flames = np.zeros(flame_len) 74 | frames = np.arange(flame_len) 75 | for (i, k) in enumerate(keys): 76 | timing_array = np.array(timing_dict[k]) 77 | new_cum_flames = cumulative_flames + timing_array 78 | ax.fill_between(frames, cumulative_flames, new_cum_flames, 79 | label=k.capitalize(), color=my_cmap(i/(len(keys)-1)), linewidth=0.0) 80 | cumulative_flames = new_cum_flames 81 | 82 | mean_time = np.mean(cumulative_flames) 83 | ax.axhline(mean_time, c='k', ls=":") 84 | 85 | # Reverse the legend 86 | handles, labels = ax.get_legend_handles_labels() 87 | ax.legend(reversed(handles), reversed(labels)) 88 | 89 | ax.set_xlabel("Frame number") 90 | ax.set_ylabel("Time taken (ms)") 91 | ax.set_title("Processing Time per Frame") 92 | ax.set_xlim(0, flame_len-1) 93 | ax.set_ylim(0, None) 94 | 95 | return {"mean time (ms)": float(mean_time)}, {"timing_flamegraph": fig} 96 | 97 | 98 | def plot_timing_boxplots(labels, timing_dict): 99 | fig, axs = plt.subplots(1, 1) 100 | axs.boxplot(list(timing_dict[lab] 101 | for lab in labels), tick_labels=labels, sym="") 102 | # axs.set_xlabel(timing_dict.keys()) 103 | 104 | return {"timing_boxplots": fig} 105 | 106 | 107 | def plot_timing_data(fname, flamegraph_type='eqvio'): 108 | labels, timing_dict = collect_timing_info(fname, 10) 109 | 110 | if flamegraph_type in flamegraph_presets: 111 | flame_graph_keys = flamegraph_presets[flamegraph_type] 112 | else: 113 | flame_graph_keys = labels 114 | 115 | figures_dict = {} 116 | figures_dict.update(plot_timing_histograms(flame_graph_keys, timing_dict)) 117 | figures_dict.update(plot_timing_boxplots(flame_graph_keys, timing_dict)) 118 | time_results, fg_fig = plot_flame_graph( 119 | labels, timing_dict, flame_graph_keys) 120 | figures_dict.update(fg_fig) 121 | 122 | return time_results, figures_dict 123 | 124 | 125 | if __name__ == '__main__': 126 | parser = argparse.ArgumentParser("Analyse timing information.") 127 | parser.add_argument("file", metavar='f', type=str, 128 | help="The file containing timing data") 129 | parser.add_argument("--type", type=str, help="The type of timing file. 'eqvio' (default) or 'openvins'", default='eqvio') 130 | args = parser.parse_args() 131 | 132 | if args.type is None: 133 | plot_timing_data(args.file) 134 | else: 135 | plot_timing_data(args.file, args.type) 136 | 137 | plt.show() 138 | -------------------------------------------------------------------------------- /scripts/euroc_sequences.yaml: -------------------------------------------------------------------------------- 1 | # YAML file containing information about EuRoC sequences. 2 | # Note that the absolute file path is given in each case. 3 | MH_01_easy: 4 | fname: "/home/pieter/datasets/MH_01_easy/" 5 | mode: "asl" 6 | start: 40.0 # After calibration motion 7 | MH_02_easy: 8 | fname: "/home/pieter/datasets/MH_02_easy/" 9 | mode: "asl" 10 | start: 35.0 # After calibration motion 11 | MH_03_medium: 12 | fname: "/home/pieter/datasets/MH_03_medium/" 13 | mode: "asl" 14 | start: 10.0 # After calibration motion 15 | MH_04_difficult: 16 | fname: "/home/pieter/datasets/MH_04_difficult/" 17 | mode: "asl" 18 | start: 17.0 # After calibration motion 19 | MH_05_difficult: 20 | fname: "/home/pieter/datasets/MH_05_difficult/" 21 | mode: "asl" 22 | start: 18.0 # After calibration motion 23 | V1_01_easy: 24 | fname: "/home/pieter/datasets/V1_01_easy/" 25 | mode: "asl" 26 | V1_02_medium: 27 | fname: "/home/pieter/datasets/V1_02_medium/" 28 | mode: "asl" 29 | V1_03_difficult: 30 | fname: "/home/pieter/datasets/V1_03_difficult/" 31 | mode: "asl" 32 | V2_01_easy: 33 | fname: "/home/pieter/datasets/V2_01_easy/" 34 | mode: "asl" 35 | V2_02_medium: 36 | fname: "/home/pieter/datasets/V2_02_medium/" 37 | mode: "asl" 38 | V2_03_difficult: 39 | fname: "/home/pieter/datasets/V2_03_difficult/" 40 | mode: "asl" -------------------------------------------------------------------------------- /scripts/run_and_analyse_dataset.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import os 4 | from analysis_tools import analyse_dataset 5 | from DatasetInfo import add_dataset_parsing, read_dataset_list 6 | import subprocess 7 | import argparse 8 | import time 9 | 10 | 11 | def run_over_dataset(ds_info: 'DatasetInfo', config_fname: str, verbose: bool = False, simvis=False): 12 | 13 | eqvio_command = ds_info.eqvio_command(config_fname, simvis) 14 | 15 | t0 = time.time() 16 | try: 17 | sts = subprocess.run(eqvio_command, shell=True, capture_output=True, text=True) 18 | except KeyboardInterrupt as e: 19 | print("COMMAND USED:") 20 | print(eqvio_command) 21 | raise e 22 | t1 = time.time() 23 | if verbose: 24 | print(sts.stdout) 25 | print("EQVIO Took {} seconds.".format(t1-t0)) 26 | ds_info.timing = t1 - t0 27 | 28 | 29 | if __name__ == '__main__': 30 | parser = argparse.ArgumentParser( 31 | description="Run and analyse EqF VIO in the given directory.") 32 | parser.add_argument("-d", "--display", action='store_true', 33 | help="Set this flag to display the output plots.") 34 | parser.add_argument("-v", "--verbose", action='store_true', 35 | help="Set this flag to give verbose output.") 36 | parser.add_argument("--simvis", action='store_true', 37 | help="Simulate vision measurements") 38 | parser.add_argument("config", type=str, help="The config file.") 39 | parser = add_dataset_parsing(parser) 40 | args = parser.parse_args() 41 | 42 | datasets_info = read_dataset_list(args.directory, args.mode) 43 | 44 | assert os.path.isfile(args.config), "Configuration file not found at {}".format(args.config) 45 | 46 | for ds_info in datasets_info: 47 | print("Running EqVIO and analysing: {}".format(ds_info.short_name())) 48 | run_over_dataset(ds_info, args.config, args.verbose, args.simvis) 49 | analyse_dataset(ds_info, save=True) 50 | -------------------------------------------------------------------------------- /scripts/summarise_results.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import yaml 3 | import os 4 | import datetime 5 | from DatasetInfo import * 6 | 7 | 8 | def nested_get(d: dict, l: list): 9 | if d is None: 10 | return 1.0e6 11 | # Given a list l of keys, retrieve an item from d recursively 12 | dn = d 13 | for k in l: 14 | dn = dn[k] 15 | return dn 16 | 17 | 18 | def collect_results(datasets_info: list): 19 | # Summarise all the EqVIO results from a given directory. 20 | all_results = {} 21 | for ds_info in datasets_info: 22 | dataset_name = ds_info.short_name() 23 | if os.path.exists(ds_info.results_yaml()): 24 | with open(ds_info.results_yaml(), 'r') as f: 25 | results_dict = yaml.safe_load(f) 26 | all_results[dataset_name] = results_dict 27 | else: 28 | print("No results available for {}".format(dataset_name)) 29 | 30 | return all_results 31 | 32 | 33 | def summarise_results(all_results, nested_key_list): 34 | sum_results = {} 35 | for nested_key in nested_key_list: 36 | nk_name = nested_key[0] 37 | if len(nested_key) > 1: 38 | for k in nested_key[1:]: 39 | nk_name = nk_name + "/" + k 40 | sum_results[nk_name] = {} 41 | 42 | for results_key in all_results: 43 | r = nested_get(all_results[results_key], nested_key) 44 | sum_results[nk_name][results_key] = r 45 | 46 | return sum_results 47 | 48 | 49 | def add_all_results_by_key(all_results, nested_key): 50 | total = 0.0 51 | for results_key in all_results: 52 | r = nested_get(all_results[results_key], nested_key) 53 | total += float(r) 54 | 55 | return total 56 | 57 | 58 | if __name__ == '__main__': 59 | parser = argparse.ArgumentParser( 60 | "Summarise the EqVIO results from a directory containing several results directories.") 61 | parser = add_dataset_parsing(parser) 62 | args = parser.parse_args() 63 | 64 | datasets_info = read_dataset_list(args.directory, args.mode) 65 | 66 | all_results = collect_results(datasets_info) 67 | 68 | summary_keys = [ 69 | ["position (m)", "rmse"], 70 | ["attitude (d)", "rmse"], 71 | ["velocity (m/s)", "rmse"], 72 | ["scale"], 73 | ["mean time (ms)"], 74 | ["Early Finish flag"], 75 | ["NaN flag"], 76 | ["Trajectory length"], 77 | ] 78 | 79 | sum_results = summarise_results(all_results, summary_keys) 80 | total_position_rmse = add_all_results_by_key( 81 | all_results, ["position (m)", "rmse"]) 82 | 83 | print("The mean of position RMSE (cm) is {:.2f}".format( 84 | 100.0*total_position_rmse / len(datasets_info))) 85 | 86 | head_dir_name = get_common_directory(datasets_info) 87 | summary_fname = head_dir_name + \ 88 | "results_summary_{}.yaml".format( 89 | datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S")) 90 | with open(summary_fname, 'w') as f: 91 | yaml.safe_dump(sum_results, f) 92 | print("Summary saved to "+summary_fname) 93 | -------------------------------------------------------------------------------- /src/LoopTimer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/LoopTimer.h" 19 | 20 | void LoopTimer::startTiming(const std::string& label) { timerStartPoints.at(label) = timer_clock::now(); } 21 | 22 | void LoopTimer::endTiming(const std::string& label) { 23 | timer_clock::time_point now = timer_clock::now(); 24 | currentLoopTimingData.timings[label] = now - timerStartPoints.at(label); 25 | } 26 | 27 | void LoopTimer::startLoop() { 28 | 29 | // Reset all timing 30 | for (const auto& [label, timing] : timerStartPoints) { 31 | currentLoopTimingData.timings[label] = timer_duration(0); 32 | } 33 | 34 | // Reset the loop timing data 35 | currentLoopTimingData.loopTimeStart = timer_clock::now() - timerOrigin; 36 | } 37 | 38 | void LoopTimer::initialise(const std::vector& headers) { 39 | const timer_clock::time_point now = timer_clock::now(); 40 | for (const std::string& header : headers) { 41 | timerStartPoints[header] = now; 42 | } 43 | } 44 | -------------------------------------------------------------------------------- /src/dataserver/APDatasetReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/APDatasetReader.h" 19 | 20 | #include 21 | 22 | APDatasetReader::APDatasetReader(const std::string& datasetFileName) { 23 | 24 | const std::string datasetDir = datasetFileName.substr(0, datasetFileName.rfind("/")) + "/"; 25 | 26 | // Set up required structures 27 | // -------------------------- 28 | 29 | // Set up the data iterator for IMU 30 | IMUCSVFile = CSVFile(datasetDir + "mav_imu.csv"); 31 | IMUCSVFile.nextLine(); // skip the header 32 | 33 | // Read the image data 34 | cam_dir = datasetDir + "frames/"; 35 | ImageCSVFile = CSVFile(datasetDir + "cam.csv"); 36 | ImageCSVFile.nextLine(); // skip the header 37 | 38 | const std::string groundtruthFileName = datasetDir + "ground_truth.csv"; 39 | 40 | // Read the camera file 41 | readCamera(datasetDir + "undistort.yaml"); 42 | } 43 | 44 | void APDatasetReader::readCamera(const std::string& cameraFileName) { 45 | assert(std::filesystem::exists(cameraFileName)); 46 | cv::FileStorage fs(cameraFileName, cv::FileStorage::READ); 47 | cv::Mat K, dist; 48 | 49 | fs["camera_matrix"] >> K; 50 | fs["dist_coeffs"] >> dist; 51 | 52 | std::array distVec; 53 | for (int i = 0; i < dist.cols; ++i) { 54 | distVec[i] = dist.at(i); 55 | } 56 | 57 | camera = std::make_shared(GIFT::EquidistantCamera(cv::Size(0, 0), K, distVec)); 58 | } 59 | 60 | std::unique_ptr APDatasetReader::nextIMU() { 61 | if (!IMUCSVFile) { 62 | return nullptr; 63 | } 64 | 65 | CSVLine imuLine = IMUCSVFile.nextLine(); 66 | IMUVelocity temp; 67 | imuLine >> temp; 68 | temp.gyrBiasVel.setZero(); 69 | temp.accBiasVel.setZero(); 70 | return std::make_unique(temp); 71 | } 72 | 73 | std::unique_ptr APDatasetReader::nextImage() { 74 | if (!ImageCSVFile) { 75 | return nullptr; 76 | } 77 | 78 | CSVLine imageLine = ImageCSVFile.nextLine(); 79 | std::string nextImageFname; 80 | double rawStamp; 81 | imageLine >> rawStamp >> nextImageFname; 82 | if (*nextImageFname.rbegin() == '\r') { 83 | nextImageFname.erase(nextImageFname.end() - 1); 84 | } 85 | 86 | nextImageFname = cam_dir + "frame_" + nextImageFname + ".jpg"; 87 | 88 | StampedImage temp; 89 | temp.stamp = rawStamp - cameraLag; 90 | temp.image = cv::imread(nextImageFname); 91 | return std::make_unique(temp); 92 | } 93 | 94 | std::vector APDatasetReader::groundtruth() { 95 | // Find the poses file 96 | assert(std::filesystem::exists(groundtruthFileName)); 97 | std::ifstream poseFile = std::ifstream(groundtruthFileName); 98 | CSVReader poseFileIter(poseFile, ','); 99 | ++poseFileIter; // skip header 100 | 101 | std::vector poses; 102 | 103 | double prevPoseTime = -1e8; 104 | for (CSVLine row : poseFileIter) { 105 | StampedPose pose; 106 | row >> pose.t >> pose.pose; 107 | if (pose.t > prevPoseTime + 1e-8) { 108 | // Avoid poses with the same timestamp. 109 | poses.emplace_back(pose); 110 | prevPoseTime = pose.t; 111 | } 112 | } 113 | 114 | return poses; 115 | } 116 | -------------------------------------------------------------------------------- /src/dataserver/ASLDatasetReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/ASLDatasetReader.h" 19 | #include "yaml-cpp/yaml.h" 20 | #include 21 | 22 | ASLDatasetReader::ASLDatasetReader(const std::string& datasetMainDir) { 23 | // Set up required structures 24 | // -------------------------- 25 | 26 | // Set up the data iterator for IMU 27 | IMUCSVFile = CSVFile(datasetMainDir + "mav0/imu0/" + "data.csv"); 28 | IMUCSVFile.nextLine(); // skip the header 29 | 30 | // Read the image data file 31 | cam_dir = datasetMainDir + "mav0/cam0/"; 32 | ImageCSVFile = CSVFile(cam_dir + "data.csv"); 33 | ImageCSVFile.nextLine(); // skip the header 34 | 35 | // Get the ground truth 36 | groundtruthFileName = datasetMainDir + "mav0/state_groundtruth_estimate0/data.csv"; 37 | 38 | // Read the camera file 39 | std::string cameraFileName = cam_dir + "sensor.yaml"; 40 | readCamera(cameraFileName); 41 | } 42 | 43 | std::unique_ptr ASLDatasetReader::nextIMU() { 44 | if (!IMUCSVFile) { 45 | return nullptr; 46 | } 47 | 48 | CSVLine imuLine = IMUCSVFile.nextLine(); 49 | IMUVelocity temp; 50 | imuLine >> temp; 51 | temp.stamp *= 1e-9; 52 | return std::make_unique(temp); 53 | } 54 | 55 | std::unique_ptr ASLDatasetReader::nextImage() { 56 | if (!ImageCSVFile) { 57 | return nullptr; 58 | } 59 | 60 | CSVLine imageLine = ImageCSVFile.nextLine(); 61 | std::string nextImageFname; 62 | double rawStamp; 63 | imageLine >> rawStamp >> nextImageFname; 64 | nextImageFname = cam_dir + "data/" + nextImageFname; 65 | 66 | if (*nextImageFname.rbegin() == '\r') { 67 | nextImageFname.erase(nextImageFname.end() - 1); 68 | } 69 | 70 | StampedImage temp; 71 | temp.stamp = 1e-9 * rawStamp - cameraLag; 72 | temp.image = cv::imread(nextImageFname); 73 | return std::make_unique(temp); 74 | } 75 | 76 | void ASLDatasetReader::readCamera(const std::string& cameraFileName) { 77 | YAML::Node cameraFileNode = YAML::LoadFile(cameraFileName); 78 | 79 | // Read the intrinsics 80 | cv::Size imageSize; 81 | imageSize.width = cameraFileNode["resolution"][0].as(); 82 | imageSize.height = cameraFileNode["resolution"][1].as(); 83 | 84 | cv::Mat K = cv::Mat::eye(3, 3, CV_64F); 85 | K.at(0, 0) = cameraFileNode["intrinsics"][0].as(); 86 | K.at(1, 1) = cameraFileNode["intrinsics"][1].as(); 87 | K.at(0, 2) = cameraFileNode["intrinsics"][2].as(); 88 | K.at(1, 2) = cameraFileNode["intrinsics"][3].as(); 89 | 90 | std::vector distortion; 91 | distortion = cameraFileNode["distortion_coefficients"].as>(); 92 | 93 | GIFT::StandardCamera stdCamera = GIFT::StandardCamera(imageSize, K, distortion); 94 | this->camera = std::make_shared(stdCamera); 95 | 96 | // Read the extrinsics 97 | const std::vector extrinsics_entries = cameraFileNode["T_BS"]["data"].as>(); 98 | Eigen::Matrix4d extrinsics_matrix(extrinsics_entries.data()); 99 | // The data is in row-major form, but eigen uses column-major by default. 100 | extrinsics_matrix.transposeInPlace(); 101 | cameraExtrinsics = std::make_unique(extrinsics_matrix); 102 | } 103 | 104 | std::vector ASLDatasetReader::groundtruth() { 105 | // Find the poses file 106 | assert(std::filesystem::exists(groundtruthFileName)); 107 | std::ifstream poseFile = std::ifstream(groundtruthFileName); 108 | CSVReader poseFileIter(poseFile, ','); 109 | ++poseFileIter; // skip header 110 | 111 | std::vector poses; 112 | 113 | double prevPoseTime = -1e8; 114 | for (CSVLine row : poseFileIter) { 115 | StampedPose pose; 116 | row >> pose.t >> pose.pose; 117 | pose.t *= 1.0e-9; 118 | if (pose.t > prevPoseTime + 1e-8) { 119 | // Avoid poses with the same timestamp. 120 | poses.emplace_back(pose); 121 | prevPoseTime = pose.t; 122 | } 123 | } 124 | 125 | return poses; 126 | } 127 | -------------------------------------------------------------------------------- /src/dataserver/DataServerBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/DataServerBase.h" 19 | 20 | DataServerBase::DataServerBase(std::unique_ptr&& datasetReader, const YAML::Node& simSettings) 21 | : datasetReaderPtr(std::move(datasetReader)) { 22 | simulator = VIOSimulator(datasetReaderPtr->groundtruth(), datasetReaderPtr->camera, simSettings); 23 | if (cameraExtrinsics()) { 24 | simulator.cameraOffset = *cameraExtrinsics(); 25 | } 26 | } 27 | 28 | void DataServerBase::readCamera(const std::string& cameraFileName) { 29 | datasetReaderPtr->readCamera(cameraFileName); 30 | simulator.cameraPtr = datasetReaderPtr->camera; 31 | if (cameraExtrinsics()) { 32 | simulator.cameraOffset = *cameraExtrinsics(); 33 | } 34 | } 35 | 36 | std::shared_ptr DataServerBase::cameraExtrinsics() const { return datasetReaderPtr->cameraExtrinsics; } 37 | 38 | GIFT::GICameraPtr DataServerBase::camera() const { return datasetReaderPtr->camera; } 39 | 40 | VisionMeasurement DataServerBase::getSimVision() { 41 | const StampedImage image = getImage(); 42 | return simulator.getVision(image.stamp); 43 | } 44 | 45 | IMUVelocity DataServerBase::getSimIMU() { 46 | const IMUVelocity imu = getIMU(); 47 | return simulator.getIMU(imu.stamp); 48 | } 49 | 50 | std::shared_ptr DataServerBase::groundTruthPose(const double stamp) const { 51 | if (simulator.viewPoses().empty()) { 52 | return nullptr; 53 | } else { 54 | return std::make_shared(simulator.getFullState(stamp, false).sensor.pose); 55 | } 56 | } -------------------------------------------------------------------------------- /src/dataserver/HiltiDatasetReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/HiltiDatasetReader.h" 19 | #include "yaml-cpp/yaml.h" 20 | 21 | void HiltiDatasetReader::readCamera(const std::string& cameraFileName) { 22 | YAML::Node calibrationNode = YAML::LoadFile(cameraFileName); 23 | const auto& cameraNode = calibrationNode["sensors"]["cam0"]; 24 | 25 | // Get the intrinsics 26 | const auto& intrinsicParams = cameraNode["intrinsics"]["parameters"]; 27 | 28 | cv::Size imageSize(intrinsicParams["image_size"][0].as(), intrinsicParams["image_size"][1].as()); 29 | 30 | const cv::Mat K = 31 | (cv::Mat_(3, 3) << intrinsicParams["fx"].as(), 0, intrinsicParams["cx"].as(), 0, 32 | intrinsicParams["fy"].as(), intrinsicParams["cy"].as(), 0, 0, 1); 33 | 34 | std::array distortion = { 35 | intrinsicParams["k1"].as(), intrinsicParams["k2"].as(), intrinsicParams["k3"].as(), 36 | intrinsicParams["k4"].as()}; 37 | 38 | camera = std::make_shared(imageSize, K, distortion); 39 | 40 | // Get the extrinsics 41 | const auto& extrinsicParams = cameraNode["extrinsics"]; 42 | // Note the quaternion is in Hamilton form (w is last instead of first) but we use standard form. 43 | Eigen::Quaterniond attitude = Eigen::Quaterniond( 44 | extrinsicParams["quaternion"][3].as(), extrinsicParams["quaternion"][0].as(), 45 | extrinsicParams["quaternion"][1].as(), extrinsicParams["quaternion"][2].as()); 46 | Eigen::Vector3d position = Eigen::Vector3d( 47 | extrinsicParams["translation"][0].as(), extrinsicParams["translation"][1].as(), 48 | extrinsicParams["translation"][2].as()); 49 | liepp::SE3d extrinsics = liepp::SE3d(liepp::SO3d(attitude), position); 50 | cameraExtrinsics = std::make_unique(extrinsics); 51 | } 52 | -------------------------------------------------------------------------------- /src/dataserver/RosbagDatasetReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/RosbagDatasetReader.h" 19 | #include "cv_bridge/cv_bridge.h" 20 | #include "sensor_msgs/Image.h" 21 | #include "sensor_msgs/Imu.h" 22 | 23 | #include 24 | 25 | static IMUVelocity msgToIMU(const sensor_msgs::ImuPtr& imuMsg) { 26 | assert(imuMsg != nullptr); 27 | IMUVelocity data; 28 | data.stamp = imuMsg->header.stamp.toSec(); 29 | data.gyr << imuMsg->angular_velocity.x, imuMsg->angular_velocity.y, imuMsg->angular_velocity.z; 30 | data.acc << imuMsg->linear_acceleration.x, imuMsg->linear_acceleration.y, imuMsg->linear_acceleration.z; 31 | return data; 32 | } 33 | 34 | static StampedImage msgToImage(const sensor_msgs::ImageConstPtr& imageMsg) { 35 | assert(imageMsg != nullptr); 36 | cv_bridge::CvImagePtr cvImageMsg = cv_bridge::toCvCopy(imageMsg); 37 | StampedImage data; 38 | data.stamp = cvImageMsg->header.stamp.toSec(); 39 | data.image = cvImageMsg->image; 40 | return data; 41 | } 42 | 43 | RosbagDatasetReader::RosbagDatasetReader( 44 | const std::string& rosbagFileName, const std::string& imuTopic, const std::string& imageTopic) { 45 | 46 | // Read the rosbag 47 | bag.open(rosbagFileName); 48 | 49 | imuBagView.addQuery(bag, rosbag::TopicQuery(imuTopic)); 50 | imageBagView.addQuery(bag, rosbag::TopicQuery(imageTopic)); 51 | 52 | imuViewIt = imuBagView.begin(); 53 | imageViewIt = imageBagView.begin(); 54 | 55 | // Try to load the camera 56 | const std::string datasetDir = rosbagFileName.substr(0, rosbagFileName.rfind("/")) + "/"; 57 | const std::string cameraFileName = datasetDir + "intrinsics.yaml"; 58 | if (!std::filesystem::exists(cameraFileName)) { 59 | std::cout << "No camera intrinsics were not found at\n" << cameraFileName << std::endl; 60 | } else { 61 | readCamera(cameraFileName); 62 | } 63 | } 64 | 65 | void RosbagDatasetReader::readCamera(const std::string& cameraFileName) { 66 | YAML::Node cameraFileNode = YAML::LoadFile(cameraFileName); 67 | 68 | // Read the intrinsics 69 | cv::Size imageSize; 70 | imageSize.width = cameraFileNode["resolution"][0].as(); 71 | imageSize.height = cameraFileNode["resolution"][1].as(); 72 | 73 | cv::Mat K = cv::Mat::eye(3, 3, CV_64F); 74 | K.at(0, 0) = cameraFileNode["intrinsics"][0].as(); 75 | K.at(1, 1) = cameraFileNode["intrinsics"][1].as(); 76 | K.at(0, 2) = cameraFileNode["intrinsics"][2].as(); 77 | K.at(1, 2) = cameraFileNode["intrinsics"][3].as(); 78 | 79 | std::vector distortion; 80 | distortion = cameraFileNode["distortion_coefficients"].as>(); 81 | 82 | GIFT::StandardCamera stdCamera = GIFT::StandardCamera(imageSize, K, distortion); 83 | this->camera = std::make_shared(stdCamera); 84 | // camera = std::make_shared(GIFT::StandardCamera(cv::String(cameraFileName))); 85 | 86 | // Read the extrinsics 87 | const std::vector extrinsics_entries = cameraFileNode["T_BS"]["data"].as>(); 88 | Eigen::Matrix4d extrinsics_matrix(extrinsics_entries.data()); 89 | // The data is in row-major form, but eigen uses column-major by default. 90 | extrinsics_matrix.transposeInPlace(); 91 | cameraExtrinsics = std::make_unique(extrinsics_matrix); 92 | 93 | } 94 | 95 | std::unique_ptr RosbagDatasetReader::nextImage() { 96 | if (imageViewIt == imageBagView.end()) { 97 | return nullptr; 98 | } 99 | StampedImage currentImage = msgToImage(imageViewIt->instantiate()); 100 | ++imageViewIt; 101 | return std::make_unique(currentImage); 102 | } 103 | 104 | std::unique_ptr RosbagDatasetReader::nextIMU() { 105 | if (imuViewIt == imuBagView.end()) { 106 | return nullptr; 107 | } 108 | IMUVelocity currentIMU = msgToIMU(imuViewIt->instantiate()); 109 | ++imuViewIt; 110 | return std::make_unique(currentIMU); 111 | } 112 | -------------------------------------------------------------------------------- /src/dataserver/SimpleDataServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/SimpleDataServer.h" 19 | 20 | MeasurementType SimpleDataServer::nextMeasurementType() const { 21 | if (nextImageData && nextIMUData) { 22 | return (nextImageData->stamp <= nextIMUData->stamp) ? MeasurementType::Image : MeasurementType::IMU; 23 | } else if (nextImageData) { 24 | return MeasurementType::Image; 25 | } else if (nextIMUData) { 26 | return MeasurementType::IMU; 27 | } 28 | return MeasurementType::None; 29 | } 30 | 31 | StampedImage SimpleDataServer::getImage() { 32 | StampedImage retImageData = *nextImageData; 33 | nextImageData = datasetReaderPtr->nextImage(); 34 | return retImageData; 35 | } 36 | 37 | IMUVelocity SimpleDataServer::getIMU() { 38 | IMUVelocity retIMUData = *nextIMUData; 39 | nextIMUData = datasetReaderPtr->nextIMU(); 40 | return retIMUData; 41 | } 42 | 43 | SimpleDataServer::SimpleDataServer(std::unique_ptr&& datasetReader, const YAML::Node& simSettings) 44 | : DataServerBase(std::move(datasetReader), simSettings) { 45 | nextImageData = datasetReaderPtr->nextImage(); 46 | nextIMUData = datasetReaderPtr->nextIMU(); 47 | } 48 | 49 | double SimpleDataServer::nextTime() const { 50 | MeasurementType nextMT = nextMeasurementType(); 51 | if (nextMT == MeasurementType::Image) { 52 | return nextImageData->stamp; 53 | } else if (nextMT == MeasurementType::IMU) { 54 | return nextIMUData->stamp; 55 | } 56 | return std::nan(""); 57 | } -------------------------------------------------------------------------------- /src/dataserver/ThreadedDataServer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/ThreadedDataServer.h" 19 | 20 | MeasurementType ThreadedDataServer::nextMeasurementType() const { 21 | std::unique_lock lck(ioMutex); 22 | queuesReady_cv.wait( 23 | lck, [this] { return (IMUDataFinished || !IMUQueue.empty()) && (imageDataFinished || !imageQueue.empty()); }); 24 | 25 | // Check if any of the data is finished 26 | if (imageQueue.empty() && IMUQueue.empty()) { 27 | return MeasurementType::None; 28 | } else if (imageQueue.empty() && !IMUQueue.empty()) { 29 | return MeasurementType::IMU; 30 | } else if (!imageQueue.empty() && IMUQueue.empty()) { 31 | return MeasurementType::Image; 32 | } 33 | 34 | // Otherwise, compare the stamps 35 | if (imageQueue.front().stamp <= IMUQueue.front().stamp) { 36 | return MeasurementType::Image; 37 | } else { 38 | return MeasurementType::IMU; 39 | } 40 | } 41 | 42 | StampedImage ThreadedDataServer::getImage() { 43 | std::unique_lock lck(ioMutex); 44 | StampedImage data = imageQueue.front(); 45 | imageQueue.pop(); 46 | return data; 47 | } 48 | 49 | IMUVelocity ThreadedDataServer::getIMU() { 50 | std::unique_lock lck(ioMutex); 51 | IMUVelocity data = IMUQueue.front(); 52 | IMUQueue.pop(); 53 | return data; 54 | } 55 | 56 | double ThreadedDataServer::nextTime() const { 57 | const MeasurementType measType = nextMeasurementType(); 58 | std::unique_lock lck(ioMutex); 59 | if (measType == MeasurementType::IMU) { 60 | return IMUQueue.front().stamp; 61 | } 62 | if (measType == MeasurementType::Image) { 63 | return imageQueue.front().stamp; 64 | } 65 | return std::nan(""); 66 | } 67 | 68 | ThreadedDataServer::ThreadedDataServer( 69 | std::unique_ptr&& datasetReader, const YAML::Node& simSettings) 70 | : DataServerBase(std::move(datasetReader), simSettings) { 71 | ioThread = std::thread(&ThreadedDataServer::fillQueues, this); 72 | } 73 | 74 | void ThreadedDataServer::fillQueues() { 75 | while ((!IMUDataFinished || !imageDataFinished) && !destructorCalled) { 76 | 77 | while (!queuesFilled()) { 78 | std::unique_lock lck(ioMutex); 79 | 80 | // Fill up the image and IMU queues 81 | if (imageQueue.size() < maxImageQueueSize) { 82 | std::unique_ptr nextImageData = datasetReaderPtr->nextImage(); 83 | if (nextImageData) { 84 | imageQueue.emplace(*nextImageData); 85 | } else { 86 | imageDataFinished = true; 87 | } 88 | } 89 | 90 | if (IMUQueue.size() < maxIMUQueueSize) { 91 | std::unique_ptr nextIMUData = datasetReaderPtr->nextIMU(); 92 | if (nextIMUData) { 93 | IMUQueue.emplace(*nextIMUData); 94 | } else { 95 | IMUDataFinished = true; 96 | } 97 | } 98 | 99 | if (IMUDataFinished && imageDataFinished) { 100 | break; 101 | } 102 | } 103 | queuesReady_cv.notify_one(); 104 | } 105 | } 106 | 107 | bool ThreadedDataServer::queuesFilled() const { 108 | std::unique_lock lck(ioMutex); 109 | return (IMUDataFinished || IMUQueue.size() == maxIMUQueueSize) && 110 | (imageDataFinished || imageQueue.size() == maxImageQueueSize); 111 | } 112 | 113 | ThreadedDataServer::~ThreadedDataServer() { 114 | destructorCalled = true; 115 | ioThread.join(); 116 | } -------------------------------------------------------------------------------- /src/dataserver/UZHFPVDatasetReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/dataserver/UZHFPVDatasetReader.h" 19 | #include "yaml-cpp/yaml.h" 20 | #include 21 | #include 22 | 23 | UZHFPVDatasetReader::UZHFPVDatasetReader(const std::string& datasetMainDir) { 24 | // Set up required structures 25 | // -------------------------- 26 | 27 | baseDir = datasetMainDir; 28 | 29 | // Set up the data iterator for IMU 30 | IMUCSVFile = CSVFile(datasetMainDir + "imu.txt", ' '); 31 | IMUCSVFile.nextLine(); // skip the header 32 | 33 | // Read the image data file 34 | ImageCSVFile = CSVFile(datasetMainDir + "left_images.txt", ' '); 35 | ImageCSVFile.nextLine(); // skip the header 36 | 37 | // Read the camera file 38 | std::string cameraFileName = 39 | baseDir + "../indoor_forward_calib_snapdragon/camchain-imucam-..indoor_forward_calib_snapdragon_imu.yaml"; 40 | if (std::filesystem::exists(cameraFileName)) { 41 | readCamera(cameraFileName); 42 | } else { 43 | std::cout << "Camera file was not found at:\n" << cameraFileName << std::endl; 44 | throw std::runtime_error("The camera file could not be found."); 45 | } 46 | } 47 | 48 | std::unique_ptr UZHFPVDatasetReader::nextIMU() { 49 | if (!IMUCSVFile) { 50 | return nullptr; 51 | } 52 | 53 | CSVLine imuLine = IMUCSVFile.nextLine(); 54 | int num; 55 | IMUVelocity temp; 56 | imuLine >> num >> temp; 57 | return std::make_unique(temp); 58 | } 59 | 60 | std::unique_ptr UZHFPVDatasetReader::nextImage() { 61 | if (!ImageCSVFile) { 62 | return nullptr; 63 | } 64 | StampedImage temp; 65 | 66 | CSVLine imageLine = ImageCSVFile.nextLine(); 67 | std::string nextImageFname; 68 | double rawStamp; 69 | int num; 70 | imageLine >> num >> rawStamp >> nextImageFname; 71 | 72 | nextImageFname = baseDir + nextImageFname; 73 | if (*nextImageFname.rbegin() == '\r') { 74 | nextImageFname.erase(nextImageFname.end() - 1); 75 | } 76 | 77 | temp.stamp = rawStamp - cameraLag; 78 | temp.image = cv::imread(nextImageFname); 79 | 80 | return std::make_unique(temp); 81 | } 82 | 83 | void UZHFPVDatasetReader::readCamera(const std::string& cameraFileName) { 84 | YAML::Node cameraFileNode = YAML::LoadFile(cameraFileName); 85 | 86 | // Read the camera intrinsics 87 | cameraFileNode = cameraFileNode["cam0"]; 88 | 89 | cv::Size imageSize; 90 | imageSize.width = cameraFileNode["resolution"][0].as(); 91 | imageSize.height = cameraFileNode["resolution"][1].as(); 92 | 93 | cv::Mat K = cv::Mat::eye(3, 3, CV_64F); 94 | K.at(0, 0) = cameraFileNode["intrinsics"][0].as(); 95 | K.at(1, 1) = cameraFileNode["intrinsics"][1].as(); 96 | K.at(0, 2) = cameraFileNode["intrinsics"][2].as(); 97 | K.at(1, 2) = cameraFileNode["intrinsics"][3].as(); 98 | 99 | std::array distortion; 100 | distortion = cameraFileNode["distortion_coeffs"].as>(); 101 | 102 | camera = std::make_shared(imageSize, K, distortion); 103 | 104 | // Read the camera extrinsics 105 | const std::vector> extrinsics_rows = 106 | cameraFileNode["T_cam_imu"].as>>(); 107 | Eigen::Matrix4d extrinsics_matrix; 108 | assert(extrinsics_rows.size() == 4); 109 | for (int i = 0; i < 4; ++i) { 110 | assert(extrinsics_rows[i].size() == 4); 111 | extrinsics_matrix.row(i) = Eigen::Vector4d(extrinsics_rows[i].data()).transpose(); 112 | } 113 | // The UZH FPV dataset reports the pose of the IMU w.r.t. the camera, so it needs to be inverted. 114 | cameraExtrinsics = std::make_unique(extrinsics_matrix.inverse()); 115 | } 116 | 117 | std::vector UZHFPVDatasetReader::groundtruth() { 118 | const std::string groundtruthFileName = baseDir + "groundtruth.txt"; 119 | 120 | assert(std::filesystem::exists(groundtruthFileName)); 121 | std::ifstream poseFile = std::ifstream(groundtruthFileName); 122 | CSVReader poseFileIter(poseFile, ' '); 123 | ++poseFileIter; // skip header 124 | 125 | std::vector poses; 126 | 127 | double prevPoseTime = -1e8; 128 | for (CSVLine row : poseFileIter) { 129 | 130 | StampedPose pose; 131 | row >> pose.t >> pose.pose; 132 | if (pose.t > prevPoseTime + 1e-8) { 133 | // Avoid poses with the same timestamp. 134 | poses.emplace_back(pose); 135 | prevPoseTime = pose.t; 136 | } 137 | } 138 | 139 | return poses; 140 | } -------------------------------------------------------------------------------- /src/mathematical/EqFMatrices.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/mathematical/EqFMatrices.h" 19 | 20 | using namespace Eigen; 21 | using namespace std; 22 | using namespace liepp; 23 | 24 | Eigen::MatrixXd EqFCoordinateSuite::stateMatrixADiscrete( 25 | const VIOGroup& X, const VIOState& xi0, const IMUVelocity& imuVel, const double& dt) const { 26 | // Compute using numerical differentiation 27 | 28 | auto a0Discrete = [&](const VectorXd& epsilon) { 29 | const auto xi_e = stateChart.inv(epsilon, xi0); 30 | const auto xi_hat = stateGroupAction(X, xi0); 31 | const auto xi = stateGroupAction(X, xi_e); 32 | const auto LambdaTilde = 33 | liftVelocityDiscrete(xi, imuVel, dt) * liftVelocityDiscrete(xi_hat, imuVel, dt).inverse(); 34 | const auto xi_e1 = stateGroupAction(X * LambdaTilde * X.inverse(), xi_e); 35 | const VectorXd epsilon1 = stateChart(xi_e1, xi0); 36 | return epsilon1; 37 | }; 38 | 39 | const auto A0tD = numericalDifferential(a0Discrete, Eigen::VectorXd::Zero(xi0.Dim())); 40 | return A0tD; 41 | } 42 | 43 | const Eigen::MatrixXd EqFCoordinateSuite::outputMatrixC( 44 | const VIOState& xi0, const VIOGroup& X, const VisionMeasurement& y, const bool useEquivariance) const { 45 | // Rows and their corresponding output components 46 | // [2i, 2i+2): Landmark measurement i 47 | 48 | // Cols and their corresponding state components 49 | // I am using zero indexing and half open ranges 50 | // [0,2): Gravity vector (deviation from e3) 51 | // [2,5) Body-fixed velocity 52 | // [5+3i,5+3(i+1)): Body-fixed landmark i 53 | 54 | const int M = xi0.cameraLandmarks.size(); 55 | const vector ids = y.getIds(); 56 | const int N = ids.size(); 57 | MatrixXd CStar = MatrixXd::Zero(2 * N, VIOSensorState::CompDim + Landmark::CompDim * M); 58 | 59 | const VisionMeasurement yHat = measureSystemState(stateGroupAction(X, xi0), y.cameraPtr); 60 | 61 | for (int i = 0; i < M; ++i) { 62 | const int& idNum = xi0.cameraLandmarks[i].id; 63 | const Vector3d& qi0 = xi0.cameraLandmarks[i].p; 64 | const auto it_y = find(ids.begin(), ids.end(), idNum); 65 | const auto it_Q = find(X.id.begin(), X.id.end(), idNum); 66 | assert(it_Q != X.id.end()); 67 | const int k = distance(X.id.begin(), it_Q); 68 | if (it_y != ids.end()) { 69 | 70 | assert(*it_y == *it_Q); 71 | assert(X.id[k] == idNum); 72 | 73 | const int j = distance(ids.begin(), it_y); 74 | CStar.block<2, 3>(2 * j, VIOSensorState::CompDim + 3 * i) = 75 | useEquivariance ? outputMatrixCiStar(qi0, X.Q[k], y.cameraPtr, y.camCoordinates.at(idNum)) 76 | : outputMatrixCi(qi0, X.Q[k], y.cameraPtr); 77 | } 78 | } 79 | 80 | assert(!CStar.hasNaN()); 81 | return CStar; 82 | } 83 | 84 | const Eigen::Matrix EqFCoordinateSuite::outputMatrixCi( 85 | const Eigen::Vector3d& q0, const liepp::SOT3d& QHat, const GIFT::GICameraPtr& camPtr) const { 86 | const Vector3d qHat = QHat.inverse() * q0; 87 | const Vector2d yHat = camPtr->projectPoint(qHat); 88 | return outputMatrixCiStar(q0, QHat, camPtr, yHat); 89 | } -------------------------------------------------------------------------------- /src/mathematical/Geometry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/mathematical/Geometry.h" 19 | #include "eigen3/Eigen/Cholesky" 20 | #include 21 | 22 | static std::random_device rd; 23 | static std::mt19937 rng = std::mt19937(rd()); 24 | 25 | Eigen::MatrixXd 26 | numericalDifferential(std::function f, const Eigen::VectorXd& x, double h) { 27 | if (h < 0) { 28 | h = std::cbrt(std::numeric_limits::epsilon()); 29 | } 30 | Eigen::MatrixXd Df(f(x).rows(), x.rows()); 31 | for (int j = 0; j < Df.cols(); ++j) { 32 | const Eigen::VectorXd ej = Eigen::VectorXd::Unit(Df.cols(), j); 33 | Df.col(j) = (f(x + h * ej) - f(x - h * ej)) / (2 * h); 34 | } 35 | return Df; 36 | } 37 | 38 | Eigen::VectorXd sampleGaussianDistribution(const Eigen::MatrixXd& covariance) { 39 | std::normal_distribution<> dist{0, 1}; 40 | const int n = covariance.rows(); 41 | Eigen::VectorXd x(n); 42 | 43 | for (int i = 0; i < n; ++i) { 44 | x(i) = dist(rng); 45 | } 46 | 47 | Eigen::MatrixXd L = covariance.llt().matrixL(); 48 | Eigen::VectorXd sample = L * x; 49 | 50 | return sample; 51 | } 52 | 53 | std::vector sampleGaussianDistribution(const Eigen::MatrixXd& covariance, const size_t& numSamples) { 54 | std::normal_distribution<> dist{0, 1}; 55 | 56 | const int n = covariance.rows(); 57 | Eigen::MatrixXd L = covariance.llt().matrixL(); 58 | 59 | std::vector samples(numSamples); 60 | for (Eigen::VectorXd& sample : samples) { 61 | Eigen::VectorXd x(n); 62 | for (int i = 0; i < n; ++i) { 63 | x(i) = dist(rng); 64 | } 65 | sample = L * x; 66 | } 67 | 68 | return samples; 69 | } -------------------------------------------------------------------------------- /src/mathematical/IMUVelocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/mathematical/IMUVelocity.h" 19 | 20 | IMUVelocity IMUVelocity::Zero() { 21 | IMUVelocity result; 22 | result.stamp = 0; 23 | result.gyr.setZero(); 24 | result.acc.setZero(); 25 | return result; 26 | } 27 | 28 | IMUVelocity::IMUVelocity(const Eigen::Matrix& vec) { 29 | stamp = 0; 30 | gyr = vec.segment<3>(0); 31 | acc = vec.segment<3>(3); 32 | } 33 | 34 | IMUVelocity::IMUVelocity(const Eigen::Matrix& vec) { 35 | stamp = 0; 36 | gyr = vec.segment<3>(0); 37 | acc = vec.segment<3>(3); 38 | gyrBiasVel = vec.segment<3>(6); 39 | accBiasVel = vec.segment<3>(9); 40 | } 41 | 42 | IMUVelocity IMUVelocity::operator+(const IMUVelocity& other) const { 43 | IMUVelocity result; 44 | result.stamp = (this->stamp > 0) ? this->stamp : other.stamp; 45 | result.gyr = this->gyr + other.gyr; 46 | result.acc = this->acc + other.acc; 47 | result.gyrBiasVel = this->gyrBiasVel + other.gyrBiasVel; 48 | result.accBiasVel = this->accBiasVel + other.accBiasVel; 49 | return result; 50 | } 51 | 52 | IMUVelocity IMUVelocity::operator-(const Eigen::Matrix& vec) const { 53 | IMUVelocity result; 54 | result.stamp = this->stamp; 55 | result.gyr = this->gyr - vec.segment<3>(0); 56 | result.acc = this->acc - vec.segment<3>(3); 57 | return result; 58 | } 59 | IMUVelocity IMUVelocity::operator-(const Eigen::Matrix& vec) const { 60 | IMUVelocity result; 61 | result.stamp = this->stamp; 62 | result.gyr = this->gyr - vec.segment<3>(0); 63 | result.acc = this->acc - vec.segment<3>(3); 64 | result.gyrBiasVel = this->gyrBiasVel - vec.segment<3>(6); 65 | result.accBiasVel = this->accBiasVel - vec.segment<3>(9); 66 | return result; 67 | } 68 | 69 | IMUVelocity IMUVelocity::operator*(const double& c) const { 70 | IMUVelocity result; 71 | result.stamp = this->stamp; 72 | result.gyr = this->gyr * c; 73 | result.acc = this->acc * c; 74 | result.gyrBiasVel = this->gyrBiasVel * c; 75 | result.accBiasVel = this->accBiasVel * c; 76 | return result; 77 | } 78 | 79 | CSVLine& operator<<(CSVLine& line, const IMUVelocity& imu) { 80 | return line << imu.stamp << imu.gyr << imu.acc << imu.gyrBiasVel << imu.accBiasVel; 81 | } 82 | CSVLine& operator>>(CSVLine& line, IMUVelocity& imu) { 83 | line >> imu.stamp >> imu.gyr >> imu.acc; 84 | if (line.empty()) { 85 | return line; 86 | } else { 87 | return line >> imu.gyrBiasVel >> imu.accBiasVel; 88 | } 89 | } -------------------------------------------------------------------------------- /src/mathematical/VisionMeasurement.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/mathematical/VisionMeasurement.h" 19 | #include "eqvio/mathematical/VIOState.h" 20 | 21 | using namespace std; 22 | using namespace Eigen; 23 | 24 | std::vector VisionMeasurement::getIds() const { 25 | std::vector ids(camCoordinates.size()); 26 | transform(camCoordinates.begin(), camCoordinates.end(), ids.begin(), [](const auto& cc) { return cc.first; }); 27 | return ids; 28 | } 29 | 30 | std::map VisionMeasurement::ocvCoordinates() const { 31 | std::map ocvPoints; 32 | for (const auto& [id, pt] : camCoordinates) { 33 | ocvPoints[id] = cv::Point2f(pt.x(), pt.y()); 34 | } 35 | return ocvPoints; 36 | } 37 | 38 | CSVLine& operator>>(CSVLine& line, VisionMeasurement& vision) { 39 | line >> vision.stamp; 40 | int numBearings; 41 | line >> numBearings; 42 | for (int i = 0; i < numBearings; ++i) { 43 | int id; 44 | Vector2d y; 45 | line >> id >> y; 46 | vision.camCoordinates[id] = y; 47 | } 48 | return line; 49 | } 50 | 51 | CSVLine& operator<<(CSVLine& line, const VisionMeasurement& vision) { 52 | line << vision.stamp; 53 | line << vision.camCoordinates.size(); 54 | for (const pair& cc : vision.camCoordinates) { 55 | line << cc.first << cc.second; 56 | } 57 | return line; 58 | } 59 | 60 | VisionMeasurement operator-(const VisionMeasurement& y1, const VisionMeasurement& y2) { 61 | VisionMeasurement yDiff; 62 | for (const pair& cc1 : y1.camCoordinates) { 63 | const auto it2 = y2.camCoordinates.find(cc1.first); 64 | if (it2 != y2.camCoordinates.end()) { 65 | yDiff.camCoordinates[cc1.first] = cc1.second - it2->second; 66 | } 67 | } 68 | assert(y1.cameraPtr == y2.cameraPtr); 69 | yDiff.cameraPtr = y1.cameraPtr; 70 | return yDiff; 71 | } 72 | VisionMeasurement::operator Eigen::VectorXd() const { 73 | vector ids = getIds(); 74 | Eigen::VectorXd result = Eigen::VectorXd(2 * ids.size()); 75 | for (size_t i = 0; i < ids.size(); ++i) { 76 | result.segment<2>(2 * i) = camCoordinates.at(ids[i]); 77 | } 78 | return result; 79 | } 80 | 81 | VisionMeasurement operator+(const VisionMeasurement& y, const Eigen::VectorXd& eta) { 82 | assert(eta.rows() == 2 * y.camCoordinates.size()); 83 | VisionMeasurement result = y; 84 | size_t i = 0; 85 | for (auto& pixelCoords : result.camCoordinates) { 86 | pixelCoords.second += eta.segment<2>(2 * i); 87 | ++i; 88 | } 89 | return result; 90 | } -------------------------------------------------------------------------------- /src/mathematical/coordinateSuite/normal.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/mathematical/EqFMatrices.h" 19 | 20 | using namespace Eigen; 21 | using namespace std; 22 | using namespace liepp; 23 | 24 | Eigen::MatrixXd EqFStateMatrixA_normal(const VIOGroup& X, const VIOState& xi0, const IMUVelocity& imuVel); 25 | Eigen::MatrixXd EqFInputMatrixB_normal(const VIOGroup& X, const VIOState& xi0); 26 | Eigen::Matrix EqFoutputMatrixCiStar_normal( 27 | const Vector3d& q0, const SOT3d& QHat, const GIFT::GICameraPtr& camPtr, const Eigen::Vector2d& y); 28 | 29 | VIOAlgebra liftInnovation_normal(const Eigen::VectorXd& baseInnovation, const VIOState& xi0); 30 | VIOAlgebra liftInnovation_normal(const Eigen::VectorXd& totalInnovation, const VIOState& xi0); 31 | VIOGroup liftInnovationDiscrete_normal(const Eigen::VectorXd& totalInnovation, const VIOState& xi0); 32 | 33 | const EqFCoordinateSuite EqFCoordinateSuite_normal{VIOChart_normal, EqFStateMatrixA_normal, 34 | EqFInputMatrixB_normal, EqFoutputMatrixCiStar_normal, 35 | liftInnovation_normal, liftInnovationDiscrete_normal}; 36 | 37 | Eigen::MatrixXd EqFStateMatrixA_normal(const VIOGroup& X, const VIOState& xi0, const IMUVelocity& imuVel) { 38 | const MatrixXd& M = coordinateDifferential_normal_euclid(xi0); 39 | return M * EqFCoordinateSuite_euclid.stateMatrixA(X, xi0, imuVel) * M.inverse(); 40 | } 41 | 42 | Eigen::MatrixXd EqFInputMatrixB_normal(const VIOGroup& X, const VIOState& xi0) { 43 | const MatrixXd& M = coordinateDifferential_normal_euclid(xi0); 44 | return M * EqFCoordinateSuite_euclid.inputMatrixB(X, xi0); 45 | } 46 | 47 | VIOAlgebra liftInnovation_normal(const Eigen::VectorXd& baseInnovation, const VIOState& xi0) { 48 | const MatrixXd& M = coordinateDifferential_normal_euclid(xi0); 49 | return EqFCoordinateSuite_euclid.liftInnovation(M.inverse() * baseInnovation, xi0); 50 | } 51 | 52 | VIOGroup liftInnovationDiscrete_normal(const Eigen::VectorXd& totalInnovation, const VIOState& xi0) { 53 | return EqFCoordinateSuite_euclid.liftInnovationDiscrete( 54 | VIOChart_euclid(VIOChart_normal.inv(totalInnovation, xi0), xi0), xi0); 55 | } 56 | 57 | Eigen::Matrix EqFoutputMatrixCiStar_normal( 58 | const Vector3d& q0, const SOT3d& QHat, const GIFT::GICameraPtr& camPtr, [[maybe_unused]] const Eigen::Vector2d& y) { 59 | const Vector3d& y0 = q0.normalized(); 60 | const Vector3d& yHat = QHat.R.inverse() * y0; 61 | Eigen::Matrix C0i = Eigen::Matrix::Zero(); 62 | C0i.block<2, 2>(0, 0) = 63 | camPtr->projectionJacobian(yHat) * QHat.R.asMatrix().transpose() * sphereChart_normal.chartInvDiff0(q0); 64 | return C0i; 65 | } -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | # Get gtest as an external project 3 | if (NOT TARGET gtest_main) 4 | include(PrepareGTest.cmake) 5 | endif() 6 | # This creates the linking target gtest_main 7 | 8 | # Add some testing utility functions 9 | add_library(testing_utilities_lib 10 | testing_utilities.h 11 | testing_utilities.cpp 12 | ) 13 | target_include_directories(testing_utilities_lib PUBLIC 14 | include 15 | test 16 | ${EIGEN_INCLUDE_DIRS} 17 | ) 18 | target_link_libraries(testing_utilities_lib 19 | eqvio_lib 20 | gtest_main 21 | ) 22 | 23 | set(TESTS_LIST 24 | test_settings 25 | test_VIOGroup 26 | test_VIOGroupActions 27 | test_VIOLift 28 | test_CoordinateCharts 29 | test_EqFMatrices 30 | test_FilterStatistics 31 | ) 32 | 33 | set(TEST_REPS 25) 34 | set(NEAR_ZERO 1e-12) 35 | 36 | set(EQVIO_DEFAULT_CONFIG_FILE "../EQVIO_config_template.yaml") 37 | get_filename_component(EQVIO_DEFAULT_CONFIG_FILE ${EQVIO_DEFAULT_CONFIG_FILE} ABSOLUTE) 38 | add_compile_definitions(EQVIO_DEFAULT_CONFIG_FILE="${EQVIO_DEFAULT_CONFIG_FILE}") 39 | 40 | foreach(TEST_NAME ${TESTS_LIST}) 41 | add_executable(${TEST_NAME} "${TEST_NAME}.cpp") 42 | target_include_directories(${TEST_NAME} PUBLIC 43 | include 44 | test 45 | ${EIGEN_INCLUDE_DIRS} 46 | ) 47 | target_link_libraries(${TEST_NAME} 48 | gtest_main 49 | eqvio_lib 50 | testing_utilities_lib 51 | ) 52 | target_compile_definitions(${TEST_NAME} 53 | PUBLIC TEST_REPS=${TEST_REPS} 54 | PUBLIC NEAR_ZERO=${NEAR_ZERO} 55 | ) 56 | add_test("${TEST_NAME}" ${TEST_NAME}) 57 | endforeach() -------------------------------------------------------------------------------- /test/GTestCMakeLists.txt.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | 3 | project(googletest-download NONE) 4 | 5 | include(ExternalProject) 6 | ExternalProject_Add(googletest 7 | GIT_REPOSITORY https://github.com/google/googletest.git 8 | GIT_TAG main 9 | SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src" 10 | BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build" 11 | CONFIGURE_COMMAND "" 12 | BUILD_COMMAND "" 13 | INSTALL_COMMAND "" 14 | TEST_COMMAND "" 15 | ) -------------------------------------------------------------------------------- /test/PrepareGTest.cmake: -------------------------------------------------------------------------------- 1 | # Download and unpack googletest at configure time 2 | configure_file(GTestCMakeLists.txt.in googletest-download/CMakeLists.txt) 3 | execute_process(COMMAND ${CMAKE_COMMAND} -G "${CMAKE_GENERATOR}" . 4 | RESULT_VARIABLE result 5 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) 6 | if(result) 7 | message(FATAL_ERROR "CMake step for googletest failed: ${result}") 8 | endif() 9 | execute_process(COMMAND ${CMAKE_COMMAND} --build . 10 | RESULT_VARIABLE result 11 | WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/googletest-download ) 12 | if(result) 13 | message(FATAL_ERROR "Build step for googletest failed: ${result}") 14 | endif() 15 | 16 | # Prevent overriding the parent project's compiler/linker 17 | # settings on Windows 18 | set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) 19 | 20 | # Add googletest directly to our build. This defines 21 | # the gtest and gtest_main targets. 22 | add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src 23 | ${CMAKE_CURRENT_BINARY_DIR}/googletest-build 24 | EXCLUDE_FROM_ALL) 25 | 26 | # The gtest/gtest_main targets carry header search path 27 | # dependencies automatically when using CMake 2.8.11 or 28 | # later. Otherwise we have to add them here ourselves. 29 | if (CMAKE_VERSION VERSION_LESS 2.8.11) 30 | include_directories("${gtest_SOURCE_DIR}/include") 31 | endif() -------------------------------------------------------------------------------- /test/test_CSVReader.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/csv/CSVReader.h" 19 | #include "gtest/gtest.h" 20 | 21 | const std::string testData = 22 | R"(1370925.32417695,0.0127831735,0.013848438,-0.013848438,-0.16759412,9.299079,2.1739352 23 | 1370925.3447114,-0.025566347,-0.043675844,-0.004261058,0.15562311,9.436746,2.2110453 24 | 1370925.36550249,-0.07243799,-0.010652645,-0.040480047,-0.033518825,9.387665,2.6360161 25 | 1370925.38669978,-0.05539375,-0.013848438,-0.020240024,-0.32202014,9.314642,3.098097 26 | 1370925.40955832,0.033023197,-0.041545313,0.024501082,-0.18794483,9.337387,2.9855695 27 | 1370925.42916753,0.043675844,-0.046871636,0.026631612,-0.16759412,9.411607,2.565387 28 | 1370925.45042336,-0.0063915867,-0.040480047,0.026631612,-0.12090719,9.491813,2.457648 29 | 1370925.4716602,-0.027696876,-0.046871636,0.018109497,0.04189853,9.411607,3.1316159 30 | 1370925.49317653,0.036218993,-0.037284255,0.00958738,-0.05865794,9.356541,3.0430303 31 | 1370925.51373899,0.084155895,-0.07243799,0.040480047,-0.09935937,9.356541,2.8323407 32 | 1370925.53606574,0.0777643,-0.080960095,0.0479369,0.10654198,9.403227,2.255338)"; 33 | 34 | TEST(CSVReaderTest, Instantiation) { 35 | std::stringstream csvFile = std::stringstream(testData); 36 | EXPECT_NO_THROW(CSVReader reader(csvFile)); 37 | } 38 | 39 | TEST(CSVReaderTest, IteratorLoop) { 40 | std::stringstream csvFile = std::stringstream(testData); 41 | CSVReader reader(csvFile); 42 | while (reader != CSVReader()) { 43 | CSVLine line = *reader; 44 | EXPECT_NO_THROW(std::string temp = line[0]); 45 | ++reader; 46 | } 47 | } 48 | 49 | TEST(CSVReaderTest, RangeLoop) { 50 | std::stringstream csvFile = std::stringstream(testData); 51 | CSVReader reader(csvFile); 52 | for (auto row : reader) { 53 | EXPECT_NO_THROW(std::string temp = row[0]); 54 | } 55 | } -------------------------------------------------------------------------------- /test/test_CoordinateCharts.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eigen3/Eigen/Dense" 19 | #include "eqvio/VIOFilter.h" 20 | #include "testing_utilities.h" 21 | #include "gtest/gtest.h" 22 | 23 | using namespace Eigen; 24 | using namespace std; 25 | 26 | TEST(CoordinateChartTest, SphereChartE3) { 27 | // Test the sphere coordinate charts 28 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 29 | const Vector3d eta = Vector3d::Random().normalized(); 30 | const Vector2d y1 = e3ProjectSphere(eta); 31 | const Vector3d eta1 = e3ProjectSphereInv(y1); 32 | const double dist_eta1 = (eta - eta1).norm(); 33 | EXPECT_LE(dist_eta1, NEAR_ZERO); 34 | 35 | const Vector2d y = Vector2d::Random(); 36 | const Vector3d eta2 = e3ProjectSphereInv(y); 37 | const Vector2d y2 = e3ProjectSphere(eta2); 38 | const double dist_y2 = (y2 - y).norm(); 39 | EXPECT_LE(dist_y2, NEAR_ZERO); 40 | } 41 | } 42 | 43 | TEST(CoordinateChartTest, SphereChartPole) { 44 | // Test the sphere coordinate charts with poles 45 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 46 | const Vector3d pole = Vector3d::Random().normalized(); 47 | 48 | const Vector2d poleCoords = sphereChart_stereo(pole, pole); 49 | EXPECT_LE(poleCoords.norm(), NEAR_ZERO); 50 | 51 | const Vector3d eta = Vector3d::Random().normalized(); 52 | const Vector2d y1 = sphereChart_stereo(eta, pole); 53 | const Vector3d eta1 = sphereChart_stereo.inv(y1, pole); 54 | const double dist_eta1 = (eta - eta1).norm(); 55 | EXPECT_LE(dist_eta1, NEAR_ZERO); 56 | 57 | const Vector2d y = Vector2d::Random(); 58 | const Vector3d eta2 = sphereChart_stereo.inv(y, pole); 59 | const Vector2d y2 = sphereChart_stereo(eta2, pole); 60 | const double dist_y2 = (y2 - y).norm(); 61 | EXPECT_LE(dist_y2, NEAR_ZERO); 62 | } 63 | } 64 | 65 | TEST(CoordinateChartTest, SphereChartPoleNormal) { 66 | // Test the sphere coordinate charts with poles 67 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 68 | const Vector3d pole = Vector3d::Random().normalized(); 69 | 70 | const Vector2d poleCoords = sphereChart_normal(pole, pole); 71 | EXPECT_LE(poleCoords.norm(), NEAR_ZERO); 72 | 73 | const Vector3d eta = Vector3d::Random().normalized(); 74 | const Vector2d y1 = sphereChart_normal(eta, pole); 75 | const Vector3d eta1 = sphereChart_normal.inv(y1, pole); 76 | const double dist_eta1 = (eta - eta1).norm(); 77 | EXPECT_LE(dist_eta1, NEAR_ZERO); 78 | 79 | const Vector2d y = Vector2d::Random(); 80 | const Vector3d eta2 = sphereChart_normal.inv(y, pole); 81 | const Vector2d y2 = sphereChart_normal(eta2, pole); 82 | const double dist_y2 = (y2 - y).norm(); 83 | EXPECT_LE(dist_y2, NEAR_ZERO); 84 | } 85 | } 86 | 87 | TEST(CoordinateChartTest, SphereChartE3Differential) { 88 | // Test the sphere chart differential 89 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 90 | const Vector3d eta = Vector3d::Random().normalized(); 91 | testDifferential(e3ProjectSphere, eta, e3ProjectSphereDiff(eta)); 92 | 93 | // Test the sphere chart inverse differential 94 | const Vector2d y = Vector2d::Random(); 95 | testDifferential(e3ProjectSphereInv, y, e3ProjectSphereInvDiff(y)); 96 | } 97 | } 98 | 99 | TEST(CoordinateChartTest, SphereChartPoleDifferential) { 100 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 101 | // Test the sphere chart differential with poles 102 | const Vector3d pole = Vector3d::Random().normalized(); 103 | testDifferential( 104 | [&](const Vector3d& eta) { return sphereChart_stereo.chart(eta, pole); }, pole, 105 | sphereChart_stereo.chartDiff0(pole)); 106 | 107 | // Test the sphere chart inverse differential 108 | testDifferential( 109 | [&](const Vector2d& y) { return sphereChart_stereo.chartInv(y, pole); }, Vector2d::Zero(), 110 | sphereChart_stereo.chartInvDiff0(pole)); 111 | } 112 | } 113 | 114 | TEST(CoordinateChartTest, SphereChartPoleDifferentialNormal) { 115 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 116 | // Test the sphere chart differential with poles 117 | const Vector3d pole = Vector3d::Random().normalized(); 118 | testDifferential( 119 | [&](const Vector3d& eta) { return sphereChart_normal.chart(eta, pole); }, pole, 120 | sphereChart_normal.chartDiff0(pole)); 121 | 122 | // Test the sphere chart inverse differential 123 | testDifferential( 124 | [&](const Vector2d& y) { return sphereChart_normal.chartInv(y, pole); }, Vector2d::Zero(), 125 | sphereChart_normal.chartInvDiff0(pole)); 126 | } 127 | } 128 | 129 | void VIOChart_test(const CoordinateChart& chart) { 130 | vector ids = {0, 1, 2, 3, 4}; 131 | // Test the VIO manifold coordinate chart 132 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 133 | const VIOState xi0 = randomStateElement(ids); 134 | const VIOState xi1 = randomStateElement(ids); 135 | 136 | const VectorXd eps = chart(xi1, xi0); 137 | const VIOState xi2 = chart.inv(eps, xi0); 138 | 139 | double dist1Total = stateDistance(xi1, xi2); 140 | EXPECT_LE(dist1Total, 1e-8); 141 | } 142 | } 143 | 144 | TEST(CoordinateChartTest, VIOChart_euclid) { VIOChart_test(VIOChart_euclid); } 145 | TEST(CoordinateChartTest, VIOChart_invdepth) { VIOChart_test(VIOChart_invdepth); } 146 | TEST(CoordinateChartTest, VIOChart_normal) { VIOChart_test(VIOChart_normal); } 147 | 148 | TEST(CoordinateChartTest, VIOChart_euclid_invdepth_diff) { 149 | const vector ids = {0, 1, 2, 3, 4}; 150 | // Test the VIO manifold coordinate chart 151 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 152 | const VIOState xi0 = randomStateElement(ids); 153 | 154 | auto coordChange = [&](const VectorXd& eps) { return VIOChart_invdepth(VIOChart_euclid.inv(eps, xi0), xi0); }; 155 | const MatrixXd changeMatrix = coordinateDifferential_invdepth_euclid(xi0); 156 | testDifferential(coordChange, VIOChart_euclid(xi0, xi0), changeMatrix); 157 | } 158 | } 159 | 160 | TEST(CoordinateChartTest, VIOChart_euclid_normal_diff) { 161 | const vector ids = {0, 1, 2, 3, 4}; 162 | // Test the VIO manifold coordinate chart 163 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 164 | const VIOState xi0 = randomStateElement(ids); 165 | 166 | auto coordChange = [&](const VectorXd& eps) { return VIOChart_normal(VIOChart_euclid.inv(eps, xi0), xi0); }; 167 | const MatrixXd changeMatrix = coordinateDifferential_normal_euclid(xi0); 168 | testDifferential(coordChange, VIOChart_euclid(xi0, xi0), changeMatrix); 169 | } 170 | } 171 | -------------------------------------------------------------------------------- /test/test_VIOGroup.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eigen3/Eigen/Dense" 19 | #include "eqvio/mathematical/VIOGroup.h" 20 | #include "testing_utilities.h" 21 | #include "gtest/gtest.h" 22 | 23 | using namespace Eigen; 24 | using namespace std; 25 | 26 | TEST(VIOGroupTest, BasicOperations) { 27 | vector allIds = {0, 1, 2, 3, 4}; 28 | const VIOGroup groupId = VIOGroup::Identity(allIds); 29 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 30 | const VIOGroup X1 = randomGroupElement(allIds); 31 | const VIOGroup X2 = randomGroupElement(allIds); 32 | const VIOGroup X3 = randomGroupElement(allIds); 33 | 34 | // Check inverse 35 | const double inverseError1 = logNorm(X1.inverse() * X1); 36 | const double inverseError2 = logNorm(X1 * X1.inverse()); 37 | EXPECT_LE(inverseError1, NEAR_ZERO); 38 | EXPECT_LE(inverseError2, NEAR_ZERO); 39 | 40 | // Check associativity 41 | const VIOGroup result12 = (X1 * X2) * X3; 42 | const VIOGroup result23 = X1 * (X2 * X3); 43 | const double assocError1 = logNorm(result12.inverse() * result23); 44 | const double assocError2 = logNorm(result23.inverse() * result12); 45 | const double assocError3 = logNorm(result12 * result23.inverse()); 46 | const double assocError4 = logNorm(result23 * result12.inverse()); 47 | EXPECT_LE(assocError1, NEAR_ZERO); 48 | EXPECT_LE(assocError2, NEAR_ZERO); 49 | EXPECT_LE(assocError3, NEAR_ZERO); 50 | EXPECT_LE(assocError4, NEAR_ZERO); 51 | 52 | // Check identity 53 | const double idError1 = logNorm(groupId); 54 | const double idError2 = logNorm((X1 * groupId) * X1.inverse()); 55 | const double idError3 = logNorm(X1.inverse() * (groupId * X1)); 56 | EXPECT_LE(idError1, NEAR_ZERO); 57 | EXPECT_LE(idError2, NEAR_ZERO); 58 | EXPECT_LE(idError3, NEAR_ZERO); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /test/test_VIOGroupActions.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eigen3/Eigen/Dense" 19 | #include "eqvio/mathematical/VIOGroup.h" 20 | #include "eqvio/mathematical/VIOState.h" 21 | #include "eqvio/mathematical/VisionMeasurement.h" 22 | #include "testing_utilities.h" 23 | #include "gtest/gtest.h" 24 | 25 | using namespace Eigen; 26 | using namespace std; 27 | 28 | TEST(VIOActionTest, StateAction) { 29 | srand(0); 30 | vector ids = {0, 1, 2, 3, 4}; 31 | const VIOGroup groupId = VIOGroup::Identity(ids); 32 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 33 | const VIOGroup X1 = randomGroupElement(ids); 34 | const VIOGroup X2 = randomGroupElement(ids); 35 | 36 | const VIOState xi0 = randomStateElement(ids); 37 | 38 | // Check the distance function works 39 | const double dist00 = stateDistance(xi0, xi0); 40 | EXPECT_LE(dist00, NEAR_ZERO); 41 | 42 | // Check action identity 43 | const VIOState xi0_id = stateGroupAction(groupId, xi0); 44 | const double dist0id = stateDistance(xi0_id, xi0); 45 | EXPECT_LE(dist0id, NEAR_ZERO); 46 | 47 | // Check action compatibility 48 | const VIOState xi1 = stateGroupAction(X2, stateGroupAction(X1, xi0)); 49 | const VIOState xi2 = stateGroupAction(X1 * X2, xi0); 50 | const double dist12 = stateDistance(xi1, xi2); 51 | EXPECT_LE(dist12, NEAR_ZERO); 52 | } 53 | } 54 | 55 | TEST(VIOActionTest, OutputAction) { 56 | srand(0); 57 | vector ids = {0, 1, 2, 3, 4}; 58 | const VIOGroup groupId = VIOGroup::Identity(ids); 59 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 60 | const VIOGroup X1 = randomGroupElement(ids); 61 | const VIOGroup X2 = randomGroupElement(ids); 62 | 63 | VisionMeasurement y0 = randomVisionMeasurement(ids); 64 | 65 | // Check the distance function works 66 | const double dist00 = measurementDistance(y0, y0); 67 | EXPECT_LE(dist00, 1e-5); 68 | 69 | // Check action identity 70 | const VisionMeasurement y0_id = outputGroupAction(groupId, y0); 71 | const double dist0id = measurementDistance(y0_id, y0); 72 | EXPECT_LE(dist0id, 1e-5); 73 | 74 | // Check action compatibility 75 | const VisionMeasurement y1 = outputGroupAction(X2, outputGroupAction(X1, y0)); 76 | const VisionMeasurement y2 = outputGroupAction(X1 * X2, y0); 77 | const double dist12 = measurementDistance(y1, y2); 78 | EXPECT_LE(dist12, 1e-5); 79 | } 80 | } 81 | 82 | TEST(VIOActionTest, OutputEquivariance) { 83 | srand(0); 84 | vector ids = {5, 0, 1, 2, 3, 4}; 85 | const GIFT::GICameraPtr camPtr = createDefaultCamera(); 86 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 87 | const VIOGroup X = randomGroupElement(ids); 88 | const VIOState xi0 = randomStateElement(ids); 89 | 90 | // Check the state/output equivariance 91 | const VisionMeasurement y1 = measureSystemState(stateGroupAction(X, xi0), camPtr); 92 | const VisionMeasurement y2 = outputGroupAction(X, measureSystemState(xi0, camPtr)); 93 | const double dist12 = measurementDistance(y1, y2); 94 | EXPECT_LE(dist12, 1e-5); 95 | } 96 | } 97 | -------------------------------------------------------------------------------- /test/test_VIOLift.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eigen3/Eigen/Dense" 19 | #include "eqvio/mathematical/EqFMatrices.h" 20 | #include "eqvio/mathematical/VIOGroup.h" 21 | #include "eqvio/mathematical/VIOState.h" 22 | #include "testing_utilities.h" 23 | #include "gtest/gtest.h" 24 | 25 | using namespace Eigen; 26 | using namespace std; 27 | 28 | TEST(VIOLiftTest, Lift) { 29 | vector ids = {0, 1, 2, 3, 4}; 30 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 31 | const VIOState xi0 = randomStateElement(ids); 32 | const IMUVelocity velocity = randomVelocityElement(); 33 | 34 | // Check the convergence of the derivative 35 | double previousDist = 1e8; 36 | for (int i = 0; i < 8; ++i) { 37 | // Integrate the system normally 38 | const double dt = pow(10.0, -i); 39 | const VIOState xi1 = integrateSystemFunction(xi0, velocity, dt); 40 | 41 | // Lift the velocity and apply as a group action 42 | const VIOAlgebra lambda = liftVelocity(xi0, velocity); 43 | const VIOGroup lambdaExp = VIOExp(dt * lambda); 44 | const VIOState xi2 = stateGroupAction(lambdaExp, xi0); 45 | 46 | // Check the error has decreased 47 | const double diffDist = stateDistance(xi1, xi2) / dt; 48 | EXPECT_LE(diffDist, previousDist); 49 | previousDist = diffDist; 50 | } 51 | } 52 | } 53 | 54 | TEST(VIOLiftTest, DiscreteLift) { 55 | vector ids = {0, 1, 2, 3, 4}; 56 | const double dt = 0.1; 57 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 58 | const VIOState xi0 = randomStateElement(ids); 59 | const IMUVelocity velocity = randomVelocityElement(); 60 | 61 | // Check the discrete result 62 | const VIOState xi1 = integrateSystemFunction(xi0, velocity, dt); 63 | 64 | const VIOGroup X = liftVelocityDiscrete(xi0, velocity, dt); 65 | const VIOState xi2 = stateGroupAction(X, xi0); 66 | 67 | const double dist12 = stateDistance(xi1, xi2); 68 | EXPECT_LE(dist12, NEAR_ZERO); 69 | } 70 | } 71 | 72 | void innovationLift_test(const EqFCoordinateSuite& coordinateSuite) { 73 | vector ids = {0, 1, 2, 3, 4}; 74 | const int N = ids.size(); 75 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 76 | const VIOState xi0 = randomStateElement(ids); 77 | const VIOGroup X = randomGroupElement(ids); 78 | MatrixXd Sigma = MatrixXd::Random(xi0.Dim(), xi0.Dim()); 79 | Sigma = Sigma * Sigma.transpose(); // Makes Sigma positive (semi) definite 80 | 81 | auto reprojectionFunc = [&](const VectorXd& eps) { 82 | const VIOAlgebra liftedInnovation = coordinateSuite.liftInnovation(eps, xi0); 83 | const VIOGroup Delta = VIOExp(liftedInnovation); 84 | const VIOState xi1 = stateGroupAction(Delta, xi0); 85 | const VectorXd reprojectedInnovation = coordinateSuite.stateChart(xi1, xi0); 86 | return reprojectedInnovation; 87 | }; 88 | 89 | testDifferential(reprojectionFunc, VectorXd::Zero(xi0.Dim()), MatrixXd::Identity(xi0.Dim(), xi0.Dim())); 90 | } 91 | } 92 | 93 | void discreteInnovationLift_test(const EqFCoordinateSuite& coordinateSuite) { 94 | vector ids = {0, 1, 2, 3, 4}; 95 | const int N = ids.size(); 96 | for (int testRep = 0; testRep < TEST_REPS; ++testRep) { 97 | const VIOState xi0 = randomStateElement(ids); 98 | const VIOGroup X = randomGroupElement(ids); 99 | MatrixXd Sigma = MatrixXd::Random(xi0.Dim(), xi0.Dim()); 100 | Sigma = Sigma * Sigma.transpose(); // Makes Sigma positive (semi) definite 101 | 102 | auto reprojectionFunc = [&](const VectorXd& eps) { 103 | const VIOGroup discreteInn = coordinateSuite.liftInnovationDiscrete(eps, xi0); 104 | const VIOState xi1 = stateGroupAction(discreteInn, xi0); 105 | const VectorXd reprojectedInnovation = coordinateSuite.stateChart(xi1, xi0); 106 | return reprojectedInnovation; 107 | }; 108 | 109 | // Check that the reprojection function yields identity 110 | for (int j = 0; j < xi0.Dim(); ++j) { 111 | const Eigen::VectorXd ej = Eigen::VectorXd::Unit(xi0.Dim(), j); 112 | assertMatrixEquality(ej, reprojectionFunc(ej)); 113 | } 114 | } 115 | } 116 | 117 | TEST(VIOLiftTest, InnovationLifts_euclid) { 118 | innovationLift_test(EqFCoordinateSuite_euclid); 119 | discreteInnovationLift_test(EqFCoordinateSuite_euclid); 120 | } 121 | 122 | TEST(VIOLiftTest, InnovationLifts_invdepth) { 123 | innovationLift_test(EqFCoordinateSuite_invdepth); 124 | discreteInnovationLift_test(EqFCoordinateSuite_invdepth); 125 | } -------------------------------------------------------------------------------- /test/test_settings.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #include "eqvio/VIOFilterSettings.h" 19 | #include "gtest/gtest.h" 20 | 21 | TEST(TestSettings, ReadTemplateSettings) { 22 | const std::string configTemplateFileName = EQVIO_DEFAULT_CONFIG_FILE; 23 | const YAML::Node configNode = YAML::LoadFile(configTemplateFileName); 24 | 25 | EXPECT_TRUE(configNode["eqf"]); 26 | for (const auto& subnode : configNode["eqf"]) { 27 | std::cout << subnode.first.as() << " "; 28 | } 29 | std::cout << std::endl; 30 | 31 | testing::internal::CaptureStdout(); 32 | VIOFilter::Settings settings(configNode["eqf"]); 33 | const std::string output = testing::internal::GetCapturedStdout(); 34 | EXPECT_STREQ(output.data(), ""); 35 | } -------------------------------------------------------------------------------- /test/testing_utilities.h: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of EqVIO. 3 | 4 | EqVIO is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | EqVIO is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details. 13 | 14 | You should have received a copy of the GNU General Public License 15 | along with EqVIO. If not, see . 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | 22 | #include "eigen3/Eigen/Dense" 23 | #include "eqvio/mathematical/Geometry.h" 24 | #include "eqvio/mathematical/IMUVelocity.h" 25 | #include "eqvio/mathematical/VIOGroup.h" 26 | #include "eqvio/mathematical/VIOState.h" 27 | #include "gtest/gtest.h" 28 | 29 | GIFT::GICameraPtr createDefaultCamera(); 30 | 31 | VIOGroup randomGroupElement(const std::vector& ids); 32 | VIOGroup reasonableGroupElement(const std::vector& ids); 33 | VIOState randomStateElement(const std::vector& ids); 34 | VIOState reasonableStateElement(const std::vector& ids); 35 | IMUVelocity randomVelocityElement(); 36 | Eigen::VectorXd stateVecDiff(const VIOState& xi1, const VIOState& xi2); 37 | double logNorm(const VIOGroup& X); 38 | 39 | VisionMeasurement randomVisionMeasurement(const std::vector& ids); 40 | double stateDistance(const VIOState& xi1, const VIOState& xi2); 41 | double measurementDistance(const VisionMeasurement& y1, const VisionMeasurement& y2); 42 | 43 | void assertMatrixEquality(const Eigen::MatrixXd& M1, const Eigen::MatrixXd& M2, double h = -1.0); 44 | 45 | template 46 | void testDifferential(const F& f, const Eigen::VectorXd& x, const Eigen::MatrixXd& Df, double h = -1.0) { 47 | // Check that each partial derivative is correct 48 | if (h < 0) { 49 | h = std::cbrt(std::numeric_limits::epsilon()); 50 | } 51 | Eigen::MatrixXd numericalDf = numericalDifferential(f, x, h); 52 | assertMatrixEquality(Df, numericalDf, h); 53 | } 54 | 55 | template 56 | std::vector weightedResample(const std::vector& samples, const std::vector& weights) { 57 | static std::mt19937 rng = std::mt19937(0); 58 | static std::uniform_real_distribution dist{0, 1}; 59 | assert(samples.size() == weights.size()); 60 | const int N = samples.size(); 61 | std::vector resamples(N); 62 | 63 | int j = 0; 64 | double totalWeight = weights.at(0); 65 | for (int k = 0; k < N; ++k) { 66 | const double nextThreshold = (dist(rng) + k) / N; 67 | while (totalWeight < nextThreshold && j + 1 < N) { 68 | ++j; 69 | totalWeight += weights.at(j); 70 | } 71 | resamples.at(k) = samples.at(j); 72 | } 73 | 74 | return resamples; 75 | } --------------------------------------------------------------------------------