├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.MD ├── cmake └── FindSuiteSparse.cmake ├── config ├── Alaska │ ├── 3_29 │ │ ├── 47_seconds.png │ │ └── dpio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ ├── 3_30 │ │ └── dpio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ └── alaska.urdf.xacro ├── GLRC │ ├── 3_13 │ │ ├── dpio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ │ ├── dpvio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ │ ├── dvio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ │ ├── pvio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ │ └── vio │ │ │ ├── image.yaml │ │ │ ├── init.yaml │ │ │ ├── prior.yaml │ │ │ └── system.yaml │ └── glrc.urdf ├── euroc │ ├── MH_02_easy │ │ ├── image.yaml │ │ ├── init.yaml │ │ ├── prior.yaml │ │ └── system.yaml │ └── MH_03_medium │ │ ├── image.yaml │ │ ├── init.yaml │ │ ├── prior.yaml │ │ └── system.yaml └── sim │ └── udel_gore │ ├── image.yaml │ ├── init.yaml │ ├── prior.yaml │ └── system.yaml ├── include ├── feature │ ├── Feature.cpp │ ├── Feature.h │ ├── FeatureDatabase.h │ ├── triangulation.cpp │ └── triangulation.h ├── initializer │ ├── initializer.h │ ├── initializer_dvl_aided.cpp │ ├── initializer_dvl_aided.h │ ├── initializer_setting.cpp │ └── initializer_setting.h ├── manager │ ├── msckf_manager.cpp │ └── msckf_manager.h ├── msckf │ ├── predictor.cpp │ ├── predictor.h │ ├── state.cpp │ ├── state.h │ ├── updater.cpp │ └── updater.h ├── tracker │ ├── Grider_FAST.h │ ├── Grider_STC.h │ ├── TrackBase.cpp │ ├── TrackBase.h │ ├── TrackFeature.cpp │ ├── TrackFeature.h │ ├── TrackKLT.cpp │ └── TrackKLT.h ├── types │ ├── msgs.h │ ├── parameters.h │ ├── poseJPL.h │ ├── quatJPL.h │ ├── type.h │ ├── type_all.h │ └── vec.h └── utils │ ├── colors.h │ ├── lambda_body.h │ ├── rapidcsv.h │ ├── recorder.h │ ├── time_cost.h │ └── utils.h ├── launch ├── Alaska │ ├── alaska_3_29_dpio.launch │ ├── alaska_3_30_dpio.launch │ ├── helper_alaska.launch │ ├── rviz.rviz │ └── urdf_alaska.launch ├── GLRC │ ├── glrc_3_13_dpio.launch │ ├── glrc_3_13_dpvio.launch │ ├── glrc_3_13_dvio.launch │ ├── glrc_3_13_pvio.launch │ ├── glrc_3_13_vio.launch │ ├── helper_glrc.launch │ ├── rviz.rviz │ └── urdf_glrc.launch ├── euroc │ ├── euroc_vio.launch │ ├── helper_euroc.launch │ └── rviz.rviz └── sim │ ├── helper_sim.launch │ ├── rviz.rviz │ └── sim_udel_gore.launch ├── media └── glrc_3_13.gif ├── package.xml └── ros ├── node.cpp ├── node.h ├── visualizer.cpp └── visualizer.h /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.toptal.com/developers/gitignore/api/clion,ros,pycharm,visualstudiocode,linux,vim,cmake,macos 3 | # Edit at https://www.toptal.com/developers/gitignore?templates=clion,ros,pycharm,visualstudiocode,linux,vim,cmake,macos 4 | 5 | ### CLion ### 6 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 7 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 8 | 9 | .idea 10 | 11 | # CMake 12 | cmake-build-*/ 13 | 14 | # File-based project format 15 | *.iws 16 | 17 | # IntelliJ 18 | out/ 19 | 20 | # mpeltonen/sbt-idea plugin 21 | .idea_modules/ 22 | 23 | # JIRA plugin 24 | atlassian-ide-plugin.xml 25 | 26 | # Cursive Clojure plugin 27 | .idea/replstate.xml 28 | 29 | # Crashlytics plugin (for Android Studio and IntelliJ) 30 | com_crashlytics_export_strings.xml 31 | crashlytics.properties 32 | crashlytics-build.properties 33 | fabric.properties 34 | 35 | 36 | ### CMake ### 37 | CMakeLists.txt.user 38 | CMakeCache.txt 39 | CMakeFiles 40 | CMakeScripts 41 | Testing 42 | Makefile 43 | cmake_install.cmake 44 | install_manifest.txt 45 | compile_commands.json 46 | CTestTestfile.cmake 47 | _deps 48 | 49 | ### CMake Patch ### 50 | # External projects 51 | *-prefix/ 52 | 53 | ### Linux ### 54 | *~ 55 | 56 | # temporary files which can be created if a process still has a handle open of a deleted file 57 | .fuse_hidden* 58 | 59 | # KDE directory preferences 60 | .directory 61 | 62 | # Linux trash folder which might appear on any partition or disk 63 | .Trash-* 64 | 65 | # .nfs files are created when an open file is removed but is still being accessed 66 | .nfs* 67 | 68 | ### macOS ### 69 | # General 70 | .DS_Store 71 | .AppleDouble 72 | .LSOverride 73 | 74 | # Icon must end with two \r 75 | Icon 76 | 77 | 78 | # Thumbnails 79 | ._* 80 | 81 | # Files that might appear in the root of a volume 82 | .DocumentRevisions-V100 83 | .fseventsd 84 | .Spotlight-V100 85 | .TemporaryItems 86 | .Trashes 87 | .VolumeIcon.icns 88 | .com.apple.timemachine.donotpresent 89 | 90 | # Directories potentially created on remote AFP share 91 | .AppleDB 92 | .AppleDesktop 93 | Network Trash Folder 94 | Temporary Items 95 | .apdisk 96 | 97 | ### PyCharm ### 98 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio, WebStorm and Rider 99 | # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 100 | 101 | # User-specific stuff 102 | 103 | # AWS User-specific 104 | 105 | # Generated files 106 | 107 | # Sensitive or high-churn files 108 | 109 | # Gradle 110 | 111 | # Gradle and Maven with auto-import 112 | # When using Gradle or Maven with auto-import, you should exclude module files, 113 | # since they will be recreated, and may cause churn. Uncomment if using 114 | # auto-import. 115 | # .idea/artifacts 116 | # .idea/compiler.xml 117 | # .idea/jarRepositories.xml 118 | # .idea/modules.xml 119 | # .idea/*.iml 120 | # .idea/modules 121 | # *.iml 122 | # *.ipr 123 | 124 | # CMake 125 | 126 | # Mongo Explorer plugin 127 | 128 | # File-based project format 129 | 130 | # IntelliJ 131 | 132 | # mpeltonen/sbt-idea plugin 133 | 134 | # JIRA plugin 135 | 136 | # Cursive Clojure plugin 137 | 138 | # Crashlytics plugin (for Android Studio and IntelliJ) 139 | 140 | # Editor-based Rest Client 141 | 142 | # Android studio 3.1+ serialized cache file 143 | 144 | ### PyCharm Patch ### 145 | # Comment Reason: https://github.com/joeblau/gitignore.io/issues/186#issuecomment-215987721 146 | 147 | # *.iml 148 | # modules.xml 149 | # .idea/misc.xml 150 | # *.ipr 151 | 152 | # Sonarlint plugin 153 | # https://plugins.jetbrains.com/plugin/7973-sonarlint 154 | 155 | # SonarQube Plugin 156 | # https://plugins.jetbrains.com/plugin/7238-sonarqube-community-plugin 157 | 158 | # Markdown Navigator plugin 159 | # https://plugins.jetbrains.com/plugin/7896-markdown-navigator-enhanced 160 | 161 | # Cache file creation bug 162 | # See https://youtrack.jetbrains.com/issue/JBR-2257 163 | 164 | # CodeStream plugin 165 | # https://plugins.jetbrains.com/plugin/12206-codestream 166 | 167 | ### ROS ### 168 | devel/ 169 | logs/ 170 | build/ 171 | bin/ 172 | lib/ 173 | msg_gen/ 174 | srv_gen/ 175 | msg/*Action.msg 176 | msg/*ActionFeedback.msg 177 | msg/*ActionGoal.msg 178 | msg/*ActionResult.msg 179 | msg/*Feedback.msg 180 | msg/*Goal.msg 181 | msg/*Result.msg 182 | msg/_*.py 183 | build_isolated/ 184 | devel_isolated/ 185 | 186 | # Generated by dynamic reconfigure 187 | *.cfgc 188 | /cfg/cpp/ 189 | /cfg/*.py 190 | 191 | # Ignore generated docs 192 | *.dox 193 | *.wikidoc 194 | 195 | # eclipse stuff 196 | .project 197 | .cproject 198 | 199 | # qcreator stuff 200 | 201 | srv/_*.py 202 | *.pcd 203 | *.pyc 204 | qtcreator-* 205 | *.user 206 | 207 | /planning/cfg 208 | /planning/docs 209 | /planning/src 210 | 211 | 212 | # Emacs 213 | .#* 214 | 215 | # Catkin custom files 216 | CATKIN_IGNORE 217 | 218 | ### Vim ### 219 | # Swap 220 | [._]*.s[a-v][a-z] 221 | !*.svg # comment out if you don't need vector files 222 | [._]*.sw[a-p] 223 | [._]s[a-rt-v][a-z] 224 | [._]ss[a-gi-z] 225 | [._]sw[a-p] 226 | 227 | # Session 228 | Session.vim 229 | Sessionx.vim 230 | 231 | # Temporary 232 | .netrwhist 233 | # Auto-generated tag files 234 | tags 235 | # Persistent undo 236 | [._]*.un~ 237 | 238 | ### VisualStudioCode ### 239 | .vscode/* 240 | !.vscode/settings.json 241 | !.vscode/tasks.json 242 | !.vscode/launch.json 243 | !.vscode/extensions.json 244 | *.code-workspace 245 | 246 | # Local History for Visual Studio Code 247 | .history/ 248 | 249 | ### VisualStudioCode Patch ### 250 | # Ignore all local history of files 251 | .history 252 | .ionide 253 | .cmake-build* 254 | 255 | .vscode 256 | 257 | .build 258 | 259 | docs/_build 260 | 261 | # End of https://www.toptal.com/developers/gitignore/api/clion,ros,pycharm,visualstudiocode,linux,vim,cmake,macos 262 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(msckf_dvio) 3 | 4 | ## Compile as C++14, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++17) 6 | 7 | ## CMAKE module path 8 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") 9 | 10 | ### Find Library 11 | # SET(OpenCV_DIR /home/lin/develop/3rd/opencv/install/opencv-4.2.0/lib/cmake/opencv4/) 12 | find_package(OpenCV 4.2.0 REQUIRED) 13 | message(STATUS "OpenCV library status:") 14 | message(STATUS " config: ${OpenCV_DIR}") 15 | message(STATUS " version: ${OpenCV_VERSION}") 16 | message(STATUS " libraries: ${OpenCV_LIBS}") 17 | message(STATUS " include path: ${OpenCV_INCLUDE_DIRS}") 18 | 19 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 20 | find_package(Eigen3 REQUIRED) 21 | find_package(PCL REQUIRED) 22 | find_package(SuiteSparse REQUIRED) 23 | find_package(magic_enum CONFIG REQUIRED) 24 | 25 | ### Find ROS package 26 | # SET(cv_bridge_DIR /home/lin/develop/ros/ros_ws/devel/share/cv_bridge/cmake/) 27 | find_package(catkin REQUIRED COMPONENTS 28 | roscpp rospy std_msgs rosbag sensor_msgs nav_msgs 29 | geometry_msgs cv_bridge message_generation tf2 30 | tf_conversions eigen_conversions std_srvs 31 | dynamic_reconfigure message_filters image_transport 32 | tf2_ros tf2_geometry_msgs pcl_conversions 33 | ) 34 | 35 | ################################### 36 | ## catkin specific configuration ## 37 | ################################### 38 | 39 | catkin_package( 40 | INCLUDE_DIRS include 41 | CATKIN_DEPENDS roscpp rospy std_msgs rosbag sensor_msgs cv_bridge dynamic_reconfigure 42 | geometry_msgs message_runtime tf_conversions eigen_conversions 43 | message_filters image_transport visualization_msgs tf nav_msgs tf2 std_srvs 44 | tf2_ros tf2_geometry_msgs pcl_conversions 45 | DEPENDS PCL EIGEN3 SUITESPARSE 46 | ) 47 | 48 | ########### 49 | ## Build ## 50 | ########### 51 | 52 | ## Specify additional locations of header files 53 | include_directories( 54 | include 55 | ${catkin_INCLUDE_DIRS} 56 | ${Boost_INCLUDE_DIR} 57 | ${EIGEN3_INCLUDE_DIR} 58 | ${OpenCV_INCLUDE_DIRS} 59 | ${PCL_INCLUDE_DIRS} 60 | ${SUITESPARSE_INCLUDE_DIRS} 61 | ) 62 | 63 | list(APPEND thirdparty_libraries 64 | ${catkin_LIBRARIES} 65 | ${Boost_LIBRARIES} 66 | ${OpenCV_LIBRARIES} 67 | ${PCL_LIBRARIES} 68 | ${SUITESPARSE_LIBRARIES} 69 | magic_enum::magic_enum 70 | ) 71 | 72 | ########################################## 73 | ############### build lib ############### 74 | ########################################## 75 | add_library(dvio_lib SHARED 76 | 77 | include/initializer/initializer_dvl_aided.cpp 78 | include/initializer/initializer_setting.cpp 79 | 80 | include/manager/msckf_manager.cpp 81 | 82 | include/msckf/state.cpp 83 | include/msckf/predictor.cpp 84 | include/msckf/updater.cpp 85 | 86 | include/feature/Feature.cpp 87 | include/feature/triangulation.cpp 88 | 89 | include/tracker/TrackBase.cpp 90 | include/tracker/TrackKLT.cpp 91 | include/tracker/TrackFeature.cpp 92 | ) 93 | target_link_libraries(dvio_lib ${thirdparty_libraries}) 94 | 95 | 96 | ########################################## 97 | ############### build ros ################ 98 | ########################################## 99 | 100 | add_executable(dvio_node ros/node.cpp ros/visualizer.cpp) 101 | target_link_libraries(dvio_node dvio_lib ${thirdparty_libraries}) 102 | 103 | ########################################## 104 | ############### build test ############### 105 | ########################################## 106 | -------------------------------------------------------------------------------- /README.MD: -------------------------------------------------------------------------------- 1 | # MSCKF-DVIO 2 | 3 | **Tightly-coupled Visual-DVL-Inertial Odometry for Robot-based Ice-water Boundary Exploration** 4 | 5 | This is the repo for MSCKF-DVIO, a multi-sensor fused odometry using IMU, DVL, Pressure Sensor and Mono camera. The implementation is based on the state-of-the art [MSCKF](https://ieeexplore.ieee.org/document/4209642) framwork and tested in our underice dataset. 6 | 7 |

8 | 9 |

10 | 11 | ## Installation: 12 | 13 | ### Dependencies: 14 | - ROS noetic 15 | - OpenCV 4.2: `sudo apt install libopencv-dev python3-opencv` or build from source 16 | - Boost: `sudo apt-get install libboost-dev libboost-filesystem-dev` 17 | - Eigen3: `sudo apt-get install libeigen3-dev` 18 | - PCL: `sudo apt install libpcl-dev` 19 | - SuiteSparse: `sudo apt-get install libsuitesparse-dev` 20 | - [magic_enum](https://github.com/Neargye/magic_enum): 21 | ``` 22 | cd ~/your_path/magic_enum 23 | mkdir build && cd build 24 | sudo make install 25 | ``` 26 | - [rapidcsv](https://github.com/d99kris/rapidcsv): alrady exist in the header, no need anything else 27 | 28 | ### Build: 29 | ```shell 30 | # download 31 | $ git clone https://github.com/GSO-soslab/msckf_dvio 32 | # ros dependencies 33 | $ rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -y 34 | # build 35 | $ catkin build msckf_dvio -DCMAKE_BUILD_TYPE=Release 36 | ``` 37 | 38 | ## Usage: 39 | ```shell 40 | ##### make sure change the your dataset path ##### 41 | 42 | # run DVL-Pressure-Visual-IMU 43 | $ roslaunch msckf_dvio glrc_3_13_dpvio.launch 44 | # run DVL-Pressure-IMU 45 | $ roslaunch msckf_dvio glrc_3_13_dpio.launch 46 | # run EuRoC VIO (the very basic MSCKF VIO but with keyfram option) 47 | $ roslaunch msckf_dvio euroc_vio.launch 48 | 49 | ##### feel free to test your own dataset by configuring the yaml 50 | ##### you can set the system initialization parameters 51 | ``` 52 | ## Dataset: 53 | The underice dataset is available on request, please contact us for more information. 54 | Our algorithm accepts following standard ROS message type: 55 | - IMU data: `sensor_msgs/Imu` 56 | - DVL bottom track velocity: `geometry_msgs/TwistWithCovarianceStamped` 57 | - DVL pressure measurement: `sensor_msgs/FluidPressure` 58 | - DVL pointcloud: `sensor_msgs/PointCloud2` 59 | - Camera image: `sensor_msgs/Image` 60 | 61 | 62 | 63 | ## Citation 64 | If you find our code or paper useful, please cite 65 | 66 | ```bibtex 67 | @inproceedings{MSCKF-DVIO_Zhao_IROS2023, 68 | author = {Zhao, Lin and Zhou, Mingxi and Loose, Brice}, 69 | title = {Tightly-coupled Visual-{DVL}-Inertial Odometry for Robot-based Ice-water Boundary Exploration}, 70 | booktitle = {2023 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 71 | year = {2023}, 72 | note = {Accepted} 73 | } 74 | ``` 75 | 76 | ## Acknowledgement 77 | 78 | This work is supported by the National Science Foundation (NSF) under the award [#1945924](https://www.nsf.gov/awardsearch/showAward?AWD_ID=1945924), and the Graduate School of Oceanography, University of Rhode Island. We also thank the field support from the Great Lake Research Center, Michigan Technological University 79 | 80 | We adapted parts of code from [OpenVINS](https://github.com/rpng/open_vins), we want to thank them for releaseing the code for the public. If you think our code or paper is useful, plesae also cite the OpenVINS paper. -------------------------------------------------------------------------------- /config/Alaska/3_29/47_seconds.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSO-soslab/msckf_dvio/709bc6f63a84f1eb40964b30a03a15bfb59a6584/config/Alaska/3_29/47_seconds.png -------------------------------------------------------------------------------- /config/Alaska/3_29/dpio/image.yaml: -------------------------------------------------------------------------------- 1 | #### Camera feature tracking parameters 2 | 3 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 4 | # FRONT_END: 5 | TRACK: 6 | mode: TRACK_NONE 7 | num_aruco: 1024 8 | downsize_aruco: false 9 | use_stereo: false 10 | max_camera: 1 11 | cam_id: 0 12 | downsample_ratio: 0.5 13 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 14 | img_enhancement: 1 -------------------------------------------------------------------------------- /config/Alaska/3_29/dpio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | ## This is the parameter must given 6 | ## Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | ## the given MODE shoud match to setting below 8 | # INIT_MODE: INIT_DVL_PRESSURE 9 | INIT_MODE: INIT_SETTING 10 | 11 | ############################################################################### 12 | INIT_SETTING: 13 | # # Example: 14 | # SENSOR: 15 | # # the timestamp that this sensor initialized 16 | # time: 1614971124.344753027 17 | # # initialized timeoffset 18 | # temporal: [0.01] 19 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 20 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 21 | # # initialized static transfomration between IMU and this sensor 22 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 23 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 24 | # intrinsic: [1.0, 2.0, 3.0] 25 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 26 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 27 | 28 | IMU: 29 | time: 1648581892.013999939 30 | # brief: orientation in quaterion form 31 | state: [ -0.997400,-0.002532,-0.044766,0.056422, 32 | 0.0000, 0.0000, 0.0000, 33 | -0.017969,0.010321,-0.019632, 34 | 0.01,0.01,-0.01, 35 | -0.003950,-0.004107,-0.016223] 36 | 37 | DVL: 38 | time: 1648581892.113800764 39 | temporal: [-0.095595598] 40 | 41 | PRESSURE: 42 | time: 1648581892.113800764 43 | global: [3.251000] 44 | 45 | 46 | ############################################################################### 47 | # INIT_STATIC: 48 | # todo: 0.0 49 | 50 | ############################################################################### 51 | INIT_DVL_PRESSURE: 52 | # Default: 100hz IMU -> 10; 200hz IMU -> 20 53 | # TODO: replace with imu_time_duration: 0.1s 54 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 55 | imu_window: 20 56 | 57 | # Default: 0.2 58 | # Note: the IMU variance threshold that indicates IMU jump inclued 59 | imu_var: 0.3 60 | 61 | # Default: 0.07 62 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 63 | imu_delta: 0.2 64 | 65 | # Default: 5 66 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 67 | dvl_window: 4 68 | 69 | # Default: 0.05 70 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 71 | dvl_delta: 0.05 72 | 73 | # Default: 1s 74 | #Note: how many second selected for initialization 75 | dvl_init_duration: 2 76 | 77 | 78 | ############################################################################### 79 | # INIT_CAMERA: 80 | # todo: 0.0 81 | 82 | 83 | ############################################################################### 84 | # INIT_DVL_CAMERA: 85 | # todo: 0.0 86 | -------------------------------------------------------------------------------- /config/Alaska/3_29/dpio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; 6 | # gravity in the place your robot deployed 7 | # The North Alaska from E. Thiel et al. GRAVITY MEASUREMENTS IN ALASKA 8 | gravity: 9.822437 9 | 10 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 11 | # Default: 1.926e-04 from datasheet 12 | accelerometer_noise_density: 4.2e-04 13 | 14 | # Note: Bias random walk from kalibr_allen 15 | accelerometer_random_walk: 2.857e-05 16 | 17 | # Note: Noise density (continuous-time) from kalibr_allen 18 | # Default: 8.7e-05 from datasheet 19 | gyroscope_noise_density: 8.014e-05 20 | 21 | # Note: Bias random walk from kalibr_allen 22 | gyroscope_random_walk: 0.534e-05 23 | 24 | ##################################################################################### 25 | ##### DVL ##### 26 | ##################################################################################### 27 | DVL: 28 | # imu is reference frame 29 | T_I_D: 30 | - [ -1.0, 0.0, 0.0, 0.470] 31 | - [ 0.0, 1.0, 0.0, 0.106] 32 | - [ 0.0, 0.0, -1.0, -0.321] 33 | - [ 0.0, 0.0, 0.0, 1.000] 34 | # Default: -0.067 from IO-SYNC time difference 35 | # Note: Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 36 | # TODO: this value will be overide by IMU-DVL alignment 37 | timeoffset_I_D: 0.0 38 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 39 | scale: 1.0 40 | # noise on DVL tottom track 3-axis velocity measurement 41 | noise_bt: [0.1, 0.1, 0.1] 42 | 43 | 44 | ##################################################################################### 45 | ##### Pressure ##### 46 | ##################################################################################### 47 | PRESSURE: 48 | # Note: the angle that the actual mounting position rotate to the standing position, 49 | # used to transfer pressure measurement into DVL frame's Z 50 | # Default: 0: x-forward, up-looking; PI: x-forward, down-looking; -PI/2: x-forward, right-looking; PI/2: x-forward, left-looking 51 | mount_angle: 0 52 | # noise for pressure update 53 | # default: 0.01 54 | noise_pressure: 0.1 55 | 56 | ##################################################################################### 57 | ##################################################################################### 58 | 59 | ## Secondary camera 60 | # CAM0: 61 | # # TODO: this tf should be not as same as GLRC, take a look at the calibration 62 | # T_C_I: 63 | # - [ 0.007862367268, 0.999953212142, 0.005635309016, -0.123220672745] 64 | # - [ 0.999968129426, -0.007854419126, -0.001431164675, -0.310185995908] 65 | # - [-0.00138683563, 0.005646381758, -0.999983097387, -0.278121515480] 66 | # - [ 0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000] 67 | 68 | # # radial-tangential[k1,k2,r1,r2] 69 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 70 | # # f_x, f_y, c_x, c_y 71 | # intrinsics: [1845.542490218492,1846.046949112978, 825.6913274592704,605.8504151895138] 72 | # resolution: [1616, 1240] 73 | # camera_model: pinhole 74 | # distortion_model: radtan 75 | # # Default: -0.0558 from IO-SYNC time difference 76 | # timeoffset_I_C: 0 77 | 78 | ##################################################################################### 79 | ##################################################################################### 80 | 81 | ## Primary camera 82 | # CAM1: 83 | 84 | -------------------------------------------------------------------------------- /config/Alaska/3_29/dpio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: 20; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing ( keep it as camera fequency) 8 | backend_hz: 30 9 | # sensors will be fused 10 | sensors: [IMU, DVL, PRESSURE] 11 | # rostopics used for each sensor 12 | topics: 13 | - {IMU: /rov/sensors/ahrs/imu/data} 14 | - {DVL: /rov/sensors/dvl/bottom_track_velocity} 15 | - {PRESSURE: /rov/sensors/dvl/pressure} 16 | # - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 17 | 18 | buffers: 19 | # DVL cloud buffer to store how many seconds 20 | - {DVL_CLOUD: 10} 21 | 22 | #################################################################################### 23 | ############################ MSCKF estimation setup ########################## 24 | #################################################################################### 25 | 26 | #### MSCKF parameters 27 | MSCKF: 28 | 29 | ################################## DVL ################################### 30 | 31 | # enable DVL exterisic rotation calibration 32 | dvl_exterisic_R: false 33 | # enable DVL exterisic translation calibration 34 | dvl_exterisic_p: false 35 | # enable DVL time offset calibration 36 | dvl_timeoffset: false 37 | # enable DVL scale calibration 38 | dvl_scale: false -------------------------------------------------------------------------------- /config/Alaska/3_30/dpio/image.yaml: -------------------------------------------------------------------------------- 1 | #### Camera feature tracking parameters 2 | 3 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 4 | # FRONT_END: 5 | TRACK: 6 | mode: TRACK_NONE 7 | num_aruco: 1024 8 | downsize_aruco: false 9 | use_stereo: false 10 | max_camera: 1 11 | cam_id: 0 12 | downsample_ratio: 0.5 13 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 14 | img_enhancement: 1 -------------------------------------------------------------------------------- /config/Alaska/3_30/dpio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | INIT_SETTING: 13 | # # Example: 14 | # SENSOR: 15 | # # the timestamp that this sensor initialized 16 | # time: 1614971124.344753027 17 | # # initialized timeoffset 18 | # temporal: [0.01] 19 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 20 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 21 | # # initialized static transfomration between IMU and this sensor 22 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 23 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 24 | # intrinsic: [1.0, 2.0, 3.0] 25 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 26 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 27 | 28 | IMU: 29 | time: 1648667257.549000025 30 | # brief: orientation in quaterion form 31 | state: [ -0.998042,-0.000759,-0.061302,0.012362, 32 | 0.0000, 0.0000, 0.0000, 33 | -0.047861,-0.006861,-0.026561, 34 | -0.007398,-0.001765,-0.022810, 35 | -0.000676,-0.000716,-0.009552] 36 | 37 | DVL: 38 | time: 1648667257.677178383 39 | # the timeoffset should not large like this ? 40 | temporal: [-0.124145746] 41 | 42 | PRESSURE: 43 | time: 1648667257.677178383 44 | global: [3.908000] 45 | 46 | 47 | ############################################################################### 48 | # INIT_STATIC: 49 | # todo: 0.0 50 | 51 | ############################################################################### 52 | INIT_DVL_PRESSURE: 53 | # Default: 100hz IMU -> 10; 200hz IMU -> 20 54 | # TODO: replace with imu_time_duration: 0.1s 55 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 56 | imu_window: 20 57 | 58 | # Default: 0.2 59 | # Note: the IMU variance threshold that indicates IMU jump inclued 60 | imu_var: 0.15 61 | 62 | # Default: 0.07 63 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 64 | imu_delta: 0.2 65 | 66 | # Default: 5 67 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 68 | dvl_window: 4 69 | 70 | # Default: 0.05 71 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 72 | dvl_delta: 0.05 73 | 74 | # Default: 1s 75 | #Note: how many second selected for initialization 76 | dvl_init_duration: 2 77 | 78 | 79 | ############################################################################### 80 | # INIT_CAMERA: 81 | # todo: 0.0 82 | 83 | 84 | ############################################################################### 85 | # INIT_DVL_CAMERA: 86 | # todo: 0.0 87 | -------------------------------------------------------------------------------- /config/Alaska/3_30/dpio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; 6 | # gravity in the place your robot deployed 7 | # The North Alaska from E. Thiel et al. GRAVITY MEASUREMENTS IN ALASKA 8 | gravity: 9.822437 9 | 10 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 11 | # Default: 1.926e-04 from datasheet 12 | accelerometer_noise_density: 4.2e-04 13 | 14 | # Note: Bias random walk from kalibr_allen 15 | accelerometer_random_walk: 2.857e-05 16 | 17 | # Note: Noise density (continuous-time) from kalibr_allen 18 | # Default: 8.7e-05 from datasheet 19 | gyroscope_noise_density: 8.014e-05 20 | 21 | # Note: Bias random walk from kalibr_allen 22 | gyroscope_random_walk: 0.534e-05 23 | 24 | ##################################################################################### 25 | ##### DVL ##### 26 | ##################################################################################### 27 | DVL: 28 | # imu is reference frame 29 | T_I_D: 30 | - [ -1.0, 0.0, 0.0, 0.470] 31 | - [ 0.0, 1.0, 0.0, 0.106] 32 | - [ 0.0, 0.0, -1.0, -0.321] 33 | - [ 0.0, 0.0, 0.0, 1.000] 34 | # Default: -0.067 from IO-SYNC time difference 35 | # Note: Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 36 | # TODO: this value will be overide by IMU-DVL alignment 37 | timeoffset_I_D: 0.0 38 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 39 | scale: 1.0 40 | # noise on DVL tottom track 3-axis velocity measurement 41 | noise_bt: [0.1, 0.1, 0.1] 42 | 43 | 44 | ##################################################################################### 45 | ##### Pressure ##### 46 | ##################################################################################### 47 | PRESSURE: 48 | # Note: the angle that the actual mounting position rotate to the standing position, 49 | # used to transfer pressure measurement into DVL frame's Z 50 | # Default: 0: x-forward, up-looking; PI: x-forward, down-looking; -PI/2: x-forward, right-looking; PI/2: x-forward, left-looking 51 | mount_angle: 0 52 | # noise for pressure update 53 | # default: 0.01 54 | noise_pressure: 0.1 55 | 56 | ##################################################################################### 57 | ##################################################################################### 58 | 59 | ## Secondary camera 60 | # CAM0: 61 | # # TODO: this tf should be not as same as GLRC, take a look at the calibration 62 | # T_C_I: 63 | # - [ 0.007862367268, 0.999953212142, 0.005635309016, -0.123220672745] 64 | # - [ 0.999968129426, -0.007854419126, -0.001431164675, -0.310185995908] 65 | # - [-0.00138683563, 0.005646381758, -0.999983097387, -0.278121515480] 66 | # - [ 0.000000000000, 0.000000000000, 0.000000000000, 1.000000000000] 67 | 68 | # # radial-tangential[k1,k2,r1,r2] 69 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 70 | # # f_x, f_y, c_x, c_y 71 | # intrinsics: [1845.542490218492,1846.046949112978, 825.6913274592704,605.8504151895138] 72 | # resolution: [1616, 1240] 73 | # camera_model: pinhole 74 | # distortion_model: radtan 75 | # # Default: -0.0558 from IO-SYNC time difference 76 | # timeoffset_I_C: 0 77 | 78 | ##################################################################################### 79 | ##################################################################################### 80 | 81 | ## Primary camera 82 | # CAM1: 83 | 84 | -------------------------------------------------------------------------------- /config/Alaska/3_30/dpio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: 20; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing ( keep it as camera fequency) 8 | backend_hz: 30 9 | # sensors will be fused 10 | sensors: [IMU, DVL, PRESSURE] 11 | # rostopics used for each sensor 12 | topics: 13 | - {IMU: /rov/sensors/ahrs/imu/data} 14 | - {DVL: /rov/sensors/dvl/bottom_track_velocity} 15 | - {PRESSURE: /rov/sensors/dvl/pressure} 16 | # - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 17 | 18 | buffers: 19 | # DVL cloud buffer to store how many seconds 20 | - {DVL_CLOUD: 10} 21 | 22 | #################################################################################### 23 | ############################ TODO: Pre-processing ########################## 24 | #################################################################################### 25 | 26 | # PRE_PROCESS: 27 | # # Default: 2.0 28 | # # Note: use this simply filter bad DVL velocity measurement 29 | # dvl_v_threshold: 2.0 30 | 31 | 32 | #################################################################################### 33 | ############################ MSCKF estimation setup ########################## 34 | #################################################################################### 35 | 36 | #### MSCKF parameters 37 | MSCKF: 38 | 39 | ################################## DVL ################################### 40 | 41 | # enable DVL exterisic rotation calibration 42 | dvl_exterisic_R: false 43 | # enable DVL exterisic translation calibration 44 | dvl_exterisic_p: false 45 | # enable DVL time offset calibration 46 | dvl_timeoffset: false 47 | # enable DVL scale calibration 48 | dvl_scale: false 49 | 50 | ################################ Camera ################################## 51 | 52 | # ## enable camera exterisic rotation calibration 53 | # cam_exterisic_R: false 54 | # ## enable camera exterisic translation calibration 55 | # cam_exterisic_p: false 56 | # ## enable camera time offset calibration 57 | # cam_timeoffset: true 58 | # ## Max camera clone 59 | # ## default: 11 for non-key-frame 60 | # cam_clone: 10 61 | 62 | ################################ Update ################################## 63 | 64 | ## Index of clone need to marginalize 65 | ## example: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 66 | ## the marg clone measurements: oldest, second latest [0,8] 67 | # marg_meas_index: [-1] 68 | 69 | ## brief: marginalized pose at slide-windows index (or clone state index) 70 | ## note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 71 | # marg_pose_index: [-1] 72 | 73 | ## in open_vins(40), but it's occasionally update at 40 74 | ## but here, tracking will stop so always update as 40 75 | # max_msckf_update: 20 76 | 77 | ## max line of residual lines for update, maybe used for futures 78 | # max_update_lines: 1500 79 | 80 | ################################ Marginalization ################################## 81 | -------------------------------------------------------------------------------- /config/Alaska/alaska.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dpio/image.yaml: -------------------------------------------------------------------------------- 1 | #### Camera feature tracking parameters 2 | 3 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 4 | # FRONT_END: 5 | TRACK: 6 | mode: TRACK_NONE 7 | num_aruco: 1024 8 | downsize_aruco: false 9 | use_stereo: false 10 | max_camera: 1 11 | cam_id: 0 12 | downsample_ratio: 0.5 13 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 14 | img_enhancement: 1 -------------------------------------------------------------------------------- /config/GLRC/3_13/dpio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | INIT_SETTING: 13 | # # Example: 14 | # SENSOR: 15 | # # the timestamp that this sensor initialized 16 | # time: 1614971124.344753027 17 | # # initialized timeoffset 18 | # temporal: [0.01] 19 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 20 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 21 | # # initialized static transfomration between IMU and this sensor 22 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 23 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 24 | # intrinsic: [1.0, 2.0, 3.0] 25 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 26 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 27 | 28 | IMU: 29 | time: 1614971124.344753027 30 | # brief: orientation in quaterion form 31 | state: [-0.998573, 0.000816, 0.015993, 0.050947, 32 | 0.0000, 0.0000, 0.0000, 33 | -0.346171, 0.008464, 0.042122, 34 | -0.001849, -0.001689, -0.002033, 35 | # 0.001, 0.001, -0.001, 36 | -0.003053, -0.004853, -0.020769] 37 | 38 | DVL: 39 | time: 1614971124.354732990 40 | temporal: [0.000000238] 41 | 42 | PRESSURE: 43 | time: 1614971124.354732990 44 | global: [4.453799] 45 | 46 | 47 | ############################################################################### 48 | # INIT_STATIC: 49 | # todo: 0.0 50 | 51 | ############################################################################### 52 | INIT_DVL_PRESSURE: 53 | # Default: 10 54 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 55 | imu_window: 10 56 | 57 | # Default: 0.2 58 | # Note: the IMU variance threshold that indicates IMU jump inclued 59 | imu_var: 0.2 60 | 61 | # Default: 0.07 62 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 63 | imu_delta: 0.07 64 | 65 | # Default: 5 66 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 67 | dvl_window: 4 68 | 69 | # Default: 0.05 70 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 71 | dvl_delta: 0.05 72 | 73 | # Default: 1s 74 | #Note: how many second selected for initialization 75 | dvl_init_duration: 1 76 | 77 | 78 | ############################################################################### 79 | # INIT_CAMERA: 80 | # todo: 0.0 81 | 82 | 83 | ############################################################################### 84 | # INIT_DVL_CAMERA: 85 | # todo: 0.0 86 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dpio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; 6 | # gravity in the place your robot deployed 7 | gravity: 9.80733 8 | 9 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 10 | # Default: 1.926e-04 from datasheet 11 | accelerometer_noise_density: 4.2e-04 12 | 13 | # Note: Bias random walk from kalibr_allen 14 | accelerometer_random_walk: 2.857e-05 15 | 16 | # Note: Noise density (continuous-time) from kalibr_allen 17 | # Default: 8.7e-05 from datasheet 18 | gyroscope_noise_density: 8.014e-05 19 | 20 | # Note: Bias random walk from kalibr_allen 21 | gyroscope_random_walk: 0.534e-05 22 | 23 | ##################################################################################### 24 | ##### DVL ##### 25 | ##################################################################################### 26 | DVL: 27 | # imu is reference frame 28 | T_I_D: 29 | - [ -0.999928, 0.0116206, -0.00288846, 0.433828] 30 | - [ 0.0115923, 0.999886, 0.00962664, 0.086180] 31 | - [ 0.003, 0.00959246, -0.999949, -0.302024] 32 | - [ 0.0, 0.0, 0.0, 1.0] 33 | # Default:0.0; Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 34 | # TODO: this value will be overide by IMU-DVL alignment 35 | timeoffset_I_D: 0.0 36 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 37 | scale: 0.944666667 38 | # noise on DVL tottom track 3-axis velocity measurement 39 | # noise_bt: [0.1, 0.15, 0.2] 40 | # noise_bt: [0.05, 0.1, 0.15] 41 | noise_bt: [0.1, 0.1, 0.1] 42 | 43 | 44 | ##################################################################################### 45 | ##### Pressure ##### 46 | ##################################################################################### 47 | PRESSURE: 48 | # Note: the angle that the actual mounting position rotate to the standing position, 49 | # used to transfer pressure measurement into DVL frame's Z 50 | # Default: 0: x-forward, up-looking; PI: x-forward, down-looking; -PI/2: x-forward, right-looking; PI/2: x-forward, left-looking 51 | mount_angle: 0 52 | # noise for pressure update 53 | # default: 0.01 54 | noise_pressure: 0.01 55 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dpio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: 20; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing ( keep it as camera fequency) 8 | backend_hz: 30 9 | # sensors will be fused 10 | sensors: [IMU, DVL, PRESSURE] 11 | # rostopics used for each sensor 12 | topics: 13 | - {IMU: /rov/sensors/ahrs/imu/data} 14 | - {DVL: /rov/sensors/dvl/bottom_track_velocity} 15 | - {PRESSURE: /rov/sensors/dvl/pressure} 16 | 17 | buffers: 18 | # DVL cloud buffer to store how many seconds 19 | - {DVL_CLOUD: 10} 20 | 21 | #################################################################################### 22 | ############################ MSCKF estimation setup ########################## 23 | #################################################################################### 24 | 25 | #### MSCKF parameters 26 | MSCKF: 27 | 28 | ################################## DVL ################################### 29 | 30 | # enable DVL exterisic rotation calibration 31 | dvl_exterisic_R: false 32 | # enable DVL exterisic translation calibration 33 | dvl_exterisic_p: false 34 | # enable DVL time offset calibration 35 | dvl_timeoffset: false 36 | # enable DVL scale calibration 37 | dvl_scale: false -------------------------------------------------------------------------------- /config/GLRC/3_13/dpvio/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | # FRONT_END: 7 | TRACK: 8 | mode: TRACK_KLT 9 | num_aruco: 1024 10 | downsize_aruco: false 11 | use_stereo: false 12 | max_camera: 1 13 | cam_id: 0 14 | downsample_ratio: 0.5 15 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 16 | img_enhancement: 1 17 | 18 | TRACK_KLT: 19 | num_pts: 250 20 | fast_threshold: 15 21 | grid_x: 5 22 | grid_y: 3 23 | min_px_dist: 8 24 | pyram: 3 25 | 26 | 27 | ############################################ 28 | #### Camera feature triangulation parameters #### 29 | ############################################ 30 | Feature: 31 | # Max condition number of linear triangulation matrix accept triangulated features 32 | max_cond_number: 10000 33 | # Minimum distance to accept triangulated features 34 | min_dist: 0.5 35 | # Minimum distance to accept triangulated features 36 | max_dist: 5 37 | # Multiplier to increase/decrease lambda 38 | lam_mult: 10 39 | # Init lambda for Levenberg-Marquardt optimization 40 | init_lamda: 0.001 41 | # Max runs for Levenberg-Marquardt 42 | max_runs: 5 43 | # Max lambda for Levenberg-Marquardt optimization 44 | max_lamda: 1e10 45 | # Cutoff for dx increment to consider as converged 46 | min_dx: 1e-6 47 | # Cutoff for cost decrement to consider as converged 48 | min_dcost: 1e-6 49 | # Max baseline ratio to accept triangulated features 50 | max_baseline: 40 51 | 52 | ############################################ 53 | #### Keyframe Strategy #### 54 | ############################################ 55 | Keyframe: 56 | # option 1: frame count 57 | frame_count: 2 58 | 59 | # option 3: minimum tracked features 60 | min_tracked: 50 61 | 62 | #D=0.1, V_xy = 0.3, ->0.3 63 | #D=0.1, V_xy = 0.4, ->0.25 64 | adaptive_factor: 0.33 65 | # 66 | adaptive_power: 1 67 | 68 | # option 2: relative motion constraint 69 | # default: 0.1 70 | # note: 0.0(no motion constraint) 71 | frame_motion: 0.1 72 | 73 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 74 | # default: 2 75 | motion_space: 2 76 | 77 | # option 4: ratio = tracked feature num from last keyframe / total features at current frame 78 | # default: 0.9 79 | # note: 1.0(no scene constraint) 80 | scene_ratio: 0.9 81 | 82 | ############################################ 83 | #### Depth Enhancement #### 84 | ############################################ 85 | Enhancement: 86 | # brief: select how many DVL pointcloud frames based on timeoffset between DVL and anchor, from small to large 87 | # note: if camera_rate is 15hz, slide_window is 15, DVl_rate is 4hz, 88 | # then camera_rate/slide_window = DVl_rate/matched_num 89 | # this is because more matched_num DVL will not inside slide_window, then can't be interpolate pose 90 | matched_num: 4 91 | 92 | # brief: larger value descibe the z of 4 points are diverge, then some measurement is not good (e.g. multi-path) 93 | # default: maybe try 0.15 94 | standard_deviation: 0.2 -------------------------------------------------------------------------------- /config/GLRC/3_13/dpvio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | 13 | # brief: only select the sensor given by system.yaml 14 | 15 | INIT_SETTING: 16 | # # Example: 17 | # SENSOR: 18 | # # the timestamp that this sensor initialized 19 | # time: 1614971124.344753027 20 | # # initialized timeoffset 21 | # temporal: [0.01] 22 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 23 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 24 | # # initialized static transfomration between IMU and this sensor 25 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 26 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 27 | # intrinsic: [1.0, 2.0, 3.0] 28 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 29 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 30 | 31 | IMU: 32 | time: 1614971124.344753027 33 | # brief: orientation in quaterion form 34 | state: [-0.998573, 0.000816, 0.015993, 0.050947, 35 | 0.0000, 0.0000, 0.0000, 36 | -0.346171, 0.008464, 0.042122, 37 | -0.001849, -0.001689, -0.002033, 38 | # 0.001, 0.001, -0.001, 39 | -0.003053, -0.004853, -0.020769] 40 | 41 | DVL: 42 | time: 1614971124.354732990 43 | temporal: [0.000000238] 44 | 45 | PRESSURE: 46 | time: 1614971124.354732990 47 | global: [4.453799] 48 | 49 | 50 | ############################################################################### 51 | # INIT_STATIC: 52 | # todo: 0.0 53 | 54 | ############################################################################### 55 | INIT_DVL_PRESSURE: 56 | # Default: 10 57 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 58 | imu_window: 10 59 | 60 | # Default: 0.2 61 | # Note: the IMU variance threshold that indicates IMU jump inclued 62 | imu_var: 0.2 63 | 64 | # Default: 0.07 65 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 66 | imu_delta: 0.07 67 | 68 | # Default: 5 69 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 70 | dvl_window: 4 71 | 72 | # Default: 0.05 73 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 74 | dvl_delta: 0.05 75 | 76 | # Default: 1s 77 | #Note: how many second selected for initialization 78 | dvl_init_duration: 1 79 | 80 | 81 | ############################################################################### 82 | # INIT_CAMERA: 83 | # todo: 0.0 84 | 85 | 86 | ############################################################################### 87 | # INIT_DVL_CAMERA: 88 | # todo: 0.0 89 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dpvio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; gravity in the place your robot deployed, e.g. GLRC 6 | gravity: 9.80733 7 | 8 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 9 | # Default: 1.926e-04 from datasheet 10 | accelerometer_noise_density: 4.2e-04 11 | 12 | # Note: Bias random walk from kalibr_allen 13 | accelerometer_random_walk: 2.857e-05 14 | 15 | # Note: Noise density (continuous-time) from kalibr_allen 16 | # Default: 8.7e-05 from datasheet 17 | gyroscope_noise_density: 8.014e-05 18 | 19 | # Note: Bias random walk from kalibr_allen 20 | gyroscope_random_walk: 0.534e-05 21 | 22 | 23 | ##################################################################################### 24 | ##### DVL ##### 25 | ##################################################################################### 26 | DVL: 27 | # imu is reference frame 28 | T_I_D: 29 | - [ -0.999928, 0.0116206, -0.00288846, 0.433828] 30 | - [ 0.0115923, 0.999886, 0.00962664, 0.086180] 31 | - [ 0.003, 0.00959246, -0.999949, -0.302024] 32 | - [ 0.0, 0.0, 0.0, 1.0] 33 | # Default:0.0; Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 34 | # TODO: this value will be overide by IMU-DVL alignment 35 | timeoffset_I_D: 0.0 36 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 37 | scale: 0.944666667 38 | # noise on DVL tottom track 3-axis velocity measurement 39 | # noise_bt: [0.1, 0.15, 0.2] 40 | # noise_bt: [0.05, 0.1, 0.15] 41 | noise_bt: [0.1, 0.1, 0.1] 42 | 43 | 44 | ##################################################################################### 45 | ##### Pressure ##### 46 | ##################################################################################### 47 | PRESSURE: 48 | # Note: the angle that the actual mounting position rotate to the standing position, 49 | # used to transfer pressure measurement into DVL frame's Z 50 | # Default: 0: x-forward, up-looking; PI: x-forward, down-looking; -PI/2: x-forward, right-looking; PI/2: x-forward, left-looking 51 | mount_angle: 0 52 | # noise for pressure update 53 | # default: 0.01 54 | noise_pressure: 0.1 55 | # noise_pressure: 0.01 56 | 57 | 58 | ##################################################################################### 59 | ##### CAM0 ##### 60 | ##################################################################################### 61 | CAM0: 62 | T_C_I: 63 | - [0.00786236726823823, 0.9999532121420647, 0.0056353090162310805, -0.1232206727449017] 64 | - [0.9999681294260654, -0.00785441912633285, -0.0014311646745676972, -0.310185995908388] 65 | - [-0.0013868356345183388, 0.005646381757991208, -0.9999830973871349, -0.27812151547998104] 66 | - [0.0, 0.0, 0.0, 1.0] 67 | ## Alaska matlab: radial-tangential[k1,k2,p1,p2] 68 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 69 | ## Alaska matlab: f_x, f_y, c_x, c_y 70 | # intrinsics: [1845.542490218492,1846.046949112978,825.6913274592704,605.8504151895138] 71 | 72 | ## GLRC kalib: radial-tangential[k1,k2,p1,p2] 73 | # distortion_coeffs: [0.23117413334281256, 0.14158516202577756, -0.0003020869226409155, 0.0016221154832216692] 74 | ## GLRC kalib: f_x, f_y, c_x, c_y 75 | # intrinsics: [1832.8127123808745, 1831.3023458914286, 818.6226797023049, 614.5433318640432] 76 | 77 | ## metashape: radial-tangential[k1,k2,p1,p2,k3] 78 | distortion_coeffs: [-2.37387184652287003e-01, 2.18272742543289017e-01, -3.44046297220564870e-05, 1.05398436356655815e-03, -1.55006764704936351e-01] 79 | ## metashape: f_x, f_y, c_x, c_y 80 | intrinsics: [1836.16899521511550, 1836.16899521511550, 815.603781369907551, 614.213209479135003] 81 | 82 | resolution: [1616, 1240] 83 | # Default:-0.09398272330912658, from kalibr 84 | timeoffset_C_I: 0 85 | camera_model: pinhole 86 | distortion_model: radtan 87 | # unit in pixel: 0.09 88 | noise: 0.09 89 | # 90 | rostopic: /rov_remote/sensors/stereo/left/image_decompressed 91 | 92 | ##################################################################################### 93 | ##### CAM1 ##### 94 | ##################################################################################### 95 | CAM1: 96 | T_C_I: 97 | - [0.011362662503790503, 0.9998882224979653, 0.00971763400653785, -0.043854791823369293] 98 | - [0.9999302479992342, -0.011330740940760782, -0.0033336835368381165, -0.3111845498969625] 99 | - [-0.003223202912534585, 0.009754835703046718, -0.9999472256791313, -0.2777771992579014] 100 | - [0.0, 0.0, 0.0, 1.0] 101 | 102 | # # Alaska matlab: radial-tangential[k1,k2,p1,p2] 103 | # distortion_coeffs: [-0.238199868633002,0.213898054968889,-4.404041598795136e-04,0.002478829048498] 104 | # # Alaska matlab: f_x, f_y, c_x, c_y 105 | # intrinsics: [1840.934190755088,1842.254538956132,820.2899392018224,605.1273183612594] 106 | 107 | ## GLRC kalib: radial-tangential[k1,k2,p1,p2] 108 | distortion_coeffs: [-0.23332951286901957, 0.17085806195320472, -0.00023603059870771657, 0.0025196292160299585] 109 | ## GLRC kalib: f_x, f_y, c_x, c_y 110 | intrinsics: [1833.167836747399, 1832.2374257267884, 823.63564229672, 608.1216877595562] 111 | 112 | resolution: [1616, 1240] 113 | timeoffset_C_I: 0 114 | camera_model: pinhole 115 | distortion_model: radtan 116 | # unit in pixel: 0.09 117 | noise: 0.09 118 | # 119 | rostopic: /rov_remote/sensors/stereo/right/image_raw/compressed -------------------------------------------------------------------------------- /config/GLRC/3_13/dpvio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: ; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing 8 | backend_hz: 100 9 | # sensors will be fused 10 | # sensors: [IMU, DVL, PRESSURE, CAM0] 11 | sensors: [IMU, DVL, PRESSURE, CAM0, DVL_CLOUD] 12 | # sensors: [IMU, DVL, PRESSURE] 13 | # sensors: [IMU, CAM0] 14 | 15 | # rostopics used for each sensor 16 | topics: 17 | - {IMU: /rov/sensors/ahrs/imu/data} 18 | - {DVL: /rov/sensors/dvl/bottom_track_velocity} 19 | - {PRESSURE: /rov/sensors/dvl/pressure} 20 | - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 21 | - {DVL_CLOUD: /rov/sensors/dvl/pointcloud} 22 | 23 | buffers: 24 | # DVL cloud buffer to store how many seconds 25 | - {DVL_CLOUD: 10} 26 | 27 | 28 | #################################################################################### 29 | ############################ MSCKF estimation setup ########################## 30 | #################################################################################### 31 | 32 | MSCKF: 33 | ################################## DVL ################################### 34 | 35 | # enable DVL exterisic rotation calibration 36 | dvl_exterisic_R: false 37 | # enable DVL exterisic translation calibration 38 | dvl_exterisic_p: false 39 | # enable DVL time offset calibration 40 | dvl_timeoffset: false 41 | # enable DVL scale calibration 42 | dvl_scale: false 43 | 44 | ################################ Camera ################################## 45 | 46 | # enable camera exterisic rotation calibration 47 | cam_exterisic_R: false 48 | # enable camera exterisic translation calibration 49 | cam_exterisic_p: false 50 | # enable camera time offset calibration 51 | cam_timeoffset: false 52 | # Max camera clone 53 | # default: 11 for non-key-frame 54 | cam_clone: 15 55 | 56 | ################################ Update ################################## 57 | 58 | # brief: marginalized measurements at slide-windows index (or clone state index) 59 | # example 1: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 60 | # the marg measurements: oldest, second latest [0,8] 61 | # example 2: [-1] means all 62 | marg_meas_index: [0, 13] 63 | # marg_meas_index: [-1] 64 | 65 | # brief: marginalized pose at slide-windows index (or clone state index) 66 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 67 | marg_pose_index: [0] 68 | 69 | # in open_vins(40), but it's occasionally update at 40 70 | # but here, tracking will stop so always update as 40 71 | # max_msckf_update: 20 72 | 73 | # max line of residual lines for update, maybe used for futures 74 | # max_update_lines: 1500 75 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dvio/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | # FRONT_END: 7 | TRACK: 8 | mode: TRACK_KLT 9 | num_aruco: 1024 10 | downsize_aruco: false 11 | use_stereo: false 12 | max_camera: 1 13 | cam_id: 0 14 | downsample_ratio: 0.5 15 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 16 | img_enhancement: 1 17 | 18 | TRACK_KLT: 19 | num_pts: 250 20 | fast_threshold: 15 21 | grid_x: 5 22 | grid_y: 3 23 | min_px_dist: 8 24 | pyram: 3 25 | 26 | 27 | ############################################ 28 | #### Camera feature triangulation parameters #### 29 | ############################################ 30 | Feature: 31 | # Max condition number of linear triangulation matrix accept triangulated features 32 | max_cond_number: 10000 33 | # Minimum distance to accept triangulated features 34 | min_dist: 0.5 35 | # Minimum distance to accept triangulated features 36 | max_dist: 5 37 | # Multiplier to increase/decrease lambda 38 | lam_mult: 10 39 | # Init lambda for Levenberg-Marquardt optimization 40 | init_lamda: 0.001 41 | # Max runs for Levenberg-Marquardt 42 | max_runs: 5 43 | # Max lambda for Levenberg-Marquardt optimization 44 | max_lamda: 1e10 45 | # Cutoff for dx increment to consider as converged 46 | min_dx: 1e-6 47 | # Cutoff for cost decrement to consider as converged 48 | min_dcost: 1e-6 49 | # Max baseline ratio to accept triangulated features 50 | max_baseline: 40 51 | 52 | ############################################ 53 | #### Keyframe Strategy #### 54 | ############################################ 55 | Keyframe: 56 | # option 1: frame count 57 | frame_count: 2 58 | 59 | # option 3: minimum tracked features 60 | min_tracked: 50 61 | 62 | #D=0.1, V_xy = 0.3, ->0.3 63 | #D=0.1, V_xy = 0.4, ->0.25 64 | adaptive_factor: 0.33 65 | # 66 | adaptive_power: 1 67 | 68 | # option 2: relative motion constraint 69 | # default: 0.1 70 | # note: 0.0(no motion constraint) 71 | frame_motion: 0.1 72 | 73 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 74 | # default: 2 75 | motion_space: 2 76 | 77 | # option 4: ratio = tracked feature num from last keyframe / total features at current frame 78 | # default: 0.9 79 | # note: 1.0 (no scene constraint) 80 | scene_ratio: 0.9 81 | 82 | ############################################ 83 | #### Depth Enhancement #### 84 | ############################################ 85 | Enhancement: 86 | # brief: select how many DVL pointcloud frames based on timeoffset between DVL and anchor, from small to large 87 | # note: if camera_rate is 15hz, slide_window is 15, DVl_rate is 4hz, 88 | # then camera_rate/slide_window = DVl_rate/matched_num 89 | # this is because more matched_num DVL will not inside slide_window, then can't be interpolate pose 90 | matched_num: 4 91 | 92 | # brief: larger value descibe the z of 4 points are diverge, then some measurement is not good (e.g. multi-path) 93 | # default: maybe try 0.15 94 | standard_deviation: 0.2 -------------------------------------------------------------------------------- /config/GLRC/3_13/dvio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | 13 | # brief: only select the sensor given by system.yaml 14 | 15 | INIT_SETTING: 16 | # # Example: 17 | # SENSOR: 18 | # # the timestamp that this sensor initialized 19 | # time: 1614971124.344753027 20 | # # initialized timeoffset 21 | # temporal: [0.01] 22 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 23 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 24 | # # initialized static transfomration between IMU and this sensor 25 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 26 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 27 | # intrinsic: [1.0, 2.0, 3.0] 28 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 29 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 30 | 31 | IMU: 32 | time: 1614971124.344753027 33 | # brief: orientation in quaterion form 34 | state: [-0.998573, 0.000816, 0.015993, 0.050947, 35 | 0.0000, 0.0000, 0.0000, 36 | -0.346171, 0.008464, 0.042122, 37 | -0.001849, -0.001689, -0.002033, 38 | # 0.001, 0.001, -0.001, 39 | -0.003053, -0.004853, -0.020769] 40 | 41 | DVL: 42 | time: 1614971124.354732990 43 | temporal: [0.000000238] 44 | 45 | 46 | ############################################################################### 47 | # INIT_STATIC: 48 | # todo: 0.0 49 | 50 | ############################################################################### 51 | INIT_DVL_PRESSURE: 52 | # Default: 10 53 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 54 | imu_window: 10 55 | 56 | # Default: 0.2 57 | # Note: the IMU variance threshold that indicates IMU jump inclued 58 | imu_var: 0.2 59 | 60 | # Default: 0.07 61 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 62 | imu_delta: 0.07 63 | 64 | # Default: 5 65 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 66 | dvl_window: 4 67 | 68 | # Default: 0.05 69 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 70 | dvl_delta: 0.05 71 | 72 | # Default: 1s 73 | #Note: how many second selected for initialization 74 | dvl_init_duration: 1 75 | 76 | 77 | ############################################################################### 78 | # INIT_CAMERA: 79 | # todo: 0.0 80 | 81 | 82 | ############################################################################### 83 | # INIT_DVL_CAMERA: 84 | # todo: 0.0 85 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dvio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; gravity in the place your robot deployed, e.g. GLRC 6 | gravity: 9.80733 7 | 8 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 9 | # Default: 1.926e-04 from datasheet 10 | accelerometer_noise_density: 4.2e-04 11 | 12 | # Note: Bias random walk from kalibr_allen 13 | accelerometer_random_walk: 2.857e-05 14 | 15 | # Note: Noise density (continuous-time) from kalibr_allen 16 | # Default: 8.7e-05 from datasheet 17 | gyroscope_noise_density: 8.014e-05 18 | 19 | # Note: Bias random walk from kalibr_allen 20 | gyroscope_random_walk: 0.534e-05 21 | 22 | 23 | ##################################################################################### 24 | ##### DVL ##### 25 | ##################################################################################### 26 | DVL: 27 | # imu is reference frame 28 | T_I_D: 29 | - [ -0.999928, 0.0116206, -0.00288846, 0.433828] 30 | - [ 0.0115923, 0.999886, 0.00962664, 0.086180] 31 | - [ 0.003, 0.00959246, -0.999949, -0.302024] 32 | - [ 0.0, 0.0, 0.0, 1.0] 33 | # Default:0.0; Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 34 | # TODO: this value will be overide by IMU-DVL alignment 35 | timeoffset_I_D: 0.0 36 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 37 | scale: 0.944666667 38 | # noise on DVL tottom track 3-axis velocity measurement 39 | # noise_bt: [0.1, 0.15, 0.2] 40 | noise_bt: [0.05, 0.1, 0.15] 41 | # noise_bt: [0.1, 0.1, 0.1] 42 | 43 | 44 | ##################################################################################### 45 | ##### CAM0 ##### 46 | ##################################################################################### 47 | CAM0: 48 | T_C_I: 49 | - [0.00786236726823823, 0.9999532121420647, 0.0056353090162310805, -0.1232206727449017] 50 | - [0.9999681294260654, -0.00785441912633285, -0.0014311646745676972, -0.310185995908388] 51 | - [-0.0013868356345183388, 0.005646381757991208, -0.9999830973871349, -0.27812151547998104] 52 | - [0.0, 0.0, 0.0, 1.0] 53 | ## Alaska matlab: radial-tangential[k1,k2,k1,k2] 54 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 55 | ## Alaska matlab: f_x, f_y, c_x, c_y 56 | # intrinsics: [1845.542490218492,1846.046949112978,825.6913274592704,605.8504151895138] 57 | 58 | ## GLRC kalib: radial-tangential[k1,k2,k1,k2] 59 | # distortion_coeffs: [0.23117413334281256, 0.14158516202577756, -0.0003020869226409155, 0.0016221154832216692] 60 | ## GLRC matlab: f_x, f_y, c_x, c_y 61 | # intrinsics: [1832.8127123808745, 1831.3023458914286, 818.6226797023049, 614.5433318640432] 62 | 63 | ## metashape: radial-tangential[k1,k2,p1,p1,k3] 64 | distortion_coeffs: [-2.37387184652287003e-01, 2.18272742543289017e-01, -3.44046297220564870e-05, 1.05398436356655815e-03, -1.55006764704936351e-01] 65 | ## metashape: f_x, f_y, c_x, c_y 66 | intrinsics: [1836.16899521511550, 1836.16899521511550, 815.603781369907551, 614.213209479135003] 67 | 68 | resolution: [1616, 1240] 69 | # Default:-0.09398272330912658, from kalibr 70 | timeoffset_C_I: 0 71 | camera_model: pinhole 72 | distortion_model: radtan 73 | # unit in pixel: 0.09 74 | noise: 0.09 75 | # 76 | rostopic: /rov_remote/sensors/stereo/left/image_decompressed 77 | -------------------------------------------------------------------------------- /config/GLRC/3_13/dvio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: ; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing 8 | backend_hz: 100 9 | # sensors will be fused 10 | # sensors: [IMU, DVL, PRESSURE, CAM0] 11 | # sensors: [IMU, DVL, PRESSURE, CAM0, DVL_CLOUD] 12 | sensors: [IMU, DVL, CAM0, DVL_CLOUD] 13 | # sensors: [IMU, DVL, CAM0] 14 | # sensors: [IMU, DVL, PRESSURE] 15 | # sensors: [IMU, CAM0] 16 | 17 | # rostopics used for each sensor 18 | topics: 19 | - {IMU: /rov/sensors/ahrs/imu/data} 20 | - {DVL: /rov/sensors/dvl/bottom_track_velocity} 21 | - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 22 | - {DVL_CLOUD: /rov/sensors/dvl/pointcloud} 23 | 24 | buffers: 25 | # DVL cloud buffer to store how many seconds 26 | - {CAM0: 10} 27 | - {DVL_CLOUD: 10} 28 | 29 | 30 | #################################################################################### 31 | ############################ MSCKF estimation setup ########################## 32 | #################################################################################### 33 | 34 | MSCKF: 35 | ################################## DVL ################################### 36 | 37 | # enable DVL exterisic rotation calibration 38 | dvl_exterisic_R: false 39 | # enable DVL exterisic translation calibration 40 | dvl_exterisic_p: false 41 | # enable DVL time offset calibration 42 | dvl_timeoffset: false 43 | # enable DVL scale calibration 44 | dvl_scale: false 45 | 46 | ################################ Camera ################################## 47 | 48 | # enable camera exterisic rotation calibration 49 | cam_exterisic_R: false 50 | # enable camera exterisic translation calibration 51 | cam_exterisic_p: false 52 | # enable camera time offset calibration 53 | cam_timeoffset: false 54 | # Max camera clone 55 | # default: 11 for non-key-frame 56 | cam_clone: 15 57 | 58 | ################################ Update ################################## 59 | 60 | # brief: marginalized measurements at slide-windows index (or clone state index) 61 | # example 1: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 62 | # the marg measurements: oldest, second latest [0,8] 63 | # example 2: [-1] means all 64 | marg_meas_index: [0, 13] 65 | # marg_meas_index: [-1] 66 | 67 | # brief: marginalized pose at slide-windows index (or clone state index) 68 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 69 | marg_pose_index: [0] 70 | 71 | # in open_vins(40), but it's occasionally update at 40 72 | # but here, tracking will stop so always update as 40 73 | # max_msckf_update: 20 74 | 75 | # max line of residual lines for update, maybe used for futures 76 | # max_update_lines: 1500 77 | -------------------------------------------------------------------------------- /config/GLRC/3_13/pvio/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | # FRONT_END: 7 | TRACK: 8 | mode: TRACK_KLT 9 | num_aruco: 1024 10 | downsize_aruco: false 11 | use_stereo: false 12 | max_camera: 1 13 | cam_id: 0 14 | downsample_ratio: 0.5 15 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 16 | img_enhancement: 1 17 | 18 | TRACK_KLT: 19 | num_pts: 250 20 | fast_threshold: 15 21 | grid_x: 5 22 | grid_y: 3 23 | min_px_dist: 8 24 | pyram: 3 25 | 26 | 27 | ############################################ 28 | #### Camera feature triangulation parameters #### 29 | ############################################ 30 | Feature: 31 | # Max condition number of linear triangulation matrix accept triangulated features 32 | max_cond_number: 10000 33 | # Minimum distance to accept triangulated features 34 | min_dist: 0.5 35 | # Minimum distance to accept triangulated features 36 | max_dist: 5 37 | # Multiplier to increase/decrease lambda 38 | lam_mult: 10 39 | # Init lambda for Levenberg-Marquardt optimization 40 | init_lamda: 0.001 41 | # Max runs for Levenberg-Marquardt 42 | max_runs: 5 43 | # Max lambda for Levenberg-Marquardt optimization 44 | max_lamda: 1e10 45 | # Cutoff for dx increment to consider as converged 46 | min_dx: 1e-6 47 | # Cutoff for cost decrement to consider as converged 48 | min_dcost: 1e-6 49 | # Max baseline ratio to accept triangulated features 50 | max_baseline: 40 51 | 52 | ############################################ 53 | #### Keyframe Strategy #### 54 | ############################################ 55 | Keyframe: 56 | # option 1: frame count 57 | frame_count: 2 58 | 59 | # option 3: minimum tracked features 60 | min_tracked: 50 61 | 62 | #D=0.1, V_xy = 0.3, ->0.3 63 | #D=0.1, V_xy = 0.4, ->0.25 64 | adaptive_factor: 0.33 65 | # 66 | adaptive_power: 1 67 | 68 | # option 2: relative motion constraint 69 | # default: 0.1 70 | # node: 0.0(no motion constraint) 71 | frame_motion: 0.1 72 | 73 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 74 | # default: 2 75 | motion_space: 2 76 | 77 | # option 4: ratio = tracked feature num from last keyframe / total features at current frame 78 | # default: 0.9 79 | # note: 1.0(no scene contraint) 80 | scene_ratio: 0.9 81 | 82 | ############################################ 83 | #### Depth Enhancement #### 84 | ############################################ 85 | Enhancement: 86 | # brief: select how many DVL pointcloud frames based on timeoffset between DVL and anchor, from small to large 87 | # note: if camera_rate is 15hz, slide_window is 15, DVl_rate is 4hz, 88 | # then camera_rate/slide_window = DVl_rate/matched_num 89 | # this is because more matched_num DVL will not inside slide_window, then can't be interpolate pose 90 | matched_num: 4 91 | 92 | # brief: larger value descibe the z of 4 points are diverge, then some measurement is not good (e.g. multi-path) 93 | # default: maybe try 0.15 94 | standard_deviation: 0.2 -------------------------------------------------------------------------------- /config/GLRC/3_13/pvio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | 13 | # brief: only select the sensor given by system.yaml 14 | 15 | INIT_SETTING: 16 | # # Example: 17 | # SENSOR: 18 | # # the timestamp that this sensor initialized 19 | # time: 1614971124.344753027 20 | # # initialized timeoffset 21 | # temporal: [0.01] 22 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 23 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 24 | # # initialized static transfomration between IMU and this sensor 25 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 26 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 27 | # intrinsic: [1.0, 2.0, 3.0] 28 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 29 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 30 | 31 | IMU: 32 | time: 1614971124.344753027 33 | # brief: orientation in quaterion form 34 | state: [-0.998573, 0.000816, 0.015993, 0.050947, 35 | 0.0000, 0.0000, 0.0000, 36 | -0.346171, 0.008464, 0.042122, 37 | -0.001849, -0.001689, -0.002033, 38 | # 0.001, 0.001, -0.001, 39 | -0.003053, -0.004853, -0.020769] 40 | 41 | PRESSURE: 42 | time: 1614971124.354732990 43 | global: [4.453799] 44 | 45 | 46 | ############################################################################### 47 | # INIT_STATIC: 48 | # todo: 0.0 49 | 50 | ############################################################################### 51 | INIT_DVL_PRESSURE: 52 | # Default: 10 53 | # Note: how many IMU data is selected to detect IMU jump (suddenly move) 54 | imu_window: 10 55 | 56 | # Default: 0.2 57 | # Note: the IMU variance threshold that indicates IMU jump inclued 58 | imu_var: 0.2 59 | 60 | # Default: 0.07 61 | # Note: IMU accleraion difference in x-axis(forward direction) shows at the suddenly movement time 62 | imu_delta: 0.07 63 | 64 | # Default: 5 65 | # Note: how many DVL data is selected to detect DVL jump (suddenly move) 66 | dvl_window: 4 67 | 68 | # Default: 0.05 69 | # Note: DVL velocity difference in x-axis(forward direction) shows at the suddenly movement time 70 | dvl_delta: 0.05 71 | 72 | # Default: 1s 73 | #Note: how many second selected for initialization 74 | dvl_init_duration: 1 75 | 76 | 77 | ############################################################################### 78 | # INIT_CAMERA: 79 | # todo: 0.0 80 | 81 | 82 | ############################################################################### 83 | # INIT_DVL_CAMERA: 84 | # todo: 0.0 85 | -------------------------------------------------------------------------------- /config/GLRC/3_13/pvio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; gravity in the place your robot deployed, e.g. GLRC 6 | gravity: 9.80733 7 | 8 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 9 | # Default: 1.926e-04 from datasheet 10 | accelerometer_noise_density: 4.2e-04 11 | 12 | # Note: Bias random walk from kalibr_allen 13 | accelerometer_random_walk: 2.857e-05 14 | 15 | # Note: Noise density (continuous-time) from kalibr_allen 16 | # Default: 8.7e-05 from datasheet 17 | gyroscope_noise_density: 8.014e-05 18 | 19 | # Note: Bias random walk from kalibr_allen 20 | gyroscope_random_walk: 0.534e-05 21 | 22 | 23 | ##################################################################################### 24 | ##### Pressure ##### 25 | ##################################################################################### 26 | PRESSURE: 27 | # Note: the angle that the actual mounting position rotate to the standing position, 28 | # used to transfer pressure measurement into DVL frame's Z 29 | # Default: 0: x-forward, up-looking; PI: x-forward, down-looking; -PI/2: x-forward, right-looking; PI/2: x-forward, left-looking 30 | mount_angle: 0 31 | # noise for pressure update 32 | # default: 0.01 33 | noise_pressure: 0.1 34 | # noise_pressure: 0.01 35 | 36 | 37 | ##################################################################################### 38 | ##### CAM0 ##### 39 | ##################################################################################### 40 | CAM0: 41 | T_C_I: 42 | - [0.00786236726823823, 0.9999532121420647, 0.0056353090162310805, -0.1232206727449017] 43 | - [0.9999681294260654, -0.00785441912633285, -0.0014311646745676972, -0.310185995908388] 44 | - [-0.0013868356345183388, 0.005646381757991208, -0.9999830973871349, -0.27812151547998104] 45 | - [0.0, 0.0, 0.0, 1.0] 46 | ## Alaska matlab: radial-tangential[k1,k2,k1,k2] 47 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 48 | ## Alaska matlab: f_x, f_y, c_x, c_y 49 | # intrinsics: [1845.542490218492,1846.046949112978,825.6913274592704,605.8504151895138] 50 | 51 | ## GLRC kalib: radial-tangential[k1,k2,k1,k2] 52 | # distortion_coeffs: [0.23117413334281256, 0.14158516202577756, -0.0003020869226409155, 0.0016221154832216692] 53 | ## GLRC matlab: f_x, f_y, c_x, c_y 54 | # intrinsics: [1832.8127123808745, 1831.3023458914286, 818.6226797023049, 614.5433318640432] 55 | 56 | ## metashape: radial-tangential[k1,k2,p1,p1,k3] 57 | distortion_coeffs: [-2.37387184652287003e-01, 2.18272742543289017e-01, -3.44046297220564870e-05, 1.05398436356655815e-03, -1.55006764704936351e-01] 58 | ## metashape: f_x, f_y, c_x, c_y 59 | intrinsics: [1836.16899521511550, 1836.16899521511550, 815.603781369907551, 614.213209479135003] 60 | 61 | resolution: [1616, 1240] 62 | # Default:-0.09398272330912658, from kalibr 63 | timeoffset_C_I: 0 64 | camera_model: pinhole 65 | distortion_model: radtan 66 | # unit in pixel: 0.09 67 | noise: 0.09 68 | # 69 | rostopic: /rov_remote/sensors/stereo/left/image_decompressed 70 | -------------------------------------------------------------------------------- /config/GLRC/3_13/pvio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: ; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing 8 | backend_hz: 100 9 | # sensors will be fused 10 | # sensors: [IMU, DVL, PRESSURE, CAM0] 11 | sensors: [IMU, PRESSURE, CAM0] 12 | # sensors: [IMU, DVL, PRESSURE, CAM0, DVL_CLOUD] 13 | # sensors: [IMU, DVL, PRESSURE] 14 | # sensors: [IMU, CAM0] 15 | 16 | # rostopics used for each sensor 17 | topics: 18 | - {IMU: /rov/sensors/ahrs/imu/data} 19 | - {PRESSURE: /rov/sensors/dvl/pressure} 20 | - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 21 | 22 | buffers: 23 | # DVL cloud buffer to store how many seconds 24 | - {CAM0: 10} 25 | 26 | 27 | #################################################################################### 28 | ############################ MSCKF estimation setup ########################## 29 | #################################################################################### 30 | 31 | MSCKF: 32 | ################################## DVL ################################### 33 | 34 | # enable DVL exterisic rotation calibration 35 | dvl_exterisic_R: false 36 | # enable DVL exterisic translation calibration 37 | dvl_exterisic_p: false 38 | # enable DVL time offset calibration 39 | dvl_timeoffset: false 40 | # enable DVL scale calibration 41 | dvl_scale: false 42 | 43 | ################################ Camera ################################## 44 | 45 | # enable camera exterisic rotation calibration 46 | cam_exterisic_R: false 47 | # enable camera exterisic translation calibration 48 | cam_exterisic_p: false 49 | # enable camera time offset calibration 50 | cam_timeoffset: false 51 | # Max camera clone 52 | # default: 11 for non-key-frame 53 | cam_clone: 15 54 | 55 | ################################ Update ################################## 56 | 57 | # brief: marginalized measurements at slide-windows index (or clone state index) 58 | # example 1: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 59 | # the marg measurements: oldest, second latest [0,8] 60 | # example 2: [-1] means all 61 | marg_meas_index: [0, 13] 62 | # marg_meas_index: [-1] 63 | 64 | # brief: marginalized pose at slide-windows index (or clone state index) 65 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 66 | marg_pose_index: [0] 67 | 68 | # in open_vins(40), but it's occasionally update at 40 69 | # but here, tracking will stop so always update as 40 70 | # max_msckf_update: 20 71 | 72 | # max line of residual lines for update, maybe used for futures 73 | # max_update_lines: 1500 74 | -------------------------------------------------------------------------------- /config/GLRC/3_13/vio/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | TRACK: 7 | mode: TRACK_KLT 8 | num_aruco: 1024 9 | downsize_aruco: false 10 | use_stereo: false 11 | max_camera: 1 12 | cam_id: 0 13 | downsample_ratio: 0.5 14 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 15 | img_enhancement: 1 16 | 17 | TRACK_KLT: 18 | num_pts: 250 19 | fast_threshold: 15 20 | grid_x: 5 21 | grid_y: 3 22 | min_px_dist: 8 23 | pyram: 3 24 | 25 | ############################################ 26 | #### Camera feature triangulation parameters #### 27 | ############################################ 28 | Feature: 29 | # Max condition number of linear triangulation matrix accept triangulated features 30 | max_cond_number: 10000 31 | # Minimum distance to accept triangulated features 32 | min_dist: 0.5 33 | # Minimum distance to accept triangulated features 34 | max_dist: 5 35 | # Multiplier to increase/decrease lambda 36 | lam_mult: 10 37 | # Init lambda for Levenberg-Marquardt optimization 38 | init_lamda: 0.001 39 | # Max runs for Levenberg-Marquardt 40 | max_runs: 5 41 | # Max lambda for Levenberg-Marquardt optimization 42 | max_lamda: 1e10 43 | # Cutoff for dx increment to consider as converged 44 | min_dx: 1e-6 45 | # Cutoff for cost decrement to consider as converged 46 | min_dcost: 1e-6 47 | # Max baseline ratio to accept triangulated features 48 | max_baseline: 40 49 | 50 | ############################################ 51 | #### Keyframe Strategy #### 52 | ############################################ 53 | Keyframe: 54 | # option 1: frame count 55 | frame_count: 2 56 | 57 | # option 3: minimum tracked features 58 | min_tracked: 50 59 | 60 | #D=0.1, V_xy = 0.3, ->0.3 61 | #D=0.1, V_xy = 0.4, ->0.25 62 | adaptive_factor: 0.33 63 | # 64 | adaptive_power: 1 65 | 66 | # option 2: relative motion constraint 67 | # default: 0.1 68 | frame_motion: 0.0 69 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 70 | # default: 2 71 | motion_space: 2 72 | # option 4: ratio = tracked feature num from last keyframe / total features at current frame 73 | # default: 0.8 74 | scene_ratio: 1.0 75 | 76 | ############################################ 77 | #### Depth Enhancement #### 78 | ############################################ 79 | Enhancement: 80 | # brief: select how many DVL pointcloud frames based on timeoffset between DVL and anchor, from small to large 81 | # note: if camera_rate is 15hz, slide_window is 15, DVl_rate is 4hz, 82 | # then camera_rate/slide_window = DVl_rate/matched_num 83 | # this is because more matched_num DVL will not inside slide_window, then can't be interpolate pose 84 | matched_num: 4 85 | 86 | # brief: larger value descibe the z of 4 points are diverge, then some measurement is not good (e.g. multi-path) 87 | # default: maybe try 0.15 88 | standard_deviation: 0.2 -------------------------------------------------------------------------------- /config/GLRC/3_13/vio/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | INIT_SETTING: 13 | # # Example: 14 | # SENSOR: 15 | # # the timestamp that this sensor initialized 16 | # time: 1614971124.344753027 17 | # # initialized timeoffset 18 | # temporal: [0.01] 19 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 20 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 21 | # # initialized static transfomration between IMU and this sensor 22 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 23 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 24 | # intrinsic: [1.0, 2.0, 3.0] 25 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 26 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 27 | 28 | IMU: 29 | time: 1614971124.344753027 30 | # brief: orientation in quaterion form 31 | state: [-0.998573, 0.000816, 0.015993, 0.050947, 32 | 0.0000, 0.0000, 0.0000, 33 | -0.346171, 0.008464, 0.042122, 34 | -0.001849, -0.001689, -0.002033, 35 | # 0.001, 0.001, -0.001, 36 | -0.003053, -0.004853, -0.020769] -------------------------------------------------------------------------------- /config/GLRC/3_13/vio/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; 6 | # gravity in the place your robot deployed 7 | # The North Alaska from E. Thiel et al. GRAVITY MEASUREMENTS IN ALASKA 8 | gravity: 9.80733 9 | 10 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 11 | # Default: 1.926e-04 from datasheet 12 | accelerometer_noise_density: 4.2e-04 13 | 14 | # Note: Bias random walk from kalibr_allen 15 | accelerometer_random_walk: 2.857e-05 16 | 17 | # Note: Noise density (continuous-time) from kalibr_allen 18 | # Default: 8.7e-05 from datasheet 19 | gyroscope_noise_density: 8.014e-05 20 | 21 | # Note: Bias random walk from kalibr_allen 22 | gyroscope_random_walk: 0.534e-05 23 | 24 | 25 | ##################################################################################### 26 | ##### CAM0 ##### 27 | ##################################################################################### 28 | 29 | CAM0: 30 | T_C_I: 31 | - [0.00786236726823823, 0.9999532121420647, 0.0056353090162310805, -0.1232206727449017] 32 | - [0.9999681294260654, -0.00785441912633285, -0.0014311646745676972, -0.310185995908388] 33 | - [-0.0013868356345183388, 0.005646381757991208, -0.9999830973871349, -0.27812151547998104] 34 | - [0.0, 0.0, 0.0, 1.0] 35 | 36 | ## Alaska matlab: radial-tangential[k1,k2,k1,k2] 37 | # distortion_coeffs: [-0.229971614369661,0.173683036312323,-7.457584765784308e-04,0.002259672821916] 38 | ## Alaska matlab: f_x, f_y, c_x, c_y 39 | # intrinsics: [1845.542490218492,1846.046949112978,825.6913274592704,605.8504151895138] 40 | 41 | ## GLRC kalib: radial-tangential[k1,k2,k1,k2] 42 | # distortion_coeffs: [0.23117413334281256, 0.14158516202577756, -0.0003020869226409155, 0.0016221154832216692] 43 | ## GLRC matlab: f_x, f_y, c_x, c_y 44 | # intrinsics: [1832.8127123808745, 1831.3023458914286, 818.6226797023049, 614.5433318640432] 45 | 46 | ## metashape: radial-tangential[k1,k2,p1,p1,k3] 47 | distortion_coeffs: [-2.37387184652287003e-01, 2.18272742543289017e-01, -3.44046297220564870e-05, 1.05398436356655815e-03, -1.55006764704936351e-01] 48 | ## metashape: f_x, f_y, c_x, c_y 49 | intrinsics: [1836.16899521511550, 1836.16899521511550, 815.603781369907551, 614.213209479135003] 50 | 51 | resolution: [1616, 1240] 52 | # Default:-0.09398272330912658, from kalibr 53 | timeoffset_C_I: 0 54 | # unit in pixel 55 | camera_model: pinhole 56 | distortion_model: radtan 57 | # unit in pixel: 0.01 58 | noise: 0.09 59 | 60 | rostopic: /rov_remote/sensors/stereo/left/image_decompressed 61 | ##################################################################################### 62 | ##################################################################################### 63 | 64 | # CAM1: 65 | 66 | -------------------------------------------------------------------------------- /config/GLRC/3_13/vio/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: 20; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing ( keep it as camera fequency) 8 | backend_hz: 100 9 | # sensors will be fused 10 | # sensors: [IMU, DVL] 11 | sensors: [IMU, CAM0] 12 | # rostopics used for each sensor 13 | topics: 14 | - {IMU: /rov/sensors/ahrs/imu/data} 15 | - {CAM0: /rov_remote/sensors/stereo/left/image_decompressed} 16 | # load some test files 17 | # csv: /home/lin/develop/data/sim/udel_gore/sim_NoCalibNoise_4_feat_position.txt 18 | buffers: 19 | # buffer to store how many seconds 20 | - {CAM0: 10} 21 | 22 | 23 | #################################################################################### 24 | ############################ MSCKF estimation setup ########################## 25 | #################################################################################### 26 | 27 | #### MSCKF parameters 28 | MSCKF: 29 | 30 | ################################ Camera ################################## 31 | 32 | ## enable camera exterisic rotation calibration 33 | cam_exterisic_R: false 34 | ## enable camera exterisic translation calibration 35 | cam_exterisic_p: false 36 | ## enable camera time offset calibration 37 | cam_timeoffset: false 38 | ## Max camera clone 39 | ## default: 11 for non-key-frame 40 | cam_clone: 15 41 | 42 | ################################ Update ################################## 43 | 44 | ## Index of clone need to marginalize 45 | ## Option 1: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 46 | ## the marg clone measurements: oldest, second latest [0,8] 47 | ## Option 2: [-1] means use all 48 | # marg_meas_index: [-1] 49 | # marg_meas_index: [0, 10] 50 | marg_meas_index: [0, 13] 51 | 52 | # brief: marginalized pose at slide-windows index (or clone state index) 53 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 54 | # example: [-1] means use first index from 'marg_meas_index' 55 | # marg_pose_index: [-1] 56 | # marg_pose_index: [0, 10] 57 | marg_pose_index: [0] 58 | 59 | ## in open_vins(40), but it's occasionally update at 40 60 | ## but here, tracking will stop so always update as 40 61 | # max_msckf_update: 20 62 | 63 | # ## max line of residual lines for update, maybe used for futures 64 | # # max_update_lines: 1500 65 | 66 | ################################ Marginalization ################################## 67 | -------------------------------------------------------------------------------- /config/GLRC/glrc.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /config/euroc/MH_02_easy/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | TRACK: 7 | mode: TRACK_KLT 8 | num_aruco: 1024 9 | downsize_aruco: false 10 | use_stereo: false 11 | max_camera: 1 12 | cam_id: 0 13 | downsample_ratio: 1.0 14 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 15 | img_enhancement: 0 16 | 17 | # TRACK_FEATURE: 18 | # todo: 1 19 | 20 | TRACK_KLT: 21 | num_pts: 250 22 | fast_threshold: 15 23 | grid_x: 5 24 | grid_y: 3 25 | min_px_dist: 8 26 | pyram: 3 27 | 28 | 29 | ############################################ 30 | #### Camera feature triangulation parameters #### 31 | ############################################ 32 | Feature: 33 | # Max condition number of linear triangulation matrix accept triangulated features 34 | max_cond_number: 10000 35 | # Minimum distance to accept triangulated features 36 | min_dist: 0.1 37 | # Minimum distance to accept triangulated features 38 | max_dist: 60 39 | # Multiplier to increase/decrease lambda 40 | lam_mult: 10 41 | # Init lambda for Levenberg-Marquardt optimization 42 | init_lamda: 0.001 43 | # Max runs for Levenberg-Marquardt 44 | max_runs: 5 45 | # Max lambda for Levenberg-Marquardt optimization 46 | max_lamda: 1e10 47 | # Cutoff for dx increment to consider as converged 48 | min_dx: 1e-6 49 | # Cutoff for cost decrement to consider as converged 50 | min_dcost: 1e-6 51 | # Max baseline ratio to accept triangulated features 52 | max_baseline: 40 53 | 54 | 55 | ############################################ 56 | #### Keyframe Strategy #### 57 | ############################################ 58 | Keyframe: 59 | # option 1: frame count 60 | frame_count: 0 61 | # option 2: relative motion constraint 62 | frame_motion: 0.0 63 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 64 | motion_space: 3 65 | # option 3: minimum tracked features 66 | min_tracked: 50 67 | # option 4: ratio = tracked features from last keyframe / total features at current frame 68 | scene_ratio: 1.0 -------------------------------------------------------------------------------- /config/euroc/MH_02_easy/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | 13 | INIT_SETTING: 14 | # # Example: 15 | # SENSOR: 16 | # # the timestamp that this sensor initialized 17 | # time: 1614971124.344753027 18 | # # initialized timeoffset 19 | # temporal: [0.01] 20 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 21 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 22 | # # initialized static transfomration between IMU and this sensor 23 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 24 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 25 | # intrinsic: [1.0, 2.0, 3.0] 26 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 27 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 28 | 29 | 30 | # start at 38.4 second 31 | IMU: 32 | time: 1403636895.961666584 33 | # brief: orientation in quaterion form 34 | state: [-0.7929, -0.0480, -0.6042, 0.0630, 35 | 0.0000, 0.0000, 0.0000, 36 | 0.0000, 0.0000, 0.0000, 37 | -0.0024, 0.0208, 0.0772, 38 | -0.0378, 0.0016, 0.0103] -------------------------------------------------------------------------------- /config/euroc/MH_02_easy/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; gravity in the place your robot deployed, e.g. GLRC 6 | gravity: 9.81 7 | 8 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 9 | accelerometer_noise_density: 2.0000e-3 10 | 11 | # Note: Bias random walk from kalibr_allen 12 | accelerometer_random_walk: 3.0000e-3 13 | 14 | # Note: Noise density (continuous-time) from kalibr_allen 15 | gyroscope_noise_density: 1.6968e-04 16 | 17 | # Note: Bias random walk from kalibr_allen 18 | gyroscope_random_walk: 1.9393e-05 19 | 20 | 21 | ##################################################################################### 22 | ##### CAM0 ##### 23 | ##################################################################################### 24 | CAM0: 25 | T_C_I: 26 | - [0.01486554, 0.99955725, -0.02577444, 0.06522291] 27 | - [-0.99988093, 0.01496721, 0.00375619, -0.02070639] 28 | - [0.0041403 , 0.02571553, 0.99966073, -0.0080546] 29 | - [0.0, 0.0, 0.0, 1.0] 30 | # radial-tangential[k1,k2,r1,r2] 31 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 32 | # f_x, f_y, c_x, c_y 33 | intrinsics: [458.654, 457.296, 367.215, 248.375] 34 | # this parameter not loaded 35 | resolution: [752, 480] 36 | camera_model: pinhole 37 | distortion_model: radtan 38 | timeoffset_C_I: 0 39 | # unit in pixel 40 | # noise: 0.035 41 | noise: 0.02 42 | # noise: 0.01 -------------------------------------------------------------------------------- /config/euroc/MH_02_easy/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: ; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing 8 | backend_hz: 100 9 | # sensors will be fused 10 | sensors: [IMU, CAM0] 11 | # rostopics used for each sensor 12 | topics: 13 | - {IMU: /imu0} 14 | - {CAM0: /cam0/image_raw} 15 | # data storage 16 | buffers: 17 | - {CAM0: 10} 18 | 19 | #################################################################################### 20 | ############################ MSCKF estimation setup ########################## 21 | #################################################################################### 22 | 23 | MSCKF: 24 | 25 | ################################ Camera ################################## 26 | 27 | # enable camera exterisic rotation calibration 28 | cam_exterisic_R: false 29 | # enable camera exterisic translation calibration 30 | cam_exterisic_p: false 31 | # enable camera time offset calibration 32 | cam_timeoffset: false 33 | # Max camera clone 34 | cam_clone: 20 35 | 36 | ################################ Update ################################## 37 | 38 | # Index of clone need to marginalize 39 | # example: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 40 | # the marg clone measurements: oldest, second latest [0,8] 41 | # marg_meas_index: [0, 18] 42 | marg_meas_index: [-1] 43 | 44 | # brief: marginalized pose at slide-windows index (or clone state index) 45 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 46 | marg_pose_index: [-1] 47 | 48 | # in open_vins(40), but it's occasionally update at 40 49 | # but here, tracking will stop so always update as 40 50 | max_msckf_update: 20 51 | 52 | # max line of residual lines for update, maybe used for futures 53 | # max_update_lines: 1500 -------------------------------------------------------------------------------- /config/euroc/MH_03_medium/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | TRACK: 7 | mode: TRACK_KLT 8 | num_aruco: 1024 9 | downsize_aruco: false 10 | use_stereo: false 11 | max_camera: 1 12 | cam_id: 0 13 | downsample_ratio: 1.0 14 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 15 | img_enhancement: 1 16 | 17 | # TRACK_FEATURE: 18 | # todo: 1 19 | 20 | TRACK_KLT: 21 | num_pts: 250 22 | fast_threshold: 15 23 | grid_x: 5 24 | grid_y: 3 25 | min_px_dist: 8 26 | pyram: 3 27 | 28 | 29 | ############################################ 30 | #### Camera feature triangulation parameters #### 31 | ############################################ 32 | Feature: 33 | # Max condition number of linear triangulation matrix accept triangulated features 34 | max_cond_number: 10000 35 | # Minimum distance to accept triangulated features 36 | min_dist: 0.1 37 | # Minimum distance to accept triangulated features 38 | max_dist: 60 39 | # Multiplier to increase/decrease lambda 40 | lam_mult: 10 41 | # Init lambda for Levenberg-Marquardt optimization 42 | init_lamda: 0.001 43 | # Max runs for Levenberg-Marquardt 44 | max_runs: 5 45 | # Max lambda for Levenberg-Marquardt optimization 46 | max_lamda: 1e10 47 | # Cutoff for dx increment to consider as converged 48 | min_dx: 1e-6 49 | # Cutoff for cost decrement to consider as converged 50 | min_dcost: 1e-6 51 | # Max baseline ratio to accept triangulated features 52 | max_baseline: 40 53 | 54 | 55 | ############################################ 56 | #### Keyframe Strategy #### 57 | ############################################ 58 | Keyframe: 59 | # option 1: frame count 60 | frame_count: 0 61 | # option 2: relative motion constraint 62 | frame_motion: 0.0 63 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 64 | motion_space: 3 65 | # option 3: minimum tracked features 66 | min_tracked: 50 67 | # option 4: ratio = tracked features from last keyframe / total features at current frame 68 | scene_ratio: 1.0 -------------------------------------------------------------------------------- /config/euroc/MH_03_medium/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | 13 | INIT_SETTING: 14 | # # Example: 15 | # SENSOR: 16 | # # the timestamp that this sensor initialized 17 | # time: 1614971124.344753027 18 | # # initialized timeoffset 19 | # temporal: [0.01] 20 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 21 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 22 | # # initialized static transfomration between IMU and this sensor 23 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 24 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 25 | # intrinsic: [1.0, 2.0, 3.0] 26 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 27 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 28 | 29 | 30 | # start at 17.5 second 31 | IMU: 32 | time: 1403637148.798319101 33 | # brief: orientation in quaterion form 34 | state: [-0.7916, -0.0762, -0.5978, 0.1009, 35 | 0.0000, 0.0000, 0.0000, 36 | 0.0000, 0.0000, 0.0000, 37 | 0.0033, 0.0177, 0.0764, 38 | -0.0521, 0.0037, 0.0143] 39 | -------------------------------------------------------------------------------- /config/euroc/MH_03_medium/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; gravity in the place your robot deployed, e.g. GLRC 6 | gravity: 9.81 7 | 8 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 9 | accelerometer_noise_density: 2.0000e-3 10 | 11 | # Note: Bias random walk from kalibr_allen 12 | accelerometer_random_walk: 3.0000e-3 13 | 14 | # Note: Noise density (continuous-time) from kalibr_allen 15 | gyroscope_noise_density: 1.6968e-04 16 | 17 | # Note: Bias random walk from kalibr_allen 18 | gyroscope_random_walk: 1.9393e-05 19 | 20 | 21 | ##################################################################################### 22 | ##### CAM0 ##### 23 | ##################################################################################### 24 | CAM0: 25 | T_C_I: 26 | - [0.01486554, 0.99955725, -0.02577444, 0.06522291] 27 | - [-0.99988093, 0.01496721, 0.00375619, -0.02070639] 28 | - [0.0041403 , 0.02571553, 0.99966073, -0.0080546] 29 | - [0.0, 0.0, 0.0, 1.0] 30 | # radial-tangential[k1,k2,r1,r2] 31 | distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05] 32 | # f_x, f_y, c_x, c_y 33 | intrinsics: [458.654, 457.296, 367.215, 248.375] 34 | # this parameter not loaded 35 | resolution: [752, 480] 36 | camera_model: pinhole 37 | distortion_model: radtan 38 | # Default:-0.09398272330912658, from kalibr 39 | timeoffset_C_I: 0 40 | # unit in pixel 41 | noise: 0.02 42 | -------------------------------------------------------------------------------- /config/euroc/MH_03_medium/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: ; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing 8 | backend_hz: 100 9 | # sensors will be fused 10 | sensors: [IMU, CAM0] 11 | # rostopics used for each sensor 12 | topics: 13 | - {IMU: /imu0} 14 | - {CAM0: /cam0/image_raw} 15 | # data storage 16 | buffers: 17 | - {CAM0: 10} 18 | 19 | #################################################################################### 20 | ############################ MSCKF estimation setup ########################## 21 | #################################################################################### 22 | 23 | MSCKF: 24 | 25 | ################################ Camera ################################## 26 | 27 | # enable camera exterisic rotation calibration 28 | cam_exterisic_R: false 29 | # enable camera exterisic translation calibration 30 | cam_exterisic_p: false 31 | # enable camera time offset calibration 32 | cam_timeoffset: false 33 | # Max camera clone 34 | cam_clone: 30 35 | 36 | ################################ Update ################################## 37 | 38 | # Index of clone need to marginalize 39 | # example: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 40 | # the marg clone measurements: oldest, second latest [0,8] 41 | # [-1] means all 42 | marg_meas_index: [-1] 43 | 44 | # brief: marginalized pose at slide-windows index (or clone state index) 45 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 46 | marg_pose_index: [-1] 47 | 48 | # in open_vins(40), but it's occasionally update at 40 49 | # but here, tracking will stop so always update as 40 50 | max_msckf_update: 20 51 | 52 | # max line of residual lines for update, maybe used for futures 53 | # max_update_lines: 1500 -------------------------------------------------------------------------------- /config/sim/udel_gore/image.yaml: -------------------------------------------------------------------------------- 1 | ############################################ 2 | #### Camera feature tracking parameters #### 3 | ############################################ 4 | 5 | # option: TRACK_FEATURE, TRACK_KLT, TRACK_DESCRIPTOR 6 | TRACK: 7 | mode: TRACK_FEATURE 8 | num_aruco: 1024 9 | downsize_aruco: false 10 | use_stereo: false 11 | max_camera: 1 12 | cam_id: 0 13 | downsample_ratio: 1.0 14 | # 0: Histogram equalization; 1: contrast limited adaptive histogram equalization (CLAHE) 15 | img_enhancement: 1 16 | 17 | TRACK_FEATURE: 18 | todo: 1 19 | 20 | ############################################ 21 | #### Camera feature triangulation parameters #### 22 | ############################################ 23 | Feature: 24 | # Max condition number of linear triangulation matrix accept triangulated features 25 | max_cond_number: 10000 26 | # Minimum distance to accept triangulated features 27 | min_dist: 0.1 28 | # Minimum distance to accept triangulated features 29 | max_dist: 60 30 | # Multiplier to increase/decrease lambda 31 | lam_mult: 10 32 | # Init lambda for Levenberg-Marquardt optimization 33 | init_lamda: 0.001 34 | # Max runs for Levenberg-Marquardt 35 | max_runs: 5 36 | # Max lambda for Levenberg-Marquardt optimization 37 | max_lamda: 1e10 38 | # Cutoff for dx increment to consider as converged 39 | min_dx: 1e-6 40 | # Cutoff for cost decrement to consider as converged 41 | min_dcost: 1e-6 42 | # Max baseline ratio to accept triangulated features 43 | max_baseline: 40 44 | 45 | ############################################ 46 | #### Keyframe Strategy #### 47 | ############################################ 48 | Keyframe: 49 | # option 1: frame count 50 | frame_count: 2 51 | # option 2: relative motion constraint 52 | frame_motion: 0.0 53 | # 2: 2D space constraint, for up-looking ice surface, 3: 3D space constraint 54 | motion_space: 3 55 | # option 3: minimum tracked features 56 | min_tracked: 50 57 | # option 4: ratio = tracked feature num from last keyframe / total features at current frame 58 | scene_ratio: 1.0 59 | 60 | adaptive_factor: 0.0 61 | 62 | adaptive_power: 1 -------------------------------------------------------------------------------- /config/sim/udel_gore/init.yaml: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | ################### System Initailization Configuration ################# 3 | ############################################################################### 4 | 5 | # This is the parameter must given 6 | # Options: INIT_SETTING, INIT_STATIC, INIT_DVL_PRESSURE, INIT_CAMERA, INIT_DVL_CAMERA 7 | # the given MODE shoud match to setting below 8 | INIT_MODE: INIT_SETTING 9 | 10 | 11 | ############################################################################### 12 | INIT_SETTING: 13 | # # Example: 14 | # SENSOR: 15 | # # the timestamp that this sensor initialized 16 | # time: 1614971124.344753027 17 | # # initialized timeoffset 18 | # temporal: [0.01] 19 | # # q_x q_y q_z q_w x y z v_x v_y v_z bg_x bg_y bg_z ba_x ba_y ba_z 20 | # state: [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01] 21 | # # initialized static transfomration between IMU and this sensor 22 | # extrinsic: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 23 | # # initialized intrinsic parameters for this sensor: the size depend on sensors. e.g. scale for DVL; intrinsics for camera ... 24 | # intrinsic: [1.0, 2.0, 3.0] 25 | # # initialized global transfomration for this sensor: Pressure, Magnetometer, GPS and so on may have this 26 | # global: [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0] 27 | 28 | IMU: 29 | time: 1521753106.831427336 30 | # brief: orientation in quaterion form 31 | state: [ 0.7497, -0.2141, -0.1667, 0.6036, 32 | 0.2620, -1.1057, 0.1599, 33 | -0.2493, -1.1452, 0.0836, 34 | 0.0000, 0.0000, 0.0000, 35 | 0.0000, 0.0000, 0.0000] -------------------------------------------------------------------------------- /config/sim/udel_gore/prior.yaml: -------------------------------------------------------------------------------- 1 | ##################################################################################### 2 | ##### IMU ##### 3 | ##################################################################################### 4 | IMU: 5 | # Default:9.81; 6 | # gravity in the place your robot deployed 7 | # The North Alaska from E. Thiel et al. GRAVITY MEASUREMENTS IN ALASKA 8 | gravity: 9.81 9 | 10 | # Note: Noise density (continuous-time) from dataset from kalibr_allen 11 | # Default: 1.926e-04 from datasheet 12 | accelerometer_noise_density: 2.0000e-3 13 | 14 | # Note: Bias random walk from kalibr_allen 15 | accelerometer_random_walk: 3.0000e-3 16 | 17 | # Note: Noise density (continuous-time) from kalibr_allen 18 | # Default: 8.7e-05 from datasheet 19 | gyroscope_noise_density: 1.6968e-04 20 | 21 | # Note: Bias random walk from kalibr_allen 22 | gyroscope_random_walk: 1.9393e-05 23 | 24 | ##################################################################################### 25 | ##### DVL ##### 26 | ##################################################################################### 27 | DVL: 28 | # imu is reference frame 29 | T_I_D: 30 | - [ 1.0, 0.0, 0.0, 0.0] 31 | - [ 0.0, 1.0, 0.0, 0.0] 32 | - [ 0.0, 0.0, 1.0, 0.0] 33 | - [ 0.0, 0.0, 0.0, 1.0] 34 | # Note: Time offset from Temporal calibrtion between DVL and IMU: (t_imu = t_dvl + t_offset) 35 | # TODO: this value will be overide by IMU-DVL alignment 36 | timeoffset_I_D: 0.0 37 | # Default:1.0; Scale factor for DVL measurement effected by sound speed (caused by salinity, temperature...) 38 | scale: 1.0 39 | # noise on DVL tottom track 3-axis velocity measurement 40 | noise_bt: [0.1, 0.1, 0.1] 41 | 42 | 43 | ##################################################################################### 44 | ##################################################################################### 45 | 46 | CAM0: 47 | T_C_I: 48 | - [ 0.0, 1.0, 0.0, 0.0] 49 | - [-1.0, 0.0, 0.0, -0.05] 50 | - [ 0.0, 0.0, 1.0, 0.0] 51 | - [ 0.0, 0.0, 0.0, 1.0] 52 | # radial-tangential[k1,k2,r1,r2] 53 | distortion_coeffs: [-0.28340811,0.07395907,0.00019359,1.76187114e-05] 54 | # f_x, f_y, c_x, c_y 55 | intrinsics: [458.654,457.296,367.215,248.375] 56 | resolution: [752,480] 57 | timeoffset_I_C: 0 58 | # unit in pixel 59 | noise: 0.0035 60 | camera_model: pinhole 61 | distortion_model: radtan 62 | 63 | ##################################################################################### 64 | ##################################################################################### 65 | 66 | # CAM1: 67 | 68 | -------------------------------------------------------------------------------- /config/sim/udel_gore/system.yaml: -------------------------------------------------------------------------------- 1 | #################################################################################### 2 | ############################ System Parameters ########################## 3 | #################################################################################### 4 | 5 | SYS: 6 | # Default: 20; 7 | # Note: frequency for backend processing(MSCKF update), increase if more data need for processing ( keep it as camera fequency) 8 | backend_hz: 100 9 | # sensors will be fused 10 | # sensors: [IMU, DVL] 11 | sensors: [IMU, CAM0_FEATURE] 12 | # rostopics used for each sensor 13 | topics: 14 | - {IMU: /ov_msckf/imu_sim} 15 | # - {DVL: /ov_msckf/dvl_sim} 16 | - {CAM0_FEATURE: /ov_msckf/feature_sim} 17 | # load some test files 18 | # csv: /home/lin/develop/data/sim/udel_gore/sim_NoCalibNoise_4_feat_position.txt 19 | buffers: 20 | # buffer to store how many seconds 21 | - {CAM0_FEATURE: 10} 22 | 23 | #################################################################################### 24 | ############################ MSCKF estimation setup ########################## 25 | #################################################################################### 26 | 27 | #### MSCKF parameters 28 | MSCKF: 29 | 30 | ################################## DVL ################################### 31 | 32 | # enable DVL exterisic rotation calibration 33 | dvl_exterisic_R: false 34 | # enable DVL exterisic translation calibration 35 | dvl_exterisic_p: false 36 | # enable DVL time offset calibration 37 | dvl_timeoffset: false 38 | # enable DVL scale calibration 39 | dvl_scale: false 40 | 41 | 42 | ################################ Camera ################################## 43 | 44 | ## enable camera exterisic rotation calibration 45 | cam_exterisic_R: false 46 | ## enable camera exterisic translation calibration 47 | cam_exterisic_p: false 48 | ## enable camera time offset calibration 49 | cam_timeoffset: false 50 | ## Max camera clone 51 | ## default: 11 for non-key-frame 52 | cam_clone: 12 53 | 54 | ################################ Update ################################## 55 | 56 | ## Index of clone need to marginalize 57 | ## Option 1: we have 10 clones [0,1,2,3,4,5,6,7,8,9] 58 | ## the marg clone measurements: oldest, second latest [0,8] 59 | ## Option 2: [-1] means use all 60 | # marg_meas_index: [-1] 61 | # marg_meas_index: [0, 10] 62 | marg_meas_index: [1, 10] 63 | 64 | # brief: marginalized pose at slide-windows index (or clone state index) 65 | # note: this select from 'marg_meas_index', no need exactly same as 'marg_meas_index' 66 | # example: [-1] means use first index from 'marg_meas_index' 67 | # marg_pose_index: [-1] 68 | # marg_pose_index: [0, 10] 69 | marg_pose_index: [1, 10] 70 | 71 | ## in open_vins(40), but it's occasionally update at 40 72 | ## but here, tracking will stop so always update as 40 73 | # max_msckf_update: 20 74 | 75 | # ## max line of residual lines for update, maybe used for futures 76 | # # max_update_lines: 1500 77 | 78 | ################################ Marginalization ################################## 79 | -------------------------------------------------------------------------------- /include/feature/Feature.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVINS: An Open Platform for Visual-Inertial Research 3 | * Copyright (C) 2021 Patrick Geneva 4 | * Copyright (C) 2021 Guoquan Huang 5 | * Copyright (C) 2021 OpenVINS Contributors 6 | * Copyright (C) 2019 Kevin Eckenhoff 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | 23 | #include "Feature.h" 24 | 25 | using namespace msckf_dvio; 26 | 27 | void Feature::clean_old_measurements(const std::vector &valid_times) { 28 | 29 | // Loop through each of the cameras we have 30 | for (auto const &pair : timestamps) { 31 | 32 | // Assert that we have all the parts of a measurement 33 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 34 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 35 | 36 | // Our iterators 37 | auto it1 = timestamps[pair.first].begin(); 38 | auto it2 = uvs[pair.first].begin(); 39 | auto it3 = uvs_norm[pair.first].begin(); 40 | 41 | // Loop through measurement times, remove ones that are not in our timestamps 42 | while (it1 != timestamps[pair.first].end()) { 43 | if (std::find(valid_times.begin(), valid_times.end(), *it1) == valid_times.end()) { 44 | it1 = timestamps[pair.first].erase(it1); 45 | it2 = uvs[pair.first].erase(it2); 46 | it3 = uvs_norm[pair.first].erase(it3); 47 | } else { 48 | ++it1; 49 | ++it2; 50 | ++it3; 51 | } 52 | } 53 | } 54 | } 55 | 56 | void Feature::clean_invalid_measurements(const std::vector &invalid_times) { 57 | 58 | // Loop through each of the cameras we have 59 | for (auto const &pair : timestamps) { 60 | 61 | // Assert that we have all the parts of a measurement 62 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 63 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 64 | 65 | // Our iterators 66 | auto it1 = timestamps[pair.first].begin(); 67 | auto it2 = uvs[pair.first].begin(); 68 | auto it3 = uvs_norm[pair.first].begin(); 69 | 70 | // Loop through measurement times, remove ones that are in our timestamps 71 | while (it1 != timestamps[pair.first].end()) { 72 | if (std::find(invalid_times.begin(), invalid_times.end(), *it1) != invalid_times.end()) { 73 | it1 = timestamps[pair.first].erase(it1); 74 | it2 = uvs[pair.first].erase(it2); 75 | it3 = uvs_norm[pair.first].erase(it3); 76 | } else { 77 | ++it1; 78 | ++it2; 79 | ++it3; 80 | } 81 | } 82 | } 83 | } 84 | 85 | void Feature::clean_older_equal_measurements(double timestamp) { 86 | 87 | // Loop through each of the cameras we have 88 | for (auto const &pair : timestamps) { 89 | 90 | // Assert that we have all the parts of a measurement 91 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 92 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 93 | 94 | // Our iterators 95 | auto it1 = timestamps[pair.first].begin(); 96 | auto it2 = uvs[pair.first].begin(); 97 | auto it3 = uvs_norm[pair.first].begin(); 98 | 99 | // Loop through measurement times, remove ones that are older then the specified one 100 | while (it1 != timestamps[pair.first].end()) { 101 | if (*it1 <= timestamp) { 102 | it1 = timestamps[pair.first].erase(it1); 103 | it2 = uvs[pair.first].erase(it2); 104 | it3 = uvs_norm[pair.first].erase(it3); 105 | } else { 106 | ++it1; 107 | ++it2; 108 | ++it3; 109 | } 110 | } 111 | } 112 | } 113 | 114 | void Feature::clean_older_measurements(double timestamp) { 115 | 116 | // Loop through each of the cameras we have 117 | for (auto const &pair : timestamps) { 118 | 119 | // Assert that we have all the parts of a measurement 120 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 121 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 122 | 123 | // Our iterators 124 | auto it1 = timestamps[pair.first].begin(); 125 | auto it2 = uvs[pair.first].begin(); 126 | auto it3 = uvs_norm[pair.first].begin(); 127 | 128 | // Loop through measurement times, remove ones that are older then the specified one 129 | while (it1 != timestamps[pair.first].end()) { 130 | if (*it1 < timestamp) { 131 | it1 = timestamps[pair.first].erase(it1); 132 | it2 = uvs[pair.first].erase(it2); 133 | it3 = uvs_norm[pair.first].erase(it3); 134 | } else { 135 | ++it1; 136 | ++it2; 137 | ++it3; 138 | } 139 | } 140 | } 141 | } 142 | 143 | void Feature::clean_newer_measurements(double timestamp) { 144 | // Loop through each of the cameras we have 145 | for (auto const &pair : timestamps) { 146 | 147 | // Assert that we have all the parts of a measurement 148 | assert(timestamps[pair.first].size() == uvs[pair.first].size()); 149 | assert(timestamps[pair.first].size() == uvs_norm[pair.first].size()); 150 | 151 | // Our iterators 152 | auto it1 = timestamps[pair.first].begin(); 153 | auto it2 = uvs[pair.first].begin(); 154 | auto it3 = uvs_norm[pair.first].begin(); 155 | 156 | // Loop through measurement times, remove ones that are older then the specified one 157 | while (it1 != timestamps[pair.first].end()) { 158 | if (*it1 > timestamp) { 159 | it1 = timestamps[pair.first].erase(it1); 160 | it2 = uvs[pair.first].erase(it2); 161 | it3 = uvs_norm[pair.first].erase(it3); 162 | } else { 163 | ++it1; 164 | ++it2; 165 | ++it3; 166 | } 167 | } 168 | 169 | } 170 | } 171 | 172 | -------------------------------------------------------------------------------- /include/feature/Feature.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVINS: An Open Platform for Visual-Inertial Research 3 | * Copyright (C) 2021 Patrick Geneva 4 | * Copyright (C) 2021 Guoquan Huang 5 | * Copyright (C) 2021 OpenVINS Contributors 6 | * Copyright (C) 2019 Kevin Eckenhoff 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | 23 | #ifndef OV_CORE_FEATURE_H 24 | #define OV_CORE_FEATURE_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace msckf_dvio { 32 | 33 | /** 34 | * @brief Sparse feature class used to collect measurements 35 | * 36 | * This feature class allows for holding of all tracking information for a given feature. 37 | * Each feature has a unique ID assigned to it, and should have a set of feature tracks alongside it. 38 | * See the FeatureDatabase class for details on how we load information into this, and how we delete features. 39 | */ 40 | class Feature { 41 | 42 | public: 43 | /// Unique ID of this feature 44 | size_t featid; 45 | 46 | /// If this feature should be deleted 47 | bool to_delete; 48 | 49 | /// UV coordinates that this feature has been seen from (mapped by camera ID) 50 | std::unordered_map> uvs; 51 | 52 | /// UV normalized coordinates that this feature has been seen from (mapped by camera ID) 53 | std::unordered_map> uvs_norm; 54 | 55 | /// Timestamps of each UV measurement (mapped by camera ID) 56 | std::unordered_map> timestamps; 57 | 58 | /// What camera ID our pose is anchored in!! By default the first measurement is the anchor. 59 | int anchor_cam_id = -1; 60 | 61 | /// Timestamp of anchor clone 62 | double anchor_clone_timestamp; 63 | 64 | /// depth of anchor clone 65 | double anchor_clone_depth = 0; 66 | 67 | /// Triangulated position of this feature, in the anchor frame 68 | Eigen::Vector3d p_FinA; 69 | 70 | /// Triangulated position of this feature, in the global frame 71 | Eigen::Vector3d p_FinG; 72 | 73 | // original Triangulated position of this feature, in the global frame 74 | Eigen::Vector3d p_FinG_original; 75 | 76 | 77 | /// Indicated if this feature is triangulated 78 | bool triangulated = false; 79 | 80 | /** 81 | * @brief Remove measurements that do not occur at passed timestamps. 82 | * 83 | * Given a series of valid timestamps, this will remove all measurements that have not occurred at these times. 84 | * This would normally be used to ensure that the measurements that we have occur at our clone times. 85 | * 86 | * @param valid_times Vector of timestamps that our measurements must occur at 87 | */ 88 | void clean_old_measurements(const std::vector &valid_times); 89 | 90 | /** 91 | * @brief Remove measurements that occur at the invalid timestamps 92 | * 93 | * Given a series of invalid timestamps, this will remove all measurements that have occurred at these times. 94 | * 95 | * @param invalid_times Vector of timestamps that our measurements should not 96 | */ 97 | void clean_invalid_measurements(const std::vector &invalid_times); 98 | 99 | /** 100 | * @brief Remove measurements that are older then and Equal To the specified timestamp. 101 | * 102 | * Given a valid timestamp, this will remove all measurements that have occured earlier then this. 103 | * 104 | * @note this is the old open_vins function(clean_older_measurements) 105 | * @param timestamp Timestamps that our measurements must occur after 106 | */ 107 | void clean_older_equal_measurements(double timestamp); 108 | 109 | /** 110 | * @brief Remove measurements that are older then the specified timestamp. 111 | * 112 | * Given a valid timestamp, this will remove all measurements that have occured earlier then this. 113 | * 114 | * @param timestamp Timestamps that our measurements must occur after 115 | */ 116 | void clean_older_measurements(double timestamp); 117 | 118 | /** 119 | * @brief Remove measurements that are newer then the specified timestamp. 120 | * 121 | * Remove the measurements after current update timestamp, 122 | * this help to select measurements before current update time for asynchronize update 123 | * 124 | * @param timestamp Timestamp 125 | */ 126 | void clean_newer_measurements(double timestamp); 127 | }; 128 | 129 | } // namespace msckf_dvio 130 | 131 | #endif /* OV_CORE_FEATURE_H */ -------------------------------------------------------------------------------- /include/feature/triangulation.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_FEATURE_TRIANGULATION_H 2 | #define MSCKF_FEATURE_TRIANGULATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "utils/utils.h" 10 | #include "types/parameters.h" 11 | #include "Feature.h" 12 | 13 | namespace msckf_dvio 14 | { 15 | 16 | class FeatureTriangulation { 17 | 18 | public: 19 | FeatureTriangulation(paramTriangulation param_trig); 20 | 21 | double compute_error(Feature *feature, 22 | const std::unordered_map &T_G_C, 23 | double alpha, double beta, double rho); 24 | 25 | bool single_triangulation(Feature *feature, 26 | const std::unordered_map &T_G_C); 27 | 28 | bool single_gaussnewton(Feature *feature, 29 | const std::unordered_map &T_G_C); 30 | 31 | private: 32 | 33 | paramTriangulation param_trig_; 34 | 35 | const char *file_path="/home/lin/Desktop/msckf_manager.txt"; 36 | std::ofstream file; 37 | }; 38 | 39 | } 40 | 41 | #endif // MSCKF_FEATURE_TRIANGULATION_H -------------------------------------------------------------------------------- /include/initializer/initializer.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_INITIALIZER_BASE_H_ 2 | #define MSCKF_INITIALIZER_BASE_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "types/type_all.h" 8 | #include "utils/utils.h" 9 | #include "msckf/state.h" 10 | 11 | namespace msckf_dvio { 12 | 13 | class Initializer 14 | { 15 | public: 16 | Initializer(paramInit param_init_) 17 | : initialized(false), param_init(param_init_) 18 | {} 19 | 20 | virtual ~Initializer(){}; 21 | 22 | bool isInit() { 23 | return initialized; 24 | } 25 | 26 | virtual void checkInit() = 0; 27 | 28 | /** 29 | * @brief update the initialization result to state 30 | * 31 | * @param state: system state 32 | * @param params: parameters 33 | * @param data_time: initialized timestamp for each sensor 34 | */ 35 | virtual void updateInit(std::shared_ptr state, Params ¶ms, std::map &data_time) = 0; 36 | 37 | virtual void cleanBuffer() = 0; 38 | 39 | // check if this sensor will used in initialization 40 | virtual bool useSensor(const Sensor &sensor) = 0; 41 | 42 | void feedImu(const ImuMsg &data) { 43 | std::unique_lock lck(buffer_mutex); 44 | 45 | buffer_imu.emplace_back(data); 46 | } 47 | 48 | void feedDvl(const DvlMsg &data) { 49 | std::unique_lock lck(buffer_mutex); 50 | 51 | buffer_dvl.emplace_back(data); 52 | } 53 | 54 | void feedPressure(const PressureMsg &data) { 55 | std::unique_lock lck(buffer_mutex); 56 | 57 | buffer_pressure.emplace_back(data); 58 | } 59 | 60 | protected: 61 | 62 | std::atomic initialized; 63 | 64 | paramInit param_init; 65 | 66 | std::vector sensors; 67 | 68 | std::recursive_mutex buffer_mutex; 69 | 70 | std::vector buffer_imu; 71 | std::vector buffer_dvl; 72 | std::vector buffer_pressure; 73 | }; 74 | 75 | } // namespace msckf_dvio 76 | 77 | #endif //MSCKF_INITIALIZER_BASE_H_ -------------------------------------------------------------------------------- /include/initializer/initializer_dvl_aided.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_INITIALIZER_DVL_AIDED_H_ 2 | #define MSCKF_INITIALIZER_DVL_AIDED_H_ 3 | 4 | #include "initializer.h" 5 | 6 | namespace msckf_dvio { 7 | 8 | class InitDvlAided : public Initializer 9 | { 10 | 11 | public: 12 | InitDvlAided(paramInit param_init_, priorImu prior_imu_, priorDvl prior_dvl_); 13 | 14 | ~InitDvlAided(){} 15 | 16 | void checkInit() override; 17 | 18 | bool useSensor(const Sensor &sensor) override; 19 | 20 | void updateInit(std::shared_ptr state, Params ¶ms, std::map &data_time) override; 21 | 22 | void cleanBuffer() override; 23 | 24 | protected: 25 | 26 | private: 27 | void findAlignmentImu(); 28 | 29 | void findAlignmentDvl(); 30 | 31 | bool grabInitializationData(std::vector &dvl_a, 32 | std::vector &imu_a, 33 | std::vector &imu_g, 34 | std::vector &pres_align); 35 | 36 | void linearInterp(const std::vector &imu_in, 37 | const std::vector &dvl_in, 38 | std::vector &dvl_out); 39 | 40 | void doInitialization(const std::vector &dvl_a, 41 | const std::vector &imu_a, 42 | const std::vector &imu_g, 43 | const std::vector &pres_align); 44 | 45 | priorImu prior_imu; 46 | priorDvl prior_dvl; 47 | 48 | int last_index_imu, last_index_dvl; 49 | 50 | //// IMU data, variance for one section in each IMU window 51 | std::vector, double>> sections_imu; 52 | std::vector sections_dvl; 53 | 54 | //! TODO: replace this with state update 55 | 56 | double time_I_align, time_I_init; 57 | Eigen::Vector4d q_I_G; 58 | Eigen::Vector3d p_G_I; 59 | Eigen::Vector3d v_G_I; 60 | Eigen::Vector3d bg_avg; 61 | Eigen::Vector3d ba_avg; 62 | 63 | double time_D_align, time_D_init; 64 | double time_I_D; 65 | DvlMsg dvl_msg; 66 | 67 | double pres_begin_avg; 68 | double pres_begin_var; 69 | PressureMsg pres_init; 70 | 71 | }; 72 | 73 | } 74 | #endif //MSCKF_INITIALIZER_DVL_AIDED_H_ -------------------------------------------------------------------------------- /include/initializer/initializer_setting.cpp: -------------------------------------------------------------------------------- 1 | #include "initializer_setting.h" 2 | 3 | using namespace msckf_dvio; 4 | 5 | InitSetting::InitSetting(paramInit param_init) 6 | : Initializer(param_init) { 7 | 8 | // setup the sensors used for this system initialization 9 | for(const auto& [sensor, param] : param_init.setting) { 10 | sensors.push_back(sensor); 11 | } 12 | 13 | } 14 | 15 | bool InitSetting::useSensor(const Sensor &sensor) { 16 | if(std::find(sensors.begin(), sensors.end(), sensor) != sensors.end()){ 17 | return true; 18 | } 19 | else { 20 | return false; 21 | } 22 | } 23 | 24 | void InitSetting::checkInit() { 25 | std::unique_lock lck(buffer_mutex); 26 | 27 | int count = 0; 28 | 29 | for(const auto& sensor : sensors) { 30 | // check all data buffer are available 31 | switch (sensor) 32 | { 33 | case Sensor::IMU: { 34 | if(buffer_imu.size() > 0 && 35 | buffer_imu.back().time >= param_init.setting[Sensor::IMU].time) { 36 | count ++; 37 | } 38 | break; 39 | } 40 | 41 | case Sensor::DVL: { 42 | if(buffer_dvl.size() > 0 && 43 | buffer_dvl.back().time >= param_init.setting[Sensor::DVL].time) { 44 | count ++; 45 | } 46 | break; 47 | } 48 | 49 | case Sensor::PRESSURE: { 50 | if(buffer_pressure.size() > 0 && 51 | buffer_pressure.back().time >= param_init.setting[Sensor::PRESSURE].time) { 52 | count ++; 53 | } 54 | break; 55 | } 56 | 57 | default: 58 | break; 59 | } 60 | } 61 | 62 | // check if all the sensors are available 63 | if(count == sensors.size()) { 64 | initialized = true; 65 | 66 | cleanBuffer(); 67 | } 68 | 69 | } 70 | 71 | void InitSetting::updateInit(std::shared_ptr state, 72 | Params ¶ms, 73 | std::map &data_time) { 74 | 75 | 76 | std::cout<<"\n+++++++++++++++++++++++++++++++++++++++\n"; 77 | std::cout<<"Initialization result: \n"; 78 | 79 | // loop each sensor's init parameter 80 | for(const auto& [sensor, param] : params.init.setting) { 81 | 82 | // stup for each sensor 83 | switch(sensor) { 84 | case Sensor::IMU: { 85 | 86 | // convert to Eigen form 87 | assert(param.state.size() == 16); 88 | Eigen::Matrix imu_state; 89 | for(size_t i = 0; i < param.state.size(); i++) { 90 | imu_state.segment(i,1) = Eigen::Matrix(param.state.at(i)); 91 | } 92 | 93 | // update to state 94 | state->setTimestamp(param.time); 95 | state->setImuValue(imu_state); 96 | 97 | // add to time 98 | data_time[Sensor::IMU] = param.time; 99 | 100 | // print 101 | std::cout<setEstimationValue(DVL, EST_TIMEOFFSET, Eigen::MatrixXd::Constant(1,1,param.temporal.at(0))); 115 | else 116 | params.prior_dvl.timeoffset = param.temporal.at(0); 117 | 118 | // add to time 119 | data_time[Sensor::DVL] = param.time; 120 | 121 | // print 122 | std::cout<setPressureInit(param.global.at(0)); 133 | 134 | // add to time 135 | data_time[Sensor::PRESSURE] = param.time; 136 | 137 | // print 138 | std::cout< lck(buffer_mutex); 154 | 155 | // clear base buffer 156 | std::vector().swap(buffer_imu); 157 | std::vector().swap(buffer_dvl); 158 | std::vector().swap(buffer_pressure); 159 | 160 | // clear this initializer buffer 161 | // do something? 162 | } -------------------------------------------------------------------------------- /include/initializer/initializer_setting.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_INITIALIZER_SETTING_H_ 2 | #define MSCKF_INITIALIZER_SETTING_H_ 3 | 4 | #include "initializer.h" 5 | 6 | namespace msckf_dvio { 7 | 8 | class InitSetting : public Initializer 9 | { 10 | 11 | public: 12 | InitSetting(paramInit param_init); 13 | 14 | ~InitSetting(){} 15 | 16 | void checkInit() override; 17 | 18 | bool useSensor(const Sensor &sensor) override; 19 | 20 | void updateInit(std::shared_ptr state, Params ¶ms, std::map &data_time) override; 21 | 22 | void cleanBuffer() override; 23 | 24 | private: 25 | 26 | // IMU 27 | 28 | // DVL 29 | 30 | // PRESSURE 31 | 32 | }; 33 | 34 | } 35 | 36 | #endif //MSCKF_INITIALIZER_SETTING_H_ 37 | -------------------------------------------------------------------------------- /include/msckf/predictor.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_MSCKF_PREDICTOR_H 2 | #define MSCKF_MSCKF_PREDICTOR_H 3 | 4 | #include 5 | 6 | #include "types/type_all.h" 7 | #include "utils/utils.h" 8 | #include "msckf/state.h" 9 | 10 | 11 | namespace msckf_dvio 12 | { 13 | 14 | class Predictor { 15 | 16 | public: 17 | Predictor(priorImu prior_imu); 18 | 19 | void propagate(std::shared_ptr state, const std::vector &data); 20 | 21 | void augment(Sensor sensor_name, 22 | Sensor clone_name, 23 | std::shared_ptr state, 24 | double sensor_time, 25 | const Eigen::Vector3d &w); 26 | 27 | private: 28 | void propagateState(std::shared_ptr state, 29 | const ImuMsg &data_begin, 30 | const ImuMsg &data_end, 31 | Eigen::Matrix &F, 32 | Eigen::Matrix &Qd); 33 | 34 | void propagateCovariance(std::shared_ptr state, 35 | const Eigen::MatrixXd &Phi, 36 | const Eigen::MatrixXd &Q); 37 | 38 | void predict_mean_rk4(std::shared_ptr state, double dt, 39 | const Eigen::Vector3d &w_hat1, const Eigen::Vector3d &a_hat1, 40 | const Eigen::Vector3d &w_hat2, const Eigen::Vector3d &a_hat2, 41 | Eigen::Vector4d &new_q, Eigen::Vector3d &new_v, Eigen::Vector3d &new_p); 42 | 43 | /// prior information for IMU 44 | priorImu prior_imu_; 45 | 46 | //! TEST: 47 | const char *file_path="/home/lin/Desktop/msckf_manager.txt"; 48 | std::ofstream file; 49 | // file.open(file_path, std::ios_base::app);//std::ios_base::app 50 | // file.close(); 51 | }; 52 | 53 | } // namespace msckf_dvio 54 | 55 | 56 | #endif // MSCKF_MSCKF_PREDICTOR_H -------------------------------------------------------------------------------- /include/msckf/updater.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_MSCKF_UPDATER_H 2 | #define MSCKF_MSCKF_UPDATER_H 3 | 4 | #include 5 | 6 | #include "types/type_all.h" 7 | #include "utils/utils.h" 8 | #include "utils/time_cost.h" 9 | #include "msckf/state.h" 10 | 11 | #include "feature/Feature.h" 12 | #include "feature/triangulation.h" 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace msckf_dvio 21 | { 22 | 23 | class Updater { 24 | 25 | public: 26 | Updater(Params ¶ms); 27 | 28 | void updateDvl(std::shared_ptr state, const Eigen::Vector3d &w_I, const Eigen::Vector3d &v_D); 29 | 30 | void updatePressure(std::shared_ptr state, const double pres_begin, const double pres_curr); 31 | 32 | void updatePressureManual(std::shared_ptr state, const double pres_begin, const double pres_curr, int option); 33 | 34 | void marginalize(std::shared_ptr state, Sensor clone_name, int index); 35 | 36 | void updateCam(std::shared_ptr state, std::vector &features); 37 | 38 | void updateCamPart(std::shared_ptr state, std::vector &features); 39 | 40 | void cameraMeasurement( 41 | std::shared_ptr state, 42 | std::vector &features); 43 | 44 | void featureTriangulation( 45 | std::shared_ptr state, 46 | std::vector &feat); 47 | 48 | void featureJacobian(std::shared_ptr state, const Feature &feature, 49 | Eigen::MatrixXd &H_x, Eigen::MatrixXd &H_f, 50 | Eigen::VectorXd & res); 51 | 52 | void featureJacobianPart( 53 | std::shared_ptr state, const Feature &feature, 54 | Eigen::MatrixXd &H_x, Eigen::MatrixXd &H_f, 55 | Eigen::VectorXd & res, std::vector> &x_order); 56 | 57 | void nullspace_project(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 58 | 59 | void nullspace_project_inplace(Eigen::MatrixXd &H_f, Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 60 | 61 | bool chiSquareCam( 62 | std::shared_ptr state, const Eigen::MatrixXd &H_x, 63 | const Eigen::VectorXd &r, std::vector> x_order); 64 | 65 | bool chiSquareDvl(const Eigen::MatrixXd &S, const Eigen::VectorXd &r); 66 | 67 | void compress(Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 68 | 69 | void compress_inplace(Eigen::MatrixXd &H_x, Eigen::VectorXd &res); 70 | 71 | void update( 72 | std::shared_ptr state, const std::vector> &H_order, 73 | const Eigen::MatrixXd &H, const Eigen::VectorXd &res, const Eigen::MatrixXd &R); 74 | 75 | private: 76 | priorDvl prior_dvl_; 77 | 78 | priorPressure prior_pressure_; 79 | 80 | priorCam prior_cam_; 81 | 82 | paramMsckf param_msckf_; 83 | 84 | std::unique_ptr triangulater; 85 | 86 | /// Chi squared 95th percentile table (lookup would be size of residual) 87 | std::map chi_squared_table; 88 | 89 | //! TEST: 90 | long long int count; 91 | const char *file_path="/home/lin/Desktop/msckf_manager.txt"; 92 | std::ofstream file; 93 | // file.open(file_path, std::ios_base::app);//std::ios_base::app 94 | // file<<"\n"<<"aaa "<. 20 | */ 21 | 22 | 23 | #ifndef OV_CORE_GRIDER_FAST_H 24 | #define OV_CORE_GRIDER_FAST_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "utils/lambda_body.h" 36 | 37 | namespace msckf_dvio { 38 | 39 | /** 40 | * @brief Extracts FAST features in a grid pattern. 41 | * 42 | * As compared to just extracting fast features over the entire image, 43 | * we want to have as uniform of extractions as possible over the image plane. 44 | * Thus we split the image into a bunch of small grids, and extract points in each. 45 | * We then pick enough top points in each grid so that we have the total number of desired points. 46 | */ 47 | class Grider_FAST { 48 | 49 | public: 50 | /** 51 | * @brief Compare keypoints based on their response value. 52 | * @param first First keypoint 53 | * @param second Second keypoint 54 | * 55 | * We want to have the keypoints with the highest values! 56 | * See: https://stackoverflow.com/a/10910921 57 | */ 58 | static bool compare_response(cv::KeyPoint first, cv::KeyPoint second) { return first.response > second.response; } 59 | 60 | /** 61 | * @brief This function will perform grid extraction using FAST. 62 | * @param img Image we will do FAST extraction on 63 | * @param pts vector of extracted points we will return 64 | * @param num_features max number of features we want to extract 65 | * @param grid_x size of grid in the x-direction / u-direction 66 | * @param grid_y size of grid in the y-direction / v-direction 67 | * @param threshold FAST threshold paramter (10 is a good value normally) 68 | * @param nonmaxSuppression if FAST should perform non-max suppression (true normally) 69 | * 70 | * Given a specified grid size, this will try to extract fast features from each grid. 71 | * It will then return the best from each grid in the return vector. 72 | */ 73 | static void perform_griding(const cv::Mat &img, std::vector &pts, int num_features, int grid_x, int grid_y, int threshold, 74 | bool nonmaxSuppression) { 75 | 76 | // Calculate the size our extraction boxes should be 77 | int size_x = img.cols / grid_x; 78 | int size_y = img.rows / grid_y; 79 | 80 | // Make sure our sizes are not zero 81 | assert(size_x > 0); 82 | assert(size_y > 0); 83 | 84 | // We want to have equally distributed features 85 | auto num_features_grid = (int)(num_features / (grid_x * grid_y)) + 1; 86 | 87 | // Parallelize our 2d grid extraction!! 88 | int ct_cols = std::floor(img.cols / size_x); 89 | int ct_rows = std::floor(img.rows / size_y); 90 | std::vector> collection(ct_cols * ct_rows); 91 | parallel_for_(cv::Range(0, ct_cols * ct_rows), LambdaBody([&](const cv::Range &range) { 92 | for (int r = range.start; r < range.end; r++) { 93 | // Calculate what cell xy value we are in 94 | int x = r % ct_cols * size_x; 95 | int y = r / ct_cols * size_y; 96 | 97 | // Skip if we are out of bounds 98 | if (x + size_x > img.cols || y + size_y > img.rows) 99 | continue; 100 | 101 | // Calculate where we should be extracting from 102 | cv::Rect img_roi = cv::Rect(x, y, size_x, size_y); 103 | 104 | // Extract FAST features for this part of the image 105 | std::vector pts_new; 106 | cv::FAST(img(img_roi), pts_new, threshold, nonmaxSuppression); 107 | 108 | // Now lets get the top number from this 109 | std::sort(pts_new.begin(), pts_new.end(), Grider_FAST::compare_response); 110 | 111 | // Append the "best" ones to our vector 112 | // Note that we need to "correct" the point u,v since we extracted it in a ROI 113 | // So we should append the location of that ROI in the image 114 | for (size_t i = 0; i < (size_t)num_features_grid && i < pts_new.size(); i++) { 115 | cv::KeyPoint pt_cor = pts_new.at(i); 116 | pt_cor.pt.x += (float)x; 117 | pt_cor.pt.y += (float)y; 118 | collection.at(r).push_back(pt_cor); 119 | } 120 | } 121 | })); 122 | 123 | // Combine all the collections into our single vector 124 | for (size_t r = 0; r < collection.size(); r++) { 125 | pts.insert(pts.end(), collection.at(r).begin(), collection.at(r).end()); 126 | } 127 | 128 | // Return if no points 129 | if (pts.empty()) 130 | return; 131 | 132 | // Sub-pixel refinement parameters 133 | cv::Size win_size = cv::Size(5, 5); 134 | cv::Size zero_zone = cv::Size(-1, -1); 135 | cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.001); 136 | 137 | // Get vector of points 138 | std::vector pts_refined; 139 | for (size_t i = 0; i < pts.size(); i++) { 140 | pts_refined.push_back(pts.at(i).pt); 141 | } 142 | 143 | // Finally get sub-pixel for all extracted features 144 | cv::cornerSubPix(img, pts_refined, win_size, zero_zone, term_crit); 145 | 146 | // Save the refined points! 147 | for (size_t i = 0; i < pts.size(); i++) { 148 | pts.at(i).pt = pts_refined.at(i); 149 | } 150 | } 151 | }; 152 | 153 | } // namespace msckf_dvio 154 | 155 | #endif /* OV_CORE_GRIDER_FAST_H */ 156 | -------------------------------------------------------------------------------- /include/tracker/Grider_STC.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef OV_CORE_GRIDER_STC_H 3 | #define OV_CORE_GRIDER_STC_H 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "utils/lambda_body.h" 15 | 16 | namespace msckf_dvio { 17 | 18 | /** 19 | * @brief Extracts shi-tomasi corner (STC) features in a grid pattern. 20 | * 21 | * As compared to just extracting shi-tomasi corner features over the entire image, 22 | * we want to have as uniform of extractions as possible over the image plane. 23 | * Thus we split the image into a bunch of small grids, and extract points in each. 24 | * We then pick enough top points in each grid so that we have the total number of desired points. 25 | */ 26 | class Grider_STC { 27 | 28 | public: 29 | 30 | /** 31 | * @brief This function will perform grid extraction using FAST. 32 | * @param img Image we will do FAST extraction on 33 | * @param pts vector of extracted points we will return 34 | * @param num_features max number of features we want to extract 35 | * @param grid_x size of grid in the x-direction / u-direction 36 | * @param grid_y size of grid in the y-direction / v-direction 37 | * @param min_dist Minimum possible Euclidean distance of corners 38 | * 39 | * Given a specified grid size, this will try to extract fast features from each grid. 40 | * It will then return the best from each grid in the return vector. 41 | */ 42 | static void perform_griding(const cv::Mat &img, std::vector &pts, int num_features, 43 | int grid_x, int grid_y, int min_dist) { 44 | 45 | // Calculate the size our extraction boxes should be 46 | int size_x = img.cols / grid_x; 47 | int size_y = img.rows / grid_y; 48 | 49 | // Make sure our sizes are not zero 50 | assert(size_x > 0); 51 | assert(size_y > 0); 52 | 53 | // We want to have equally distributed features 54 | auto num_features_grid = (int)(num_features / (grid_x * grid_y)) + 1; 55 | 56 | // Parallelize our 2d grid extraction!! 57 | int ct_cols = std::floor(img.cols / size_x); 58 | int ct_rows = std::floor(img.rows / size_y); 59 | std::vector> collection(ct_cols * ct_rows); 60 | 61 | parallel_for_(cv::Range(0, ct_cols * ct_rows), LambdaBody([&](const cv::Range &range) { 62 | for (int r = range.start; r < range.end; r++) { 63 | // Calculate what cell xy value we are in 64 | int x = r % ct_cols * size_x; 65 | int y = r / ct_cols * size_y; 66 | 67 | // Skip if we are out of bounds 68 | if (x + size_x > img.cols || y + size_y > img.rows) 69 | continue; 70 | 71 | // Calculate where we should be extracting from 72 | cv::Rect img_roi = cv::Rect(x, y, size_x, size_y); 73 | 74 | // Extract shi-tomasi corner features for this part of the image 75 | std::vector pts_new; 76 | cv::goodFeaturesToTrack(img(img_roi), pts_new, num_features_grid, 0.01, 77 | min_dist, cv::Mat(), 7, false, 0.04); 78 | 79 | // Append the pts to our vector 80 | // Note that we need to "correct" the point u,v since we extracted it in a ROI 81 | // So we should append the location of that ROI in the image 82 | for (size_t i =0; i < pts_new.size(); i++) { 83 | cv::KeyPoint pt_cor; 84 | pt_cor.pt.x = pts_new.at(i).x + (float)x; 85 | pt_cor.pt.y = pts_new.at(i).y + (float)y; 86 | collection.at(r).push_back(pt_cor); 87 | } 88 | } 89 | })); 90 | 91 | // Combine all the collections into our single vector 92 | for (size_t r = 0; r < collection.size(); r++) { 93 | pts.insert(pts.end(), collection.at(r).begin(), collection.at(r).end()); 94 | } 95 | 96 | // Return if no points 97 | if (pts.empty()) 98 | return; 99 | 100 | // Sub-pixel refinement parameters 101 | cv::Size win_size = cv::Size(5, 5); 102 | cv::Size zero_zone = cv::Size(-1, -1); 103 | cv::TermCriteria term_crit = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 20, 0.001); 104 | 105 | // Get vector of points 106 | std::vector pts_refined; 107 | for (size_t i = 0; i < pts.size(); i++) { 108 | pts_refined.push_back(pts.at(i).pt); 109 | } 110 | 111 | // Finally get sub-pixel for all extracted features 112 | cv::cornerSubPix(img, pts_refined, win_size, zero_zone, term_crit); 113 | 114 | // Save the refined points! 115 | for (size_t i = 0; i < pts.size(); i++) { 116 | pts.at(i).pt = pts_refined.at(i); 117 | } 118 | 119 | } 120 | 121 | }; 122 | 123 | } // namesapce msckf_dvio 124 | 125 | #endif /* OV_CORE_GRIDER_STC_H */ 126 | -------------------------------------------------------------------------------- /include/tracker/TrackFeature.cpp: -------------------------------------------------------------------------------- 1 | #include "TrackFeature.h" 2 | 3 | using namespace msckf_dvio; 4 | 5 | //! TODO: only support mono camera features 6 | void TrackFeature::feed_features(FeatureMsg &data) { 7 | // make sure all the data size are same 8 | assert(data.id.size() == data.u.size()); 9 | assert(data.u.size() == data.v.size()); 10 | 11 | // only support mono camera features 12 | int cam_id = 0; 13 | 14 | // Our good ids and points 15 | std::vector good_left; 16 | std::vector good_ids_left; 17 | 18 | for(size_t i = 0; i < data.id.size(); i++ ) { 19 | // id 20 | size_t id = data.id[i] + currid; 21 | 22 | // Create the keypoint 23 | cv::KeyPoint kpt; 24 | kpt.pt.x = data.u[i]; 25 | kpt.pt.y = data.v[i]; 26 | 27 | // save 28 | good_left.push_back(kpt); 29 | good_ids_left.push_back(id); 30 | 31 | // Append to the database 32 | cv::Point2f npt_l = undistort_point(kpt.pt, cam_id); 33 | database->update_feature(id, data.time, cam_id, kpt.pt.x, kpt.pt.y, npt_l.x, npt_l.y); 34 | } 35 | 36 | // Get our width and height 37 | auto wh = camera_wh.at(cam_id); 38 | 39 | // Move forward in time 40 | img_last[cam_id] = cv::Mat::zeros(cv::Size(wh.first, wh.second), CV_8UC1); 41 | pts_last[cam_id] = good_left; 42 | ids_last[cam_id] = good_ids_left; 43 | } -------------------------------------------------------------------------------- /include/tracker/TrackFeature.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVINS: An Open Platform for Visual-Inertial Research 3 | * Copyright (C) 2021 Patrick Geneva 4 | * Copyright (C) 2021 Guoquan Huang 5 | * Copyright (C) 2021 OpenVINS Contributors 6 | * Copyright (C) 2019 Kevin Eckenhoff 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | 23 | #ifndef OV_CORE_TRACK_FEATURE_H 24 | #define OV_CORE_TRACK_FEATURE_H 25 | 26 | #include "TrackBase.h" 27 | 28 | namespace msckf_dvio { 29 | 30 | /** 31 | * @brief Tracker to append uv measurements to database 32 | * 33 | * used for simulated data, or tracking processing did on different side 34 | */ 35 | class TrackFeature : public TrackBase { 36 | 37 | public: 38 | /** 39 | * @brief Public constructor with configuration variables 40 | * @param numaruco the max id of the arucotags, so we ensure that we start our non-auroc features above this value 41 | */ 42 | TrackFeature(int numaruco, std::map> &wh) : TrackBase(0, numaruco) { 43 | this->camera_wh = wh; 44 | } 45 | 46 | /// @warning This function should not be used!! Use @ref feed_measurement_simulation() instead. 47 | void feed_monocular(double timestamp, cv::Mat &img, size_t cam_id) override { 48 | printf("[TrackFeature]: THIS feed_monocular SHOULD NEVER HAPPEN!\n"); 49 | std::exit(EXIT_FAILURE); 50 | } 51 | 52 | /// @warning This function should not be used!! Use @ref feed_measurement_simulation() instead. 53 | void feed_stereo(double timestamp, cv::Mat &img_left, cv::Mat &img_right, size_t cam_id_left, size_t cam_id_right) override { 54 | printf("[TrackFeature]: THIS feed_stereo SHOULD NEVER HAPPEN!\n"); 55 | std::exit(EXIT_FAILURE); 56 | } 57 | 58 | /** 59 | * @brief Feed function for given tracked features (either from simulation or another tracker) 60 | * @param data time, features that each one has u,v,id 61 | */ 62 | void feed_features(FeatureMsg &data) override; 63 | 64 | protected: 65 | /// Width and height of our cameras 66 | std::map> camera_wh; 67 | }; 68 | 69 | } // namespace msckf_dvio 70 | 71 | #endif /* OV_CORE_TRACK_FEATURE_H */ 72 | -------------------------------------------------------------------------------- /include/types/msgs.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_MSG_H_ 2 | #define MSCKF_TYPE_MSG_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace msckf_dvio { 8 | 9 | /******* ROS message data ********/ 10 | 11 | struct ImuMsg { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | // IMU timestamp 15 | double time; 16 | 17 | // IMU 3-axis acceleration 18 | Eigen::Vector3d a; 19 | 20 | // IMU 3-axis gyro 21 | Eigen::Vector3d w; 22 | 23 | // Sort IMU measurement with time 24 | bool operator<(const ImuMsg &other) const { return time < other.time; } 25 | }; 26 | 27 | struct DvlMsg { 28 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 29 | 30 | // timestamp 31 | double time; 32 | 33 | // 3-axis velocity 34 | Eigen::Vector3d v; 35 | 36 | // 3-axis acceleration 37 | Eigen::Vector3d a; 38 | 39 | // Sort sensor measurement with time 40 | bool operator<(const DvlMsg &other) const { return time < other.time; } 41 | 42 | DvlMsg() {} 43 | DvlMsg(const double &time_, const Eigen::Vector3d &v_, const Eigen::Vector3d &a_) 44 | : time(time_), v(v_), a(a_) {} 45 | }; 46 | 47 | struct ImageMsg { 48 | 49 | // Image timestamp 50 | double time; 51 | 52 | // Image 53 | cv::Mat image; 54 | 55 | // Sort image with time 56 | bool operator<(const ImageMsg &other) const { return time < other.time; } 57 | }; 58 | 59 | struct PressureMsg { 60 | 61 | // timestamp 62 | double time; 63 | 64 | // pressure 65 | double p; 66 | 67 | // Sort data with time 68 | bool operator<(const PressureMsg &other) const { return time < other.time; } 69 | }; 70 | 71 | 72 | struct FeatureMsg { 73 | 74 | // timestamp 75 | double time; 76 | 77 | // u measurement of a image 78 | std::vector u; 79 | 80 | // v measurement of a image 81 | std::vector v; 82 | 83 | // id of this feature measurement 84 | std::vector id; 85 | 86 | // Sort data with time 87 | bool operator<(const FeatureMsg &other) const { return time < other.time; } 88 | }; 89 | 90 | } // namespace msckf_dvio 91 | 92 | #endif //MSCKF_TYPE_MSG_H_ -------------------------------------------------------------------------------- /include/types/poseJPL.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_POSE_JPL_H_ 2 | #define MSCKF_TYPE_POSE_JPL_H_ 3 | 4 | #include "type.h" 5 | #include "utils/utils.h" 6 | 7 | namespace msckf_dvio { 8 | 9 | class PoseJPL : public Type 10 | { 11 | public: 12 | PoseJPL() : Type(6) { 13 | Eigen::MatrixXd pose0 = Eigen::MatrixXd::Zero(7,1); 14 | pose0(3) = 1.0; 15 | setValue(pose0); 16 | setFej(pose0); 17 | } 18 | 19 | ~PoseJPL() {} 20 | 21 | void setValue(const Eigen::MatrixXd &new_value) override { 22 | assert(new_value.rows() == 7); 23 | assert(new_value.cols() == 1); 24 | 25 | value_ = new_value; 26 | } 27 | 28 | void setFej(const Eigen::MatrixXd &new_value) override { 29 | assert(new_value.rows() == 7); 30 | assert(new_value.cols() == 1); 31 | 32 | fej_ = new_value; 33 | } 34 | 35 | void update(const Eigen::VectorXd &dx) override { 36 | 37 | assert(dx.rows() == size_); 38 | 39 | // quaternion 40 | Eigen::Matrix dq; 41 | dq << .5 * dx.block(0,0,3,1), 1.0; 42 | dq = normalizeQuat(dq); 43 | 44 | // Update 45 | Eigen::Matrix pose; 46 | pose.block(0,0,4,1) = multiplyQuat(dq, value_.block(0,0,4,1)); 47 | pose.block(4,0,3,1) = value_.block(4,0,3,1) + dx.block(3,0,3,1); 48 | setValue(pose); 49 | } 50 | 51 | std::shared_ptr clone() override { 52 | auto Clone = std::shared_ptr(new PoseJPL()); 53 | Clone->setValue(getValue()); 54 | Clone->setFej(getFej()); 55 | return Clone; 56 | } 57 | 58 | protected: 59 | 60 | }; 61 | 62 | } // namespace msckf_dvio 63 | 64 | #endif //MSCKF_TYPE_POSE_JPL_H_ -------------------------------------------------------------------------------- /include/types/quatJPL.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_QUAT_JPL_H_ 2 | #define MSCKF_TYPE_QUAT_JPL_H_ 3 | 4 | #include "type.h" 5 | #include "utils/utils.h" 6 | 7 | namespace msckf_dvio { 8 | 9 | //! @brief: Derived Type class that implements JPL quaternion 10 | //! @note: 11 | //! This quaternion uses a left-multiplicative error state and follows the "JPL convention". 12 | //! Please checkout our utility functions in the quat_ops.h file. 13 | //! We recommend that people new quaternions check out the following resources: 14 | //! - http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf 15 | //! - ftp://naif.jpl.nasa.gov/pub/naif/misc/Quaternion_White_Paper/Quaternions_White_Paper.pdf 16 | //! 17 | class QuatJPL : public Type 18 | { 19 | public: 20 | QuatJPL() : Type(3) { 21 | Eigen::Vector4d q0 = Eigen::Vector4d::Zero(); 22 | q0(3) = 1.0; 23 | setValue(q0); 24 | setFej(q0); 25 | } 26 | 27 | ~QuatJPL() {} 28 | 29 | //! @brief Sets the value of the estimate and recomputes the internal rotation matrix 30 | //! @param new_value New value for the quaternion estimate 31 | //! 32 | void setValue(const Eigen::MatrixXd &new_value) override { 33 | assert(new_value.rows() == 4); 34 | assert(new_value.cols() == 1); 35 | 36 | value_ = new_value; 37 | 38 | // // compute associated rotation 39 | // R_ = toRotationMatrix(new_value); 40 | } 41 | 42 | //! @brief Sets the fej value and recomputes the fej rotation matrix 43 | //! @param new_value New value for the quaternion estimate 44 | //! 45 | void setFej(const Eigen::MatrixXd &new_value) override { 46 | assert(new_value.rows() == 4); 47 | assert(new_value.cols() == 1); 48 | 49 | fej_ = new_value; 50 | 51 | // // compute associated rotation 52 | // R_fej_ = toRotationMatrix(new_value); 53 | } 54 | 55 | // //! @brief Rotation access 56 | // //! 57 | // Eigen::Matrix getRot() const { return R_; } 58 | 59 | // //! @brief FEJ Rotation access 60 | // //! 61 | // Eigen::Matrix getRotFej() const { return R_fej_; } 62 | 63 | //! @brief Implements update operation by left-multiplying the current 64 | //! @note quaternion with a quaternion built from a small axis-angle perturbation. 65 | //! @param dx Axis-angle representation of the perturbing quaternion 66 | //! 67 | void update(const Eigen::VectorXd &dx) override { 68 | 69 | assert(dx.rows() == size_); 70 | 71 | // Build perturbing quaternion 72 | Eigen::Matrix dq; 73 | dq << .5 * dx, 1.0; 74 | dq = normalizeQuat(dq); 75 | 76 | // Update estimate and recompute R 77 | setValue(multiplyQuat(dq, value_)); 78 | } 79 | 80 | //! @brief clone 81 | //! 82 | std::shared_ptr clone() override { 83 | auto Clone = std::shared_ptr(new QuatJPL()); 84 | Clone->setValue(getValue()); 85 | Clone->setFej(getFej()); 86 | return Clone; 87 | } 88 | 89 | protected: 90 | // // Stores the rotation 91 | // Eigen::Matrix R_; 92 | 93 | // // Stores the first-estimate rotation 94 | // Eigen::Matrix R_fej_; 95 | 96 | }; 97 | 98 | } // namespace msckf_dvio 99 | 100 | #endif //MSCKF_TYPE_QUAT_JPL_H_ -------------------------------------------------------------------------------- /include/types/type.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_H_ 2 | #define MSCKF_TYPE_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace msckf_dvio { 8 | 9 | class Type 10 | { 11 | 12 | public: 13 | 14 | //! @brief Default constructor for our Type 15 | //! @param new_size: degrees of freedom of variable (i.e., the size of the error state) 16 | Type(int new_size) { size_ = new_size; } 17 | 18 | virtual ~Type(){}; 19 | 20 | //! @brief Sets id used to track location of variable in the filter covariance 21 | //! @note Note that the minimum ID is -1 which says that the state is not in our covariance. 22 | //! If the ID is larger than -1 then this is the index location in the covariance matrix. 23 | //! @param new_id: entry in filter covariance corresponding to this variable 24 | virtual void setId(int new_id) { id_ = new_id; } 25 | 26 | //! @brief Overwrite value of state's estimate 27 | //! @param new_value New value that will overwrite state's value 28 | virtual void setValue(const Eigen::MatrixXd &new_value) { 29 | assert(value_.rows() == new_value.rows()); 30 | assert(value_.cols() == new_value.cols()); 31 | value_ = new_value; 32 | } 33 | 34 | //! @brief Overwrite value of first-estimate 35 | //! @param new_value New value that will overwrite state's fej 36 | virtual void setFej(const Eigen::MatrixXd &new_value) { 37 | assert(fej_.rows() == new_value.rows()); 38 | assert(fej_.cols() == new_value.cols()); 39 | fej_ = new_value; 40 | } 41 | 42 | //! @brief Access to variable id (i.e. its location in the covariance) 43 | const int getId() const { return id_; } 44 | 45 | //! @brief Access variable's estimate 46 | virtual const Eigen::MatrixXd &getValue() const { return value_; } 47 | 48 | //! @brief Access variable's first-estimate 49 | virtual const Eigen::MatrixXd &getFej() const { return fej_; } 50 | 51 | //! @brief Access to variable size (i.e. its error state size) 52 | int getSize() { return size_; } 53 | 54 | //! @brief Update variable due to perturbation of error state 55 | //! @param dx Perturbation used to update the variable through a defined "boxplus" operation 56 | virtual void update(const Eigen::VectorXd &dx) = 0; 57 | 58 | //! @brief Create a clone of this variable 59 | virtual std::shared_ptr clone() = 0; 60 | 61 | protected: 62 | /// First-estimate 63 | Eigen::MatrixXd fej_; 64 | 65 | /// Current best estimate 66 | Eigen::MatrixXd value_; 67 | 68 | /// state location 69 | int id_ = -1; 70 | 71 | /// Dimension of error state 72 | int size_ = -1; 73 | }; 74 | 75 | } // namespace msckf_dvio 76 | 77 | #endif //MSCKF_TYPE_H_ -------------------------------------------------------------------------------- /include/types/type_all.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_ALL_H_ 2 | #define MSCKF_TYPE_ALL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "type.h" 8 | #include "vec.h" 9 | #include "quatJPL.h" 10 | #include "poseJPL.h" 11 | 12 | #include "msgs.h" 13 | #include "parameters.h" 14 | 15 | 16 | // #include 17 | // namespace msckf_dvio { 18 | 19 | // using MatrixX = Eigen::Matrix; 20 | // using VectorX = Eigen::Matrix; 21 | 22 | // //// check: https://eigen.tuxfamily.org/dox/group__TopicStlContainers.html 23 | // using VectorOfVector2d = std::vector>; 24 | // using VectorOfVector3d = std::vector>; 25 | // using VectorOfMatrix3d = std::vector>; 26 | 27 | // } // namespace msckf_dvio 28 | 29 | #endif //MSCKF_TYPE_ALL_H_ -------------------------------------------------------------------------------- /include/types/vec.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_TYPE_VEC_H_ 2 | #define MSCKF_TYPE_VEC_H_ 3 | 4 | #include "type.h" 5 | 6 | namespace msckf_dvio { 7 | 8 | //! @brief Derived Type class that implements vector variables 9 | class Vec : public Type 10 | { 11 | public: 12 | //! @brief Default constructor for Vec 13 | //! @param dim: Size of the vector (will be same as error state) 14 | Vec(int dim) : Type(dim) { 15 | value_ = Eigen::VectorXd::Zero(dim); 16 | fej_ = Eigen::VectorXd::Zero(dim); 17 | } 18 | 19 | ~Vec() {} 20 | 21 | //! @brief Implements the update operation through standard vector addition 22 | //! @param dx Additive error state correction 23 | void update(const Eigen::VectorXd &dx) override { 24 | assert(dx.rows() == size_); 25 | setValue(value_ + dx); 26 | } 27 | 28 | //! @brief Performs all the cloning 29 | std::shared_ptr clone() override { 30 | auto Clone = std::shared_ptr(new Vec(size_)); 31 | Clone->setValue(getValue()); 32 | Clone->setFej(getFej()); 33 | return Clone; 34 | } 35 | }; 36 | 37 | } // namespace msckf_dvio 38 | 39 | #endif //MSCKF_TYPE_VEC_H_ -------------------------------------------------------------------------------- /include/utils/colors.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVINS: An Open Platform for Visual-Inertial Research 3 | * Copyright (C) 2021 Patrick Geneva 4 | * Copyright (C) 2021 Guoquan Huang 5 | * Copyright (C) 2021 OpenVINS Contributors 6 | * Copyright (C) 2019 Kevin Eckenhoff 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | 23 | 24 | #ifndef OV_CORE_COLOR_MACROS 25 | #define OV_CORE_COLOR_MACROS 26 | 27 | #define RESET "\033[0m" 28 | #define BLACK "\033[30m" /* Black */ 29 | #define RED "\033[31m" /* Red */ 30 | #define GREEN "\033[32m" /* Green */ 31 | #define YELLOW "\033[33m" /* Yellow */ 32 | #define BLUE "\033[34m" /* Blue */ 33 | #define MAGENTA "\033[35m" /* Magenta */ 34 | #define CYAN "\033[36m" /* Cyan */ 35 | #define WHITE "\033[37m" /* White */ 36 | #define REDPURPLE "\033[95m" /* Red Purple */ 37 | #define BOLDBLACK "\033[1m\033[30m" /* Bold Black */ 38 | #define BOLDRED "\033[1m\033[31m" /* Bold Red */ 39 | #define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ 40 | #define BOLDYELLOW "\033[1m\033[33m" /* Bold Yellow */ 41 | #define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ 42 | #define BOLDMAGENTA "\033[1m\033[35m" /* Bold Magenta */ 43 | #define BOLDCYAN "\033[1m\033[36m" /* Bold Cyan */ 44 | #define BOLDWHITE "\033[1m\033[37m" /* Bold White */ 45 | #define BOLDREDPURPLE "\033[1m\033[95m" /* Bold Red Purple */ 46 | 47 | #endif /* OV_CORE_COLOR_MACROS */ 48 | -------------------------------------------------------------------------------- /include/utils/lambda_body.h: -------------------------------------------------------------------------------- 1 | /* 2 | * OpenVINS: An Open Platform for Visual-Inertial Research 3 | * Copyright (C) 2021 Patrick Geneva 4 | * Copyright (C) 2021 Guoquan Huang 5 | * Copyright (C) 2021 OpenVINS Contributors 6 | * Copyright (C) 2019 Kevin Eckenhoff 7 | * 8 | * This program is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * This program is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with this program. If not, see . 20 | */ 21 | 22 | 23 | #ifndef LAMBDA_BODY_H 24 | #define LAMBDA_BODY_H 25 | 26 | #include 27 | #include 28 | 29 | namespace msckf_dvio { 30 | 31 | /** 32 | * This is a utility class required to build with older version of opencv 33 | * On newer versions this doesn't seem to be needed, but here we just use it to ensure we can work for more opencv version. 34 | * https://answers.opencv.org/question/65800/how-to-use-lambda-as-a-parameter-to-parallel_for_/?answer=130691#post-id-130691 35 | */ 36 | class LambdaBody : public cv::ParallelLoopBody { 37 | public: 38 | explicit LambdaBody(const std::function &body) { _body = body; } 39 | void operator()(const cv::Range &range) const override { _body(range); } 40 | 41 | private: 42 | std::function _body; 43 | }; 44 | 45 | } /* namespace msckf_dvio */ 46 | 47 | #endif /* LAMBDA_BODY_H */ 48 | -------------------------------------------------------------------------------- /include/utils/recorder.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_UTILS_RECORDER_H_ 2 | #define MSCKF_UTILS_RECORDER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "feature/Feature.h" 9 | 10 | namespace msckf_dvio { 11 | 12 | class Recorder { 13 | public: 14 | Recorder(std::string filename) { 15 | // check if the file exist 16 | 17 | // check the file 18 | outfile.open(filename,std::ios_base::app); 19 | if (outfile.fail()) { 20 | printf("Unable to open output file!!"); 21 | printf("Path: %s", filename.c_str()); 22 | std::exit(EXIT_FAILURE); 23 | } 24 | 25 | init("id, timestamp"); 26 | } 27 | 28 | void init(std::string header) { 29 | // wirte header 30 | 31 | // outfile << "id timestamps" << std::endl; 32 | outfile << header << std::endl; 33 | } 34 | 35 | void writeFeature(Feature &feat) { 36 | outfile << feat.featid << "," << std::fixed << std::setprecision(9); 37 | 38 | for(const auto& t :feat.timestamps[0]) 39 | outfile<< t << ","; 40 | 41 | outfile <<"\n"; 42 | } 43 | 44 | void writeString(std::string str) { 45 | outfile << str; 46 | } 47 | 48 | protected: 49 | // Output stream file 50 | std::ofstream outfile; 51 | }; 52 | 53 | } 54 | 55 | 56 | #endif //MSCKF_UTILS_RECORDER_H_ -------------------------------------------------------------------------------- /include/utils/time_cost.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_UTILS_TIMECOST_H 2 | #define MSCKF_UTILS_TIMECOST_H 3 | 4 | #include 5 | 6 | class TimeCost{ 7 | public: 8 | TimeCost() { 9 | start = std::chrono::high_resolution_clock::now(); 10 | } 11 | 12 | double timecost() { 13 | end = std::chrono::high_resolution_clock::now(); 14 | 15 | double cost = std::chrono::duration_cast(end - start).count() * 1e-6; 16 | 17 | start = end; 18 | 19 | return cost; 20 | } 21 | 22 | private: 23 | std::chrono::time_point start, end; 24 | }; 25 | 26 | #endif // MSCKF_UTILS_TIMECOST_H 27 | -------------------------------------------------------------------------------- /launch/Alaska/alaska_3_29_dpio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /launch/Alaska/alaska_3_30_dpio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /launch/Alaska/helper_alaska.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /launch/Alaska/urdf_alaska.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /launch/GLRC/glrc_3_13_dpio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/GLRC/glrc_3_13_dpvio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/GLRC/glrc_3_13_dvio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/GLRC/glrc_3_13_pvio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/GLRC/glrc_3_13_vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /launch/GLRC/helper_glrc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 14 | 15 | 16 | 17 | 20 | 21 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /launch/GLRC/urdf_glrc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /launch/euroc/euroc_vio.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /launch/euroc/helper_euroc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/sim/helper_sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/sim/sim_udel_gore.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /media/glrc_3_13.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GSO-soslab/msckf_dvio/709bc6f63a84f1eb40964b30a03a15bfb59a6584/media/glrc_3_13.gif -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | msckf_dvio 4 | 0.0.0 5 | The msckf_dvio package 6 | 7 | lin 8 | 9 | 10 | 11 | 12 | 13 | TODO 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | catkin 49 | roscpp 50 | rospy 51 | std_msgs 52 | rosbag 53 | sensor_msgs 54 | cv_bridge 55 | geometry_msgs 56 | tf_conversions 57 | eigen_conversions 58 | dynamic_reconfigure 59 | message_filters 60 | image_transport 61 | visualization_msgs 62 | tf 63 | tf2 64 | nav_msgs 65 | std_srvs 66 | tf2_ros 67 | tf2_geometry_msgs 68 | pcl_conversions 69 | 70 | message_generation 71 | rospy 72 | rospy 73 | 74 | roscpp 75 | rospy 76 | std_msgs 77 | rosbag 78 | sensor_msgs 79 | cv_bridge 80 | message_runtime 81 | geometry_msgs 82 | tf_conversions 83 | eigen_conversions 84 | dynamic_reconfigure 85 | message_filters 86 | image_transport 87 | visualization_msgs 88 | tf 89 | tf2 90 | nav_msgs 91 | std_srvs 92 | tf2_ros 93 | tf2_geometry_msgs 94 | pcl_conversions 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /ros/node.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_ROS_NODE_H_ 2 | #define MSCKF_ROS_NODE_H_ 3 | 4 | //c++ 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // ros 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | // 35 | #include "manager/msckf_manager.h" 36 | #include "types/type_all.h" 37 | 38 | #include "visualizer.h" 39 | #include "utils/rapidcsv.h" 40 | 41 | namespace msckf_dvio 42 | { 43 | 44 | class RosNode 45 | { 46 | public: 47 | RosNode(const ros::NodeHandle &nh, 48 | const ros::NodeHandle &nh_private); 49 | 50 | ~RosNode(){} 51 | 52 | void imuCallback(const sensor_msgs::Imu::ConstPtr &msg); 53 | 54 | void dvlCallback(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr &msg); 55 | 56 | void imageCallback(const sensor_msgs::Image::ConstPtr &msg); 57 | 58 | void featureCallback(const sensor_msgs::PointCloud::ConstPtr &msg); 59 | 60 | void pressureCallback(const sensor_msgs::FluidPressure::ConstPtr &msg); 61 | 62 | void dvlCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg); 63 | 64 | bool srvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 65 | 66 | void process(); 67 | 68 | void loadParamSystem(Params ¶ms); 69 | 70 | void loadParamInit(Params ¶ms); 71 | 72 | void loadParamPrior(Params ¶ms); 73 | 74 | void loadParamImage(Params ¶ms); 75 | 76 | void loadCSV(); 77 | 78 | private: 79 | ros::NodeHandle nh_; 80 | ros::NodeHandle nh_private_; 81 | 82 | ros::ServiceServer service_; 83 | 84 | std::map> subscribers; 85 | 86 | double last_t_img = 0; 87 | double last_t_dvl = 0; 88 | double last_t_pressure = 0; 89 | double last_t_pointcloud = 0; 90 | 91 | std::shared_ptr manager; 92 | 93 | std::shared_ptr visualizer; 94 | 95 | Params parameters; 96 | 97 | }; // end of class 98 | 99 | } // namespace msckf_dvio 100 | 101 | #endif //MSCKF_ROS_NODE_H_ -------------------------------------------------------------------------------- /ros/visualizer.h: -------------------------------------------------------------------------------- 1 | #ifndef MSCKF_ROS_VISUALIZER_H 2 | #define MSCKF_ROS_VISUALIZER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include "manager/msckf_manager.h" 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace msckf_dvio 32 | { 33 | 34 | class RosVisualizer { 35 | public: 36 | 37 | /** 38 | * @brief Default constructor 39 | * 40 | * @param nh ROS node handler 41 | * @param app Core estimator manager 42 | */ 43 | RosVisualizer(const ros::NodeHandle &nh, std::shared_ptr manager); 44 | 45 | /** 46 | * @brief Will visualize the system if we have new things 47 | */ 48 | void visualize(); 49 | 50 | /// Publish the active tracking image 51 | void publishImage(); 52 | 53 | bool srvCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res); 54 | 55 | private: 56 | 57 | /// Publish the current state 58 | void publishState(); 59 | 60 | /// Publish current features 61 | void publishFeatures(); 62 | 63 | void publishPointCloud(); 64 | 65 | std::shared_ptr msckf_manager; 66 | 67 | ros::NodeHandle nh_; 68 | image_transport::ImageTransport it_; 69 | 70 | ros::ServiceServer service_; 71 | 72 | ros::Publisher pub_odom, pub_path, pub_bias, pub_pc; 73 | ros::Publisher pub_features; 74 | image_transport::Publisher pub_img; 75 | 76 | nav_msgs::Path path; 77 | 78 | std::unique_ptr odom_broadcaster; 79 | 80 | double last_visual_times; 81 | 82 | tf2_ros::Buffer transform_buffer; 83 | 84 | Eigen::Isometry3d T_B_I; 85 | 86 | pcl::PointCloud saved_pcd; 87 | 88 | std::atomic save_flag; 89 | }; 90 | 91 | } // end of namespace msckf_dvio 92 | 93 | #endif // MSCKF_ROS_VISUALIZER_H --------------------------------------------------------------------------------