├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── display_multi.rviz ├── display_rosbag.rviz └── run_record.launch ├── package.xml └── src ├── estimator.h ├── residuals.h ├── run_record.cpp └── utils ├── calibration_options.h ├── eval.h ├── gen_noise.h ├── math.h ├── parse_ros.h ├── read_imu_csv.h └── utils.h /.gitignore: -------------------------------------------------------------------------------- 1 | # files under development 2 | launch/run_sim*.launch 3 | src/core/ 4 | src/sim/ 5 | src/run_sim*.cpp 6 | 7 | scripts/ 8 | dataset/ -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.8) 2 | 3 | # Project name 4 | project(imucalib) 5 | 6 | # Include our cmake files 7 | #set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/) 8 | 9 | # Find catkin (the ROS build system) 10 | find_package(catkin QUIET COMPONENTS roscpp) 11 | 12 | # Include libraries 13 | find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) 14 | find_package(Eigen3 REQUIRED) 15 | find_package(Ceres REQUIRED) 16 | 17 | # display message to user 18 | message(STATUS "BOOST VERSION: " ${Boost_VERSION}) 19 | message(STATUS "EIGEN VERSION: " ${EIGEN3_VERSION}) 20 | message(STATUS "CERES VERSION: " ${CERES_VERSION}) 21 | 22 | # Describe catkin project 23 | if (catkin_FOUND) 24 | add_definitions(-DROS_AVAILABLE=1) 25 | catkin_package( 26 | CATKIN_DEPENDS roscpp 27 | INCLUDE_DIRS src 28 | LIBRARIES ${PROJECT_NAME} 29 | ) 30 | else() 31 | message(WARNING "CATKIN NOT FOUND BUILDING WITHOUT ROS!") 32 | endif() 33 | 34 | #[[ 35 | # Try to compile with c++11 36 | # http://stackoverflow.com/a/25836953 37 | include(CheckCXXCompilerFlag) 38 | CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) 39 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 40 | if(COMPILER_SUPPORTS_CXX11) 41 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 42 | elseif(COMPILER_SUPPORTS_CXX0X) 43 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 44 | else() 45 | message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") 46 | endif() 47 | ]] 48 | 49 | # Enable compile optimizations 50 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops") 51 | 52 | # Enable debug flags (use if you want to debug in gdb) 53 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g3 -Wall -Wuninitialized -Wmaybe-uninitialized") 54 | 55 | # Include our header files 56 | include_directories( 57 | src 58 | ${Boost_INCLUDE_DIRS} 59 | ${EIGEN3_INCLUDE_DIR} 60 | ${CERES_INCLUDE_DIRS} 61 | ${catkin_INCLUDE_DIRS} 62 | ) 63 | 64 | ################################################## 65 | # Make binary files! 66 | ################################################## 67 | 68 | add_executable(run_record src/run_record.cpp) 69 | target_link_libraries(run_record ${Boost_LIBRARIES} ${CERES_LIBRARIES} ${catkin_LIBRARIES}) 70 | 71 | 72 | #[[ 73 | ################################################## 74 | # Below is OpenVINS-wrapped codes 75 | ################################################## 76 | 77 | # Find OpenVINS packages 78 | find_package(catkin QUIET COMPONENTS rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge) 79 | find_package(ov_core REQUIRED) 80 | find_package(ov_msckf REQUIRED) 81 | find_package(ov_eval REQUIRED) 82 | find_package(OpenCV 4 REQUIRED) 83 | 84 | # display message to user 85 | message(STATUS "OPENCV VERSION: " ${OpenCV_VERSION}) 86 | 87 | # Describe catkin project 88 | if (catkin_FOUND) 89 | add_definitions(-DROS_AVAILABLE=1) 90 | catkin_package( 91 | CATKIN_DEPENDS roscpp rosbag tf std_msgs geometry_msgs sensor_msgs nav_msgs visualization_msgs cv_bridge ov_core 92 | INCLUDE_DIRS src 93 | LIBRARIES imucalib_lib 94 | ) 95 | else() 96 | message(WARNING "CATKIN NOT FOUND BUILDING WITHOUT ROS!") 97 | endif() 98 | 99 | # Set source directories 100 | # set(ov_core_SOURCE_DIR ${PROJECT_SOURCE_DIR}/../ov_core) 101 | # set(ov_msckf_SOURCE_DIR ${PROJECT_SOURCE_DIR}/../ov_msckf) 102 | # set(ov_eval_SOURCE_DIR ${PROJECT_SOURCE_DIR}/../ov_eval) 103 | # set(ov_calib_SOURCE_DIR ${PROJECT_SOURCE_DIR}/../ov_calib) 104 | 105 | # Include our header files 106 | include_directories( 107 | ${ov_core_SOURCE_DIR}/src 108 | ${ov_msckf_SOURCE_DIR}/src 109 | ${ov_eval_SOURCE_DIR}/src 110 | ) 111 | 112 | # Set link libraries used by all binaries 113 | list(APPEND thirdparty_libraries 114 | ${Boost_LIBRARIES} 115 | ${OpenCV_LIBRARIES} 116 | ${catkin_LIBRARIES} 117 | ) 118 | 119 | # If we are not building with ROS then we need to manually link to its headers 120 | # This isn't that elegant of a way, but this at least allows for building without ROS 121 | # See this stackoverflow answer: https://stackoverflow.com/a/11217008/7718197 122 | if (NOT catkin_FOUND) 123 | message(WARNING "MANUALLY LINKING TO OV_CORE LIBRARY....") 124 | include_directories(${ov_core_SOURCE_DIR}/src/) 125 | list(APPEND thirdparty_libraries ov_core_lib) 126 | endif() 127 | 128 | 129 | 130 | ################################################## 131 | # Make simulator library 132 | ################################################## 133 | list(APPEND sim_lib_source_files 134 | ${ov_msckf_SOURCE_DIR}/src/sim/Simulator.cpp 135 | src/sim/SimulatorMultiIMU.cpp 136 | src/core/RosTinyVisualizer.cpp 137 | ) 138 | add_library(sim_lib SHARED ${sim_lib_source_files}) 139 | target_link_libraries(sim_lib ${thirdparty_libraries}) 140 | target_include_directories(sim_lib PUBLIC src) 141 | ]] -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2021, Jongwon Lee (jongwon5@illinois.edu) 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Extrinsic Calibration of Multiple Inertial Sensors from Arbitrary Trajectories 2 | 3 | This program serves the extrinsic calibration of multiple 6DOF inertial measurement unit (IMU) with arbitrary measurements. This program, wrapped with [ROS](https://www.ros.org/) as of now, requires the installation of [Ceres Solver](http://ceres-solver.org/). Some code has been adapted from the [OpenVINS](https://docs.openvins.com/), a open-sourced visual-inertial simulator. 4 | 5 | 6 | ## Dependencies 7 | 8 | The codebase has a dependency to the following libraries and tools: 9 | - Ceres Solver: http://ceres-solver.org/ 10 | - Eigen: https://eigen.tuxfamily.org/ 11 | - ROS: https://www.ros.org/ 12 | 13 | 14 | 15 | ## Dataset [[Link](https://uofi.box.com/s/nemjm0v2q05hmgzg6ewef4iwqpwh1tkd)] 16 | 17 | The dataset consists of (1) two IMUs, (2) one camera, and (3) trajectory captured by VICON for ground-truth, under three different perceptual conditions: (1) baseline, (2) ill-lit, and (3) blurry scenes. Collected while the camera heads toward the fiducal marker, this dataset is organized to our framework's performance to [Kalibr](https://github.com/ethz-asl/kalibr), a camera-IMU calibration toolbox widely used. 18 | 19 | 20 | ## How to execute 21 | 22 | **Disclaimer**: The command below shows an example of how to build the program and execute it by `roslaunch`. The details on running it with the aforementioned dataset will be appeared shortly. 23 | 24 | ``` 25 | # setup your own workspace 26 | mkdir -p ${YOUR_WORKSPACE}/catkin_ws/src/ 27 | cd ${YOUR_WORKSPACE}/catkin_ws 28 | catkin init 29 | # repositories to clone 30 | cd src 31 | git clone https://github.com/jongwonjlee/mix-cal.git 32 | # go back to root and build 33 | cd .. 34 | catkin build -j4 35 | # run the calibration 36 | source devel/setup.bash 37 | roslaunch imucalib run_record.launch csv_filepath:="${IMU_DATA_PATH}/" csv_filename:="results.csv" 38 | ``` 39 | 40 | 41 | ## Credit / Licensing 42 | 43 | Please cite the following papers below for any academic usage: 44 | 45 | ``` 46 | @article{lee2022extrinsic, 47 | title={Extrinsic Calibration of Multiple Inertial Sensors from Arbitrary Trajectories}, 48 | author={Lee, Jongwon and Hanley, David and Bretl, Timothy}, 49 | journal={IEEE Robotics and Automation Letters}, 50 | year={2022}, 51 | publisher={IEEE} 52 | } 53 | 54 | @inproceedings{lee2024efficient, 55 | title = {Efficient Extrinsic Self-Calibration of Multiple IMUs using Measurement Subset Selection}, 56 | author = {Lee, Jongwon and Hanley, David and Bretl, Timothy}, 57 | booktitle = {IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 58 | year = {2024}, 59 | month = {October}, 60 | address = {Abu Dhabi}, 61 | organization = {IEEE} 62 | } 63 | ``` 64 | 65 | The program is licensed under the [GNU General Public License v3 (GPL-3)](https://www.gnu.org/licenses/gpl-3.0.txt) inherited from that of [OpenVINS](https://github.com/rpng/open_vins) where some part of it is adapted. If you use code in this program pertaining to [OpenVINS](https://github.com/rpng/open_vins), please also cite the following: 66 | 67 | ``` 68 | @Conference{Geneva2020ICRA, 69 | Title = {OpenVINS: A Research Platform for Visual-Inertial Estimation}, 70 | Author = {Patrick Geneva and Kevin Eckenhoff and Woosik Lee and Yulin Yang and Guoquan Huang}, 71 | Booktitle = {Proc. of the IEEE International Conference on Robotics and Automation}, 72 | Year = {2020}, 73 | Address = {Paris, France}, 74 | Url = {\url{https://github.com/rpng/open_vins}} 75 | } 76 | ``` 77 | -------------------------------------------------------------------------------- /launch/display_multi.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.6000000238418579 10 | Tree Height: 460 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 112; 112; 115 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 80 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 999 58 | Frames: 59 | All Enabled: false 60 | IMU0: 61 | Value: true 62 | IMU1: 63 | Value: true 64 | IMU2: 65 | Value: true 66 | IMU3: 67 | Value: true 68 | cam0: 69 | Value: false 70 | global: 71 | Value: true 72 | imu: 73 | Value: false 74 | truth: 75 | Value: true 76 | truth_U0: 77 | Value: false 78 | truth_U1: 79 | Value: false 80 | truth_U2: 81 | Value: false 82 | truth_U3: 83 | Value: false 84 | Marker Alpha: 1 85 | Marker Scale: 1 86 | Name: TF 87 | Show Arrows: false 88 | Show Axes: true 89 | Show Names: true 90 | Tree: 91 | global: 92 | IMU0: 93 | {} 94 | IMU1: 95 | {} 96 | IMU2: 97 | {} 98 | IMU3: 99 | {} 100 | imu: 101 | cam0: 102 | {} 103 | truth: 104 | {} 105 | truth_U0: 106 | {} 107 | truth_U1: 108 | {} 109 | truth_U2: 110 | {} 111 | truth_U3: 112 | {} 113 | Update Interval: 0 114 | Value: true 115 | - Class: rviz/Image 116 | Enabled: true 117 | Image Topic: /ov_msckf/trackhist 118 | Max Value: 1 119 | Median window: 5 120 | Min Value: 0 121 | Name: Image 122 | Normalize Range: true 123 | Queue Size: 2 124 | Transport Hint: raw 125 | Unreliable: false 126 | Value: true 127 | - Alpha: 1 128 | Buffer Length: 1 129 | Class: rviz/Path 130 | Color: 0; 255; 255 131 | Enabled: true 132 | Head Diameter: 0.30000001192092896 133 | Head Length: 0.20000000298023224 134 | Length: 0.30000001192092896 135 | Line Style: Billboards 136 | Line Width: 0.00800000037997961 137 | Name: Path Base 138 | Offset: 139 | X: 0 140 | Y: 0 141 | Z: 0 142 | Pose Color: 255; 85; 255 143 | Pose Style: None 144 | Queue Size: 10 145 | Radius: 0.029999999329447746 146 | Shaft Diameter: 0.10000000149011612 147 | Shaft Length: 0.10000000149011612 148 | Topic: /ov_msckf/pathgt 149 | Unreliable: false 150 | Value: true 151 | - Alpha: 1 152 | Buffer Length: 1 153 | Class: rviz/Path 154 | Color: 255; 116; 230 155 | Enabled: false 156 | Head Diameter: 0.30000001192092896 157 | Head Length: 0.20000000298023224 158 | Length: 0.30000001192092896 159 | Line Style: Billboards 160 | Line Width: 0.029999999329447746 161 | Name: Path U1 162 | Offset: 163 | X: 0 164 | Y: 0 165 | Z: 0 166 | Pose Color: 255; 85; 255 167 | Pose Style: None 168 | Queue Size: 10 169 | Radius: 0.029999999329447746 170 | Shaft Diameter: 0.10000000149011612 171 | Shaft Length: 0.10000000149011612 172 | Topic: /ov_msckf/pathgt_U1 173 | Unreliable: false 174 | Value: false 175 | - Alpha: 1 176 | Buffer Length: 1 177 | Class: rviz/Path 178 | Color: 56; 82; 255 179 | Enabled: false 180 | Head Diameter: 0.30000001192092896 181 | Head Length: 0.20000000298023224 182 | Length: 0.30000001192092896 183 | Line Style: Billboards 184 | Line Width: 0.029999999329447746 185 | Name: Path U2 186 | Offset: 187 | X: 0 188 | Y: 0 189 | Z: 0 190 | Pose Color: 255; 85; 255 191 | Pose Style: None 192 | Queue Size: 10 193 | Radius: 0.029999999329447746 194 | Shaft Diameter: 0.10000000149011612 195 | Shaft Length: 0.10000000149011612 196 | Topic: /ov_msckf/pathgt_U2 197 | Unreliable: false 198 | Value: false 199 | - Alpha: 1 200 | Autocompute Intensity Bounds: true 201 | Autocompute Value Bounds: 202 | Max Value: 10 203 | Min Value: -10 204 | Value: true 205 | Axis: Z 206 | Channel Name: intensity 207 | Class: rviz/PointCloud2 208 | Color: 255; 255; 127 209 | Color Transformer: FlatColor 210 | Decay Time: 0 211 | Enabled: false 212 | Invert Rainbow: false 213 | Max Color: 255; 255; 255 214 | Min Color: 0; 0; 0 215 | Name: SIM Points 216 | Position Transformer: XYZ 217 | Queue Size: 10 218 | Selectable: true 219 | Size (Pixels): 3 220 | Size (m): 0.009999999776482582 221 | Style: Points 222 | Topic: /ov_msckf/points_sim 223 | Unreliable: false 224 | Use Fixed Frame: true 225 | Use rainbow: true 226 | Value: false 227 | Enabled: true 228 | Global Options: 229 | Background Color: 0; 0; 0 230 | Default Light: true 231 | Fixed Frame: global 232 | Frame Rate: 30 233 | Name: root 234 | Tools: 235 | - Class: rviz/Interact 236 | Hide Inactive Objects: true 237 | - Class: rviz/MoveCamera 238 | - Class: rviz/Select 239 | - Class: rviz/FocusCamera 240 | - Class: rviz/Measure 241 | - Class: rviz/SetInitialPose 242 | Theta std deviation: 0.2617993950843811 243 | Topic: /initialpose 244 | X std deviation: 0.5 245 | Y std deviation: 0.5 246 | - Class: rviz/SetGoal 247 | Topic: /move_base_simple/goal 248 | - Class: rviz/PublishPoint 249 | Single click: true 250 | Topic: /clicked_point 251 | Value: true 252 | Views: 253 | Current: 254 | Class: rviz/Orbit 255 | Distance: 5.549187660217285 256 | Enable Stereo Rendering: 257 | Stereo Eye Separation: 0.05999999865889549 258 | Stereo Focal Distance: 1 259 | Swap Stereo Eyes: false 260 | Value: false 261 | Field of View: 0.7853981852531433 262 | Focal Point: 263 | X: -0.5387392044067383 264 | Y: -1.179045557975769 265 | Z: 0.4184381067752838 266 | Focal Shape Fixed Size: false 267 | Focal Shape Size: 0.05000000074505806 268 | Invert Z Axis: false 269 | Name: Current View 270 | Near Clip Distance: 0.009999999776482582 271 | Pitch: 0.4052034914493561 272 | Target Frame: 273 | Yaw: 0.8885934948921204 274 | Saved: ~ 275 | Window Geometry: 276 | Displays: 277 | collapsed: false 278 | Height: 1016 279 | Hide Left Dock: false 280 | Hide Right Dock: true 281 | Image: 282 | collapsed: false 283 | QMainWindow State: 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 284 | Selection: 285 | collapsed: false 286 | Time: 287 | collapsed: false 288 | Tool Properties: 289 | collapsed: false 290 | Views: 291 | collapsed: true 292 | Width: 1848 293 | X: 1080 294 | Y: 202 295 | -------------------------------------------------------------------------------- /launch/display_rosbag.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Path Base1 9 | - /Image2 10 | - /Image2/Status1 11 | - /TF1 12 | - /TF1/Frames1 13 | Splitter Ratio: 0.6000000238418579 14 | Tree Height: 904 15 | - Class: rviz/Selection 16 | Name: Selection 17 | - Class: rviz/Tool Properties 18 | Expanded: 19 | - /2D Pose Estimate1 20 | - /2D Nav Goal1 21 | - /Publish Point1 22 | Name: Tool Properties 23 | Splitter Ratio: 0.5886790156364441 24 | - Class: rviz/Views 25 | Expanded: 26 | - /Current View1 27 | Name: Views 28 | Splitter Ratio: 0.5 29 | - Class: rviz/Time 30 | Experimental: false 31 | Name: Time 32 | SyncMode: 0 33 | SyncSource: "" 34 | Preferences: 35 | PromptSaveOnExit: true 36 | Toolbars: 37 | toolButtonStyle: 2 38 | Visualization Manager: 39 | Class: "" 40 | Displays: 41 | - Alpha: 0.5 42 | Cell Size: 1 43 | Class: rviz/Grid 44 | Color: 112; 112; 115 45 | Enabled: true 46 | Line Style: 47 | Line Width: 0.029999999329447746 48 | Value: Lines 49 | Name: Grid 50 | Normal Cell Count: 0 51 | Offset: 52 | X: 0 53 | Y: 0 54 | Z: 0 55 | Plane: XY 56 | Plane Cell Count: 80 57 | Reference Frame: 58 | Value: true 59 | - Class: rviz/Image 60 | Enabled: false 61 | Image Topic: /ov_msckf/trackhist 62 | Max Value: 1 63 | Median window: 5 64 | Min Value: 0 65 | Name: Image 66 | Normalize Range: true 67 | Queue Size: 2 68 | Transport Hint: raw 69 | Unreliable: false 70 | Value: false 71 | - Alpha: 1 72 | Buffer Length: 1 73 | Class: rviz/Path 74 | Color: 0; 255; 255 75 | Enabled: true 76 | Head Diameter: 0.30000001192092896 77 | Head Length: 0.20000000298023224 78 | Length: 0.30000001192092896 79 | Line Style: Billboards 80 | Line Width: 0.0020000000949949026 81 | Name: Path Base 82 | Offset: 83 | X: 0 84 | Y: 0 85 | Z: 0 86 | Pose Color: 255; 85; 255 87 | Pose Style: None 88 | Queue Size: 10 89 | Radius: 0.029999999329447746 90 | Shaft Diameter: 0.10000000149011612 91 | Shaft Length: 0.10000000149011612 92 | Topic: /mocap/path 93 | Unreliable: false 94 | Value: true 95 | - Class: rviz/Image 96 | Enabled: false 97 | Image Topic: /camera/color/image_raw 98 | Max Value: 1 99 | Median window: 5 100 | Min Value: 0 101 | Name: Image 102 | Normalize Range: true 103 | Queue Size: 2 104 | Transport Hint: raw 105 | Unreliable: false 106 | Value: false 107 | - Class: rviz/TF 108 | Enabled: true 109 | Frame Timeout: 15 110 | Frames: 111 | All Enabled: true 112 | body: 113 | Value: true 114 | global: 115 | Value: true 116 | Marker Alpha: 1 117 | Marker Scale: 0.20000000298023224 118 | Name: TF 119 | Show Arrows: false 120 | Show Axes: true 121 | Show Names: true 122 | Tree: 123 | global: 124 | body: 125 | {} 126 | Update Interval: 0 127 | Value: true 128 | Enabled: true 129 | Global Options: 130 | Background Color: 0; 0; 0 131 | Default Light: true 132 | Fixed Frame: global 133 | Frame Rate: 30 134 | Name: root 135 | Tools: 136 | - Class: rviz/Interact 137 | Hide Inactive Objects: true 138 | - Class: rviz/MoveCamera 139 | - Class: rviz/Select 140 | - Class: rviz/FocusCamera 141 | - Class: rviz/Measure 142 | - Class: rviz/SetInitialPose 143 | Theta std deviation: 0.2617993950843811 144 | Topic: /initialpose 145 | X std deviation: 0.5 146 | Y std deviation: 0.5 147 | - Class: rviz/SetGoal 148 | Topic: /move_base_simple/goal 149 | - Class: rviz/PublishPoint 150 | Single click: true 151 | Topic: /clicked_point 152 | Value: true 153 | Views: 154 | Current: 155 | Class: rviz/Orbit 156 | Distance: 1.0422135591506958 157 | Enable Stereo Rendering: 158 | Stereo Eye Separation: 0.05999999865889549 159 | Stereo Focal Distance: 1 160 | Swap Stereo Eyes: false 161 | Value: false 162 | Field of View: 0.7853981852531433 163 | Focal Point: 164 | X: -0.7846085429191589 165 | Y: -0.39713186025619507 166 | Z: 0.30449414253234863 167 | Focal Shape Fixed Size: false 168 | Focal Shape Size: 0.05000000074505806 169 | Invert Z Axis: false 170 | Name: Current View 171 | Near Clip Distance: 0.009999999776482582 172 | Pitch: 0.3502035439014435 173 | Target Frame: 174 | Yaw: 0.8235965371131897 175 | Saved: ~ 176 | Window Geometry: 177 | Displays: 178 | collapsed: false 179 | Height: 1016 180 | Hide Left Dock: false 181 | Hide Right Dock: true 182 | Image: 183 | collapsed: false 184 | QMainWindow State: 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 185 | Selection: 186 | collapsed: false 187 | Time: 188 | collapsed: false 189 | Tool Properties: 190 | collapsed: false 191 | Views: 192 | collapsed: true 193 | Width: 1848 194 | X: 1080 195 | Y: 202 196 | -------------------------------------------------------------------------------- /launch/run_record.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | [ 64 | 1, 0, 0, 0.00000, 65 | 0, 1, 0, 0.00000, 66 | 0, 0, 1, 0.00000, 67 | 0, 0, 0, 1.00000 68 | ] 69 | 70 | 71 | [ 72 | 1, 0, 0, 0.10000, 73 | 0, 1, 0, 0.10000, 74 | 0, 0, 1, 0.00000, 75 | 0, 0, 0, 1.00000 76 | ] 77 | 78 | 79 | 80 | [0.0,0.0,9.81] 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imucalib 4 | 1.0.0 5 | The imucalib package 6 | 7 | 8 | 9 | 10 | Jongwon Lee 11 | 12 | 13 | 14 | 15 | 16 | GNU General Public License v3.0 17 | 18 | 19 | catkin 20 | 21 | 22 | cmake_modules 23 | roscpp 24 | 36 | 37 | 38 | roscpp 39 | 53 | 54 | -------------------------------------------------------------------------------- /src/estimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #ifndef ESTIMATOR_H 19 | #define ESTIMATOR_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | // timer 26 | #include 27 | #include 28 | 29 | #include "utils/calibration_options.h" 30 | 31 | /* SUGGESTED SELF-CALIBRATION WITH BIAS ESTIMATION */ 32 | namespace Ours { 33 | 34 | class Estimator 35 | { 36 | protected: 37 | /// True vio manager params (a copy of the parsed ones) 38 | CalibrationOptions params; 39 | 40 | // Parameter blocks 41 | std::map imu_pos; 42 | std::map imu_ori; 43 | std::map gyr_mis; // gyroscope misalignment 44 | std::vector wd_inI; 45 | std::vector > acc_bias_arr; 46 | std::vector > gyr_bias_arr; 47 | 48 | // Define sampling time interval 49 | double dt; 50 | 51 | // Define variances for IMU bias and noise 52 | double var_a, var_w, var_ab, var_wb; 53 | 54 | // Define covariance matrices having above as diagonal components 55 | Eigen::Matrix3d Cov_a, Cov_w, Cov_ab, Cov_wb; 56 | std::map Cov_a_arr, Cov_w_arr, Cov_ab_arr, Cov_wb_arr; 57 | 58 | // Define local parameterization to optimize quaternion on its own manifold (i.e. q \in R^4 where \norm(q) = 1) 59 | ceres::LocalParameterization* quat_loc_parameterization = new ceres::EigenQuaternionParameterization; 60 | 61 | // ceres::Problem object 62 | ceres::Problem problem; 63 | Solver::Options options; 64 | Solver::Summary summary; 65 | 66 | // Define timer 67 | std::chrono::time_point tic; 68 | std::chrono::time_point toc; 69 | 70 | // cost function value before and after optimization 71 | double initial_cost; 72 | double final_cost; 73 | 74 | // number of data to be proccessed 75 | int num_data; 76 | 77 | public: 78 | Estimator(const CalibrationOptions& params, 79 | const std::map& imu_extrinsics); 80 | ~Estimator(); 81 | void feed_init(const std::vector& wd_inI); 82 | void feed_bias(const std::vector >& acc_bias_arr, 83 | const std::vector >& gyr_bias_arr); 84 | void construct_problem(const std::vector >& a_measurements, 85 | const std::vector >& w_measurements); 86 | void solve_problem(); 87 | void get_extrinsics(std::map& imu_extrinsics_estimated); 88 | void get_gyr_mis(std::map& gyr_mis_estimated); 89 | void show_results(); 90 | 91 | // Internal member functions 92 | std::pair print_covariance(); 93 | int print_timer(); 94 | void print_report(); 95 | }; 96 | 97 | Estimator::Estimator(const CalibrationOptions& params, const std::map& imu_extrinsics) 98 | { 99 | // Store a copy of our params 100 | this->params = params; 101 | 102 | // Define sampling time interval 103 | dt = 1 / params.sim_freq_imu; 104 | // Define variances for IMU bias and noise 105 | var_a = pow(params.imu_noises.sigma_a, 2) / dt; 106 | var_w = pow(params.imu_noises.sigma_w, 2) / dt; 107 | var_ab = dt * pow(params.imu_noises.sigma_ab, 2); 108 | var_wb = dt * pow(params.imu_noises.sigma_wb, 2); 109 | 110 | for (int k = 0; k < 3; k ++) { 111 | Cov_a(k,k) = var_a; 112 | Cov_w(k,k) = var_w; 113 | Cov_ab(k,k) = var_ab; 114 | Cov_wb(k,k) = var_wb; 115 | } 116 | 117 | // ceres options 118 | options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; 119 | options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT; 120 | options.max_num_iterations = 200; 121 | options.num_threads = 4; 122 | options.minimizer_progress_to_stdout = true; 123 | 124 | // Initialize imu_pos, imu_ori, and gyr_mis 125 | for (int n = 0; n < params.num_imus; n ++) { 126 | Eigen::Matrix3d R_BtoIn = quat_2_Rot(imu_extrinsics.at(n).block(0,0,4,1)); 127 | Eigen::Quaterniond q_BtoIn(R_BtoIn); 128 | Eigen::Vector3d p_BinIn = imu_extrinsics.at(n).block(4,0,3,1); 129 | 130 | imu_pos.insert({n, p_BinIn}); 131 | imu_ori.insert({n, q_BtoIn}); 132 | 133 | Eigen::Quaterniond q_gyr = Eigen::Quaterniond::Identity(); // q_IntoGn 134 | gyr_mis.insert({n, q_gyr}); 135 | } 136 | 137 | } 138 | 139 | Estimator::~Estimator() {} 140 | 141 | void Estimator::feed_init(const std::vector& wd_inI) { 142 | // Initialize wd_inI 143 | this->wd_inI = wd_inI; 144 | 145 | // declare number of data used for construction 146 | num_data = this->wd_inI.size(); 147 | 148 | // Initialize biases 149 | std::map bias_zeros; 150 | for (int n = 0; n < params.num_imus; n ++) bias_zeros.insert({n, Eigen::Vector3d::Zero()}); 151 | acc_bias_arr.resize(num_data, bias_zeros); 152 | gyr_bias_arr.resize(num_data, bias_zeros); 153 | } 154 | 155 | 156 | void Estimator::feed_bias(const std::vector >& acc_bias_arr, 157 | const std::vector >& gyr_bias_arr) { 158 | this->acc_bias_arr = acc_bias_arr; 159 | this->gyr_bias_arr = gyr_bias_arr; 160 | } 161 | 162 | void Estimator::construct_problem(const std::vector >& a_measurements, 163 | const std::vector >& w_measurements) { 164 | // assert data length consistency 165 | assert((int)(a_measurements.size()) == num_data && (int)(w_measurements.size()) == num_data); 166 | 167 | // STEP 1-1 : ADD PARAMATERS TO BE OPTIMIZED 168 | // add all parameters in concern 169 | for (int n = 0; n < params.num_imus; n ++) { 170 | problem.AddParameterBlock(imu_pos.at(n).data(), 3); 171 | problem.AddParameterBlock(imu_ori.at(n).coeffs().data(), 4, quat_loc_parameterization); 172 | problem.AddParameterBlock(gyr_mis.at(n).coeffs().data(), 4, quat_loc_parameterization); 173 | } 174 | for (int t = 0; t < num_data; t ++) { 175 | problem.AddParameterBlock(wd_inI.at(t).data(), 3); 176 | for (int n = 0; n < params.num_imus; n ++) { 177 | problem.AddParameterBlock(acc_bias_arr.at(t).at(n).data(), 3); 178 | problem.AddParameterBlock(gyr_bias_arr.at(t).at(n).data(), 3); 179 | } 180 | } 181 | 182 | // STEP 1-2 : FIX CONSTANT PARAMATERS 183 | // fix base IMU pose 184 | problem.SetParameterBlockConstant(imu_pos.at(0).data()); 185 | problem.SetParameterBlockConstant(imu_ori.at(0).coeffs().data()); 186 | 187 | // fix gyroscope axis to be identical to that of imu if not applicable 188 | if (params.gyroscope_misalignment == 0) { 189 | for (int n = 0; n < params.num_imus; n ++) problem.SetParameterBlockConstant(gyr_mis.at(n).coeffs().data()); 190 | } 191 | 192 | // STEP 1-3 : SET BOUNDS OF PARAMETERS 193 | // bound bias 194 | for (int t = 0; t < num_data; t ++) { 195 | for (int n = 0; n < params.num_imus; n ++) { 196 | for (int i = 0; i < 3; i ++) { 197 | problem.SetParameterLowerBound(acc_bias_arr.at(t).at(n).data(), i, -params.ba_bound); 198 | problem.SetParameterUpperBound(acc_bias_arr.at(t).at(n).data(), i, +params.ba_bound); 199 | problem.SetParameterLowerBound(gyr_bias_arr.at(t).at(n).data(), i, -params.bw_bound); 200 | problem.SetParameterUpperBound(gyr_bias_arr.at(t).at(n).data(), i, +params.bw_bound); 201 | } 202 | } 203 | } 204 | 205 | // STEP 2 : CONSTRUCT FACTORS 206 | for (int n = 1; n < params.num_imus; n ++) { 207 | for (int t = 0; t < num_data; t ++) { 208 | // Construct each residual's noise covariance matrices 209 | Eigen::Matrix3d Cov_acc = 2 * Cov_a + Cov_w * Cov_w; 210 | Eigen::Matrix3d Cov_gyr = 2 * Cov_w; 211 | Eigen::Matrix sqrt_Info_acc = Eigen::LLT >(Cov_acc.inverse()).matrixL().transpose(); 212 | Eigen::Matrix sqrt_Info_gyr = Eigen::LLT >(Cov_gyr.inverse()).matrixL().transpose(); 213 | 214 | // Add residual 215 | ceres::CostFunction* cost_acc = Ours::AcclResidual::Create(dt, a_measurements.at(t).at(n), w_measurements.at(t).at(0), a_measurements.at(t).at(0), sqrt_Info_acc); 216 | ceres::CostFunction* cost_gyr = Ours::AngvelResidual::Create(w_measurements.at(t).at(n), w_measurements.at(t).at(0), sqrt_Info_gyr); 217 | 218 | // Assign variable (subject to be optimized) 219 | problem.AddResidualBlock(cost_acc, new ceres::CauchyLoss(1.0), 220 | imu_pos.at(n).data(), imu_ori.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), wd_inI.at(t).data(), 221 | acc_bias_arr.at(t).at(n).data(), gyr_bias_arr.at(t).at(0).data(), acc_bias_arr.at(t).at(0).data()); 222 | problem.AddResidualBlock(cost_gyr, new ceres::CauchyLoss(1.0), 223 | imu_ori.at(n).coeffs().data(), gyr_mis.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), 224 | gyr_bias_arr.at(t).at(n).data(), gyr_bias_arr.at(t).at(0).data()); 225 | } 226 | } 227 | 228 | // bias evolution 229 | for (int n = 0; n < params.num_imus; n ++) { 230 | for (int t = 0; t < num_data-1; t ++) { 231 | // biases 232 | Eigen::Matrix sqrt_Info_ab = Eigen::LLT >(Cov_ab.inverse()).matrixL().transpose(); 233 | Eigen::Matrix sqrt_Info_wb = Eigen::LLT >(Cov_wb.inverse()).matrixL().transpose(); 234 | 235 | problem.AddResidualBlock(Ours::BiasResidual::Create(sqrt_Info_ab), new ceres::CauchyLoss(1.0), 236 | acc_bias_arr.at(t).at(n).data(), acc_bias_arr.at(t+1).at(n).data()); 237 | problem.AddResidualBlock(Ours::BiasResidual::Create(sqrt_Info_wb), new ceres::CauchyLoss(1.0), 238 | gyr_bias_arr.at(t).at(n).data(), gyr_bias_arr.at(t+1).at(n).data()); 239 | } 240 | } 241 | 242 | /* 243 | // anchor the very first bias to avoid rank deficiency while calculating covariance matrices 244 | for (int n = 0; n < params.num_imus; n ++) { 245 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, acc_bias_arr.at(0).at(n).data()); 246 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, gyr_bias_arr.at(0).at(n).data()); 247 | } 248 | */ 249 | } 250 | 251 | void Estimator::solve_problem() 252 | { 253 | tic = std::chrono::high_resolution_clock::now(); 254 | 255 | // Run the solver 256 | Solve(options, &problem, &summary); 257 | 258 | // Record initial and final cost 259 | initial_cost = summary.initial_cost; 260 | final_cost = summary.final_cost; 261 | summary.FullReport(); 262 | 263 | toc = std::chrono::high_resolution_clock::now(); 264 | } 265 | 266 | void Estimator::get_extrinsics(std::map& imu_extrinsics_estimated) 267 | { 268 | // Export imu_ori and imu_pos as imu_extrinsics_estimated 269 | for (int n = 0; n < params.num_imus; n ++) { 270 | Eigen::VectorXd imu_eigen(7); 271 | // read estimated extrinsics (w.r.t. I); p_BinIn, q_BtoIn 272 | imu_eigen.block(0,0,4,1) = imu_ori.at(n).coeffs(); 273 | imu_eigen.block(4,0,3,1) = imu_pos.at(n); 274 | imu_extrinsics_estimated.insert({n, imu_eigen}); 275 | } 276 | } 277 | 278 | void Estimator::get_gyr_mis(std::map& gyr_mis_estimated) 279 | { 280 | gyr_mis_estimated = gyr_mis; 281 | } 282 | 283 | void Estimator::show_results() 284 | { 285 | if (params.show_report) print_report(); 286 | if (params.show_timer) print_timer(); 287 | if (params.show_covariance) print_covariance(); 288 | } 289 | 290 | std::pair Estimator::print_covariance() 291 | { 292 | Eigen::Matrix cov_pos = Eigen::Matrix::Zero(); 293 | Eigen::Matrix cov_ori = Eigen::Matrix::Zero(); 294 | 295 | ceres::Covariance::Options cov_options; 296 | ceres::Covariance covariance(cov_options); 297 | 298 | std::vector > covariance_blocks; 299 | 300 | double imu_pos_trace = 0; 301 | double imu_ori_trace = 0; 302 | 303 | for (int n = 1; n < params.num_imus; n ++) { 304 | covariance_blocks.push_back(std::make_pair(imu_pos.at(n).data(), imu_pos.at(n).data())); 305 | covariance_blocks.push_back(std::make_pair(imu_ori.at(n).coeffs().data(), imu_ori.at(n).coeffs().data())); 306 | covariance.Compute(covariance_blocks, &problem); 307 | 308 | covariance.GetCovarianceBlock(imu_pos.at(n).data(), imu_pos.at(n).data(), cov_pos.data()); 309 | covariance.GetCovarianceBlock(imu_ori.at(n).coeffs().data(), imu_ori.at(n).coeffs().data(), cov_ori.data()); 310 | 311 | // Since above covariance matrix is represented in quaternion, it implies nothing for human. 312 | // Therefore, we adopt a jacobian matrix of euler angle w.r.t. quaternion such that any covariance in quaternion can be converted into euler angle. 313 | // C_euler = J_dEdq * C_quat * J_dEdq; 314 | // This may result in the covariance along yaw (psi)- pitch (theta) - roll (phi) 315 | // For detailed explanation, please refer to https://www.ucalgary.ca/engo_webdocs/GL/96.20096.JSchleppe.pdf 316 | double q1 = imu_ori.at(n).x(); 317 | double q2 = imu_ori.at(n).y(); 318 | double q3 = imu_ori.at(n).z(); 319 | double q4 = imu_ori.at(n).w(); 320 | 321 | double D1 = pow(q3 + q2, 2) + pow(q4 + q1, 2); 322 | double D2 = pow(q3 - q2, 2) + pow(q4 - q1, 2); 323 | double Dt = pow(1 - 4 * pow(q2 * q3 + q1 * q4, 2), 0.5); 324 | Eigen::Matrix J_dEdq; 325 | J_dEdq << -(q3 + q2)/D1+(q3-q2)/D2, (q4+q1)/D1-(q4-q1)/D2, (q4+q1)/D1-(q4-q1)/D2, -(q3+q2)/D1+(q3-q2)/D2, 326 | 2*q4/Dt, 2*q3/Dt, 2*q2/Dt, 2*q1/Dt, 327 | -(q3 + q2)/D1+(q3-q2)/D2, (q4+q1)/D1-(q4-q1)/D2, (q4+q1)/D1-(q4-q1)/D2, -(q3+q2)/D1+(q3-q2)/D2; 328 | 329 | Eigen::Matrix3d cov_ori_3x3 = J_dEdq * cov_ori * J_dEdq.transpose(); 330 | 331 | imu_pos_trace += cov_pos.trace(); 332 | imu_ori_trace += cov_ori_3x3.trace(); 333 | } 334 | 335 | printf(" -- COVARIANCE OF ESTIMATION: %.3g\n", imu_pos_trace + imu_ori_trace); 336 | 337 | return std::make_pair(imu_pos_trace, imu_ori_trace); 338 | } 339 | 340 | int Estimator::print_timer() 341 | { 342 | auto duration = std::chrono::duration_cast(toc - tic); 343 | std::cout << "Time taken by function: " << duration.count() << " [ms]" << std::endl; 344 | return duration.count(); 345 | } 346 | 347 | void Estimator::print_report() 348 | { 349 | // Report optimization process 350 | std::cout << summary.FullReport() << std::endl << std::endl; 351 | } 352 | 353 | } 354 | 355 | // SUGGESTED SELF-CALIBRATION WITH CONSTANT BIAS ESTIMATION 356 | namespace OursConstBias { 357 | 358 | class Estimator : public Ours::Estimator 359 | { 360 | protected: 361 | // parameter blocks 362 | std::map acc_bias, gyr_bias; 363 | 364 | public: 365 | Estimator(const CalibrationOptions& params, 366 | const std::map& imu_extrinsics); 367 | ~Estimator(); 368 | void feed_init(const std::vector& wd_inI); 369 | void construct_problem(const std::vector >& a_measurements, 370 | const std::vector >& w_measurements); 371 | 372 | // For Schneider et al. 373 | void get_biases(std::map& acc_bias_estimated, 374 | std::map& gyr_bias_estimated) { 375 | acc_bias_estimated = acc_bias; 376 | gyr_bias_estimated = gyr_bias; 377 | } 378 | void get_wd_inB(std::vector& wd_inI_estimated) { 379 | wd_inI_estimated = wd_inI; 380 | }; 381 | }; 382 | 383 | Estimator::Estimator(const CalibrationOptions& params, const std::map& imu_extrinsics) 384 | : Ours::Estimator::Estimator(params, imu_extrinsics) {} 385 | 386 | Estimator::~Estimator() {} 387 | 388 | void Estimator::feed_init(const std::vector& wd_inI) { 389 | // Initialize wd_inI 390 | this->wd_inI = wd_inI; 391 | 392 | // declare number of data used for construction 393 | num_data = this->wd_inI.size(); 394 | 395 | // Initialize biases 396 | for (int n = 0; n < params.num_imus; n ++) { 397 | acc_bias.insert({n, Eigen::Vector3d::Zero()}); 398 | gyr_bias.insert({n, Eigen::Vector3d::Zero()}); 399 | } 400 | } 401 | 402 | void Estimator::construct_problem(const std::vector >& a_measurements, 403 | const std::vector >& w_measurements) { 404 | // assert data length consistency 405 | assert((int)(a_measurements.size()) == num_data && (int)(w_measurements.size()) == num_data); 406 | 407 | // STEP 1 : ADD PARAMATERS TO BE OPTIMIZED 408 | // add all parameters in concern 409 | for (int n = 0; n < params.num_imus; n ++) { 410 | problem.AddParameterBlock(imu_pos.at(n).data(), 3); 411 | problem.AddParameterBlock(imu_ori.at(n).coeffs().data(), 4, quat_loc_parameterization); 412 | problem.AddParameterBlock(gyr_mis.at(n).coeffs().data(), 4, quat_loc_parameterization); 413 | problem.AddParameterBlock(acc_bias.at(n).data(), 3); 414 | problem.AddParameterBlock(gyr_bias.at(n).data(), 3); 415 | } 416 | for (int t = 0; t < num_data; t ++) problem.AddParameterBlock(wd_inI.at(t).data(), 3); 417 | 418 | // fix base IMU pose 419 | problem.SetParameterBlockConstant(imu_pos.at(0).data()); 420 | problem.SetParameterBlockConstant(imu_ori.at(0).coeffs().data()); 421 | 422 | // fix gyroscope axis to be identical to that of imu if not applicable 423 | if (params.gyroscope_misalignment == 0 || params.fix_gyr_mis) { 424 | for (int n = 0; n < params.num_imus; n ++) problem.SetParameterBlockConstant(gyr_mis.at(n).coeffs().data()); 425 | } 426 | 427 | // STEP 2 : CONSTRUCT FACTORS 428 | for (int n = 1; n < params.num_imus; n ++) { 429 | for (int t = 0; t < num_data-1; t ++) { 430 | // Construct each residual's noise covariance matrices 431 | Eigen::Matrix3d Cov_acc = 2 * (Cov_a + t * Cov_ab) + (Cov_w + t * Cov_wb) * (Cov_w + t * Cov_wb); 432 | Eigen::Matrix3d Cov_gyr = 2 * (Cov_w + t * Cov_wb); 433 | Eigen::Matrix sqrt_Info_acc = Eigen::LLT >(Cov_acc.inverse()).matrixL().transpose(); 434 | Eigen::Matrix sqrt_Info_gyr = Eigen::LLT >(Cov_gyr.inverse()).matrixL().transpose(); 435 | 436 | // Add residual 437 | ceres::CostFunction* cost_acc = Ours::AcclResidual::Create(dt, a_measurements.at(t).at(n), w_measurements.at(t).at(n), a_measurements.at(t).at(0), sqrt_Info_acc); 438 | ceres::CostFunction* cost_gyr = Ours::AngvelResidual::Create(w_measurements.at(t).at(n), w_measurements.at(t).at(0), sqrt_Info_gyr); 439 | 440 | // Assign variable (subject to be optimized) 441 | problem.AddResidualBlock(cost_acc, NULL, imu_pos.at(n).data(), imu_ori.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), wd_inI.at(t).data(), 442 | acc_bias.at(n).data(), gyr_bias.at(0).data(), acc_bias.at(0).data()); 443 | problem.AddResidualBlock(cost_gyr, NULL, imu_ori.at(n).coeffs().data(), gyr_mis.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), 444 | gyr_bias.at(n).data(), gyr_bias.at(0).data()); 445 | } 446 | } 447 | 448 | // anchor biases 449 | for (int n = 0; n < params.num_imus; n ++) { 450 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, acc_bias.at(n).data()); 451 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, gyr_bias.at(n).data()); 452 | } 453 | } 454 | 455 | } 456 | 457 | // SUGGESTED SELF-CALIBRATION AS INTRODUCED IN SCHNEIDER ET AL. 458 | namespace Schneider { 459 | 460 | class Estimator : public Ours::Estimator { 461 | public: 462 | Estimator(const CalibrationOptions& params, 463 | const std::map& imu_extrinsics); 464 | ~Estimator(); 465 | void feed_init(const std::map >& wd_inI, 466 | const std::map >& acc_bias_map, 467 | const std::map >& gyr_bias_map); 468 | void construct_problem(const std::map > >& a_measurements, 469 | const std::map > >& w_measurements); 470 | 471 | protected: 472 | // Parameter blocks 473 | std::map > wd_inI_arr; 474 | std::map > acc_bias_map, gyr_bias_map; 475 | 476 | int num_segments; 477 | }; 478 | 479 | Estimator::Estimator(const CalibrationOptions& params, const std::map& imu_extrinsics) 480 | : Ours::Estimator::Estimator(params, imu_extrinsics) {} 481 | 482 | Estimator::~Estimator() {} 483 | 484 | void Estimator::feed_init(const std::map >& wd_inI, 485 | const std::map >& acc_bias_map, 486 | const std::map >& gyr_bias_map) { 487 | // assert consistency of the number of sensors 488 | for (const auto& kv : acc_bias_map) { 489 | size_t k = kv.first; 490 | assert(acc_bias_map.at(k).size() == gyr_bias_map.at(k).size()); 491 | } 492 | 493 | // Initialize wd_inI_arr 494 | this->wd_inI_arr = wd_inI_arr; 495 | 496 | // declare number of data used for construction 497 | num_segments = wd_inI_arr.size(); 498 | 499 | // Initialize biases 500 | this->acc_bias_map = acc_bias_map; 501 | this->gyr_bias_map = gyr_bias_map; 502 | } 503 | 504 | void Estimator::construct_problem(const std::map > >& a_measurements, 505 | const std::map > >& w_measurements) { 506 | // assert data length consistency 507 | assert((int)(a_measurements.size()) == num_segments && (int)(w_measurements.size()) == num_segments); 508 | 509 | // STEP 1 : ADD PARAMATERS TO BE OPTIMIZED 510 | // add all parameters in concern 511 | for (int n = 0; n < params.num_imus; n ++) { 512 | problem.AddParameterBlock(imu_pos.at(n).data(), 3); 513 | problem.AddParameterBlock(imu_ori.at(n).coeffs().data(), 4, quat_loc_parameterization); 514 | problem.AddParameterBlock(gyr_mis.at(n).coeffs().data(), 4, quat_loc_parameterization); 515 | } 516 | for (const auto& kv : wd_inI_arr) { 517 | size_t k = kv.first; 518 | num_data = (int)(wd_inI_arr.at(k).size()); 519 | for (int n = 0; n < params.num_imus; n ++) { 520 | problem.AddParameterBlock(acc_bias_map.at(k).at(n).data(), 3); 521 | problem.AddParameterBlock(gyr_bias_map.at(k).at(n).data(), 3); 522 | } 523 | for (int t = 0; t < num_data; t ++) problem.AddParameterBlock(wd_inI_arr.at(k).at(t).data(), 3); 524 | } 525 | 526 | // fix base IMU pose 527 | problem.SetParameterBlockConstant(imu_pos.at(0).data()); 528 | problem.SetParameterBlockConstant(imu_ori.at(0).coeffs().data()); 529 | 530 | // fix gyroscope axis to be identical to that of imu if not applicable 531 | if (params.gyroscope_misalignment == 0) { 532 | for (int n = 0; n < params.num_imus; n ++) problem.SetParameterBlockConstant(gyr_mis.at(n).coeffs().data()); 533 | } 534 | 535 | // STEP 2 : CONSTRUCT FACTORS 536 | for (const auto& kv : wd_inI_arr) { 537 | size_t k = kv.first; 538 | num_data = (int)(wd_inI_arr.at(k).size()); 539 | 540 | for (int n = 1; n < params.num_imus; n ++) { 541 | for (int t = 0; t < num_data-1; t ++) { 542 | // Construct each residual's noise covariance matrices 543 | Eigen::Matrix3d Cov_acc = 2 * (Cov_a + t * Cov_ab) + (Cov_w + t * Cov_wb) * (Cov_w + t * Cov_wb); 544 | Eigen::Matrix3d Cov_gyr = 2 * (Cov_w + t * Cov_wb); 545 | Eigen::Matrix sqrt_Info_acc = Eigen::LLT >(Cov_acc.inverse()).matrixL().transpose(); 546 | Eigen::Matrix sqrt_Info_gyr = Eigen::LLT >(Cov_gyr.inverse()).matrixL().transpose(); 547 | 548 | // Add residual 549 | ceres::CostFunction* cost_acc = Ours::AcclResidual::Create(dt, a_measurements.at(k).at(t).at(n), w_measurements.at(k).at(t).at(n), a_measurements.at(k).at(t).at(0), sqrt_Info_acc); 550 | ceres::CostFunction* cost_gyr = Ours::AngvelResidual::Create(w_measurements.at(k).at(t).at(n), w_measurements.at(k).at(t).at(0), sqrt_Info_gyr); 551 | 552 | problem.AddResidualBlock(cost_acc, NULL, imu_pos.at(n).data(), imu_ori.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), wd_inI_arr.at(k).at(t).data(), 553 | acc_bias_map.at(k).at(n).data(), gyr_bias_map.at(k).at(0).data(), acc_bias_map.at(k).at(0).data()); 554 | problem.AddResidualBlock(cost_gyr, NULL, imu_ori.at(n).coeffs().data(), gyr_mis.at(n).coeffs().data(), gyr_mis.at(0).coeffs().data(), 555 | gyr_bias_map.at(k).at(n).data(), gyr_bias_map.at(k).at(0).data()); 556 | } 557 | } 558 | 559 | // anchor biases 560 | for (int n = 0; n < params.num_imus; n ++) { 561 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, acc_bias_map.at(k).at(n).data()); 562 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, gyr_bias_map.at(k).at(n).data()); 563 | } 564 | } 565 | } 566 | 567 | } 568 | 569 | 570 | // SELF-CALIBRATION FROM BURGARD ET AL. 571 | namespace Burgard { 572 | 573 | class Estimator : public Ours::Estimator { 574 | 575 | public: 576 | Estimator(const CalibrationOptions& params, 577 | const std::map& imu_extrinsics); 578 | ~Estimator(); 579 | void feed_init(const std::vector& f_inI, 580 | const std::vector& w_inI, 581 | const std::vector& wd_inI); 582 | void construct_problem(const std::vector >& a_measurements); 583 | 584 | protected: 585 | // Parameter blocks 586 | std::vector f_inI; 587 | std::vector w_inI; 588 | std::vector wd_inI; 589 | }; 590 | 591 | Estimator::Estimator(const CalibrationOptions& params, const std::map& imu_extrinsics) 592 | : Ours::Estimator::Estimator(params, imu_extrinsics) {} 593 | 594 | Estimator::~Estimator() {} 595 | 596 | void Estimator::feed_init(const std::vector& f_inI, 597 | const std::vector& w_inI, 598 | const std::vector& wd_inI) { 599 | // assert data length consistency 600 | assert(f_inI.size() == w_inI.size() && w_inI.size() == wd_inI.size()); 601 | 602 | // Copy initial guess of specific force, angular velocity, and angular acceleration 603 | this->f_inI = f_inI; 604 | this->w_inI = w_inI; 605 | this->wd_inI = wd_inI; 606 | 607 | // declare number of data used for construction 608 | num_data = f_inI.size(); 609 | } 610 | 611 | void Estimator::construct_problem(const std::vector >& a_measurements) 612 | { 613 | // assert data length consistency 614 | assert((int)(a_measurements.size()) == num_data); 615 | 616 | // STEP 1 : ADD PARAMATERS TO BE OPTIMIZED 617 | // add all parameters in concern 618 | for (int n = 0; n < params.num_imus; n ++) { 619 | problem.AddParameterBlock(imu_pos.at(n).data(), 3); 620 | problem.AddParameterBlock(imu_ori.at(n).coeffs().data(), 4, quat_loc_parameterization); 621 | } 622 | for (int t = 0; t < num_data; t ++) { 623 | problem.AddParameterBlock(f_inI.at(t).data(), 3); 624 | problem.AddParameterBlock(w_inI.at(t).data(), 3); 625 | problem.AddParameterBlock(wd_inI.at(t).data(), 3); 626 | } 627 | 628 | // fix base accelerometer pose 629 | problem.SetParameterBlockConstant(imu_pos.at(0).data()); 630 | problem.SetParameterBlockConstant(imu_ori.at(0).coeffs().data()); 631 | 632 | // STEP 2 : CONSTRUCT FACTORS 633 | // state observation error (i.e. spatial error) 634 | for (int n = 0; n < params.num_imus; n ++) { 635 | for (int t = 0; t < num_data; t ++) { 636 | // Assign measurement inputs (will remain in same) 637 | ceres::CostFunction* cost = Burgard::SpatialResidual::Create(a_measurements.at(t).at(n), params.imu_noises.sigma_a); 638 | // Assign variable (subject to be optimized): only the base pose should be obtained! 639 | problem.AddResidualBlock(cost, NULL, f_inI.at(t).data(), w_inI.at(t).data(), wd_inI.at(t).data(), imu_pos.at(n).data(), imu_ori.at(n).coeffs().data()); 640 | } 641 | } 642 | 643 | // state transition error (i.e. temporal error) 644 | for (int t = 0; t < num_data-1; t ++) { 645 | ceres::CostFunction* cost = Burgard::TemporalResidual::Create(1/params.sim_freq_imu, params.accel_transition, params.alpha_transition); 646 | problem.AddResidualBlock(cost, NULL, f_inI.at(t).data(), f_inI.at(t+1).data(), w_inI.at(t).data(), w_inI.at(t+1).data(), wd_inI.at(t).data(), wd_inI.at(t+1).data()); 647 | } 648 | } 649 | 650 | } 651 | 652 | 653 | // SELF-CALIBRATION FROM KORTIER ET AL. 654 | namespace Kortier { 655 | 656 | class Estimator : public Ours::Estimator { 657 | 658 | public: 659 | Estimator(const CalibrationOptions& params, 660 | const std::map& imu_extrinsics); 661 | ~Estimator(); 662 | void feed_init(const std::vector& p_IinG, 663 | const std::vector& v_IinG, 664 | const std::vector& a_IinG, 665 | const std::vector& q_GtoI, 666 | const std::vector& w_IinG, 667 | const std::vector& wd_IinG); 668 | void construct_problem(const std::vector >& a_measurements, 669 | const std::vector >& w_measurements); 670 | 671 | protected: 672 | // Parameter blocks 673 | std::vector p_IinG_arr; 674 | std::vector v_IinG_arr; 675 | std::vector a_IinG_arr; 676 | std::vector q_GtoI_arr; 677 | std::vector w_IinG_arr; 678 | std::vector wd_IinG_arr; 679 | std::map acc_bias; 680 | std::map gyr_bias; 681 | }; 682 | 683 | Estimator::Estimator(const CalibrationOptions& params, const std::map& imu_extrinsics) 684 | : Ours::Estimator::Estimator(params, imu_extrinsics) {} 685 | 686 | Estimator::~Estimator() {} 687 | 688 | void Estimator::feed_init(const std::vector& p_IinG, 689 | const std::vector& v_IinG, 690 | const std::vector& a_IinG, 691 | const std::vector& q_GtoI, 692 | const std::vector& w_IinG, 693 | const std::vector& wd_IinG) { 694 | // assert data length consistency 695 | assert(p_IinG.size() == v_IinG.size() && v_IinG.size() == a_IinG.size() && a_IinG.size() == q_GtoI.size() 696 | && q_GtoI.size() == w_IinG.size() && w_IinG.size() == wd_IinG.size()); 697 | 698 | // Copy initial guess of states 699 | this->p_IinG_arr = p_IinG; 700 | this->v_IinG_arr = v_IinG; 701 | this->a_IinG_arr = a_IinG; 702 | this->q_GtoI_arr = q_GtoI; 703 | this->w_IinG_arr = w_IinG; 704 | this->wd_IinG_arr = wd_IinG; 705 | 706 | // declare number of data used for construction 707 | num_data = p_IinG_arr.size(); 708 | 709 | // Initialize biases 710 | for (int n = 0; n < params.num_imus; n ++) { 711 | acc_bias.insert({n, Eigen::Vector3d::Zero()}); 712 | gyr_bias.insert({n, Eigen::Vector3d::Zero()}); 713 | } 714 | } 715 | 716 | void Estimator::construct_problem(const std::vector >& a_measurements, 717 | const std::vector >& w_measurements) 718 | { 719 | // assert data length consistency 720 | assert((int)(a_measurements.size()) == num_data && (int)(w_measurements.size()) == num_data); 721 | 722 | // STEP 1 : ADD PARAMATERS TO BE OPTIMIZED 723 | // add all parameters in concern 724 | for (int n = 0; n < params.num_imus; n ++) { 725 | problem.AddParameterBlock(imu_pos.at(n).data(), 3); 726 | problem.AddParameterBlock(imu_ori.at(n).coeffs().data(), 4, quat_loc_parameterization); 727 | problem.AddParameterBlock(gyr_mis.at(n).coeffs().data(), 4, quat_loc_parameterization); 728 | } 729 | for (int t = 0; t < num_data; t ++) { 730 | problem.AddParameterBlock(p_IinG_arr.at(t).data(), 3); 731 | problem.AddParameterBlock(v_IinG_arr.at(t).data(), 3); 732 | problem.AddParameterBlock(a_IinG_arr.at(t).data(), 3); 733 | problem.AddParameterBlock(q_GtoI_arr.at(t).coeffs().data(), 4, quat_loc_parameterization); 734 | problem.AddParameterBlock(w_IinG_arr.at(t).data(), 3); 735 | problem.AddParameterBlock(wd_IinG_arr.at(t).data(), 3); 736 | } 737 | 738 | // fix base IMU pose 739 | problem.SetParameterBlockConstant(imu_pos.at(0).data()); 740 | problem.SetParameterBlockConstant(imu_ori.at(0).coeffs().data()); 741 | 742 | // fix gyroscope axis to be identical to that of imu if not applicable 743 | if (params.gyroscope_misalignment == 0) { 744 | for (int n = 0; n < params.num_imus; n ++) problem.SetParameterBlockConstant(gyr_mis.at(n).coeffs().data()); 745 | } 746 | 747 | // STEP 2 : CONSTRUCT FACTORS 748 | // state transition error (i.e. temporal error) 749 | for (int t = 0; t < num_data-1; t ++) { 750 | ceres::CostFunction* trans_res = Kortier::TranslationalResidual::Create(1/params.sim_freq_imu, params.accel_transition); 751 | problem.AddResidualBlock(trans_res, NULL, 752 | p_IinG_arr.at(t).data(), p_IinG_arr.at(t+1).data(), 753 | v_IinG_arr.at(t).data(), v_IinG_arr.at(t+1).data(), 754 | a_IinG_arr.at(t).data(), a_IinG_arr.at(t+1).data()); 755 | 756 | ceres::CostFunction* rot_res = Kortier::RotationalResidual::Create(1/params.sim_freq_imu, params.alpha_transition); 757 | problem.AddResidualBlock(rot_res, NULL, 758 | q_GtoI_arr.at(t).coeffs().data(), q_GtoI_arr.at(t+1).coeffs().data(), 759 | w_IinG_arr.at(t).data(), w_IinG_arr.at(t+1).data(), 760 | wd_IinG_arr.at(t).data(), wd_IinG_arr.at(t+1).data()); 761 | } 762 | 763 | // measurement error 764 | for (int n = 0; n < params.num_imus; n ++) { 765 | for (int t = 0; t < num_data; t ++) { 766 | ceres::CostFunction* accl_res = Kortier::AcclResidual::Create(a_measurements.at(t).at(n), -params.gravity, params.imu_noises.sigma_a); 767 | ceres::CostFunction* gyro_res = Kortier::GyroResidual::Create(w_measurements.at(t).at(n), params.imu_noises.sigma_w); 768 | problem.AddResidualBlock(accl_res, NULL, 769 | a_IinG_arr.at(t).data(), 770 | q_GtoI_arr.at(t).coeffs().data(), w_IinG_arr.at(t).data(), wd_IinG_arr.at(t).data(), 771 | imu_pos.at(n).data(), imu_ori.at(n).coeffs().data(), acc_bias.at(n).data()); 772 | problem.AddResidualBlock(gyro_res, NULL, 773 | q_GtoI_arr.at(t).coeffs().data(), w_IinG_arr.at(t).data(), 774 | imu_ori.at(n).coeffs().data(), gyr_mis.at(n).coeffs().data(), gyr_bias.at(n).data()); 775 | } 776 | } 777 | 778 | // biases 779 | for (int n = 0; n < params.num_imus; n ++) { 780 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, acc_bias.at(n).data()); 781 | problem.AddResidualBlock(Ours::FixVec3::Create(), NULL, gyr_bias.at(n).data()); 782 | } 783 | 784 | 785 | } 786 | 787 | } 788 | 789 | 790 | # endif // ESTIMATOR_H -------------------------------------------------------------------------------- /src/residuals.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #ifndef RESIDUALS_H 19 | #define RESIDUALS_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include "ceres/ceres.h" 27 | #include "glog/logging.h" 28 | 29 | using ceres::AutoDiffCostFunction; 30 | using ceres::CostFunction; 31 | using ceres::Problem; 32 | using ceres::Solver; 33 | using ceres::Solve; 34 | 35 | 36 | 37 | /*** RESIDUALS FOR SUGGESTED SELF-CALIBRATION WITH BIAS ESTIMATION ***/ 38 | namespace Ours { 39 | class AcclResidual { 40 | public: 41 | AcclResidual(const double dt_, const Eigen::Vector3d& f_inIn_, const Eigen::Vector3d& w_inI0_, const Eigen::Vector3d& f_inI0_, const Eigen::Matrix3d& sqrt_Info_) 42 | : dt(dt_), f_inIn(f_inIn_), w_inI0(w_inI0_), f_inI0(f_inI0_), sqrt_Info(sqrt_Info_) {} 43 | 44 | template 45 | bool operator()(const T* const p_BinIn_ptr, const T* const q_BtoIn_ptr, const T* const q_I0tog_ptr, const T* const wd_inB_ptr, 46 | const T* const am_bias_In_ptr, const T* const wm_bias_I0_ptr, const T* const am_bias_I0_ptr, 47 | T* residuals_ptr) const { 48 | Eigen::Map > p_BinIn(p_BinIn_ptr); 49 | Eigen::Map > q_BtoIn(q_BtoIn_ptr); 50 | Eigen::Map > q_I0tog(q_I0tog_ptr); 51 | Eigen::Map > wd_inB(wd_inB_ptr); 52 | 53 | Eigen::Map > am_bias_In(am_bias_In_ptr); 54 | Eigen::Map > wm_bias_I0(wm_bias_I0_ptr); 55 | Eigen::Map > am_bias_I0(am_bias_I0_ptr); 56 | 57 | Eigen::Matrix R_BtoIn(q_BtoIn); 58 | Eigen::Matrix R_I0tog(q_I0tog); 59 | Eigen::Matrix p_IninB = - R_BtoIn.transpose() * p_BinIn; 60 | 61 | Eigen::Matrix w_inB = R_I0tog.transpose() * (w_inI0 - wm_bias_I0); 62 | 63 | Eigen::Matrix accel_transverse = wd_inB.cross(p_IninB); // transverse acceleration 64 | Eigen::Matrix accel_centripetal = w_inB.cross(w_inB.cross(p_IninB)); // centripetal acceleration 65 | 66 | Eigen::Matrix f_IninB = (f_inI0 - am_bias_I0) + accel_transverse + accel_centripetal; 67 | Eigen::Matrix f_IninIn = R_BtoIn * f_IninB; 68 | 69 | Eigen::Map > residuals(residuals_ptr); 70 | residuals.template block<3, 1>(0, 0) = (f_inIn - am_bias_In) - f_IninIn; 71 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 72 | 73 | return true; 74 | } 75 | 76 | static ceres::CostFunction* Create(const double dt_, const Eigen::Vector3d& f_inIn_, const Eigen::Vector3d& w_inI0_, const Eigen::Vector3d& f_inI0_, const Eigen::Matrix3d& sqrt_Info_) { 77 | return new ceres::AutoDiffCostFunction(new AcclResidual(dt_, f_inIn_, w_inI0_, f_inI0_, sqrt_Info_)); 78 | } 79 | 80 | private: 81 | const double dt; 82 | const Eigen::Vector3d f_inIn; 83 | const Eigen::Vector3d w_inI0; 84 | const Eigen::Vector3d f_inI0; 85 | const Eigen::Matrix3d sqrt_Info; 86 | }; 87 | 88 | class AngvelResidual { 89 | public: 90 | AngvelResidual(const Eigen::Vector3d& w_inIn_, const Eigen::Vector3d& w_inI0_, const Eigen::Matrix3d& sqrt_Info_) 91 | : w_inIn(w_inIn_), w_inI0(w_inI0_), sqrt_Info(sqrt_Info_) {} 92 | 93 | template 94 | bool operator()(const T* const q_BtoIn_ptr, const T* const q_Intog_ptr, const T* const q_I0tog_ptr, 95 | const T* const wm_bias_In_ptr, const T* const wm_bias_I0_ptr, 96 | T* residuals_ptr) const { 97 | Eigen::Map > q_BtoIn(q_BtoIn_ptr); 98 | Eigen::Map > q_Intog(q_Intog_ptr); 99 | Eigen::Map > q_I0tog(q_I0tog_ptr); 100 | 101 | Eigen::Map > wm_bias_In(wm_bias_In_ptr); 102 | Eigen::Map > wm_bias_I0(wm_bias_I0_ptr); 103 | 104 | Eigen::Matrix R_BtoIn(q_BtoIn); 105 | Eigen::Matrix R_Intog(q_Intog); 106 | Eigen::Matrix R_I0tog(q_I0tog); 107 | 108 | Eigen::Map > residuals(residuals_ptr); 109 | residuals.template block<3, 1>(0, 0) = R_BtoIn.transpose() * R_Intog.transpose() * (w_inIn - wm_bias_In) - R_I0tog.transpose() * (w_inI0 - wm_bias_I0); 110 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 111 | 112 | return true; 113 | } 114 | 115 | static ceres::CostFunction* Create(const Eigen::Vector3d& w_inIn_, const Eigen::Vector3d& w_inI0_, const Eigen::Matrix3d& sqrt_Info_) { 116 | return new ceres::AutoDiffCostFunction(new AngvelResidual(w_inIn_, w_inI0_, sqrt_Info_)); 117 | } 118 | 119 | private: 120 | const Eigen::Vector3d w_inIn; 121 | const Eigen::Vector3d w_inI0; 122 | const Eigen::Matrix3d sqrt_Info; 123 | }; 124 | 125 | class BiasResidual { 126 | public: 127 | BiasResidual(const Eigen::Matrix3d& sqrt_Info_) : sqrt_Info(sqrt_Info_) {} 128 | 129 | template 130 | bool operator()(const T* const bias_curr_ptr, const T* const bias_next_ptr, T* residuals_ptr) const { 131 | Eigen::Map > bias_curr(bias_curr_ptr); 132 | Eigen::Map > bias_next(bias_next_ptr); 133 | 134 | Eigen::Map > residuals(residuals_ptr); 135 | residuals.template block<3, 1>(0, 0) = bias_next - bias_curr; 136 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 137 | 138 | return true; 139 | } 140 | 141 | static ceres::CostFunction* Create(const Eigen::Matrix3d& sqrt_Info_) { 142 | return new ceres::AutoDiffCostFunction(new BiasResidual(sqrt_Info_)); 143 | } 144 | 145 | private: 146 | const Eigen::Matrix3d sqrt_Info; 147 | }; 148 | 149 | class FixVec3 { 150 | public: 151 | FixVec3() {} 152 | 153 | template 154 | bool operator()(const T* const p_ptr, T* residuals_ptr) const { 155 | Eigen::Map > p(p_ptr); 156 | 157 | Eigen::Map > residuals(residuals_ptr); 158 | residuals.template block<3, 1>(0, 0) = p; 159 | 160 | return true; 161 | } 162 | 163 | static ceres::CostFunction* Create() { 164 | return new ceres::AutoDiffCostFunction(new FixVec3()); 165 | } 166 | }; 167 | 168 | class FixQuat { 169 | public: 170 | FixQuat() {} 171 | 172 | template 173 | bool operator()(const T* const q_ptr, T* residuals_ptr) const { 174 | Eigen::Map > q(q_ptr); 175 | 176 | Eigen::Map > residuals(residuals_ptr); 177 | residuals.template block<4, 1>(0, 0) = q.coeffs(); 178 | return true; 179 | } 180 | 181 | static ceres::CostFunction* Create() { 182 | return new ceres::AutoDiffCostFunction(new FixQuat()); 183 | } 184 | }; 185 | 186 | } 187 | 188 | 189 | /*** RESIDUALS FOR SELF-CALIBRATION OF BURGARD'S PAPER ***/ 190 | namespace Burgard { 191 | 192 | class TemporalResidual { 193 | public: 194 | TemporalResidual(const double dt_, const double accel_transition_, const double alpha_transition_) 195 | : dt(dt_), accel_transition(accel_transition_), alpha_transition(alpha_transition_) {} 196 | 197 | template 198 | bool operator()(const T* const a_prev_ptr, const T* const a_curr_ptr, const T* const w_prev_ptr, const T* const w_curr_ptr, const T* const wd_prev_ptr, const T* const wd_curr_ptr, T* residuals_ptr) const { 199 | Eigen::Map > a_prev(a_prev_ptr); 200 | Eigen::Map > a_curr(a_curr_ptr); 201 | Eigen::Map > w_prev(w_prev_ptr); 202 | Eigen::Map > w_curr(w_curr_ptr); 203 | Eigen::Map > wd_prev(wd_prev_ptr); 204 | Eigen::Map > wd_curr(wd_curr_ptr); 205 | 206 | Eigen::Matrix x_prev; 207 | Eigen::Matrix x_curr; 208 | 209 | x_prev.template block<3,1>(0,0) = a_prev; 210 | x_prev.template block<3,1>(3,0) = w_prev; 211 | x_prev.template block<3,1>(6,0) = wd_prev; 212 | x_curr.template block<3,1>(0,0) = a_curr; 213 | x_curr.template block<3,1>(3,0) = w_curr; 214 | x_curr.template block<3,1>(6,0) = wd_curr; 215 | 216 | Eigen::Matrix F; 217 | Eigen::Matrix Fsub; 218 | F.setIdentity(); 219 | Fsub.setIdentity(); 220 | F.template block<3, 3>(3, 6) = Fsub * dt; 221 | 222 | Eigen::Matrix t_diff = x_curr - F * x_prev; 223 | 224 | Eigen::Map> residuals(residuals_ptr); 225 | residuals.template block<9, 1>(0, 0) = t_diff; 226 | 227 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 228 | for (int k=0; k<3; k++) Cov(k,k) = pow(accel_transition, 2); 229 | for (int k=3; k<6; k++) Cov(k,k) = pow((alpha_transition * dt), 2); 230 | for (int k=6; k<9; k++) Cov(k,k) = pow(alpha_transition, 2); 231 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 232 | 233 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 234 | 235 | return true; 236 | } 237 | 238 | static ceres::CostFunction* Create(const double dt_, const double accel_transition_, const double alpha_transition_) { 239 | return new ceres::AutoDiffCostFunction(new TemporalResidual(dt_, accel_transition_, alpha_transition_)); 240 | } 241 | 242 | private: 243 | const double dt; 244 | const double accel_transition; 245 | const double alpha_transition; 246 | }; 247 | 248 | class SpatialResidual { 249 | public: 250 | SpatialResidual(const Eigen::Vector3d& f_inU_, const double sigma_a_) 251 | : f_inU(f_inU_), sigma_a(sigma_a_) {} 252 | 253 | template 254 | bool operator()(const T* const f_IinI_ptr, const T* const w_IinI_ptr, const T* const wd_IinI_ptr, const T* const p_IinU_ptr, const T* const q_ItoU_ptr, T* residuals_ptr) const { 255 | Eigen::Map > f_IinI(f_IinI_ptr); 256 | Eigen::Map > w_IinI(w_IinI_ptr); 257 | Eigen::Map > wd_IinI(wd_IinI_ptr); 258 | Eigen::Map > p_IinU(p_IinU_ptr); 259 | Eigen::Map > q_ItoU(q_ItoU_ptr); 260 | 261 | Eigen::Matrix R_ItoU(q_ItoU); 262 | Eigen::Matrix p_UinI = - R_ItoU.transpose() * p_IinU; 263 | 264 | Eigen::Matrix accel_transverse = wd_IinI.cross(p_UinI); // transverse acceleration 265 | Eigen::Matrix accel_centripetal = w_IinI.cross(w_IinI.cross(p_UinI)); // centripetal acceleration 266 | 267 | Eigen::Matrix f_UinI = f_IinI + accel_transverse + accel_centripetal; 268 | Eigen::Matrix f_UinU = R_ItoU * f_UinI; 269 | 270 | Eigen::Map > residuals(residuals_ptr); 271 | residuals.template block<3, 1>(0, 0) = f_UinU - f_inU; 272 | 273 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 274 | for (int k=0; k<3; k++) Cov(k,k) = pow(sigma_a, 2); 275 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 276 | 277 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 278 | 279 | return true; 280 | } 281 | 282 | static ceres::CostFunction* Create(const Eigen::Vector3d& f_inU_, const double sigma_a_) { 283 | return new ceres::AutoDiffCostFunction(new SpatialResidual(f_inU_, sigma_a_)); 284 | } 285 | 286 | private: 287 | const Eigen::Vector3d f_inU; 288 | const double sigma_a; 289 | }; 290 | 291 | } 292 | 293 | 294 | /*** RESIDUALS FOR SELF-CALIBRATION OF KORTIER'S PAPER ***/ 295 | namespace Kortier { 296 | 297 | class TranslationalResidual { 298 | public: 299 | TranslationalResidual(double dt_, const double accel_transition_) 300 | : dt(dt_), accel_transition(accel_transition_) {} 301 | 302 | template 303 | bool operator()(const T* const p_IkinG_ptr, const T* const p_Ikp1inG_ptr, 304 | const T* const v_IkinG_ptr, const T* const v_Ikp1inG_ptr, 305 | const T* const a_IkinG_ptr, const T* const a_Ikp1inG_ptr, 306 | T* residuals_ptr) const { 307 | Eigen::Map > p_IkinG(p_IkinG_ptr); 308 | Eigen::Map > v_IkinG(v_IkinG_ptr); 309 | Eigen::Map > a_IkinG(a_IkinG_ptr); 310 | 311 | Eigen::Map > p_Ikp1inG(p_Ikp1inG_ptr); 312 | Eigen::Map > v_Ikp1inG(v_Ikp1inG_ptr); 313 | Eigen::Map > a_Ikp1inG(a_Ikp1inG_ptr); 314 | 315 | Eigen::Map> residuals(residuals_ptr); 316 | 317 | residuals.template block<3,1>(0,0) = p_Ikp1inG - (p_IkinG + v_IkinG * dt + a_IkinG * dt * dt * 0.5); 318 | residuals.template block<3,1>(3,0) = v_Ikp1inG - (v_IkinG + a_IkinG * dt); 319 | residuals.template block<3,1>(6,0) = a_Ikp1inG - a_IkinG; 320 | 321 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 322 | for (int k=0; k<3; k++) Cov(k,k) = pow(accel_transition * dt * dt * 0.5, 2); 323 | for (int k=3; k<6; k++) Cov(k,k) = pow(accel_transition * dt, 2); 324 | for (int k=6; k<9; k++) Cov(k,k) = pow(accel_transition, 2); 325 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 326 | 327 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 328 | 329 | return true; 330 | } 331 | 332 | static ceres::CostFunction* Create(const double dt_, const double accel_transition_) { 333 | return new ceres::AutoDiffCostFunction(new TranslationalResidual(dt_, accel_transition_)); 334 | } 335 | 336 | private: 337 | const double dt; 338 | const double accel_transition; 339 | }; 340 | 341 | class RotationalResidual { 342 | public: 343 | RotationalResidual(double dt_, const double alpha_transition_) 344 | : dt(dt_), alpha_transition(alpha_transition_) {} 345 | 346 | template 347 | bool operator()(const T* const q_GtoIk_ptr, const T* const q_GtoIkp1_ptr, 348 | const T* const w_IkinG_ptr, const T* const w_Ikp1inG_ptr, 349 | const T* const wd_IkinG_ptr, const T* const wd_Ikp1inG_ptr, 350 | T* residuals_ptr) const { 351 | Eigen::Map > q_GtoIk(q_GtoIk_ptr); 352 | Eigen::Map > w_IkinG(w_IkinG_ptr); 353 | Eigen::Map > wd_IkinG(wd_IkinG_ptr); 354 | 355 | Eigen::Map > q_GtoIkp1(q_GtoIkp1_ptr); 356 | Eigen::Map > w_Ikp1inG(w_Ikp1inG_ptr); 357 | Eigen::Map > wd_Ikp1inG(wd_Ikp1inG_ptr); 358 | 359 | Eigen::Map> residuals(residuals_ptr); 360 | 361 | Eigen::Matrix w_IkinG_tmp = w_IkinG_tmp + wd_IkinG * dt; 362 | Eigen::Matrix Omega = getOmega(w_IkinG_tmp); 363 | Eigen::Matrix Exp = getExp(Omega); 364 | 365 | Eigen::Matrix quat_GtoIk = q_GtoIk.coeffs(); 366 | Eigen::Matrix quat_GtoIkp1 = q_GtoIkp1.coeffs(); 367 | 368 | residuals.template block<4,1>(0,0) = quat_GtoIkp1 - (- 0.5 * Exp * quat_GtoIk); 369 | residuals.template block<3,1>(4,0) = w_Ikp1inG - (w_IkinG + wd_IkinG * dt); 370 | residuals.template block<3,1>(7,0) = wd_Ikp1inG - wd_IkinG; 371 | 372 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 373 | 374 | Eigen::Matrix vec_cov; 375 | for (int k=0; k<3; k++) vec_cov(k,0) = pow(alpha_transition * dt, 2); 376 | Eigen::Matrix Omega_cov = getOmega(vec_cov); 377 | Eigen::Matrix Exp_cov = getExp(Omega_cov); 378 | 379 | Cov.block<4,4>(0,0) = Exp_cov; 380 | for (int k=4; k<7; k++) Cov(k,k) = pow(alpha_transition * dt, 2); 381 | for (int k=7; k<10; k++) Cov(k,k) = pow(alpha_transition, 2); 382 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 383 | 384 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 385 | 386 | return true; 387 | } 388 | 389 | static ceres::CostFunction* Create(const double dt_, const double alpha_transition_) { 390 | return new ceres::AutoDiffCostFunction(new RotationalResidual(dt_, alpha_transition_)); 391 | } 392 | 393 | template 394 | Eigen::Matrix getOmega(Eigen::Matrix w) const { 395 | Eigen::Matrix Omega = Eigen::Matrix::Zero(); 396 | Omega.template block<1,3>(0,1) = w.transpose(); 397 | Omega.template block<3,1>(1,0) = -w; 398 | Omega.template block<3,3>(1,1) = getSkew(w); 399 | return Omega; 400 | } 401 | 402 | template 403 | Eigen::Matrix getSkew(Eigen::Matrix w) const { 404 | Eigen::Matrix Skew;// = Eigen::Matrix::Zero(); 405 | Skew(1,2) = -w(0,0); 406 | Skew(2,1) = w(0,0); 407 | Skew(0,2) = w(1,0); 408 | Skew(2,0) = -w(1,0); 409 | Skew(0,1) = -w(2,0); 410 | Skew(1,0) = w(2,0); 411 | return Skew; 412 | } 413 | 414 | template 415 | Eigen::Matrix getExp(Eigen::Matrix Omega) const { 416 | Eigen::Matrix Exp; 417 | Exp = Eigen::Matrix::Identity() + Omega * dt * 0.5; 418 | return Exp; 419 | } 420 | 421 | private: 422 | const double dt; 423 | const double alpha_transition; 424 | }; 425 | 426 | class AcclResidual { 427 | public: 428 | AcclResidual(const Eigen::Vector3d& f_inU_, const Eigen::Vector3d& g_inG_, const double& sigma_a_) 429 | : f_inU(f_inU_), g_inG(g_inG_), sigma_a(sigma_a_) {} 430 | 431 | template 432 | bool operator()(const T* const a_IinG_ptr, 433 | const T* const q_GtoI_ptr, const T* const w_IinG_ptr, const T* const wd_IinG_ptr, 434 | const T* const p_IinU_ptr, const T* const q_ItoU_ptr, const T* const U_bias_ptr, 435 | T* residuals_ptr) const { 436 | Eigen::Map > a_IinG(a_IinG_ptr); 437 | 438 | Eigen::Map > q_GtoI(q_GtoI_ptr); 439 | Eigen::Map > w_IinG(w_IinG_ptr); 440 | Eigen::Map > wd_IinG(wd_IinG_ptr); 441 | 442 | Eigen::Map > p_IinU(p_IinU_ptr); 443 | Eigen::Map > q_ItoU(q_ItoU_ptr); 444 | 445 | Eigen::Map > U_bias(U_bias_ptr); 446 | 447 | Eigen::Matrix R_ItoU(q_ItoU); 448 | Eigen::Matrix p_UinI = - R_ItoU.transpose() * p_IinU; 449 | 450 | Eigen::Matrix R_GtoI(q_GtoI); 451 | Eigen::Matrix w_IinI = R_GtoI * w_IinG; 452 | Eigen::Matrix wd_IinI = R_GtoI * wd_IinG; 453 | 454 | Eigen::Matrix accel_transverse = wd_IinI.cross(p_UinI); // transverse acceleration 455 | Eigen::Matrix accel_centripetal = w_IinI.cross(w_IinI.cross(p_UinI)); // centripetal acceleration 456 | 457 | Eigen::Matrix a_UinI = R_GtoI * a_IinG + accel_transverse + accel_centripetal; 458 | Eigen::Matrix f_UinU = R_ItoU * (a_UinI - R_GtoI * g_inG); 459 | 460 | Eigen::Map > residuals(residuals_ptr); 461 | residuals.template block<3, 1>(0, 0) = f_UinU - (f_inU - U_bias); 462 | 463 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 464 | for (int k=0; k<3; k++) Cov(k,k) = sigma_a; 465 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 466 | 467 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 468 | 469 | return true; 470 | } 471 | 472 | static ceres::CostFunction* Create(const Eigen::Vector3d& f_inU_, const Eigen::Vector3d& g_inG_, const double& sigma_a_) { 473 | return new ceres::AutoDiffCostFunction(new AcclResidual(f_inU_, g_inG_, sigma_a_)); 474 | } 475 | 476 | private: 477 | const Eigen::Vector3d f_inU; 478 | const Eigen::Vector3d g_inG; 479 | const double sigma_a; 480 | }; 481 | 482 | class GyroResidual { 483 | public: 484 | GyroResidual(const Eigen::Vector3d& w_inU_, const double& sigma_w_) 485 | : w_inU(w_inU_), sigma_w(sigma_w_) {} 486 | 487 | template 488 | bool operator()(const T* const q_GtoI_ptr, const T* const w_IinG_ptr, 489 | const T* const q_ItoU_ptr, const T* const q_Utog_ptr, const T* const U_bias_ptr, 490 | T* residuals_ptr) const { 491 | Eigen::Map > q_GtoI(q_GtoI_ptr); 492 | 493 | Eigen::Map > w_IinG(w_IinG_ptr); 494 | 495 | Eigen::Map > q_ItoU(q_ItoU_ptr); 496 | Eigen::Map > q_Utog(q_Utog_ptr); 497 | 498 | Eigen::Map > U_bias(U_bias_ptr); 499 | 500 | Eigen::Matrix R_ItoU(q_ItoU); 501 | Eigen::Matrix R_Utog(q_Utog); 502 | Eigen::Matrix R_GtoI(q_GtoI); 503 | 504 | Eigen::Matrix w_UinU = R_Utog * R_ItoU * R_GtoI * w_IinG; 505 | 506 | Eigen::Map > residuals(residuals_ptr); 507 | residuals.template block<3, 1>(0, 0) = (w_inU - U_bias) - w_UinU; 508 | 509 | Eigen::Matrix Cov = Eigen::Matrix::Zero(); 510 | for (int k=0; k<3; k++) Cov(k,k) = sigma_w; 511 | Eigen::Matrix sqrt_Info = Eigen::LLT >(Cov.inverse()).matrixL().transpose(); 512 | 513 | residuals.applyOnTheLeft(sqrt_Info.template cast()); 514 | 515 | return true; 516 | } 517 | 518 | static ceres::CostFunction* Create(const Eigen::Vector3d& w_inU_, const double& sigma_w_) { 519 | return new ceres::AutoDiffCostFunction(new GyroResidual(w_inU_, sigma_w_)); 520 | } 521 | 522 | private: 523 | const Eigen::Vector3d w_inU; 524 | const double sigma_w; 525 | }; 526 | 527 | } 528 | 529 | 530 | # endif // RESIDUALS_H -------------------------------------------------------------------------------- /src/run_record.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #include // for setiosflags 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "residuals.h" 25 | #include "estimator.h" 26 | #include "utils/parse_ros.h" 27 | #include "utils/calibration_options.h" 28 | #include "utils/utils.h" 29 | #include "utils/read_imu_csv.h" 30 | #include "utils/eval.h" 31 | 32 | // Main function 33 | int main(int argc, char** argv) 34 | { 35 | google::InitGoogleLogging(argv[0]); 36 | 37 | // Read in our parameters 38 | CalibrationOptions params; 39 | ros::init(argc, argv, "run_record"); 40 | ros::NodeHandle nh("~"); 41 | params = parse_ros_nodehandler(nh); 42 | 43 | // Step through the rosbag 44 | signal(SIGINT, signal_callback_handler); 45 | 46 | std::vector > am_arr, wm_arr; 47 | 48 | // ##### FROM HERE ADDED BLOCKS 49 | // load data 50 | std::string filename_imu0 = params.filepath_csv + "imu0.csv"; 51 | std::string filename_imu1 = params.filepath_csv + "imu1.csv"; 52 | std::vector am_imu0, am_imu1, wm_imu0, wm_imu1; 53 | read_imu_csv(filename_imu0, am_imu0, wm_imu0); 54 | read_imu_csv(filename_imu1, am_imu1, wm_imu1); 55 | printf("Read csv done.\n"); 56 | 57 | // crop data 58 | if (params.len_sequence != -1) { 59 | am_imu0.resize(params.len_sequence); 60 | wm_imu0.resize(params.len_sequence); 61 | am_imu1.resize(params.len_sequence); 62 | wm_imu1.resize(params.len_sequence); 63 | } 64 | 65 | // create arbitrary map data 66 | std::map > am_map, wm_map; 67 | am_map.insert({0, am_imu0}); 68 | am_map.insert({1, am_imu1}); 69 | wm_map.insert({0, wm_imu0}); 70 | wm_map.insert({1, wm_imu1}); 71 | 72 | // swap data 73 | swap_imu_readings(am_map, am_arr); 74 | swap_imu_readings(wm_map, wm_arr); 75 | 76 | int count = am_arr.size(); 77 | printf("[COUNT]: %d \n", count); 78 | 79 | // ##### HERE ADDED BLOCKS END 80 | 81 | // Initialize pose information for imus 82 | std::map imu_pose_initial = params.imu_extrinsics; 83 | for (int n = 1; n < params.num_imus; n ++) { 84 | // Eigen::VectorXd zero_vec(7); zero_vec << 0,0,0,1,0,0,0; 85 | // imu_pose_initial.at(n) = zero_vec; 86 | add_noise(imu_pose_initial.at(n), params.pos_offset_mu, params.pos_offset_sd, params.ori_offset_mu, params.ori_offset_sd); 87 | } 88 | 89 | // Initialize wd_inB_init, an initial guess for angular acceleration seen by base IMU 90 | std::vector wd_inI_init(count); 91 | for (int t = 1; t < count-1; t ++) wd_inI_init.at(t) = 0.5 * (wm_arr.at(t+1).at(0) - wm_arr.at(t-1).at(0)) * params.sim_freq_imu; 92 | 93 | // Container for pose estimation 94 | std::map imu_pose_estimated; 95 | std::map gyr_mis_estimated; 96 | 97 | // Create estimator 98 | Ours::Estimator estimator(params, imu_pose_initial); 99 | estimator.feed_init(wd_inI_init); 100 | // Do calibration 101 | estimator.construct_problem(am_arr, wm_arr); 102 | estimator.solve_problem(); 103 | estimator.get_extrinsics(imu_pose_estimated); 104 | estimator.get_gyr_mis(gyr_mis_estimated); 105 | estimator.show_results(); 106 | 107 | // Print all pose estimation result 108 | print_results(params.imu_extrinsics, imu_pose_initial, imu_pose_estimated, gyr_mis_estimated); 109 | 110 | // export estimated pose to file 111 | std::string export_filename = params.filepath_csv + params.filename_csv; 112 | std::cout << " -- EXPORT RESULTS AT " << export_filename << "..." << std::endl; 113 | export_pose(export_filename, imu_pose_estimated.at(1), gyr_mis_estimated, estimator.get_result(), estimator.print_timer()); 114 | 115 | // erase A0 pose information 116 | imu_pose_initial.erase(0); 117 | imu_pose_estimated.erase(0); 118 | params.imu_extrinsics.erase(0); 119 | 120 | // Print estimation error 121 | print_rmse(params.imu_extrinsics, imu_pose_initial, imu_pose_estimated, params.gyr_mis_extrinsics, gyr_mis_estimated); 122 | 123 | // Done! 124 | return EXIT_SUCCESS; 125 | 126 | } 127 | -------------------------------------------------------------------------------- /src/utils/calibration_options.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | */ 19 | #ifndef CALIBRATION_OPTIONS_H 20 | #define CALIBRATION_OPTIONS_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include "math.h" 29 | 30 | 31 | using namespace std; 32 | // using namespace ov_core; 33 | 34 | 35 | # if defined(OV_MSCKF_VIOMANAGEROPTIONS_H) // OpenVINS wrapper 36 | struct CalibrationOptions : ov_msckf::VioManagerOptions { 37 | # else // without OpenVINS 38 | struct CalibrationOptions { 39 | # endif 40 | 41 | // ESTIMATOR =============================== 42 | 43 | /// Frequency (Hz) that we will simulate our inertial measurement unit 44 | double sim_freq_imu = 400.0; 45 | 46 | /// IMU noise (gyroscope and accelerometer) 47 | /** 48 | * @brief Struct of our imu noise parameters 49 | */ 50 | struct NoiseManager { 51 | 52 | /// Gyroscope white noise (rad/s/sqrt(hz)) 53 | double sigma_w = 1.6968e-04; 54 | 55 | /// Gyroscope white noise covariance 56 | double sigma_w_2 = pow(1.6968e-04, 2); 57 | 58 | /// Gyroscope random walk (rad/s^2/sqrt(hz)) 59 | double sigma_wb = 1.9393e-05; 60 | 61 | /// Gyroscope random walk covariance 62 | double sigma_wb_2 = pow(1.9393e-05, 2); 63 | 64 | /// Accelerometer white noise (m/s^2/sqrt(hz)) 65 | double sigma_a = 2.0000e-3; 66 | 67 | /// Accelerometer white noise covariance 68 | double sigma_a_2 = pow(2.0000e-3, 2); 69 | 70 | /// Accelerometer random walk (m/s^3/sqrt(hz)) 71 | double sigma_ab = 3.0000e-03; 72 | 73 | /// Accelerometer random walk covariance 74 | double sigma_ab_2 = pow(3.0000e-03, 2); 75 | 76 | /// Nice print function of what parameters we have loaded 77 | void print() { 78 | printf("\t- gyroscope_noise_density: %.6f\n", sigma_w); 79 | printf("\t- accelerometer_noise_density: %.5f\n", sigma_a); 80 | printf("\t- gyroscope_random_walk: %.7f\n", sigma_wb); 81 | printf("\t- accelerometer_random_walk: %.6f\n", sigma_ab); 82 | } 83 | 84 | }; 85 | NoiseManager imu_noises; 86 | 87 | /** 88 | * @brief This function will print out all noise parameters loaded. 89 | * This allows for visual checking that everything was loaded properly from ROS/CMD parsers. 90 | */ 91 | void print_noise() { 92 | printf("NOISE PARAMETERS:\n"); 93 | imu_noises.print(); 94 | } 95 | 96 | /// newly added 97 | int num_imus = 1; 98 | int len_sequence = 500; 99 | int num_sequence = 1; 100 | 101 | // Seed for initial states pertaining to calibration (i.e. initial guess for sensor pose) 102 | int sim_seed_calibration = 0; 103 | 104 | // for run_sim_only 105 | int playback_speed = 1; 106 | 107 | bool show_report = true; 108 | bool show_timer = true; 109 | bool show_covariance = true; 110 | /// Map between imuid and imu extrinsics (q_ItoU, p_IinU). 111 | std::map imu_extrinsics; 112 | // Map btween imuid and gyro mislignment for each imu (R_Utog). 113 | int gyroscope_misalignment = 0; 114 | std::map gyr_mis_extrinsics; 115 | 116 | void print_imus() { 117 | printf("IMU PARAMETERS:\n"); 118 | assert((int)imu_extrinsics.size() == num_imus); 119 | for(int n=0; n. 18 | */ 19 | #ifndef EVAL_H 20 | #define EVAL_H 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include "gen_noise.h" 27 | #include "math.h" 28 | 29 | 30 | class extrinsicError { 31 | public: 32 | double pos_rmse; 33 | double ori_rmse; 34 | }; 35 | 36 | 37 | void imu_extrinsics_error(const std::map& imu_extrinsics_gt, const std::map& imu_extrinsics_et, extrinsicError& result) { 38 | int num_sensors = imu_extrinsics_et.size(); 39 | 40 | // pose difference 41 | std::vector p_IinU_diff; 42 | std::vector q_ItoU_diff; 43 | 44 | for (const auto& p : imu_extrinsics_et) { 45 | int m = p.first; // used only to read sensor index 46 | Eigen::Vector3d p_IinU_et = imu_extrinsics_et.at(m).block(4,0,3,1); 47 | Eigen::Vector3d p_IinU_gt = imu_extrinsics_gt.at(m).block(4,0,3,1); 48 | 49 | Eigen::Vector4d quat_ItoU_et = imu_extrinsics_et.at(m).block(0,0,4,1); 50 | Eigen::Vector4d quat_ItoU_gt = imu_extrinsics_gt.at(m).block(0,0,4,1); 51 | Eigen::Map q_ItoU_et(quat_ItoU_et.data()); 52 | Eigen::Map q_ItoU_gt(quat_ItoU_gt.data()); 53 | 54 | p_IinU_diff.push_back((p_IinU_et - p_IinU_gt).array().abs().matrix().norm()); 55 | q_ItoU_diff.push_back(abs(q_ItoU_et.angularDistance(q_ItoU_gt)) * (180/M_PI)); 56 | } 57 | 58 | // calculate mean and standard deviation 59 | double pos_mse_sum = std::inner_product(p_IinU_diff.begin(), p_IinU_diff.end(), p_IinU_diff.begin(), 0.0); // sum[X^2] 60 | double ori_mse_sum = std::inner_product(q_ItoU_diff.begin(), q_ItoU_diff.end(), q_ItoU_diff.begin(), 0.0); // sum[X^2] 61 | 62 | result.pos_rmse = sqrt(pos_mse_sum / num_sensors); 63 | result.ori_rmse = sqrt(ori_mse_sum / num_sensors); 64 | } 65 | 66 | 67 | void print_imus(std::map imu_extrinsics) { 68 | // Print IMU poses 69 | printf("IMU PARAMETERS:\n"); 70 | for(size_t n=0; n& imupose_gt, 82 | const std::map& imupose_init, 83 | const std::map& imupose_estm) { 84 | // Print all IMU's GT, initial guess, and estimated pose, respectively 85 | printf(" -- IMU EXTRINSICS (GROUND TRUTH): \n"); 86 | print_imus(imupose_gt); 87 | printf(" -- IMU EXTRINSICS (INITIAL) : \n"); 88 | print_imus(imupose_init); 89 | printf(" -- IMU EXTRINSICS (ESTIMATED) : \n"); 90 | print_imus(imupose_estm); 91 | } 92 | 93 | 94 | void print_results(const std::map& imupose_gt, 95 | const std::map& imupose_init, 96 | const std::map& imupose_estm, 97 | const std::map& gyr_mis_estm) { 98 | // Print all IMU's GT, initial guess, and estimated pose, respectively 99 | printf(" -- IMU EXTRINSICS (GROUND TRUTH): \n"); 100 | print_imus(imupose_gt); 101 | printf(" -- IMU EXTRINSICS (INITIAL) : \n"); 102 | print_imus(imupose_init); 103 | printf(" -- IMU EXTRINSICS (ESTIMATED) : \n"); 104 | print_imus(imupose_estm); 105 | printf(" -- GYRO MISALIGNMENT (ESTIMATED): \n"); 106 | for (const auto & gyr_mis_est : gyr_mis_estm){ 107 | printf("GYR_MIS_%d:\n", gyr_mis_est.first); 108 | printf("%f %f %f %f\n", gyr_mis_est.second.x(), gyr_mis_est.second.y(), gyr_mis_est.second.z(), gyr_mis_est.second.w()); 109 | } 110 | } 111 | 112 | 113 | void print_rmse(const std::map& imupose_gt, 114 | const std::map& imupose_init, 115 | const std::map& imupose_estm) { 116 | // Print RMSE averaged over imus in concern 117 | extrinsicError rmse_bfore, rmse_after; 118 | imu_extrinsics_error(imupose_gt, imupose_init, rmse_bfore); 119 | imu_extrinsics_error(imupose_gt, imupose_estm, rmse_after); 120 | printf(" -- AVERAGE POSE RMSE ERROR : \n"); 121 | printf(" TRANSLATION [mm] : %.4f --> %.4f\n", rmse_bfore.pos_rmse * 1e3, rmse_after.pos_rmse * 1e3); 122 | printf(" ROTATION [deg] : %.4f --> %.4f\n", rmse_bfore.ori_rmse, rmse_after.ori_rmse); 123 | } 124 | 125 | 126 | void print_rmse(const std::map& imupose_gt, 127 | const std::map& imupose_init, 128 | const std::map& imupose_estm, 129 | const std::map& gyr_mis_gt, 130 | const std::map& gyr_mis_estm) { 131 | // Print RMSE averaged over imus in concern 132 | extrinsicError rmse_bfore, rmse_after; 133 | imu_extrinsics_error(imupose_gt, imupose_init, rmse_bfore); 134 | imu_extrinsics_error(imupose_gt, imupose_estm, rmse_after); 135 | printf(" -- AVERAGE POSE RMSE ERROR : \n"); 136 | printf(" TRANSLATION [mm] : %.4f --> %.4f\n", rmse_bfore.pos_rmse * 1e3, rmse_after.pos_rmse * 1e3); 137 | printf(" ROTATION [deg] : %.4f --> %.4f\n", rmse_bfore.ori_rmse, rmse_after.ori_rmse); 138 | 139 | // Get gyro misalignment RMSE ... 140 | std::vector gyr_mis_angle_bfore, gyr_mis_angle_after; 141 | 142 | for (const auto& R : gyr_mis_gt) { 143 | int m = R.first; 144 | Eigen::Quaterniond quat_gyr_mis_gt(R.second); 145 | gyr_mis_angle_bfore.push_back(abs(Eigen::Quaterniond::Identity().angularDistance(quat_gyr_mis_gt)) * (180/M_PI)); 146 | gyr_mis_angle_after.push_back(abs(gyr_mis_estm.at(m).angularDistance(quat_gyr_mis_gt)) * (180/M_PI)); 147 | } 148 | 149 | // calculate mean and standard deviation 150 | double gyr_mis_bfore_mse_sum = std::inner_product(gyr_mis_angle_bfore.begin(), gyr_mis_angle_bfore.end(), gyr_mis_angle_bfore.begin(), 0.0); // sum[X^2] 151 | double gyr_mis_after_mse_sum = std::inner_product(gyr_mis_angle_after.begin(), gyr_mis_angle_after.end(), gyr_mis_angle_after.begin(), 0.0); // sum[X^2] 152 | 153 | printf(" GYRO MIS [deg] : %.4f --> %.4f\n", sqrt(gyr_mis_bfore_mse_sum / gyr_mis_gt.size()), sqrt(gyr_mis_after_mse_sum / gyr_mis_gt.size())); 154 | } 155 | 156 | 157 | void export_errors(const std::string filename, 158 | const std::map& imu_extrinsics_gt, 159 | const std::map& imu_extrinsics_et, 160 | const int consumed_time) { 161 | // export rotational and translational absolute errors as a file 162 | size_t num_imus = imu_extrinsics_et.size(); 163 | 164 | std::ofstream f; 165 | if (!file_exists(filename)) { // if no previous file exists 166 | f.open(filename, ios_base::out); 167 | for (size_t n = 0; n < num_imus; n ++) { 168 | f.clear(); 169 | f << "dp_imu" << n << " dq_imu" << n << " "; // add index 170 | } 171 | f << "ms" << std::endl; 172 | f.close(); 173 | } 174 | 175 | f.open(filename, ios_base::out | ios_base::app); // open file 176 | 177 | for (size_t n = 0; n < num_imus; n++) { 178 | Eigen::Vector3d p_IinU_et = imu_extrinsics_et.at(n).block(4,0,3,1); 179 | Eigen::Vector3d p_IinU_gt = imu_extrinsics_gt.at(n).block(4,0,3,1); 180 | 181 | Eigen::Vector4d quat_ItoU_et = imu_extrinsics_et.at(n).block(0,0,4,1); 182 | Eigen::Vector4d quat_ItoU_gt = imu_extrinsics_gt.at(n).block(0,0,4,1); 183 | Eigen::Map q_ItoU_et(quat_ItoU_et.data()); 184 | Eigen::Map q_ItoU_gt(quat_ItoU_gt.data()); 185 | 186 | f << (p_IinU_et - p_IinU_gt).array().abs().matrix().norm() << " "; // position error [m] 187 | f << abs(q_ItoU_et.angularDistance(q_ItoU_gt)) << " "; // orientation error [rad] 188 | } 189 | f << consumed_time << std::endl; 190 | f.close(); 191 | 192 | } 193 | 194 | void export_pose(const std::string filename, 195 | const Eigen::VectorXd& imu_extrinsic, 196 | const std::map& gyr_mis, 197 | const bool status, 198 | const int consumed_time) { 199 | // export (1) position of I1 w.r.t. I0 in I0 frame (^{I0}p_{I0}_{I1}) and (2) orientation axis of I1 w.r.t. I0 (^{I0}_{I1}R) as a file 200 | std::ofstream f; 201 | if (!file_exists(filename)) { // if no previous file exists 202 | f.open(filename, ios_base::out); 203 | f.clear(); 204 | f << "p_x p_y p_z q_x q_y q_z q_w "; 205 | for (int i=0; i q_ItoU(quat_ItoU.data()); 217 | Eigen::Matrix3d R_ItoU = q_ItoU.matrix(); 218 | 219 | Eigen::Matrix3d R_UtoI = R_ItoU.transpose(); 220 | Eigen::Vector3d p_ItoUinU = -p_UtoIinU; 221 | 222 | Eigen::Vector3d p_ItoUinI = R_UtoI * p_ItoUinU; 223 | Eigen::Quaterniond q_UtoI(R_UtoI); 224 | 225 | f << p_ItoUinI(0) << " " << p_ItoUinI(1) << " " << p_ItoUinI(2) << " "; // position [m] 226 | f << q_UtoI.x() << " " << q_UtoI.y() << " " << q_UtoI.z() << " " << q_UtoI.w() << " "; // orientation [quat] 227 | 228 | for (int i=0; i. 18 | */ 19 | #ifndef GEN_NOISE_H 20 | #define GEN_NOISE_H 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | template 29 | T norm(std::vector v) { 30 | return std::sqrt(pow(v.at(0), 2) + pow(v.at(1), 2) + pow(v.at(2), 2)); 31 | } 32 | 33 | 34 | template 35 | std::vector create_random_vector() { 36 | static std::random_device rd; 37 | static std::mt19937 rg(rd()); 38 | 39 | static std::uniform_real_distribution distr(-1000, 1000); 40 | 41 | std::vector random_vec; 42 | 43 | for(size_t i = 0; i < 3; i ++) { 44 | random_vec.emplace_back(distr(rg)); 45 | } 46 | 47 | return random_vec; 48 | 49 | } 50 | 51 | 52 | template 53 | std::vector create_random_unit_vector() { 54 | std::vector v = create_random_vector(); 55 | constexpr double eps = 0.01; 56 | T m = norm(v); 57 | 58 | if (m > eps) { 59 | typename std::vector::iterator it; 60 | for(it = v.begin(); it != v.end(); it ++ ) *it = *it/m; 61 | return v; 62 | } 63 | else { 64 | return create_random_unit_vector(); 65 | } 66 | } 67 | 68 | 69 | Eigen::Vector3d create_pos_noise(double mean, double stdv) { 70 | std::vector v = create_random_unit_vector(); 71 | Eigen::Vector3d pos_noise{v.at(0), v.at(1), v.at(2)}; 72 | 73 | std::random_device rd; 74 | std::mt19937 rg(rd()); 75 | 76 | std::normal_distribution dist(mean, stdv); 77 | double offset = dist(rg); 78 | 79 | return pos_noise * offset; 80 | } 81 | 82 | 83 | Eigen::Quaterniond create_ori_noise(double mean, double stdv) { 84 | std::vector v = create_random_unit_vector(); 85 | Eigen::Quaterniond ori_noise; 86 | 87 | std::random_device rd; 88 | std::mt19937 rg(rd()); 89 | 90 | std::normal_distribution dist(mean, stdv); 91 | double offset = dist(rg); 92 | 93 | ori_noise.w() = cos(offset/2); 94 | ori_noise.x() = sin(offset/2) * v.at(0); 95 | ori_noise.y() = sin(offset/2) * v.at(1); 96 | ori_noise.z() = sin(offset/2) * v.at(2); 97 | 98 | return ori_noise; 99 | } 100 | 101 | 102 | Eigen::Matrix3d add_noise(const Eigen::Matrix3d& R_raw, double noise_angle) { 103 | Eigen::Quaterniond q_raw(R_raw); 104 | Eigen::Quaterniond q_noise; 105 | 106 | // Calculate direction vector of q_raw 107 | Eigen::Vector3d direction; 108 | if (fabs(q_raw.x()) <= 0.001 && fabs(q_raw.y()) <= 0.001 && fabs(q_raw.z()) <= 0.001) { 109 | direction.setOnes(); 110 | } else { 111 | direction(0) = q_raw.x(); 112 | direction(1) = q_raw.y(); 113 | direction(2) = q_raw.z(); 114 | } 115 | direction.normalize(); 116 | 117 | // Create q_noise 118 | q_noise.w() = cos(noise_angle/2); 119 | q_noise.x() = direction(0) * sin(noise_angle/2); 120 | q_noise.y() = direction(1) * sin(noise_angle/2); 121 | q_noise.z() = direction(2) * sin(noise_angle/2); 122 | 123 | // Calculate noised q_raw 124 | Eigen::Matrix3d R; 125 | Eigen::Matrix3d R_noise(q_noise); 126 | R = R_noise * R_raw; 127 | 128 | return R; 129 | } 130 | 131 | 132 | void add_noise(Eigen::VectorXd& imu_extrinsic, const double pos_mu, const double pos_sd, const double ori_mu, const double ori_sd) { 133 | // add noise to imu orientation 134 | Eigen::Quaterniond ori_noise = create_ori_noise(ori_mu * M_PI/180, ori_sd * M_PI/180); 135 | Eigen::Vector4d quat_ItoU = imu_extrinsic.block(0,0,4,1); 136 | Eigen::Map q_ItoU(quat_ItoU.data()); 137 | q_ItoU = ori_noise * q_ItoU; 138 | Eigen::Map quat_ItoU_result(q_ItoU.coeffs().data()); 139 | imu_extrinsic.block(0,0,4,1) = quat_ItoU_result; 140 | 141 | // add noise to imu position 142 | Eigen::Vector3d pos_noise = create_pos_noise(pos_mu, pos_sd); 143 | Eigen::Vector3d p_IinU = imu_extrinsic.block(4,0,3,1); 144 | p_IinU = p_IinU + pos_noise; 145 | imu_extrinsic.block(4,0,3,1) = p_IinU; 146 | } 147 | 148 | 149 | #endif /* GEN_NOISE_H */ -------------------------------------------------------------------------------- /src/utils/math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | * 19 | * Original Code 20 | * OpenVINS: An Open Platform for Visual-Inertial Research 21 | * Copyright (C) 2019 Patrick Geneva 22 | * Copyright (C) 2019 Kevin Eckenhoff 23 | * Copyright (C) 2019 Guoquan Huang 24 | * Copyright (C) 2019 OpenVINS Contributors 25 | */ 26 | #if !defined(OV_EVAL_MATH_H) && !defined(OV_CORE_QUAT_OPS_H) 27 | #ifndef MATH_H 28 | #define MATH_H 29 | 30 | #include 31 | #include 32 | 33 | /** 34 | * @brief Returns a JPL quaternion from a rotation matrix 35 | * 36 | * This is based on the equation 74 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf). 37 | * In the implementation, we have 4 statements so that we avoid a division by zero and 38 | * instead always divide by the largest diagonal element. This all comes from the 39 | * definition of a rotation matrix, using the diagonal elements and an off-diagonal. 40 | * \f{align*}{ 41 | * \mathbf{R}(\bar{q})= 42 | * \begin{bmatrix} 43 | * q_1^2-q_2^2-q_3^2+q_4^2 & 2(q_1q_2+q_3q_4) & 2(q_1q_3-q_2q_4) \\ 44 | * 2(q_1q_2-q_3q_4) & -q_2^2+q_2^2-q_3^2+q_4^2 & 2(q_2q_3+q_1q_4) \\ 45 | * 2(q_1q_3+q_2q_4) & 2(q_2q_3-q_1q_4) & -q_1^2-q_2^2+q_3^2+q_4^2 46 | * \end{bmatrix} 47 | * \f} 48 | * 49 | * @param[in] rot 3x3 rotation matrix 50 | * @return 4x1 quaternion 51 | */ 52 | static inline Eigen::Matrix rot_2_quat(const Eigen::Matrix &rot) { 53 | Eigen::Matrix q; 54 | double T = rot.trace(); 55 | if ((rot(0, 0) >= T) && (rot(0, 0) >= rot(1, 1)) && (rot(0, 0) >= rot(2, 2))) { 56 | //cout << "case 1- " << endl; 57 | q(0) = sqrt((1 + (2 * rot(0, 0)) - T) / 4); 58 | q(1) = (1 / (4 * q(0))) * (rot(0, 1) + rot(1, 0)); 59 | q(2) = (1 / (4 * q(0))) * (rot(0, 2) + rot(2, 0)); 60 | q(3) = (1 / (4 * q(0))) * (rot(1, 2) - rot(2, 1)); 61 | 62 | } else if ((rot(1, 1) >= T) && (rot(1, 1) >= rot(0, 0)) && (rot(1, 1) >= rot(2, 2))) { 63 | //cout << "case 2- " << endl; 64 | q(1) = sqrt((1 + (2 * rot(1, 1)) - T) / 4); 65 | q(0) = (1 / (4 * q(1))) * (rot(0, 1) + rot(1, 0)); 66 | q(2) = (1 / (4 * q(1))) * (rot(1, 2) + rot(2, 1)); 67 | q(3) = (1 / (4 * q(1))) * (rot(2, 0) - rot(0, 2)); 68 | } else if ((rot(2, 2) >= T) && (rot(2, 2) >= rot(0, 0)) && (rot(2, 2) >= rot(1, 1))) { 69 | //cout << "case 3- " << endl; 70 | q(2) = sqrt((1 + (2 * rot(2, 2)) - T) / 4); 71 | q(0) = (1 / (4 * q(2))) * (rot(0, 2) + rot(2, 0)); 72 | q(1) = (1 / (4 * q(2))) * (rot(1, 2) + rot(2, 1)); 73 | q(3) = (1 / (4 * q(2))) * (rot(0, 1) - rot(1, 0)); 74 | } else { 75 | //cout << "case 4- " << endl; 76 | q(3) = sqrt((1 + T) / 4); 77 | q(0) = (1 / (4 * q(3))) * (rot(1, 2) - rot(2, 1)); 78 | q(1) = (1 / (4 * q(3))) * (rot(2, 0) - rot(0, 2)); 79 | q(2) = (1 / (4 * q(3))) * (rot(0, 1) - rot(1, 0)); 80 | } 81 | if (q(3) < 0) { 82 | q = -q; 83 | } 84 | // normalize and return 85 | q = q / (q.norm()); 86 | return q; 87 | } 88 | 89 | /** 90 | * @brief Skew-symmetric matrix from a given 3x1 vector 91 | * 92 | * This is based on equation 6 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf): 93 | * \f{align*}{ 94 | * \lfloor\mathbf{v}\times\rfloor = 95 | * \begin{bmatrix} 96 | * 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 97 | * \end{bmatrix} 98 | * @f} 99 | * 100 | * @param[in] w 3x1 vector to be made a skew-symmetric 101 | * @return 3x3 skew-symmetric matrix 102 | */ 103 | static inline Eigen::Matrix skew_x(const Eigen::Matrix &w) { 104 | Eigen::Matrix w_x; 105 | w_x << 0, -w(2), w(1), 106 | w(2), 0, -w(0), 107 | -w(1), w(0), 0; 108 | return w_x; 109 | } 110 | 111 | 112 | /** 113 | * @brief Converts JPL quaterion to SO(3) rotation matrix 114 | * 115 | * This is based on equation 62 in [Indirect Kalman Filter for 3D Attitude Estimation](http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf): 116 | * \f{align*}{ 117 | * \mathbf{R} = (2q_4^2-1)\mathbf{I}_3-2q_4\lfloor\mathbf{q}\times\rfloor+2\mathbf{q}^\top\mathbf{q} 118 | * @f} 119 | * 120 | * @param[in] q JPL quaternion 121 | * @return 3x3 SO(3) rotation matrix 122 | */ 123 | static inline Eigen::Matrix quat_2_Rot(const Eigen::Matrix &q) { 124 | Eigen::Matrix q_x = skew_x(q.block(0, 0, 3, 1)); 125 | Eigen::MatrixXd Rot = (2 * std::pow(q(3, 0), 2) - 1) * Eigen::MatrixXd::Identity(3, 3) 126 | - 2 * q(3, 0) * q_x + 127 | 2 * q.block(0, 0, 3, 1) * (q.block(0, 0, 3, 1).transpose()); 128 | return Rot; 129 | } 130 | 131 | 132 | #endif /* MATH_H */ 133 | #endif /* neither OV_EVAL_MATH_H nor OV_CORE_QUAT_OPS_H */ -------------------------------------------------------------------------------- /src/utils/parse_ros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * 6 | * This program is free software: you can redistribute it and/or modify 7 | * it under the terms of the GNU General Public License as published by 8 | * the Free Software Foundation, either version 3 of the License, or 9 | * (at your option) any later version. 10 | * 11 | * This program is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | * GNU General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU General Public License 17 | * along with this program. If not, see . 18 | * 19 | * Original Code 20 | * OpenVINS: An Open Platform for Visual-Inertial Research 21 | * Copyright (C) 2019 Patrick Geneva 22 | * Copyright (C) 2019 Kevin Eckenhoff 23 | * Copyright (C) 2019 Guoquan Huang 24 | * Copyright (C) 2019 OpenVINS Contributors 25 | */ 26 | #if defined(ROS_AVAILABLE) || defined(DOXYGEN) 27 | #ifndef PARSE_ROS_H 28 | #define PARSE_ROS_H 29 | 30 | #include 31 | #include 32 | 33 | #include "calibration_options.h" 34 | #include "math.h" 35 | 36 | /** 37 | * @brief This function will load paramters from the ros node handler / parameter server 38 | * This is the recommended way of loading parameters as compared to the command line version. 39 | * @param nh ROS node handler 40 | * @return A fully loaded VioManagerOptions object 41 | */ 42 | CalibrationOptions parse_ros_nodehandler(ros::NodeHandle &nh) { 43 | 44 | // Our vio manager options with defaults 45 | CalibrationOptions params; 46 | 47 | # if defined(OV_MSCKF_VIOMANAGEROPTIONS_H) // OpenVINS wrapper 48 | // ESTIMATOR ====================================================================== 49 | 50 | // Main EKF parameters 51 | nh.param("use_fej", params.state_options.do_fej, params.state_options.do_fej); 52 | nh.param("use_imuavg", params.state_options.imu_avg, params.state_options.imu_avg); 53 | nh.param("use_rk4int", params.state_options.use_rk4_integration, params.state_options.use_rk4_integration); 54 | nh.param("calib_cam_extrinsics", params.state_options.do_calib_camera_pose, params.state_options.do_calib_camera_pose); 55 | nh.param("calib_cam_intrinsics", params.state_options.do_calib_camera_intrinsics, params.state_options.do_calib_camera_intrinsics); 56 | nh.param("calib_cam_timeoffset", params.state_options.do_calib_camera_timeoffset, params.state_options.do_calib_camera_timeoffset); 57 | nh.param("max_clones", params.state_options.max_clone_size, params.state_options.max_clone_size); 58 | nh.param("max_slam", params.state_options.max_slam_features, params.state_options.max_slam_features); 59 | nh.param("max_slam_in_update", params.state_options.max_slam_in_update, params.state_options.max_slam_in_update); 60 | nh.param("max_msckf_in_update", params.state_options.max_msckf_in_update, params.state_options.max_msckf_in_update); 61 | nh.param("max_aruco", params.state_options.max_aruco_features, params.state_options.max_aruco_features); 62 | nh.param("max_cameras", params.state_options.num_cameras, params.state_options.num_cameras); 63 | nh.param("dt_slam_delay", params.dt_slam_delay, params.dt_slam_delay); 64 | 65 | // Enforce that we have enough cameras to run 66 | if(params.state_options.num_cameras < 1) { 67 | printf(RED "VioManager(): Specified number of cameras needs to be greater than zero\n" RESET); 68 | printf(RED "VioManager(): num cameras = %d\n" RESET, params.state_options.num_cameras); 69 | std::exit(EXIT_FAILURE); 70 | } 71 | 72 | // Read in what representation our feature is 73 | std::string feat_rep_msckf_str = "GLOBAL_3D"; 74 | std::string feat_rep_slam_str = "GLOBAL_3D"; 75 | std::string feat_rep_aruco_str = "GLOBAL_3D"; 76 | nh.param("feat_rep_msckf", feat_rep_msckf_str, feat_rep_msckf_str); 77 | nh.param("feat_rep_slam", feat_rep_slam_str, feat_rep_slam_str); 78 | nh.param("feat_rep_aruco", feat_rep_aruco_str, feat_rep_aruco_str); 79 | 80 | // Set what representation we should be using 81 | std::transform(feat_rep_msckf_str.begin(), feat_rep_msckf_str.end(),feat_rep_msckf_str.begin(), ::toupper); 82 | std::transform(feat_rep_slam_str.begin(), feat_rep_slam_str.end(),feat_rep_slam_str.begin(), ::toupper); 83 | std::transform(feat_rep_aruco_str.begin(), feat_rep_aruco_str.end(),feat_rep_aruco_str.begin(), ::toupper); 84 | params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string(feat_rep_msckf_str); 85 | params.state_options.feat_rep_slam = LandmarkRepresentation::from_string(feat_rep_slam_str); 86 | params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string(feat_rep_aruco_str); 87 | if(params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN || 88 | params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN || 89 | params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN) { 90 | printf(RED "VioManager(): invalid feature representation specified:\n" RESET); 91 | printf(RED "\t- GLOBAL_3D\n" RESET); 92 | printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET); 93 | printf(RED "\t- ANCHORED_3D\n" RESET); 94 | printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET); 95 | printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET); 96 | printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET); 97 | std::exit(EXIT_FAILURE); 98 | } 99 | 100 | // Filter initialization 101 | nh.param("init_window_time", params.init_window_time, params.init_window_time); 102 | nh.param("init_imu_thresh", params.init_imu_thresh, params.init_imu_thresh); 103 | 104 | // Zero velocity update 105 | nh.param("try_zupt", params.try_zupt, params.try_zupt); 106 | nh.param("zupt_chi2_multipler", params.zupt_options.chi2_multipler, params.zupt_options.chi2_multipler); 107 | nh.param("zupt_max_velocity", params.zupt_max_velocity, params.zupt_max_velocity); 108 | nh.param("zupt_noise_multiplier", params.zupt_noise_multiplier, params.zupt_noise_multiplier); 109 | 110 | // Recording of timing information to file 111 | nh.param("record_timing_information", params.record_timing_information, params.record_timing_information); 112 | nh.param("record_timing_filepath", params.record_timing_filepath, params.record_timing_filepath); 113 | 114 | 115 | // Read in update parameters 116 | nh.param("up_msckf_sigma_px", params.msckf_options.sigma_pix, params.msckf_options.sigma_pix); 117 | nh.param("up_msckf_chi2_multipler", params.msckf_options.chi2_multipler, params.msckf_options.chi2_multipler); 118 | nh.param("up_slam_sigma_px", params.slam_options.sigma_pix, params.slam_options.sigma_pix); 119 | nh.param("up_slam_chi2_multipler", params.slam_options.chi2_multipler, params.slam_options.chi2_multipler); 120 | nh.param("up_aruco_sigma_px", params.aruco_options.sigma_pix, params.aruco_options.sigma_pix); 121 | nh.param("up_aruco_chi2_multipler", params.aruco_options.chi2_multipler, params.aruco_options.chi2_multipler); 122 | 123 | 124 | // STATE ====================================================================== 125 | 126 | // Timeoffset from camera to IMU 127 | nh.param("calib_camimu_dt", params.calib_camimu_dt, params.calib_camimu_dt); 128 | 129 | // TRACKERS ====================================================================== 130 | 131 | // Tracking flags 132 | nh.param("use_stereo", params.use_stereo, params.use_stereo); 133 | nh.param("use_klt", params.use_klt, params.use_klt); 134 | nh.param("use_aruco", params.use_aruco, params.use_aruco); 135 | nh.param("downsize_aruco", params.downsize_aruco, params.downsize_aruco); 136 | nh.param("downsample_cameras", params.downsample_cameras, params.downsample_cameras); 137 | 138 | // General parameters 139 | nh.param("num_pts", params.num_pts, params.num_pts); 140 | nh.param("fast_threshold", params.fast_threshold, params.fast_threshold); 141 | nh.param("grid_x", params.grid_x, params.grid_x); 142 | nh.param("grid_y", params.grid_y, params.grid_y); 143 | nh.param("min_px_dist", params.min_px_dist, params.min_px_dist); 144 | nh.param("knn_ratio", params.knn_ratio, params.knn_ratio); 145 | 146 | // Feature initializer parameters 147 | nh.param("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs); 148 | nh.param("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda); 149 | nh.param("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda); 150 | nh.param("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx); 151 | nh.param("fi_min_dcost", params.featinit_options.min_dcost, params.featinit_options.min_dcost); 152 | nh.param("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult); 153 | nh.param("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist); 154 | nh.param("fi_max_dist", params.featinit_options.max_dist, params.featinit_options.max_dist); 155 | nh.param("fi_max_baseline", params.featinit_options.max_baseline, params.featinit_options.max_baseline); 156 | nh.param("fi_max_cond_number", params.featinit_options.max_cond_number, params.featinit_options.max_cond_number); 157 | 158 | 159 | 160 | // SIMULATION ====================================================================== 161 | 162 | // Load the groundtruth trajectory and its spline 163 | nh.param("sim_traj_path", params.sim_traj_path, params.sim_traj_path); 164 | nh.param("sim_distance_threshold", params.sim_distance_threshold, params.sim_distance_threshold); 165 | nh.param("sim_do_perturbation", params.sim_do_perturbation, params.sim_do_perturbation); 166 | 167 | // Read in sensor simulation frequencies 168 | nh.param("sim_freq_cam", params.sim_freq_cam, params.sim_freq_cam); 169 | 170 | // Load the seeds for the random number generators 171 | nh.param("sim_seed_state_init", params.sim_seed_state_init, params.sim_seed_state_init); 172 | nh.param("sim_seed_preturb", params.sim_seed_preturb, params.sim_seed_preturb); 173 | nh.param("sim_seed_measurements", params.sim_seed_measurements, params.sim_seed_measurements); 174 | 175 | //==================================================================================== 176 | //==================================================================================== 177 | //==================================================================================== 178 | 179 | // Loop through through, and load each of the cameras 180 | for(int i=0; i("cam"+std::to_string(i)+"_is_fisheye", is_fisheye, false); 185 | 186 | // If the desired fov we should simulate 187 | std::vector matrix_wh; 188 | std::vector matrix_wd_default = {752,480}; 189 | nh.param>("cam"+std::to_string(i)+"_wh", matrix_wh, matrix_wd_default); 190 | matrix_wh.at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; 191 | matrix_wh.at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; 192 | std::pair wh(matrix_wh.at(0),matrix_wh.at(1)); 193 | 194 | // Camera intrinsic properties 195 | Eigen::Matrix cam_calib; 196 | std::vector matrix_k, matrix_d; 197 | std::vector matrix_k_default = {458.654,457.296,367.215,248.375}; 198 | std::vector matrix_d_default = {-0.28340811,0.07395907,0.00019359,1.76187114e-05}; 199 | nh.param>("cam"+std::to_string(i)+"_k", matrix_k, matrix_k_default); 200 | nh.param>("cam"+std::to_string(i)+"_d", matrix_d, matrix_d_default); 201 | matrix_k.at(0) /= (params.downsample_cameras) ? 2.0 : 1.0; 202 | matrix_k.at(1) /= (params.downsample_cameras) ? 2.0 : 1.0; 203 | matrix_k.at(2) /= (params.downsample_cameras) ? 2.0 : 1.0; 204 | matrix_k.at(3) /= (params.downsample_cameras) ? 2.0 : 1.0; 205 | cam_calib << matrix_k.at(0),matrix_k.at(1),matrix_k.at(2),matrix_k.at(3),matrix_d.at(0),matrix_d.at(1),matrix_d.at(2),matrix_d.at(3); 206 | 207 | // Our camera extrinsics transform 208 | Eigen::Matrix4d T_CtoI; 209 | std::vector matrix_TCtoI; 210 | std::vector matrix_TtoI_default = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; 211 | 212 | // Read in from ROS, and save into our eigen mat 213 | nh.param>("T_C"+std::to_string(i)+"toI", matrix_TCtoI, matrix_TtoI_default); 214 | T_CtoI << matrix_TCtoI.at(0),matrix_TCtoI.at(1),matrix_TCtoI.at(2),matrix_TCtoI.at(3), 215 | matrix_TCtoI.at(4),matrix_TCtoI.at(5),matrix_TCtoI.at(6),matrix_TCtoI.at(7), 216 | matrix_TCtoI.at(8),matrix_TCtoI.at(9),matrix_TCtoI.at(10),matrix_TCtoI.at(11), 217 | matrix_TCtoI.at(12),matrix_TCtoI.at(13),matrix_TCtoI.at(14),matrix_TCtoI.at(15); 218 | 219 | // Load these into our state 220 | Eigen::Matrix cam_eigen; 221 | cam_eigen.block(0,0,4,1) = rot_2_quat(T_CtoI.block(0,0,3,3).transpose()); 222 | cam_eigen.block(4,0,3,1) = -T_CtoI.block(0,0,3,3).transpose()*T_CtoI.block(0,3,3,1); 223 | 224 | // Insert 225 | params.camera_fisheye.insert({i, is_fisheye}); 226 | params.camera_intrinsics.insert({i, cam_calib}); 227 | params.camera_extrinsics.insert({i, cam_eigen}); 228 | params.camera_wh.insert({i, wh}); 229 | 230 | } 231 | 232 | # endif 233 | 234 | // NOISE ====================================================================== 235 | 236 | // Our noise values for inertial sensor 237 | nh.param("gyroscope_noise_density", params.imu_noises.sigma_w, params.imu_noises.sigma_w); 238 | nh.param("accelerometer_noise_density", params.imu_noises.sigma_a, params.imu_noises.sigma_a); 239 | nh.param("gyroscope_random_walk", params.imu_noises.sigma_wb, params.imu_noises.sigma_wb); 240 | nh.param("accelerometer_random_walk", params.imu_noises.sigma_ab, params.imu_noises.sigma_ab); 241 | 242 | 243 | // SIMULATION ====================================================================== 244 | nh.param("sim_freq_imu", params.sim_freq_imu, params.sim_freq_imu); 245 | nh.param("num_imus", params.num_imus, params.num_imus); 246 | nh.param("len_sequence", params.len_sequence, params.len_sequence); 247 | nh.param("num_sequence", params.num_sequence, params.num_sequence); 248 | 249 | // for run_sim_only 250 | nh.param("playback_speed", params.playback_speed, params.playback_speed); 251 | 252 | nh.param("show_report", params.show_report, params.show_report); 253 | nh.param("show_timer", params.show_timer, params.show_timer); 254 | nh.param("show_covariance", params.show_covariance, params.show_covariance); 255 | 256 | nh.param("pos_offset_mu", params.pos_offset_mu, params.pos_offset_mu); 257 | nh.param("pos_offset_sd", params.pos_offset_sd, params.pos_offset_sd); 258 | nh.param("ori_offset_mu", params.ori_offset_mu, params.ori_offset_mu); 259 | nh.param("ori_offset_sd", params.ori_offset_sd, params.ori_offset_sd); 260 | 261 | nh.param("ba_bound", params.ba_bound, params.ba_bound); 262 | nh.param("bw_bound", params.bw_bound, params.bw_bound); 263 | 264 | nh.param("fix_gyr_mis", params.fix_gyr_mis, params.fix_gyr_mis); 265 | 266 | nh.param("accel_transition", params.accel_transition, params.accel_transition); 267 | nh.param("alpha_transition", params.alpha_transition, params.alpha_transition); 268 | 269 | // Loop through and load each imu extrinsic 270 | for(int i=0; i matrix_T_UtoI; 274 | std::vector matrix_T_UtoI_default = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1}; 275 | 276 | // Read in from ROS, and save into our eigen mat 277 | nh.param>("T_I"+std::to_string(i)+"toI", matrix_T_UtoI, matrix_T_UtoI_default); 278 | T_UtoI << matrix_T_UtoI.at(0),matrix_T_UtoI.at(1),matrix_T_UtoI.at(2),matrix_T_UtoI.at(3), 279 | matrix_T_UtoI.at(4),matrix_T_UtoI.at(5),matrix_T_UtoI.at(6),matrix_T_UtoI.at(7), 280 | matrix_T_UtoI.at(8),matrix_T_UtoI.at(9),matrix_T_UtoI.at(10),matrix_T_UtoI.at(11), 281 | matrix_T_UtoI.at(12),matrix_T_UtoI.at(13),matrix_T_UtoI.at(14),matrix_T_UtoI.at(15); 282 | 283 | // Load these into our state 284 | Eigen::Matrix imu_eigen; 285 | imu_eigen.block(0,0,4,1) = rot_2_quat(T_UtoI.block(0,0,3,3).transpose()); 286 | imu_eigen.block(4,0,3,1) = -T_UtoI.block(0,0,3,3).transpose()*T_UtoI.block(0,3,3,1); 287 | 288 | // Insert 289 | params.imu_extrinsics.insert({i, imu_eigen}); 290 | } 291 | 292 | // gyroscope mislignment 293 | nh.param("gyroscope_misalignment", params.gyroscope_misalignment, params.gyroscope_misalignment); 294 | std::mt19937 rd_gen = std::mt19937(params.sim_seed_calibration); 295 | rd_gen.seed(params.sim_seed_calibration); 296 | std::normal_distribution w(0, static_cast(params.gyroscope_misalignment) * M_PI / 180); 297 | 298 | // Loop through and generate each gyro misalignment 299 | for(int i=0; i("filepath_csv", params.filepath_csv, params.filepath_csv); 314 | nh.param("filename_csv", params.filename_csv, params.filename_csv); 315 | 316 | // Load the seeds for the random number generators 317 | nh.param("sim_seed_calibration", params.sim_seed_calibration, params.sim_seed_calibration); 318 | 319 | // Global gravity 320 | std::vector gravity = {params.gravity(0), params.gravity(1), params.gravity(2)}; 321 | nh.param>("gravity", gravity, gravity); 322 | assert(gravity.size()==3); 323 | params.gravity << gravity.at(0),gravity.at(1),gravity.at(2); 324 | 325 | // Success, lets returned the parsed options 326 | return params; 327 | 328 | } 329 | 330 | 331 | #endif // PARSE_ROS_H 332 | #endif // ROS_AVAILABLE -------------------------------------------------------------------------------- /src/utils/read_imu_csv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #ifndef READ_IMU_CSV_H 19 | #define READ_IMU_CSV_H 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | 32 | void swap_imu_readings(const std::map >& src, 33 | std::vector >& dst) { 34 | // clear data 35 | dst.clear(); 36 | 37 | // make sure every vector has same length 38 | int len = 0; 39 | for (const auto& x : src) { 40 | if (len == 0) len = x.second.size(); // initialize len 41 | else assert(len == x.second.size()); // assert length consistency 42 | } 43 | 44 | // swap data 45 | for (int i = 0; i < len; i ++) { 46 | std::map element; 47 | for (const auto& x : src) element.insert({x.first, x.second.at(i)}); 48 | dst.push_back(element); 49 | } 50 | } 51 | 52 | void read_imu_csv(const std::string filename, 53 | std::vector& am_arr, 54 | std::vector& wm_arr) 55 | { 56 | 57 | // clear data 58 | am_arr.clear(); 59 | wm_arr.clear(); 60 | 61 | // File pointer 62 | std::fstream fin; 63 | 64 | // Open an existing file 65 | fin.open(filename, std::ios::in); 66 | if (fin.fail()) { 67 | std::cout << "[ERROR] file could not be opened." << std::endl; 68 | std::cout << "filename: " << filename << std::endl; 69 | return; 70 | } 71 | else { 72 | std::cout << "opened " << filename << std::endl; 73 | } 74 | 75 | // Read the Data from the file as String Vector 76 | std::string line, cell; 77 | // pass header line 78 | std::getline(fin, line); 79 | 80 | // read remaining lines 81 | while (std::getline(fin, line)) { 82 | // used for breaking cells 83 | std::stringstream s(line); 84 | 85 | // read every column data of a row and store only accelerometer and gyroscope readings 86 | Eigen::Vector3d am, wm; 87 | int col = 0; 88 | while (std::getline(s, cell, ',')) { 89 | if (col == 17) wm(0) = std::stod(cell); 90 | else if (col == 18) wm(1) = std::stod(cell); 91 | else if (col == 19) wm(2) = std::stod(cell); 92 | else if (col == 29) am(0) = std::stod(cell); 93 | else if (col == 30) am(1) = std::stod(cell); 94 | else if (col == 31) am(2) = std::stod(cell); 95 | 96 | col ++; 97 | } 98 | 99 | am_arr.push_back(am); 100 | wm_arr.push_back(wm); 101 | } 102 | 103 | } 104 | 105 | #endif // READ_IMU_CSV_H -------------------------------------------------------------------------------- /src/utils/utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Extrinsic calibration of multiple inertial sensors from in-flight data 3 | * Copyright (c) 2021 Jongwon Lee (jongwon5@illinois.edu) 4 | * http://www.github.com/jongwonjlee/mixcal 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, either version 3 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License 16 | * along with this program. If not, see . 17 | */ 18 | #ifndef UTILS_H 19 | #define UTILS_H 20 | 21 | #include 22 | 23 | // Define the function to be called when ctrl-c (SIGINT) is sent to process 24 | void signal_callback_handler(int signum) { 25 | std::exit(signum); 26 | } 27 | 28 | 29 | bool file_exists(const std::string filename) { 30 | std::ifstream f(filename); 31 | return f.good(); 32 | } 33 | 34 | 35 | #endif /* UTILS_H */ --------------------------------------------------------------------------------