├── CMakeLists.txt ├── README.md ├── README_original.md ├── apps ├── .gitignore ├── imu_calib_node.cpp ├── test_imu_calib.cpp ├── test_integration.cpp └── test_phone_calib.cpp ├── bin └── test_data │ ├── xsens_acc.mat │ └── xsens_gyro.mat ├── include ├── .gitignore └── imu_tk │ ├── base.h │ ├── calibration.h │ ├── filters.h │ ├── imu_tk.h │ ├── integration.h │ ├── io_utils.h │ ├── vis_extra │ ├── gl_camera.h │ └── opengl_3d_scene.h │ └── visualization.h ├── lib └── .gitignore ├── package.xml └── src ├── .gitignore ├── calibration.cpp ├── filters.cpp ├── io_utils.cpp ├── vis_extra ├── gl_camera.cpp └── opengl_3d_scene.cpp └── visualization.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(imu_tk) 2 | 3 | cmake_minimum_required(VERSION 2.8.3) 4 | cmake_policy(SET CMP0015 NEW) 5 | 6 | ## Compile as C++11, supported in ROS Kinetic and newer 7 | add_compile_options(-std=c++11) 8 | 9 | if ("${CMAKE_BUILD_TYPE}" STREQUAL "") 10 | set(CMAKE_BUILD_TYPE "Release") 11 | endif() 12 | 13 | if(NOT DEFINED BUILD_IMU_TK_EXAMPLES) 14 | set(BUILD_IMU_TK_EXAMPLES "ON") 15 | endif(NOT DEFINED BUILD_IMU_TK_EXAMPLES) 16 | 17 | find_package(Boost REQUIRED) 18 | find_package(Eigen3 REQUIRED) 19 | find_package(Ceres REQUIRED) 20 | 21 | include_directories(./include 22 | /usr/include 23 | ${Boost_INCLUDE_DIRS} 24 | ${EIGEN_INCLUDE_DIR} 25 | ${CERES_INCLUDE_DIRS}) 26 | 27 | #Vis3D 28 | find_package(Qt4 COMPONENTS QtCore QtGui QtOpenGL REQUIRED ) 29 | find_package(OpenGL REQUIRED) 30 | find_package(GLUT REQUIRED) 31 | 32 | include_directories( ${OPENGL_INCLUDE_DIRS} 33 | ${GLUT_INCLUDE_DIRS} 34 | ${CMAKE_CURRENT_BINARY_DIR} ) 35 | 36 | set(IMU_TK_VIS_EXTRA_SOURCES 37 | ./src/vis_extra/gl_camera.cpp 38 | ./src/vis_extra/opengl_3d_scene.cpp ) 39 | set(IMU_TK_VIS_EXTRA_HEADERS 40 | ./include/imu_tk/vis_extra/opengl_3d_scene.h ) 41 | 42 | qt4_wrap_cpp(IMU_TK_VIS_EXTRA_HEADERS_MOC ${IMU_TK_VIS_EXTRA_HEADERS}) 43 | 44 | include(${QT_USE_FILE}) 45 | add_definitions(${QT_DEFINITIONS}) 46 | 47 | aux_source_directory( ./src IMU_TK_CORE_SRC ) 48 | set( IMU_TK_SRC ${IMU_TK_CORE_SRC} ${IMU_TK_VIS_EXTRA_HEADERS_MOC} ${IMU_TK_VIS_EXTRA_SOURCES} ) 49 | 50 | add_library(imu_tk ${IMU_TK_SRC}) 51 | set_target_properties(imu_tk PROPERTIES ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib) 52 | 53 | set (IMU_TK_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/include CACHE STRING "imu_tk include directories") 54 | set (IMU_TK_LIB_DIR ${CMAKE_CURRENT_SOURCE_DIR}/lib CACHE STRING "imu_tk libraries directories") 55 | set (IMU_TK_LIBS imu_tk ${CERES_LIBRARIES} ${QT_LIBRARIES} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} 56 | CACHE STRING "imu_tk libraries") 57 | 58 | message( "${IMU_TK_LIBS}" ) 59 | 60 | if( BUILD_IMU_TK_EXAMPLES ) 61 | 62 | add_executable(test_imu_calib apps/test_imu_calib.cpp) 63 | target_link_libraries( test_imu_calib ${IMU_TK_LIBS}) 64 | set_target_properties( test_imu_calib PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 65 | 66 | 67 | add_executable(test_integration apps/test_integration.cpp) 68 | target_link_libraries( test_integration ${IMU_TK_LIBS}) 69 | 70 | set_target_properties( test_integration PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin) 71 | 72 | endif( BUILD_IMU_TK_EXAMPLES ) 73 | 74 | ## Find catkin macros and libraries 75 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 76 | ## is used, also find other catkin packages 77 | find_package(catkin REQUIRED COMPONENTS 78 | rosbag 79 | sensor_msgs 80 | ) 81 | 82 | ## System dependencies are found with CMake's conventions 83 | # find_package(Boost REQUIRED COMPONENTS system) 84 | 85 | 86 | ## Uncomment this if the package has a setup.py. This macro ensures 87 | ## modules and global scripts declared therein get installed 88 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 89 | # catkin_python_setup() 90 | 91 | ################################################ 92 | ## Declare ROS messages, services and actions ## 93 | ################################################ 94 | 95 | ## To declare and build messages, services or actions from within this 96 | ## package, follow these steps: 97 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 98 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 99 | ## * In the file package.xml: 100 | ## * add a build_depend tag for "message_generation" 101 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 102 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 103 | ## but can be declared for certainty nonetheless: 104 | ## * add a exec_depend tag for "message_runtime" 105 | ## * In this file (CMakeLists.txt): 106 | ## * add "message_generation" and every package in MSG_DEP_SET to 107 | ## find_package(catkin REQUIRED COMPONENTS ...) 108 | ## * add "message_runtime" and every package in MSG_DEP_SET to 109 | ## catkin_package(CATKIN_DEPENDS ...) 110 | ## * uncomment the add_*_files sections below as needed 111 | ## and list every .msg/.srv/.action file to be processed 112 | ## * uncomment the generate_messages entry below 113 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 114 | 115 | ## Generate messages in the 'msg' folder 116 | # add_message_files( 117 | # FILES 118 | # Message1.msg 119 | # Message2.msg 120 | # ) 121 | 122 | ## Generate services in the 'srv' folder 123 | # add_service_files( 124 | # FILES 125 | # Service1.srv 126 | # Service2.srv 127 | # ) 128 | 129 | ## Generate actions in the 'action' folder 130 | # add_action_files( 131 | # FILES 132 | # Action1.action 133 | # Action2.action 134 | # ) 135 | 136 | ## Generate added messages and services with any dependencies listed here 137 | # generate_messages( 138 | # DEPENDENCIES 139 | # sensor_msgs 140 | # ) 141 | 142 | ################################################ 143 | ## Declare ROS dynamic reconfigure parameters ## 144 | ################################################ 145 | 146 | ## To declare and build dynamic reconfigure parameters within this 147 | ## package, follow these steps: 148 | ## * In the file package.xml: 149 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 150 | ## * In this file (CMakeLists.txt): 151 | ## * add "dynamic_reconfigure" to 152 | ## find_package(catkin REQUIRED COMPONENTS ...) 153 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 154 | ## and list every .cfg file to be processed 155 | 156 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 157 | # generate_dynamic_reconfigure_options( 158 | # cfg/DynReconf1.cfg 159 | # cfg/DynReconf2.cfg 160 | # ) 161 | 162 | ################################### 163 | ## catkin specific configuration ## 164 | ################################### 165 | ## The catkin_package macro generates cmake config files for your package 166 | ## Declare things to be passed to dependent projects 167 | ## INCLUDE_DIRS: uncomment this if your package contains header files 168 | ## LIBRARIES: libraries you create in this project that dependent projects also need 169 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 170 | ## DEPENDS: system dependencies of this project that dependent projects also need 171 | catkin_package( 172 | # INCLUDE_DIRS include 173 | # LIBRARIES imu_tk 174 | # CATKIN_DEPENDS rosbag sensor_msgs 175 | # DEPENDS system_lib 176 | ) 177 | 178 | ########### 179 | ## Build ## 180 | ########### 181 | 182 | ## Specify additional locations of header files 183 | ## Your package locations should be listed before other locations 184 | include_directories( 185 | # include 186 | ${catkin_INCLUDE_DIRS} 187 | ) 188 | 189 | ## Declare a C++ library 190 | # add_library(${PROJECT_NAME} 191 | # src/${PROJECT_NAME}/imu_tk.cpp 192 | # ) 193 | 194 | ## Add cmake target dependencies of the library 195 | ## as an example, code may need to be generated before libraries 196 | ## either from message generation or dynamic reconfigure 197 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 198 | 199 | ## Declare a C++ executable 200 | ## With catkin_make all packages are built within a single CMake context 201 | ## The recommended prefix ensures that target names across packages don't collide 202 | add_executable(imu_calib_node apps/imu_calib_node.cpp) 203 | 204 | ## Rename C++ executable without prefix 205 | ## The above recommended prefix causes long target names, the following renames the 206 | ## target back to the shorter version for ease of user use 207 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 208 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 209 | 210 | ## Add cmake target dependencies of the executable 211 | ## same as for the library above 212 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 213 | 214 | ## Specify libraries to link a library or executable target against 215 | target_link_libraries(imu_calib_node 216 | ${catkin_LIBRARIES} 217 | ${IMU_TK_LIBS} 218 | ) 219 | 220 | ############# 221 | ## Install ## 222 | ############# 223 | 224 | # all install targets should use catkin DESTINATION variables 225 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 226 | 227 | ## Mark executable scripts (Python etc.) for installation 228 | ## in contrast to setup.py, you can choose the destination 229 | # install(PROGRAMS 230 | # scripts/my_python_script 231 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 232 | # ) 233 | 234 | ## Mark executables and/or libraries for installation 235 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 236 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 237 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 238 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 239 | # ) 240 | 241 | ## Mark cpp header files for installation 242 | # install(DIRECTORY include/${PROJECT_NAME}/ 243 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 244 | # FILES_MATCHING PATTERN "*.h" 245 | # PATTERN ".svn" EXCLUDE 246 | # ) 247 | 248 | ## Mark other files for installation (e.g. launch and bag files, etc.) 249 | # install(FILES 250 | # # myfile1 251 | # # myfile2 252 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 253 | # ) 254 | 255 | ############# 256 | ## Testing ## 257 | ############# 258 | 259 | ## Add gtest based cpp test target and link libraries 260 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_imu_tk.cpp) 261 | # if(TARGET ${PROJECT_NAME}-test) 262 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 263 | # endif() 264 | 265 | ## Add folders to be run by python nosetests 266 | # catkin_add_nosetests(test) 267 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # IMU-TK 2 | 3 | This is a ROS wrapper for imu_tk, learn details from original [README.md](./README_original.md) please. 4 | 5 | ## Build 6 | 7 | Place this package inside your catkin workspace (e.g. `~/catkin_ws/src`), then build it through `catkin_make`. 8 | 9 | ```Bash 10 | cd ~/catkin_ws 11 | catkin_make 12 | rospack profile 13 | source devel/setup.bash 14 | ``` 15 | 16 | ## Test 17 | 18 | ### Collect IMU Data 19 | 20 | Record a bag file with IMU topic (e.g. `rosbag record -O imu /imu`). 21 | 22 | Procedure: 23 | 24 | 1. Left the IMU static for 50 seconds. 25 | 2. Rotate the IMU and then lay it in a different attitude. 26 | 3. Wait for at least 1 seconds. 27 | 4. Have you rotated the IMU 36 ~ 50 times? If not, go back to step 2. 28 | 5. Done. 29 | 30 | ### Calibration 31 | 32 | Run `imu_calib_node` to get calibration result(e.g. misalignment, scale and bias). 33 | 34 | `imu_calib_node` usage: 35 | 36 | > rosrun imu_tk imu_calib_node [BAG] [IMU_TOPIC] 37 | 38 | ```Bash 39 | rosrun imu_tk imu_calib_node imu.bag /imu 40 | ``` 41 | 42 | ### Correction 43 | 44 | Given a raw sensor reading X (e.g., the acceleration ), the calibrated "unbiased" reading X' is obtained. 45 | 46 | ```Txt 47 | Misalignment matrix: 48 | 49 | [ 1 -mis_yz mis_zy ] 50 | T = [ mis_xz 1 -mis_zx ] 51 | [ -mis_xy mis_yx 1 ] 52 | 53 | Scale matrix: 54 | 55 | [ s_x 0 0 ] 56 | K = [ 0 s_y 0 ] 57 | [ 0 0 s_z ] 58 | 59 | Bias vector: 60 | 61 | [ b_x ] 62 | B = [ b_y ] 63 | [ b_z ] 64 | 65 | X' = T*K*(X - B) 66 | ``` -------------------------------------------------------------------------------- /README_original.md: -------------------------------------------------------------------------------- 1 | # IMU-TK: Inertial Measurement Unit ToolKit # 2 | 3 | The C++ IMU-TK Library (Inertial Measurement Unit ToolKit) provides simple functions and data structures to calibrate MEMS-based inertial navigation units, and to process and display IMU data. 4 | IMU-TK implements a multi-position calibration method that does not require any parameter tuning and simply requires the sensor to be moved by hand and placed in a set of different, automatically detected, static positions. IMU-TK also provides a collection of functions for data integration. 5 | 6 | ## References ## 7 | 8 | Papers Describing the Approach: 9 | 10 | D. Tedaldi, A. Pretto and E. Menegatti, "A Robust and Easy to Implement Method for IMU Calibration without External Equipments". In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2014), May 31 - June 7, 2014 Hong Kong, China, Page(s): 3042 - 3049 ([PDF](http://www.dis.uniroma1.it/~pretto/papers/tpm_icra2014.pdf)) 11 | 12 | 13 | ``` 14 | #!bibtex 15 | 16 | @inproceedings{tpm_icra2014, 17 | title={A Robust and Easy to Implement Method for IMU Calibration 18 | without External Equipments}, 19 | author={Tedaldi, A. and Pretto, A. and Menegatti, E.}, 20 | booktitle={Proc. of: IEEE International Conference on Robotics and 21 | Automation (ICRA)}, 22 | year={2014}, 23 | pages={3042--3049} 24 | } 25 | ``` 26 | 27 | 28 | 29 | A. Pretto and G. Grisetti, "Calibration and performance evaluation of low-cost IMUs". In Proceedings of the 20th IMEKO TC4 International Symposium, Sep. 15 - 17, 2014 Benevento, Italy, pages: 429 - 434 ([PDF](http://www.dis.uniroma1.it/~pretto/papers/pg_imeko2014.pdf)) 30 | 31 | 32 | ``` 33 | #!bibtex 34 | 35 | @inproceedings{pg_imeko2014, 36 | title={Calibration and performance evaluation of low-cost IMUs}, 37 | author={Pretto, A. and Grisetti, G.}, 38 | booktitle={Proc. of: 20th IMEKO TC4 International Symposium}, 39 | year={2014}, 40 | pages={429--434} 41 | } 42 | ``` 43 | 44 | ## License ## 45 | 46 | IMU-TK is licensed under the BSD License. 47 | IMU-TK is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the licenses for more details. 48 | 49 | ## Requirements ## 50 | 51 | The code is tested on Ubuntu 14.04. IMU-TK requires the following tools and libraries: CMake, Eigen3, Ceres Solver, OpenGL, QT and Gnuplot. To install these required packages on Ubuntu, use the terminal command: 52 | 53 | 54 | ``` 55 | #!bash 56 | 57 | sudo apt-get install build-essential cmake libeigen3-dev libqt4-dev libqt4-opengl-dev freeglut3-dev gnuplot 58 | ``` 59 | and follow this [guide](http://ceres-solver.org/building.html) to install Ceres Solver. 60 | 61 | ## Building ## 62 | 63 | To build IMU-TK on Ubuntu, type in a terminal the following command sequence. 64 | 65 | ``` 66 | #!bash 67 | 68 | cd imu_tk 69 | mkdir build 70 | cd build 71 | cmake .. 72 | make 73 | 74 | ``` 75 | Test the library with the **test_imu_calib** app (binary in /bin, source code in src/test_imu_calib.cpp): **test_imu_calib** performs an IMU calibration given the data included in bin/test_data/: 76 | 77 | ``` 78 | #!bash 79 | 80 | ./test_imu_calib test_data/xsens_acc.mat test_data/xsens_gyro.mat 81 | 82 | ``` 83 | 84 | ## Contact information ## 85 | 86 | Alberto Pretto [pretto@dis.uniroma1.it](mailto:pretto@dis.uniroma1.it) -------------------------------------------------------------------------------- /apps/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Neil-Oyoung/imu_tk/a46a76364429b22d1bf313bbfb4ae3e8e2ec50fd/apps/.gitignore -------------------------------------------------------------------------------- /apps/imu_calib_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "imu_tk/io_utils.h" 9 | #include "imu_tk/calibration.h" 10 | #include "imu_tk/filters.h" 11 | #include "imu_tk/integration.h" 12 | #include "imu_tk/visualization.h" 13 | 14 | using namespace std; 15 | using namespace imu_tk; 16 | using namespace Eigen; 17 | 18 | #define foreach BOOST_FOREACH 19 | 20 | int main(int argc, char** argv) 21 | { 22 | if( argc < 3 ) { 23 | cout << "usage:\n" << argv[0] <<" BAG IMU_TOPIC\n"; 24 | return -1; 25 | } 26 | 27 | vector< TriadData > acc_data, gyro_data; 28 | 29 | rosbag::Bag bag; 30 | bag.open(argv[1], rosbag::bagmode::Read); 31 | 32 | std::vector topics; 33 | topics.push_back(std::string(argv[2])); 34 | 35 | rosbag::View view(bag, rosbag::TopicQuery(topics)); 36 | 37 | foreach(rosbag::MessageInstance const m, view) { 38 | sensor_msgs::Imu::ConstPtr s = m.instantiate(); 39 | if (s != NULL) { 40 | acc_data.push_back(TriadData(s->header.stamp.toSec(), s->linear_acceleration.x, s->linear_acceleration.y, s->linear_acceleration.z)); 41 | gyro_data.push_back(TriadData(s->header.stamp.toSec(), s->angular_velocity.x, s->angular_velocity.y, s->angular_velocity.z)); 42 | } 43 | } 44 | 45 | bag.close(); 46 | 47 | /* 48 | cout<<"Importing IMU data from the Matlab matrix file : "<< argv[1]< 2 | 3 | #include "imu_tk/io_utils.h" 4 | #include "imu_tk/calibration.h" 5 | #include "imu_tk/filters.h" 6 | #include "imu_tk/integration.h" 7 | #include "imu_tk/visualization.h" 8 | 9 | using namespace std; 10 | using namespace imu_tk; 11 | using namespace Eigen; 12 | 13 | int main(int argc, char** argv) 14 | { 15 | if( argc < 3 ) 16 | return -1; 17 | 18 | vector< TriadData > acc_data, gyro_data; 19 | 20 | cout<<"Importing IMU data from the Matlab matrix file : "<< argv[1]< 2 | 3 | #include 4 | 5 | #include "imu_tk/io_utils.h" 6 | #include "imu_tk/calibration.h" 7 | #include "imu_tk/filters.h" 8 | #include "imu_tk/integration.h" 9 | #include "imu_tk/visualization.h" 10 | 11 | using namespace std; 12 | using namespace imu_tk; 13 | using namespace Eigen; 14 | 15 | int main(int argc, char** argv) 16 | { 17 | if( argc < 2 ) 18 | return -1; 19 | 20 | QApplication app(argc, argv); 21 | 22 | vector< TriadData > acc_data, gyro_data, mag_data; 23 | 24 | cout<<"Importing IMU data from file : "<< argv[1]< 2 | #include 3 | 4 | #include "imu_tk/imu_tk.h" 5 | 6 | using namespace std; 7 | using namespace imu_tk; 8 | using namespace Eigen; 9 | 10 | int main(int argc, char** argv) 11 | { 12 | if( argc < 3 ) 13 | return -1; 14 | 15 | int n_intervals = 1; 16 | if( argc >= 4) 17 | n_intervals = atoi(argv[3]); 18 | 19 | vector< TriadData > acc_data_calib, gyro_data_calib; 20 | 21 | cout<<"Importing IMU data from the comma separated file : "<< argv[1]< acc_data_test, gyro_data_test, gyro_data_test_calib; 37 | 38 | cout<<"Importing IMU data from the comma separated file : "<< argv[2]<tmp_intervals, static_intervals; 47 | vectoracc_means; 48 | Vector3d variance = dataVariance( acc_data_test, DataInterval(100, 3000)); 49 | staticIntervalsDetector( acc_data_test, 4*variance.norm(), tmp_intervals); 50 | extractIntervalsSamples( acc_data_test, tmp_intervals, acc_means, static_intervals, 100, true); 51 | 52 | 53 | Plot plot1, plot2; 54 | plot1.plotIntervals(gyro_data_test, static_intervals); 55 | // plot1.plotSamples( gyro_data_test); 56 | 57 | Vector3d gyro_bias = dataMean( gyro_data_test_calib, DataInterval(100, 3000)); 58 | CalibratedTriad bias_calib; 59 | bias_calib.setBias(gyro_bias); 60 | 61 | for(int i = 0; i < gyro_data_test_calib.size(); i++) 62 | gyro_data_test_calib[i] = bias_calib.unbias(gyro_data_test_calib[i]); 63 | 64 | gyro_bias = dataMean( gyro_data_test, DataInterval(100, 3000)); 65 | bias_calib.setBias(gyro_bias); 66 | 67 | for(int i = 0; i < gyro_data_test.size(); i++) 68 | gyro_data_test[i] = bias_calib.unbias(gyro_data_test[i]); 69 | 70 | 71 | // plot1.plotSamples( gyro_data_test); 72 | // waitForKey(); 73 | 74 | 75 | std::ofstream file( "results.mat", ios_base::app ); 76 | if (!file.is_open()) 77 | { 78 | return -1; 79 | } 80 | // 81 | int uncalib_score = 0, calib_score = 0; 82 | cout<<"n_intervals :"< 0.01) 108 | // { 109 | if(res.norm() <= res_calib.norm()) 110 | uncalib_score++; 111 | else 112 | calib_score++; 113 | // } 114 | } 115 | cout<<"Uncalib : "< 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace imu_tk 37 | { 38 | /** @brief Simple container for a data item (e.g., timestamp + x, y, z accelerometers or 39 | * gyroscopes reading */ 40 | template class TriadData_ 41 | { 42 | public: 43 | /** @brief Construct an uninitialized TriadData_ object */ 44 | TriadData_() {}; 45 | 46 | /** @brief Construct a TriadData_ object from a timestamp and three values */ 47 | TriadData_ ( _T timestamp, _T x, _T y, _T z ) : 48 | timestamp_ ( timestamp ), 49 | data_ ( x, y, z ) {}; 50 | 51 | /** @brief Construct an TriadData_ object from a timestamp and an Eigen vector */ 52 | TriadData_ ( _T timestamp, const Eigen::Matrix< _T, 3, 1> &data ) : 53 | timestamp_ ( timestamp ), 54 | data_ ( data ) {}; 55 | 56 | /** @brief Construct an TriadData_ object from a timestamp and an array with 3 elements */ 57 | TriadData_ ( _T timestamp, const _T *data ) : 58 | timestamp_ ( timestamp ), 59 | data_ ( data[0], data[1], data[2] ) {}; 60 | 61 | /** @brief Copy constructor */ 62 | TriadData_( const TriadData_ &o ) : 63 | timestamp_(o.timestamp_), data_(o.data_) {}; 64 | 65 | /** @brief Copy assignment operator */ 66 | TriadData_ & operator = (const TriadData_ &o ) 67 | { 68 | timestamp_ = o.timestamp_; 69 | data_ = o.data_; 70 | return *this; 71 | }; 72 | 73 | /** @brief Copy constructor 74 | * 75 | * Supporting coercion using member template constructor */ 76 | template< typename _newT > 77 | TriadData_( const TriadData_<_newT> &o ) : 78 | timestamp_(_T(o.timestamp())), data_(o.data().template cast<_T>()) 79 | {}; 80 | 81 | /** @brief Copy assignment operator 82 | * 83 | * Supporting coercion using member template assignment operator */ 84 | template< typename _newT > 85 | TriadData_ & operator = (const TriadData_<_newT> &o ) 86 | { 87 | timestamp_ = _T(o.timestamp()); 88 | data_ = o.data().template cast<_T>(); 89 | return *this; 90 | }; 91 | 92 | ~TriadData_() {}; 93 | 94 | inline const _T& timestamp() const 95 | { 96 | return timestamp_; 97 | }; 98 | inline const Eigen::Matrix< _T, 3, 1>& data() const 99 | { 100 | return data_; 101 | }; 102 | inline const _T& operator() ( int index ) const 103 | { 104 | return data_[index]; 105 | }; 106 | inline const _T& x() const 107 | { 108 | return data_[0]; 109 | }; 110 | inline const _T& y() const 111 | { 112 | return data_[1]; 113 | }; 114 | inline const _T& z() const 115 | { 116 | return data_[2]; 117 | }; 118 | 119 | private: 120 | Eigen::Matrix< _T, 3, 1> data_; 121 | _T timestamp_; 122 | }; 123 | 124 | typedef TriadData_ TriadData; 125 | 126 | /** @brief Generates a sequence of characters with a properly formatted 127 | * representation of a TriadData_ instance (triad_data), 128 | * and inserts them into the output stream os. */ 129 | template 130 | std::ostream& operator<<(std::ostream& os, const TriadData_<_T>& triad_data); 131 | 132 | 133 | /** @brief Simple structure that defines a data vector interval by means of an initial and 134 | * and a final index. */ 135 | struct DataInterval 136 | { 137 | /** @brief Default constructor (without parameters, it constructs an invalid interval, 138 | * i.e. both -1 indices ) */ 139 | DataInterval ( int start_idx = -1, int end_idx = -1 ) : 140 | start_idx ( start_idx ), end_idx ( end_idx ) {}; 141 | 142 | /** @brief Provides a DataInterval object from a time interval. The indices are extracted 143 | * from the data samples vector using a nearest neighbor approach. 144 | * 145 | * @param samples Input signal (data samples vector) 146 | * @param start_ts Initial timestamp 147 | * @param end_ts Final timestamp 148 | */ 149 | template 150 | static DataInterval fromTimestamps( const std::vector< TriadData_<_T> > &samples, 151 | _T start_ts, _T end_ts ) 152 | { 153 | if( start_ts < 0 || end_ts <= start_ts ) 154 | throw std::invalid_argument("Invalid timestamps"); 155 | if( samples.size() < 3 ) 156 | throw std::invalid_argument("Invalid data samples vector"); 157 | 158 | int start_idx, end_idx; 159 | if( start_ts <= samples[0].timestamp() ) 160 | start_idx = 0; 161 | else 162 | start_idx = timeToIndex( samples, start_ts ); 163 | 164 | if( end_ts >= samples[samples.size() - 1].timestamp() ) 165 | end_idx = samples.size() - 1; 166 | else 167 | end_idx = timeToIndex( samples, end_ts ); 168 | 169 | return DataInterval( start_idx, end_idx ); 170 | }; 171 | 172 | /** @brief Extracts from the data samples vector a DataInterval object that represents 173 | * the initial interval with a given duration. 174 | * 175 | * @param samples Input signal (data samples vector) 176 | * @param duration Interval duration 177 | */ 178 | template 179 | static DataInterval initialInterval( const std::vector< TriadData_<_T> > &samples, 180 | _T duration ) 181 | { 182 | if( duration <= 0) 183 | throw std::invalid_argument("Invalid interval duration"); 184 | if( samples.size() < 3 ) 185 | throw std::invalid_argument("Invalid data samples vector"); 186 | 187 | _T end_ts = samples[0].timestamp() + duration; 188 | int end_idx; 189 | if ( end_ts >= samples[samples.size() - 1].timestamp() ) 190 | end_idx = samples.size() - 1; 191 | else 192 | end_idx = timeToIndex( samples, end_ts ); 193 | 194 | return DataInterval( 0, end_idx ); 195 | }; 196 | 197 | /** @brief Extracts from the data samples vector a DataInterval object that represents 198 | * the final interval with a given duration. 199 | * 200 | * @param samples Input signal (data samples vector) 201 | * @param duration Interval duration 202 | */ 203 | template 204 | static DataInterval finalInterval( const std::vector< TriadData_<_T> > &samples, 205 | _T duration ) 206 | { 207 | if( duration <= 0) 208 | throw std::invalid_argument("Invalid interval duration"); 209 | if( samples.size() < 3 ) 210 | throw std::invalid_argument("Invalid data samples vector"); 211 | 212 | _T start_ts = samples[samples.size() - 1].timestamp() - duration; 213 | int start_idx; 214 | if ( start_ts <= 0 ) 215 | start_idx = 0; 216 | else 217 | start_idx = timeToIndex( samples, start_ts ); 218 | 219 | return DataInterval( start_idx, samples.size() - 1 ); 220 | }; 221 | 222 | int start_idx, end_idx; 223 | 224 | private: 225 | 226 | template static int timeToIndex( const std::vector< TriadData_<_T> > &samples, 227 | _T ts ) 228 | { 229 | int idx0 = 0, idx1 = samples.size() - 1, idxm; 230 | while( idx1 - idx0 > 1 ) 231 | { 232 | idxm = (idx1 + idx0)/2; 233 | if( ts > samples[idxm].timestamp() ) 234 | idx0 = idxm; 235 | else 236 | idx1 = idxm; 237 | } 238 | 239 | if( ts - samples[idx0].timestamp() < samples[idx1].timestamp() - ts ) 240 | return idx0; 241 | else 242 | return idx1; 243 | }; 244 | }; 245 | 246 | /** @brief Perform a simple consistency check on a target input interval, 247 | * given an input data sample vector, and return the "corrected" 248 | * interval 249 | * 250 | * @param samples Input signal (data samples vector) 251 | * @param interval Interval to be checked 252 | * 253 | * @returns The "corrected" interval 254 | */ 255 | template 256 | DataInterval checkInterval( const std::vector< TriadData_<_T> > &samples, 257 | const DataInterval &interval ); 258 | 259 | /** @brief Compute the arithmetic mean of a sequence of TriadData_ objects. If a valid data 260 | * interval is provided, the mean is computed only inside this interval 261 | * 262 | * @param samples Input signal (data samples vector) 263 | * @param interval Data interval where to compute the mean. If this interval is not valid, 264 | * i.e., one of the two indices is -1, the mean is computed for the whole data 265 | * sequence. 266 | * 267 | * @returns A three dimensional vector representing the mean. 268 | */ 269 | template 270 | Eigen::Matrix< _T, 3, 1> dataMean ( const std::vector< TriadData_<_T> > &samples, 271 | const DataInterval &interval = DataInterval() ); 272 | 273 | /** @brief Compute the variance of a sequence of TriadData_ objects. If a valid data 274 | * interval is provided, the variance is computed only inside this interval 275 | * 276 | * @param samples Input signal (data samples vector) 277 | * @param interval Data interval where to compute the variance. If this interval is not valid, 278 | * i.e., one of the two indices is -1, the variance is computed for the whole data 279 | * sequence. 280 | * 281 | * @returns A three dimensional vector representing the variance. 282 | */ 283 | template 284 | Eigen::Matrix< _T, 3, 1> dataVariance ( const std::vector< TriadData_<_T> > &samples, 285 | const DataInterval &interval = DataInterval() ); 286 | 287 | /** @brief If the flag only_means is set to false, for each interval 288 | * (input vector intervals) extract from the input signal 289 | * (samples) the first interval_n_samps samples, and store them 290 | * in the output vector extracted_samples. If the flag only_means 291 | * is set to true, extract for each interval only the local mean, computed 292 | * in interval with size at least interval_n_samps samples. 293 | * Only intervals with at least interval_n_samps samples are considered. 294 | * 295 | * @param samples Input signal (data samples vector) 296 | * @param intervals Input intervals vector 297 | * @param[out] extracted_samples Output signal that contains the extracted samples 298 | * @param[out] extracted_intervals Output intervals vector with all the used intervals, i.e. 299 | * intervals with size at least interval_n_samps samples 300 | * @param interval_n_samps Number of samples to be extracted from each interval (or 301 | * interval size to be used to compute the local mean if 302 | * only_means is set to true) 303 | * @param only_means If true, extract for each interval only the local mean, computed 304 | * in intervals with size at least interval_n_samps samples. The timestamp is 305 | * the one of the center of the interval. 306 | * 307 | */ 308 | template 309 | void extractIntervalsSamples ( const std::vector< TriadData_<_T> > &samples, 310 | const std::vector< DataInterval > &intervals, 311 | std::vector< TriadData_<_T> > &extracted_samples, 312 | std::vector< DataInterval > &extracted_intervals, 313 | int interval_n_samps = 100, bool only_means = false ); 314 | 315 | /** @brief Decompose a rotation matrix into the roll, pitch, and yaw angular components 316 | * 317 | * @param rot_mat Input rotation matrix 318 | * @param[out] rpy_rot_vec Output roll, pitch, and yaw angular components 319 | */ 320 | template void decomposeRotation( const Eigen::Matrix< _T, 3, 3> &rot_mat, 321 | Eigen::Matrix< _T, 3, 1> &rpy_rot_vec ); 322 | 323 | /* Implementations */ 324 | 325 | template 326 | std::ostream& operator<<(std::ostream& os, const TriadData_<_T>& triad_data) 327 | { 328 | os<<"ts : "< 338 | DataInterval checkInterval( const std::vector< TriadData_<_T> > &samples, 339 | const DataInterval &interval ) 340 | { 341 | int start_idx = interval.start_idx, end_idx = interval.end_idx; 342 | if( start_idx < 0) start_idx = 0; 343 | if( end_idx < start_idx || end_idx > samples.size() - 1 ) 344 | end_idx = samples.size() - 1; 345 | 346 | return DataInterval( start_idx, end_idx ); 347 | } 348 | 349 | template 350 | Eigen::Matrix< _T, 3, 1> dataMean( const std::vector< TriadData_<_T> >& samples, 351 | const DataInterval& interval ) 352 | { 353 | DataInterval rev_interval = checkInterval( samples, interval ); 354 | int n_samp = rev_interval.end_idx - rev_interval.start_idx + 1; 355 | Eigen::Matrix< _T, 3, 1> mean(0, 0, 0); 356 | 357 | for( int i = rev_interval.start_idx; i <= rev_interval.end_idx; i++) 358 | mean += samples[i].data(); 359 | 360 | mean /= _T(n_samp); 361 | 362 | return mean; 363 | } 364 | 365 | template 366 | Eigen::Matrix< _T, 3, 1> dataVariance( const std::vector< TriadData_<_T> >& samples, 367 | const DataInterval& interval ) 368 | { 369 | DataInterval rev_interval = checkInterval( samples, interval ); 370 | int n_samp = rev_interval.end_idx - rev_interval.start_idx + 1; 371 | Eigen::Matrix< _T, 3, 1> mean = dataMean( samples, rev_interval ); 372 | 373 | Eigen::Matrix< _T, 3, 1> variance(0, 0, 0); 374 | for( int i = rev_interval.start_idx; i <= rev_interval.end_idx; i++) 375 | { 376 | Eigen::Matrix< _T, 3, 1> diff = samples[i].data() - mean ; 377 | variance += (diff.array() * diff.array()).matrix(); 378 | } 379 | variance /= _T(n_samp - 1); 380 | 381 | return variance; 382 | } 383 | 384 | template 385 | void extractIntervalsSamples ( const std::vector< TriadData_<_T> >& samples, 386 | const std::vector< DataInterval >& intervals, 387 | std::vector< TriadData_<_T> >& extracted_samples, 388 | std::vector< DataInterval > &extracted_intervals, 389 | int interval_n_samps, bool only_means ) 390 | { 391 | // Check for valid intervals (i.e., intervals with at least interval_n_samps samples) 392 | int n_valid_intervals = 0, n_static_samples; 393 | for( int i = 0; i < intervals.size(); i++) 394 | { 395 | if( ( intervals[i].end_idx - intervals[i].start_idx + 1 ) >= interval_n_samps ) 396 | n_valid_intervals++; 397 | } 398 | 399 | if( only_means ) 400 | n_static_samples = n_valid_intervals; 401 | else 402 | n_static_samples = n_valid_intervals*interval_n_samps; 403 | 404 | extracted_samples.clear(); 405 | extracted_intervals.clear(); 406 | extracted_samples.reserve(n_static_samples); 407 | extracted_intervals.reserve(n_valid_intervals); 408 | 409 | // For each valid interval, extract the first interval_n_samps samples 410 | for( int i = 0; i < intervals.size(); i++) 411 | { 412 | int interval_size = intervals[i].end_idx - intervals[i].start_idx + 1; 413 | if( interval_size >= interval_n_samps ) 414 | { 415 | extracted_intervals.push_back( intervals[i] ); 416 | if( only_means ) 417 | { 418 | DataInterval mean_inerval( intervals[i].start_idx, intervals[i].end_idx ); 419 | // Take the timestamp centered in the interval where the mean is computed 420 | _T timestamp = samples[ intervals[i].start_idx + interval_size/2 ].timestamp(); 421 | Eigen::Matrix< _T, 3, 1> mean_val = dataMean ( samples, mean_inerval ); 422 | extracted_samples.push_back( TriadData_<_T>(timestamp, mean_val ) ); 423 | } 424 | else 425 | { 426 | for(int j = intervals[i].start_idx; j < intervals[i].start_idx + interval_n_samps; j++) 427 | extracted_samples.push_back( samples[j] ); 428 | } 429 | } 430 | } 431 | } 432 | 433 | template void decomposeRotation( const Eigen::Matrix< _T, 3, 3> &rot_mat, 434 | Eigen::Matrix< _T, 3, 1> &rpy_rot_vec ) 435 | { 436 | rpy_rot_vec(0) = atan2(rot_mat(2,1), rot_mat(2,2)); 437 | rpy_rot_vec(1) = atan2(-rot_mat(2,0), sqrt(rot_mat(2,1)*rot_mat(2,1) + rot_mat(2,2)*rot_mat(2,2))); 438 | rpy_rot_vec(2) = atan2(rot_mat(1,0), rot_mat(0,0)); 439 | } 440 | 441 | } -------------------------------------------------------------------------------- /include/imu_tk/calibration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "imu_tk/base.h" 36 | 37 | namespace imu_tk 38 | { 39 | /** @brief This object contains the calibration parameters (misalignment, scale factors, ...) 40 | * of a generic orthogonal sensor triad (accelerometers, gyroscopes, etc.) 41 | * 42 | * Triad model: 43 | * 44 | * -Misalignment matrix: 45 | * 46 | * general case: 47 | * 48 | * [ 1 -mis_yz mis_zy ] 49 | * T = [ mis_xz 1 -mis_zx ] 50 | * [ -mis_xy mis_yx 1 ] 51 | * 52 | * "body" frame spacial case: 53 | * 54 | * [ 1 -mis_yz mis_zy ] 55 | * T = [ 0 1 -mis_zx ] 56 | * [ 0 0 1 ] 57 | * 58 | * Scale matrix: 59 | * 60 | * [ s_x 0 0 ] 61 | * K = [ 0 s_y 0 ] 62 | * [ 0 0 s_z ] 63 | * 64 | * Bias vector: 65 | * 66 | * [ b_x ] 67 | * B = [ b_y ] 68 | * [ b_z ] 69 | * 70 | * Given a raw sensor reading X (e.g., the acceleration ), the calibrated "unbiased" reading X' is obtained 71 | * 72 | * X' = T*K*(X - B) 73 | * 74 | * with B the bias (variable) + offset (constant, possibbly 0), or, equivalently: 75 | * 76 | * X' = T*K*X - B' 77 | * 78 | * with B' = T*K*B 79 | * 80 | * Without knowing the value of the bias (and with offset == 0), the calibrated reading X'' is simply: 81 | * 82 | * X'' = T*K*X 83 | */ 84 | template < typename _T > class CalibratedTriad_ 85 | { 86 | public: 87 | /** @brief Basic "default" constructor: without any parameter, it initilizes the calibration parameter with 88 | * default values (zero scaling factors and biases, identity misalignment matrix) 89 | */ 90 | CalibratedTriad_( const _T &mis_yz = _T(0), const _T &mis_zy = _T(0), const _T &mis_zx = _T(0), 91 | const _T &mis_xz = _T(0), const _T &mis_xy = _T(0), const _T &mis_yx = _T(0), 92 | const _T &s_x = _T(1), const _T &s_y = _T(1), const _T &s_z = _T(1), 93 | const _T &b_x = _T(0), const _T &b_y = _T(0), const _T &b_z = _T(0) ); 94 | 95 | ~CalibratedTriad_(){}; 96 | 97 | inline _T misYZ() const { return -mis_mat_(0,1); }; 98 | inline _T misZY() const { return mis_mat_(0,2); }; 99 | inline _T misZX() const { return -mis_mat_(1,2); }; 100 | inline _T misXZ() const { return mis_mat_(1,0); }; 101 | inline _T misXY() const { return -mis_mat_(2,0); }; 102 | inline _T misYX() const { return mis_mat_(2,1); }; 103 | 104 | inline _T scaleX() const { return scale_mat_(0,0); }; 105 | inline _T scaleY() const { return scale_mat_(1,1); }; 106 | inline _T scaleZ() const { return scale_mat_(2,2); }; 107 | 108 | inline _T biasX() const { return bias_vec_(0); }; 109 | inline _T biasY() const { return bias_vec_(1); }; 110 | inline _T biasZ() const { return bias_vec_(2); }; 111 | 112 | inline const Eigen::Matrix< _T, 3 , 3>& getMisalignmentMatrix() const { return mis_mat_; }; 113 | inline const Eigen::Matrix< _T, 3 , 3>& getScaleMatrix() const { return scale_mat_; }; 114 | inline const Eigen::Matrix< _T, 3 , 1>& getBiasVector() const { return bias_vec_; }; 115 | 116 | inline void setScale( const Eigen::Matrix< _T, 3 , 1> &s_vec ) 117 | { 118 | scale_mat_(0,0) = s_vec(0); scale_mat_(1,1) = s_vec(1); scale_mat_(2,2) = s_vec(2); 119 | update(); 120 | }; 121 | 122 | inline void setBias( const Eigen::Matrix< _T, 3 , 1> &b_vec ) 123 | { 124 | bias_vec_ = b_vec; 125 | update(); 126 | }; 127 | 128 | /** @brief Load the calibration parameters from a simple text file. 129 | * 130 | * The file should containts a sequence of two, space separated 3X3 matrixes 131 | * (the misalignment and the scale matrix) followed by a 3x1 biases vector (see the load() 132 | * function) 133 | */ 134 | bool load( std::string filename ); 135 | 136 | /** @brief Save the calibration parameters in a simple text file. 137 | * 138 | * The file will containts a sequence of two, space separated 3X3 matrixes 139 | * (the misalignment and the scale matrix) followed by a 3x1 biases vector 140 | */ 141 | bool save( std::string filename ) const; 142 | 143 | /** @brief Normalize a raw data X by correcting the misalignment and the scale, 144 | * i.e., by applying the equation X'' = T*K*X 145 | */ 146 | inline Eigen::Matrix< _T, 3 , 1> normalize( const Eigen::Matrix< _T, 3 , 1> &raw_data ) const 147 | { 148 | return ms_mat_*raw_data; 149 | }; 150 | 151 | /** @brief Normalize a raw data X by correcting the misalignment and the scale, 152 | * i.e., by applying the equation X'' = T*K*X 153 | */ 154 | inline TriadData_<_T> normalize( const TriadData_<_T> &raw_data ) const 155 | { 156 | return TriadData_<_T>( raw_data.timestamp(), normalize( raw_data.data()) ); 157 | }; 158 | 159 | /** @brief Normalize a raw data X by removing the biases and 160 | * correcting the misalignment and the scale, 161 | * i.e., by applying the equation X' = T*K*(X - B) 162 | */ 163 | inline Eigen::Matrix< _T, 3 , 1> unbiasNormalize( const Eigen::Matrix< _T, 3 , 1> &raw_data ) const 164 | { 165 | return ms_mat_*(raw_data - bias_vec_); 166 | }; 167 | 168 | /** @brief Normalize a raw data X by removing the biases and 169 | * correcting the misalignment and the scale, 170 | * i.e., by applying the equation X' = T*K*(X - B) 171 | */ 172 | inline TriadData_<_T> unbiasNormalize( const TriadData_<_T> &raw_data ) const 173 | { 174 | return TriadData_<_T>( raw_data.timestamp(), unbiasNormalize( raw_data.data()) ); 175 | }; 176 | 177 | /** @brief Remove the biases from a raw data */ 178 | inline Eigen::Matrix< _T, 3 , 1> unbias( const Eigen::Matrix< _T, 3 , 1> &raw_data ) const 179 | { 180 | return raw_data - bias_vec_; 181 | }; 182 | 183 | /** @brief Remove the biases from a raw data */ 184 | inline TriadData_<_T> unbias( const TriadData_<_T> &raw_data ) const 185 | { 186 | return TriadData_<_T>( raw_data.timestamp(), unbias( raw_data.data()) ); 187 | }; 188 | 189 | private: 190 | 191 | /** @brief Update internal data (e.g., compute Misalignment * scale matrix) 192 | * after a parameter is changed */ 193 | void update(); 194 | 195 | /** @brief Misalignment matrix */ 196 | Eigen::Matrix< _T, 3 , 3> mis_mat_; 197 | /** @brief Scale matrix */ 198 | Eigen::Matrix< _T, 3 , 3> scale_mat_; 199 | /** @brief Bias vector */ 200 | Eigen::Matrix< _T, 3 , 1> bias_vec_; 201 | /** @brief Misalignment * scale matrix */ 202 | Eigen::Matrix< _T, 3 , 3> ms_mat_; 203 | }; 204 | 205 | typedef CalibratedTriad_ CalibratedTriad; 206 | 207 | /** @brief Generates a sequence of characters with a properly formatted 208 | * representation of a CalibratedTriad_ instance (calib_triad), 209 | * and inserts them into the output stream os. */ 210 | template std::ostream& operator<<(std::ostream& os, 211 | const imu_tk::CalibratedTriad_<_T>& calib_triad); 212 | 213 | /** @brief This object enables to calibrate an accelerometers triad and eventually 214 | * a related gyroscopes triad (i.e., to estimate theirs misalignment matrix, 215 | * scale factors and biases) using the multi-position calibration method. 216 | * 217 | * For more details, please see: 218 | * 219 | * D. Tedaldi, A. Pretto and E. Menegatti 220 | * "A Robust and Easy to Implement Method for IMU Calibration without External Equipments" 221 | * In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA 2014), 222 | * May 31 - June 7, 2014 Hong Kong, China, Page(s): 3042 - 3049 223 | */ 224 | template class MultiPosCalibration_ 225 | { 226 | public: 227 | 228 | /** @brief Default constructor: initilizes all the internal members with default values */ 229 | MultiPosCalibration_(); 230 | ~MultiPosCalibration_(){}; 231 | 232 | /** @brief Provides the magnitude of the gravitational filed 233 | * used in the calibration (i.e., the gravity measured in 234 | * the place where the calibration dataset has been acquired) */ 235 | _T gravityMagnitede() const { return g_mag_; }; 236 | 237 | /** @brief Provides the duration in seconds of the initial static interval */ 238 | _T initStaticIntervalDuration() const { return init_interval_duration_; }; 239 | 240 | /** @brief Provides the number of data samples to be extracted from each detected static intervals */ 241 | int intarvalsNumSamples() const { return interval_n_samples_; }; 242 | 243 | /** @brief Provides the accelerometers initial guess calibration parameters */ 244 | const CalibratedTriad_<_T>& initAccCalibration(){ return init_acc_calib_; }; 245 | 246 | /** @brief Provides the gyroscopes initial guess calibration parameters */ 247 | const CalibratedTriad_<_T>& initGyroCalibration(){ return init_gyro_calib_; }; 248 | 249 | /** @brief True if the accelerometers calibration is obtained using the mean 250 | * accelerations of each static interval instead of all samples */ 251 | bool accUseMeans() const { return acc_use_means_; }; 252 | 253 | /** @brief Provides the (fixed) data period used in the gyroscopes integration. 254 | * If this period is less than 0, the gyroscopes timestamps are used 255 | * in place of this period. */ 256 | _T gyroDataPeriod() const{ return gyro_dt_; }; 257 | 258 | /** @brief True if the gyroscopes biases are estimated along with the calibration 259 | * parameters. If false, the gyroscopes biases (computed in the initial static 260 | * period) are assumed known. */ 261 | bool optimizeGyroBias() const { return optimize_gyro_bias_; }; 262 | 263 | /** @brief True if the verbose output is enabled */ 264 | bool verboseOutput() const { return verbose_output_; }; 265 | 266 | /** @brief Set the magnitude of the gravitational filed 267 | * used in the calibration (i.e., the gravity measured in 268 | * the place where the calibration dataset has been acquired) 269 | * 270 | * To find your magnitude of the gravitational filed, 271 | * take a look for example to https://www.wolframalpha.com 272 | */ 273 | void setGravityMagnitude( _T g ){ g_mag_ = g; }; 274 | 275 | /** @brief Set the duration in seconds of the initial static interval. Default 30 seconds. */ 276 | _T setInitStaticIntervalDuration( _T duration ) { init_interval_duration_ = duration; }; 277 | 278 | /** @brief Set the number of data samples to be extracted from each detected static intervals. 279 | * Default is 100. */ 280 | int setIntarvalsNumSamples( int num ) { interval_n_samples_ = num; }; 281 | 282 | /** @brief Set the accelerometers initial guess calibration parameters */ 283 | void setInitAccCalibration( CalibratedTriad_<_T> &init_calib ){ init_acc_calib_ = init_calib; }; 284 | 285 | /** @brief Set the gyroscopes initial guess calibration parameters */ 286 | void setInitGyroCalibration( CalibratedTriad_<_T> &init_calib ){ init_gyro_calib_ = init_calib; }; 287 | 288 | /** @brief If the parameter enabled is true, the accelerometers calibration is obtained 289 | * using the mean accelerations of each static interval instead of all samples. 290 | * Default is false. 291 | */ 292 | void enableAccUseMeans ( bool enabled ){ acc_use_means_ = enabled; }; 293 | 294 | /** @brief Set the (fixed) data period used in the gyroscopes integration. 295 | * If this period is less than 0, the gyroscopes timestamps are used 296 | * in place of this period. Default is -1. 297 | */ 298 | void setGyroDataPeriod( _T dt ){ gyro_dt_ = dt; }; 299 | 300 | /** @brief If the parameter enabled is true, the gyroscopes biases are estimated along 301 | * with the calibration parameters. If false, the gyroscopes biases 302 | * (computed in the initial static period) are assumed known. */ 303 | bool enableGyroBiasOptimization( bool enabled ) { optimize_gyro_bias_ = enabled; }; 304 | 305 | /** @brief If the parameter enabled is true, verbose output is activeted */ 306 | void enableVerboseOutput( bool enabled ){ verbose_output_ = enabled; }; 307 | 308 | /** @brief Estimate the calibration parameters for the acceleremoters triad 309 | * (see CalibratedTriad_) using the multi-position calibration method 310 | * 311 | * @param acc_samples Acceleremoters data vector, ordered by increasing timestamps, 312 | * collected at the sensor data rate. 313 | */ 314 | bool calibrateAcc( const std::vector< TriadData_<_T> > &acc_samples ); 315 | 316 | /** @brief Estimate the calibration parameters for both the acceleremoters 317 | * and the gyroscopes triads (see CalibratedTriad_) using the 318 | * multi-position calibration method 319 | * 320 | * @param acc_samples Acceleremoters data vector, ordered by increasing timestamps, 321 | * collected at the sensor data rate. 322 | * @param gyro_samples Gyroscopes data vector, ordered by increasing timestamps, 323 | * collected in parallel with the acceleations 324 | * at the sensor data rate. 325 | */ 326 | bool calibrateAccGyro( const std::vector< TriadData_<_T> > &acc_samples, 327 | const std::vector< TriadData_<_T> > &gyro_samples ); 328 | 329 | /** @brief Provide the calibration parameters for the acceleremoters triad (it should be called after 330 | * calibrateAcc() or calibrateAccGyro() ) */ 331 | const CalibratedTriad_<_T>& getAccCalib() const { return acc_calib_; }; 332 | /** @brief Provide the calibration parameters for the gyroscopes triad (it should be called after 333 | * calibrateAccGyro() ). */ 334 | const CalibratedTriad_<_T>& getGyroCalib() const { return gyro_calib_; }; 335 | 336 | /** @brief Provide the calibrated acceleremoters data vector (it should be called after 337 | * calibrateAcc() or calibrateAccGyro() ) */ 338 | const std::vector< TriadData_<_T> >& getCalibAccSamples() const { return calib_acc_samples_; }; 339 | 340 | /** @brief Provide the calibrated gyroscopes data vector (it should be called after 341 | * calibrateAccGyro() ) */ 342 | const std::vector< TriadData_<_T> >& getCalibGyroSamples() const { return calib_gyro_samples_; }; 343 | 344 | private: 345 | 346 | _T g_mag_; 347 | const int min_num_intervals_; 348 | _T init_interval_duration_; 349 | int interval_n_samples_; 350 | bool acc_use_means_; 351 | _T gyro_dt_; 352 | bool optimize_gyro_bias_; 353 | std::vector< DataInterval > min_cost_static_intervals_; 354 | CalibratedTriad_<_T> init_acc_calib_, init_gyro_calib_; 355 | CalibratedTriad_<_T> acc_calib_, gyro_calib_; 356 | std::vector< TriadData_<_T> > calib_acc_samples_, calib_gyro_samples_; 357 | 358 | bool verbose_output_; 359 | }; 360 | 361 | typedef MultiPosCalibration_ MultiPosCalibration; 362 | 363 | } 364 | 365 | /* Implementations */ 366 | 367 | template 368 | imu_tk::CalibratedTriad_<_T>::CalibratedTriad_( const _T &mis_yz, const _T &mis_zy, const _T &mis_zx, 369 | const _T &mis_xz, const _T &mis_xy, const _T &mis_yx, 370 | const _T &s_x, const _T &s_y, const _T &s_z, 371 | const _T &b_x, const _T &b_y, const _T &b_z ) 372 | { 373 | mis_mat_ << _T(1) , -mis_yz , mis_zy , 374 | mis_xz , _T(1) , -mis_zx , 375 | -mis_xy , mis_yx , _T(1) ; 376 | 377 | scale_mat_ << s_x , _T(0) , _T(0) , 378 | _T(0) , s_y , _T(0) , 379 | _T(0) , _T(0) , s_z ; 380 | 381 | bias_vec_ << b_x , b_y , b_z ; 382 | 383 | update(); 384 | } 385 | 386 | template 387 | bool imu_tk::CalibratedTriad_<_T>::load( std::string filename ) 388 | { 389 | std::ifstream file( filename.data() ); 390 | if (file.is_open()) 391 | { 392 | _T mat[9] = {0}; 393 | 394 | for( int i=0; i<9; i++) 395 | file >> mat[i]; 396 | 397 | mis_mat_ = Eigen::Map< const Eigen::Matrix< _T, 3, 3, Eigen::RowMajor> >(mat); 398 | 399 | for( int i=0; i<9; i++) 400 | file >> mat[i]; 401 | 402 | scale_mat_ = Eigen::Map< const Eigen::Matrix< _T, 3, 3, Eigen::RowMajor> >(mat); 403 | 404 | for( int i=0; i<3; i++) 405 | file >> mat[i]; 406 | 407 | bias_vec_ = Eigen::Map< const Eigen::Matrix< _T, 3, 1> >(mat); 408 | 409 | update(); 410 | 411 | return true; 412 | } 413 | return false; 414 | } 415 | 416 | template 417 | bool imu_tk::CalibratedTriad_<_T>::save( std::string filename ) const 418 | { 419 | std::ofstream file( filename.data() ); 420 | if (file.is_open()) 421 | { 422 | file< void imu_tk::CalibratedTriad_<_T>::update() 432 | { 433 | ms_mat_ = mis_mat_*scale_mat_; 434 | } 435 | 436 | template std::ostream& imu_tk::operator<<(std::ostream& os, 437 | const imu_tk::CalibratedTriad_<_T>& calib_triad) 438 | { 439 | os<<"Misalignment Matrix"< 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include "imu_tk/base.h" 33 | 34 | namespace imu_tk 35 | { 36 | 37 | /** 38 | * @brief Classify between static and motion intervals checking if for each sample 39 | * of the input signal \f$s\f$ (samples) the local variance magnitude 40 | * is lower or greater then a threshold. 41 | * 42 | * @param samples Input 3D signal (e.g, the acceleremeter readings) 43 | * @param threshold Threshold used in the classification 44 | * @param[out] intervals Ouput detected static intervals 45 | * @param win_size Size of the sliding window (i.e., number of samples) 46 | * used to compute the local variance magnitude. It should be equal or 47 | * greater than 11 48 | * 49 | * 50 | * The variance magnitude is a scalar computed in a temporal sliding window of size 51 | * \f$w_s\f$ (i.e., win_size) as: 52 | * \f[ \varsigma(t) = 53 | * \sqrt{[var_{w_s}(s^t_x)]^2 + [var_{w_s}(s^t_y)]^2 + [var_{w_s}(s^t_z)]^2} \f] 54 | * 55 | * Where \f$var_{w_s}(s^t)\f$ is an operator that compute the variance of 56 | * a general 1D signal in a interval of length \f$w_s\f$ samples 57 | * centered in \f$t\f$. 58 | */ 59 | template 60 | void staticIntervalsDetector ( const std::vector< TriadData_<_T> > &samples, 61 | _T threshold, std::vector< DataInterval > &intervals, 62 | int win_size = 101 ); 63 | } 64 | -------------------------------------------------------------------------------- /include/imu_tk/imu_tk.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include "imu_tk/base.h" 32 | #include "imu_tk/calibration.h" 33 | #include "imu_tk/io_utils.h" 34 | #include "imu_tk/filters.h" 35 | #include "imu_tk/integration.h" 36 | #include "imu_tk/integration.h" 37 | #include "imu_tk/visualization.h" -------------------------------------------------------------------------------- /include/imu_tk/integration.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include "imu_tk/base.h" 35 | 36 | #include 37 | 38 | namespace imu_tk 39 | { 40 | /** @brief Normalize a input quaternion to an unit vector 41 | * 42 | * @param[in,out] quat The Eigen 4D vector representing the quaternion to be normlized 43 | */ 44 | template inline void normalizeQuaternion( Eigen::Matrix< _T, 4, 1> &quat ); 45 | 46 | /** @brief Normalize a input quaternion to an unit vector 47 | * 48 | * @param[in,out] quat The 4D array representing the quaternion to be normlized 49 | */ 50 | template inline void normalizeQuaternion( _T quat[4] ); 51 | 52 | /** @brief Perform a RK4 Runge-Kutta integration step 53 | * 54 | * @param quat The input Eigen 4D vector representing the initial rotation 55 | * @param omega0 Initial rotational velocity at time t0 56 | * @param omega1 Final rotational velocity at time t1 57 | * @param dt Time step (t1 - t0). 58 | * @param[out] quat_res Resulting final rotation 59 | */ 60 | template inline void quatIntegrationStepRK4( const Eigen::Matrix< _T, 4, 1> &quat, 61 | const Eigen::Matrix< _T, 3, 1> &omega0, 62 | const Eigen::Matrix< _T, 3, 1> &omega1, 63 | const _T &dt, Eigen::Matrix< _T, 4, 1> &quat_res ); 64 | 65 | /** @brief Perform a RK4 Runge-Kutta integration step 66 | * 67 | * @param quat The input 4D array representing the initial rotation 68 | * @param omega0 Initial rotational velocity at time t0 69 | * @param omega1 Final rotational velocity at time t1 70 | * @param dt Time step (t1 - t0). 71 | * @param[out] quat_res Resulting final rotation 72 | */ 73 | template inline void quatIntegrationStepRK4( const _T quat[4], 74 | const _T omega0[3], 75 | const _T omega1[3], 76 | const _T &dt, _T quat_res[4] ); 77 | 78 | /** @brief Integrate a sequence of rotational velocities using the RK4 79 | * Runge-Kutta discrete integration method. The initial rotation is assumed to be the 80 | * identity quaternion. 81 | * 82 | * @param gyro_samples Input gyroscope signal (rotational velocity samples vector) 83 | * @param[out] quat_res Resulting final rotation quaternion 84 | * @param dt Fixed time step (t1 - t0) between samples. If is -1, the sample timestamps are used instead. 85 | * @param interval Data interval where to compute the integration. If this interval is not valid, 86 | * i.e., one of the two indices is -1, the integration is computed for the whole data 87 | * sequence. 88 | */ 89 | template void integrateGyroInterval( const std::vector< TriadData_<_T> > &gyro_samples, 90 | Eigen::Matrix< _T, 4, 1> &quat_res, _T data_dt = _T(-1), 91 | const DataInterval &interval = DataInterval() ); 92 | 93 | /** @brief Integrate a sequence of rotational velocities using the RK4 94 | * Runge-Kutta discrete integration method. The initial rotation is assumed to be the 95 | * identity rotation matrix. 96 | * 97 | * @param gyro_samples Input gyroscope signal (rotational velocity samples vector) 98 | * @param[out] quat_res Resulting final rotation matrix 99 | * @param dt Fixed time step (t1 - t0) between samples. If is -1, the sample timestamps are used instead. 100 | * @param interval Data interval where to compute the integration. If this interval is not valid, 101 | * i.e., one of the two indices is -1, the integration is computed for the whole data 102 | * sequence. 103 | */ 104 | template void integrateGyroInterval( const std::vector< TriadData_<_T> > &gyro_samples, 105 | Eigen::Matrix< _T, 3, 3> &rot_res, _T data_dt = _T(-1), 106 | const DataInterval &interval = DataInterval() ); 107 | 108 | } 109 | 110 | /* Implementation */ 111 | 112 | template inline void imu_tk::normalizeQuaternion ( Eigen::Matrix< _T, 4 , 1 >& quat ) 113 | { 114 | _T quat_norm = quat.norm(); 115 | quat /= quat_norm; 116 | } 117 | 118 | template inline void imu_tk::normalizeQuaternion ( _T quat[4] ) 119 | { 120 | Eigen::Matrix< _T, 4 , 1 > tmp_q = Eigen::Map< Eigen::Matrix< _T, 4 , 1 > >(quat); 121 | imu_tk::normalizeQuaternion ( tmp_q ); 122 | } 123 | 124 | template 125 | static inline void computeOmegaSkew( const Eigen::Matrix< _T, 3, 1> &omega, 126 | Eigen::Matrix< _T, 4, 4> &skew ) 127 | { 128 | skew << _T(0), -omega(0), -omega(1), -omega(2), 129 | omega(0), _T(0), omega(2), -omega(1), 130 | omega(1), -omega(2), _T(0), omega(0), 131 | omega(2), omega(1), -omega(0), _T(0); 132 | } 133 | 134 | template 135 | inline void imu_tk::quatIntegrationStepRK4( const Eigen::Matrix< _T, 4, 1> &quat, 136 | const Eigen::Matrix< _T, 3, 1> &omega0, 137 | const Eigen::Matrix< _T, 3, 1> &omega1, 138 | const _T &dt, Eigen::Matrix< _T, 4, 1> &quat_res ) 139 | { 140 | Eigen::Matrix< _T, 3, 1> omega01 = _T(0.5)*( omega0 + omega1 ); 141 | Eigen::Matrix< _T, 4, 1> k1, k2, k3, k4, tmp_q; 142 | Eigen::Matrix< _T, 4, 4> omega_skew; 143 | 144 | // First Runge-Kutta coefficient 145 | computeOmegaSkew( omega0, omega_skew ); 146 | k1 = _T(0.5)*omega_skew*quat; 147 | // Second Runge-Kutta coefficient 148 | tmp_q = quat + _T(0.5)*dt*k1; 149 | computeOmegaSkew( omega01, omega_skew ); 150 | k2 = _T(0.5)*omega_skew*tmp_q; 151 | // Third Runge-Kutta coefficient (same omega skew as second coeff.) 152 | tmp_q = quat + _T(0.5)*dt*k2; 153 | k3 = _T(0.5)*omega_skew*tmp_q; 154 | // Forth Runge-Kutta coefficient 155 | tmp_q = quat + dt*k3; 156 | computeOmegaSkew( omega1, omega_skew ); 157 | k4 = _T(0.5)*omega_skew*tmp_q; 158 | _T mult1 = _T(1.0)/_T(6.0), mult2 = _T(1.0)/_T(3.0); 159 | quat_res = quat + dt*(mult1*k1 + mult2*k2 + mult2*k3 + mult1*k4); 160 | normalizeQuaternion(quat_res); 161 | } 162 | 163 | template 164 | inline void imu_tk::quatIntegrationStepRK4( const _T quat[4], const _T omega0[3], const _T omega1[3], 165 | const _T &dt, _T quat_res[4] ) 166 | { 167 | const Eigen::Matrix< _T, 4, 1> m_quat = Eigen::Map< const Eigen::Matrix< _T, 4, 1> >(quat); 168 | const Eigen::Matrix< _T, 3, 1> m_omega0 = Eigen::Map< const Eigen::Matrix< _T, 3, 1> >(omega0), 169 | m_omega1 = Eigen::Map< const Eigen::Matrix< _T, 3, 1> >(omega1); 170 | Eigen::Matrix< _T, 4, 1> m_quat_res; 171 | 172 | quatIntegrationStepRK4( m_quat, m_omega0, m_omega1, dt, m_quat_res ); 173 | 174 | quat_res[0] = m_quat_res(0); 175 | quat_res[1] = m_quat_res(1); 176 | quat_res[2] = m_quat_res(2); 177 | quat_res[3] = m_quat_res(3); 178 | } 179 | 180 | template void imu_tk::integrateGyroInterval( const std::vector< TriadData_<_T> > &gyro_samples, 181 | Eigen::Matrix< _T, 4, 1> &quat_res, 182 | _T data_dt, const DataInterval &interval ) 183 | { 184 | DataInterval rev_interval = checkInterval( gyro_samples, interval ); 185 | 186 | quat_res = Eigen::Matrix< _T, 4, 1>(_T(1.0), _T(0), _T(0), _T(0)); // Identity quaternion 187 | 188 | for( int i = rev_interval.start_idx; i < rev_interval.end_idx; i++) 189 | { 190 | _T dt = ( data_dt > _T(0))?data_dt:gyro_samples[i+1].timestamp() - gyro_samples[i].timestamp(); 191 | 192 | quatIntegrationStepRK4( quat_res, 193 | gyro_samples[i].data(), 194 | gyro_samples[i + 1].data(), 195 | dt, 196 | quat_res ); 197 | } 198 | } 199 | 200 | template void imu_tk::integrateGyroInterval( const std::vector< TriadData_<_T> >& gyro_samples, 201 | Eigen::Matrix< _T, 3 , 3 >& rot_res, 202 | _T data_dt, const DataInterval& interval ) 203 | { 204 | Eigen::Matrix< _T, 4, 1> quat_res; 205 | integrateGyroInterval( gyro_samples, quat_res, data_dt, interval ); 206 | ceres::MatrixAdapter<_T, 1, 3> rot_mat = ceres::ColumnMajorAdapter3x3(rot_res.data()); 207 | ceres::QuaternionToRotation( quat_res.data(), rot_mat ); 208 | } -------------------------------------------------------------------------------- /include/imu_tk/io_utils.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | 33 | #include "imu_tk/base.h" 34 | 35 | namespace imu_tk 36 | { 37 | enum TimestampUnit 38 | { 39 | TIMESTAMP_UNIT_SEC = 1, 40 | TIMESTAMP_UNIT_MSEC = 1000, 41 | TIMESTAMP_UNIT_USEC = 1000000, 42 | TIMESTAMP_UNIT_NSEC = 1000000000 43 | }; 44 | 45 | enum DatasetType 46 | { 47 | DATASET_SPACE_SEPARATED, 48 | DATASET_COMMA_SEPARATED 49 | }; 50 | 51 | template 52 | void importAsciiData( const char *filename, 53 | std::vector< TriadData_<_T> > &samples, 54 | TimestampUnit unit = TIMESTAMP_UNIT_USEC, 55 | DatasetType type = DATASET_SPACE_SEPARATED ); 56 | template 57 | void importAsciiData( const char *filename, 58 | std::vector< TriadData_<_T> > &samples0, 59 | std::vector< TriadData_<_T> > &samples1, 60 | TimestampUnit unit = TIMESTAMP_UNIT_USEC, 61 | DatasetType type = DATASET_SPACE_SEPARATED ); 62 | template 63 | void importAsciiData( const char *filename, 64 | std::vector< TriadData_<_T> > &samples0, 65 | std::vector< TriadData_<_T> > &samples1, 66 | std::vector< TriadData_<_T> > &samples2, 67 | TimestampUnit unit = TIMESTAMP_UNIT_USEC, 68 | DatasetType type = DATASET_SPACE_SEPARATED ); 69 | } -------------------------------------------------------------------------------- /include/imu_tk/vis_extra/gl_camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #define PIdiv180 GLfloat(3.1415265359)/GLfloat(180.0) 35 | 36 | 37 | /*! @brief Utility structures and functions used by GLCamera object */ 38 | namespace GLCameraUtils 39 | { 40 | /*! @brief Basic 3D vector */ 41 | struct Vector3D 42 | { 43 | GLfloat x,y,z; 44 | }; 45 | 46 | /*! @brief Return a Vector3D with the given coordinates */ 47 | inline Vector3D vector3D ( GLfloat x, GLfloat y, GLfloat z ); 48 | 49 | /*! @brief Compute the length of a Vector3D. */ 50 | inline GLfloat getVector3DLength ( Vector3D * v ); 51 | 52 | /*! @brief Normalize the coordinates of a Vector3D to the unit vector. */ 53 | inline Vector3D normalizeVector3D ( Vector3D v ); 54 | 55 | /*! @brief Computes the sum between two Vector3D */ 56 | inline Vector3D operator+ ( Vector3D v, Vector3D u ); 57 | 58 | /*! @brief Computes the difference between two Vector3D */ 59 | inline Vector3D operator- ( Vector3D v, Vector3D u ); 60 | 61 | /*! @brief Computes the scalar multiplication between a Vector3D and a scalar */ 62 | inline Vector3D operator* ( Vector3D v, float r ); 63 | 64 | /*! @brief Computes the cross product between two Vector3D */ 65 | inline Vector3D crossProduct ( Vector3D * u, Vector3D * v ); 66 | 67 | /*! @brief Computes the dot product between two Vector3D */ 68 | inline float operator* ( Vector3D v, Vector3D u ); 69 | }; 70 | 71 | /*! 72 | @brief This is an utility class to move inside an Open GL scene. 73 | */ 74 | class GLCamera 75 | { 76 | public: 77 | /*! @brief Object constructor. */ 78 | GLCamera(); 79 | /*! @brief Object destructor. */ 80 | ~GLCamera() {}; 81 | 82 | /*! @brief Reset the position of the camera to the frame origin. */ 83 | void reset(); 84 | 85 | /*! @brief Perform the camera movement, i.e. call the OpenGL functions to change the view */ 86 | void render (); 87 | 88 | /*! 89 | @brief Move the camera relatively to the current position 90 | @param tx X coordinate of the translation vector 91 | @param ty y coordinate of the translation vector 92 | @param tz z coordinate of the translation vector 93 | */ 94 | void move ( GLfloat tx, GLfloat ty, GLfloat tz ); 95 | 96 | /*! 97 | @brief Translate the position relatively to the world frame 98 | @param tx X coordinate of the translation vector 99 | @param ty y coordinate of the translation vector 100 | @param tz z coordinate of the translation vector 101 | */ 102 | void moveAbs ( GLfloat tx, GLfloat ty, GLfloat tz ); 103 | 104 | /*! 105 | @brief Rotate around the X axis 106 | @param angle The angle in deg 107 | */ 108 | void rotateX ( GLfloat angle ); 109 | /*! 110 | @brief Rotate around the Y axis 111 | @param angle The angle in deg 112 | */ 113 | void rotateY ( GLfloat angle ); 114 | /*! 115 | @brief Rotate around the Z axis 116 | @param angle The angle in deg 117 | */ 118 | void rotateZ ( GLfloat angle ); 119 | 120 | /*! 121 | @brief Translate forward 122 | @param distance The distance to translate 123 | */ 124 | void moveForward ( GLfloat distance ); 125 | /*! 126 | @brief Translate upward 127 | @param distance The distance to translate 128 | */ 129 | void moveUpward ( GLfloat distance ); 130 | /*! 131 | @brief Translate sideways 132 | @param distance The distance to translate 133 | */ 134 | void strafeRight ( GLfloat distance ); 135 | 136 | private: 137 | 138 | GLCameraUtils::Vector3D view_dir_; 139 | GLCameraUtils::Vector3D right_vector_; 140 | GLCameraUtils::Vector3D up_vector_; 141 | //! Current translation of the camera 142 | GLCameraUtils::Vector3D position_; 143 | 144 | //! Current rotation (euler angles) of the camera 145 | GLfloat rot_x, rot_y, rot_z; 146 | 147 | }; 148 | -------------------------------------------------------------------------------- /include/imu_tk/vis_extra/opengl_3d_scene.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include "gl_camera.h" 43 | 44 | /** @brief This class provides to the visualization inside a QWidget of simple 3D structures, 45 | using the OpenGL library */ 46 | class OpenGL3DScene: public QGLWidget 47 | { 48 | 49 | Q_OBJECT 50 | 51 | public: 52 | 53 | OpenGL3DScene( QWidget* parent = 0, QColor background_color = QColor(0,0,0), 54 | bool global_axes = true, QColor global_frame_color = QColor(255,255,255)); 55 | 56 | ~OpenGL3DScene(); 57 | 58 | void registerPath( std::string name, QColor path_color = QColor(255,255,255) ); 59 | void unregisterPath( std::string name ); 60 | void pushPosition( std::string name, const Eigen::Vector3d &t ); 61 | 62 | void registerLine( std::string name, QColor line_color = QColor(255,255,255) ); 63 | void unregisterLine( std::string name ); 64 | void setLine( std::string name, const Eigen::Vector3d& p0, const Eigen::Vector3d& p1 ); 65 | 66 | void registerCloud( std::string name, QColor cloud_color = QColor(255,255,255) ); 67 | void unregisterCloud( std::string name ); 68 | void pushCloud( std::string name, const std::vector< Eigen::Vector3d > &pts ); 69 | 70 | void registerAxes( std::string name, QColor axes_color = QColor(255,255,255) ); 71 | void unregisterAxes( std::string name ); 72 | void setAxesPos( std::string name, const Eigen::Vector3d &r, const Eigen::Vector3d &t ); 73 | 74 | void unregisterAllStructures(); 75 | 76 | void resetScene() 77 | { 78 | //TODO 79 | max_x_ = max_y_= -std::numeric_limits< float >::max(); 80 | min_x_ = min_y_ =std::numeric_limits< float >::min(); 81 | }; 82 | 83 | void updateNow(); 84 | 85 | void enableGlobalAxes( bool enable, QColor color = QColor(0,0,0) ); 86 | void setCameraIncrements(float translation_inc, float rotation_inc ) 87 | { 88 | t_inc_ = GLfloat(translation_inc); 89 | r_inc_ = GLfloat(rotation_inc); 90 | }; 91 | 92 | 93 | protected: 94 | 95 | /*! 96 | @brief Sets up the OpenGL rendering context, defines display lists, etc. (QGLWidget). 97 | 98 | Gets called once before the first time resizeGL() or paintGL() is called. 99 | DON'T call this function directly! 100 | */ 101 | void initializeGL(); 102 | /*! 103 | @brief Renders the OpenGL scene (QGLWidget). 104 | 105 | Gets called whenever the widget needs to be updated. 106 | DON'T call this function directly! If you need to trigger a repaint, you should call the widget's updateGL() function. 107 | */ 108 | void paintGL(); 109 | 110 | /*! 111 | @brief Sets up the OpenGL viewport, projection, etc. (QGLWidget). 112 | 113 | @param width New width of the widget 114 | @param height New height of the widget 115 | 116 | Gets called whenever the widget has been resized (and also when it is shown for the first time because all newly 117 | created widgets get a resize event automatically). 118 | DON'T call this function directly! 119 | */ 120 | void resizeGL(int width, int height); 121 | 122 | /*! 123 | @brief Event handler to receive key press events for the widget. 124 | 125 | @param event The key press event. 126 | */ 127 | virtual void keyPressEvent ( QKeyEvent * event ); 128 | virtual void wheelEvent ( QWheelEvent * event ); 129 | virtual void mousePressEvent ( QMouseEvent * event ); 130 | virtual void mouseReleaseEvent ( QMouseEvent * event ); 131 | virtual void mouseMoveEvent ( QMouseEvent * event ); 132 | private: 133 | /*! 134 | @brief Compute the translation of the GL "camera" that should be performed in order to view all the 135 | scene (only for global upper view, see setGlobalView() ). 136 | */ 137 | void autoAdjustGView(); 138 | 139 | 140 | //! Mutex varable for data synchronization. 141 | QMutex mutex_; 142 | //! Flag used to activate automatic adjustment that allows to view all the scene (only for global upper view, see setGlobalView() ). 143 | bool auto_adjust_; 144 | float auto_adjust_offset_; 145 | //! The utility object used to move inside an Open GL scene. 146 | GLCamera camera_; 147 | //! The background color of the scene 148 | QColor bg_color_; 149 | //! Draw x,y,z-axis flag 150 | bool draw_axes_; 151 | //! First person view flag 152 | bool fp_view_ ; 153 | 154 | struct PathItem 155 | { 156 | PathItem(){ removed = false; }; 157 | std::vector< Eigen::Vector3d > unprocessed_poses; 158 | std::vector< GLuint > lists; 159 | QColor color; 160 | bool removed; 161 | }; 162 | 163 | struct LineItem 164 | { 165 | LineItem(){ updated = removed = false; }; 166 | Eigen::Vector3d p0, p1; 167 | GLuint list; 168 | QColor color; 169 | bool updated, removed; 170 | }; 171 | 172 | struct AxesItem 173 | { 174 | AxesItem(){ updated = removed = false; }; 175 | Eigen::Vector3d r, t; 176 | QColor color; 177 | bool updated; 178 | GLuint list; 179 | GLUquadric *qobjs[7]; 180 | bool removed; 181 | }; 182 | 183 | struct CloudItem 184 | { 185 | CloudItem(){ removed = false; }; 186 | std::vector< Eigen::Vector3d > unprcessed_cloud; 187 | std::vector< GLuint > lists; 188 | QColor color; 189 | bool removed; 190 | }; 191 | 192 | std::map path_items_; 193 | std::map line_items_; 194 | std::map cloud_items_; 195 | std::map axes_items_; 196 | 197 | //! The filed of view angle of the GL "camera". 198 | GLfloat view_angle_; 199 | //! The current form factor of the view port. 200 | GLfloat form_factor_; 201 | //! 2D boundaries (X,Y) of the current drawn scene. 202 | GLfloat max_x_, max_y_, min_x_, min_y_; 203 | //! x,y,z-axes OpenGL display list 204 | GLuint axis_list_; 205 | //! Translation increment while moving the camera 206 | GLfloat t_inc_; 207 | //! Rotation increment while moving the camera 208 | GLfloat r_inc_; 209 | 210 | bool left_mouse_button_pressed_, right_mouse_button_pressed_; 211 | int mouse_init_x_, mouse_init_y_; 212 | 213 | signals: 214 | 215 | void zoomChanged(int value); 216 | void sceneUpdated(); 217 | 218 | public slots: 219 | 220 | void updateStructure( std::string name, bool update_gl = true ); 221 | void updateAllStructure(); 222 | 223 | /*! 224 | @brief Move and rotate the camera. 225 | 226 | @param tx Camera X position 227 | @param ty Camera Y position 228 | @param tz Camera Z position 229 | @param r1x First rotation (degrees) around X axis 230 | @param r2y Second rotation (degrees) around Y axis 231 | @param r3z Third rotation (degrees) around Z axis 232 | 233 | Camera is moved to the (tx, ty, tz) position then: 1) it is rotated of r1x degrees arount X axis; 234 | 2) it is rotated of r2y degrees arount Y axis and 3) finally it is rotated of r3z degrees arount Z axis 235 | */ 236 | void moveCamera(GLfloat tx, GLfloat ty, GLfloat tz, GLfloat r1x, GLfloat r2y, GLfloat r3z); 237 | 238 | /*! 239 | @brief Enable or disable the automatic adjustment of the viewport in order to show the whole scene 240 | (only for global upper view, see setGlobalView()). 241 | */ 242 | void setAutoAdjust(bool enable, float offset = 20.0 ) { auto_adjust_ = enable; auto_adjust_offset_ = offset; }; 243 | 244 | 245 | /*! 246 | @brief Set the global upper view. 247 | */ 248 | void setGlobalView(); 249 | 250 | /*! 251 | @brief Set the first person view. 252 | 253 | @param good_pos Move the camera in a convenient position for the first person view 254 | */ 255 | void setFirstPersonView( bool good_pos = true ); 256 | 257 | void setZoom( int z ); 258 | 259 | void moveCameraUp( GLfloat inc = 0.6f ) 260 | { QMutexLocker locker(&mutex_); camera_.moveUpward(inc); updateGL(); }; 261 | void moveCameraDown( GLfloat inc = 0.6f ) 262 | { QMutexLocker locker(&mutex_); camera_.moveUpward(-inc); updateGL(); }; 263 | void moveCameraLeft( GLfloat inc = 0.6f ) 264 | { QMutexLocker locker(&mutex_);camera_.strafeRight(-inc); updateGL(); }; 265 | void moveCameraRight( GLfloat inc = 0.6f ) 266 | { QMutexLocker locker(&mutex_); camera_.strafeRight(inc); updateGL(); }; 267 | void moveCameraForward( GLfloat inc = 0.6f ) 268 | { QMutexLocker locker(&mutex_); camera_.moveForward( inc ); updateGL(); }; 269 | void moveCameraBackward(GLfloat inc = 0.6f) 270 | { QMutexLocker locker(&mutex_); camera_.moveForward( -inc ); updateGL(); }; 271 | void turnCameraCCWY(GLfloat inc = 5.0f) 272 | { QMutexLocker locker(&mutex_); camera_.rotateY(inc); updateGL(); }; 273 | void turnCameraCWY(GLfloat inc = 5.0f) 274 | { QMutexLocker locker(&mutex_); camera_.rotateY(-inc); updateGL(); }; 275 | void turnCameraCCWX(GLfloat inc = 5.0f) 276 | { QMutexLocker locker(&mutex_); camera_.rotateX(inc); updateGL(); }; 277 | void turnCameraCWX(GLfloat inc = 5.0f) 278 | { QMutexLocker locker(&mutex_); camera_.rotateX(-inc); updateGL(); }; 279 | void turnCameraCCWZ(GLfloat inc = 5.0f) 280 | { QMutexLocker locker(&mutex_); camera_.rotateZ(inc); updateGL(); }; 281 | void turnCameraCWZ(GLfloat inc = 5.0f) 282 | { QMutexLocker locker(&mutex_); camera_.rotateZ(-inc); updateGL(); }; 283 | 284 | }; 285 | -------------------------------------------------------------------------------- /include/imu_tk/visualization.h: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | #include "imu_tk/base.h" 36 | 37 | 38 | namespace imu_tk 39 | { 40 | 41 | class Plot 42 | { 43 | public: 44 | Plot(); 45 | ~Plot(){}; 46 | 47 | template 48 | void plotSamples( const std::vector< TriadData_<_T> > &samples, 49 | DataInterval range = DataInterval() ); 50 | template 51 | void plotIntervals( const std::vector< TriadData_<_T> > &samples, 52 | const std::vector< DataInterval > &intervals, 53 | DataInterval range = DataInterval() ); 54 | private: 55 | 56 | /* Pimpl idiom */ 57 | class PlotImpl; 58 | boost::shared_ptr< PlotImpl > plot_impl_ptr_; 59 | }; 60 | 61 | void waitForKey(); 62 | 63 | 64 | class Vis3D 65 | { 66 | public: 67 | 68 | Vis3D( const std::string win_name = "imu_tk" ); 69 | ~Vis3D(){}; 70 | 71 | void registerFrame( std::string name, uint8_t r = 255, uint8_t g = 255, uint8_t b = 255 ); 72 | void unregisterFrame( std::string name ); 73 | template 74 | void setFramePos( std::string name, const _T quat[4], const _T t[3] ); 75 | 76 | void registerLine( std::string name, uint8_t r = 255, uint8_t g = 255, uint8_t b = 255 ); 77 | void unregisterLine( std::string name ); 78 | template 79 | void setLinePos( std::string name, const _T p0[3], const _T p1[3] ); 80 | 81 | void updateAndWait( int delay_ms = 0 ); 82 | 83 | private: 84 | 85 | /* Pimpl idiom */ 86 | class VisualizerImpl; 87 | boost::shared_ptr< VisualizerImpl > vis_impl_ptr_; 88 | }; 89 | 90 | 91 | } -------------------------------------------------------------------------------- /lib/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Neil-Oyoung/imu_tk/a46a76364429b22d1bf313bbfb4ae3e8e2ec50fd/lib/.gitignore -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_tk 4 | 0.0.0 5 | The imu_tk package 6 | 7 | 8 | 9 | 10 | neil 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rosbag 53 | sensor_msgs 54 | rosbag 55 | sensor_msgs 56 | rosbag 57 | sensor_msgs 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /src/.gitignore: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Neil-Oyoung/imu_tk/a46a76364429b22d1bf313bbfb4ae3e8e2ec50fd/src/.gitignore -------------------------------------------------------------------------------- /src/calibration.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include "imu_tk/calibration.h" 30 | #include "imu_tk/filters.h" 31 | #include "imu_tk/integration.h" 32 | #include "imu_tk/visualization.h" 33 | 34 | #include 35 | #include 36 | #include "ceres/ceres.h" 37 | 38 | using namespace imu_tk; 39 | using namespace Eigen; 40 | using namespace std; 41 | 42 | template struct MultiPosAccResidual 43 | { 44 | MultiPosAccResidual( const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample ) : 45 | g_mag_(g_mag), 46 | sample_(sample){} 47 | 48 | template 49 | bool operator() ( const _T2* const params, _T2* residuals ) const 50 | { 51 | Eigen::Matrix< _T2, 3 , 1> raw_samp( _T2(sample_(0)), _T2(sample_(1)), _T2(sample_(2)) ); 52 | /* Assume body frame same as accelerometer frame, 53 | * so bottom left params in the misalignment matris are set to zero */ 54 | CalibratedTriad_<_T2> calib_triad( params[0], params[1], params[2], 55 | _T2(0), _T2(0), _T2(0), 56 | params[3], params[4], params[5], 57 | params[6], params[7], params[8] ); 58 | 59 | Eigen::Matrix< _T2, 3 , 1> calib_samp = calib_triad.unbiasNormalize( raw_samp ); 60 | residuals[0] = _T2 ( g_mag_ ) - calib_samp.norm(); 61 | return true; 62 | } 63 | 64 | static ceres::CostFunction* Create ( const _T1 &g_mag, const Eigen::Matrix< _T1, 3 , 1> &sample ) 65 | { 66 | return ( new ceres::AutoDiffCostFunction< MultiPosAccResidual, 1, 9 > ( 67 | new MultiPosAccResidual<_T1>( g_mag, sample ) ) ); 68 | } 69 | 70 | const _T1 g_mag_; 71 | const Eigen::Matrix< _T1, 3 , 1> sample_; 72 | }; 73 | 74 | template struct MultiPosGyroResidual 75 | { 76 | MultiPosGyroResidual( const Eigen::Matrix< _T1, 3 , 1> &g_versor_pos0, 77 | const Eigen::Matrix< _T1, 3 , 1> &g_versor_pos1, 78 | const std::vector< TriadData_<_T1> > &gyro_samples, 79 | const DataInterval &gyro_interval_pos01, 80 | _T1 dt, bool optimize_bias) : 81 | 82 | g_versor_pos0_(g_versor_pos0), 83 | g_versor_pos1_(g_versor_pos1), 84 | gyro_samples_(gyro_samples), 85 | interval_pos01_(gyro_interval_pos01), 86 | dt_(dt), optimize_bias_(optimize_bias){} 87 | 88 | template 89 | bool operator() ( const _T2* const params, _T2* residuals ) const 90 | { 91 | CalibratedTriad_<_T2> calib_triad( params[0], params[1], params[2], 92 | params[3], params[4], params[5], 93 | params[6], params[7], params[8], 94 | optimize_bias_?params[9]:_T2(0), 95 | optimize_bias_?params[10]:_T2(0), 96 | optimize_bias_?params[11]:_T2(0) ); 97 | 98 | std::vector< TriadData_<_T2> > calib_gyro_samples; 99 | calib_gyro_samples.reserve( interval_pos01_.end_idx - interval_pos01_.start_idx + 1 ); 100 | 101 | for( int i = interval_pos01_.start_idx; i <= interval_pos01_.end_idx; i++ ) 102 | calib_gyro_samples.push_back( TriadData_<_T2>( calib_triad.unbiasNormalize( gyro_samples_[i] ) ) ); 103 | 104 | Eigen::Matrix< _T2, 3 , 3> rot_mat; 105 | integrateGyroInterval( calib_gyro_samples, rot_mat, _T2(dt_) ); 106 | 107 | Eigen::Matrix< _T2, 3 , 1> diff = rot_mat.transpose()*g_versor_pos0_.template cast<_T2>() - 108 | g_versor_pos1_.template cast<_T2>(); 109 | 110 | residuals[0] = diff(0); 111 | residuals[1] = diff(1); 112 | residuals[2] = diff(2); 113 | 114 | return true; 115 | } 116 | 117 | static ceres::CostFunction* Create ( const Eigen::Matrix< _T1, 3 , 1> &g_versor_pos0, 118 | const Eigen::Matrix< _T1, 3 , 1> &g_versor_pos1, 119 | const std::vector< TriadData_<_T1> > &gyro_samples, 120 | const DataInterval &gyro_interval_pos01, 121 | _T1 dt, bool optimize_bias ) 122 | { 123 | if( optimize_bias ) 124 | return ( new ceres::AutoDiffCostFunction< MultiPosGyroResidual, 3, 12 > ( 125 | new MultiPosGyroResidual( g_versor_pos0, g_versor_pos1, gyro_samples, 126 | gyro_interval_pos01, dt, optimize_bias ) ) ); 127 | else 128 | return ( new ceres::AutoDiffCostFunction< MultiPosGyroResidual, 3, 9 > ( 129 | new MultiPosGyroResidual( g_versor_pos0, g_versor_pos1, gyro_samples, 130 | gyro_interval_pos01, dt, optimize_bias ) ) ); 131 | } 132 | 133 | const Eigen::Matrix< _T1, 3 , 1> g_versor_pos0_, g_versor_pos1_; 134 | const std::vector< TriadData_<_T1> > gyro_samples_; 135 | const DataInterval interval_pos01_; 136 | const _T1 dt_; 137 | const bool optimize_bias_; 138 | }; 139 | 140 | template 141 | MultiPosCalibration_<_T>::MultiPosCalibration_() : 142 | g_mag_(9.81), 143 | min_num_intervals_(12), 144 | init_interval_duration_(_T(30.0)), 145 | interval_n_samples_(100), 146 | acc_use_means_(false), 147 | gyro_dt_(-1.0), 148 | optimize_gyro_bias_(false), 149 | verbose_output_(false){} 150 | 151 | template 152 | bool MultiPosCalibration_<_T>::calibrateAcc ( const std::vector< TriadData_<_T> >& acc_samples ) 153 | { 154 | cout<<"Accelerometers calibration: calibrating..."< acc_variance = dataVariance( acc_samples, init_static_interval ); 164 | _T norm_th = acc_variance.norm(); 165 | 166 | _T min_cost = std::numeric_limits< _T >::max(); 167 | int min_cost_th = -1; 168 | std::vector< double > min_cost_calib_params; 169 | 170 | for (int th_mult = 2; th_mult <= 10; th_mult++) 171 | { 172 | std::vector< imu_tk::DataInterval > static_intervals; 173 | std::vector< imu_tk::TriadData_<_T> > static_samples; 174 | std::vector< double > acc_calib_params(9); 175 | 176 | acc_calib_params[0] = init_acc_calib_.misYZ(); 177 | acc_calib_params[1] = init_acc_calib_.misZY(); 178 | acc_calib_params[2] = init_acc_calib_.misZX(); 179 | 180 | acc_calib_params[3] = init_acc_calib_.scaleX(); 181 | acc_calib_params[4] = init_acc_calib_.scaleY(); 182 | acc_calib_params[5] = init_acc_calib_.scaleZ(); 183 | 184 | acc_calib_params[6] = init_acc_calib_.biasX(); 185 | acc_calib_params[7] = init_acc_calib_.biasY(); 186 | acc_calib_params[8] = init_acc_calib_.biasZ(); 187 | 188 | std::vector< DataInterval > extracted_intervals; 189 | staticIntervalsDetector ( acc_samples, th_mult*norm_th, static_intervals ); 190 | extractIntervalsSamples ( acc_samples, static_intervals, 191 | static_samples, extracted_intervals, 192 | interval_n_samples_, acc_use_means_ ); 193 | 194 | if(verbose_output_) 195 | cout<<"Accelerometers calibration: extracted "< "; 197 | 198 | // TODO Perform here a quality test 199 | if( extracted_intervals.size() < min_num_intervals_) 200 | { 201 | if( verbose_output_) cout<<"Not enough intervals, calibration is not possible"<::Create ( g_mag_, static_samples[i].data() ); 212 | 213 | problem.AddResidualBlock ( cost_function, NULL /* squared loss */, acc_calib_params.data() ); 214 | } 215 | 216 | ceres::Solver::Options options; 217 | options.linear_solver_type = ceres::DENSE_QR; 218 | options.minimizer_progress_to_stdout = verbose_output_; 219 | 220 | ceres::Solver::Summary summary; 221 | ceres::Solve ( options, &problem, &summary ); 222 | if( summary.final_cost < min_cost) 223 | { 224 | min_cost = summary.final_cost; 225 | min_cost_th = th_mult; 226 | min_cost_static_intervals_ = static_intervals; 227 | min_cost_calib_params = acc_calib_params; 228 | } 229 | cout<<"residual "<( min_cost_calib_params[0], 240 | min_cost_calib_params[1], 241 | min_cost_calib_params[2], 242 | 0,0,0, 243 | min_cost_calib_params[3], 244 | min_cost_calib_params[4], 245 | min_cost_calib_params[5], 246 | min_cost_calib_params[6], 247 | min_cost_calib_params[7], 248 | min_cost_calib_params[8] ); 249 | 250 | calib_acc_samples_.reserve(n_samps); 251 | 252 | // Calibrate the input accelerometer data with the obtained calibration 253 | for( int i = 0; i < n_samps; i++) 254 | calib_acc_samples_.push_back( acc_calib_.unbiasNormalize( acc_samples[i]) ); 255 | 256 | if(verbose_output_) 257 | { 258 | Plot plot; 259 | plot.plotIntervals( calib_acc_samples_, min_cost_static_intervals_); 260 | 261 | cout<<"Accelerometers calibration: Better calibration obtained using threshold multiplier "< 277 | bool MultiPosCalibration_<_T>::calibrateAccGyro ( const vector< TriadData_<_T> >& acc_samples, 278 | const vector< TriadData_<_T> >& gyro_samples ) 279 | { 280 | if( !calibrateAcc( acc_samples ) ) 281 | return false; 282 | 283 | cout<<"Gyroscopes calibration: calibrating..."< > static_acc_means; 286 | std::vector< DataInterval > extracted_intervals; 287 | extractIntervalsSamples ( calib_acc_samples_, min_cost_static_intervals_, 288 | static_acc_means, extracted_intervals, 289 | interval_n_samples_, true ); 290 | 291 | int n_static_pos = static_acc_means.size(), n_samps = gyro_samples.size(); 292 | 293 | // Compute the gyroscopes biases in the (static) initialization interval 294 | DataInterval init_static_interval = DataInterval::initialInterval( gyro_samples, init_interval_duration_ ); 295 | Eigen::Matrix<_T, 3, 1> gyro_bias = dataMean( gyro_samples, init_static_interval ); 296 | 297 | gyro_calib_ = CalibratedTriad_<_T>(0, 0, 0, 0, 0, 0, 298 | 1.0, 1.0, 1.0, 299 | gyro_bias(0), gyro_bias(1), gyro_bias(2) ); 300 | 301 | 302 | // calib_gyro_samples_ already cleared in calibrateAcc() 303 | calib_gyro_samples_.reserve(n_samps); 304 | // Remove the bias 305 | for( int i = 0; i < n_samps; i++ ) 306 | calib_gyro_samples_.push_back(gyro_calib_.unbias(gyro_samples[i])); 307 | 308 | std::vector< double > gyro_calib_params(12); 309 | 310 | gyro_calib_params[0] = init_gyro_calib_.misYZ(); 311 | gyro_calib_params[1] = init_gyro_calib_.misZY(); 312 | gyro_calib_params[2] = init_gyro_calib_.misZX(); 313 | gyro_calib_params[3] = init_gyro_calib_.misXZ(); 314 | gyro_calib_params[4] = init_gyro_calib_.misXY(); 315 | gyro_calib_params[5] = init_gyro_calib_.misYX(); 316 | 317 | gyro_calib_params[6] = init_gyro_calib_.scaleX(); 318 | gyro_calib_params[7] = init_gyro_calib_.scaleY(); 319 | gyro_calib_params[8] = init_gyro_calib_.scaleZ(); 320 | 321 | // Bias has been estimated and removed in the initialization period 322 | gyro_calib_params[9] = 0.0; 323 | gyro_calib_params[10] = 0.0; 324 | gyro_calib_params[11] = 0.0; 325 | 326 | ceres::Problem problem; 327 | 328 | for( int i = 0, t_idx = 0; i < n_static_pos - 1; i++ ) 329 | { 330 | Eigen::Matrix<_T, 3, 1> g_versor_pos0 = static_acc_means[i].data(), 331 | g_versor_pos1 = static_acc_means[i + 1].data(); 332 | 333 | g_versor_pos0 /= g_versor_pos0.norm(); 334 | g_versor_pos1 /= g_versor_pos1.norm(); 335 | 336 | int gyro_idx0 = -1, gyro_idx1 = -1; 337 | _T ts0 = calib_acc_samples_[extracted_intervals[i].end_idx].timestamp(), 338 | ts1 = calib_acc_samples_[extracted_intervals[i + 1].start_idx].timestamp(); 339 | 340 | // Assume monotone signal time 341 | for( ; t_idx < n_samps; t_idx++ ) 342 | { 343 | if( gyro_idx0 < 0 ) 344 | { 345 | if( calib_gyro_samples_[t_idx].timestamp() >= ts0 ) 346 | gyro_idx0 = t_idx; 347 | } 348 | else 349 | { 350 | if( calib_gyro_samples_[t_idx].timestamp() >= ts1 ) 351 | { 352 | gyro_idx1 = t_idx - 1; 353 | break; 354 | } 355 | } 356 | } 357 | 358 | // cout<<"from "<::Create ( g_versor_pos0, g_versor_pos1, calib_gyro_samples_, 367 | gyro_interval, gyro_dt_, optimize_gyro_bias_ ); 368 | 369 | problem.AddResidualBlock ( cost_function, NULL /* squared loss */, gyro_calib_params.data() ); 370 | 371 | } 372 | 373 | ceres::Solver::Options options; 374 | options.linear_solver_type = ceres::DENSE_QR; 375 | options.minimizer_progress_to_stdout = verbose_output_; 376 | 377 | ceres::Solver::Summary summary; 378 | 379 | ceres::Solve ( options, &problem, &summary ); 380 | gyro_calib_ = CalibratedTriad_<_T>( gyro_calib_params[0], 381 | gyro_calib_params[1], 382 | gyro_calib_params[2], 383 | gyro_calib_params[3], 384 | gyro_calib_params[4], 385 | gyro_calib_params[5], 386 | gyro_calib_params[6], 387 | gyro_calib_params[7], 388 | gyro_calib_params[8], 389 | gyro_bias(0) + gyro_calib_params[9], 390 | gyro_bias(1) + gyro_calib_params[10], 391 | gyro_bias(2) + gyro_calib_params[11]); 392 | 393 | // Calibrate the input gyroscopes data with the obtained calibration 394 | for( int i = 0; i < n_samps; i++) 395 | calib_gyro_samples_.push_back( gyro_calib_.unbiasNormalize( gyro_samples[i]) ); 396 | 397 | if(verbose_output_) 398 | { 399 | 400 | cout<; 413 | template class MultiPosCalibration_; -------------------------------------------------------------------------------- /src/filters.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include "imu_tk/filters.h" 30 | 31 | using namespace Eigen; 32 | 33 | template 34 | void imu_tk::staticIntervalsDetector ( const std::vector< imu_tk::TriadData_<_T> >& samples, 35 | _T threshold, std::vector< imu_tk::DataInterval >& intervals, 36 | int win_size ) 37 | { 38 | if ( win_size < 11 ) win_size = 11; 39 | if( !(win_size % 2) ) win_size++; 40 | 41 | int h = win_size / 2; 42 | 43 | if( win_size >= samples.size() ) 44 | return; 45 | 46 | intervals.clear(); 47 | 48 | bool look_for_start = true; 49 | imu_tk::DataInterval current_interval; 50 | 51 | for( int i = h; i < samples.size() - h; i++ ) 52 | { 53 | Matrix< _T, 3, 1> variance = dataVariance( samples, DataInterval( i - h, i + h) ); 54 | _T norm = variance.norm(); 55 | 56 | if( look_for_start ) 57 | { 58 | if (norm < threshold) 59 | { 60 | current_interval.start_idx = i; 61 | look_for_start = false; 62 | } 63 | } 64 | else 65 | { 66 | if (norm >= threshold) 67 | { 68 | current_interval.end_idx = i - 1; 69 | look_for_start = true; 70 | intervals.push_back(current_interval); 71 | } 72 | } 73 | } 74 | 75 | // If the last interval has not been included in the intervals vector 76 | if( !look_for_start ) 77 | { 78 | current_interval.end_idx = samples.size() - h - 1; 79 | //current_interval.end_ts = samples[current_interval.end_idx].timestamp(); 80 | intervals.push_back(current_interval); 81 | } 82 | } 83 | 84 | template void imu_tk::staticIntervalsDetector ( const std::vector< TriadData_ > &samples, 85 | double threshold, std::vector< DataInterval > &intervals, 86 | int win_size = 101 ); 87 | template void imu_tk::staticIntervalsDetector ( const std::vector< TriadData_ > &samples, 88 | float threshold, std::vector< DataInterval > &intervals, 89 | int win_size = 101 ); 90 | -------------------------------------------------------------------------------- /src/io_utils.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include "imu_tk/io_utils.h" 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // TODO Add setlocale() 37 | 38 | using namespace std; 39 | 40 | template 41 | void imu_tk::importAsciiData ( const char *filename, 42 | vector< TriadData_<_T> > &samples, 43 | TimestampUnit unit, DatasetType type ) 44 | { 45 | samples.clear(); 46 | 47 | string line; 48 | ifstream infile; 49 | double ts, d[3]; 50 | 51 | infile.open ( filename ); 52 | if ( infile.is_open() ) 53 | { 54 | char format[266]; 55 | switch ( type ) 56 | { 57 | case DATASET_COMMA_SEPARATED: 58 | sprintf ( format,"%%lf, %%lf, %%lf, %%lf" ); 59 | break; 60 | case DATASET_SPACE_SEPARATED: 61 | default: 62 | sprintf ( format,"%%lf %%lf %%lf %%lf" ); 63 | break; 64 | } 65 | 66 | int l = 0; 67 | while ( getline ( infile,line ) ) 68 | { 69 | int res = sscanf ( line.data(), format, &ts, &d[0], &d[1], &d[2] ); 70 | if ( res != 4 ) 71 | { 72 | cout<<"importAsciiData(): error importing data in line "< ( _T ( ts ), _T ( d[0] ), _T ( d[1] ), _T ( d[2] ) ) ); 78 | } 79 | l++; 80 | } 81 | infile.close(); 82 | } 83 | } 84 | 85 | template 86 | void imu_tk::importAsciiData ( const char *filename, 87 | vector< TriadData_<_T> > &samples0, 88 | vector< TriadData_<_T> > &samples1, 89 | TimestampUnit unit, DatasetType type ) 90 | { 91 | samples0.clear(); 92 | samples1.clear(); 93 | 94 | string line; 95 | ifstream infile; 96 | double ts, d[6]; 97 | 98 | infile.open ( filename ); 99 | if ( infile.is_open() ) 100 | { 101 | char format[266]; 102 | switch ( type ) 103 | { 104 | case DATASET_COMMA_SEPARATED: 105 | sprintf ( format,"%%lf, %%lf, %%lf, %%lf, %%lf, %%lf, %%lf" ); 106 | break; 107 | case DATASET_SPACE_SEPARATED: 108 | default: 109 | sprintf ( format,"%%lf %%lf %%lf %%lf %%lf %%lf %%lf" ); 110 | break; 111 | } 112 | 113 | int l = 0; 114 | while ( getline ( infile,line ) ) 115 | { 116 | int res = sscanf ( line.data(), format, &ts, &d[0], &d[1], &d[2], 117 | &d[3], &d[4], &d[5] ); 118 | if ( res != 7 ) 119 | { 120 | cout<<"importAsciiData(): error importing data in line "< ( _T ( ts ), _T ( d[0] ), _T ( d[1] ), _T ( d[2] ) ) ); 126 | samples1.push_back ( TriadData_<_T> ( _T ( ts ), _T ( d[3] ), _T ( d[4] ), _T ( d[5] ) ) ); 127 | } 128 | l++; 129 | } 130 | infile.close(); 131 | } 132 | } 133 | 134 | template 135 | void imu_tk::importAsciiData ( const char *filename, 136 | vector< TriadData_<_T> > &samples0, 137 | vector< TriadData_<_T> > &samples1, 138 | vector< TriadData_<_T> > &samples2, 139 | TimestampUnit unit, DatasetType type ) 140 | { 141 | samples0.clear(); 142 | samples1.clear(); 143 | samples2.clear(); 144 | 145 | string line; 146 | ifstream infile; 147 | double ts, d[9]; 148 | 149 | infile.open ( filename ); 150 | if ( infile.is_open() ) 151 | { 152 | char format[266]; 153 | switch ( type ) 154 | { 155 | case DATASET_COMMA_SEPARATED: 156 | sprintf ( format,"%%lf, %%lf, %%lf, %%lf, %%lf, %%lf, %%lf, %%lf, %%lf, %%lf" ); 157 | break; 158 | case DATASET_SPACE_SEPARATED: 159 | default: 160 | sprintf ( format,"%%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf %%lf" ); 161 | break; 162 | } 163 | 164 | int l = 0; 165 | while ( getline ( infile,line ) ) 166 | { 167 | int res = sscanf ( line.data(), format, &ts, &d[0], &d[1], &d[2], 168 | &d[3], &d[4], &d[5], &d[6], &d[7], &d[8] ); 169 | if ( res != 10 ) 170 | { 171 | cout<<"importAsciiData(): error importing data in line "< ( _T ( ts ), _T ( d[0] ), _T ( d[1] ), _T ( d[2] ) ) ); 177 | samples1.push_back ( TriadData_<_T> ( _T ( ts ), _T ( d[3] ), _T ( d[4] ), _T ( d[5] ) ) ); 178 | samples2.push_back ( TriadData_<_T> ( _T ( ts ), _T ( d[6] ), _T ( d[7] ), _T ( d[8] ) ) ); 179 | } 180 | l++; 181 | } 182 | infile.close(); 183 | } 184 | } 185 | 186 | template void imu_tk::importAsciiData ( const char *filename, 187 | vector< TriadData_ > &samples, 188 | TimestampUnit unit, DatasetType type ); 189 | template void imu_tk::importAsciiData ( const char *filename, 190 | vector< TriadData_ > &samples, 191 | TimestampUnit unit, DatasetType type ); 192 | template void imu_tk::importAsciiData ( const char *filename, 193 | vector< TriadData_ > &samples0, 194 | vector< TriadData_ > &samples1, 195 | TimestampUnit unit, DatasetType type ); 196 | template void imu_tk::importAsciiData ( const char *filename, 197 | vector< TriadData_ > &samples0, 198 | vector< TriadData_ > &samples1, 199 | TimestampUnit unit, DatasetType type ); 200 | template void imu_tk::importAsciiData ( const char *filename, 201 | vector< TriadData_ > &samples0, 202 | vector< TriadData_ > &samples1, 203 | vector< TriadData_ > &samples2, 204 | TimestampUnit unit, DatasetType type ); 205 | template void imu_tk::importAsciiData ( const char *filename, 206 | vector< TriadData_ > &samples0, 207 | vector< TriadData_ > &samples1, 208 | vector< TriadData_ > &samples2, 209 | TimestampUnit unit, DatasetType type ); 210 | -------------------------------------------------------------------------------- /src/vis_extra/gl_camera.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include "imu_tk/vis_extra/gl_camera.h" 30 | 31 | #include 32 | #include 33 | 34 | GLCamera::GLCamera() 35 | { 36 | reset(); 37 | } 38 | 39 | void GLCamera::reset() 40 | { 41 | position_ = GLCameraUtils::vector3D ( 0.0f, 0.0f, 0.0f ); 42 | view_dir_ = GLCameraUtils::vector3D ( 0.0f, 0.0f, -1.0f ); 43 | right_vector_ = GLCameraUtils::vector3D ( 1.0f, 0.0f, 0.0f ); 44 | up_vector_ = GLCameraUtils::vector3D ( 0.0f, 1.0f, 0.0f ); 45 | 46 | rot_x = rot_y = rot_z = 0.0f; 47 | } 48 | 49 | // std::string GLCamera :: toString() 50 | // { 51 | // std::stringstream ss; 52 | // ss<<"Custom Camera"<x ) * ( v->x ) + ( v->y ) * ( v->y ) + ( v->z ) * ( v->y ) ) ); 142 | } 143 | 144 | GLCameraUtils::Vector3D GLCameraUtils::normalizeVector3D ( Vector3D v ) 145 | { 146 | Vector3D res; 147 | float l = GLCameraUtils::getVector3DLength ( &v ); 148 | if ( l == 0.0f ) return vector3D ( 0.0f,0.0f,0.0f ); 149 | res.x = v.x / l; 150 | res.y = v.y / l; 151 | res.z = v.z / l; 152 | return res; 153 | } 154 | 155 | GLCameraUtils::Vector3D GLCameraUtils::operator + ( Vector3D v, Vector3D u ) 156 | { 157 | Vector3D res; 158 | res.x = v.x+u.x; 159 | res.y = v.y+u.y; 160 | res.z = v.z+u.z; 161 | return res; 162 | } 163 | 164 | GLCameraUtils::Vector3D GLCameraUtils::operator - ( Vector3D v, Vector3D u ) 165 | { 166 | Vector3D res; 167 | res.x = v.x-u.x; 168 | res.y = v.y-u.y; 169 | res.z = v.z-u.z; 170 | return res; 171 | } 172 | 173 | GLCameraUtils::Vector3D GLCameraUtils::operator * ( Vector3D v, float r ) 174 | { 175 | Vector3D res; 176 | res.x = v.x*r; 177 | res.y = v.y*r; 178 | res.z = v.z*r; 179 | return res; 180 | } 181 | 182 | GLCameraUtils::Vector3D GLCameraUtils::crossProduct ( Vector3D * u, Vector3D * v ) 183 | { 184 | Vector3D res; 185 | res.x = u->y*v->z - u->z*v->y; 186 | res.y = u->z*v->x - u->x*v->z; 187 | res.z = u->x*v->y - u->y*v->x; 188 | 189 | return res; 190 | } 191 | 192 | float GLCameraUtils::operator * ( Vector3D v, Vector3D u ) 193 | { 194 | return v.x*u.x+v.y*u.y+v.z*u.z; 195 | } 196 | 197 | 198 | 199 | -------------------------------------------------------------------------------- /src/vis_extra/opengl_3d_scene.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "imu_tk/vis_extra/opengl_3d_scene.h" 39 | 40 | #define OPENGL3DSCENE_DRAW_AXIS_TICKS(val) { \ 41 | glVertex3d(val, -0.02f, 0.0f);\ 42 | glVertex3d(val, 0.02f, 0.0f);\ 43 | glVertex3d(val, 0.0f, -0.02f);\ 44 | glVertex3d(val, 0.0f, 0.012);\ 45 | glVertex3d(-0.02f, val, 0.0f);\ 46 | glVertex3d(0.02f, val, 0.0f);\ 47 | glVertex3d(0.0f, val, -0.02f);\ 48 | glVertex3d(0.0f, val, 0.02f);\ 49 | glVertex3d(-0.01f, 0.0f, val);\ 50 | glVertex3d(0.02f, 0.0f, val);\ 51 | glVertex3d(0.0f, -0.02f, val);\ 52 | glVertex3d(0.0f, 0.02f, val); } 53 | 54 | OpenGL3DScene::OpenGL3DScene ( QWidget* parent, QColor background_color, 55 | bool global_frame, QColor global_frame_color ) 56 | : QGLWidget ( parent ), 57 | mutex_ ( QMutex::Recursive ), 58 | bg_color_ ( background_color ), 59 | left_mouse_button_pressed_(false), 60 | right_mouse_button_pressed_(false), 61 | mouse_init_x_(0), 62 | mouse_init_y_(0) 63 | { 64 | fp_view_ = false; 65 | auto_adjust_ = true; 66 | auto_adjust_offset_ = 20.0; 67 | 68 | view_angle_ = GLfloat ( M_PI/3.0 ); 69 | form_factor_ = 1.0f; 70 | max_x_ = max_y_= -std::numeric_limits< float >::max(); 71 | min_x_ = min_y_ =std::numeric_limits< float >::min(); 72 | 73 | t_inc_ = 0.6f; 74 | r_inc_ = 5.0f; 75 | 76 | makeCurrent(); 77 | axis_list_ = glGenLists ( 1 ); 78 | enableGlobalAxes ( global_frame, global_frame_color ); 79 | camera_.reset(); 80 | 81 | setFocusPolicy ( Qt::StrongFocus ); 82 | 83 | connect( this, SIGNAL( sceneUpdated() ), this, SLOT( updateAllStructure() )); 84 | //setFocusPolicy ( Qt::NoFocus );//WARNING debug!! 85 | } 86 | 87 | OpenGL3DScene::~OpenGL3DScene() 88 | { 89 | unregisterAllStructures(); 90 | } 91 | 92 | void OpenGL3DScene::enableGlobalAxes ( bool enable, QColor color ) 93 | { 94 | draw_axes_ = enable; 95 | 96 | if ( enable ) 97 | { 98 | makeCurrent(); 99 | glNewList ( axis_list_,GL_COMPILE ); 100 | 101 | qglColor ( color ); 102 | GLfloat axis_limit = 1.0f; 103 | 104 | glBegin ( GL_LINES ); 105 | 106 | glVertex3d ( -axis_limit, 0.0f, 0.0f ); 107 | glVertex3d ( axis_limit, 0.0f, 0.0f ); 108 | 109 | glVertex3d ( 0.0f, -axis_limit, 0.0f ); 110 | glVertex3d ( 0.0f, axis_limit, 0.0f ); 111 | 112 | glVertex3d ( 0.0f, 0.0f, -axis_limit ); 113 | glVertex3d ( 0.0f, 0.0f, axis_limit ); 114 | 115 | for ( GLfloat inc = -1.0f; inc <= 1.0f; inc +=0.1f ) 116 | OPENGL3DSCENE_DRAW_AXIS_TICKS ( inc ); 117 | 118 | for ( GLfloat inc = -axis_limit; inc <= axis_limit; inc +=1.0f ) 119 | OPENGL3DSCENE_DRAW_AXIS_TICKS ( inc ); 120 | 121 | glEnd(); 122 | 123 | glEndList(); 124 | } 125 | } 126 | 127 | void OpenGL3DScene::resizeGL ( int width, int height ) 128 | { 129 | /* Specifies the affine transformation of x and y 130 | from normalized device coordinates to window coordinate */ 131 | glViewport ( 0, 0, ( GLint ) width, ( GLint ) height ); 132 | 133 | int side = qMin ( width, height ); 134 | form_factor_ = GLfloat ( width ) /GLfloat ( height ); 135 | 136 | GLfloat clipping_planes_hside = GLfloat ( tanf ( view_angle_/2.0f ) ); 137 | 138 | glMatrixMode ( GL_PROJECTION ); 139 | glLoadIdentity(); 140 | 141 | glFrustum ( -clipping_planes_hside*form_factor_, clipping_planes_hside*form_factor_, 142 | -clipping_planes_hside, clipping_planes_hside, 0.5, 2001.0 ); 143 | 144 | glMatrixMode ( GL_MODELVIEW ); 145 | 146 | } 147 | 148 | void OpenGL3DScene::initializeGL() 149 | { 150 | //glClearColor(0.0, 0.0, 0.0, 0.0); 151 | qglClearColor ( bg_color_ ); 152 | //glShadeModel(GL_FLAT); 153 | glShadeModel ( GL_SMOOTH ); 154 | glEnable ( GL_DEPTH_TEST ); 155 | glEnable ( GL_CULL_FACE ); 156 | glDisable ( GL_LIGHTING ); 157 | glEnable ( GL_NORMALIZE ); 158 | 159 | GLfloat mat_shininess[] = { 50.0 }; 160 | GLfloat light_position[] = { 100.0, 100.0, 100.0, 0.0 }; 161 | GLfloat model_ambient[] = { 0.5, 0.5, 0.5, 1.0 }; 162 | 163 | glMaterialfv ( GL_FRONT, GL_SHININESS, mat_shininess ); 164 | glLightfv ( GL_LIGHT0, GL_POSITION, light_position ); 165 | glLightModelfv ( GL_LIGHT_MODEL_AMBIENT, model_ambient ); 166 | 167 | glEnable ( GL_LIGHT0 ); 168 | 169 | } 170 | 171 | void OpenGL3DScene::paintGL() 172 | { 173 | glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); 174 | 175 | glLoadIdentity(); 176 | camera_.render(); 177 | // Call all object lists 178 | 179 | if ( draw_axes_ ) 180 | glCallList ( axis_list_ ); 181 | 182 | 183 | std::vector< GLuint > lists; 184 | 185 | std::map ::iterator path_iter; 186 | for ( path_iter = path_items_.begin(); path_iter != path_items_.end(); path_iter++ ) 187 | { 188 | lists.insert(lists.end(), path_iter->second.lists.begin(), path_iter->second.lists.end()); 189 | } 190 | 191 | std::map ::iterator cloud_iter; 192 | for ( cloud_iter = cloud_items_.begin(); cloud_iter != cloud_items_.end(); cloud_iter++ ) 193 | { 194 | lists.insert(lists.end(), cloud_iter->second.lists.begin(), cloud_iter->second.lists.end()); 195 | } 196 | 197 | 198 | if( lists.size() ) 199 | { 200 | // Execute the list of display lists 201 | glCallLists ( lists.size(), GL_UNSIGNED_INT, ( const GLvoid * ) lists.data() ); 202 | } 203 | 204 | 205 | std::map ::iterator axes_iter; 206 | for ( axes_iter = axes_items_.begin(); axes_iter != axes_items_.end(); axes_iter++ ) 207 | { 208 | if ( axes_iter->second.list != GL_INVALID_VALUE ) 209 | { 210 | glCallList ( axes_iter->second.list ); 211 | } 212 | } 213 | } 214 | 215 | void OpenGL3DScene::setZoom ( int z ) 216 | { 217 | 218 | // if( !fp_view ) 219 | // { 220 | // zT = float(z); 221 | // updateGL(); 222 | // } 223 | 224 | } 225 | 226 | void OpenGL3DScene::updateNow() 227 | { 228 | sceneUpdated(); 229 | } 230 | 231 | void OpenGL3DScene::updateStructure( std::string name, bool update_gl ) 232 | { 233 | makeCurrent(); 234 | 235 | std::map ::iterator path_iter; 236 | if( (path_iter = path_items_.find(name)) != path_items_.end() ) 237 | { 238 | PathItem &p_item = path_iter->second; 239 | if( p_item.removed ) 240 | { 241 | for( int i = 0; i < path_iter->second.lists.size(); i++) 242 | glDeleteLists ( path_iter->second.lists[i], 1 ); 243 | path_items_.erase(path_iter); 244 | 245 | } 246 | else 247 | { 248 | // Create a display list 249 | GLuint new_list = glGenLists ( 1 ); 250 | p_item.lists.push_back(new_list); 251 | glNewList ( new_list, GL_COMPILE ); 252 | qglColor ( p_item.color ); 253 | 254 | for( int i = 0; i < int(p_item.unprocessed_poses.size()) - 1; i++ ) 255 | { 256 | Eigen::Vector3d &p0 = p_item.unprocessed_poses[i], &p1 = p_item.unprocessed_poses[i+1]; 257 | 258 | // Update 2D boundaries (X,Y) of the current drawn scene 259 | if ( p0(0) > max_x_ ) max_x_ = p0(0); 260 | if ( p0(0) < min_x_ ) min_x_ = p0(0); 261 | if ( p0(1) > max_y_ ) max_y_ = p0(1); 262 | if ( p0(1) < min_y_ ) min_y_ = p0(1); 263 | 264 | if ( p1(0) > max_x_ ) max_x_ = p1(0); 265 | if ( p1(0) < min_x_ ) min_x_ = p1(0); 266 | if ( p1(1) > max_y_ ) max_y_ = p1(1); 267 | if ( p1(1) < min_y_ ) min_y_ = p1(1); 268 | 269 | glBegin ( GL_LINES ); 270 | 271 | glVertex3d ( p0(0), p0(1), p0(2) ); 272 | glVertex3d ( p1(0), p1(1), p1(2) ); 273 | 274 | glEnd(); 275 | } 276 | 277 | glEndList(); 278 | 279 | Eigen::Vector3d last_pos = p_item.unprocessed_poses.back(); 280 | p_item.unprocessed_poses.clear(); 281 | p_item.unprocessed_poses.push_back(last_pos); 282 | } 283 | } 284 | 285 | std::map ::iterator lines_iter; 286 | if( (lines_iter = line_items_.find(name)) != line_items_.end() ) 287 | { 288 | LineItem &l_item = lines_iter->second; 289 | 290 | if(l_item.removed) 291 | { 292 | if ( lines_iter->second.list != GL_INVALID_VALUE ) 293 | glDeleteLists ( lines_iter->second.list, 1 ); 294 | line_items_.erase(lines_iter); 295 | } 296 | else if( l_item.updated ) 297 | { 298 | if ( l_item.list != GL_INVALID_VALUE ) 299 | glDeleteLists ( l_item.list, 1 ); 300 | 301 | Eigen::Vector3d &p0 = l_item.p0, &p1 = l_item.p1; 302 | 303 | l_item.list = glGenLists ( 1 ); 304 | 305 | glNewList ( l_item.list, GL_COMPILE ); 306 | qglColor ( l_item.color ); 307 | 308 | glBegin ( GL_LINES ); 309 | 310 | glVertex3d ( p0(0), p0(1), p0(2) ); 311 | glVertex3d ( p1(0), p1(1), p1(2) ); 312 | 313 | glEnd(); 314 | 315 | glEndList(); 316 | } 317 | } 318 | 319 | std::map ::iterator cloud_iter; 320 | if( (cloud_iter = cloud_items_.find(name)) != cloud_items_.end() ) 321 | { 322 | CloudItem &c_item = cloud_iter->second; 323 | 324 | if( c_item.removed ) 325 | { 326 | for( int i = 0; i < cloud_iter->second.lists.size(); i++) 327 | glDeleteLists ( cloud_iter->second.lists[i], 1 ); 328 | cloud_items_.erase(cloud_iter); 329 | } 330 | else 331 | { 332 | // Create a display list 333 | GLuint new_list = glGenLists ( 1 ); 334 | c_item.lists.push_back(new_list); 335 | glNewList ( new_list, GL_COMPILE ); 336 | qglColor ( c_item.color ); 337 | 338 | glBegin ( GL_POINTS ); 339 | for( int i = 0; i < int(c_item.unprcessed_cloud.size()); i++ ) 340 | { 341 | Eigen::Vector3d &p = c_item.unprcessed_cloud[i]; 342 | glVertex3d ( GLfloat(p(0)), GLfloat(p(1)), GLfloat(p(2)) ); 343 | } 344 | glEnd(); 345 | glEndList(); 346 | 347 | c_item.unprcessed_cloud.clear(); 348 | } 349 | } 350 | 351 | std::map ::iterator axes_iter; 352 | if( (axes_iter = axes_items_.find(name)) != axes_items_.end() ) 353 | { 354 | AxesItem &a_item = axes_iter->second; 355 | 356 | if(a_item.removed) 357 | { 358 | if ( axes_iter->second.list != GL_INVALID_VALUE ) 359 | glDeleteLists ( axes_iter->second.list, 1 ); 360 | for ( int i = 0; i < 7; i++ ) 361 | gluDeleteQuadric ( axes_iter->second.qobjs[i] ); 362 | axes_items_.erase(axes_iter); 363 | } 364 | else if( a_item.updated ) 365 | { 366 | if ( a_item.list != GL_INVALID_VALUE ) 367 | glDeleteLists ( a_item.list, 1 ); 368 | 369 | a_item.list = glGenLists ( 1 ); 370 | 371 | glNewList ( a_item.list, GL_COMPILE ); 372 | 373 | glEnable ( GL_LIGHTING ); 374 | 375 | qglColor ( a_item.color ); 376 | 377 | GLfloat mat_ambient[] = { a_item.color.redF(), a_item.color.greenF(), a_item.color.blueF(), 1.0f }; 378 | GLfloat mat_diffuse[] = { a_item.color.redF() /2.0f, a_item.color.greenF() /2.0f, 379 | a_item.color.blueF() /2.0f, 1.0f }; 380 | glMaterialfv ( GL_FRONT, GL_AMBIENT, mat_ambient ); 381 | glMaterialfv ( GL_FRONT, GL_DIFFUSE, mat_diffuse ); 382 | 383 | glPushMatrix(); 384 | 385 | glTranslatef ( a_item.t(0), a_item.t(1), a_item.t(2) ); 386 | 387 | glRotatef ( 180.0*sqrt ( ( a_item.r(0)*a_item.r(0) ) + ( a_item.r(1)*a_item.r(1) ) + ( a_item.r(2)*a_item.r(2) ) ) /M_PI, 388 | a_item.r(0), a_item.r(1), a_item.r(2) ); 389 | 390 | 391 | // Center 392 | gluSphere ( a_item.qobjs[0], 0.02, 5, 5 ); 393 | 394 | // Z-Axis 395 | gluCylinder ( a_item.qobjs[1], 0.02, 0.02, 0.5, 10, 5 ); 396 | glPushMatrix(); 397 | glTranslatef ( 0.0, 0.0, 0.45 ); 398 | 399 | mat_ambient[0] = 1.0f - mat_ambient[0]; 400 | mat_ambient[1] = 1.0f - mat_ambient[1]; 401 | mat_ambient[2] = 1.0f - mat_ambient[2]; 402 | mat_diffuse[0] = 1.0f - mat_diffuse[0]; 403 | mat_diffuse[1] = 1.0f - mat_diffuse[1]; 404 | mat_diffuse[2] = 1.0f - mat_diffuse[2]; 405 | 406 | glMaterialfv ( GL_FRONT, GL_AMBIENT, mat_ambient ); 407 | glMaterialfv ( GL_FRONT, GL_DIFFUSE, mat_diffuse ); 408 | 409 | gluCylinder ( a_item.qobjs[2], 0.04, 0.0, 0.1, 15, 5 ); 410 | 411 | mat_ambient[0] = 1.0f - mat_ambient[0]; 412 | mat_ambient[1] = 1.0f - mat_ambient[1]; 413 | mat_ambient[2] = 1.0f - mat_ambient[2]; 414 | mat_diffuse[0] = 1.0f - mat_diffuse[0]; 415 | mat_diffuse[1] = 1.0f - mat_diffuse[1]; 416 | mat_diffuse[2] = 1.0f - mat_diffuse[2]; 417 | glMaterialfv ( GL_FRONT, GL_AMBIENT, mat_ambient ); 418 | glMaterialfv ( GL_FRONT, GL_DIFFUSE, mat_diffuse ); 419 | 420 | glPopMatrix(); 421 | 422 | // Y-Axis 423 | glPushMatrix(); 424 | glRotatef ( -90.0,1.0,0, 0 ); 425 | gluCylinder ( a_item.qobjs[3], 0.02, 0.02, 0.5, 10, 5 ); 426 | glPushMatrix(); 427 | glTranslatef ( 0.0, 0.0, 0.45 ); 428 | gluCylinder ( a_item.qobjs[4], 0.04, 0.0, 0.1, 15, 5 ); 429 | glPopMatrix(); 430 | glPopMatrix(); 431 | 432 | // X-Axis 433 | glPushMatrix(); 434 | glRotatef ( 90.0,0.0,1.0, 0 ); 435 | gluCylinder ( a_item.qobjs[5], 0.02, 0.02, 0.5, 10, 5 ); 436 | glPushMatrix(); 437 | glTranslatef ( 0.0, 0.0, 0.45 ); 438 | gluCylinder ( a_item.qobjs[6], 0.04, 0.0, 0.1, 15, 5 ); 439 | glPopMatrix(); 440 | glPopMatrix(); 441 | 442 | glPopMatrix(); 443 | 444 | glDisable ( GL_LIGHTING ); 445 | glEndList(); 446 | } 447 | } 448 | 449 | if ( auto_adjust_ ) 450 | autoAdjustGView(); 451 | 452 | if(update_gl) 453 | updateGL(); 454 | } 455 | 456 | void OpenGL3DScene::moveCamera ( GLfloat tx, GLfloat ty, GLfloat tz, GLfloat r1x, GLfloat r2y, GLfloat r3z ) 457 | { 458 | camera_.reset(); 459 | 460 | camera_.reset(); 461 | camera_.moveAbs ( tx, ty, tz ); 462 | camera_.rotateX ( r1x ); 463 | camera_.rotateY ( r2y ); 464 | camera_.rotateZ ( r3z ); 465 | 466 | updateGL(); 467 | } 468 | 469 | void OpenGL3DScene::setFirstPersonView ( bool good_pos ) 470 | { 471 | fp_view_ = true; 472 | 473 | if ( good_pos ) 474 | moveCamera ( 0.0f, 0.0f, 1.8f, 90.0, 0.0f, 0.0f ); 475 | updateGL(); 476 | } 477 | 478 | void OpenGL3DScene::setGlobalView() 479 | { 480 | fp_view_ = false; 481 | 482 | camera_.reset(); 483 | autoAdjustGView(); 484 | 485 | updateGL(); 486 | } 487 | 488 | void OpenGL3DScene::autoAdjustGView() 489 | { 490 | 491 | makeCurrent(); 492 | GLfloat center_x = 0.0f, center_y = 0.0f; 493 | GLfloat x_t = 0.0f, y_t = 0.0f, z_t = 1.0f; 494 | 495 | if ( max_x_ != -std::numeric_limits< float >::max() && max_y_ != -std::numeric_limits< float >::max() && 496 | min_x_ != std::numeric_limits< float >::min() && min_y_ != std::numeric_limits< float >::min() ) 497 | { 498 | center_x = min_x_ + ( max_x_ - min_x_ ) / 2.0f; 499 | center_y = min_y_ + ( max_y_ - min_y_ ) / 2.0f; 500 | 501 | x_t = center_x; 502 | y_t = center_y; 503 | 504 | GLfloat half_width = qMax ( center_x - min_x_,max_x_ - center_x ) + auto_adjust_offset_; 505 | GLfloat half_height = qMax ( center_y - min_y_,max_y_ - center_y ) + auto_adjust_offset_; 506 | 507 | GLfloat half_side = qMax ( half_width,half_height ); 508 | 509 | GLfloat z_t = half_side/tanf ( view_angle_/2.0 ); 510 | 511 | if ( z_t < 1.0f ) z_t = 1.0f; 512 | 513 | camera_.moveAbs ( x_t, y_t, z_t ); 514 | 515 | } 516 | // emit zoomChanged(zT); 517 | } 518 | 519 | 520 | void OpenGL3DScene::registerPath( std::string name, QColor path_color ) 521 | { 522 | QMutexLocker locker ( &mutex_ ); 523 | 524 | if( path_items_.find(name) != path_items_.end() ) 525 | return; 526 | 527 | PathItem new_path; 528 | new_path.color = path_color; 529 | path_items_[name] = new_path; 530 | } 531 | 532 | void OpenGL3DScene::unregisterPath( std::string name ) 533 | { 534 | QMutexLocker locker ( &mutex_ ); 535 | 536 | std::map ::iterator path_iter = path_items_.find(name); 537 | if( path_iter != path_items_.end() ) 538 | path_iter->second.removed = true; 539 | } 540 | 541 | void OpenGL3DScene::pushPosition( std::string name, const Eigen::Vector3d& t ) 542 | { 543 | QMutexLocker locker ( &mutex_ ); 544 | 545 | if( path_items_.find(name) == path_items_.end() ) 546 | return; 547 | path_items_[name].unprocessed_poses.push_back(t); 548 | } 549 | 550 | void OpenGL3DScene::registerLine( std::string name, QColor line_color ) 551 | { 552 | QMutexLocker locker ( &mutex_ ); 553 | 554 | if( line_items_.find(name) != line_items_.end() ) 555 | return; 556 | 557 | LineItem new_line; 558 | new_line.color = line_color; 559 | new_line.list = GL_INVALID_VALUE; 560 | line_items_[name] = new_line; 561 | } 562 | 563 | void OpenGL3DScene::unregisterLine( std::string name ) 564 | { 565 | QMutexLocker locker ( &mutex_ ); 566 | 567 | std::map ::iterator lines_iter = line_items_.find(name); 568 | if( lines_iter != line_items_.end() ) 569 | lines_iter->second.removed = true; 570 | } 571 | 572 | void OpenGL3DScene::setLine( std::string name, const Eigen::Vector3d& p0, const Eigen::Vector3d& p1 ) 573 | { 574 | QMutexLocker locker ( &mutex_ ); 575 | 576 | if( line_items_.find(name) == line_items_.end() ) 577 | return; 578 | 579 | line_items_[name].p0 = p0; 580 | line_items_[name].p1 = p1; 581 | line_items_[name].updated = true; 582 | } 583 | 584 | void OpenGL3DScene::registerCloud( std::string name, QColor cloud_color ) 585 | { 586 | QMutexLocker locker ( &mutex_ ); 587 | 588 | if( cloud_items_.find(name) != cloud_items_.end() ) 589 | return; 590 | 591 | CloudItem new_cloud; 592 | new_cloud.color = cloud_color; 593 | cloud_items_[name] = new_cloud; 594 | } 595 | 596 | 597 | void OpenGL3DScene::unregisterCloud( std::string name ) 598 | { 599 | 600 | QMutexLocker locker ( &mutex_ ); 601 | 602 | std::map ::iterator cloud_iter = cloud_items_.find(name); 603 | if( cloud_iter != cloud_items_.end() ) 604 | cloud_iter->second.removed = true; 605 | } 606 | 607 | void OpenGL3DScene::pushCloud( std::string name, const std::vector< Eigen::Vector3d > &pts ) 608 | { 609 | QMutexLocker locker ( &mutex_ ); 610 | if( cloud_items_.find(name) == cloud_items_.end() ) 611 | return; 612 | 613 | cloud_items_[name].unprcessed_cloud.insert(cloud_items_[name].unprcessed_cloud.end(), 614 | pts.begin(), pts.end()); 615 | } 616 | 617 | void OpenGL3DScene::registerAxes( std::string name, QColor axes_color ) 618 | { 619 | QMutexLocker locker ( &mutex_ ); 620 | 621 | if( axes_items_.find(name) != axes_items_.end() ) 622 | return; 623 | 624 | AxesItem new_axes; 625 | new_axes.color = axes_color; 626 | new_axes.list = GL_INVALID_VALUE; 627 | 628 | for ( int i = 0; i < 7; i++ ) 629 | { 630 | new_axes.qobjs[i] = gluNewQuadric(); 631 | gluQuadricNormals ( new_axes.qobjs[i], GLU_SMOOTH ); 632 | } 633 | 634 | axes_items_[name] = new_axes; 635 | } 636 | 637 | void OpenGL3DScene::unregisterAxes( std::string name ) 638 | { 639 | QMutexLocker locker ( &mutex_ ); 640 | 641 | std::map ::iterator axes_iter = axes_items_.find(name); 642 | if( axes_iter != axes_items_.end() ) 643 | axes_iter->second.removed = true; 644 | } 645 | 646 | void OpenGL3DScene::setAxesPos( std::string name, const Eigen::Vector3d& r, const Eigen::Vector3d& t ) 647 | { 648 | QMutexLocker locker ( &mutex_ ); 649 | 650 | if( axes_items_.find(name) == axes_items_.end() ) 651 | return; 652 | 653 | axes_items_[name].r = r; 654 | axes_items_[name].t = t; 655 | axes_items_[name].updated = true; 656 | } 657 | 658 | void OpenGL3DScene::unregisterAllStructures() 659 | { 660 | // WARNING MAYBE BUG 661 | QMutexLocker locker ( &mutex_ ); 662 | 663 | std::map ::iterator path_iter; 664 | for ( path_iter = path_items_.begin(); path_iter != path_items_.end(); path_iter++ ) 665 | { 666 | for( int i = 0; i < path_iter->second.lists.size(); i++) 667 | glDeleteLists ( path_iter->second.lists[i], 1 ); 668 | } 669 | 670 | path_items_.clear(); 671 | 672 | std::map ::iterator lines_iter; 673 | for ( lines_iter = line_items_.begin(); lines_iter != line_items_.end(); lines_iter++ ) 674 | { 675 | if ( lines_iter->second.list != GL_INVALID_VALUE ) 676 | glDeleteLists ( lines_iter->second.list, 1 ); 677 | } 678 | 679 | line_items_.clear(); 680 | 681 | std::map ::iterator cloud_iter; 682 | for ( cloud_iter = cloud_items_.begin(); cloud_iter != cloud_items_.end(); cloud_iter++ ) 683 | { 684 | for( int i = 0; i < cloud_iter->second.lists.size(); i++) 685 | glDeleteLists ( cloud_iter->second.lists[i], 1 ); 686 | } 687 | 688 | cloud_items_.clear(); 689 | 690 | std::map ::iterator axes_iter; 691 | for ( axes_iter = axes_items_.begin(); axes_iter != axes_items_.end(); axes_iter++ ) 692 | { 693 | if ( axes_iter->second.list != GL_INVALID_VALUE ) 694 | glDeleteLists ( axes_iter->second.list, 1 ); 695 | for ( int i = 0; i < 7; i++ ) 696 | gluDeleteQuadric ( axes_iter->second.qobjs[i] ); 697 | } 698 | 699 | axes_items_.clear(); 700 | } 701 | 702 | void OpenGL3DScene::updateAllStructure() 703 | { 704 | QMutexLocker locker ( &mutex_ ); 705 | std::vector< std::string > structure_names; 706 | std::map ::iterator path_iter; 707 | for ( path_iter = path_items_.begin(); path_iter != path_items_.end(); path_iter++ ) 708 | structure_names.push_back(path_iter->first); 709 | 710 | std::map ::iterator lines_iter; 711 | for ( lines_iter = line_items_.begin(); lines_iter != line_items_.end(); lines_iter++ ) 712 | structure_names.push_back(lines_iter->first); 713 | 714 | std::map ::iterator cloud_iter; 715 | for ( cloud_iter = cloud_items_.begin(); cloud_iter != cloud_items_.end(); cloud_iter++ ) 716 | structure_names.push_back(cloud_iter->first); 717 | 718 | std::map ::iterator axes_iter; 719 | for ( axes_iter = axes_items_.begin(); axes_iter != axes_items_.end(); axes_iter++ ) 720 | structure_names.push_back(axes_iter->first); 721 | 722 | for( int i = 0; i < structure_names.size(); i++) 723 | updateStructure ( structure_names[i], false ); 724 | 725 | updateGL(); 726 | } 727 | 728 | void OpenGL3DScene::keyPressEvent ( QKeyEvent * event ) 729 | { 730 | 731 | QWidget::keyPressEvent ( event ); 732 | makeCurrent(); 733 | 734 | switch ( event->key() ) 735 | { 736 | case Qt::Key_Left: 737 | moveCameraLeft ( t_inc_ ); 738 | break; 739 | case Qt::Key_Right: 740 | moveCameraRight ( t_inc_ ); 741 | break; 742 | case Qt::Key_Up: 743 | if ( fp_view_ ) 744 | moveCameraForward ( t_inc_ ); 745 | else 746 | moveCameraUp ( t_inc_ ); 747 | break; 748 | case Qt::Key_Down: 749 | if ( fp_view_ ) 750 | moveCameraBackward ( t_inc_ ); 751 | else 752 | moveCameraDown ( t_inc_ ); 753 | break; 754 | case Qt::Key_PageUp : 755 | if ( fp_view_ ) 756 | moveCameraUp ( t_inc_ ); 757 | else 758 | moveCameraForward ( t_inc_ ); 759 | break; 760 | case Qt::Key_PageDown : 761 | if ( fp_view_ ) 762 | moveCameraDown ( t_inc_ ); 763 | else 764 | moveCameraBackward ( t_inc_ ); 765 | break; 766 | case Qt::Key_A: 767 | if ( fp_view_ ) 768 | turnCameraCCWY ( r_inc_ ); 769 | break; 770 | case Qt::Key_S: 771 | if ( fp_view_ ) 772 | turnCameraCWY ( r_inc_ ); 773 | break; 774 | case Qt::Key_Q: 775 | if ( !fp_view_ ) 776 | turnCameraCWZ( r_inc_ ); 777 | break; 778 | case Qt::Key_W: 779 | if ( !fp_view_ ) 780 | turnCameraCCWZ( r_inc_ ); 781 | break; 782 | } 783 | } 784 | 785 | void OpenGL3DScene::wheelEvent ( QWheelEvent * event ) 786 | { 787 | QWidget::wheelEvent ( event ); 788 | int num_degrees = event->delta() / 8; 789 | int num_steps = num_degrees / 15; 790 | 791 | ( num_steps > 0 ) ?moveCameraForward ( num_steps*t_inc_ ) :moveCameraBackward ( abs ( num_steps ) *t_inc_ ); 792 | event->accept(); 793 | } 794 | 795 | void OpenGL3DScene::mousePressEvent ( QMouseEvent* event ) 796 | { 797 | QWidget::mousePressEvent ( event ); 798 | if( event->button() == Qt::LeftButton ) 799 | left_mouse_button_pressed_ = true; 800 | else if( event->button() == Qt::RightButton ) 801 | right_mouse_button_pressed_ = true; 802 | 803 | mouse_init_x_ = event->x(); 804 | mouse_init_y_ = event->y(); 805 | } 806 | 807 | void OpenGL3DScene::mouseReleaseEvent ( QMouseEvent* event ) 808 | { 809 | QWidget::mouseReleaseEvent( event ); 810 | left_mouse_button_pressed_ = false; 811 | right_mouse_button_pressed_ = false; 812 | } 813 | 814 | void OpenGL3DScene::mouseMoveEvent ( QMouseEvent* event ) 815 | { 816 | QWidget::mouseMoveEvent ( event ); 817 | GLfloat mouse_inc_x = GLfloat(event->x() - mouse_init_x_); 818 | GLfloat mouse_inc_y = GLfloat(event->y() - mouse_init_y_); 819 | mouse_inc_x /= GLfloat(this->geometry().width()); 820 | mouse_inc_y /= GLfloat(this->geometry().height()); 821 | 822 | if( left_mouse_button_pressed_ ) 823 | { 824 | moveCameraLeft( 2*mouse_inc_x ); 825 | moveCameraUp( 2*mouse_inc_y ); 826 | mouse_init_x_ = event->x(); 827 | mouse_init_y_ = event->y(); 828 | } 829 | else if( right_mouse_button_pressed_ ) 830 | { 831 | if ( fp_view_ ) 832 | turnCameraCCWY( 100*mouse_inc_x ); 833 | else 834 | turnCameraCCWZ( 100*mouse_inc_x ); 835 | //moveCameraUp( 2*mouse_inc_y ); 836 | mouse_init_x_ = event->x(); 837 | mouse_init_y_ = event->y(); 838 | } 839 | } 840 | -------------------------------------------------------------------------------- /src/visualization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * imu_tk - Inertial Measurement Unit Toolkit 3 | * 4 | * Copyright (c) 2014, Alberto Pretto 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright notice, 11 | * this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright notice, 13 | * this list of conditions and the following disclaimer in the documentation 14 | * and/or other materials provided with the distribution. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | #include "imu_tk/visualization.h" 33 | #include "imu_tk/vis_extra/opengl_3d_scene.h" 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | using namespace imu_tk; 49 | 50 | static int tmp_argc = 1; 51 | static char *tmp_argv[] = { (char *)"" }; 52 | 53 | class Plot::PlotImpl 54 | { 55 | public: 56 | 57 | PlotImpl() 58 | { 59 | gnuplot_pipe_ = popen("gnuplot", "w"); 60 | if( gnuplot_pipe_ == NULL ) 61 | std::cerr<<"WARNING : missing gnuplot application!"< ( new PlotImpl() ); 88 | } 89 | 90 | template 91 | void Plot::plotSamples ( const std::vector< TriadData_<_T> >& samples, 92 | DataInterval range ) 93 | { 94 | if( !plot_impl_ptr_->ready() ) 95 | { 96 | std::cerr<<"WARNING : can't plot samples: check that gnuplot is installed in your system!"<write( strs.str() ); 118 | } 119 | 120 | template 121 | void Plot::plotIntervals ( const std::vector< TriadData_< _T > >& samples, 122 | const std::vector< DataInterval >& intervals, 123 | DataInterval range ) 124 | { 125 | if( !plot_impl_ptr_->ready() ) 126 | { 127 | std::cerr<<"WARNING : can't plot samples: check that gnuplot is installed in your system!"< max ) max = double(samples[i].x()); 139 | if( double(samples[i].y()) > max ) max = double(samples[i].y()); 140 | if( double(samples[i].z()) > max ) max = double(samples[i].z()); 141 | 142 | mean += (double(samples[i].x()) + double(samples[i].y()) + double(samples[i].z()))/3; 143 | } 144 | 145 | mean /= n_pts; 146 | max -= mean; 147 | double step_h = mean + max/2, val = 0; 148 | int interval_idx = 0; 149 | for( ; interval_idx < n_intervals; interval_idx++ ) 150 | { 151 | if (intervals[interval_idx].start_idx >= range.start_idx ) 152 | break; 153 | } 154 | 155 | std::stringstream strs; 156 | strs<<"plot '-' title 'x' with lines, " 157 | <<"'-' title 'y' with lines, " 158 | <<"'-' title 'z' with lines, " 159 | <<"'-' title 'intervals' with lines"<write( strs.str() ); 189 | } 190 | 191 | void imu_tk::waitForKey() 192 | { 193 | do 194 | { 195 | std::cout<thread()); 212 | 213 | setAutoAdjust(false); 214 | setFirstPersonView(false); 215 | moveCamera(-1.5f, -1.5f, 0.5f, 90.0f, -45.0f,0.0f); 216 | setCameraIncrements(0.1,2.0); 217 | resize( 640,480 ); 218 | show(); 219 | 220 | }; 221 | ~VisualizerImpl() {}; 222 | // TODO Ugly workaround 223 | bool waiting_for_key; 224 | 225 | protected: 226 | 227 | virtual void keyPressEvent ( QKeyEvent * event ) 228 | { 229 | OpenGL3DScene::keyPressEvent ( event ); 230 | if( event->key() == Qt::Key_Escape ) 231 | waiting_for_key = false; 232 | }; 233 | }; 234 | 235 | 236 | Vis3D::Vis3D ( const std::string win_name ) 237 | { 238 | vis_impl_ptr_ = boost::shared_ptr< VisualizerImpl > ( new VisualizerImpl() ); 239 | QString w_name( win_name.c_str() ); 240 | w_name += " - press h for help"; 241 | vis_impl_ptr_->setWindowTitle(w_name); 242 | } 243 | 244 | void Vis3D::registerFrame( std::string name, uint8_t r, uint8_t g, uint8_t b ) 245 | { 246 | vis_impl_ptr_->registerAxes(name, QColor(r,g,b) ); 247 | } 248 | 249 | void Vis3D::unregisterFrame( std::string name ) 250 | { 251 | vis_impl_ptr_->unregisterAxes(name); 252 | } 253 | 254 | template 255 | void Vis3D::setFramePos( std::string name, const _T quat[4], const _T t[3] ) 256 | { 257 | Eigen::Vector4d q_vec; 258 | Eigen::Vector3d r_vec, t_vec; 259 | q_vec<setAxesPos(name, r_vec, t_vec ); 263 | } 264 | 265 | 266 | void Vis3D::registerLine( std::string name, uint8_t r, uint8_t g, uint8_t b ) 267 | { 268 | vis_impl_ptr_->registerLine(name, QColor(r,g,b)); 269 | } 270 | 271 | void Vis3D::unregisterLine( std::string name ) 272 | { 273 | vis_impl_ptr_->unregisterLine(name); 274 | } 275 | 276 | template 277 | void Vis3D::setLinePos( std::string name, const _T p0[3], const _T p1[3] ) 278 | { 279 | Eigen::Vector3d p0_vec, p1_vec; 280 | p0_vec<setLine(name, p0_vec, p1_vec); 284 | } 285 | 286 | void Vis3D::updateAndWait( int delay_ms ) 287 | { 288 | vis_impl_ptr_->waiting_for_key = true; 289 | QTime time; 290 | 291 | if( delay_ms > 0 ) 292 | time.start(); 293 | 294 | while( vis_impl_ptr_->waiting_for_key ) 295 | { 296 | if ( !QApplication::instance() ) 297 | { 298 | new QApplication( tmp_argc, tmp_argv ); 299 | } 300 | vis_impl_ptr_->updateNow(); 301 | QApplication::instance()->processEvents(); 302 | 303 | usleep(1000); 304 | 305 | // TODO Improve here using a timer 306 | if( delay_ms > 0 && time.elapsed() > delay_ms ) 307 | vis_impl_ptr_->waiting_for_key = false; 308 | } 309 | } 310 | 311 | template void Plot::plotSamples ( const std::vector< TriadData_ >& samples, 312 | DataInterval range ); 313 | template void Plot::plotSamples ( const std::vector< TriadData_ >& samples, 314 | DataInterval range ); 315 | template void Plot::plotIntervals ( const std::vector< TriadData_ >& samples, 316 | const std::vector< DataInterval >& intervals, 317 | DataInterval range ); 318 | template void Plot::plotIntervals ( const std::vector< TriadData_ >& samples, 319 | const std::vector< DataInterval >& intervals, 320 | DataInterval range ); 321 | 322 | template void Vis3D::setFramePos( std::string name, const double quat[4], const double t[3] ); 323 | template void Vis3D::setFramePos( std::string name, const float quat[4], const float t[3] ); 324 | template void Vis3D::setLinePos( std::string name, const double p0[3], const double p1[3] ); 325 | template void Vis3D::setLinePos( std::string name, const float p0[3], const float p1[3] ); 326 | --------------------------------------------------------------------------------