├── .clang-format ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cfg ├── estimator.yaml ├── rqt_multiplot.xml └── rviz_config.rviz ├── include └── mav_state_estimation │ ├── absolute_position_factor.h │ ├── initialization.h │ ├── mav_state_estimator.h │ └── moving_baseline_factor.h ├── install ├── dependencies.rosinstall ├── dependencies_https.rosinstall └── prepare-jenkins-slave.sh ├── launch ├── mav_state_estimator.launch └── mav_state_estimator_nodelet.launch ├── msg ├── BatchStatus.msg └── Timing.msg ├── nodelet_plugin.xml ├── nodes └── bag_to_csv ├── package.xml ├── python └── mav_state_estimation │ ├── __init__.py │ ├── csv_export.py │ ├── filter_imu.py │ └── plot.py ├── scripts ├── compare.ipynb ├── export_csv ├── filter_imu ├── graphviz.ipynb └── plot ├── setup.py ├── src ├── absolute_position_factor.cc ├── initialization.cc ├── mav_state_estimator.cc ├── mav_state_estimator_node.cc ├── mav_state_estimator_nodelet.cc └── moving_baseline_factor.cc ├── srv ├── BagToCsv.srv └── Batch.srv └── test ├── absolute_position_factor-test.cc └── moving_baseline_factor-test.cc /.clang-format: -------------------------------------------------------------------------------- 1 | BasedOnStyle: Google 2 | DerivePointerAlignment: false 3 | PointerAlignment: Left 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mav_state_estimation) 3 | 4 | set(CMAKE_CXX_STANDARD 17) 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 6 | set(CMAKE_BUILD_TYPE Release) 7 | 8 | find_package(catkin_simple REQUIRED) 9 | catkin_python_setup() 10 | catkin_simple(ALL_DEPS_REQUIRED) 11 | find_package(GTSAM REQUIRED) 12 | find_package(Eigen3 REQUIRED) 13 | 14 | ############# 15 | # LIBRARIES # 16 | ############# 17 | cs_add_library(${PROJECT_NAME}_nodelet 18 | src/absolute_position_factor.cc 19 | src/initialization.cc 20 | src/mav_state_estimator.cc 21 | src/mav_state_estimator_nodelet.cc 22 | src/moving_baseline_factor.cc 23 | ) 24 | target_link_libraries(${PROJECT_NAME}_nodelet gtsam gtsam_unstable) 25 | 26 | ######### 27 | # TESTS # 28 | ######### 29 | catkin_add_gtest(test_absolute_position_factor 30 | test/absolute_position_factor-test.cc 31 | ) 32 | target_link_libraries(test_absolute_position_factor ${PROJECT_NAME}_nodelet) 33 | 34 | catkin_add_gtest(test_moving_baseline_factor 35 | test/moving_baseline_factor-test.cc 36 | ) 37 | target_link_libraries(test_moving_baseline_factor ${PROJECT_NAME}_nodelet) 38 | 39 | ############ 40 | # BINARIES # 41 | ############ 42 | cs_add_executable(mav_state_estimator src/mav_state_estimator_node.cc) 43 | target_link_libraries(mav_state_estimator ${PROJECT_NAME}_nodelet) 44 | 45 | catkin_install_python(PROGRAMS nodes/bag_to_csv scripts/export_csv scripts/filter_imu scripts/plot 46 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 47 | 48 | cs_install() 49 | cs_export() 50 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mav_state_estimation 2 | This package implements a GTSAM based state estimation framework. 3 | 4 | At the moment it supports 5 | - IMU 6 | - GNSS position measurements 7 | - GNSS moving baseline measurements 8 | 9 | Please cite our [accompanying publication](https://arxiv.org/pdf/2106.10108.pdf) when using it. 10 | ``` 11 | Bähnemann, Rik, et al. 12 | "Under the Sand: Navigation and Localization of a Small Unmanned Aerial Vehicle for Landmine Detection with Ground Penetrating Synthetic Aperture Radar" 13 | Field Robotics, Vol. 2, (2022), 1028–1067. 14 | ``` 15 | ![](https://user-images.githubusercontent.com/11293852/104023073-6af65a80-51c1-11eb-8a77-a9cfd53860fd.png) 16 | 17 | ## Installation on Ubuntu 18.04 and ROS melodic 18 | Install [ROS melodic](http://wiki.ros.org/melodic/Installation/Ubuntu). 19 | 20 | Add [GTSAM](https://gtsam.org/get_started/) PPA repository: 21 | ``` 22 | sudo add-apt-repository ppa:borglab/gtsam-release-4.0 23 | ``` 24 | 25 | Create a merged catkin workspace. 26 | ``` 27 | sudo apt install -y python-catkin-tools 28 | cd ~ 29 | mkdir -p catkin_ws/src 30 | cd catkin_ws 31 | catkin init 32 | catkin config --extend /opt/ros/melodic 33 | catkin config --merge-devel 34 | ``` 35 | 36 | Install [piksi_multi_cpp](https://github.com/ethz-asl/ethz_piksi_ros/tree/master/piksi_multi_cpp) within the same workspace. 37 | 38 | Download this package. 39 | ``` 40 | cd ~/catkin_ws/src 41 | git clone git@github.com:ethz-asl/mav_gtsam_estimator.git 42 | ``` 43 | 44 | Install all [PPA dependencies](install/prepare-jenkins-slave.sh). 45 | ``` 46 | ./mav_state_estimation/install/prepare-jenkins-slave.sh 47 | ``` 48 | 49 | Next download all individual ROS package dependencies. 50 | **Note**: If you have not setup [SSH keys in GitHub](https://help.github.com/en/enterprise/2.16/user/articles/generating-a-new-ssh-key-and-adding-it-to-the-ssh-agent) use [dependencies_https.rosinstall](install/dependencies_https.rosinstall). 51 | ``` 52 | wstool init 53 | wstool merge mav_state_estimation/install/dependencies.rosinstall 54 | wstool update 55 | ``` 56 | 57 | Finally, build the workspace. 58 | ``` 59 | catkin build 60 | ``` 61 | -------------------------------------------------------------------------------- /cfg/estimator.yaml: -------------------------------------------------------------------------------- 1 | prior_noise_rot_IB: [0.03, 0.03, 0.03] 2 | prior_noise_I_t_B: [0.1, 0.1, 0.1] 3 | prior_noise_I_v_B: [0.01, 0.01, 0.01] 4 | # Gyro bias up to sensitivity 0.01 deg/s, because it is calibrated at startup. 5 | prior_noise_gyro_bias: [1.7e-4, 1.7e-4, 1.7e-4] 6 | # Acc bias repeatability from data sheet 7 | prior_noise_acc_bias: [2.0e-1, 2.0e-1, 2.0e-1] 8 | #prior_acc_bias: [0.11, 0.06, 0.00] 9 | prior_acc_bias: [0.0, 0.0, 0.0] 10 | prior_gyro_bias: [0.0, 0.0, 0.0] 11 | 12 | # IMU params from J. Rehder: Extending kalibr: Calibrating the Extrinsics of Multiple IMUs and of Individual Axes, Table II 13 | # https://github.com/ethz-asl/kalibr/issues/63#issuecomment-236140184 14 | # Two magnitudes greater to account for modelling errors 15 | acc_sigma: 1.86e-1 16 | gyro_sigma: 1.87e-2 17 | # One magnitude smaller to fix 18 | bias_acc_sigma: 4.33e-4 19 | bias_omega_sigma: 2.66e-5 20 | 21 | bias_acc_int_sigma: 1.0e-5 22 | bias_omega_int_sigma: 1.0e-5 23 | integration_sigma: 1.0e-7 24 | use_2nd_order_coriolis: true 25 | 26 | estimate_antenna_positions: true 27 | 28 | position_receiver: 29 | # (Initial) extrinsic calibration 30 | B_t: [0.110, 0.196, 0.336] 31 | prior_noise_B_t: 0.01 32 | process_noise_B_t: 1.0e-5 33 | scale_cov: 1.0 34 | rate: 10 35 | 36 | attitude_receiver: 37 | # (Initial) extrinsic calibration IMU frame 38 | B_t: [0.110, -0.204, 0.336] 39 | prior_noise_B_t: 0.01 40 | process_noise_B_t: 1.0e-5 41 | scale_cov: 2.0 42 | rate: 5 43 | 44 | isam2: 45 | relinearize_threshold_rot: 0.02 # rad 46 | relinearize_threshold_pos: 0.01 # m 47 | relinearize_threshold_vel: 0.01 # m/s 48 | relinearize_threshold_acc_bias: 0.01 # m/s**2 49 | relinearize_threshold_gyro_bias: 0.02 # rad/s 50 | relinearize_threshold_antenna_calibration: 0.01 # m 51 | relinearize_skip: 1 52 | enable_partial_relinarization_check: False 53 | smoother_lag: 3.0 # s WARNING: This time needs to be greater than any imaginable sensor arrival delay 54 | 55 | # External poses to evaluate during batch process. 56 | external_poses: ["a1", "a2", "a3", "a4", "bfly", "velodyne", "FLU"] 57 | 58 | # Publish odometry, prediction and TF every nth IMU message. 59 | odometry_throttle: 10 60 | -------------------------------------------------------------------------------- /cfg/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1 10 | - /TF1/Frames1 11 | - /Marker1 12 | - /Marker2 13 | - /PoseWithCovariance1 14 | - /PoseWithCovariance2 15 | - /Odometry1 16 | - /Odometry1/Shape1 17 | - /Odometry1/Covariance1/Position1 18 | - /PointStamped1 19 | - /PointStamped2 20 | - /PointStamped3 21 | Splitter Ratio: 0.5 22 | Tree Height: 363 23 | - Class: rviz/Selection 24 | Name: Selection 25 | - Class: rviz/Tool Properties 26 | Expanded: 27 | - /2D Pose Estimate1 28 | - /2D Nav Goal1 29 | - /Publish Point1 30 | Name: Tool Properties 31 | Splitter Ratio: 0.5886790156364441 32 | - Class: rviz/Views 33 | Expanded: 34 | - /Current View1 35 | Name: Views 36 | Splitter Ratio: 0.5 37 | - Class: rviz/Time 38 | Experimental: false 39 | Name: Time 40 | SyncMode: 0 41 | SyncSource: "" 42 | Preferences: 43 | PromptSaveOnExit: true 44 | Toolbars: 45 | toolButtonStyle: 2 46 | Visualization Manager: 47 | Class: "" 48 | Displays: 49 | - Alpha: 0.5 50 | Cell Size: 1 51 | Class: rviz/Grid 52 | Color: 160; 160; 164 53 | Enabled: true 54 | Line Style: 55 | Line Width: 0.029999999329447746 56 | Value: Lines 57 | Name: Grid 58 | Normal Cell Count: 0 59 | Offset: 60 | X: 0 61 | Y: 0 62 | Z: 0 63 | Plane: XY 64 | Plane Cell Count: 10 65 | Reference Frame: 66 | Value: true 67 | - Class: rviz/TF 68 | Enabled: true 69 | Frame Timeout: 100 70 | Frames: 71 | All Enabled: false 72 | FLU: 73 | Value: false 74 | a1: 75 | Value: true 76 | a1_intermediate: 77 | Value: false 78 | a1_tilt: 79 | Value: false 80 | a2: 81 | Value: true 82 | a2_intermediate: 83 | Value: false 84 | a2_tilt: 85 | Value: false 86 | a3: 87 | Value: true 88 | a3_intermediate: 89 | Value: false 90 | a3_tilt: 91 | Value: false 92 | a4: 93 | Value: true 94 | a4_intermediate: 95 | Value: false 96 | a4_tilt: 97 | Value: false 98 | adis16448bmlz: 99 | Value: true 100 | adis16448bmlz_optimization: 101 | Value: false 102 | adis16448bmlz_vicon: 103 | Value: true 104 | att: 105 | Value: true 106 | bfly: 107 | Value: true 108 | enu: 109 | Value: true 110 | leica: 111 | Value: true 112 | moa: 113 | Value: true 114 | pos: 115 | Value: true 116 | vicon: 117 | Value: true 118 | Marker Scale: 0.699999988079071 119 | Name: TF 120 | Show Arrows: true 121 | Show Axes: true 122 | Show Names: true 123 | Tree: 124 | vicon: 125 | adis16448bmlz: 126 | FLU: 127 | {} 128 | a1_intermediate: 129 | a1_tilt: 130 | a1: 131 | {} 132 | a2_intermediate: 133 | a2_tilt: 134 | a2: 135 | {} 136 | a3_intermediate: 137 | a3_tilt: 138 | a3: 139 | {} 140 | a4_intermediate: 141 | a4_tilt: 142 | a4: 143 | {} 144 | bfly: 145 | {} 146 | leica: 147 | {} 148 | adis16448bmlz_optimization: 149 | {} 150 | enu: 151 | {} 152 | moa: 153 | adis16448bmlz_vicon: 154 | {} 155 | att: 156 | {} 157 | pos: 158 | {} 159 | Update Interval: 0 160 | Value: true 161 | - Class: rviz/Marker 162 | Enabled: true 163 | Marker Topic: /moa/piksi_ref/baseline_ned_cov_viz 164 | Name: Marker 165 | Namespaces: 166 | {} 167 | Queue Size: 100 168 | Value: true 169 | - Class: rviz/Marker 170 | Enabled: true 171 | Marker Topic: /moa/piksi_att/baseline_ned_cov_viz 172 | Name: Marker 173 | Namespaces: 174 | {} 175 | Queue Size: 100 176 | Value: true 177 | - Alpha: 1 178 | Axes Length: 1 179 | Axes Radius: 0.10000000149011612 180 | Class: rviz/PoseWithCovariance 181 | Color: 255; 25; 0 182 | Covariance: 183 | Orientation: 184 | Alpha: 0.5 185 | Color: 255; 255; 127 186 | Color Style: Unique 187 | Frame: Local 188 | Offset: 1 189 | Scale: 1 190 | Value: true 191 | Position: 192 | Alpha: 0.30000001192092896 193 | Color: 204; 51; 204 194 | Scale: 1 195 | Value: true 196 | Value: true 197 | Enabled: false 198 | Head Length: 0.10000000149011612 199 | Head Radius: 0.05000000074505806 200 | Name: PoseWithCovariance 201 | Shaft Length: 0.20000000298023224 202 | Shaft Radius: 0.004999999888241291 203 | Shape: Arrow 204 | Topic: /moa/piksi/position_receiver_0/ros/pose_enu_cov 205 | Unreliable: false 206 | Value: false 207 | - Alpha: 1 208 | Axes Length: 1 209 | Axes Radius: 0.10000000149011612 210 | Class: rviz/PoseWithCovariance 211 | Color: 255; 25; 0 212 | Covariance: 213 | Orientation: 214 | Alpha: 0.5 215 | Color: 255; 255; 127 216 | Color Style: Unique 217 | Frame: Local 218 | Offset: 1 219 | Scale: 1 220 | Value: true 221 | Position: 222 | Alpha: 0.30000001192092896 223 | Color: 204; 51; 204 224 | Scale: 1 225 | Value: true 226 | Value: true 227 | Enabled: false 228 | Head Length: 0.10000000149011612 229 | Head Radius: 0.05000000074505806 230 | Name: PoseWithCovariance 231 | Shaft Length: 0.20000000298023224 232 | Shaft Radius: 0.004999999888241291 233 | Shape: Arrow 234 | Topic: /moa/piksi/attitude_receiver_0/ros/pose_enu_cov 235 | Unreliable: false 236 | Value: false 237 | - Angle Tolerance: 0.10000000149011612 238 | Class: rviz/Odometry 239 | Covariance: 240 | Orientation: 241 | Alpha: 0.5 242 | Color: 255; 255; 127 243 | Color Style: Unique 244 | Frame: Local 245 | Offset: 1 246 | Scale: 1 247 | Value: true 248 | Position: 249 | Alpha: 0.30000001192092896 250 | Color: 204; 51; 204 251 | Scale: 1 252 | Value: true 253 | Value: true 254 | Enabled: false 255 | Keep: 100 256 | Name: Odometry 257 | Position Tolerance: 0.10000000149011612 258 | Shape: 259 | Alpha: 1 260 | Axes Length: 1 261 | Axes Radius: 0.10000000149011612 262 | Color: 255; 25; 0 263 | Head Length: 0.05999999865889549 264 | Head Radius: 0.019999999552965164 265 | Shaft Length: 0.20000000298023224 266 | Shaft Radius: 0.009999999776482582 267 | Value: Arrow 268 | Topic: /moa/mav_state_estimator/odometry 269 | Unreliable: false 270 | Value: false 271 | - Alpha: 1 272 | Class: rviz/PointStamped 273 | Color: 204; 41; 204 274 | Enabled: true 275 | History Length: 1 276 | Name: PointStamped 277 | Radius: 0.05000000074505806 278 | Topic: /moa/leica/position 279 | Unreliable: false 280 | Value: true 281 | - Alpha: 1 282 | Class: rviz/PointStamped 283 | Color: 204; 41; 204 284 | Enabled: true 285 | History Length: 1 286 | Name: PointStamped 287 | Radius: 0.20000000298023224 288 | Topic: /moa/piksi/position_receiver_0/ros/pos_enu 289 | Unreliable: false 290 | Value: true 291 | - Alpha: 1 292 | Class: rviz/PointStamped 293 | Color: 204; 41; 204 294 | Enabled: true 295 | History Length: 1 296 | Name: PointStamped 297 | Radius: 0.20000000298023224 298 | Topic: /moa/piksi/attitude_receiver_0/ros/pos_enu 299 | Unreliable: false 300 | Value: true 301 | Enabled: true 302 | Global Options: 303 | Background Color: 211; 215; 207 304 | Default Light: true 305 | Fixed Frame: enu 306 | Frame Rate: 30 307 | Name: root 308 | Tools: 309 | - Class: rviz/Interact 310 | Hide Inactive Objects: true 311 | - Class: rviz/MoveCamera 312 | - Class: rviz/Select 313 | - Class: rviz/FocusCamera 314 | - Class: rviz/Measure 315 | - Class: rviz/SetInitialPose 316 | Theta std deviation: 0.2617993950843811 317 | Topic: /initialpose 318 | X std deviation: 0.5 319 | Y std deviation: 0.5 320 | - Class: rviz/SetGoal 321 | Topic: /move_base_simple/goal 322 | - Class: rviz/PublishPoint 323 | Single click: true 324 | Topic: /clicked_point 325 | - Class: rviz_polygon_tool/PolygonSelection 326 | Value: true 327 | Views: 328 | Current: 329 | Class: rviz/Orbit 330 | Distance: 2.901003360748291 331 | Enable Stereo Rendering: 332 | Stereo Eye Separation: 0.05999999865889549 333 | Stereo Focal Distance: 1 334 | Swap Stereo Eyes: false 335 | Value: false 336 | Focal Point: 337 | X: 0.4026688039302826 338 | Y: -0.20153716206550598 339 | Z: 0.08136642724275589 340 | Focal Shape Fixed Size: false 341 | Focal Shape Size: 0.05000000074505806 342 | Invert Z Axis: false 343 | Name: Current View 344 | Near Clip Distance: 0.009999999776482582 345 | Pitch: 0.9397969245910645 346 | Target Frame: vicon 347 | Value: Orbit (rviz) 348 | Yaw: 1.2203937768936157 349 | Saved: ~ 350 | Window Geometry: 351 | Displays: 352 | collapsed: true 353 | Height: 1030 354 | Hide Left Dock: true 355 | Hide Right Dock: false 356 | QMainWindow State: 000000ff00000000fd00000004000000000000023a00000367fc0200000008fb000000100044006900730070006c006100790073000000003d00000367000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000367fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000367000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003be0000003ffc0100000002fb0000000800540069006d00650100000000000003be0000025600fffffffb0000000800540069006d00650100000000000004500000000000000000000002a90000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 357 | Selection: 358 | collapsed: false 359 | Time: 360 | collapsed: false 361 | Tool Properties: 362 | collapsed: false 363 | Views: 364 | collapsed: false 365 | Width: 958 366 | X: 0 367 | Y: 25 368 | -------------------------------------------------------------------------------- /include/mav_state_estimation/absolute_position_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #ifndef MAV_STATE_ESTIMATION_ABSOLUTE_POSITION_FACTOR_H_ 22 | #define MAV_STATE_ESTIMATION_ABSOLUTE_POSITION_FACTOR_H_ 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | namespace mav_state_estimation { 37 | 38 | // Absolute position factor that determines the error and Jacobian w.r.t. a 39 | // measured position in mission coordinates. 40 | class AbsolutePositionFactor1 : public gtsam::NoiseModelFactor1 { 41 | public: 42 | // Factor that determines the error and jacobian w.r.t. a measured position 43 | // in inertial coordinates. 44 | AbsolutePositionFactor1( 45 | gtsam::Key T_I_B_key, const gtsam::Point3& I_t_P_measured, 46 | const gtsam::Point3& B_t_P, 47 | const gtsam::noiseModel::Base::shared_ptr& noise_model); 48 | 49 | // Evaluates the error term and corresponding jacobians w.r.t. the pose. 50 | gtsam::Vector evaluateError( 51 | const gtsam::Pose3& T_I_B, 52 | boost::optional D_Tt_T = boost::none) const override; 53 | 54 | // Returns a deep copy of the factor. 55 | inline gtsam::NonlinearFactor::shared_ptr clone() const override { 56 | return gtsam::NonlinearFactor::shared_ptr(new This(*this)); 57 | } 58 | 59 | // Prints out information about the factor. 60 | void print(const std::string& s, 61 | const gtsam::KeyFormatter& key_formatter = 62 | gtsam::DefaultKeyFormatter) const override; 63 | 64 | // Equality operator. 65 | bool equals(const gtsam::NonlinearFactor& expected, 66 | double tolerance) const override; 67 | 68 | // Returns the measured absolute position. 69 | inline const gtsam::Point3& measured() const { return I_t_P_measured_; } 70 | 71 | // Factory method. 72 | inline static shared_ptr Create( 73 | gtsam::Key T_I_B_key, const gtsam::Point3& I_t_P_measured, 74 | const gtsam::Point3& B_t_P, 75 | const gtsam::noiseModel::Base::shared_ptr& noise_model) { 76 | return shared_ptr(new AbsolutePositionFactor1(T_I_B_key, I_t_P_measured, 77 | B_t_P, noise_model)); 78 | } 79 | 80 | protected: 81 | // Absolute position measurement in local mission coordinates. 82 | const gtsam::Point3 I_t_P_measured_; 83 | // Extrinsic calibration from base frame to position sensor. 84 | const gtsam::Point3 B_t_P_; 85 | 86 | private: 87 | typedef gtsam::NoiseModelFactor1 Base; 88 | typedef AbsolutePositionFactor1 This; 89 | }; 90 | 91 | // Absolute position factor that determines the error and Jacobian w.r.t. a 92 | // measured position in mission coordinates. Additionally, estimates antenna 93 | // position. 94 | class AbsolutePositionFactor2 95 | : public gtsam::NoiseModelFactor2 { 96 | public: 97 | // Factor that determines the error and jacobian w.r.t. a measured position 98 | // in inertial coordinates. 99 | AbsolutePositionFactor2( 100 | gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, 101 | const gtsam::Point3& I_t_P_measured, 102 | const gtsam::noiseModel::Base::shared_ptr& noise_model); 103 | 104 | // Evaluates the error term and corresponding jacobians w.r.t. the pose. 105 | gtsam::Vector evaluateError( 106 | const gtsam::Pose3& T_I_B, const gtsam::Point3& B_t_P, 107 | boost::optional D_Tt_T = boost::none, 108 | boost::optional D_Tt_t = boost::none) const override; 109 | 110 | // Returns a deep copy of the factor. 111 | inline gtsam::NonlinearFactor::shared_ptr clone() const override { 112 | return gtsam::NonlinearFactor::shared_ptr(new This(*this)); 113 | } 114 | 115 | // Prints out information about the factor. 116 | void print(const std::string& s, 117 | const gtsam::KeyFormatter& key_formatter = 118 | gtsam::DefaultKeyFormatter) const override; 119 | 120 | // Equality operator. 121 | bool equals(const gtsam::NonlinearFactor& expected, 122 | double tolerance) const override; 123 | 124 | // Returns the measured absolute position. 125 | inline const gtsam::Point3& measured() const { return I_t_P_measured_; } 126 | 127 | // Factory method. 128 | inline static shared_ptr Create( 129 | gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, 130 | const gtsam::Point3& I_t_P_measured, 131 | const gtsam::noiseModel::Base::shared_ptr& noise_model) { 132 | return shared_ptr(new AbsolutePositionFactor2(T_I_B_key, B_t_P_key, 133 | I_t_P_measured, noise_model)); 134 | } 135 | 136 | protected: 137 | // Absolute position measurement in local mission coordinates. 138 | const gtsam::Point3 I_t_P_measured_; 139 | 140 | private: 141 | typedef gtsam::NoiseModelFactor2 Base; 142 | typedef AbsolutePositionFactor2 This; 143 | }; 144 | 145 | } // namespace mav_state_estimation 146 | 147 | #endif // MAV_STATE_ESTIMATION_ABSOLUTE_POSITION_FACTOR_H_ 148 | -------------------------------------------------------------------------------- /include/mav_state_estimation/initialization.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #ifndef MAV_STATE_ESTIMATION_INITIALIZATION_H_ 22 | #define MAV_STATE_ESTIMATION_INITIALIZATION_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | namespace mav_state_estimation { 29 | 30 | // Orientation (and position) initialization using the TRIAD method. 31 | class Initialization { 32 | public: 33 | // Add a vector measurement both in inertial and base frame. 34 | void addOrientationConstraint1(const Eigen::Vector3d& r_I, 35 | const Eigen::Vector3d& r_B, 36 | const ros::Time& t); 37 | // Add a second vector measurement both in inertial and base frame. 38 | void addOrientationConstraint2(const Eigen::Vector3d& r_I, 39 | const Eigen::Vector3d& r_B, 40 | const ros::Time& t); 41 | // Add a position constraint and its corresponding transformation from base 42 | // frame to position sensor. 43 | void addPositionConstraint(const Eigen::Vector3d& I_r_P, 44 | const Eigen::Vector3d& B_t_P, const ros::Time& t); 45 | 46 | inline void setInertialFrame(const std::string id) { 47 | inertial_frame_ = std::make_optional(id); 48 | } 49 | inline void setBaseFrame(const std::string id) { 50 | base_frame_ = std::make_optional(id); 51 | } 52 | 53 | inline bool isInitialized() const { return T_IB_.has_value(); }; 54 | bool getInitialPose(geometry_msgs::TransformStamped* T_IB) const; 55 | 56 | private: 57 | void computeInitialState(); 58 | 59 | void calculateTriadOrientation(const Eigen::Vector3d& R1, 60 | const Eigen::Vector3d& r1, 61 | const Eigen::Vector3d& R2, 62 | const Eigen::Vector3d& r2, 63 | Eigen::Matrix3d* R_IB) const; 64 | 65 | typedef std::pair VectorPair; 66 | std::optional r_1; 67 | std::optional r_2; 68 | std::optional p; 69 | ros::Time stamp_; 70 | std::optional inertial_frame_; 71 | std::optional base_frame_; 72 | std::optional T_IB_; 73 | }; 74 | 75 | } // namespace mav_state_estimation 76 | #endif // MAV_STATE_ESTIMATION_INITIALIZATION_H_ 77 | -------------------------------------------------------------------------------- /include/mav_state_estimation/mav_state_estimator.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #ifndef MAV_STATE_ESTIMATOR_MAV_STATE_ESTIMATOR_H_ 22 | #define MAV_STATE_ESTIMATOR_MAV_STATE_ESTIMATOR_H_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include "mav_state_estimation/Batch.h" 48 | #include "mav_state_estimation/Timing.h" 49 | #include "mav_state_estimation/initialization.h" 50 | 51 | namespace mav_state_estimation { 52 | 53 | class MavStateEstimator { 54 | public: 55 | MavStateEstimator(const ros::NodeHandle& nh, 56 | const ros::NodeHandle& nh_private); 57 | ~MavStateEstimator(); 58 | 59 | private: 60 | Eigen::Vector3d getVectorFromParams(const std::string& param) const; 61 | 62 | void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg); 63 | void posCallback( 64 | const piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr& pos_msg); 65 | void baselineCallback( 66 | const piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr& 67 | baseline_msg); 68 | 69 | geometry_msgs::TransformStamped getTransform( 70 | const gtsam::NavState& state, const ros::Time& stamp, 71 | const std::string& child_frame_id) const; 72 | void broadcastTf(const gtsam::NavState& state, const ros::Time& stamp, 73 | const std::string& child_frame_id); 74 | void broadcastTf(const gtsam::NavState& state, const ros::Time& stamp, 75 | const std::string& child_frame_id, 76 | geometry_msgs::TransformStamped* T_IB); 77 | void publishOdometry(const Eigen::Vector3d& v_I, 78 | const Eigen::Vector3d& omega_B, 79 | const gtsam::imuBias::ConstantBias& bias, 80 | const ros::Time& stamp, 81 | const geometry_msgs::TransformStamped& T_IB) const; 82 | void createBiasMessage(const gtsam::imuBias::ConstantBias& bias, 83 | const ros::Time& stamp, 84 | geometry_msgs::Vector3Stamped* acc_bias, 85 | geometry_msgs::Vector3Stamped* gyro_bias) const; 86 | void publishBias(const gtsam::imuBias::ConstantBias& bias, 87 | const ros::Time& stamp, const ros::Publisher& acc_bias_pub, 88 | const ros::Publisher& gyro_bias_pub) const; 89 | void publishBias(const gtsam::imuBias::ConstantBias& bias, 90 | const ros::Time& stamp, const ros::Publisher& acc_bias_pub, 91 | const ros::Publisher& gyro_bias_pub, 92 | geometry_msgs::Vector3Stamped* acc_bias, 93 | geometry_msgs::Vector3Stamped* gyro_bias) const; 94 | void publishAntennaPosition(const gtsam::Point3& B_t, const ros::Time& stamp, 95 | const ros::Publisher& pub) const; 96 | 97 | void loadGnssParams(const std::string& antenna_ns, 98 | const gtsam::Symbol& symbol, gtsam::Point3* B_t, 99 | gtsam::noiseModel::Isotropic::shared_ptr* process_noise, 100 | double* cov_scale); 101 | 102 | void updateTimestampMap(); 103 | 104 | void addSensorTimes(const uint16_t rate); 105 | bool addUnaryStamp(const ros::Time& stamp); 106 | gtsam::imuBias::ConstantBias getCurrentBias(); 107 | gtsam::NavState getCurrentState(); 108 | 109 | ros::NodeHandle nh_; 110 | ros::NodeHandle nh_private_; 111 | 112 | ros::Subscriber imu_sub_; 113 | ros::Subscriber pos_0_sub_; 114 | ros::Subscriber baseline_sub_; 115 | 116 | ros::Publisher timing_pub_; 117 | mav_state_estimation::Timing timing_msg_; 118 | int odometry_throttle_ = 1; 119 | 120 | ros::Publisher prediction_pub_; 121 | ros::Publisher optimization_pub_; 122 | ros::Publisher acc_bias_pub_; 123 | ros::Publisher gyro_bias_pub_; 124 | ros::Publisher position_antenna_pub_; 125 | ros::Publisher attitude_antenna_pub_; 126 | ros::Publisher odometry_pub_; 127 | 128 | ros::ServiceServer batch_srv_; 129 | 130 | tf2_ros::TransformBroadcaster tfb_; 131 | tf2_ros::Buffer tf_buffer_; 132 | tf2_ros::TransformListener tfl_ = tf2_ros::TransformListener(tf_buffer_); 133 | std::vector external_poses_; 134 | 135 | // Initial parameters. 136 | Initialization init_; 137 | gtsam::Point3 B_t_P_ = Eigen::Vector3d::Zero(); // Position receiver. 138 | gtsam::Point3 B_t_A_ = Eigen::Vector3d::Zero(); // Attitude receiver. 139 | gtsam::noiseModel::Isotropic::shared_ptr process_noise_model_B_t_P_; 140 | gtsam::noiseModel::Isotropic::shared_ptr process_noise_model_B_t_A_; 141 | std::string inertial_frame_; 142 | std::string base_frame_; 143 | gtsam::noiseModel::Diagonal::shared_ptr prior_noise_model_T_I_B_; 144 | gtsam::noiseModel::Diagonal::shared_ptr prior_noise_model_I_v_B_; 145 | gtsam::noiseModel::Diagonal::shared_ptr prior_noise_model_imu_bias_; 146 | bool estimate_antenna_positions_ = false; 147 | 148 | double pos_receiver_cov_scale_ = 1.0; 149 | double att_receiver_cov_scale_ = 1.0; 150 | 151 | void initializeState(); 152 | inline bool isInitialized() const { return !stamp_to_idx_.empty(); } 153 | std::set unary_times_ns_; 154 | std::map stamp_to_idx_; 155 | std::map idx_to_stamp_; 156 | 157 | // IMU interation 158 | gtsam::PreintegratedCombinedMeasurements integrator_; 159 | sensor_msgs::Imu::ConstPtr prev_imu_; 160 | 161 | // Optimization 162 | gtsam::IncrementalFixedLagSmoother smoother_; 163 | gtsam::NonlinearFactorGraph new_graph_; 164 | gtsam::FixedLagSmoother::KeyTimestampMap new_timestamps_; 165 | gtsam::Values new_values_; 166 | uint64_t idx_ = 0; 167 | std::vector> 168 | new_unary_factors_; 169 | gtsam::NavState prev_state_; 170 | gtsam::imuBias::ConstantBias prev_bias_; 171 | 172 | // Batch 173 | std::mutex batch_mtx_; 174 | gtsam::NonlinearFactorGraph batch_graph_; 175 | gtsam::Values batch_values_; 176 | std::vector batch_imu_; 177 | void solveBatch( 178 | std::unique_ptr graph, 179 | std::unique_ptr values, 180 | std::unique_ptr> imus, 181 | std::unique_ptr integrator, 182 | std::unique_ptr> idx_to_stamp, 183 | std::unique_ptr bag_file); 184 | bool computeBatchSolution(Batch::Request& request, Batch::Response& response); 185 | std::thread batch_thread_; 186 | std::atomic_bool batch_running_ = false; 187 | 188 | // Extra thread to solve factor graph. 189 | std::thread solver_thread_; 190 | std::atomic_bool is_solving_ = false; 191 | std::recursive_mutex update_mtx_; 192 | 193 | void solve(); 194 | 195 | void solveThreaded( 196 | std::unique_ptr graph, 197 | std::unique_ptr values, 198 | std::unique_ptr stamps, 199 | std::unique_ptr time, const uint64_t i); 200 | }; 201 | 202 | } // namespace mav_state_estimation 203 | 204 | #endif // MAV_STATE_ESTIMATOR_MAV_STATE_ESTIMATOR_H_ 205 | -------------------------------------------------------------------------------- /include/mav_state_estimation/moving_baseline_factor.h: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #ifndef MAV_STATE_ESTIMATION_MOVING_BASELINE_FACTOR_H_ 22 | #define MAV_STATE_ESTIMATION_MOVING_BASELINE_FACTOR_H_ 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | 38 | namespace mav_state_estimation { 39 | 40 | // Moving baseline factor that determines the error and Jacobian w.r.t. to the 41 | // vector between two GNSS antennas in inertial frame. 42 | class MovingBaselineFactor1 : public gtsam::NoiseModelFactor1 { 43 | public: 44 | MovingBaselineFactor1(gtsam::Key T_I_B_key, 45 | const Eigen::Vector3d& I_t_PA_measured, 46 | const Eigen::Vector3d& B_t_PA, 47 | const gtsam::noiseModel::Base::shared_ptr& noise_model); 48 | 49 | // Evaluates the error term and corresponding jacobians w.r.t. the pose. 50 | gtsam::Vector evaluateError( 51 | const gtsam::Pose3& T_I_B, 52 | boost::optional D_Tt_T = boost::none) const override; 53 | 54 | // Returns a deep copy of the factor. 55 | inline gtsam::NonlinearFactor::shared_ptr clone() const override { 56 | return gtsam::NonlinearFactor::shared_ptr(new This(*this)); 57 | } 58 | 59 | // Prints out information about the factor. 60 | void print(const std::string& s, 61 | const gtsam::KeyFormatter& key_formatter = 62 | gtsam::DefaultKeyFormatter) const override; 63 | 64 | // Equality operator. 65 | bool equals(const gtsam::NonlinearFactor& expected, 66 | double tolerance) const override; 67 | 68 | // Returns the measured moving baseline. 69 | inline const Eigen::Vector3d& measured() const { return I_t_PA_measured_; } 70 | 71 | // Factory method. 72 | inline static shared_ptr Create( 73 | gtsam::Key T_I_B_key, const Eigen::Vector3d& I_t_PA_measured, 74 | const Eigen::Vector3d& B_t_PA, 75 | const gtsam::noiseModel::Base::shared_ptr& noise_model) { 76 | return shared_ptr(new MovingBaselineFactor1(T_I_B_key, I_t_PA_measured, 77 | B_t_PA, noise_model)); 78 | } 79 | 80 | protected: 81 | // Moving baseline measurement in inertial frame coordinates. 82 | const Eigen::Vector3d I_t_PA_measured_; 83 | // Extrinsic calibration from reference receiver antenna to attitude receiver 84 | // antenna in base frame. 85 | const Eigen::Vector3d B_t_PA_; 86 | 87 | private: 88 | typedef gtsam::NoiseModelFactor1 Base; 89 | typedef MovingBaselineFactor1 This; 90 | }; 91 | 92 | // Moving baseline factor that determines the error and Jacobian w.r.t. to the 93 | // vector between two GNSS antennas in inertial frame. Additionally estimates 94 | // position antenna calibrations. 95 | class MovingBaselineFactor2 96 | : public gtsam::NoiseModelFactor3 { 98 | public: 99 | MovingBaselineFactor2(gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, 100 | gtsam::Key B_t_A_key, 101 | const Eigen::Vector3d& I_t_PA_measured, 102 | const gtsam::noiseModel::Base::shared_ptr& noise_model); 103 | 104 | // Evaluates the error term and corresponding jacobians w.r.t. the pose. 105 | gtsam::Vector evaluateError( 106 | const gtsam::Pose3& T_I_B, const gtsam::Point3& B_t_P, 107 | const gtsam::Point3& B_t_A, 108 | boost::optional D_Tt_T = boost::none, 109 | boost::optional D_Tt_tP = boost::none, 110 | boost::optional D_Tt_tA = boost::none) const override; 111 | 112 | // Returns a deep copy of the factor. 113 | inline gtsam::NonlinearFactor::shared_ptr clone() const override { 114 | return gtsam::NonlinearFactor::shared_ptr(new This(*this)); 115 | } 116 | 117 | // Prints out information about the factor. 118 | void print(const std::string& s, 119 | const gtsam::KeyFormatter& key_formatter = 120 | gtsam::DefaultKeyFormatter) const override; 121 | 122 | // Equality operator. 123 | bool equals(const gtsam::NonlinearFactor& expected, 124 | double tolerance) const override; 125 | 126 | // Returns the measured moving baseline. 127 | inline const Eigen::Vector3d& measured() const { return I_t_PA_measured_; } 128 | 129 | // Factory method. 130 | inline static shared_ptr Create( 131 | gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, gtsam::Key B_t_A_key, 132 | const Eigen::Vector3d& I_t_PA_measured, 133 | const gtsam::noiseModel::Base::shared_ptr& noise_model) { 134 | return shared_ptr(new MovingBaselineFactor2(T_I_B_key, B_t_P_key, B_t_A_key, 135 | I_t_PA_measured, noise_model)); 136 | } 137 | 138 | protected: 139 | // Moving baseline measurement in inertial frame coordinates. 140 | const Eigen::Vector3d I_t_PA_measured_; 141 | 142 | private: 143 | typedef gtsam::NoiseModelFactor3 144 | Base; 145 | typedef MovingBaselineFactor2 This; 146 | }; 147 | 148 | } // namespace mav_state_estimation 149 | 150 | #endif // MAV_STATE_ESTIMATION_MOVING_BASELINE_FACTOR_H_ 151 | -------------------------------------------------------------------------------- /install/dependencies.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: catkin_simple, uri: 'git@github.com:catkin/catkin_simple.git'} 2 | - git: {local-name: ethz_piksi_ros, uri: 'git@github.com:ethz-asl/ethz_piksi_ros.git'} 3 | - git: {local-name: mav_state_estimation, uri: 'git@github.com:rikba/mav_state_estimation.git'} 4 | -------------------------------------------------------------------------------- /install/dependencies_https.rosinstall: -------------------------------------------------------------------------------- 1 | - git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple.git'} 2 | - git: {local-name: ethz_piksi_ros, uri: 'https://github.com/ethz-asl/ethz_piksi_ros.git'} 3 | - git: {local-name: mav_state_estimation, uri: 'https://github.com/rikba/mav_state_estimation.git'} 4 | -------------------------------------------------------------------------------- /install/prepare-jenkins-slave.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | echo "Running the prepare script for mav_state_estimation."; 3 | source /opt/ros/melodic/setup.bash 4 | ROS_VERSION=`rosversion -d` 5 | echo "ROS version: ${ROS_VERSION}" 6 | 7 | # Build dependencies. 8 | sudo apt install -y python-wstool python-catkin-tools 9 | 10 | # Eigen3 11 | sudo apt install -y libeigen3-dev 12 | 13 | # GTSAM. 14 | sudo apt install -y libgtsam-dev libgtsam-unstable-dev 15 | 16 | # Package dependencies. 17 | sudo apt install -y ros-${ROS_VERSION}-eigen-conversions 18 | sudo apt install -y ros-${ROS_VERSION}-geometry-msgs 19 | sudo apt install -y ros-${ROS_VERSION}-nav-msgs 20 | sudo apt install -y ros-${ROS_VERSION}-nodelet 21 | sudo apt install -y ros-${ROS_VERSION}-rosbag 22 | sudo apt install -y ros-${ROS_VERSION}-roscpp 23 | sudo apt install -y ros-${ROS_VERSION}-sensor-msgs 24 | sudo apt install -y ros-${ROS_VERSION}-std-msgs 25 | sudo apt install -y ros-${ROS_VERSION}-tf2-geometry-msgs 26 | sudo apt install -y ros-${ROS_VERSION}-tf2-msgs 27 | sudo apt install -y ros-${ROS_VERSION}-tf2-ros 28 | sudo apt install -y ros-${ROS_VERSION}-message-generation 29 | sudo apt install -y ros-${ROS_VERSION}-message-runtime 30 | -------------------------------------------------------------------------------- /launch/mav_state_estimator.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 | 39 | 40 | 46 | 47 | 53 | 54 | 55 | 56 | 57 | 58 | 64 | 65 | 66 | 67 | 68 | 69 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | -------------------------------------------------------------------------------- /launch/mav_state_estimator_nodelet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /msg/BatchStatus.msg: -------------------------------------------------------------------------------- 1 | uint64 current_idx # [1] 2 | uint64 total_idx # [1] 3 | bool finished 4 | -------------------------------------------------------------------------------- /msg/Timing.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | float64 iteration # [s] 4 | float64 time # [s] 5 | float64 min # [s] 6 | float64 max # [s] 7 | float64 mean # [s] 8 | -------------------------------------------------------------------------------- /nodelet_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is the MAV state estimator nodelet. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /nodes/bag_to_csv: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import rospy 26 | 27 | from mav_state_estimation.csv_export import* 28 | from mav_state_estimation.srv import BagToCsv, BagToCsvResponse 29 | from mav_state_estimation.msg import BatchStatus 30 | 31 | global new_request 32 | global request 33 | 34 | def bag_to_csv(req): 35 | global new_request 36 | rospy.loginfo("CSV export requested.") 37 | if new_request: 38 | rospy.logwarn("Already exporting.") 39 | else: 40 | new_request = True 41 | global request 42 | request = req 43 | return BagToCsvResponse() 44 | 45 | if __name__ == '__main__': 46 | rospy.init_node('bag_to_csv') 47 | new_request = False 48 | rospy.Service('~bag_to_csv', BagToCsv, bag_to_csv) 49 | status_pub = rospy.Publisher('~status', BatchStatus, queue_size=1) 50 | 51 | rate = rospy.Rate(2) 52 | while not rospy.is_shutdown(): 53 | if new_request: 54 | request.topics = request.topics.replace(" ", "") 55 | topics = request.topics.split(",") 56 | rospy.loginfo("Exporting the following poses: %s from bag %s" %(topics, request.bag_file)) 57 | if toCsv(request.bag_file, topics, feedback=status_pub): 58 | rospy.loginfo("Finished export.") 59 | else: 60 | rospy.logerr("Failed export.") 61 | new_request = False 62 | 63 | rate.sleep() 64 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mav_state_estimation 4 | 1.0.0 5 | 6 | A GTSAM based framework for MAV state estimation. 7 | 8 | 9 | Rik Bähnemann 10 | 11 | MIT 12 | 13 | catkin 14 | catkin_simple 15 | 16 | eigen_conversions 17 | geometry_msgs 18 | nav_msgs 19 | nodelet 20 | piksi_rtk_msgs 21 | rosbag 22 | roscpp 23 | sensor_msgs 24 | std_msgs 25 | tf2_geometry_msgs 26 | tf2_msgs 27 | tf2_ros 28 | 29 | message_generation 30 | message_runtime 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /python/mav_state_estimation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ethz-asl/mav_gtsam_estimator/b4bb6b042d6939c9be377e4dec3909d24b12e4c6/python/mav_state_estimation/__init__.py -------------------------------------------------------------------------------- /python/mav_state_estimation/csv_export.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import os 26 | import rosbag 27 | import rospy 28 | from datetime import datetime 29 | 30 | def getStamp(stamp): 31 | dt = datetime.utcfromtimestamp(stamp.secs) 32 | us = stamp.nsecs // 1e3 33 | us_mod = stamp.nsecs % 1e3 34 | return "%d,%d,%d,%d,%d,%d,%d.%d" % (dt.year, dt.month, dt.day, dt.hour, dt.minute, dt.second, us, us_mod) 35 | 36 | def getTranslation(translation): 37 | return "%f,%f,%f" %(translation.x, translation.y, translation.z) 38 | 39 | def getRotation(rotation): 40 | return "%f,%f,%f,%f" %(rotation.w, rotation.x, rotation.y, rotation.z) 41 | 42 | 43 | def toCsv(bag_file, topics, stamp_topic=None, feedback=None): 44 | 45 | if feedback: 46 | from mav_state_estimation.msg import BatchStatus 47 | status = BatchStatus() 48 | status.finished = False 49 | status.current_idx = 0 50 | status.total_idx = 0 51 | 52 | if not os.path.exists(bag_file): 53 | rospy.logerr("File %s does not exist." % (bag_file)) 54 | return False 55 | 56 | if not os.path.isfile(bag_file): 57 | rospy.logerr("File %s is not a file." % (bag_file)) 58 | return False 59 | 60 | 61 | csv_path = os.path.dirname(bag_file) + '/export/' 62 | if not os.path.exists(csv_path): 63 | os.mkdir(csv_path) 64 | 65 | bag = rosbag.Bag(bag_file) 66 | 67 | f = {} 68 | info = bag.get_type_and_topic_info(topics)[1] 69 | for topic in topics: 70 | if topic in info.keys() and info[topic].message_count > 0 and info[topic].msg_type == 'geometry_msgs/TransformStamped': 71 | csv_file = csv_path + topic + '.csv' 72 | rospy.loginfo("Creating new CSV file %s" % (csv_file)) 73 | f[topic] = open(csv_file, 'w') 74 | f[topic].write('year, month, day, hour, min, second, microsecond, x, y, z, qw, qx, qy, qz\n') 75 | 76 | if feedback: 77 | status.total_idx = status.total_idx + info[topic].message_count 78 | 79 | stamps = set() 80 | if stamp_topic: 81 | for topic, msg, t in bag.read_messages(topics=[stamp_topic]): 82 | stamps.add(msg.header.stamp) 83 | 84 | for topic, msg, t in bag.read_messages(topics=f.keys()): 85 | if msg.header.stamp and msg.transform.translation and msg.transform.rotation and f[topic]: 86 | 87 | if feedback: 88 | status.current_idx = status.current_idx + 1 89 | if (10 * status.current_idx) % status.total_idx == 0: 90 | feedback.publish(status) # Every 10 percent 91 | 92 | if stamp_topic and msg.header.stamp not in stamps: 93 | continue 94 | 95 | f[topic].write(getStamp(msg.header.stamp)) 96 | f[topic].write(",") 97 | f[topic].write(getTranslation(msg.transform.translation)) 98 | f[topic].write(",") 99 | f[topic].write(getRotation(msg.transform.rotation)) 100 | f[topic].write("\n") 101 | 102 | for topic in f: 103 | f[topic].close() 104 | 105 | if feedback: 106 | status.finished = True 107 | feedback.publish(status) 108 | return True 109 | -------------------------------------------------------------------------------- /python/mav_state_estimation/filter_imu.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import os 26 | 27 | import rosbag 28 | import collections 29 | import numpy as np 30 | 31 | # See https://www.analog.com/media/en/technical-documentation/data-sheets/ADIS16448.pdf 32 | def filterImu(inbag, outbag, imu_topic, bartlett_filter_size = 0): 33 | imu_filtered = imu_topic + '_filtered' 34 | 35 | N_B = 2 ** bartlett_filter_size 36 | deque1 = collections.deque(maxlen=N_B) 37 | deque2 = collections.deque(maxlen=N_B) 38 | stamps = collections.deque(maxlen=N_B) 39 | 40 | with rosbag.Bag(outbag, 'w') as bag: 41 | for topic, msg, t in rosbag.Bag(inbag).read_messages(): 42 | if (msg._has_header and msg.header.stamp.secs == 0): 43 | continue 44 | bag.write(topic, msg, t) 45 | if (topic == imu_topic): 46 | # First stage 47 | deque1.append(np.array([msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z])) 48 | x1 = np.average(np.stack(deque1), axis=0) 49 | # Second stage 50 | deque2.append(x1) 51 | x2 = np.average(np.stack(deque2), axis=0) 52 | stamps.append(msg.header.stamp) 53 | filtered_msg = msg 54 | filtered_msg.header.stamp = stamps[0] 55 | filtered_msg.angular_velocity.x = x2[0] 56 | filtered_msg.angular_velocity.y = x2[1] 57 | filtered_msg.angular_velocity.z = x2[2] 58 | filtered_msg.linear_acceleration.x = x2[3] 59 | filtered_msg.linear_acceleration.y = x2[4] 60 | filtered_msg.linear_acceleration.z = x2[5] 61 | bag.write(imu_filtered, filtered_msg, t) 62 | -------------------------------------------------------------------------------- /python/mav_state_estimation/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | import rosbag 26 | import matplotlib.pyplot as plt 27 | import numpy as np 28 | 29 | def plot(input_bag, receiver_state_topic): 30 | topics = [receiver_state_topic] 31 | bag = rosbag.Bag(input_bag) 32 | 33 | info = bag.get_type_and_topic_info(topics)[1] 34 | for topic in topics: 35 | if topic == receiver_state_topic: 36 | state = np.zeros([info[topic].message_count, 2]) 37 | 38 | state_idx = 0 39 | 40 | for topic, msg, t in bag.read_messages(topics=topics): 41 | if topic == receiver_state_topic: 42 | if msg.rtk_mode_fix: 43 | fix = 1 44 | else: 45 | fix = 0 46 | state[state_idx, :] = [msg.header.stamp.to_sec(), fix] 47 | state_idx += 1 48 | 49 | print("Loaded %d receiver state messages." % (len(state))) 50 | 51 | plt.plot(state[:,0], state[:,1], 'o', label='GNSS Fix') 52 | plt.legend() 53 | plt.grid() 54 | plt.xlabel('Timestamp [s]') 55 | plt.ylabel('Fix [True/False]') 56 | 57 | plt.show() 58 | -------------------------------------------------------------------------------- /scripts/compare.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": "import rosbag_pandas\nimport pandas as pd\nimport numpy as np\n\nfile_1 = '/home/rik/data/2020_08_05_gannertshofen/estimator_development/sensors_2020-08-05-13-13-56_estimator_2020-08-30-17-48-25_w_baseline.bag'\ndf_1 = rosbag_pandas.bag_to_dataframe(file_1)" 9 | }, 10 | { 11 | "cell_type": "code", 12 | "execution_count": 19, 13 | "metadata": {}, 14 | "outputs": [], 15 | "source": "file_2 = '/home/rik/data/2020_08_05_gannertshofen/estimator_development/sensors_2020-08-05-13-13-56_estimator_2020-08-31-09-22-44_spp.bag'\ndf_2 = rosbag_pandas.bag_to_dataframe(file_2)" 16 | }, 17 | { 18 | "cell_type": "code", 19 | "execution_count": 20, 20 | "metadata": {}, 21 | "outputs": [], 22 | "source": "import pandas as pd\ndef getPoseTf(df, topic):\n df_pose = pd.to_datetime(df[topic + '/header/stamp/secs'], unit='s') + pd.to_timedelta(df[topic + '/header/stamp/nsecs'], unit='ns') \n df_pose = pd.concat([df_pose, df[topic + '/pose/position/x']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/position/y']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/position/z']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/orientation/x']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/orientation/y']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/orientation/z']], axis=1)\n df_pose = pd.concat([df_pose, df[topic + '/pose/orientation/w']], axis=1)\n df_pose.reset_index(inplace=True)\n df_pose.columns = ['t_arrival', 't', 'x', 'y', 'z', 'q_x', 'q_y', 'q_z', 'q_w']\n df_pose.dropna(inplace=True)\n df_pose.reset_index(inplace=True)\n df_pose.drop('t_arrival', axis=1, inplace=True)\n df_pose.drop('index', axis=1, inplace=True)\n \n from scipy.spatial.transform import Rotation as R\n ypr = df_pose.apply(lambda row: R.from_quat([row.q_x, row.q_y, row.q_z, row.q_w]).as_euler('ZYX', degrees=True), axis=1)\n ypr = pd.DataFrame(ypr.values.tolist(), columns=['yaw', 'pitch', 'roll'])\n df_pose = pd.concat([df_pose, ypr], axis=1)\n df_pose.set_index('t', inplace=True)\n return df_pose" 23 | }, 24 | { 25 | "cell_type": "code", 26 | "execution_count": 21, 27 | "metadata": {}, 28 | "outputs": [], 29 | "source": "import pandas as pd\ndef getBias(df, topic):\n df_bias = pd.to_datetime(df[topic + '/header/stamp/secs'], unit='s') + pd.to_timedelta(df[topic + '/header/stamp/nsecs'], unit='ns') \n df_bias = pd.concat([df_bias, df[topic + '/vector/x']], axis=1)\n df_bias = pd.concat([df_bias, df[topic + '/vector/y']], axis=1)\n df_bias = pd.concat([df_bias, df[topic + '/vector/z']], axis=1)\n df_bias.reset_index(inplace=True)\n df_bias.columns = ['t_arrival', 't', 'x', 'y', 'z']\n df_bias.dropna(inplace=True)\n df_bias.reset_index(inplace=True)\n df_bias.drop('t_arrival', axis=1, inplace=True)\n df_bias.drop('index', axis=1, inplace=True)\n return df_bias" 30 | }, 31 | { 32 | "cell_type": "code", 33 | "execution_count": 22, 34 | "metadata": {}, 35 | "outputs": [], 36 | "source": "def getHeading(df, topic):\n df_heading = pd.to_datetime(df[topic + '/header/stamp/secs'], unit='s') + pd.to_timedelta(df[topic + '/header/stamp/nsecs'], unit='ns')\n df_heading = pd.concat([df_heading, df[topic + '/position/position/x']], axis=1)\n df_heading = pd.concat([df_heading, df[topic + '/position/position/y']], axis=1)\n df_heading.reset_index(inplace=True)\n df_heading.columns = ['t_arrival', 't', 'base_x', 'base_y']\n df_heading.dropna(inplace=True)\n df_heading.set_index('t', inplace=True)\n df_heading.drop('t_arrival', axis=1, inplace=True)\n # Convert NED->ENU\n import numpy as np\n x = df_heading['base_y'].values\n y = df_heading['base_x'].values\n\n from scipy.spatial.transform import Rotation as R\n r = R.from_rotvec(np.pi/2 * np.array([0, 0, 1]))\n vectors = np.array([x, y, np.zeros(len(x))]).transpose()\n heading_vectors = r.apply(vectors)\n\n heading = np.arctan2(heading_vectors[:, 1], heading_vectors[:, 0]) * 180.0 / np.pi\n\n df_heading['rtk heading'] = heading\n \n return df_heading" 37 | }, 38 | { 39 | "cell_type": "code", 40 | "execution_count": 23, 41 | "metadata": {}, 42 | "outputs": [], 43 | "source": "df_pose_1 = getPoseTf(df_1, '/moa/mav_state_estimator/optimization')\ndf_pose_2 = getPoseTf(df_2, '/moa/mav_state_estimator/optimization')\ndf_heading = getHeading(df_1, '/moa/piksi/attitude_receiver_0/ros/baseline_ned')\n\ndf_acc_bias_1 = getBias(df_1, '/moa/mav_state_estimator/acc_bias')\ndf_acc_bias_2 = getBias(df_2, '/moa/mav_state_estimator/acc_bias')\n\ndf_gyro_bias_1 = getBias(df_1, '/moa/mav_state_estimator/gyro_bias')\ndf_gyro_bias_2 = getBias(df_2, '/moa/mav_state_estimator/gyro_bias')" 44 | }, 45 | { 46 | "cell_type": "code", 47 | "execution_count": 14, 48 | "metadata": {}, 49 | "outputs": [ 50 | { 51 | "data": { 52 | "text/plain": "Text(0,0.5,'Angle [deg]')" 53 | }, 54 | "execution_count": 14, 55 | "metadata": {}, 56 | "output_type": "execute_result" 57 | } 58 | ], 59 | "source": "import matplotlib.pyplot as plt\nimport matplotlib.dates as mdates\n%matplotlib qt\n\nfontsize=12\nfig, axs = plt.subplots(nrows=1, sharex=True)\n\ndf_pose_1['yaw'].plot(ax=axs)\ndf_pose_2['yaw'].plot(ax=axs)\ndf_heading['rtk heading'].plot(style='-', ax=axs)\naxs.legend(['heading 1', 'heading 2', 'heading rtk'])\naxs.set_xlabel('Timestamp', fontsize=fontsize)\naxs.set_ylabel('Angle [deg]', fontsize=fontsize)" 60 | }, 61 | { 62 | "cell_type": "code", 63 | "execution_count": 1, 64 | "metadata": {}, 65 | "outputs": [ 66 | { 67 | "ename": "NameError", 68 | "evalue": "name 'df_pose_1' is not defined", 69 | "output_type": "error", 70 | "traceback": [ 71 | "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m", 72 | "\u001b[0;31mNameError\u001b[0m Traceback (most recent call last)", 73 | "\u001b[0;32m\u001b[0m in \u001b[0;36m\u001b[0;34m()\u001b[0m\n\u001b[1;32m 6\u001b[0m \u001b[0mfig\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0maxs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msubplots\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnrows\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m3\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0msharex\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mTrue\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 7\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 8\u001b[0;31m \u001b[0mdf_pose_1\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'x'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mplot\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0max\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0maxs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m 9\u001b[0m \u001b[0mdf_pose_2\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'x'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mplot\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0max\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0maxs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m 10\u001b[0m \u001b[0maxs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mset_xlabel\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'Timestamp'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mfontsize\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mfontsize\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n", 74 | "\u001b[0;31mNameError\u001b[0m: name 'df_pose_1' is not defined" 75 | ] 76 | } 77 | ], 78 | "source": "import matplotlib.pyplot as plt\nimport matplotlib.dates as mdates\n%matplotlib qt\n\nfontsize=12\nfig, axs = plt.subplots(nrows=3, sharex=True)\n\ndf_pose_1['x'].plot(ax=axs[0])\ndf_pose_2['x'].plot(ax=axs[0])\naxs[0].set_xlabel('Timestamp', fontsize=fontsize)\naxs[0].set_ylabel('Position [m]', fontsize=fontsize)\naxs[0].legend(['x 1', 'x 2'])\n\ndf_pose_1['y'].plot(ax=axs[1])\ndf_pose_2['y'].plot(ax=axs[1])\naxs[1].set_xlabel('Timestamp', fontsize=fontsize)\naxs[1].set_ylabel('Position [m]', fontsize=fontsize)\naxs[1].legend(['y 1', 'y 2'])\n\ndf_pose_1['z'].plot(ax=axs[2])\ndf_pose_2['z'].plot(ax=axs[2])\naxs[2].set_xlabel('Timestamp', fontsize=fontsize)\naxs[2].set_ylabel('Position [m]', fontsize=fontsize)\naxs[2].legend(['z 1', 'z 2'])\n\n#df_pose_1['roll'].plot(ax=axs[3])\n#df_pose_2['roll'].plot(ax=axs[3])\n#axs[3].set_xlabel('Timestamp', fontsize=fontsize)\n#axs[3].set_ylabel('Angle [deg]', fontsize=fontsize)\n#axs[3].legend(['roll 1', 'roll 2'])\n\n#df_pose_1['pitch'].plot(ax=axs[4])\n#df_pose_2['pitch'].plot(ax=axs[4])\n#axs[4].set_xlabel('Timestamp', fontsize=fontsize)\n#axs[4].set_ylabel('Angle [deg]', fontsize=fontsize)\n#axs[4].legend(['pitch 1', 'pitch 2'])\n\n#df_pose_1['yaw'].plot(ax=axs[5])\n#df_pose_2['yaw'].plot(ax=axs[5])\n#axs[5].set_xlabel('Timestamp', fontsize=fontsize)\n#axs[5].set_ylabel('Angle [deg]', fontsize=fontsize)\n#axs[5].legend(['yaw 1', 'yaw 2'])" 79 | }, 80 | { 81 | "cell_type": "code", 82 | "execution_count": 18, 83 | "metadata": {}, 84 | "outputs": [ 85 | { 86 | "data": { 87 | "text/plain": "" 88 | }, 89 | "execution_count": 18, 90 | "metadata": {}, 91 | "output_type": "execute_result" 92 | } 93 | ], 94 | "source": "import matplotlib.pyplot as plt\nimport matplotlib.dates as mdates\n%matplotlib qt\n\nfontsize=12\nfig, axs = plt.subplots(nrows=2, sharex=True)\n\ndf_acc_bias_1[['x', 'y', 'z']].plot(ax=axs[0])\n#df_acc_bias_2[['x', 'y', 'z']].plot(ax=axs[0], style='k--')\naxs[0].set_xlabel('Timestamp', fontsize=fontsize)\naxs[0].set_ylabel('Accelerometer bias [m/s**2]', fontsize=fontsize)\naxs[0].legend(['x 1', 'y 1', 'z 1', 'x 2', 'y 2', 'z 2'])\n\ndf_gyro_bias_1[['x', 'y', 'z']].plot(ax=axs[1])\ndf_gyro_bias_2[['x', 'y', 'z']].plot(ax=axs[1], style='k--')\naxs[1].set_xlabel('Timestamp', fontsize=fontsize)\naxs[1].set_ylabel('Gyroscope bias [rad/s]', fontsize=fontsize)\naxs[1].legend(['x 1', 'y 1', 'z 1', 'x 2', 'y 2', 'z 2'], loc='upper right')" 95 | } 96 | ], 97 | "metadata": { 98 | "kernelspec": { 99 | "display_name": "Python 2", 100 | "language": "python", 101 | "name": "python2" 102 | }, 103 | "language_info": { 104 | "codemirror_mode": { 105 | "name": "ipython", 106 | "version": 2 107 | }, 108 | "file_extension": ".py", 109 | "mimetype": "text/x-python", 110 | "name": "python", 111 | "nbconvert_exporter": "python", 112 | "pygments_lexer": "ipython2", 113 | "version": "2.7.17" 114 | } 115 | }, 116 | "nbformat": 4, 117 | "nbformat_minor": 4 118 | } 119 | -------------------------------------------------------------------------------- /scripts/export_csv: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | from mav_state_estimation.csv_export import* 26 | 27 | if __name__ == '__main__': 28 | import argparse 29 | parser = argparse.ArgumentParser(description='Bag merging.') 30 | parser.add_argument('input_bag', nargs=1, help='The bag containing stamped poses.') 31 | parser.add_argument('pose_topics', nargs='+', help='All pose topics to export.') 32 | parser.add_argument('--stamp_topic', dest='stamp_topic', nargs=1, default='', help='Only export topics with this timestamp.') 33 | 34 | args = parser.parse_args() 35 | 36 | print("Input bag: %s" %(args.input_bag[0])) 37 | print("Pose topics: %s" %(args.pose_topics)) 38 | stamp_topic = None 39 | if args.stamp_topic: 40 | print("Stamp topic: %s" %(args.stamp_topic[0])) 41 | stamp_topic = args.stamp_topic[0] 42 | 43 | toCsv(args.input_bag[0], args.pose_topics, stamp_topic=stamp_topic) 44 | -------------------------------------------------------------------------------- /scripts/filter_imu: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2020 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | from mav_state_estimation.filter_imu import * 26 | 27 | if __name__ == '__main__': 28 | import argparse 29 | parser = argparse.ArgumentParser(description='Filter IMU data.') 30 | parser.add_argument('inbag_path', nargs=1, help='The bag containing the topics.') 31 | parser.add_argument('outbag_path', nargs=1, help='The bag to export all data to.') 32 | parser.add_argument('topic', nargs=1, help='The imu topic name.') 33 | parser.add_argument('bartlett_filter_size', type=int, nargs=1, help='The size of the filter N_B = 2**size.') 34 | 35 | args = parser.parse_args() 36 | 37 | print("Filtering rosbag: %s" % args.inbag_path[0]) 38 | print("Filtered outbag: %s" % args.outbag_path[0]) 39 | print("IMU topic: %s" % args.topic[0]) 40 | print("Bartlett window size: 2**%d" % args.bartlett_filter_size[0]) 41 | 42 | filterImu(args.inbag_path[0], args.outbag_path[0], args.topic[0], args.bartlett_filter_size[0]) 43 | -------------------------------------------------------------------------------- /scripts/graphviz.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 2, 6 | "metadata": {}, 7 | "outputs": [], 8 | "source": "import glob\nimport graphviz\n\ngraphs = sorted(glob.glob('/tmp/*.dot'))\n \nfor graph in graphs:\n graphviz.render('dot', 'png', graph)" 9 | } 10 | ], 11 | "metadata": { 12 | "kernelspec": { 13 | "display_name": "Python 2", 14 | "language": "python", 15 | "name": "python2" 16 | }, 17 | "language_info": { 18 | "codemirror_mode": { 19 | "name": "ipython", 20 | "version": 2 21 | }, 22 | "file_extension": ".py", 23 | "mimetype": "text/x-python", 24 | "name": "python", 25 | "nbconvert_exporter": "python", 26 | "pygments_lexer": "ipython2", 27 | "version": "2.7.17" 28 | } 29 | }, 30 | "nbformat": 4, 31 | "nbformat_minor": 4 32 | } 33 | -------------------------------------------------------------------------------- /scripts/plot: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # MIT License 4 | # 5 | # Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 6 | # 7 | # Permission is hereby granted, free of charge, to any person obtaining a copy 8 | # of this software and associated documentation files (the "Software"), to deal 9 | # in the Software without restriction, including without limitation the rights 10 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sellpython 11 | # copies of the Software, and to permit persons to whom the Software is 12 | # furnished to do so, subject to the following conditions: 13 | # 14 | # The above copyright notice and this permission notice shall be included in all 15 | # copies or substantial portions of the Software. 16 | # 17 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | # SOFTWARE. 24 | 25 | from mav_state_estimation.plot import* 26 | 27 | if __name__ == '__main__': 28 | import argparse 29 | parser = argparse.ArgumentParser(description='Bag merging.') 30 | parser.add_argument('input_bag', nargs=1, help='The bag containing estimator topics.') 31 | parser.add_argument('receiver_state', nargs=1, help='The GNSS state topic.') 32 | 33 | args = parser.parse_args() 34 | 35 | print("Input bag: %s" %(args.input_bag[0])) 36 | print("Receiver state topic: %s" %(args.receiver_state[0])) 37 | 38 | plot(args.input_bag[0], args.receiver_state[0]) 39 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup( 8 | packages=['mav_state_estimation'], 9 | package_dir={'':'python'} 10 | ) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /src/absolute_position_factor.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include "mav_state_estimation/absolute_position_factor.h" 22 | 23 | #include 24 | #include 25 | 26 | namespace mav_state_estimation { 27 | 28 | AbsolutePositionFactor1::AbsolutePositionFactor1( 29 | gtsam::Key T_I_B_key, const gtsam::Point3& I_t_P_measured, 30 | const gtsam::Point3& B_t_P, 31 | const gtsam::noiseModel::Base::shared_ptr& noise_model) 32 | : Base(noise_model, T_I_B_key), 33 | I_t_P_measured_(I_t_P_measured), 34 | B_t_P_(B_t_P) { 35 | assert(noise_model); 36 | } 37 | 38 | gtsam::Vector AbsolutePositionFactor1::evaluateError( 39 | const gtsam::Pose3& T_I_B, boost::optional D_Tt_T) const { 40 | // h(R,p) = I_t_B + R_I_B * B_t_P 41 | gtsam::Vector3 h; 42 | if (D_Tt_T) { 43 | D_Tt_T->resize(3, 6); 44 | h = T_I_B.transform_from(B_t_P_, D_Tt_T); 45 | } else { 46 | h = T_I_B.transform_from(B_t_P_); 47 | } 48 | 49 | // error = h - z 50 | return h - I_t_P_measured_; 51 | } 52 | 53 | void AbsolutePositionFactor1::print( 54 | const std::string& text, const gtsam::KeyFormatter& key_formatter) const { 55 | std::cout << text << "AbsolutePositionFactor1(" << key_formatter(this->key()) 56 | << ")\n"; 57 | std::cout << " measured: " << I_t_P_measured_.transpose(); 58 | std::cout << " extrinsic calibration: " << B_t_P_.transpose(); 59 | this->noiseModel_->print(" noise model: "); 60 | } 61 | 62 | bool AbsolutePositionFactor1::equals(const gtsam::NonlinearFactor& expected, 63 | double tolerance) const { 64 | const This* expected_casted = dynamic_cast(&expected); // NOLINT 65 | if (!expected_casted) return false; 66 | bool measured_equal = (I_t_P_measured_ == expected_casted->I_t_P_measured_); 67 | bool calibration_equal = (B_t_P_ == expected_casted->B_t_P_); 68 | return Base::equals(*expected_casted, tolerance) && measured_equal && 69 | calibration_equal; 70 | } 71 | 72 | AbsolutePositionFactor2::AbsolutePositionFactor2( 73 | gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, 74 | const gtsam::Point3& I_t_P_measured, 75 | const gtsam::noiseModel::Base::shared_ptr& noise_model) 76 | : Base(noise_model, T_I_B_key, B_t_P_key), I_t_P_measured_(I_t_P_measured) { 77 | assert(noise_model); 78 | } 79 | 80 | gtsam::Vector AbsolutePositionFactor2::evaluateError( 81 | const gtsam::Pose3& T_I_B, const gtsam::Point3& B_t_P, 82 | boost::optional D_Tt_T, 83 | boost::optional D_Tt_t) const { 84 | // h(R,p) = I_t_B + R_I_B * B_t_P 85 | gtsam::Vector3 h; 86 | if (D_Tt_T || D_Tt_t) { 87 | gtsam::Matrix36 D_Tt_T_tmp; 88 | gtsam::Matrix33 D_Tt_t_tmp; 89 | h = T_I_B.transform_from(B_t_P, D_Tt_T_tmp, D_Tt_t_tmp); 90 | if (D_Tt_T) { 91 | D_Tt_T->resize(3, 6); 92 | *D_Tt_T = D_Tt_T_tmp; 93 | } 94 | if (D_Tt_t) { 95 | D_Tt_t->resize(3, 3); 96 | *D_Tt_t = D_Tt_t_tmp; 97 | } 98 | } else { 99 | h = T_I_B.transform_from(B_t_P); 100 | } 101 | 102 | // error = h - z 103 | return h - I_t_P_measured_; 104 | } 105 | 106 | void AbsolutePositionFactor2::print( 107 | const std::string& text, const gtsam::KeyFormatter& key_formatter) const { 108 | std::cout << text << "AbsolutePositionFactor2(" << key_formatter(this->key1()) 109 | << ", " << key_formatter(this->key2()) << ")\n"; 110 | std::cout << " measured: " << I_t_P_measured_.transpose(); 111 | this->noiseModel_->print(" noise model: "); 112 | } 113 | 114 | bool AbsolutePositionFactor2::equals(const gtsam::NonlinearFactor& expected, 115 | double tolerance) const { 116 | const This* expected_casted = dynamic_cast(&expected); // NOLINT 117 | if (!expected_casted) return false; 118 | bool measured_equal = (I_t_P_measured_ == expected_casted->I_t_P_measured_); 119 | return Base::equals(*expected_casted, tolerance) && measured_equal; 120 | } 121 | 122 | } // namespace mav_state_estimation 123 | -------------------------------------------------------------------------------- /src/initialization.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include "mav_state_estimation/initialization.h" 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | namespace mav_state_estimation { 28 | 29 | void Initialization::addOrientationConstraint1(const Eigen::Vector3d& r_I, 30 | const Eigen::Vector3d& r_B, 31 | const ros::Time& t) { 32 | r_1 = std::make_optional(r_I, r_B); 33 | stamp_ = t; 34 | computeInitialState(); 35 | } 36 | 37 | void Initialization::addOrientationConstraint2(const Eigen::Vector3d& r_I, 38 | const Eigen::Vector3d& r_B, 39 | const ros::Time& t) { 40 | r_2 = std::make_optional(r_I, r_B); 41 | stamp_ = t; 42 | computeInitialState(); 43 | } 44 | 45 | void Initialization::addPositionConstraint(const Eigen::Vector3d& I_t_P, 46 | const Eigen::Vector3d& B_t_P, 47 | const ros::Time& t) { 48 | p = std::make_optional(I_t_P, B_t_P); 49 | stamp_ = t; 50 | computeInitialState(); 51 | } 52 | 53 | bool Initialization::getInitialPose( 54 | geometry_msgs::TransformStamped* T_IB) const { 55 | bool is_initialized = isInitialized(); 56 | if (is_initialized) { 57 | *T_IB = *T_IB_; 58 | } 59 | return is_initialized; 60 | } 61 | 62 | void Initialization::computeInitialState() { 63 | bool is_initialized = r_1 && r_2 && p && inertial_frame_ && base_frame_; 64 | if (is_initialized) { 65 | Eigen::Matrix3d R_IB; 66 | calculateTriadOrientation(r_1->first, r_1->second, r_2->first, r_2->second, 67 | &R_IB); 68 | Eigen::Vector3d I_t_B = p->first - R_IB * p->second; 69 | T_IB_ = std::make_optional(); 70 | T_IB_->header.stamp = stamp_; 71 | T_IB_->header.frame_id = *inertial_frame_; 72 | T_IB_->child_frame_id = *base_frame_; 73 | tf::vectorEigenToMsg(I_t_B, T_IB_->transform.translation); 74 | tf::quaternionEigenToMsg(Eigen::Quaterniond(R_IB), 75 | T_IB_->transform.rotation); 76 | } else { 77 | ROS_DEBUG("Missing initial conditions."); 78 | } 79 | } 80 | 81 | void Initialization::calculateTriadOrientation(const Eigen::Vector3d& R1, 82 | const Eigen::Vector3d& r1, 83 | const Eigen::Vector3d& R2, 84 | const Eigen::Vector3d& r2, 85 | Eigen::Matrix3d* R_IB) const { 86 | ROS_ASSERT(R_IB); 87 | 88 | // Calculate the rotation between coordinate frames given two linearly 89 | // independent measurements. https://en.wikipedia.org/wiki/Triad_method 90 | Eigen::Vector3d S = R1.normalized(); 91 | Eigen::Vector3d s = r1.normalized(); 92 | Eigen::Vector3d M = (R1.cross(R2)).normalized(); 93 | Eigen::Vector3d m = (r1.cross(r2)).normalized(); 94 | 95 | Eigen::Matrix3d a; 96 | a << s, m, s.cross(m); 97 | Eigen::Matrix3d b; 98 | b << S, M, S.cross(M); 99 | *R_IB = b * a.transpose(); 100 | } 101 | 102 | } // namespace mav_state_estimation 103 | -------------------------------------------------------------------------------- /src/mav_state_estimator.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include "mav_state_estimation/mav_state_estimator.h" 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "mav_state_estimation/BatchStatus.h" 37 | #include "mav_state_estimation/Timing.h" 38 | #include "mav_state_estimation/absolute_position_factor.h" 39 | #include "mav_state_estimation/moving_baseline_factor.h" 40 | 41 | using gtsam::symbol_shorthand::A; // Attitude receiver antenna (x,y,z) 42 | using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) 43 | using gtsam::symbol_shorthand::P; // Position receiver antenna (x,y,z) 44 | using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) 45 | using gtsam::symbol_shorthand::X; // Pose3 (x,y,z,r,p,y) 46 | 47 | typedef Eigen::Matrix Matrix3dRow; 48 | 49 | namespace mav_state_estimation { 50 | 51 | MavStateEstimator::MavStateEstimator(const ros::NodeHandle& nh, 52 | const ros::NodeHandle& nh_private) 53 | : nh_(nh), nh_private_(nh_private) { 54 | // Get parameters. 55 | nh_private_.getParam("external_poses", external_poses_); 56 | nh_private_.getParam("odometry_throttle", odometry_throttle_); 57 | 58 | // GNSS 59 | nh_private_.getParam("estimate_antenna_positions", 60 | estimate_antenna_positions_); 61 | loadGnssParams("position_receiver", P(0), &B_t_P_, 62 | &process_noise_model_B_t_P_, &pos_receiver_cov_scale_); 63 | loadGnssParams("attitude_receiver", A(0), &B_t_A_, 64 | &process_noise_model_B_t_A_, &att_receiver_cov_scale_); 65 | 66 | Eigen::Vector3d prior_noise_rot_IB, prior_noise_I_t_B; 67 | prior_noise_rot_IB = getVectorFromParams("prior_noise_rot_IB"); 68 | prior_noise_I_t_B = getVectorFromParams("prior_noise_I_t_B"); 69 | prior_noise_model_T_I_B_ = gtsam::noiseModel::Diagonal::Sigmas( 70 | (gtsam::Vector(6) << prior_noise_rot_IB, prior_noise_I_t_B).finished()); 71 | prior_noise_model_T_I_B_->print("prior_noise_model_T_I_B: "); 72 | 73 | Eigen::Vector3d prior_noise_I_v_B; 74 | prior_noise_I_v_B = getVectorFromParams("prior_noise_I_v_B"); 75 | prior_noise_model_I_v_B_ = gtsam::noiseModel::Diagonal::Sigmas( 76 | (gtsam::Vector(3) << prior_noise_I_v_B).finished()); 77 | prior_noise_model_I_v_B_->print("prior_noise_model_I_v_B: "); 78 | 79 | Eigen::Vector3d prior_noise_acc_bias, prior_noise_gyro_bias; 80 | prior_noise_acc_bias = getVectorFromParams("prior_noise_acc_bias"); 81 | prior_noise_gyro_bias = getVectorFromParams("prior_noise_gyro_bias"); 82 | prior_noise_model_imu_bias_ = gtsam::noiseModel::Diagonal::Sigmas( 83 | (gtsam::Vector(6) << prior_noise_acc_bias, prior_noise_gyro_bias) 84 | .finished()); 85 | prior_noise_model_imu_bias_->print("prior_noise_model_imu_bias: "); 86 | 87 | Eigen::Vector3d prior_acc_bias, prior_gyro_bias; 88 | prior_acc_bias = getVectorFromParams("prior_acc_bias"); 89 | prior_gyro_bias = getVectorFromParams("prior_gyro_bias"); 90 | prev_bias_ = gtsam::imuBias::ConstantBias(prior_acc_bias, prior_gyro_bias); 91 | prev_bias_.print("prior_imu_bias: "); 92 | 93 | double bias_acc_sigma = 0.0, bias_omega_sigma = 0.0, bias_acc_int_sigma = 0.0, 94 | bias_omega_int_sigma = 0.0, acc_sigma = 0.0, integration_sigma = 0.0, 95 | gyro_sigma = 0.0; 96 | bool use_2nd_order_coriolis = false; 97 | nh_private_.getParam("bias_acc_sigma", bias_acc_sigma); 98 | nh_private_.getParam("bias_omega_sigma", bias_omega_sigma); 99 | nh_private_.getParam("bias_acc_int_sigma", bias_acc_int_sigma); 100 | nh_private_.getParam("bias_omega_int_sigma", bias_omega_int_sigma); 101 | nh_private_.getParam("acc_sigma", acc_sigma); 102 | nh_private_.getParam("integration_sigma", integration_sigma); 103 | nh_private_.getParam("gyro_sigma", gyro_sigma); 104 | nh_private_.getParam("use_2nd_order_coriolis", use_2nd_order_coriolis); 105 | 106 | const gtsam::Matrix I = gtsam::eye(3, 3); 107 | auto imu_params = 108 | gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedU(); 109 | imu_params->biasAccCovariance = I * pow(bias_acc_sigma, 2); 110 | imu_params->biasOmegaCovariance = I * pow(bias_omega_sigma, 2); 111 | gtsam::Matrix bias_acc_omega_int = gtsam::zeros(6, 6); 112 | bias_acc_omega_int.block<3, 3>(0, 0) = I * pow(bias_acc_int_sigma, 2); 113 | bias_acc_omega_int.block<3, 3>(3, 3) = I * pow(bias_omega_int_sigma, 2); 114 | imu_params->biasAccOmegaInt = bias_acc_omega_int; 115 | imu_params->accelerometerCovariance = I * pow(acc_sigma, 2); 116 | imu_params->integrationCovariance = I * pow(integration_sigma, 2); 117 | imu_params->gyroscopeCovariance = I * pow(gyro_sigma, 2); 118 | imu_params->use2ndOrderCoriolis = use_2nd_order_coriolis; 119 | imu_params->print("IMU settings: "); 120 | integrator_ = 121 | gtsam::PreintegratedCombinedMeasurements(imu_params, prev_bias_); 122 | 123 | // ISAM2 124 | gtsam::ISAM2Params parameters; 125 | // TODO(rikba): Set more ISAM2 params here. 126 | double relinearize_threshold_rot, relinearize_threshold_pos, 127 | relinearize_threshold_vel, relinearize_threshold_acc_bias, 128 | relinearize_threshold_gyro_bias, 129 | relinearize_threshold_antenna_calibration; 130 | nh_private_.getParam("isam2/relinearize_threshold_rot", 131 | relinearize_threshold_rot); 132 | nh_private_.getParam("isam2/relinearize_threshold_pos", 133 | relinearize_threshold_pos); 134 | nh_private_.getParam("isam2/relinearize_threshold_vel", 135 | relinearize_threshold_vel); 136 | nh_private_.getParam("isam2/relinearize_threshold_acc_bias", 137 | relinearize_threshold_acc_bias); 138 | nh_private_.getParam("isam2/relinearize_threshold_gyro_bias", 139 | relinearize_threshold_gyro_bias); 140 | nh_private_.getParam("isam2/relinearize_threshold_antenna_calibration", 141 | relinearize_threshold_antenna_calibration); 142 | gtsam::FastMap thresholds; 143 | thresholds['x'] = 144 | (gtsam::Vector(6) << Eigen::Vector3d::Constant(relinearize_threshold_rot), 145 | Eigen::Vector3d::Constant(relinearize_threshold_pos)) 146 | .finished(); 147 | thresholds['v'] = Eigen::Vector3d::Constant(relinearize_threshold_vel); 148 | thresholds['b'] = (gtsam::Vector(6) << Eigen::Vector3d::Constant( 149 | relinearize_threshold_acc_bias), 150 | Eigen::Vector3d::Constant(relinearize_threshold_gyro_bias)) 151 | .finished(); 152 | thresholds['p'] = 153 | Eigen::Vector3d::Constant(relinearize_threshold_antenna_calibration); 154 | thresholds['a'] = 155 | Eigen::Vector3d::Constant(relinearize_threshold_antenna_calibration); 156 | parameters.relinearizeThreshold = thresholds; 157 | nh_private_.getParam("isam2/relinearize_skip", parameters.relinearizeSkip); 158 | nh_private_.getParam("isam2/enable_partial_relinarization_check", 159 | parameters.enablePartialRelinearizationCheck); 160 | 161 | double smoother_lag = 0.0; 162 | nh_private_.getParam("isam2/smoother_lag", smoother_lag); 163 | 164 | smoother_ = gtsam::IncrementalFixedLagSmoother(smoother_lag, parameters); 165 | 166 | // Subscribe to topics. 167 | const uint32_t kQueueSize = 1000; 168 | imu_sub_ = 169 | nh_.subscribe("imu0", kQueueSize, &MavStateEstimator::imuCallback, this); 170 | ROS_INFO("Subscribing to: %s", imu_sub_.getTopic().c_str()); 171 | pos_0_sub_ = 172 | nh_.subscribe("pos0", kQueueSize, &MavStateEstimator::posCallback, this); 173 | ROS_INFO("Subscribing to: %s", pos_0_sub_.getTopic().c_str()); 174 | baseline_sub_ = nh_.subscribe("baseline0", kQueueSize, 175 | &MavStateEstimator::baselineCallback, this); 176 | ROS_INFO("Subscribing to: %s", baseline_sub_.getTopic().c_str()); 177 | 178 | // Advertise topics. 179 | timing_pub_ = nh_private_.advertise( 180 | "solveThreaded", kQueueSize); 181 | prediction_pub_ = nh_private_.advertise( 182 | "prediction", kQueueSize); 183 | optimization_pub_ = nh_private_.advertise( 184 | "optimization", kQueueSize); 185 | acc_bias_pub_ = nh_private_.advertise( 186 | "acc_bias", kQueueSize); 187 | gyro_bias_pub_ = nh_private_.advertise( 188 | "gyro_bias", kQueueSize); 189 | position_antenna_pub_ = nh_private_.advertise( 190 | "position_antenna", kQueueSize); 191 | attitude_antenna_pub_ = nh_private_.advertise( 192 | "attitude_antenna", kQueueSize); 193 | odometry_pub_ = 194 | nh_private_.advertise("odometry", kQueueSize); 195 | 196 | batch_srv_ = nh_private_.advertiseService( 197 | "compute_batch_solution", &MavStateEstimator::computeBatchSolution, this); 198 | } 199 | 200 | void MavStateEstimator::loadGnssParams( 201 | const std::string& antenna_ns, const gtsam::Symbol& symbol, 202 | gtsam::Point3* B_t, gtsam::noiseModel::Isotropic::shared_ptr* process_noise, 203 | double* cov_scale) { 204 | assert(B_t); 205 | assert(cov_scale); 206 | 207 | // General GNSS parameters. 208 | *B_t = getVectorFromParams(antenna_ns + "/B_t"); 209 | ROS_INFO_STREAM("Initial guess " << antenna_ns.c_str() 210 | << " location: " << B_t->transpose()); 211 | int rate = 0; 212 | nh_private_.getParam(antenna_ns + "/rate", rate); 213 | ROS_INFO_STREAM(antenna_ns.c_str() << " rate: " << rate << " Hz"); 214 | addSensorTimes(rate); 215 | nh_private_.getParam(antenna_ns + "/scale_cov", *cov_scale); 216 | ROS_INFO_STREAM(antenna_ns.c_str() << " cov scale: " << *cov_scale); 217 | 218 | if (estimate_antenna_positions_) { 219 | // Prior 220 | double prior_noise_B_t; 221 | nh_private_.getParam(antenna_ns + "/prior_noise_B_t", prior_noise_B_t); 222 | auto prior_noise = 223 | gtsam::noiseModel::Isotropic::Sigma(B_t->size(), prior_noise_B_t); 224 | auto prior_B_t = boost::make_shared>( 225 | symbol, *B_t, prior_noise); 226 | prior_B_t->print(antenna_ns + " prior factor:\n"); 227 | new_unary_factors_.emplace_back(0, prior_B_t); 228 | 229 | // Initial value 230 | new_values_.insert(symbol, *B_t); 231 | 232 | // Process noise 233 | double process_noise_B_t; 234 | nh_private_.getParam(antenna_ns + "/process_noise_B_t", process_noise_B_t); 235 | *process_noise = 236 | gtsam::noiseModel::Isotropic::Sigma(B_t->size(), process_noise_B_t); 237 | process_noise_model_B_t_P_->print(antenna_ns + " process noise:\n"); 238 | } 239 | } 240 | 241 | Eigen::Vector3d MavStateEstimator::getVectorFromParams( 242 | const std::string& param) const { 243 | std::vector vec; 244 | nh_private_.getParam(param, vec); 245 | Eigen::Vector3d eig = Eigen::Vector3d::Zero(); 246 | if (vec.size() == 3) { 247 | eig = Eigen::Vector3d(vec.data()); 248 | } else { 249 | ROS_WARN("Cannot process parameter: %s Vector size: %lu", param.c_str(), 250 | vec.size()); 251 | } 252 | return eig; 253 | } 254 | 255 | void MavStateEstimator::addSensorTimes(const uint16_t rate) { 256 | uint32_t period = 1e9 / rate; 257 | uint32_t ns = 0; 258 | while (ns < 1e9) { 259 | auto success = unary_times_ns_.insert(ns); 260 | if (success.second) { 261 | ROS_INFO("Inserted new sensor time: %u", ns); 262 | } 263 | ns += period; 264 | } 265 | } 266 | 267 | void MavStateEstimator::initializeState() { 268 | geometry_msgs::TransformStamped T_IB_0; 269 | if (init_.getInitialPose(&T_IB_0)) { 270 | // Get initial values. 271 | Eigen::Vector3d I_t_B; 272 | Eigen::Quaterniond q_IB; 273 | tf::vectorMsgToEigen(T_IB_0.transform.translation, I_t_B); 274 | tf::quaternionMsgToEigen(T_IB_0.transform.rotation, q_IB); 275 | Eigen::Vector3d I_v_B = Eigen::Vector3d::Zero(); 276 | gtsam::Pose3 T_IB(gtsam::Rot3(q_IB), I_t_B); 277 | 278 | // Fill initial ISAM state. 279 | new_values_.insert(X(0), T_IB); 280 | new_values_.insert(V(0), I_v_B); 281 | new_values_.insert(B(0), prev_bias_); 282 | 283 | prev_state_ = gtsam::NavState(T_IB, I_v_B); 284 | 285 | auto prior_pose = boost::make_shared>( 286 | X(0), T_IB, prior_noise_model_T_I_B_); 287 | auto prior_vel = boost::make_shared>( 288 | V(0), I_v_B, prior_noise_model_I_v_B_); 289 | auto prior_bias = 290 | boost::make_shared>( 291 | B(0), prev_bias_, prior_noise_model_imu_bias_); 292 | new_unary_factors_.emplace_back(0, prior_pose); 293 | new_unary_factors_.emplace_back(0, prior_vel); 294 | new_unary_factors_.emplace_back(0, prior_bias); 295 | 296 | // Initialize time stamps. 297 | stamp_to_idx_[T_IB_0.header.stamp] = 0; 298 | idx_to_stamp_[stamp_to_idx_[T_IB_0.header.stamp]] = T_IB_0.header.stamp; 299 | auto next_state_time = T_IB_0.header.stamp; 300 | next_state_time.nsec = 301 | *unary_times_ns_.upper_bound(T_IB_0.header.stamp.nsec); 302 | addUnaryStamp(next_state_time); 303 | updateTimestampMap(); 304 | 305 | // Print 306 | new_values_.print("Initial state: "); 307 | ROS_INFO_STREAM("Initialization stamp: " << idx_to_stamp_[idx_]); 308 | ROS_INFO_STREAM("Next unary stamp: " << idx_to_stamp_[1]); 309 | } 310 | } 311 | 312 | void MavStateEstimator::imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg) { 313 | // Do not update prediction while initial values are updated. 314 | std::unique_lock lock(update_mtx_); 315 | ROS_INFO_ONCE("Received first IMU message."); 316 | if (!isInitialized()) { 317 | // Gravitational acceleration in inertial frame (ENU). 318 | Eigen::Vector3d I_g = integrator_.p().getGravity(); 319 | // Gravitational acceleration in base frame (IMU). 320 | Eigen::Vector3d B_g; 321 | tf::vectorMsgToEigen(imu_msg->linear_acceleration, B_g); 322 | B_g = prev_bias_.correctAccelerometer(B_g); 323 | B_g *= -1.0; 324 | base_frame_ = imu_msg->header.frame_id; 325 | ROS_INFO_ONCE("Base frame: %s", base_frame_.c_str()); 326 | init_.setBaseFrame(base_frame_); 327 | init_.addOrientationConstraint1(I_g, B_g, imu_msg->header.stamp); 328 | initializeState(); 329 | } else if (imu_msg->header.stamp > idx_to_stamp_[idx_ + 1]) { 330 | // Handle dropped IMU message. 331 | // TODO(rikba): Linearly inpterpolate IMU message. 332 | auto in_between_imu = boost::make_shared(*prev_imu_); 333 | in_between_imu->header.stamp = idx_to_stamp_[idx_ + 1]; 334 | ROS_DEBUG_STREAM("Inserting missing IMU message at " 335 | << in_between_imu->header.stamp); 336 | ROS_DEBUG_STREAM("Prev IMU stamp: " << prev_imu_->header.stamp); 337 | ROS_DEBUG_STREAM("This IMU stamp: " << imu_msg->header.stamp); 338 | imuCallback(in_between_imu); 339 | } else if (imu_msg->header.stamp > prev_imu_->header.stamp) { 340 | // Integrate IMU (zero-order-hold). 341 | Eigen::Vector3d lin_acc, ang_vel; 342 | tf::vectorMsgToEigen(prev_imu_->linear_acceleration, lin_acc); 343 | tf::vectorMsgToEigen(prev_imu_->angular_velocity, ang_vel); 344 | double dt = (imu_msg->header.stamp - prev_imu_->header.stamp).toSec(); 345 | integrator_.integrateMeasurement(lin_acc, ang_vel, dt); 346 | 347 | // Publish high rate IMU prediction. 348 | auto imu_state = integrator_.predict(prev_state_, prev_bias_); 349 | if ((imu_msg->header.seq % odometry_throttle_) == 0) { 350 | geometry_msgs::TransformStamped T_IB = 351 | getTransform(imu_state, imu_msg->header.stamp, base_frame_); 352 | tfb_.sendTransform(T_IB); 353 | prediction_pub_.publish(T_IB); 354 | publishOdometry(imu_state.velocity(), ang_vel, prev_bias_, 355 | imu_msg->header.stamp, T_IB); 356 | } 357 | 358 | // Setup new inbetween IMU factor. 359 | if (addUnaryStamp(imu_msg->header.stamp)) { 360 | idx_ = stamp_to_idx_[imu_msg->header.stamp]; 361 | prev_state_ = imu_state; 362 | 363 | new_values_.insert(B(idx_), prev_bias_); 364 | new_values_.insert(X(idx_), prev_state_.pose()); 365 | new_values_.insert(V(idx_), prev_state_.v()); 366 | 367 | auto imu_factor = boost::make_shared( 368 | X(idx_ - 1), V(idx_ - 1), X(idx_), V(idx_), B(idx_ - 1), B(idx_), 369 | integrator_); 370 | integrator_.resetIntegrationAndSetBias(prev_bias_); 371 | new_graph_.add(imu_factor); 372 | 373 | if (estimate_antenna_positions_) { 374 | // Add antenna offset calibration factors. 375 | new_graph_.add(gtsam::BetweenFactor( 376 | P(idx_ - 1), P(idx_), gtsam::Point3::Zero(), 377 | process_noise_model_B_t_P_)); 378 | new_graph_.add(gtsam::BetweenFactor( 379 | A(idx_ - 1), A(idx_), gtsam::Point3::Zero(), 380 | process_noise_model_B_t_A_)); 381 | 382 | new_values_.insert(P(idx_), B_t_P_); 383 | new_values_.insert(A(idx_), B_t_A_); 384 | } 385 | 386 | updateTimestampMap(); 387 | 388 | // Attempt to run solver thread. 389 | solve(); 390 | } 391 | } else { 392 | ROS_ERROR("Cannot handle IMU message."); 393 | ROS_ERROR_STREAM("This IMU stamp: " << imu_msg->header.stamp); 394 | ROS_ERROR_STREAM("Prev IMU stamp: " << prev_imu_->header.stamp); 395 | } 396 | prev_imu_ = imu_msg; 397 | batch_imu_.push_back(imu_msg); 398 | } 399 | 400 | void MavStateEstimator::updateTimestampMap() { 401 | auto new_stamp = idx_to_stamp_[idx_].toSec(); 402 | new_timestamps_[B(idx_)] = new_stamp; 403 | new_timestamps_[X(idx_)] = new_stamp; 404 | new_timestamps_[V(idx_)] = new_stamp; 405 | if (estimate_antenna_positions_) { 406 | new_timestamps_[P(idx_)] = new_stamp; 407 | new_timestamps_[A(idx_)] = new_stamp; 408 | } 409 | } 410 | 411 | void MavStateEstimator::posCallback( 412 | const piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr& pos_msg) { 413 | static piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr prev_msg = 414 | nullptr; 415 | // Do not update factors while initial values are updated. 416 | std::unique_lock lock(update_mtx_); 417 | ROS_INFO_ONCE("Received first POS message."); 418 | Eigen::Vector3d I_t_P; 419 | tf::pointMsgToEigen(pos_msg->position.position, I_t_P); 420 | if (!isInitialized()) { 421 | // GNSS antenna position in inertial frame (ENU). 422 | inertial_frame_ = pos_msg->header.frame_id; 423 | ROS_INFO_ONCE("Inertial frame: %s", inertial_frame_.c_str()); 424 | init_.setInertialFrame(inertial_frame_); 425 | init_.addPositionConstraint(I_t_P, B_t_P_, pos_msg->header.stamp); 426 | } else if (addUnaryStamp(pos_msg->header.stamp) && prev_msg) { 427 | const bool kSmart = false; 428 | auto cov = gtsam::noiseModel::Gaussian::Covariance( 429 | pos_receiver_cov_scale_ / 430 | (pos_msg->header.stamp - prev_msg->header.stamp).toSec() * 431 | Matrix3dRow::Map(pos_msg->position.covariance.data()), 432 | kSmart); 433 | gtsam::NonlinearFactor::shared_ptr pos_factor; 434 | if (estimate_antenna_positions_) { 435 | pos_factor = boost::make_shared( 436 | X(stamp_to_idx_[pos_msg->header.stamp]), 437 | P(stamp_to_idx_[pos_msg->header.stamp]), I_t_P, cov); 438 | } else { 439 | pos_factor = boost::make_shared( 440 | X(stamp_to_idx_[pos_msg->header.stamp]), I_t_P, B_t_P_, cov); 441 | } 442 | new_unary_factors_.emplace_back(stamp_to_idx_[pos_msg->header.stamp], 443 | pos_factor); 444 | solve(); 445 | } else { 446 | ROS_WARN("Failed to add unary position factor."); 447 | } 448 | prev_msg = pos_msg; 449 | } 450 | 451 | bool MavStateEstimator::addUnaryStamp(const ros::Time& stamp) { 452 | bool valid = (stamp >= stamp_to_idx_.begin()->first); 453 | ROS_WARN_COND(!valid, 454 | "The new stamp %u.%u is before initialization time %u.%u.", 455 | stamp.sec, stamp.nsec, stamp_to_idx_.begin()->first.sec, 456 | stamp_to_idx_.begin()->first.nsec); 457 | bool is_unary = valid & unary_times_ns_.count(stamp.nsec); 458 | ROS_DEBUG_COND(!is_unary, 459 | "The new stamp %u.%u is not expected as a unary factor time.", 460 | stamp.sec, stamp.nsec); 461 | 462 | // Always make sure that the previous, current, and next n seconds are part 463 | // of the index lookup map already. 464 | const uint8_t kIdxWindow = 1; 465 | auto max_time = ros::Time(stamp.sec + kIdxWindow + 1, 0); 466 | if (!stamp_to_idx_.count(max_time)) { 467 | auto min_time = std::max(stamp_to_idx_.rbegin()->first, 468 | ros::Time(stamp.sec - kIdxWindow, 0)); 469 | auto ns = unary_times_ns_.begin(); 470 | while (!stamp_to_idx_.count(max_time)) { 471 | if (!stamp_to_idx_.count(min_time) && 472 | min_time > stamp_to_idx_.rbegin()->first) { 473 | stamp_to_idx_[min_time] = stamp_to_idx_.rbegin()->second + 1; 474 | idx_to_stamp_[stamp_to_idx_[min_time]] = min_time; 475 | } 476 | ns = std::next(ns); 477 | if (ns == unary_times_ns_.end()) { 478 | min_time.sec += 1; 479 | ns = unary_times_ns_.begin(); 480 | } 481 | min_time.nsec = *ns; 482 | } 483 | } 484 | 485 | return is_unary; 486 | } 487 | 488 | void MavStateEstimator::baselineCallback( 489 | const piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr& 490 | baseline_msg) { 491 | static piksi_rtk_msgs::PositionWithCovarianceStamped::ConstPtr prev_msg = 492 | nullptr; 493 | // Do not update factors while initial values are updated. 494 | std::unique_lock lock(update_mtx_); 495 | ROS_INFO_ONCE("Received first BASELINE message."); 496 | Eigen::Vector3d NED_t_PA; 497 | tf::pointMsgToEigen(baseline_msg->position.position, NED_t_PA); 498 | // TODO(rikba): Use ECEF frame by default. 499 | // TODO(rikba): Account for different frame positions. This rotation is only 500 | // correct if ENU and NED origin coincide. 501 | const auto R_ENU_NED = 502 | (Eigen::Matrix3d() << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0) 503 | .finished(); 504 | Eigen::Vector3d I_t_PA = R_ENU_NED * NED_t_PA; 505 | // Moving baseline heading in base frame (IMU). 506 | const Eigen::Vector3d B_t_PA = B_t_A_ - B_t_P_; 507 | if (!isInitialized()) { 508 | init_.addOrientationConstraint2(I_t_PA, B_t_PA, baseline_msg->header.stamp); 509 | } else if (addUnaryStamp(baseline_msg->header.stamp) && prev_msg) { 510 | auto cov = gtsam::noiseModel::Gaussian::Covariance( 511 | R_ENU_NED * att_receiver_cov_scale_ / 512 | (baseline_msg->header.stamp - prev_msg->header.stamp).toSec() * 513 | Matrix3dRow::Map(baseline_msg->position.covariance.data()) * 514 | R_ENU_NED.transpose()); 515 | gtsam::NonlinearFactor::shared_ptr baseline_factor; 516 | if (estimate_antenna_positions_) { 517 | baseline_factor = boost::make_shared( 518 | X(stamp_to_idx_[baseline_msg->header.stamp]), 519 | P(stamp_to_idx_[baseline_msg->header.stamp]), 520 | A(stamp_to_idx_[baseline_msg->header.stamp]), I_t_PA, cov); 521 | } else { 522 | baseline_factor = boost::make_shared( 523 | X(stamp_to_idx_[baseline_msg->header.stamp]), I_t_PA, B_t_PA, cov); 524 | } 525 | new_unary_factors_.emplace_back(stamp_to_idx_[baseline_msg->header.stamp], 526 | baseline_factor); 527 | solve(); 528 | } else { 529 | ROS_WARN("Failed to add unary baseline factor."); 530 | } 531 | prev_msg = baseline_msg; 532 | } 533 | 534 | void MavStateEstimator::publishOdometry( 535 | const Eigen::Vector3d& v_I, const Eigen::Vector3d& omega_B, 536 | const gtsam::imuBias::ConstantBias& bias, const ros::Time& stamp, 537 | const geometry_msgs::TransformStamped& T_IB) const { 538 | assert(T_IB.header.stamp == stamp); 539 | assert(T_IB.header.frame_id == inertial_frame_); 540 | assert(T_IB.child_frame_id == base_frame_); 541 | 542 | nav_msgs::Odometry msg; 543 | msg.header.stamp = stamp; 544 | msg.header.frame_id = inertial_frame_; 545 | msg.child_frame_id = "FLU"; // Publish odometry in FLU frame. 546 | 547 | try { 548 | geometry_msgs::TransformStamped T_BF; 549 | T_BF = tf_buffer_.lookupTransform(base_frame_, msg.child_frame_id, stamp); 550 | tf2::Stamped tf2_T_IB, tf2_T_BF; 551 | tf2::fromMsg(T_IB, tf2_T_IB); 552 | tf2::fromMsg(T_BF, tf2_T_BF); 553 | 554 | // Pose 555 | auto tf2_T_IF = tf2_T_IB * tf2_T_BF; 556 | 557 | auto t_IF = tf2::toMsg(tf2_T_IF.getOrigin()); 558 | Eigen::Vector3d p_IF; 559 | tf::vectorMsgToEigen(t_IF, p_IF); 560 | tf::pointEigenToMsg(p_IF, msg.pose.pose.position); 561 | 562 | msg.pose.pose.orientation = tf2::toMsg(tf2_T_IF.getRotation()); 563 | 564 | // Twist 565 | tf::vectorEigenToMsg(v_I, msg.twist.twist.linear); 566 | tf2::Vector3 tf_v_I; 567 | tf2::fromMsg(msg.twist.twist.linear, tf_v_I); 568 | // TODO(rikba): Account for possible lever arm omega x r 569 | msg.twist.twist.linear = 570 | tf2::toMsg(tf2::quatRotate(tf2_T_IF.getRotation().inverse(), tf_v_I)); 571 | 572 | auto omega_B_corrected = omega_B - bias.gyroscope(); 573 | tf::vectorEigenToMsg(omega_B_corrected, msg.twist.twist.angular); 574 | tf2::Vector3 tf_omega_B; 575 | tf2::fromMsg(msg.twist.twist.angular, tf_omega_B); 576 | msg.twist.twist.angular = tf2::toMsg( 577 | tf2::quatRotate(tf2_T_BF.getRotation().inverse(), tf_omega_B)); 578 | } catch (...) { 579 | ROS_WARN_THROTTLE(10.0, 580 | "No odometry. Cannot lookup T_BF from B: %s to F: %s", 581 | base_frame_.c_str(), msg.child_frame_id.c_str()); 582 | } 583 | 584 | odometry_pub_.publish(msg); 585 | } 586 | 587 | geometry_msgs::TransformStamped MavStateEstimator::getTransform( 588 | const gtsam::NavState& state, const ros::Time& stamp, 589 | const std::string& child_frame_id) const { 590 | geometry_msgs::TransformStamped T_IB; 591 | T_IB.header.stamp = stamp; 592 | T_IB.header.frame_id = inertial_frame_; 593 | T_IB.child_frame_id = child_frame_id; 594 | 595 | tf::vectorEigenToMsg(state.position(), T_IB.transform.translation); 596 | tf::quaternionEigenToMsg(state.attitude().toQuaternion(), 597 | T_IB.transform.rotation); 598 | 599 | return T_IB; 600 | } 601 | 602 | void MavStateEstimator::publishAntennaPosition( 603 | const gtsam::Point3& B_t, const ros::Time& stamp, 604 | const ros::Publisher& pub) const { 605 | geometry_msgs::Vector3Stamped antenna_position; 606 | antenna_position.header.stamp = stamp; 607 | antenna_position.header.frame_id = base_frame_; 608 | tf::vectorEigenToMsg(B_t, antenna_position.vector); 609 | pub.publish(antenna_position); 610 | } 611 | 612 | void MavStateEstimator::createBiasMessage( 613 | const gtsam::imuBias::ConstantBias& bias, const ros::Time& stamp, 614 | geometry_msgs::Vector3Stamped* acc_bias, 615 | geometry_msgs::Vector3Stamped* gyro_bias) const { 616 | assert(acc_bias); 617 | assert(gyro_bias); 618 | 619 | acc_bias->header.stamp = stamp; 620 | gyro_bias->header.stamp = stamp; 621 | 622 | tf::vectorEigenToMsg(bias.accelerometer(), acc_bias->vector); 623 | tf::vectorEigenToMsg(bias.gyroscope(), gyro_bias->vector); 624 | } 625 | 626 | void MavStateEstimator::publishBias( 627 | const gtsam::imuBias::ConstantBias& bias, const ros::Time& stamp, 628 | const ros::Publisher& acc_bias_pub, const ros::Publisher& gyro_bias_pub, 629 | geometry_msgs::Vector3Stamped* acc_bias, 630 | geometry_msgs::Vector3Stamped* gyro_bias) const { 631 | assert(acc_bias); 632 | assert(gyro_bias); 633 | createBiasMessage(bias, stamp, acc_bias, gyro_bias); 634 | 635 | acc_bias_pub.publish(*acc_bias); 636 | gyro_bias_pub.publish(*gyro_bias); 637 | } 638 | 639 | void MavStateEstimator::publishBias(const gtsam::imuBias::ConstantBias& bias, 640 | const ros::Time& stamp, 641 | const ros::Publisher& acc_bias_pub, 642 | const ros::Publisher& gyro_bias_pub) const { 643 | geometry_msgs::Vector3Stamped acc_bias, gyro_bias; 644 | publishBias(bias, stamp, acc_bias_pub, gyro_bias_pub, &acc_bias, &gyro_bias); 645 | } 646 | 647 | MavStateEstimator::~MavStateEstimator() { 648 | if (solver_thread_.joinable()) solver_thread_.join(); 649 | if (batch_thread_.joinable()) batch_thread_.join(); 650 | } 651 | 652 | void MavStateEstimator::solve() { 653 | // Transfer new unary factors to graph if IMU inbetween exists. 654 | new_unary_factors_.erase( 655 | std::remove_if( 656 | new_unary_factors_.begin(), new_unary_factors_.end(), 657 | [this](const std::pair& 658 | factor) -> bool { 659 | bool remove = (factor.first <= this->idx_); 660 | if (remove) { 661 | this->new_graph_.add(factor.second); 662 | } 663 | return remove; 664 | }), 665 | new_unary_factors_.end()); 666 | 667 | // Check for new result. 668 | if (is_solving_.load() || new_graph_.empty()) { 669 | return; 670 | } 671 | if (solver_thread_.joinable()) solver_thread_.join(); 672 | 673 | // Copy new factors to graph. 674 | auto graph = std::make_unique(new_graph_); 675 | auto values = std::make_unique(new_values_); 676 | auto stamps = std::make_unique( 677 | new_timestamps_); 678 | auto time = std::make_unique(idx_to_stamp_[idx_]); 679 | new_graph_.resize(0); 680 | new_values_.clear(); 681 | new_timestamps_.clear(); 682 | 683 | // Solve. 684 | is_solving_.store(true); 685 | solver_thread_ = 686 | std::thread(&MavStateEstimator::solveThreaded, this, std::move(graph), 687 | std::move(values), std::move(stamps), std::move(time), idx_); 688 | } 689 | 690 | void MavStateEstimator::solveThreaded( 691 | std::unique_ptr graph, 692 | std::unique_ptr values, 693 | std::unique_ptr stamps, 694 | std::unique_ptr time, const uint64_t i) { 695 | assert(graph); 696 | assert(values); 697 | assert(stamps); 698 | assert(time); 699 | 700 | // Solve iterative problem. 701 | gttic_(solveThreaded); 702 | smoother_.update(*graph, *values, *stamps); 703 | 704 | auto pose = smoother_.calculateEstimate(X(i)); 705 | auto velocity = smoother_.calculateEstimate(V(i)); 706 | auto bias = smoother_.calculateEstimate(B(i)); 707 | gtsam::Point3 B_t_P = B_t_P_; 708 | gtsam::Point3 B_t_A = B_t_A_; 709 | if (estimate_antenna_positions_) { 710 | B_t_P = smoother_.calculateEstimate(P(i)); 711 | B_t_A = smoother_.calculateEstimate(A(i)); 712 | } 713 | gttoc_(solveThreaded); 714 | gtsam::tictoc_finishedIteration_(); 715 | 716 | // Update new values (threadsafe, blocks all sensor callbacks). 717 | gtsam::NavState new_state(pose, velocity); 718 | std::unique_lock lock(update_mtx_); 719 | { 720 | prev_state_ = new_state; 721 | prev_bias_ = bias; 722 | 723 | if (estimate_antenna_positions_) { 724 | B_t_P_ = B_t_P; 725 | B_t_A_ = B_t_A; 726 | } 727 | 728 | auto idx = i; 729 | for (auto it = new_graph_.begin(); it != new_graph_.end(); ++it) { 730 | auto imu = boost::dynamic_pointer_cast(*it); 731 | if (imu) { 732 | prev_state_ = 733 | imu->preintegratedMeasurements().predict(prev_state_, prev_bias_); 734 | new_values_.update(X(idx + 1), prev_state_.pose()); 735 | new_values_.update(V(idx + 1), prev_state_.velocity()); 736 | new_values_.update(B(idx + 1), prev_bias_); 737 | if (estimate_antenna_positions_) { 738 | new_values_.update(P(idx + 1), B_t_P_); 739 | new_values_.update(A(idx + 1), B_t_A_); 740 | } 741 | idx++; 742 | } 743 | } 744 | } 745 | 746 | // ROS publishers 747 | tictoc_getNode(solveThreaded, solveThreaded); 748 | timing_msg_.header.stamp = *time; 749 | timing_msg_.iteration = solveThreaded->self() - timing_msg_.time; 750 | timing_msg_.time = solveThreaded->self(); 751 | timing_msg_.min = solveThreaded->min(); 752 | timing_msg_.max = solveThreaded->max(); 753 | timing_msg_.mean = solveThreaded->mean(); 754 | timing_pub_.publish(timing_msg_); 755 | 756 | geometry_msgs::TransformStamped T_IB = 757 | getTransform(new_state, *time, base_frame_ + "_optimization"); 758 | tfb_.sendTransform(T_IB); 759 | optimization_pub_.publish(T_IB); 760 | publishBias(bias, *time, acc_bias_pub_, gyro_bias_pub_); 761 | publishAntennaPosition(B_t_P, *time, position_antenna_pub_); 762 | publishAntennaPosition(B_t_A, *time, attitude_antenna_pub_); 763 | 764 | // Update batch graph. 765 | // TODO(rikba): Maybe filter out prior factors. 766 | { 767 | std::unique_lock lock(batch_mtx_); 768 | batch_graph_.push_back(graph->begin(), graph->end()); 769 | batch_values_.insert(*values); 770 | batch_values_.update(X(i), pose); 771 | batch_values_.update(V(i), velocity); 772 | batch_values_.update(B(i), bias); 773 | if (estimate_antenna_positions_) { 774 | batch_values_.update(P(i), B_t_P); 775 | batch_values_.update(A(i), B_t_A); 776 | } 777 | } 778 | 779 | // Print. 780 | // static uint32_t iteration = 0; 781 | // char buffer[50]; 782 | // sprintf(buffer, "/tmp/graph_%04d.dot", iteration); 783 | // std::ofstream os(buffer); 784 | // ROS_INFO_STREAM("Storing graph " << iteration); 785 | // isam2_.getFactorsUnsafe().saveGraph(os, isam2_.getLinearizationPoint()); 786 | // ROS_INFO_STREAM("Storing bayes " << iteration); 787 | // sprintf(buffer, "/tmp/bayes_%04d.dot", iteration++); 788 | // isam2_.saveGraph(buffer); 789 | // ROS_INFO_STREAM("Computation time " << timing_msg_.iteration); 790 | 791 | is_solving_.store(false); 792 | } 793 | 794 | bool MavStateEstimator::computeBatchSolution(Batch::Request& request, 795 | Batch::Response& response) { 796 | auto already_running = batch_running_.load(); 797 | bool initialized = batch_graph_.size(); 798 | if (!already_running && initialized) { 799 | batch_running_.store(true); 800 | // Create deep copies of current graph and initial values. 801 | { 802 | std::unique_lock lock(batch_mtx_); 803 | 804 | auto graph = std::make_unique(batch_graph_); 805 | auto values = std::make_unique(batch_values_); 806 | auto imus = 807 | std::make_unique>(batch_imu_); 808 | auto integrator = 809 | std::make_unique( 810 | integrator_); 811 | auto idx_to_stamp = 812 | std::make_unique>(idx_to_stamp_); 813 | auto bag_file = std::make_unique(request.bag_file); 814 | 815 | if (batch_thread_.joinable()) batch_thread_.join(); 816 | batch_thread_ = 817 | std::thread(&MavStateEstimator::solveBatch, this, std::move(graph), 818 | std::move(values), std::move(imus), std::move(integrator), 819 | std::move(idx_to_stamp), std::move(bag_file)); 820 | } 821 | } 822 | 823 | return !already_running && initialized; 824 | } 825 | 826 | void MavStateEstimator::solveBatch( 827 | std::unique_ptr graph, 828 | std::unique_ptr values, 829 | std::unique_ptr> imus, 830 | std::unique_ptr integrator, 831 | std::unique_ptr> idx_to_stamp, 832 | std::unique_ptr bag_file) { 833 | assert(graph); 834 | assert(values); 835 | assert(imu); 836 | assert(integrator); 837 | assert(idx_to_stamp); 838 | assert(bag_file); 839 | ROS_INFO("Solving batch with %lu factors and %lu values.", graph->size(), 840 | values->size()); 841 | 842 | rosbag::Bag bag; 843 | try { 844 | bag.open(*bag_file, rosbag::bagmode::Write); 845 | } catch (const rosbag::BagException& e) { 846 | ROS_WARN("Cannot open bag file %s: %s", bag_file->c_str(), e.what()); 847 | } 848 | 849 | const std::string kAccBiasTopic = 850 | nh_private_.getNamespace() + "/batch_acc_bias"; 851 | const std::string kGyroBiasTopic = 852 | nh_private_.getNamespace() + "/batch_gyro_bias"; 853 | const std::string kTfTopic = nh_private_.getNamespace() + "/batch"; 854 | 855 | ros::Publisher status_pub; 856 | const uint32_t kBatchStatusQueueSize = 1; 857 | status_pub = nh_private_.advertise( 858 | "batch_status", kBatchStatusQueueSize); 859 | mav_state_estimation::BatchStatus batch_status; 860 | batch_status.total_idx = idx_to_stamp->size(); 861 | batch_status.finished = false; 862 | 863 | gtsam::LevenbergMarquardtOptimizer optimizer(*graph, *values); 864 | auto result = optimizer.optimize(); 865 | 866 | try { 867 | uint64_t idx = 0; 868 | batch_status.current_idx = idx; 869 | auto start = idx_to_stamp->at(idx); 870 | auto prev_imu = std::find_if(imus->begin(), imus->end(), 871 | [&start](const sensor_msgs::Imu::ConstPtr& x) { 872 | return x->header.stamp == start; 873 | }); 874 | if (prev_imu == imus->end()) { 875 | ROS_ERROR("Cannot not find start IMU message."); 876 | } else { 877 | gtsam::NavState prev_state(result.at(X(idx)), 878 | result.at(V(idx))); 879 | auto prev_bias = result.at(B(idx)); 880 | integrator->resetIntegrationAndSetBias(prev_bias); 881 | for (; prev_imu != std::prev(imus->end()); ++prev_imu) { 882 | auto imu = std::next(prev_imu); 883 | Eigen::Vector3d lin_acc, ang_vel; 884 | tf::vectorMsgToEigen((*prev_imu)->linear_acceleration, lin_acc); 885 | tf::vectorMsgToEigen((*prev_imu)->angular_velocity, ang_vel); 886 | auto dt = ((*imu)->header.stamp - (*prev_imu)->header.stamp).toSec(); 887 | integrator->integrateMeasurement(lin_acc, ang_vel, dt); 888 | auto imu_state = integrator->predict(prev_state, prev_bias); 889 | 890 | // Update solution index. 891 | if ((*imu)->header.stamp == idx_to_stamp->at(idx + 1)) { 892 | idx++; 893 | batch_status.current_idx = idx; 894 | // Publish status every 10 percent. 895 | if (((10 * batch_status.current_idx) % batch_status.total_idx) == 0) { 896 | status_pub.publish(batch_status); 897 | } 898 | try { 899 | prev_state = gtsam::NavState(result.at(X(idx)), 900 | result.at(V(idx))); 901 | imu_state = prev_state; 902 | prev_bias = result.at(B(idx)); 903 | integrator->resetIntegrationAndSetBias(prev_bias); 904 | try { 905 | geometry_msgs::Vector3Stamped acc_bias, gyro_bias; 906 | createBiasMessage(prev_bias, (*imu)->header.stamp, &acc_bias, 907 | &gyro_bias); 908 | bag.write(kAccBiasTopic, acc_bias.header.stamp, acc_bias); 909 | bag.write(kGyroBiasTopic, gyro_bias.header.stamp, gyro_bias); 910 | } catch (const rosbag::BagIOException& e) { 911 | ROS_WARN_ONCE("Cannot write batch bias to bag: %s", e.what()); 912 | } catch (const rosbag::BagException& e) { 913 | ROS_WARN("Cannot write batch bias: %s", e.what()); 914 | } 915 | } catch (const gtsam::ValuesKeyDoesNotExist& e) { 916 | ROS_WARN_STREAM("Missing value at index: " 917 | << idx << ", stamp: " << idx_to_stamp->at(idx) 918 | << " error: " << e.what()); 919 | } 920 | } else if ((*imu)->header.stamp > idx_to_stamp->at(idx + 1)) { 921 | ROS_ERROR("Cannot find unary IMU message."); 922 | break; 923 | } 924 | 925 | // Log 926 | geometry_msgs::TransformStamped T_IB = 927 | getTransform(imu_state, (*imu)->header.stamp, base_frame_); 928 | try { 929 | bag.write(kTfTopic, T_IB.header.stamp, T_IB); 930 | } catch (const rosbag::BagIOException& e) { 931 | ROS_WARN_ONCE("Cannot write batch pose to bag: %s", e.what()); 932 | } catch (const rosbag::BagException& e) { 933 | ROS_WARN("Cannot write batch pose: %s", e.what()); 934 | } 935 | 936 | // Additional external poses 937 | tf2::Stamped tf2_T_IB; 938 | tf2::fromMsg(T_IB, tf2_T_IB); 939 | for (const auto& frame : external_poses_) { 940 | geometry_msgs::TransformStamped T_BE; 941 | try { 942 | T_BE = tf_buffer_.lookupTransform(base_frame_, frame, 943 | T_IB.header.stamp); 944 | } catch (...) { 945 | ROS_WARN_ONCE("Cannot lookup T_BE from B: %s E: %s", 946 | base_frame_.c_str(), frame.c_str()); 947 | continue; 948 | } 949 | tf2::Stamped tf2_T_BE; 950 | tf2::fromMsg(T_BE, tf2_T_BE); 951 | auto tf2_T_IE = tf2_T_IB * tf2_T_BE; 952 | 953 | geometry_msgs::TransformStamped T_IE; 954 | T_IE.transform = tf2::toMsg(tf2_T_IE); 955 | T_IE.header.stamp = T_IB.header.stamp; 956 | T_IE.header.frame_id = inertial_frame_; 957 | T_IE.child_frame_id = frame; 958 | 959 | try { 960 | bag.write(frame, T_IE.header.stamp, T_IE); 961 | } catch (const rosbag::BagIOException& e) { 962 | ROS_WARN_ONCE("Cannot write batch pose %s to bag: %s", 963 | frame.c_str(), e.what()); 964 | } catch (const rosbag::BagException& e) { 965 | ROS_WARN("Cannot write batch pose %s: %s", frame.c_str(), e.what()); 966 | } 967 | } 968 | } 969 | ROS_INFO_STREAM("Solved batch solution from " 970 | << start << " to " << (*prev_imu)->header.stamp); 971 | } 972 | } catch (const std::out_of_range& e) { 973 | ROS_ERROR("Unary index out of range: %s", e.what()); 974 | } 975 | 976 | bag.close(); 977 | batch_running_.store(false); 978 | 979 | batch_status.finished = true; 980 | status_pub.publish(batch_status); 981 | } 982 | 983 | } // namespace mav_state_estimation 984 | -------------------------------------------------------------------------------- /src/mav_state_estimator_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include "mav_state_estimation/mav_state_estimator.h" 22 | 23 | #include 24 | 25 | int main(int argc, char** argv) { 26 | ros::init(argc, argv, "mav_state_estimator"); 27 | ros::NodeHandle nh; 28 | ros::NodeHandle nh_private("~"); 29 | mav_state_estimation::MavStateEstimator estimator(nh, nh_private); 30 | ros::spin(); 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /src/mav_state_estimator_nodelet.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include 22 | #include 23 | 24 | #include "mav_state_estimation/mav_state_estimator.h" 25 | 26 | namespace mav_state_estimation { 27 | 28 | class MavStateEstimatorNodelet : public nodelet::Nodelet { 29 | virtual void onInit() { 30 | try { 31 | estimator_ = std::make_shared(getNodeHandle(), 32 | getPrivateNodeHandle()); 33 | } catch (std::runtime_error e) { 34 | ROS_ERROR("%s", e.what()); 35 | } 36 | } 37 | 38 | std::shared_ptr estimator_; 39 | }; 40 | } // namespace mav_state_estimation 41 | 42 | PLUGINLIB_EXPORT_CLASS(mav_state_estimation::MavStateEstimatorNodelet, 43 | nodelet::Nodelet) 44 | -------------------------------------------------------------------------------- /src/moving_baseline_factor.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include "mav_state_estimation/moving_baseline_factor.h" 22 | 23 | #include 24 | #include 25 | 26 | namespace mav_state_estimation { 27 | 28 | MovingBaselineFactor1::MovingBaselineFactor1( 29 | gtsam::Key T_I_B_key, const Eigen::Vector3d& I_t_PA_measured, 30 | const Eigen::Vector3d& B_t_PA, 31 | const gtsam::noiseModel::Base::shared_ptr& noise_model) 32 | : Base(noise_model, T_I_B_key), 33 | I_t_PA_measured_(I_t_PA_measured), 34 | B_t_PA_(B_t_PA) { 35 | assert(noise_model); 36 | } 37 | 38 | gtsam::Vector MovingBaselineFactor1::evaluateError( 39 | const gtsam::Pose3& T_I_B, boost::optional D_Tt_T) const { 40 | // h(R,p) = R_IB * (B_t_PA) 41 | Eigen::Vector3d h; 42 | if (D_Tt_T) { 43 | gtsam::Matrix33 D_Rt_R; 44 | h = T_I_B.rotation().rotate(B_t_PA_, D_Rt_R); 45 | D_Tt_T->resize(3, 6); 46 | D_Tt_T->leftCols<3>() = D_Rt_R; 47 | D_Tt_T->rightCols<3>() = gtsam::Matrix::Zero(3, 3); 48 | } else { 49 | h = T_I_B.rotation().rotate(B_t_PA_); 50 | } 51 | 52 | // error = h - z 53 | return h - I_t_PA_measured_; 54 | } 55 | 56 | void MovingBaselineFactor1::print( 57 | const std::string& text, const gtsam::KeyFormatter& key_formatter) const { 58 | std::cout << text << "MovingBaselineFactor1(" << key_formatter(this->key()) 59 | << ")\n"; 60 | std::cout << " measured I_t_PA: " << I_t_PA_measured_.transpose(); 61 | std::cout << " extrinsic calibration B_t_PA: " << B_t_PA_.transpose(); 62 | this->noiseModel_->print(" noise model: "); 63 | } 64 | 65 | bool MovingBaselineFactor1::equals(const gtsam::NonlinearFactor& expected, 66 | double tolerance) const { 67 | const This* expected_casted = dynamic_cast(&expected); // NOLINT 68 | if (!expected_casted) return false; 69 | bool measured_equal = (I_t_PA_measured_ == expected_casted->I_t_PA_measured_); 70 | bool calibration_equal = (B_t_PA_ == expected_casted->B_t_PA_); 71 | return Base::equals(*expected_casted, tolerance) && measured_equal && 72 | calibration_equal; 73 | } 74 | 75 | MovingBaselineFactor2::MovingBaselineFactor2( 76 | gtsam::Key T_I_B_key, gtsam::Key B_t_P_key, gtsam::Key B_t_A_key, 77 | const Eigen::Vector3d& I_t_PA_measured, 78 | const gtsam::noiseModel::Base::shared_ptr& noise_model) 79 | : Base(noise_model, T_I_B_key, B_t_P_key, B_t_A_key), 80 | I_t_PA_measured_(I_t_PA_measured) { 81 | assert(noise_model); 82 | } 83 | 84 | gtsam::Vector MovingBaselineFactor2::evaluateError( 85 | const gtsam::Pose3& T_I_B, const gtsam::Point3& B_t_P, 86 | const gtsam::Point3& B_t_A, boost::optional D_Tt_T, 87 | boost::optional D_Tt_tP, 88 | boost::optional D_Tt_tA) const { 89 | // h(R,p) = R_IB * (B_t_A - B_t_P) 90 | Eigen::Vector3d h; 91 | auto B_t_PA = B_t_A - B_t_P; 92 | if (D_Tt_T || D_Tt_tP || D_Tt_tA) { 93 | gtsam::Matrix33 D_Rt_R, D_Rt_tA; 94 | h = T_I_B.rotation().rotate(B_t_PA, D_Rt_R, D_Rt_tA); 95 | if (D_Tt_T) { 96 | D_Tt_T->resize(3, 6); 97 | D_Tt_T->leftCols<3>() = D_Rt_R; 98 | D_Tt_T->rightCols<3>() = gtsam::Matrix::Zero(3, 3); 99 | } 100 | if (D_Tt_tP) { 101 | D_Tt_tP->resize(3, 3); 102 | // The partial derivative D_Rt_tA is matrix R*I, thus this one is -R*I. 103 | *D_Tt_tP = -D_Rt_tA; 104 | } 105 | if (D_Tt_tA) { 106 | D_Tt_tA->resize(3, 3); 107 | *D_Tt_tA = D_Rt_tA; 108 | } 109 | } else { 110 | h = T_I_B.rotation().rotate(B_t_PA); 111 | } 112 | 113 | // error = h - z 114 | return h - I_t_PA_measured_; 115 | } 116 | 117 | void MovingBaselineFactor2::print( 118 | const std::string& text, const gtsam::KeyFormatter& key_formatter) const { 119 | std::cout << text << "MovingBaselineFactor2(" << key_formatter(this->key1()) 120 | << ", " << key_formatter(this->key2()) << ", " 121 | << key_formatter(this->key3()) << ")\n"; 122 | std::cout << " measured I_t_PA: " << I_t_PA_measured_.transpose(); 123 | this->noiseModel_->print(" noise model: "); 124 | } 125 | 126 | bool MovingBaselineFactor2::equals(const gtsam::NonlinearFactor& expected, 127 | double tolerance) const { 128 | const This* expected_casted = dynamic_cast(&expected); // NOLINT 129 | if (!expected_casted) return false; 130 | bool measured_equal = (I_t_PA_measured_ == expected_casted->I_t_PA_measured_); 131 | return Base::equals(*expected_casted, tolerance) && measured_equal; 132 | } 133 | 134 | } // namespace mav_state_estimation 135 | -------------------------------------------------------------------------------- /srv/BagToCsv.srv: -------------------------------------------------------------------------------- 1 | string bag_file 2 | string topics 3 | --- 4 | -------------------------------------------------------------------------------- /srv/Batch.srv: -------------------------------------------------------------------------------- 1 | string bag_file 2 | --- 3 | -------------------------------------------------------------------------------- /test/absolute_position_factor-test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "mav_state_estimation/absolute_position_factor.h" 30 | 31 | using namespace gtsam; 32 | using namespace mav_state_estimation; 33 | using symbol_shorthand::P; 34 | using symbol_shorthand::X; 35 | 36 | TEST(AbsolutePositionFactor, Error) { 37 | Rot3 R = Rot3::rodriguez(0.0, 0.0, 0.0); 38 | Point3 t(1.0, 0.5, 0.2); 39 | Pose3 pose(R, t); 40 | Point3 B_t_P(10.0, 0.0, 0.0); 41 | Point3 I_t_P_measured(11.0, 0.5, 0.2); 42 | 43 | AbsolutePositionFactor1 factor1(X(1), I_t_P_measured, B_t_P, 44 | noiseModel::Isotropic::Sigma(3, 0.05)); 45 | AbsolutePositionFactor2 factor2(X(1), P(1), I_t_P_measured, 46 | noiseModel::Isotropic::Sigma(3, 0.05)); 47 | Vector expected_error = (Vector(3) << 0.0, 0.0, 0.0).finished(); 48 | Vector actual_error1 = factor1.evaluateError(pose); 49 | Vector actual_error2 = factor2.evaluateError(pose, B_t_P); 50 | EXPECT_TRUE(assert_equal(expected_error, actual_error1, 1e-5)); 51 | EXPECT_TRUE(assert_equal(expected_error, actual_error2, 1e-5)); 52 | } 53 | 54 | TEST(AbsolutePositionFactor, ErrorHeading) { 55 | Rot3 R = Rot3::rodriguez(Unit3(0.0, 0.0, 1.0), M_PI); 56 | Point3 t(1.0, 0.5, 0.2); 57 | Pose3 pose(R, t); 58 | Point3 B_t_P(10.0, 0.0, 0.0); 59 | Point3 I_t_P_measured(-9.0, 0.5, 0.2); 60 | 61 | AbsolutePositionFactor1 factor1(X(1), I_t_P_measured, B_t_P, 62 | noiseModel::Isotropic::Sigma(3, 0.05)); 63 | AbsolutePositionFactor2 factor2(X(1), P(1), I_t_P_measured, 64 | noiseModel::Isotropic::Sigma(3, 0.05)); 65 | Vector expected_error = (Vector(3) << 0.0, 0.0, 0.0).finished(); 66 | Vector actual_error1 = factor1.evaluateError(pose); 67 | Vector actual_error2 = factor2.evaluateError(pose, B_t_P); 68 | EXPECT_TRUE(assert_equal(expected_error, actual_error1, 1e-5)); 69 | EXPECT_TRUE(assert_equal(expected_error, actual_error2, 1e-5)); 70 | } 71 | 72 | TEST(AbsolutePositionFactor, Jacobian) { 73 | Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); 74 | Point3 t(1.0, 0.5, 0.2); 75 | Pose3 pose(R, t); 76 | Point3 B_t_P(10.0, 0.0, 0.0); 77 | Point3 I_t_P_measured(11.0, 0.5, 0.2); 78 | 79 | AbsolutePositionFactor1 factor1(X(1), I_t_P_measured, B_t_P, 80 | noiseModel::Isotropic::Sigma(3, 0.05)); 81 | AbsolutePositionFactor2 factor2(X(1), P(1), I_t_P_measured, 82 | noiseModel::Isotropic::Sigma(3, 0.05)); 83 | 84 | Matrix actual1_D_Tt_T; 85 | factor1.evaluateError(pose, actual1_D_Tt_T); 86 | 87 | Matrix numerical1_D_Tt_T = numericalDerivative11( 88 | boost::function(boost::bind( 89 | &AbsolutePositionFactor1::evaluateError, factor1, _1, boost::none)), 90 | pose, 1e-5); 91 | EXPECT_TRUE(assert_equal(numerical1_D_Tt_T, actual1_D_Tt_T, 1e-5)); 92 | 93 | Matrix actual2_D_Tt_T, actual2_D_Tt_t; 94 | factor2.evaluateError(pose, B_t_P, actual2_D_Tt_T, actual2_D_Tt_t); 95 | 96 | Matrix numerical2_D_Tt_T = numericalDerivative21( 97 | boost::function( 98 | boost::bind(&AbsolutePositionFactor2::evaluateError, factor2, _1, _2, 99 | boost::none, boost::none)), 100 | pose, B_t_P, 1e-5); 101 | EXPECT_TRUE(assert_equal(numerical2_D_Tt_T, actual2_D_Tt_T, 1e-5)); 102 | 103 | Matrix numerical2_D_Tt_t = numericalDerivative22( 104 | boost::function( 105 | boost::bind(&AbsolutePositionFactor2::evaluateError, factor2, _1, _2, 106 | boost::none, boost::none)), 107 | pose, B_t_P, 1e-5); 108 | EXPECT_TRUE(assert_equal(numerical2_D_Tt_t, actual2_D_Tt_t, 1e-5)); 109 | } 110 | 111 | int main(int argc, char** argv) { 112 | testing::InitGoogleTest(&argc, argv); 113 | return RUN_ALL_TESTS(); 114 | } 115 | -------------------------------------------------------------------------------- /test/moving_baseline_factor-test.cc: -------------------------------------------------------------------------------- 1 | /* 2 | MIT License 3 | Copyright (c) 2021 Rik Baehnemann, ASL, ETH Zurich, Switzerland 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | The above copyright notice and this permission notice shall be included in all 11 | copies or substantial portions of the Software. 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 14 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 15 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 16 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 17 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 18 | SOFTWARE. 19 | */ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "mav_state_estimation/moving_baseline_factor.h" 30 | 31 | using namespace gtsam; 32 | using namespace mav_state_estimation; 33 | using symbol_shorthand::A; 34 | using symbol_shorthand::P; 35 | using symbol_shorthand::X; 36 | 37 | TEST(MovingBaselineFactor2, Error) { 38 | Rot3 R = Rot3::Yaw(M_PI / 2); 39 | Point3 t(1.0, 0.5, 0.2); 40 | Pose3 pose(R, t); 41 | Point3 B_t_P(10.0, 0.0, 0.0); 42 | Point3 B_t_A(11.0, 0.0, 0.0); 43 | Point3 I_t_PA_measured(0.0, 1.0, 0.0); 44 | 45 | MovingBaselineFactor1 factor1(X(1), I_t_PA_measured, B_t_A - B_t_P, 46 | noiseModel::Isotropic::Sigma(3, 0.05)); 47 | MovingBaselineFactor2 factor2(X(1), P(1), A(1), I_t_PA_measured, 48 | noiseModel::Isotropic::Sigma(3, 0.05)); 49 | Vector expected_error = (Vector(3) << 0.0, 0.0, 0.0).finished(); 50 | Vector actual_error1 = factor1.evaluateError(pose); 51 | Vector actual_error2 = factor2.evaluateError(pose, B_t_P, B_t_A); 52 | EXPECT_TRUE(assert_equal(expected_error, actual_error1, 1e-5)); 53 | EXPECT_TRUE(assert_equal(expected_error, actual_error2, 1e-5)); 54 | } 55 | 56 | TEST(MovingBaselineFactor2, Jacobian) { 57 | Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); 58 | Point3 t(1.0, 0.5, 0.2); 59 | Pose3 pose(R, t); 60 | Point3 B_t_P(10.0, 0.0, 0.0); 61 | Point3 B_t_A(11.0, 0.0, 0.0); 62 | Point3 I_t_PA_measured(0.0, 1.0, 0.0); 63 | 64 | MovingBaselineFactor1 factor1(X(1), I_t_PA_measured, B_t_A - B_t_P, 65 | noiseModel::Isotropic::Sigma(3, 0.05)); 66 | MovingBaselineFactor2 factor2(X(1), P(1), A(1), I_t_PA_measured, 67 | noiseModel::Isotropic::Sigma(3, 0.05)); 68 | 69 | Matrix actual1_D_Tt_T; 70 | factor1.evaluateError(pose, actual1_D_Tt_T); 71 | 72 | Matrix numerical1_D_Tt_T = numericalDerivative11( 73 | boost::function(boost::bind( 74 | &MovingBaselineFactor1::evaluateError, factor1, _1, boost::none)), 75 | pose, 1e-5); 76 | EXPECT_TRUE(assert_equal(numerical1_D_Tt_T, actual1_D_Tt_T, 1e-5)); 77 | 78 | Matrix actual2_D_Tt_T, actual2_D_Tt_TP, actual2_D_Tt_TA; 79 | factor2.evaluateError(pose, B_t_P, B_t_A, actual2_D_Tt_T, actual2_D_Tt_TP, 80 | actual2_D_Tt_TA); 81 | 82 | Matrix numerical2_D_Tt_T = numericalDerivative31( 83 | boost::function( 84 | boost::bind(&MovingBaselineFactor2::evaluateError, factor2, _1, _2, 85 | _3, boost::none, boost::none, boost::none)), 86 | pose, B_t_P, B_t_A, 1e-5); 87 | EXPECT_TRUE(assert_equal(numerical2_D_Tt_T, actual2_D_Tt_T, 1e-5)); 88 | 89 | Matrix numerical2_D_Tt_TP = numericalDerivative32( 90 | boost::function( 91 | boost::bind(&MovingBaselineFactor2::evaluateError, factor2, _1, _2, 92 | _3, boost::none, boost::none, boost::none)), 93 | pose, B_t_P, B_t_A, 1e-5); 94 | EXPECT_TRUE(assert_equal(numerical2_D_Tt_TP, actual2_D_Tt_TP, 1e-5)); 95 | 96 | Matrix numerical2_D_Tt_TA = numericalDerivative33( 97 | boost::function( 98 | boost::bind(&MovingBaselineFactor2::evaluateError, factor2, _1, _2, 99 | _3, boost::none, boost::none, boost::none)), 100 | pose, B_t_P, B_t_A, 1e-5); 101 | EXPECT_TRUE(assert_equal(numerical2_D_Tt_TA, actual2_D_Tt_TA, 1e-5)); 102 | } 103 | 104 | int main(int argc, char** argv) { 105 | testing::InitGoogleTest(&argc, argv); 106 | return RUN_ALL_TESTS(); 107 | } 108 | --------------------------------------------------------------------------------