├── README.md ├── common_msgs ├── CMakeLists.txt ├── msg │ ├── GPSVelocity.msg │ ├── MaxDynamics.msg │ ├── MeasurementPosVel.msg │ ├── MeasurementPosition.msg │ ├── MeasurementVelocity.msg │ ├── Mission.msg │ ├── MissionStatus.msg │ ├── NavigationState.msg │ ├── PathPlanningStatus.msg │ ├── PixhawkCMD.msg │ ├── PixhawkServo.msg │ ├── Reference.msg │ ├── UWB_DataInfo.msg │ ├── UWB_EchoedRangeInfo.msg │ ├── UWB_FullNeighborDatabase.msg │ ├── UWB_FullNeighborDatabaseEntry.msg │ ├── UWB_FullRangeInfo.msg │ ├── UWB_SendData.msg │ └── Waypoint.msg └── package.xml └── uwb-imu-positioning ├── CMakeLists.txt ├── LICENSE ├── include ├── anchor_coordinate_factor.h ├── anchor_distance_factor.h ├── anchor_triangulation_factor.h ├── basic_function.h ├── imu_factor.h ├── localization_node.h ├── parameter.h ├── pose_local_parameterization.h ├── pre_integration.h ├── tic_toc.h ├── utility.h ├── uwb.h ├── uwb_init_loc.h ├── uwb_position_factor.h └── uwb_range_factor.h ├── launch ├── slam.launch └── slam.yaml ├── package.xml └── src ├── localization_node.cc ├── pose_local_parameterization.cc ├── uwb.cc └── uwb_init_loc.cc /README.md: -------------------------------------------------------------------------------- 1 | # uwb-imu-positioning 2 | ## Tightly-coupled integrated navigation system between UWB and IMU for UAV indoor localization 3 | ### The FGO fusion part is written according to the paper 4 | [Tightly coupled integrated navigation system via factor graph for UAV indoor localization](https://www.sciencedirect.com/science/article/pii/S127096382031052X) 5 | #### Currently it provides loosely and tightly coupled FGO integration. More codes is coming.... 6 | 7 | ### Introduction 8 | A ROS based library to perform localization for robot swarms using Ultra Wide Band (UWB) and Inertial Measurement Unit (UWB). 9 | 10 | ### Features 11 | - [x] beacon positioning with only UWB. 12 | - [x] FGO localization algorithms that integrate UWB and IMU. 13 | - [ ] Initialization with frame alignment and anchor refinement. **Coming...** 14 | - [ ] EKF localization algotithms that integrate UWB and IMU. **Coming...** 15 | 16 | ### 1. Prerequisites 17 | #### 1.1 Ubuntu and ROS 18 | * Ubuntu 64-bit 18.04 19 | * ROS Melodic. [ROS INSTALLATION](http://wiki.ros.org/ROS/Installation) 20 | 21 | #### 1.2 Ceres Solver 22 | * Ceres. [Ceres INSTALLATION](http://ceres-solver.org/installation.html) 23 | 24 | 25 | 26 | ### Acknowledge 27 | The Code is modified from the work done by [Vins-mono](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) and [uwb-localization](https://github.com/lijx10/uwb-localization) 28 | -------------------------------------------------------------------------------- /common_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(common_msgs) 3 | 4 | set(CMAKE_BUILD_TYPE "Release") 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | angles 11 | geometry_msgs 12 | roscpp 13 | rospy 14 | sensor_msgs 15 | std_msgs 16 | tf 17 | message_generation 18 | ) 19 | 20 | ## System dependencies are found with CMake's conventions 21 | # find_package(Boost REQUIRED COMPONENTS system) 22 | 23 | 24 | ## Uncomment this if the package has a setup.py. This macro ensures 25 | ## modules and global scripts declared therein get installed 26 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 27 | # catkin_python_setup() 28 | 29 | ################################################ 30 | ## Declare ROS messages, services and actions ## 31 | ################################################ 32 | 33 | ## To declare and build messages, services or actions from within this 34 | ## package, follow these steps: 35 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 36 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 37 | ## * In the file package.xml: 38 | ## * add a build_depend tag for "message_generation" 39 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 40 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 41 | ## but can be declared for certainty nonetheless: 42 | ## * add a run_depend tag for "message_runtime" 43 | ## * In this file (CMakeLists.txt): 44 | ## * add "message_generation" and every package in MSG_DEP_SET to 45 | ## find_package(catkin REQUIRED COMPONENTS ...) 46 | ## * add "message_runtime" and every package in MSG_DEP_SET to 47 | ## catkin_package(CATKIN_DEPENDS ...) 48 | ## * uncomment the add_*_files sections below as needed 49 | ## and list every .msg/.srv/.action file to be processed 50 | ## * uncomment the generate_messages entry below 51 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 52 | 53 | ## Generate messages in the 'msg' folder 54 | add_message_files( 55 | FILES 56 | NavigationState.msg 57 | GPSVelocity.msg 58 | MeasurementPosition.msg 59 | MeasurementVelocity.msg 60 | PixhawkCMD.msg 61 | PixhawkServo.msg 62 | MeasurementPosVel.msg 63 | MaxDynamics.msg 64 | Reference.msg 65 | Mission.msg 66 | MissionStatus.msg 67 | PathPlanningStatus.msg 68 | Waypoint.msg 69 | UWB_DataInfo.msg 70 | UWB_EchoedRangeInfo.msg 71 | UWB_FullNeighborDatabase.msg 72 | UWB_FullNeighborDatabaseEntry.msg 73 | UWB_FullRangeInfo.msg 74 | UWB_SendData.msg 75 | ) 76 | 77 | ## Generate services in the 'srv' folder 78 | # add_service_files( 79 | # FILES 80 | # Service1.srv 81 | # Service2.srv 82 | # ) 83 | 84 | ## Generate actions in the 'action' folder 85 | # add_action_files( 86 | # FILES 87 | # Action1.action 88 | # Action2.action 89 | # ) 90 | 91 | ## Generate added messages and services with any dependencies listed here 92 | generate_messages( 93 | DEPENDENCIES 94 | geometry_msgs 95 | sensor_msgs 96 | std_msgs 97 | ) 98 | 99 | #Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view. 100 | file(GLOB_RECURSE EXTRA_FILES */*) 101 | add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES}) 102 | 103 | ################################################ 104 | ## Declare ROS dynamic reconfigure parameters ## 105 | ################################################ 106 | 107 | ## To declare and build dynamic reconfigure parameters within this 108 | ## package, follow these steps: 109 | ## * In the file package.xml: 110 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 111 | ## * In this file (CMakeLists.txt): 112 | ## * add "dynamic_reconfigure" to 113 | ## find_package(catkin REQUIRED COMPONENTS ...) 114 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 115 | ## and list every .cfg file to be processed 116 | 117 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 118 | # generate_dynamic_reconfigure_options( 119 | # cfg/DynReconf1.cfg 120 | # cfg/DynReconf2.cfg 121 | # ) 122 | 123 | ################################### 124 | ## catkin specific configuration ## 125 | ################################### 126 | ## The catkin_package macro generates cmake config files for your package 127 | ## Declare things to be passed to dependent projects 128 | ## INCLUDE_DIRS: uncomment this if you package contains header files 129 | ## LIBRARIES: libraries you create in this project that dependent projects also need 130 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 131 | ## DEPENDS: system dependencies of this project that dependent projects also need 132 | catkin_package( 133 | # INCLUDE_DIRS include 134 | # LIBRARIES common_msgs 135 | CATKIN_DEPENDS angles geometry_msgs roscpp rospy sensor_msgs std_msgs tf message_runtime 136 | # DEPENDS system_lib 137 | ) 138 | 139 | ########### 140 | ## Build ## 141 | ########### 142 | 143 | ## Specify additional locations of header files 144 | ## Your package locations should be listed before other locations 145 | # include_directories(include) 146 | include_directories( 147 | ${catkin_INCLUDE_DIRS} 148 | ) 149 | 150 | ## Declare a C++ library 151 | # add_library(common_msgs 152 | # src/${PROJECT_NAME}/common_msgs.cpp 153 | # ) 154 | 155 | ## Add cmake target dependencies of the library 156 | ## as an example, code may need to be generated before libraries 157 | ## either from message generation or dynamic reconfigure 158 | # add_dependencies(common_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 159 | 160 | ## Declare a C++ executable 161 | # add_executable(common_msgs_node src/common_msgs_node.cpp) 162 | 163 | ## Add cmake target dependencies of the executable 164 | ## same as for the library above 165 | # add_dependencies(common_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 166 | 167 | ## Specify libraries to link a library or executable target against 168 | # target_link_libraries(common_msgs_node 169 | # ${catkin_LIBRARIES} 170 | # ) 171 | 172 | ############# 173 | ## Install ## 174 | ############# 175 | 176 | # all install targets should use catkin DESTINATION variables 177 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 178 | 179 | ## Mark executable scripts (Python etc.) for installation 180 | ## in contrast to setup.py, you can choose the destination 181 | # install(PROGRAMS 182 | # scripts/my_python_script 183 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 184 | # ) 185 | 186 | ## Mark executables and/or libraries for installation 187 | # install(TARGETS common_msgs common_msgs_node 188 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 189 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 190 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 191 | # ) 192 | 193 | ## Mark cpp header files for installation 194 | # install(DIRECTORY include/${PROJECT_NAME}/ 195 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 196 | # FILES_MATCHING PATTERN "*.h" 197 | # PATTERN ".svn" EXCLUDE 198 | # ) 199 | 200 | ## Mark other files for installation (e.g. launch and bag files, etc.) 201 | # install(FILES 202 | # # myfile1 203 | # # myfile2 204 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 205 | # ) 206 | 207 | ############# 208 | ## Testing ## 209 | ############# 210 | 211 | ## Add gtest based cpp test target and link libraries 212 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_common_msgs.cpp) 213 | # if(TARGET ${PROJECT_NAME}-test) 214 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 215 | # endif() 216 | 217 | ## Add folders to be run by python nosetests 218 | # catkin_add_nosetests(test) 219 | -------------------------------------------------------------------------------- /common_msgs/msg/GPSVelocity.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 lat_vel 3 | float64 lon_vel 4 | float64 alt_vel 5 | -------------------------------------------------------------------------------- /common_msgs/msg/MaxDynamics.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Twist vel_max 3 | geometry_msgs/Twist acc_max 4 | geometry_msgs/Twist jerk_max 5 | -------------------------------------------------------------------------------- /common_msgs/msg/MeasurementPosVel.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | MeasurementPosition position 3 | MeasurementVelocity velocity 4 | -------------------------------------------------------------------------------- /common_msgs/msg/MeasurementPosition.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | bool is_xy_valid 4 | bool is_z_valid 5 | bool is_yaw_valid 6 | -------------------------------------------------------------------------------- /common_msgs/msg/MeasurementVelocity.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Twist velocity 3 | bool is_xy_valid 4 | bool is_z_valid 5 | bool is_yaw_valid 6 | -------------------------------------------------------------------------------- /common_msgs/msg/Mission.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string m_type 3 | uint32 m_id 4 | Waypoint waypoint 5 | sensor_msgs/NavSatFix gps_waypoint 6 | PixhawkCMD pixhawkCMD 7 | PixhawkServo pixhawkServo 8 | float64[] redundancy 9 | -------------------------------------------------------------------------------- /common_msgs/msg/MissionStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | Mission mission 3 | PathPlanningStatus pp_status 4 | bool is_reference_reached 5 | bool is_measurement_reached 6 | -------------------------------------------------------------------------------- /common_msgs/msg/NavigationState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist velocity 4 | geometry_msgs/Twist acceleration 5 | -------------------------------------------------------------------------------- /common_msgs/msg/PathPlanningStatus.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | bool is_reached 3 | bool is_target_reachable 4 | common_msgs/Waypoint waypoint 5 | geometry_msgs/Pose alternative_target_position_NWU 6 | -------------------------------------------------------------------------------- /common_msgs/msg/PixhawkCMD.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8[3] cmd 3 | -------------------------------------------------------------------------------- /common_msgs/msg/PixhawkServo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[4] servo 3 | -------------------------------------------------------------------------------- /common_msgs/msg/Reference.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | int32 status 3 | uint32 m_id 4 | NavigationState navigation 5 | PixhawkCMD pixhawkCMD 6 | PixhawkServo pixhawkServo 7 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_DataInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 nodeId 4 | 5 | uint16 msgType 6 | uint16 msgId 7 | 8 | uint32 sourceId 9 | 10 | uint16 noise 11 | uint16 vPeak 12 | 13 | uint32 timestamp 14 | 15 | uint8 antennaId 16 | uint8 reserved 17 | 18 | uint16 dataSize 19 | uint8[] data 20 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_EchoedRangeInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint16 msgType 4 | uint16 msgId 5 | 6 | uint32 requesterId 7 | uint32 responderId 8 | 9 | uint32 precisionRangeMm 10 | uint16 precisionRangeErrEst 11 | 12 | uint16 ledFlags 13 | uint32 timestamp 14 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_FullNeighborDatabase.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 nodeId 4 | 5 | uint16 msgType 6 | uint16 msgId 7 | uint8 numNeighborEntries 8 | uint8 sortType 9 | uint16 reserved 10 | uint32 timestamp 11 | uint32 status 12 | UWB_FullNeighborDatabaseEntry[] neighbors 13 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_FullNeighborDatabaseEntry.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 nodeId 4 | uint8 rangeStatus 5 | uint8 antennaMode 6 | uint16 stopwatchTime 7 | uint32 rangeMm 8 | uint16 rangeErrorEstimate 9 | uint16 rangeVelocity 10 | uint8 rangeMeasurementType 11 | uint8 flags 12 | uint16 ledFlags 13 | uint16 noise 14 | uint16 vPeak 15 | uint16 statsNumRangeAttempts 16 | uint16 statsNumRangeSuccesses 17 | uint32 statsAgeMs 18 | uint32 rangeUpdateTimestampMs 19 | uint32 lastHeardTimestampMs 20 | uint32 addedToNDBTimestampMs 21 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_FullRangeInfo.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint32 nodeId 4 | 5 | uint16 msgType 6 | uint16 msgId 7 | 8 | uint32 responderId 9 | 10 | uint8 rangeStatus 11 | uint8 antennaMode 12 | uint16 stopwatchTime 13 | 14 | uint32 precisionRangeMm 15 | uint32 coarseRangeMm 16 | uint32 filteredRangeMm 17 | 18 | uint16 precisionRangeErrEst 19 | uint16 coarseRangeErrEst 20 | uint16 filteredRangeErrEst 21 | 22 | uint16 filteredRangeVel 23 | uint16 filteredRangeVelErrEst 24 | 25 | uint8 rangeMeasurementType 26 | uint8 reserved 27 | 28 | uint16 reqLEDFlags 29 | uint16 respLEDFlags 30 | 31 | uint16 noise 32 | uint16 vPeak 33 | 34 | uint32 coarseTOFInBins 35 | 36 | uint32 timestamp 37 | -------------------------------------------------------------------------------- /common_msgs/msg/UWB_SendData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8[] bytes 3 | float32[] floats 4 | -------------------------------------------------------------------------------- /common_msgs/msg/Waypoint.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | common_msgs/NavigationState navigation_state 3 | string m_type 4 | uint32 m_id 5 | -------------------------------------------------------------------------------- /common_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | common_msgs 4 | 0.0.0 5 | The common_msgs package 6 | 7 | 8 | 9 | 10 | jiaxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | angles 44 | geometry_msgs 45 | roscpp 46 | rospy 47 | sensor_msgs 48 | std_msgs 49 | tf 50 | message_generation 51 | angles 52 | geometry_msgs 53 | roscpp 54 | rospy 55 | sensor_msgs 56 | std_msgs 57 | tf 58 | message_generation 59 | message_runtime 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /uwb-imu-positioning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(uwb-imu-positioning) 3 | 4 | #set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}") 5 | 6 | find_package( 7 | OpenCV 8 | NO_DEFAULT_PATH 9 | PATHS /usr/local/share/OpenCV /opt/ros/kinetic/share/OpenCV-3.2.0 10 | ) 11 | 12 | set(CMAKE_BUILD_TYPE "Release") 13 | 14 | ## Find catkin macros and libraries 15 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 16 | ## is used, also find other catkin packages 17 | find_package(catkin REQUIRED COMPONENTS 18 | common_msgs REQUIRED 19 | visualization_msgs 20 | nav_msgs 21 | cv_bridge 22 | roscpp 23 | rospy 24 | std_msgs 25 | tf 26 | tf_conversions 27 | dynamic_reconfigure 28 | ) 29 | 30 | # pay attention that Pangolin should be modified and compiled with boost and c++99. 31 | 32 | find_library(Reflexxes_LIBRARIES flexxes ${PROJECT_SOURCE_DIR}/exLib/Reflexxes) 33 | set(Relfexxes_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/exLib/Reflexxes/Reflexxes) 34 | 35 | find_package(Boost REQUIRED COMPONENTS system thread iostreams chrono) 36 | 37 | find_package(Eigen3) 38 | find_package(Ceres REQUIRED) 39 | 40 | 41 | ################################### 42 | ## catkin specific configuration ## 43 | ################################### 44 | ## The catkin_package macro generates cmake config files for your package 45 | ## Declare things to be passed to dependent projects 46 | ## INCLUDE_DIRS: uncomment this if you package contains header files 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 49 | ## DEPENDS: system dependencies of this project that dependent projects also need 50 | catkin_package( 51 | INCLUDE_DIRS include 52 | LIBRARIES ${PROJECT_NAME} 53 | CATKIN_DEPENDS common_msgs roscpp rospy std_msgs tf 54 | DEPENDS 55 | ) 56 | 57 | ########### 58 | ## Build ## 59 | ########### 60 | 61 | 62 | 63 | include_directories( 64 | include 65 | ${catkin_INCLUDE_DIRS} 66 | ${Boost_INCLUDE_DIRS} 67 | ${OpenCV_INCLUDE_DIRS} 68 | ${PROJECT_SOURCE_DIR}/exLib 69 | ${Eigen_INCLUDE_DIRS} 70 | ${CERES_INCLUDE_DIRS} 71 | ) 72 | 73 | 74 | # with C++99, please use ORB_SLAM2_boost 75 | set(Library_Set 76 | ${OpenCV_LIBRARIES} 77 | ${catkin_LIBRARIES} 78 | ${Boost_LIBRARIES} 79 | ${CERES_LIBRARIES} 80 | ) 81 | 82 | 83 | 84 | ## Declare a C++ library 85 | file(GLOB_RECURSE HEADERS */*.hpp */*.h) 86 | 87 | ## Declare a C++ executable 88 | add_executable(uwb-imu-positioning 89 | ${HEADERS} # for qtcreator... 90 | 91 | src/localization_node.cc 92 | src/pose_local_parameterization.cc 93 | src/uwb_init_loc.cc 94 | src/uwb.cc 95 | 96 | ) 97 | 98 | ## Specify libraries to link a library or executable target against 99 | 100 | target_link_libraries(uwb-imu-positioning 101 | ${Library_Set} 102 | ) 103 | 104 | ## Add cmake target dependencies of the executable 105 | ## same as for the library above 106 | add_dependencies(uwb-imu-positioning ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 107 | 108 | 109 | file(GLOB_RECURSE EXTRA_FILES */*) 110 | add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES}) 111 | -------------------------------------------------------------------------------- /uwb-imu-positioning/LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /uwb-imu-positioning/include/anchor_coordinate_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef _ANCHOR_COORDINATE_H_ 2 | #define _ANCHOR_COORDINATE_H_ 3 | 4 | #include "Eigen/Core" 5 | #include "ceres/ceres.h" 6 | #include 7 | #include 8 | #include 9 | 10 | class CoordinateErrorTerm{ 11 | public: 12 | CoordinateErrorTerm(const std::map coordinate_constraint_map): 13 | m_coordinate_constraint_map(coordinate_constraint_map){} 14 | 15 | template 16 | bool operator()(const T* const x, const T* const y, const T* const z, 17 | T* residuals_ptr) const{ 18 | residuals_ptr[0] = T(0); 19 | 20 | std::map::iterator iter; 21 | for(iter=m_coordinate_constraint_map.begin();iter!=m_coordinate_constraint_map.end();++iter){ 22 | if(std::string("x")==iter->first){ 23 | residuals_ptr[0] += T(100) * (*x - T(iter->second)); 24 | } else if(std::string("y")==iter->first){ 25 | residuals_ptr[0] += T(100) * (*y - T(iter->second)); 26 | } else if(std::string("z")==iter->first){ 27 | residuals_ptr[0] += T(100) * (*z - T(iter->second)); 28 | } 29 | } 30 | 31 | return true; 32 | } 33 | 34 | static ceres::CostFunction* Create(const std::map coordinate_constraint_map){ 35 | return (new ceres::AutoDiffCostFunction( new CoordinateErrorTerm(coordinate_constraint_map) ) ); 36 | } 37 | 38 | private: 39 | mutable std::map m_coordinate_constraint_map; 40 | 41 | }; 42 | 43 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/anchor_distance_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef _ANCHOR_DISTANCE_FACTOR_H_ 2 | #define _ANCHOR_DISTANCE_FACTOR_H_ 3 | 4 | #include "Eigen/Core" 5 | #include "ceres/ceres.h" 6 | #include 7 | 8 | class PointLineErrorTerm{ 9 | public: 10 | PointLineErrorTerm(const double a0, const double a1, const double a2, 11 | const double n0, const double n1, const double n2){ 12 | a << a0, a1, a2; 13 | n << n0, n1, n2; 14 | n /= n.norm(); 15 | } 16 | 17 | template 18 | bool operator()(const T* const x, const T* const y, const T* const z, 19 | T* residuals_ptr) const{ 20 | Eigen::Matrix p(*x, *y, *z); 21 | 22 | Eigen::Matrix a_casted = a.cast(); 23 | Eigen::Matrix n_casted = n.cast(); 24 | Eigen::Matrix dist_vec = (a_casted-p) - ( (a_casted-p).dot(n_casted) ) * n_casted; 25 | 26 | if(dist_vec(0,0)==T(0) && dist_vec(1,0)==T(0) && dist_vec(2,0)==T(0)){ 27 | dist_vec(0,0) = T(1e-6); 28 | } 29 | 30 | residuals_ptr[0] = dist_vec.norm() * T(100); 31 | 32 | // avoid zero derivate for sqrt() 33 | const T threshold = T(1e-6); 34 | if(residuals_ptr[0]( new PointLineErrorTerm(a0, a1, a2, n0, n1, n2) ) ); 44 | } 45 | 46 | 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | private: 49 | Eigen::Vector3d a; 50 | Eigen::Vector3d n; 51 | }; 52 | 53 | 54 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/anchor_triangulation_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef _ANCHOR_TRIANGULATION_H_ 2 | #define _ANCHOR_TRIANGULATION_H_ 3 | 4 | #include "Eigen/Core" 5 | #include "ceres/ceres.h" 6 | #include 7 | 8 | 9 | class AnchorTriangulationErrorTerm{ 10 | public: 11 | AnchorTriangulationErrorTerm(const double anchor_x, const double anchor_y, const double anchor_z, 12 | const double r, const double covariance): 13 | m_r(r), m_covariance(covariance){ 14 | m_anchor << anchor_x, anchor_y, anchor_z; 15 | } 16 | 17 | 18 | template 19 | bool operator()(const T* const x, const T* const y, const T* const z, 20 | T* residuals_ptr) const 21 | { 22 | const Eigen::Matrix anchor = m_anchor.cast(); 23 | const Eigen::Matrix mobile(*x, *y, *z); 24 | Eigen::Matrix diff = anchor - mobile; 25 | if(diff(0,0)==T(0) && diff(1,0)==T(0) && diff(2,0)==T(0)){ 26 | diff(0,0) = T(1e-6); 27 | } 28 | 29 | residuals_ptr[0] = ( ceres::abs( diff.norm() - T(m_r) ) ) * T(1.0/std::sqrt(m_covariance)); 30 | 31 | // avoid zero derivate for sqrt() 32 | const T threshold = T(1e-6); 33 | if(residuals_ptr[0] 43 | ( new AnchorTriangulationErrorTerm(anchor_x, anchor_y, anchor_z, r, covariance ) ) 44 | ); 45 | } 46 | 47 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 48 | 49 | private: 50 | Eigen::Vector3d m_anchor; 51 | const double m_r, m_covariance; 52 | }; 53 | 54 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/basic_function.h: -------------------------------------------------------------------------------- 1 | #ifndef _BASIC_FUNCTION_H_ 2 | #define _BASIC_FUNCTION_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | template 11 | inline std::string num2str(const T src){ 12 | std::stringstream ss; 13 | ss << src; 14 | return ss.str(); 15 | } 16 | 17 | // vector arithematic 18 | template 19 | inline boost::array vectorSubtraction(const boost::array &a, const boost::array &b){ 20 | boost::array dst; 21 | for(int i=0;i 27 | inline boost::array vectorAddition(const boost::array &a, const boost::array &b){ 28 | boost::array dst; 29 | for(int i=0;i 35 | inline void setZeroVector(boost::array &a){ 36 | for(int i=0;i 43 | inline std::vector vectorSubtraction(const std::vector &a, const std::vector &b){ 44 | std::vector dst; 45 | for(int i=0;i 51 | inline std::vector vectorAddition(const std::vector &a, const std::vector &b){ 52 | std::vector dst; 53 | for(int i=0;i 59 | inline void setZeroVector(std::vector &a){ 60 | for(int i=0;i 5 | #include 6 | #include 7 | 8 | #include "utility.h" 9 | #include "pre_integration.h" 10 | 11 | #include 12 | 13 | 14 | class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 15 | { 16 | public: 17 | IMUFactor() = delete; 18 | IMUFactor(IntegrationBase* _pre_integration):pre_integration(_pre_integration) 19 | { 20 | ACC_N = 0.1; 21 | ACC_W = 0.01; 22 | GYR_N = 0.01; 23 | GYR_W = 2.0e-5; 24 | 25 | G = Eigen::Vector3d(0.0,0.0,9.805); 26 | } 27 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 28 | { 29 | 30 | Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); 31 | Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]); 32 | 33 | Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]); 34 | Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]); 35 | Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]); 36 | 37 | Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]); 38 | Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]); 39 | 40 | Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]); 41 | Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]); 42 | Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]); 43 | 44 | //Eigen::Matrix Fd; 45 | //Eigen::Matrix Gd; 46 | 47 | //Eigen::Vector3d pPj = Pi + Vi * sum_t - 0.5 * g * sum_t * sum_t + corrected_delta_p; 48 | //Eigen::Quaterniond pQj = Qi * delta_q; 49 | //Eigen::Vector3d pVj = Vi - g * sum_t + corrected_delta_v; 50 | //Eigen::Vector3d pBaj = Bai; 51 | //Eigen::Vector3d pBgj = Bgi; 52 | 53 | //Vi + Qi * delta_v - g * sum_dt = Vj; 54 | //Qi * delta_q = Qj; 55 | 56 | //delta_p = Qi.inverse() * (0.5 * g * sum_dt * sum_dt + Pj - Pi); 57 | //delta_v = Qi.inverse() * (g * sum_dt + Vj - Vi); 58 | //delta_q = Qi.inverse() * Qj; 59 | 60 | #if 0 61 | if ((Bai - pre_integration->linearized_ba).norm() > 0.10 || 62 | (Bgi - pre_integration->linearized_bg).norm() > 0.01) 63 | { 64 | pre_integration->repropagate(Bai, Bgi); 65 | } 66 | #endif 67 | 68 | Eigen::Map> residual(residuals); 69 | residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi, 70 | Pj, Qj, Vj, Baj, Bgj); 71 | 72 | Eigen::Matrix sqrt_info = Eigen::LLT>(pre_integration->covariance.inverse()).matrixL().transpose(); 73 | //sqrt_info.setIdentity(); 74 | residual = sqrt_info * residual; 75 | 76 | if (jacobians) 77 | { 78 | double sum_dt = pre_integration->sum_dt; 79 | Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA); 80 | Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG); 81 | 82 | Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG); 83 | 84 | Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA); 85 | Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG); 86 | 87 | if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8) 88 | { 89 | ROS_WARN("numerical unstable in preintegration"); 90 | //std::cout << pre_integration->jacobian << std::endl; 91 | /// ROS_BREAK(); 92 | } 93 | 94 | if (jacobians[0]) 95 | { 96 | Eigen::Map> jacobian_pose_i(jacobians[0]); 97 | jacobian_pose_i.setZero(); 98 | 99 | jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix(); 100 | jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt)); 101 | 102 | #if 0 103 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix(); 104 | #else 105 | Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 106 | jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>(); 107 | #endif 108 | 109 | jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi)); 110 | 111 | jacobian_pose_i = sqrt_info * jacobian_pose_i; 112 | 113 | if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8) 114 | { 115 | ROS_WARN("numerical unstable in preintegration"); 116 | //std::cout << sqrt_info << std::endl; 117 | //ROS_BREAK(); 118 | } 119 | } 120 | if (jacobians[1]) 121 | { 122 | Eigen::Map> jacobian_speedbias_i(jacobians[1]); 123 | jacobian_speedbias_i.setZero(); 124 | jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt; 125 | jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba; 126 | jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg; 127 | 128 | #if 0 129 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg; 130 | #else 131 | //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 132 | //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg; 133 | jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg; 134 | #endif 135 | 136 | jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix(); 137 | jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba; 138 | jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg; 139 | 140 | jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity(); 141 | 142 | jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity(); 143 | 144 | jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i; 145 | 146 | //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8); 147 | //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8); 148 | } 149 | if (jacobians[2]) 150 | { 151 | Eigen::Map> jacobian_pose_j(jacobians[2]); 152 | jacobian_pose_j.setZero(); 153 | 154 | jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix(); 155 | 156 | #if 0 157 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity(); 158 | #else 159 | Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg)); 160 | jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>(); 161 | #endif 162 | 163 | jacobian_pose_j = sqrt_info * jacobian_pose_j; 164 | 165 | //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8); 166 | //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8); 167 | } 168 | if (jacobians[3]) 169 | { 170 | Eigen::Map> jacobian_speedbias_j(jacobians[3]); 171 | jacobian_speedbias_j.setZero(); 172 | 173 | jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix(); 174 | 175 | jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity(); 176 | 177 | jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity(); 178 | 179 | jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j; 180 | 181 | //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8); 182 | //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8); 183 | } 184 | } 185 | 186 | return true; 187 | } 188 | 189 | //bool Evaluate_Direct(double const *const *parameters, Eigen::Matrix &residuals, Eigen::Matrix &jacobians); 190 | 191 | //void checkCorrection(); 192 | //void checkTransition(); 193 | //void checkJacobian(double **parameters); 194 | double ACC_N; 195 | double ACC_W; 196 | double GYR_N; 197 | double GYR_W; 198 | 199 | Eigen::Vector3d G; 200 | 201 | IntegrationBase* pre_integration; 202 | 203 | 204 | }; 205 | 206 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/localization_node.h: -------------------------------------------------------------------------------- 1 | #ifndef _LOCALIZATION_NODE_H_ 2 | #define _LOCALIZATION_NODE_H_ 3 | 4 | #include "uwb.h" 5 | #include "uwb_init_loc.h" 6 | #include "utility.h" 7 | #include "parameter.h" 8 | 9 | class Localization_Node{ 10 | public: 11 | explicit Localization_Node(ros::NodeHandle& nh); 12 | ~Localization_Node(){ 13 | } 14 | 15 | private: 16 | ros::NodeHandle m_nh; 17 | ros::Subscriber m_ndb_sub; 18 | ros::Subscriber m_imu_info_sub; 19 | ros::Publisher m_uwb_odometry_pub; 20 | ros::Timer m_print_timer; 21 | 22 | double m_last_imu_t; 23 | 24 | double m_slam_fps; 25 | bool m_is_fix_fps; 26 | bool m_is_initialize_with_ceres; 27 | double m_td; 28 | double m_current_time; 29 | 30 | int m_mobile_id; 31 | boost::shared_ptr m_p_mobile; 32 | geometry_msgs::Pose m_position; 33 | geometry_msgs::Twist m_velocity; 34 | 35 | std::vector m_anchor_list; 36 | std::map > m_anchor_map; 37 | 38 | std::queue m_imu_buf; 39 | std::queue m_ndb_buf; 40 | std::mutex m_buf; 41 | std::mutex m_process; 42 | std::condition_variable m_con; 43 | 44 | 45 | public: 46 | std::vector, common_msgs::UWB_FullNeighborDatabase::ConstPtr>> getMeasurements(); 47 | void createUWBMobile(const int mobile_id); 48 | void imuInfoCallback(const sensor_msgs::ImuConstPtr &imu_msg); 49 | void ndbCallback(const common_msgs::UWB_FullNeighborDatabase::ConstPtr& msg); 50 | void printLocalizationCallback(const ros::TimerEvent& event); 51 | bool getValidNeighborDatabse(const common_msgs::UWB_FullNeighborDatabase &ndb); 52 | void solveSLAM(const ros::TimerEvent& event); 53 | void process(); 54 | void pubOdometry(double header); 55 | }; 56 | 57 | 58 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/parameter.h: -------------------------------------------------------------------------------- 1 | #ifndef _PARAMETER_H_ 2 | #define _PARAMETER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // extern double ACC_N, ACC_W; 10 | // extern double GYR_N, GYR_W; 11 | 12 | // extern Eigen::Vector3d G; 13 | 14 | // extern double BIAS_ACC_THRESHOLD; 15 | // extern double BIAS_GYR_THRESHOLD; 16 | 17 | 18 | enum SIZE_PARAMETERIZATION 19 | { 20 | SIZE_POSE = 7, 21 | SIZE_SPEEDBIAS = 9, 22 | SIZE_FEATURE = 1 23 | }; 24 | 25 | enum StateOrder 26 | { 27 | O_P = 0, 28 | O_R = 3, 29 | O_V = 6, 30 | O_BA = 9, 31 | O_BG = 12 32 | }; 33 | 34 | enum NoiseOrder 35 | { 36 | O_AN = 0, 37 | O_GN = 3, 38 | O_AW = 6, 39 | O_GW = 9 40 | }; 41 | 42 | 43 | 44 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | #include "utility.h" 6 | 7 | class PoseLocalParameterization : public ceres::LocalParameterization 8 | { 9 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 10 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 11 | virtual int GlobalSize() const { return 7; }; 12 | virtual int LocalSize() const { return 6; }; 13 | }; -------------------------------------------------------------------------------- /uwb-imu-positioning/include/pre_integration.h: -------------------------------------------------------------------------------- 1 | #ifndef _PRE_INTEGRATION_H_ 2 | #define _PRE_INTEGRATION_H_ 3 | 4 | #include "utility.h" 5 | #include "parameter.h" 6 | 7 | #include 8 | using namespace Eigen; 9 | 10 | class IntegrationBase 11 | { 12 | public: 13 | IntegrationBase() = delete; 14 | IntegrationBase(const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 15 | const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) 16 | : acc_0{_acc_0}, gyr_0{_gyr_0}, linearized_acc{_acc_0}, linearized_gyr{_gyr_0}, 17 | linearized_ba{_linearized_ba}, linearized_bg{_linearized_bg}, 18 | jacobian{Eigen::Matrix::Identity()}, covariance{Eigen::Matrix::Zero()}, 19 | sum_dt{0.0}, delta_p{Eigen::Vector3d::Zero()}, delta_q{Eigen::Quaterniond::Identity()}, delta_v{Eigen::Vector3d::Zero()} 20 | 21 | { 22 | ACC_N = 0.1; 23 | ACC_W = 0.01; 24 | GYR_N = 0.01; 25 | GYR_W = 2.0e-5; 26 | 27 | G = Eigen::Vector3d(0.0,0.0,9.805); 28 | 29 | noise = Eigen::Matrix::Zero(); 30 | noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); 31 | noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); 32 | noise.block<3, 3>(6, 6) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); 33 | noise.block<3, 3>(9, 9) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); 34 | noise.block<3, 3>(12, 12) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); 35 | noise.block<3, 3>(15, 15) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); 36 | } 37 | 38 | void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) 39 | { 40 | dt_buf.push_back(dt); 41 | acc_buf.push_back(acc); 42 | gyr_buf.push_back(gyr); 43 | propagate(dt, acc, gyr); 44 | } 45 | 46 | void repropagate(const Eigen::Vector3d &_linearized_ba, const Eigen::Vector3d &_linearized_bg) 47 | { 48 | sum_dt = 0.0; 49 | acc_0 = linearized_acc; 50 | gyr_0 = linearized_gyr; 51 | delta_p.setZero(); 52 | delta_q.setIdentity(); 53 | delta_v.setZero(); 54 | linearized_ba = _linearized_ba; 55 | linearized_bg = _linearized_bg; 56 | jacobian.setIdentity(); 57 | covariance.setZero(); 58 | for (int i = 0; i < static_cast(dt_buf.size()); i++) 59 | propagate(dt_buf[i], acc_buf[i], gyr_buf[i]); 60 | } 61 | 62 | void midPointIntegration(double _dt, 63 | const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 64 | const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, 65 | const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v, 66 | const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg, 67 | Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v, 68 | Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian) 69 | { 70 | //ROS_INFO("midpoint integration"); 71 | Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba); 72 | Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; 73 | result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2); 74 | Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba); 75 | Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); 76 | result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt; 77 | result_delta_v = delta_v + un_acc * _dt; 78 | result_linearized_ba = linearized_ba; 79 | result_linearized_bg = linearized_bg; 80 | 81 | if(update_jacobian) 82 | { 83 | Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg; 84 | Vector3d a_0_x = _acc_0 - linearized_ba; 85 | Vector3d a_1_x = _acc_1 - linearized_ba; 86 | Matrix3d R_w_x, R_a_0_x, R_a_1_x; 87 | 88 | R_w_x<<0, -w_x(2), w_x(1), 89 | w_x(2), 0, -w_x(0), 90 | -w_x(1), w_x(0), 0; 91 | R_a_0_x<<0, -a_0_x(2), a_0_x(1), 92 | a_0_x(2), 0, -a_0_x(0), 93 | -a_0_x(1), a_0_x(0), 0; 94 | R_a_1_x<<0, -a_1_x(2), a_1_x(1), 95 | a_1_x(2), 0, -a_1_x(0), 96 | -a_1_x(1), a_1_x(0), 0; 97 | 98 | MatrixXd F = MatrixXd::Zero(15, 15); 99 | F.block<3, 3>(0, 0) = Matrix3d::Identity(); 100 | F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt + 101 | -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt; 102 | F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt; 103 | F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt; 104 | F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt; 105 | F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt; 106 | F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt; 107 | F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt + 108 | -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt; 109 | F.block<3, 3>(6, 6) = Matrix3d::Identity(); 110 | F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt; 111 | F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt; 112 | F.block<3, 3>(9, 9) = Matrix3d::Identity(); 113 | F.block<3, 3>(12, 12) = Matrix3d::Identity(); 114 | //cout<<"A"<(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt; 118 | V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt; 119 | V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt; 120 | V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3); 121 | V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt; 122 | V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt; 123 | V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt; 124 | V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt; 125 | V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt; 126 | V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3); 127 | V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt; 128 | V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt; 129 | 130 | //step_jacobian = F; 131 | //step_V = V; 132 | jacobian = F * jacobian; 133 | covariance = F * covariance * F.transpose() + V * noise * V.transpose(); 134 | } 135 | 136 | } 137 | 138 | void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1) 139 | { 140 | dt = _dt; 141 | acc_1 = _acc_1; 142 | gyr_1 = _gyr_1; 143 | Vector3d result_delta_p; 144 | Quaterniond result_delta_q; 145 | Vector3d result_delta_v; 146 | Vector3d result_linearized_ba; 147 | Vector3d result_linearized_bg; 148 | 149 | midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v, 150 | linearized_ba, linearized_bg, 151 | result_delta_p, result_delta_q, result_delta_v, 152 | result_linearized_ba, result_linearized_bg, 1); 153 | 154 | //checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v, 155 | // linearized_ba, linearized_bg); 156 | delta_p = result_delta_p; 157 | delta_q = result_delta_q; 158 | delta_v = result_delta_v; 159 | linearized_ba = result_linearized_ba; 160 | linearized_bg = result_linearized_bg; 161 | delta_q.normalize(); 162 | sum_dt += dt; 163 | acc_0 = acc_1; 164 | gyr_0 = gyr_1; 165 | 166 | } 167 | 168 | Eigen::Matrix evaluate(const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi, 169 | const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj) 170 | { 171 | Eigen::Matrix residuals; 172 | 173 | Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA); 174 | Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG); 175 | 176 | Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG); 177 | 178 | Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA); 179 | Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_BG); 180 | 181 | Eigen::Vector3d dba = Bai - linearized_ba; 182 | Eigen::Vector3d dbg = Bgi - linearized_bg; 183 | 184 | Eigen::Quaterniond corrected_delta_q = delta_q * Utility::deltaQ(dq_dbg * dbg); 185 | Eigen::Vector3d corrected_delta_v = delta_v + dv_dba * dba + dv_dbg * dbg; 186 | Eigen::Vector3d corrected_delta_p = delta_p + dp_dba * dba + dp_dbg * dbg; 187 | 188 | residuals.block<3, 1>(O_P, 0) = Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt) - corrected_delta_p; 189 | residuals.block<3, 1>(O_R, 0) = 2 * (corrected_delta_q.inverse() * (Qi.inverse() * Qj)).vec(); 190 | residuals.block<3, 1>(O_V, 0) = Qi.inverse() * (G * sum_dt + Vj - Vi) - corrected_delta_v; 191 | residuals.block<3, 1>(O_BA, 0) = Baj - Bai; 192 | residuals.block<3, 1>(O_BG, 0) = Bgj - Bgi; 193 | return residuals; 194 | } 195 | 196 | double dt; 197 | Eigen::Vector3d acc_0, gyr_0; 198 | Eigen::Vector3d acc_1, gyr_1; 199 | 200 | const Eigen::Vector3d linearized_acc, linearized_gyr; 201 | Eigen::Vector3d linearized_ba, linearized_bg; 202 | 203 | Eigen::Matrix jacobian, covariance; 204 | Eigen::Matrix step_jacobian; 205 | Eigen::Matrix step_V; 206 | Eigen::Matrix noise; 207 | 208 | double sum_dt; 209 | Eigen::Vector3d delta_p; 210 | Eigen::Quaterniond delta_q; 211 | Eigen::Vector3d delta_v; 212 | 213 | std::vector dt_buf; 214 | std::vector acc_buf; 215 | std::vector gyr_buf; 216 | 217 | double ACC_N; 218 | double ACC_W; 219 | double GYR_N; 220 | double GYR_W; 221 | 222 | Eigen::Vector3d G; 223 | 224 | }; 225 | /* 226 | void eulerIntegration(double _dt, const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, 227 | const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, 228 | const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v, 229 | const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg, 230 | Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v, 231 | Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian) 232 | { 233 | result_delta_p = delta_p + delta_v * _dt + 0.5 * (delta_q * (_acc_1 - linearized_ba)) * _dt * _dt; 234 | result_delta_v = delta_v + delta_q * (_acc_1 - linearized_ba) * _dt; 235 | Vector3d omg = _gyr_1 - linearized_bg; 236 | omg = omg * _dt / 2; 237 | Quaterniond dR(1, omg(0), omg(1), omg(2)); 238 | result_delta_q = (delta_q * dR); 239 | result_linearized_ba = linearized_ba; 240 | result_linearized_bg = linearized_bg; 241 | if(update_jacobian) 242 | { 243 | Vector3d w_x = _gyr_1 - linearized_bg; 244 | Vector3d a_x = _acc_1 - linearized_ba; 245 | Matrix3d R_w_x, R_a_x; 246 | R_w_x<<0, -w_x(2), w_x(1), 247 | w_x(2), 0, -w_x(0), 248 | -w_x(1), w_x(0), 0; 249 | R_a_x<<0, -a_x(2), a_x(1), 250 | a_x(2), 0, -a_x(0), 251 | -a_x(1), a_x(0), 0; 252 | MatrixXd A = MatrixXd::Zero(15, 15); 253 | // one step euler 0.5 254 | A.block<3, 3>(0, 3) = 0.5 * (-1 * delta_q.toRotationMatrix()) * R_a_x * _dt; 255 | A.block<3, 3>(0, 6) = MatrixXd::Identity(3,3); 256 | A.block<3, 3>(0, 9) = 0.5 * (-1 * delta_q.toRotationMatrix()) * _dt; 257 | A.block<3, 3>(3, 3) = -R_w_x; 258 | A.block<3, 3>(3, 12) = -1 * MatrixXd::Identity(3,3); 259 | A.block<3, 3>(6, 3) = (-1 * delta_q.toRotationMatrix()) * R_a_x; 260 | A.block<3, 3>(6, 9) = (-1 * delta_q.toRotationMatrix()); 261 | //cout<<"A"<(0, 0) = 0.5 * delta_q.toRotationMatrix() * _dt; 264 | U.block<3, 3>(3, 3) = MatrixXd::Identity(3,3); 265 | U.block<3, 3>(6, 0) = delta_q.toRotationMatrix(); 266 | U.block<3, 3>(9, 6) = MatrixXd::Identity(3,3); 267 | U.block<3, 3>(12, 9) = MatrixXd::Identity(3,3); 268 | // put outside 269 | Eigen::Matrix noise = Eigen::Matrix::Zero(); 270 | noise.block<3, 3>(0, 0) = (ACC_N * ACC_N) * Eigen::Matrix3d::Identity(); 271 | noise.block<3, 3>(3, 3) = (GYR_N * GYR_N) * Eigen::Matrix3d::Identity(); 272 | noise.block<3, 3>(6, 6) = (ACC_W * ACC_W) * Eigen::Matrix3d::Identity(); 273 | noise.block<3, 3>(9, 9) = (GYR_W * GYR_W) * Eigen::Matrix3d::Identity(); 274 | //write F directly 275 | MatrixXd F, V; 276 | F = (MatrixXd::Identity(15,15) + _dt * A); 277 | V = _dt * U; 278 | step_jacobian = F; 279 | step_V = V; 280 | jacobian = F * jacobian; 281 | covariance = F * covariance * F.transpose() + V * noise * V.transpose(); 282 | } 283 | } */ 284 | 285 | #endif 286 | -------------------------------------------------------------------------------- /uwb-imu-positioning/include/tic_toc.h: -------------------------------------------------------------------------------- 1 | #ifndef _TIC_TOC_H_ 2 | #define _TIC_TOC_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class TicToc 9 | { 10 | public: 11 | TicToc() 12 | { 13 | tic(); 14 | } 15 | 16 | void tic() 17 | { 18 | start = std::chrono::system_clock::now(); 19 | } 20 | 21 | double toc() 22 | { 23 | end = std::chrono::system_clock::now(); 24 | std::chrono::duration elapsed_seconds = end - start; 25 | return elapsed_seconds.count() * 1000; 26 | } 27 | 28 | private: 29 | std::chrono::time_point start, end; 30 | }; 31 | 32 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/utility.h: -------------------------------------------------------------------------------- 1 | #ifndef _UTILITY_H_ 2 | #define _UTILITY_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | class Utility 15 | { 16 | public: 17 | 18 | template 19 | static Eigen::Quaternion deltaQ(const Eigen::MatrixBase &theta) 20 | { 21 | typedef typename Derived::Scalar Scalar_t; 22 | 23 | Eigen::Quaternion dq; 24 | Eigen::Matrix half_theta = theta; 25 | half_theta /= static_cast(2.0); 26 | dq.w() = static_cast(1.0); 27 | dq.x() = half_theta.x(); 28 | dq.y() = half_theta.y(); 29 | dq.z() = half_theta.z(); 30 | return dq; 31 | } 32 | 33 | template 34 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase &q) 35 | { 36 | Eigen::Matrix ans; 37 | ans << typename Derived::Scalar(0), -q(2), q(1), 38 | q(2), typename Derived::Scalar(0), -q(0), 39 | -q(1), q(0), typename Derived::Scalar(0); 40 | return ans; 41 | } 42 | 43 | template 44 | static Eigen::Quaternion positify(const Eigen::QuaternionBase &q) 45 | { 46 | //printf("a: %f %f %f %f", q.w(), q.x(), q.y(), q.z()); 47 | //Eigen::Quaternion p(-q.w(), -q.x(), -q.y(), -q.z()); 48 | //printf("b: %f %f %f %f", p.w(), p.x(), p.y(), p.z()); 49 | //return q.template w() >= (typename Derived::Scalar)(0.0) ? q : Eigen::Quaternion(-q.w(), -q.x(), -q.y(), -q.z()); 50 | return q; 51 | } 52 | 53 | template 54 | static Eigen::Matrix Qleft(const Eigen::QuaternionBase &q) 55 | { 56 | Eigen::Quaternion qq = positify(q); 57 | Eigen::Matrix ans; 58 | ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose(); 59 | ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix::Identity() + skewSymmetric(qq.vec()); 60 | return ans; 61 | } 62 | 63 | template 64 | static Eigen::Matrix Qright(const Eigen::QuaternionBase &p) 65 | { 66 | Eigen::Quaternion pp = positify(p); 67 | Eigen::Matrix ans; 68 | ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose(); 69 | ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix::Identity() - skewSymmetric(pp.vec()); 70 | return ans; 71 | } 72 | 73 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 74 | { 75 | Eigen::Vector3d n = R.col(0); 76 | Eigen::Vector3d o = R.col(1); 77 | Eigen::Vector3d a = R.col(2); 78 | 79 | Eigen::Vector3d ypr(3); 80 | double y = atan2(n(1), n(0)); 81 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 82 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 83 | ypr(0) = y; 84 | ypr(1) = p; 85 | ypr(2) = r; 86 | 87 | return ypr / M_PI * 180.0; 88 | } 89 | 90 | template 91 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 92 | { 93 | typedef typename Derived::Scalar Scalar_t; 94 | 95 | Scalar_t y = ypr(0) / 180.0 * M_PI; 96 | Scalar_t p = ypr(1) / 180.0 * M_PI; 97 | Scalar_t r = ypr(2) / 180.0 * M_PI; 98 | 99 | Eigen::Matrix Rz; 100 | Rz << cos(y), -sin(y), 0, 101 | sin(y), cos(y), 0, 102 | 0, 0, 1; 103 | 104 | Eigen::Matrix Ry; 105 | Ry << cos(p), 0., sin(p), 106 | 0., 1., 0., 107 | -sin(p), 0., cos(p); 108 | 109 | Eigen::Matrix Rx; 110 | Rx << 1., 0., 0., 111 | 0., cos(r), -sin(r), 112 | 0., sin(r), cos(r); 113 | 114 | return Rz * Ry * Rx; 115 | } 116 | 117 | static Eigen::Matrix3d g2R(const Eigen::Vector3d &g) 118 | { 119 | Eigen::Matrix3d R0; 120 | Eigen::Vector3d ng1 = g.normalized(); 121 | Eigen::Vector3d ng2{0, 0, 1.0}; 122 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 123 | double yaw = Utility::R2ypr(R0).x(); 124 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 125 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 126 | return R0; 127 | } 128 | 129 | template 130 | struct uint_ 131 | { 132 | }; 133 | 134 | template 135 | void unroller(const Lambda &f, const IterT &iter, uint_) 136 | { 137 | unroller(f, iter, uint_()); 138 | f(iter + N); 139 | } 140 | 141 | template 142 | void unroller(const Lambda &f, const IterT &iter, uint_<0>) 143 | { 144 | f(iter); 145 | } 146 | 147 | template 148 | static T normalizeAngle(const T& angle_degrees) { 149 | T two_pi(2.0 * 180); 150 | if (angle_degrees > 0) 151 | return angle_degrees - 152 | two_pi * std::floor((angle_degrees + T(180)) / two_pi); 153 | else 154 | return angle_degrees + 155 | two_pi * std::floor((-angle_degrees + T(180)) / two_pi); 156 | }; 157 | }; 158 | 159 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/uwb.h: -------------------------------------------------------------------------------- 1 | #ifndef _UWB_H_ 2 | #define _UWB_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "ros/ros.h" 17 | #include "geometry_msgs/PointStamped.h" 18 | #include "common_msgs/UWB_FullRangeInfo.h" 19 | #include "common_msgs/UWB_FullNeighborDatabase.h" 20 | #include "common_msgs/UWB_DataInfo.h" 21 | #include "common_msgs/UWB_SendData.h" 22 | #include "common_msgs/UWB_EchoedRangeInfo.h" 23 | #include "common_msgs/NavigationState.h" 24 | #include "pose_local_parameterization.h" 25 | #include "basic_function.h" 26 | #include "utility.h" 27 | #include "imu_factor.h" 28 | #include "uwb_position_factor.h" 29 | #include "uwb_range_factor.h" 30 | 31 | 32 | typedef Eigen::Matrix Vector6d; 33 | typedef Eigen::Matrix Matrix6d; 34 | 35 | 36 | // ----------------------------- 37 | struct comp{ 38 | template 39 | inline bool operator()(const T& a, const T& b) const{ 40 | return (a 46 | double robustAverage(std::vector array){ 47 | // the paramter is copied, so that the original data will not be affected. 48 | const int cut_length = 2; 49 | 50 | if(array.size()<=2*cut_length){ 51 | return 0; 52 | } 53 | 54 | double sum = 0; 55 | std::sort(array.begin(), array.end(), comp()); 56 | for(int i=cut_length;i 65 | bool medianAndVariance(std::vector array, double& median, double& variance){ 66 | if(array.size()==0){ 67 | return false; 68 | } 69 | 70 | // get median 71 | std::sort(array.begin(), array.end(), comp()); 72 | median = array.at(std::floor(array.size()/2)); 73 | 74 | // get mean 75 | double sum = 0; 76 | for(int i=0;i> m_range_map; 163 | 164 | void insertRangeInfo(const common_msgs::UWB_EchoedRangeInfo& msg); 165 | void insertRangeInfo(const common_msgs::UWB_FullRangeInfo& msg); 166 | double getAverageRangeToNode(const int anchor_id); 167 | bool getMedianAndVarianceToNode(const int anchor_id, double& median, double& variance); 168 | 169 | inline void printPosition(){ 170 | std::cout< > anchor_map, 180 | ros::NodeHandle& nh); 181 | 182 | private: 183 | // stores last correct/accepted measurements 184 | std::map m_ranges_to_anchors; 185 | ros::Time m_last_range_time; 186 | 187 | Eigen::Vector3d m_last_position; 188 | Eigen::Vector3d m_last_velocity; 189 | Eigen::Matrix3d m_last_rotation; 190 | 191 | Eigen::Vector3d m_pred_position; 192 | Eigen::Vector3d m_pred_velocity; 193 | Eigen::Matrix3d m_pred_rotation; 194 | 195 | 196 | // provided by parameter server 197 | std::map > m_anchor_map; 198 | Eigen::Vector3d m_g; 199 | 200 | Eigen::Vector3d m_acceleration_bias; 201 | Eigen::Vector3d m_angular_bias; 202 | Eigen::Matrix3d m_rotation; 203 | 204 | std::vector m_td_buf; 205 | std::vector m_linear_acceleration_buf; 206 | std::vector m_angular_velocity_buf; 207 | 208 | Eigen::Vector3d acc_0, gyr_0; 209 | 210 | 211 | //imu 212 | IntegrationBase *tmp_pre_integration; 213 | IntegrationBase *pre_integration; 214 | bool m_first_imu; 215 | 216 | // parameters 217 | ros::NodeHandle m_nh; 218 | std::string m_filter_type; 219 | 220 | 221 | public: 222 | // reading statistics, to get effective frequency 223 | std::map m_anchor_reading_counter; 224 | std::map m_anchor_successful_reading_counter; 225 | int m_least_anchor_num; 226 | 227 | public: 228 | 229 | inline void setLastPosition(const Eigen::Vector3d position) 230 | { 231 | m_last_position = position; 232 | m_pred_position = position; 233 | } 234 | 235 | inline void setLastVelocity(const Eigen::Vector3d velocity) 236 | { 237 | m_last_velocity = velocity; 238 | m_pred_velocity = velocity; 239 | } 240 | 241 | inline void setLastRotation(const Eigen::Matrix3d rotation) 242 | { 243 | m_last_rotation = rotation; 244 | m_pred_rotation = rotation; 245 | } 246 | 247 | 248 | inline void setAngularBias(const Eigen::Vector3d angular_bias){ 249 | m_angular_bias = angular_bias; 250 | } 251 | 252 | inline Eigen::Vector3d getAngularBias() const 253 | { 254 | return m_angular_bias; 255 | 256 | } 257 | 258 | inline void setAccelerationBias(const Eigen::Vector3d acceleration_bias){ 259 | m_acceleration_bias = acceleration_bias; 260 | 261 | } 262 | 263 | inline Eigen::Vector3d getAccelerationBias() const 264 | { 265 | return m_acceleration_bias; 266 | 267 | } 268 | 269 | inline common_msgs::UWB_FullRangeInfo getRangeInfoToAnchor(const int anchor_id) const{ 270 | return m_ranges_to_anchors.find(anchor_id)->second; 271 | } 272 | inline double getRangeMeterToAnchor(const int anchor_id) const{ 273 | return double(m_ranges_to_anchors.find(anchor_id)->second.precisionRangeMm) / 1000.0; 274 | } 275 | 276 | inline void setRangeInfoToAnchor(const common_msgs::UWB_FullRangeInfo& range_info){ 277 | m_ranges_to_anchors[range_info.responderId] = range_info; 278 | } 279 | 280 | inline bool getAnchorPosition(const int anchor_id, geometry_msgs::PointStamped& anchor_position) const { 281 | if(m_anchor_map.find(anchor_id)!=m_anchor_map.end()){ 282 | anchor_position = m_anchor_map.find(anchor_id)->second->getPosition(); 283 | return true; 284 | } else { 285 | return false; 286 | } 287 | } 288 | inline int getAnchorNumber() const{ 289 | return m_anchor_map.size(); 290 | } 291 | 292 | 293 | void fullInitialize(const common_msgs::UWB_FullNeighborDatabase& ndb, const geometry_msgs::PointStamped& position); 294 | double getAverageRangeErrEst(); 295 | 296 | // dynamic innovation threshold, to avoid filter divergence 297 | void getInnovation(); 298 | void processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); 299 | bool filter3DUpdate(); 300 | bool tightlyFGOUpdate(); 301 | bool looselyFGOUpdate(); 302 | void doubleToVector(); 303 | void vectorToDouble(); 304 | double para_pose[2][7]; 305 | double para_speed_bias[2][9]; 306 | 307 | 308 | // print 309 | inline void printPosition(){ 310 | std::cout< p_mobile): 14 | m_ndb(ndb),m_p_mobile(p_mobile){ 15 | m_mobile_position = m_p_mobile->getPosition(); 16 | 17 | } 18 | ~UWB_Loc_Init(){ 19 | ROS_INFO("UWB Localization Initialization module is now destructed."); 20 | } 21 | 22 | private: 23 | boost::shared_ptr m_p_mobile; 24 | geometry_msgs::PointStamped m_mobile_position; 25 | common_msgs::UWB_FullNeighborDatabase m_ndb; 26 | ceres::Problem m_problem; 27 | 28 | public: 29 | 30 | bool initializeMobileByCeres(); 31 | void buildOptimizationProblem(const common_msgs::UWB_FullNeighborDatabase& ndb); 32 | bool solveOptimizationProblem(); 33 | }; 34 | 35 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/uwb_position_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef _UWB_POSITION_FACTOR_H_ 2 | #define _UWB_POSITION_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "utility.h" 9 | #include 10 | 11 | class UWBPositionFactor{ 12 | public: 13 | UWBPositionFactor(const double uwb_x, const double uwb_y, const double uwb_z, 14 | const double covariance): 15 | m_covariance(covariance){ 16 | m_uwb_position << uwb_x, uwb_y, uwb_z; 17 | } 18 | 19 | 20 | template 21 | bool operator()(const T* const x, const T* const y, const T* const z, 22 | T* residuals_ptr) const{ 23 | const Eigen::Matrix uwb = m_uwb_position.cast(); 24 | const Eigen::Matrix mobile(*x, *y, *z); 25 | Eigen::Matrix diff = uwb - mobile; 26 | if(diff(0,0)==T(0) && diff(1,0)==T(0) && diff(2,0)==T(0)){ 27 | diff(0,0) = T(1e-6); 28 | } 29 | 30 | residuals_ptr[0] = ( ceres::abs( diff.norm() ) ) * T(1.0/std::sqrt(m_covariance)); 31 | 32 | // avoid zero derivate for sqrt() 33 | const T threshold = T(1e-6); 34 | if(residuals_ptr[0] 44 | ( new UWBPositionFactor(uwb_x, uwb_y, uwb_z, covariance ) ) 45 | ); 46 | } 47 | 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | private: 51 | Eigen::Vector3d m_uwb_position; 52 | const double m_covariance; 53 | }; 54 | 55 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/include/uwb_range_factor.h: -------------------------------------------------------------------------------- 1 | #ifndef _UWB_RANGE_FACTOR_H_ 2 | #define _UWB_RANGE_FACTOR_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "utility.h" 9 | #include 10 | 11 | class UWBRangeFactor{ 12 | public: 13 | UWBRangeFactor(const double anchor_x, const double anchor_y, const double anchor_z, 14 | const double r, const double covariance): 15 | m_r(r), m_covariance(covariance){ 16 | m_anchor << anchor_x, anchor_y, anchor_z; 17 | } 18 | 19 | 20 | template 21 | bool operator()(const T* const x, const T* const y, const T* const z, 22 | T* residuals_ptr) const{ 23 | const Eigen::Matrix anchor = m_anchor.cast(); 24 | const Eigen::Matrix mobile(*x, *y, *z); 25 | Eigen::Matrix diff = anchor - mobile; 26 | if(diff(0,0)==T(0) && diff(1,0)==T(0) && diff(2,0)==T(0)){ 27 | diff(0,0) = T(1e-6); 28 | } 29 | 30 | residuals_ptr[0] = ( ceres::abs( diff.norm() - T(m_r) ) ) * T(1.0/std::sqrt(m_covariance)); 31 | 32 | // avoid zero derivate for sqrt() 33 | const T threshold = T(1e-6); 34 | if(residuals_ptr[0] 44 | ( new UWBRangeFactor(anchor_x, anchor_y, anchor_z, r, covariance ) ) 45 | ); 46 | } 47 | 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | 50 | private: 51 | Eigen::Vector3d m_anchor; 52 | const double m_r, m_covariance; 53 | }; 54 | 55 | 56 | 57 | 58 | 59 | #endif -------------------------------------------------------------------------------- /uwb-imu-positioning/launch/slam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /uwb-imu-positioning/launch/slam.yaml: -------------------------------------------------------------------------------- 1 | # %YAML:1.0 2 | # WARNING: AVOID any TAB in this file, otherwise there will be error. 3 | # parameters for SLAM 4 | 5 | # if < 20, fps is not used in uwb. 6 | slam_fps: 50 7 | td: 0.03 8 | 9 | # parameters for UWB localization 10 | # UWB setting ==================================== 11 | # Mobile, if /uav_id is set, will follow the /uav_id 12 | mobile_id: 301 13 | 14 | # Anchor 15 | # Changi 16 | anchor_list: [101, 102, 103, 104, 105, 106] 17 | anchor_101: [0, 0, 0] 18 | anchor_102: [14.6339, 0.00044865, -0.000732158] 19 | anchor_103: [14.7428, 25.4994, -0.0135969] 20 | anchor_104: [-0.020438, -1.00138, 5.29275] 21 | anchor_105: [-0.0852587, 26.5864, 5.25772] 22 | anchor_106: [17.3644, 10.056, 5.24208] 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /uwb-imu-positioning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | uwb-imu-positioning 4 | 0.0.0 5 | uwb-imu-positioning package 6 | 7 | 8 | 9 | 10 | jiaxin 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | common_msgs 44 | octomap 45 | roscpp 46 | rospy 47 | std_msgs 48 | tf 49 | libpcl-all-dev 50 | cv_bridge 51 | pcl_conversions 52 | pcl_ros 53 | pcl_msgs 54 | tf_conversions 55 | common_msgs 56 | octomap 57 | roscpp 58 | rospy 59 | std_msgs 60 | tf 61 | libpcl-all 62 | cv_bridge 63 | tf_conversions 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /uwb-imu-positioning/src/localization_node.cc: -------------------------------------------------------------------------------- 1 | #include "localization_node.h" 2 | 3 | Localization_Node::Localization_Node(ros::NodeHandle& nh): 4 | m_nh(nh){ 5 | // load param 6 | m_nh.param("slam_fps", m_slam_fps, 0); 7 | m_nh.param("td", m_td, 0.3); 8 | 9 | if(m_slam_fps<20){ 10 | m_is_fix_fps = false; 11 | } else { 12 | m_is_fix_fps = true; 13 | } 14 | m_nh.param("is_initialize_with_ceres", m_is_initialize_with_ceres, true); 15 | m_last_imu_t = 0; 16 | m_current_time = -1; 17 | 18 | // create UWB_Mobile 19 | if( m_nh.getParam(std::string("/uav_id"), m_mobile_id ) ) { 20 | ROS_INFO("Mobile ID (/uav_id): %d", m_mobile_id); 21 | } else if( m_nh.getParam(std::string("mobile_id"), m_mobile_id ) ){ 22 | ROS_INFO("Mobile ID (yaml): %d", m_mobile_id); 23 | } else { 24 | ROS_ERROR("Mobile ID not found!"); 25 | return; 26 | } 27 | createUWBMobile(m_mobile_id); 28 | ROS_INFO("UWB_Mobile created."); 29 | 30 | // initialize range info callback 31 | m_ndb_sub = m_nh.subscribe("/time_domain/NDB", 32 | 10, 33 | &Localization_Node::ndbCallback, 34 | this); 35 | 36 | m_imu_info_sub = m_nh.subscribe("/nax/imu",1000, &Localization_Node::imuInfoCallback,this); 37 | 38 | m_uwb_odometry_pub = m_nh.advertise("odometry", 1000, this); 39 | 40 | //m_print_timer = m_nh.createTimer(ros::Duration(1), &Localization_Node::printLocalizationCallback, this); 41 | } 42 | 43 | 44 | void Localization_Node::imuInfoCallback(const sensor_msgs::ImuConstPtr &imu_msg) 45 | { 46 | if(imu_msg->header.stamp.toSec() <= m_last_imu_t) 47 | { 48 | ROS_WARN("imu message in disorder"); 49 | return; 50 | } 51 | 52 | m_last_imu_t = imu_msg->header.stamp.toSec(); 53 | 54 | m_buf.lock(); 55 | m_imu_buf.push(imu_msg); 56 | m_buf.unlock(); 57 | m_con.notify_one(); 58 | 59 | } 60 | 61 | void Localization_Node::ndbCallback(const common_msgs::UWB_FullNeighborDatabase::ConstPtr& msg) 62 | { 63 | common_msgs::UWB_FullNeighborDatabase m_ndb = *msg; 64 | 65 | if(getValidNeighborDatabse(m_ndb)) 66 | { 67 | m_buf.lock(); 68 | m_ndb_buf.push(msg); 69 | m_buf.unlock(); 70 | m_con.notify_one(); 71 | } 72 | 73 | } 74 | 75 | bool Localization_Node::getValidNeighborDatabse(const common_msgs::UWB_FullNeighborDatabase &ndb) 76 | { 77 | int valid_anchor_counter = 0; 78 | 79 | for(int i=0;igetAnchorPosition(ndb.neighbors.at(i).nodeId, anchor_position) 84 | && ndb.neighbors.at(i).rangeMm/1000.0>0.3) 85 | { 86 | valid_anchor_counter += 1; 87 | } 88 | ROS_INFO("[Ceres Init] Total ndb anchor: %d", valid_anchor_counter); 89 | 90 | } 91 | 92 | if(valid_anchor_counter < m_p_mobile->m_least_anchor_num) 93 | { 94 | return false; 95 | }else 96 | { 97 | return true; 98 | } 99 | 100 | } 101 | 102 | std::vector, common_msgs::UWB_FullNeighborDatabase::ConstPtr>> Localization_Node::getMeasurements() 103 | { 104 | std::vector, common_msgs::UWB_FullNeighborDatabase::ConstPtr>> measurements; 105 | while(true) 106 | { 107 | if(m_imu_buf.empty() || m_ndb_buf.empty()) 108 | { 109 | return measurements; 110 | } 111 | 112 | if(!(m_imu_buf.back()->header.stamp.toSec() > m_ndb_buf.front()->header.stamp.toSec() + m_td)) 113 | { 114 | return measurements; 115 | } 116 | 117 | if(!(m_imu_buf.front()->header.stamp.toSec() < m_ndb_buf.front()->header.stamp.toSec()+ m_td)) 118 | { 119 | ROS_WARN("throw ndb, only should happen at the beginning"); 120 | m_ndb_buf.pop(); 121 | continue; 122 | } 123 | 124 | common_msgs::UWB_FullNeighborDatabase::ConstPtr ndb_msg = m_ndb_buf.front(); 125 | m_ndb_buf.pop(); 126 | 127 | std::vector IMUs; 128 | while (m_imu_buf.front()->header.stamp.toSec() < ndb_msg->header.stamp.toSec() + m_td) 129 | { 130 | IMUs.emplace_back(m_imu_buf.front()); 131 | m_imu_buf.pop(); 132 | } 133 | IMUs.emplace_back(m_imu_buf.front()); 134 | if (IMUs.empty()) 135 | ROS_WARN("no imu between two uwb range"); 136 | measurements.emplace_back(IMUs, ndb_msg); 137 | 138 | } 139 | 140 | return measurements; 141 | 142 | } 143 | 144 | void Localization_Node::process() 145 | { 146 | while(true) 147 | { 148 | std::vector, common_msgs::UWB_FullNeighborDatabase::ConstPtr>> measurements; 149 | std::unique_lock lk(m_buf); 150 | m_con.wait(lk, [&] 151 | { 152 | return (measurements = getMeasurements()).size() != 0; 153 | }); 154 | lk.unlock(); 155 | 156 | m_process.lock(); 157 | for(auto &measurement : measurements) 158 | { 159 | common_msgs::UWB_FullNeighborDatabase::ConstPtr ndb_msg = measurement.second; 160 | 161 | // initialize by zero or Ceres 162 | UWB_Loc_Init initializer(*ndb_msg, m_p_mobile); 163 | if(!initializer.initializeMobileByCeres()) 164 | { 165 | ROS_ERROR("UWB is not initialized!"); 166 | return; 167 | } 168 | 169 | double dx = 0, dy = 0, dz = 0, rx = 0, ry = 0, rz = 0; 170 | double ndb_t = ndb_msg->header.stamp.toSec() + m_td; 171 | 172 | for(auto &imu_msg: measurement.first) 173 | { 174 | double t = imu_msg->header.stamp.toSec(); 175 | 176 | if(t <= ndb_t) 177 | { 178 | if(m_current_time < 0) 179 | { 180 | m_current_time = t; 181 | } 182 | 183 | double dt = t - m_current_time; 184 | ROS_ASSERT(dt >= 0); 185 | 186 | m_current_time = t; 187 | 188 | dx = imu_msg->linear_acceleration.x; 189 | dy = imu_msg->linear_acceleration.y; 190 | dz = imu_msg->linear_acceleration.z; 191 | rx = imu_msg->angular_velocity.x; 192 | ry = imu_msg->angular_velocity.y; 193 | rz = imu_msg->angular_velocity.z; 194 | 195 | m_p_mobile->processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); 196 | 197 | }else 198 | { 199 | double dt_1 = ndb_t - m_current_time; 200 | double dt_2 = t-ndb_t; 201 | 202 | m_current_time = ndb_t; 203 | 204 | ROS_ASSERT(dt_1 >= 0); 205 | ROS_ASSERT(dt_2 >= 0); 206 | ROS_ASSERT(dt_1 + dt_2 > 0); 207 | double w1 = dt_2 / (dt_1 + dt_2); 208 | double w2 = dt_1 / (dt_1 + dt_2); 209 | dx = w1 * dx + w2 * imu_msg->linear_acceleration.x; 210 | dy = w1 * dy + w2 * imu_msg->linear_acceleration.y; 211 | dz = w1 * dz + w2 * imu_msg->linear_acceleration.z; 212 | rx = w1 * rx + w2 * imu_msg->angular_velocity.x; 213 | ry = w1 * ry + w2 * imu_msg->angular_velocity.y; 214 | rz = w1 * rz + w2 * imu_msg->angular_velocity.z; 215 | 216 | m_p_mobile->processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); 217 | 218 | } 219 | 220 | } 221 | 222 | if(m_p_mobile->filter3DUpdate()) 223 | { 224 | pubOdometry(ndb_t); 225 | 226 | } 227 | 228 | } 229 | 230 | m_process.unlock(); 231 | 232 | } 233 | 234 | } 235 | 236 | void Localization_Node::pubOdometry(double header) 237 | { 238 | m_p_mobile->printPosition(); 239 | m_p_mobile->printVelocity(); 240 | m_p_mobile->printOrientation(); 241 | 242 | geometry_msgs::PointStamped position = m_p_mobile->getPosition(); 243 | geometry_msgs::PointStamped velocity = m_p_mobile->getVelocity(); 244 | Eigen::Quaterniond orientation = m_p_mobile->getOrientation(); 245 | 246 | 247 | nav_msgs::Odometry odometry; 248 | odometry.header.stamp = ros::Time(header); 249 | odometry.header.frame_id = "world"; 250 | odometry.pose.pose.position.x = position.point.x; 251 | odometry.pose.pose.position.y = position.point.y; 252 | odometry.pose.pose.position.z = position.point.z; 253 | 254 | odometry.pose.pose.orientation.w = orientation.w(); 255 | odometry.pose.pose.orientation.x = orientation.x(); 256 | odometry.pose.pose.orientation.y = orientation.y(); 257 | odometry.pose.pose.orientation.z = orientation.z(); 258 | 259 | odometry.twist.twist.linear.x = velocity.point.x; 260 | odometry.twist.twist.linear.y = velocity.point.y; 261 | odometry.twist.twist.linear.z = velocity.point.z; 262 | 263 | m_uwb_odometry_pub.publish(odometry); 264 | 265 | } 266 | 267 | void Localization_Node::createUWBMobile(const int mobile_id){ 268 | // get anchor list from parameter server 269 | std::string param_key("anchor_list"); 270 | if(!m_nh.getParam(param_key, m_anchor_list)){ 271 | ROS_ERROR("Can't find anchor list param."); 272 | return; 273 | } 274 | 275 | // get anchor position, build anchor_map 276 | for(int i=0;i position; 281 | if(m_nh.getParam(param_key, position) && position.size()==3){ 282 | // has valid position 283 | // set anchor, variance set to constant 1 284 | boost::shared_ptr p_anchor( 285 | new UWB_Anchor( anchor_id, 286 | position.at(0), position.at(1), position.at(2), 287 | 1, 1, 1 ) ); 288 | m_anchor_map.insert(std::pair >(anchor_id, p_anchor) ); 289 | } 290 | } 291 | 292 | // create pointer 293 | m_p_mobile = boost::shared_ptr(new UWB_Mobile(mobile_id, m_anchor_map, m_nh) ); 294 | } 295 | 296 | 297 | int main(int argc, char **argv){ 298 | 299 | //initialize ros 300 | ros::init(argc, argv, "uwb_odom"); 301 | ros::NodeHandle nh("~"); 302 | 303 | Localization_Node loc_node(nh); 304 | 305 | std::thread loc_thread(&Localization_Node::process, &loc_node); 306 | 307 | ros::spin(); 308 | 309 | //google::InitGoogleLogging(argv[0]); 310 | 311 | return 0; 312 | } 313 | -------------------------------------------------------------------------------- /uwb-imu-positioning/src/pose_local_parameterization.cc: -------------------------------------------------------------------------------- 1 | #include "pose_local_parameterization.h" 2 | 3 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 4 | { 5 | Eigen::Map _p(x); 6 | Eigen::Map _q(x + 3); 7 | 8 | Eigen::Map dp(delta); 9 | 10 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 11 | 12 | Eigen::Map p(x_plus_delta); 13 | Eigen::Map q(x_plus_delta + 3); 14 | 15 | p = _p + dp; 16 | q = (_q * dq).normalized(); 17 | 18 | return true; 19 | } 20 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const 21 | { 22 | Eigen::Map> j(jacobian); 23 | j.topRows<6>().setIdentity(); 24 | j.bottomRows<1>().setZero(); 25 | 26 | return true; 27 | } -------------------------------------------------------------------------------- /uwb-imu-positioning/src/uwb.cc: -------------------------------------------------------------------------------- 1 | #include "uwb.h" 2 | 3 | UWB_Node::UWB_Node(const int node_id):m_node_id(node_id){} 4 | 5 | // --------------------------------------------- 6 | UWB_Anchor::UWB_Anchor(const int node_id, 7 | const double x, const double y, const double z, 8 | const double var_x, const double var_y, const double var_z) 9 | :UWB_Node(node_id) 10 | { 11 | setPosition(x,y,z); 12 | } 13 | 14 | void UWB_Anchor::insertRangeInfo(const common_msgs::UWB_EchoedRangeInfo &msg){ 15 | // only insert range info that is requested by myself. 16 | 17 | int requester_id = msg.requesterId; 18 | int responder_id = msg.responderId; 19 | double range = double(msg.precisionRangeMm) / 1000.0; 20 | 21 | if(m_node_id==requester_id){ 22 | // if haven't seen the responder yet, create it 23 | if(m_range_map.find(responder_id)==m_range_map.end()){ 24 | m_range_map.insert(std::pair >(responder_id, std::vector())); 25 | // add the range measurement into the vector 26 | m_range_map.find(responder_id)->second.push_back(range); 27 | } else { 28 | // add the range measurement into the vector 29 | m_range_map.find(responder_id)->second.push_back(range); 30 | } 31 | } 32 | } 33 | 34 | void UWB_Anchor::insertRangeInfo(const common_msgs::UWB_FullRangeInfo& msg){ 35 | // only insert range info that is requested by myself. 36 | 37 | int requester_id = msg.nodeId; 38 | int responder_id = msg.responderId; 39 | double range = double(msg.precisionRangeMm) / 1000.0; 40 | 41 | if(m_node_id==requester_id){ 42 | // if haven't seen the responder yet, create it 43 | if(m_range_map.find(responder_id)==m_range_map.end()){ 44 | m_range_map.insert(std::pair >(responder_id, std::vector())); 45 | // add the range measurement into the vector 46 | m_range_map.find(responder_id)->second.push_back(range); 47 | } else { 48 | // add the range measurement into the vector 49 | m_range_map.find(responder_id)->second.push_back(range); 50 | } 51 | } 52 | } 53 | 54 | double UWB_Anchor::getAverageRangeToNode(const int anchor_id){ 55 | // check whether the anchor_id has been seen 56 | std::map >::iterator it = m_range_map.find(anchor_id); 57 | if(it==m_range_map.end()){ 58 | return 0; 59 | } else { 60 | // check the size of the vector 61 | if(it->second.size()==0){ 62 | return 0; 63 | } else { 64 | return robustAverage(it->second); 65 | } 66 | } 67 | } 68 | 69 | bool UWB_Anchor::getMedianAndVarianceToNode(const int anchor_id, double &median, double &variance){ 70 | std::map >::iterator it = m_range_map.find(anchor_id); 71 | if(it==m_range_map.end()){ 72 | return false; 73 | } else { 74 | if(it->second.size()==0){ 75 | return false; 76 | } else { 77 | medianAndVariance(it->second, median, variance); 78 | return true; 79 | } 80 | } 81 | } 82 | 83 | 84 | UWB_Mobile::UWB_Mobile(const int node_id, 85 | const std::map > anchor_map, 86 | ros::NodeHandle& nh) 87 | :UWB_Node(node_id), 88 | m_anchor_map(anchor_map), 89 | m_nh(nh) 90 | { 91 | m_g.setZero(); 92 | m_nh.param("G", m_g.z(), 9.8); 93 | 94 | m_first_imu = false; 95 | 96 | } 97 | 98 | void UWB_Mobile::fullInitialize(const common_msgs::UWB_FullNeighborDatabase& ndb, const geometry_msgs::PointStamped& position){ 99 | setPosition(position.point.x, position.point.y, position.point.z); 100 | setVelocity(0,0,0); 101 | setAccelerationBias(Eigen::Vector3d(0,0,0)); 102 | setAngularBias(Eigen::Vector3d(0,0,0)); 103 | 104 | m_ranges_to_anchors.clear(); 105 | 106 | common_msgs::UWB_FullRangeInfo zero_range_info; 107 | for(int i=0;i50) 117 | { 118 | m_ranges_to_anchors.insert(std::make_pair(anchor_id,zero_range_info)); 119 | } 120 | } 121 | 122 | m_last_range_time = ros::Time::now(); 123 | } 124 | 125 | double UWB_Mobile::getAverageRangeErrEst() 126 | { 127 | double sum_range_err = 0.0; 128 | std::map::iterator iter; 129 | for(iter=m_ranges_to_anchors.begin();iter!=m_ranges_to_anchors.end();++iter) 130 | { 131 | sum_range_err += iter->second.precisionRangeErrEst; 132 | } 133 | 134 | return sum_range_err/m_ranges_to_anchors.size(); 135 | } 136 | 137 | 138 | void UWB_Mobile::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity) 139 | { 140 | if(!m_first_imu) 141 | { 142 | m_first_imu = true; 143 | acc_0 = linear_acceleration; 144 | gyr_0 = angular_velocity; 145 | } 146 | 147 | if(!pre_integration) 148 | { 149 | pre_integration = new IntegrationBase{acc_0, gyr_0, m_acceleration_bias,m_angular_bias}; 150 | } 151 | 152 | pre_integration->push_back(dt, linear_acceleration,angular_velocity); 153 | m_td_buf.push_back(dt); 154 | m_linear_acceleration_buf.push_back(linear_acceleration); 155 | m_angular_velocity_buf.push_back(angular_velocity); 156 | 157 | Eigen::Vector3d un_acc_0 = m_pred_rotation * (acc_0 - m_acceleration_bias) -m_g; 158 | Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - m_angular_bias; 159 | m_pred_rotation *= Utility::deltaQ(un_gyr * dt).toRotationMatrix(); 160 | Eigen::Vector3d un_acc_1 = m_pred_rotation * (linear_acceleration - m_acceleration_bias) - m_g; 161 | Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1); 162 | 163 | m_pred_position += dt * m_pred_velocity + 0.5 * dt * dt * un_acc; 164 | m_pred_velocity += dt * un_acc; 165 | 166 | acc_0 = linear_acceleration; 167 | gyr_0 = angular_velocity; 168 | 169 | } 170 | 171 | bool UWB_Mobile::filter3DUpdate() 172 | { 173 | if(std::string("tightly")==m_filter_type) 174 | { 175 | return tightlyFGOUpdate(); 176 | 177 | } else if(std::string("loosely")==m_filter_type) 178 | { 179 | return looselyFGOUpdate(); 180 | 181 | }else 182 | { 183 | ROS_WARN("Unknown filter type, use tightly-FGO now."); 184 | return tightlyFGOUpdate(); 185 | } 186 | 187 | } 188 | 189 | void UWB_Mobile::doubleToVector() 190 | { 191 | setLastPosition(Eigen::Vector3d(para_pose[1][0],para_pose[1][1],para_pose[1][2])); 192 | setLastVelocity(Eigen::Vector3d(para_speed_bias[1][0],para_speed_bias[1][1],para_speed_bias[1][2])); 193 | setLastRotation(Eigen::Quaterniond(para_pose[1][6],para_pose[1][3],para_pose[1][4],para_pose[1][5]).toRotationMatrix()); 194 | 195 | setPosition(para_pose[1][0],para_pose[1][1],para_pose[1][2]); 196 | setVelocity(para_speed_bias[1][0],para_speed_bias[1][1],para_speed_bias[1][2]); 197 | setOrientation(para_pose[1][3], para_pose[1][4], para_pose[1][5], para_pose[1][6]); 198 | 199 | m_acceleration_bias = Eigen::Vector3d(para_speed_bias[1][3],para_speed_bias[1][4],para_speed_bias[1][5]); 200 | m_angular_bias = Eigen::Vector3d(para_speed_bias[1][6],para_speed_bias[1][7], para_speed_bias[1][8]); 201 | 202 | } 203 | 204 | void UWB_Mobile::vectorToDouble() 205 | { 206 | //last 207 | para_pose[0][0] = m_last_position.x(); 208 | para_pose[0][1] = m_last_position.y(); 209 | para_pose[0][2] = m_last_position.z(); 210 | Eigen::Quaterniond q{m_last_rotation}; 211 | 212 | para_pose[0][3] = q.x(); 213 | para_pose[0][4] = q.y(); 214 | para_pose[0][5] = q.z(); 215 | para_pose[0][6] = q.w(); 216 | 217 | para_speed_bias[0][0] = m_last_velocity.x(); 218 | para_speed_bias[0][1] = m_last_velocity.y(); 219 | para_speed_bias[0][2] = m_last_velocity.z(); 220 | 221 | para_speed_bias[0][3] = m_acceleration_bias.x(); 222 | para_speed_bias[0][4] = m_acceleration_bias.y(); 223 | para_speed_bias[0][5] = m_acceleration_bias.z(); 224 | 225 | para_speed_bias[0][6] = m_angular_bias.x(); 226 | para_speed_bias[0][7] = m_angular_bias.y(); 227 | para_speed_bias[0][8] = m_angular_bias.z(); 228 | 229 | //current pred 230 | para_pose[1][0] = m_pred_position.x(); 231 | para_pose[1][1] = m_pred_position.y(); 232 | para_pose[1][2] = m_pred_position.z(); 233 | Eigen::Quaterniond q1{m_pred_rotation}; 234 | 235 | para_pose[1][3] = q1.x(); 236 | para_pose[1][4] = q1.y(); 237 | para_pose[1][5] = q1.z(); 238 | para_pose[1][6] = q1.w(); 239 | 240 | para_speed_bias[1][0] = m_pred_velocity.x(); 241 | para_speed_bias[1][1] = m_pred_velocity.y(); 242 | para_speed_bias[1][2] = m_pred_velocity.z(); 243 | 244 | para_speed_bias[1][3] = m_acceleration_bias.x(); 245 | para_speed_bias[1][4] = m_acceleration_bias.y(); 246 | para_speed_bias[1][5] = m_acceleration_bias.z(); 247 | 248 | para_speed_bias[1][6] = m_angular_bias.x(); 249 | para_speed_bias[1][7] = m_angular_bias.y(); 250 | para_speed_bias[1][8] = m_angular_bias.z(); 251 | } 252 | 253 | bool UWB_Mobile::tightlyFGOUpdate() 254 | { 255 | vectorToDouble(); 256 | ceres::Problem problem; 257 | ceres::LossFunction *loss_function; 258 | loss_function = new ceres::CauchyLoss(1.0); 259 | 260 | for(int i =0; i < 2; i++) 261 | { 262 | ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); 263 | problem.AddParameterBlock(para_pose[i], 7, local_parameterization); 264 | problem.AddParameterBlock(para_speed_bias[i],9); 265 | } 266 | 267 | problem.SetParameterBlockConstant(para_pose[0]); 268 | problem.SetParameterBlockConstant(para_speed_bias[0]); 269 | 270 | if(pre_integration->sum_dt > 10.0) 271 | { 272 | return false; 273 | } 274 | 275 | IMUFactor* imu_factor = new IMUFactor(pre_integration); 276 | problem.AddResidualBlock(imu_factor, NULL, para_pose[0], para_speed_bias[0], para_pose[1], para_speed_bias[1]); 277 | 278 | std::map::iterator iter; 279 | for(iter=m_ranges_to_anchors.begin();iter!=m_ranges_to_anchors.end();++iter) 280 | { 281 | int anchor_id = iter->first; 282 | geometry_msgs::PointStamped anchor_position; 283 | if(getAnchorPosition(anchor_id, anchor_position)) 284 | { 285 | ceres::CostFunction* cost_function = UWBRangeFactor::Create(anchor_position.point.x, 286 | anchor_position.point.y, 287 | anchor_position.point.z, 288 | iter->second.precisionRangeMm/1000.0, 289 | iter->second.precisionRangeErrEst/1000.0*5); 290 | 291 | problem.AddResidualBlock(cost_function, loss_function, &(para_pose[1][0]), &(para_pose[1][1]),&(para_pose[1][2])); 292 | } 293 | 294 | } 295 | 296 | ceres::Solver::Options options; 297 | options.max_num_iterations = 10000; 298 | options.minimizer_progress_to_stdout = false; 299 | options.linear_solver_type = ceres::DENSE_QR; 300 | 301 | ceres::Solver::Summary summary; 302 | ceres::Solve(options, &problem, &summary); 303 | 304 | std::cout<<"\n"<sum_dt > 10.0) 331 | { 332 | return false; 333 | } 334 | 335 | IMUFactor* imu_factor = new IMUFactor(pre_integration); 336 | problem.AddResidualBlock(imu_factor, NULL, para_pose[0], para_speed_bias[0], para_pose[1], para_speed_bias[1]); 337 | 338 | ceres::CostFunction* cost_function = UWBPositionFactor::Create(m_position.point.x, 339 | m_position.point.y, 340 | m_position.point.z, 341 | getAverageRangeErrEst()/1000.0 * 5); 342 | 343 | problem.AddResidualBlock(cost_function, loss_function, &(para_pose[1][0]), &(para_pose[1][1]),&(para_pose[1][2])); 344 | 345 | 346 | ceres::Solver::Options options; 347 | options.max_num_iterations = 10000; 348 | options.minimizer_progress_to_stdout = false; 349 | options.linear_solver_type = ceres::DENSE_QR; 350 | 351 | ceres::Solver::Summary summary; 352 | ceres::Solve(options, &problem, &summary); 353 | 354 | std::cout<<"\n"<fullInitialize(m_ndb, m_mobile_position); 7 | ROS_INFO("Innitialization with Ceres OK. %f, %f, %f", 8 | m_mobile_position.point.x, 9 | m_mobile_position.point.y, 10 | m_mobile_position.point.z); 11 | return true; 12 | } else { 13 | ROS_INFO("Fail to initialize with Ceres."); 14 | return false; 15 | } 16 | } 17 | 18 | void UWB_Loc_Init::buildOptimizationProblem(const common_msgs::UWB_FullNeighborDatabase& ndb){ 19 | ceres::LossFunction* loss_function = new ceres::CauchyLoss(1.0); 20 | for(int i=0;igetAnchorPosition(m_ndb.neighbors.at(i).nodeId, anchor_position)){ 23 | ceres::CostFunction* cost_function = AnchorTriangulationErrorTerm::Create(anchor_position.point.x, 24 | anchor_position.point.y, 25 | anchor_position.point.z, 26 | m_ndb.neighbors.at(i).rangeMm/1000.0, 27 | m_ndb.neighbors.at(i).rangeErrorEstimate/1000.0*5); 28 | m_problem.AddResidualBlock(cost_function, loss_function, 29 | &(m_mobile_position.point.x), 30 | &(m_mobile_position.point.y), 31 | &(m_mobile_position.point.z)); 32 | } 33 | } 34 | } 35 | 36 | bool UWB_Loc_Init::solveOptimizationProblem(){ 37 | ceres::Solver::Options options; 38 | options.max_num_iterations = 10000; 39 | options.minimizer_progress_to_stdout = false; 40 | options.linear_solver_type = ceres::DENSE_QR; 41 | 42 | ceres::Solver::Summary summary; 43 | ceres::Solve(options, &m_problem, &summary); 44 | 45 | std::cout<<"\n"<